Esempio n. 1
0
    // void Update()
    void PublishHololensCamera2HololensInit()
    {
        if (RCLdotnet.Ok())
        {
            geometry_msgs.msg.TransformStamped w2h = new geometry_msgs.msg.TransformStamped();

            w2h.Header.Frame_id = "hololens_camera_init";

            System.Tuple <int, uint> ts = RCLdotnet.Now();

            w2h.Header.Stamp.Sec     = ts.Item1;
            w2h.Header.Stamp.Nanosec = ts.Item2;
            w2h.Child_frame_id       = "hololens_camera";

            w2h.Transform.Translation.X = Camera.main.transform.position.z;
            w2h.Transform.Translation.Y = -Camera.main.transform.position.x;
            w2h.Transform.Translation.Z = Camera.main.transform.position.y;
            w2h.Transform.Rotation.X    = Camera.main.transform.rotation.z;
            w2h.Transform.Rotation.Y    = -Camera.main.transform.rotation.x;
            w2h.Transform.Rotation.Z    = Camera.main.transform.rotation.y;
            w2h.Transform.Rotation.W    = -Camera.main.transform.rotation.w;

            /*
             * w2h.Transform.Translation.X = Camera.main.transform.position.x;
             * w2h.Transform.Translation.Y = Camera.main.transform.position.z;
             * w2h.Transform.Translation.Z = Camera.main.transform.position.y;
             * w2h.Transform.Rotation.X = -Camera.main.transform.rotation.x;
             * w2h.Transform.Rotation.Y = -Camera.main.transform.rotation.z;
             * w2h.Transform.Rotation.Z = -Camera.main.transform.rotation.y;
             * w2h.Transform.Rotation.W = Camera.main.transform.rotation.w;
             */
            tfbr_.SendTransform(ref w2h);
        }
    }
Esempio n. 2
0
    void Update()
    {
        if (RCLdotnet.Ok())
        {
            //ROS2.TF2.Transform tf_W2H = tflt_.LookUpLastTransform("world", "hololens_camera");
            ROS2.TF2.Transform W2M  = tflt_.LookUpLastTransform("world", "marker");
            ROS2.TF2.Transform M2HI = tflt_.LookUpLastTransform("marker", "hololens_camera_init");
            ROS2.TF2.Transform HI2H = tflt_.LookUpLastTransform("hololens_camera_init", "hololens_camera");

            // W2H = W2M * M2HI * HI2H
            ROS2.TF2.Transform W2HI = TF2dotnet.MultiplyTransform(W2M, M2HI);
            ROS2.TF2.Transform W2H  = TF2dotnet.MultiplyTransform(W2HI, HI2H);

            geometry_msgs.msg.TransformStamped w2h = new geometry_msgs.msg.TransformStamped();
            w2h.Header.Frame_id = "world";
            System.Tuple <int, uint> ts = RCLdotnet.Now();
            w2h.Header.Stamp.Sec        = ts.Item1;
            w2h.Header.Stamp.Nanosec    = ts.Item2;
            w2h.Child_frame_id          = "hololens_estimated";
            w2h.Transform.Translation.X = W2H.Translation_x;
            w2h.Transform.Translation.Y = W2H.Translation_y;
            w2h.Transform.Translation.Z = W2H.Translation_z;
            w2h.Transform.Rotation.X    = W2H.Rotation_x;
            w2h.Transform.Rotation.Y    = W2H.Rotation_y;
            w2h.Transform.Rotation.Z    = W2H.Rotation_z;
            w2h.Transform.Rotation.W    = W2H.Rotation_w;
            tfbr_.SendTransform(ref w2h);

            RCLdotnet.SpinOnce(node, 500);
        }
    }
