// 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); } }
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); } }
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); } }
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"); }
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>(); } }
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; } } }
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); } }
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(); } } } }
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); } }
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); } }