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 getTransformTree() { Debug.Log("########## HololensInitPositionPublisher init getTransformTree"); RCLdotnet.SpinOnce(node, 500); ROS2.TF2.Transform W2M = tflt_.LookUpLastTransform("world", "marker"); ROS2.TF2.Transform M2HI = tflt_.LookUpLastTransform("marker", "hololens_camera_init"); W2HI = TF2dotnet.MultiplyTransform(W2M, M2HI); Debug.Log("############## Hololens Init: " + W2HI.Translation_x + " " + W2HI.Translation_y + " " + W2HI.Translation_z); if (W2HI.Translation_x > 0.01 && W2HI.Translation_x > 0.01 && W2HI.Translation_z < 0.01) { is_W2HI = true; Debug.Log("########## HololensInitPositionPublisher end getTransformTree"); } }
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); } }