Esempio n. 3
0
    void PublishMarker()
    // void Update()
    {
        if (RCLdotnet.Ok())
        {
            Debug.Log("########## PublishMarker");

            geometry_msgs.msg.TransformStamped w2m = new geometry_msgs.msg.TransformStamped();

            w2m.Header.Frame_id = "world";

            System.Tuple <int, uint> ts = RCLdotnet.Now();

            w2m.Header.Stamp.Sec     = ts.Item1;
            w2m.Header.Stamp.Nanosec = ts.Item2;
            w2m.Child_frame_id       = "marker";

            // FRONT (red)
            w2m.Transform.Translation.X = 3;
            // LEFT (green)
            w2m.Transform.Translation.Y = 0;
            // UP (blue)
            w2m.Transform.Translation.Z = 0;
            w2m.Transform.Rotation.X    = 0;
            w2m.Transform.Rotation.Y    = 0;
            w2m.Transform.Rotation.Z    = 0;
            w2m.Transform.Rotation.W    = 1;

            tfbr_.SendTransform(ref w2m);
        }
    }
Esempio n. 4
0
 void PublishHololensInit2Marker()
 // void Update()
 {
     if (RCLdotnet.Ok())
     {
         if (init_info == true)
         {
             //Debug.Log("########## HololensPositionPublisher init_info == true");
             geometry_msgs.msg.TransformStamped hi2m = new geometry_msgs.msg.TransformStamped();
             //Debug.Log("########## HololensPositionPublisher 111");
             hi2m.Header.Frame_id = "marker";
             //Debug.Log("########## HololensPositionPublisher 222");
             System.Tuple <int, uint> ts = RCLdotnet.Now();
             //Debug.Log("########## HololensPositionPublisher 333");
             hi2m.Header.Stamp.Sec     = ts.Item1;
             hi2m.Header.Stamp.Nanosec = ts.Item2;
             hi2m.Child_frame_id       = "hololens_camera_init";
             //Debug.Log("########## HololensPositionPublisher 444");
             hi2m.Transform.Translation.X = -init_pos.z;
             hi2m.Transform.Translation.Y = -init_pos.x;
             hi2m.Transform.Translation.Z = -init_pos.y;
             hi2m.Transform.Rotation.X    = 0;
             hi2m.Transform.Rotation.Y    = 0;
             hi2m.Transform.Rotation.Z    = 0;
             hi2m.Transform.Rotation.W    = 1;
             //Debug.Log("########## HololensPositionPublisher 444");
             tfbr_.SendTransform(ref hi2m);
             //Debug.Log("########## HololensPositionPublisher SendTransform");
         }
     }
     Debug.Log("########## HololensPositionPublisher update");
 }
Esempio n. 5
0
    void Start()
    {
        odometryPublisher = node.CreatePublisher <nav_msgs.msg.Odometry>(OdometryTopic);
        tfPublisher       = node.CreatePublisher <tf2_msgs.msg.TFMessage>(TfTopic);

        clock = new rclcs.Clock();

        odometryMsg = new nav_msgs.msg.Odometry();
        odometryMsg.header.frame_id = "/odom";
        odometryMsg.child_frame_id  = "/base_footprint";
        robotPose  = odometryMsg.pose;
        robotTwist = odometryMsg.twist;

        tfMsg = new tf2_msgs.msg.TFMessage();
        tfMsg.transforms_init(1);
        tfTransformStamped = tfMsg.transforms[0];
        tfTransformStamped.header.frame_id = "/odom";
        tfTransformStamped.child_frame_id  = "/base_footprint";

        odometryMsg.pose.covariance  = poseCovarianceDiagonal.CovarianceMatrixFromDiagonal();
        odometryMsg.twist.covariance = twistCovarianceDiagonal.CovarianceMatrixFromDiagonal();

        if (BaseRigidbody == null)
        {
            BaseRigidbody = GetComponentInChildren <Rigidbody>();
        }
    }
Esempio n. 6
0
 void Update()
 {
     if (RCLdotnet.Ok())
     {
         geometry_msgs.msg.TransformStamped w2o = new geometry_msgs.msg.TransformStamped();
         w2o.Header.Frame_id = "odom";
         System.Tuple <int, uint> r_ts2 = RCLdotnet.Now();
         w2o.Header.Stamp.Sec        = r_ts2.Item1;
         w2o.Header.Stamp.Nanosec    = r_ts2.Item2;
         w2o.Child_frame_id          = "base_link";
         w2o.Transform.Translation.X = moveX;
         w2o.Transform.Translation.Y = 0;
         w2o.Transform.Translation.Z = 0;
         w2o.Transform.Rotation.X    = 0;
         w2o.Transform.Rotation.Y    = 0;
         w2o.Transform.Rotation.Z    = 0;
         w2o.Transform.Rotation.W    = 1;
         kobuki_tfbr_.SendTransform(ref w2o);
         RCLdotnet.SpinOnce(kobuki_tfbr_node, 200);
         if (r_ts2.Item1 != sec)
         {
             sec    = r_ts2.Item1;
             moveX += 0.05f;
         }
     }
 }
Esempio n. 7
0
    void Update()
    {
        if (RCLdotnet.Ok())
        {
            geometry_msgs.msg.TransformStamped w2h = new geometry_msgs.msg.TransformStamped();

            w2h.Header.Frame_id = "world";

            System.Tuple <int, uint> ts = RCLdotnet.Now();

            w2h.Header.Stamp.Sec     = ts.Item1;
            w2h.Header.Stamp.Nanosec = ts.Item2;
            w2h.Child_frame_id       = "hololens_camera";

            w2h.Transform.Translation.X = Camera.main.transform.position.z;
            w2h.Transform.Translation.Y = -Camera.main.transform.position.x;
            w2h.Transform.Translation.Z = Camera.main.transform.position.y;
            w2h.Transform.Rotation.X    = Camera.main.transform.rotation.z;
            w2h.Transform.Rotation.Y    = -Camera.main.transform.rotation.x;
            w2h.Transform.Rotation.Z    = Camera.main.transform.rotation.y;
            w2h.Transform.Rotation.W    = -Camera.main.transform.rotation.w;

            tfbr_.SendTransform(ref w2h);

            //Debug.Log("############## CameraPositionPublisher - W2H: " + w2h.Transform.Translation.X + " " + w2h.Transform.Translation.Y + " " + w2h.Transform.Translation.Z);
        }
    }
Esempio n. 8
0
    private tf2_msgs.msg.TFMessage CreateTfMsg()
    {
        var msg = new tf2_msgs.msg.TFMessage
        {
            Transforms = new geometry_msgs.msg.TransformStamped[1],
        };

        var transformStamped = new geometry_msgs.msg.TransformStamped();

        transformStamped.Header.Frame_id = "/odom";
        transformStamped.Child_frame_id  = "/base_footprint";
        msg.Transforms[0] = transformStamped;

        return(msg);
    }
    private tf2_msgs.msg.TFMessage CreateTfMsg()
    {
        var msg = new tf2_msgs.msg.TFMessage
        {
            Transforms = new geometry_msgs.msg.TransformStamped[1],
        };

        var transformStamped = new geometry_msgs.msg.TransformStamped();

        transformStamped.Header.Frame_id = ParentFrame.name;
        transformStamped.Child_frame_id  = ChildFrame.name;
        msg.Transforms[0] = transformStamped;

        return(msg);
    }
 void Update()
 {
     if (RCLdotnet.Ok())
     {
         geometry_msgs.msg.TransformStamped w2c = new geometry_msgs.msg.TransformStamped();
         w2c.Header.Frame_id = "world";
         System.Tuple <int, uint> r_ts2 = RCLdotnet.Now();
         w2c.Header.Stamp.Sec        = r_ts2.Item1;
         w2c.Header.Stamp.Nanosec    = r_ts2.Item2;
         w2c.Child_frame_id          = "turtle_1";
         w2c.Transform.Translation.X = transform.position.x;
         w2c.Transform.Translation.Y = transform.position.y;
         w2c.Transform.Translation.Z = transform.position.z;
         w2c.Transform.Rotation.X    = 0;
         w2c.Transform.Rotation.Y    = 0;
         w2c.Transform.Rotation.Z    = 0;
         w2c.Transform.Rotation.W    = 1;
         turtle_tfbr_.SendTransform(ref w2c);
         RCLdotnet.SpinOnce(turtle_tfbr_node, 200);
     }
 }
    void PublishHololensInit2World()
    //void Update()
    {
        if (RCLdotnet.Ok())
        {
            if (init_info == true)
            {
                Debug.Log("########## HololensInitPositionPublisher init_info == true");
                if (is_W2HI == true)
                {
                    //init_info = false;
                    geometry_msgs.msg.TransformStamped w2hi = new geometry_msgs.msg.TransformStamped();

                    w2hi.Header.Frame_id = "world";

                    System.Tuple <int, uint> ts = RCLdotnet.Now();

                    w2hi.Header.Stamp.Sec     = ts.Item1;
                    w2hi.Header.Stamp.Nanosec = ts.Item2;
                    w2hi.Child_frame_id       = "hololens_camera_init";

                    w2hi.Transform.Translation.X = W2HI.Translation_x;
                    w2hi.Transform.Translation.Y = W2HI.Translation_y;
                    w2hi.Transform.Translation.Z = W2HI.Translation_z;
                    w2hi.Transform.Rotation.X    = W2HI.Rotation_x;
                    w2hi.Transform.Rotation.Y    = W2HI.Rotation_y;
                    w2hi.Transform.Rotation.Z    = W2HI.Rotation_z;
                    w2hi.Transform.Rotation.W    = W2HI.Rotation_w;

                    tfbr_.SendTransform(ref w2hi);
                    Debug.Log("########## HololensInitPositionPublisher END");
                    RCLdotnet.SpinOnce(node, 500);
                }
                else
                {
                    getTransformTree();
                }
            }
        }
    }
Esempio n. 12
0
 void PublishW2O()
 {
     if (RCLdotnet.Ok())
     {
         geometry_msgs.msg.TransformStamped w2o = new geometry_msgs.msg.TransformStamped();
         w2o.Header.Frame_id = "world";
         System.Tuple <int, uint> r_ts2 = RCLdotnet.Now();
         w2o.Header.Stamp.Sec        = r_ts2.Item1;
         w2o.Header.Stamp.Nanosec    = r_ts2.Item2;
         w2o.Child_frame_id          = "odom";
         w2o.Transform.Translation.X = W2O.x;
         w2o.Transform.Translation.Y = W2O.y;
         w2o.Transform.Translation.Z = W2O.z;
         w2o.Transform.Rotation.X    = 0;
         w2o.Transform.Rotation.Y    = 0;
         w2o.Transform.Rotation.Z    = 0;
         w2o.Transform.Rotation.W    = 1;
         tfbr_.SendTransform(ref w2o);
         RCLdotnet.SpinOnce(tfbr_node, 200);
         Debug.Log("############## TF_Test - PublishW2O: " + W2O + " at " + r_ts2.Item1 + "." + r_ts2.Item2);
         //RCLdotnet.Spin(tfbr_node);
     }
 }
Esempio n. 13
0
    void Update()
    {
        if (RCLdotnet.Ok())
        {
            // Publish World2Odom (both are at the same position)
            geometry_msgs.msg.TransformStamped w2o1 = new geometry_msgs.msg.TransformStamped();
            w2o1.Header.Frame_id = "world";
            System.Tuple <int, uint> ts = RCLdotnet.Now();
            w2o1.Header.Stamp.Sec        = ts.Item1;
            w2o1.Header.Stamp.Nanosec    = ts.Item2;
            w2o1.Child_frame_id          = "robot1/odom";
            w2o1.Transform.Translation.X = 0;
            w2o1.Transform.Translation.Y = 0;
            w2o1.Transform.Translation.Z = 0;
            w2o1.Transform.Rotation.X    = 0;
            w2o1.Transform.Rotation.Y    = 0;
            w2o1.Transform.Rotation.Z    = 0;
            w2o1.Transform.Rotation.W    = 1;
            tfbr_.SendTransform(ref w2o1);

            geometry_msgs.msg.TransformStamped w2o2 = new geometry_msgs.msg.TransformStamped();
            w2o2.Header.Frame_id = "world";
            System.Tuple <int, uint> ts2 = RCLdotnet.Now();
            w2o2.Header.Stamp.Sec        = ts2.Item1;
            w2o2.Header.Stamp.Nanosec    = ts2.Item2;
            w2o2.Child_frame_id          = "robot2/odom";
            w2o2.Transform.Translation.X = 3;
            w2o2.Transform.Translation.Y = 0;
            w2o2.Transform.Translation.Z = 0;
            w2o2.Transform.Rotation.X    = 0;
            w2o2.Transform.Rotation.Y    = 0;
            w2o2.Transform.Rotation.Z    = 0;
            w2o2.Transform.Rotation.W    = 1;
            tfbr_.SendTransform(ref w2o2);

            RCLdotnet.SpinOnce(node, 500);
            //RCLdotnet.Spin(node);
            ROS2.TF2.Transform W2HI   = tflt_.LookUpLastTransform("world", "hololens_camera_init");
            ROS2.TF2.Transform O12BF1 = tflt_.LookUpLastTransform("robot1/odom", "robot1/base_footprint");
            ROS2.TF2.Transform O22BF2 = tflt_.LookUpLastTransform("robot2/odom", "robot2/base_footprint");
            ROS2.TF2.Transform W2HI_1 = TF2dotnet.InverseTransform(W2HI);
            // W2HI_1 * W2BF = HI2BF
            // HI2W   * W2BF = HI2BF
            ROS2.TF2.Transform HI2BF1 = TF2dotnet.MultiplyTransform(W2HI_1, O12BF1);
            ROS2.TF2.Transform HI2BF2 = TF2dotnet.MultiplyTransform(W2HI_1, O22BF2);
            //Debug.Log("########## BaseFootprintEstimated2HololensInit " + (float)BF2HI.Translation_x + " " + (float)BF2HI.Translation_y + " " + (float)BF2HI.Translation_z);
            //LaserVisualizer_test.origin = new Vector3((float)HI2BF.Translation_x, (float)HI2BF.Translation_y, (float)HI2BF.Translation_z);
            RobotPositionVisualization.origin_1 = new Vector3((float)HI2BF1.Translation_x, (float)HI2BF1.Translation_y, (float)HI2BF1.Translation_z);
            RobotPositionVisualization.origin_2 = new Vector3((float)HI2BF2.Translation_x, (float)HI2BF2.Translation_y, (float)HI2BF2.Translation_z);


            geometry_msgs.msg.TransformStamped h2bf1 = new geometry_msgs.msg.TransformStamped();
            h2bf1.Header.Frame_id = "hololens_camera_init";
            System.Tuple <int, uint> ts3 = RCLdotnet.Now();
            h2bf1.Header.Stamp.Sec        = ts3.Item1;
            h2bf1.Header.Stamp.Nanosec    = ts3.Item2;
            h2bf1.Child_frame_id          = "robot1/base_footprint_estimated";
            h2bf1.Transform.Translation.X = HI2BF1.Translation_x;
            h2bf1.Transform.Translation.Y = HI2BF1.Translation_y;
            h2bf1.Transform.Translation.Z = HI2BF1.Translation_z;
            h2bf1.Transform.Rotation.X    = 0;
            h2bf1.Transform.Rotation.Y    = 0;
            h2bf1.Transform.Rotation.Z    = 0;
            h2bf1.Transform.Rotation.W    = 1;
            tfbr_.SendTransform(ref h2bf1);

            geometry_msgs.msg.TransformStamped h2bf2 = new geometry_msgs.msg.TransformStamped();
            h2bf2.Header.Frame_id = "hololens_camera_init";
            System.Tuple <int, uint> ts4 = RCLdotnet.Now();
            h2bf2.Header.Stamp.Sec        = ts4.Item1;
            h2bf2.Header.Stamp.Nanosec    = ts4.Item2;
            h2bf2.Child_frame_id          = "robot2/base_footprint_estimated";
            h2bf2.Transform.Translation.X = HI2BF2.Translation_x;
            h2bf2.Transform.Translation.Y = HI2BF2.Translation_y;
            h2bf2.Transform.Translation.Z = HI2BF2.Translation_z;
            h2bf2.Transform.Rotation.X    = 0;
            h2bf2.Transform.Rotation.Y    = 0;
            h2bf2.Transform.Rotation.Z    = 0;
            h2bf2.Transform.Rotation.W    = 1;
            tfbr_.SendTransform(ref h2bf2);
        }
    }