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 Update()
{
if (RCLdotnet.Ok())
{
RCLdotnet.SpinOnce(tflt_node, 500);
ROS2.TF2.Transform tf = tflt_.LookUpLastTransform("world", "robot");
this.transform.position = new Vector3((float)tf.Translation_x, (float)tf.Translation_y, (float)tf.Translation_z);
}
}
void Update()
{
if (RCLdotnet.Ok())
{
System.Tuple <int, uint> r_ts2 = RCLdotnet.Now();
System.Tuple <int, uint> r_ts2_past = new System.Tuple <int, uint>(r_ts2.Item1 - 1, r_ts2.Item2);
//ROS2.TF2.Transform tf = capsule_tflt_.LookUpLastTransform("world", "cube");
ROS2.TF2.Transform tf = turtle_tflt_.LookUpTransform("world", "turtle_1", r_ts2_past);
this.transform.position = new Vector3((float)tf.Translation_x + 0.3f, (float)tf.Translation_y, (float)tf.Translation_z);
RCLdotnet.SpinOnce(turtle_tflt_node, 1000);
}
}
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())
{
ROS2.TF2.Transform tf = tflt_.LookUpLastTransform("world", "base_link");
//ROS2.TF2.Transform tf = tflt_.LookUpTransform("world", "base_link", RCLdotnet.Now());
float xpos = (float)Math.Round(tf.Translation_x, 3, MidpointRounding.ToEven);
//float ypos = 0;
//float zpos = 0;
float ypos = (float)Math.Round(tf.Translation_y, 3, MidpointRounding.ToEven);
float zpos = (float)Math.Round(tf.Translation_z, 3, MidpointRounding.ToEven);
//this.transform.position = new Vector3(xpos, ypos, zpos);
//Debug.Log("############## TF_update_cube - W2B valid: " + tf.Valid);
Debug.Log("############## TF_update_cube - W2B pos: " + xpos + " " + ypos + " " + zpos);
RCLdotnet.SpinOnce(tflt_node, 200);
//RCLdotnet.Spin(tflt_node);
}
}
void Start()
{
//Debug.Log("************ (00) ************");
RCLdotnet.Init();
//Debug.Log("************ (01) ************");
//TF2dotnet.Init();
//Debug.Log("************ (02) ************");
node = RCLdotnet.CreateNode("camera_pos_listener");
tf = new ROS2.TF2.Transform();
//Debug.Log("************ (1) ************");
tflt_ = new TransformListener(ref node);
//Debug.Log("************ (2) ************");
if (tflt_ == null)
{
Debug.Log("************ (3) ************");
Debug.Log("tflt_ SI es null :(");
}
else
{
Debug.Log("************ (4) ************");
Debug.Log("tflt_ NO es null :)");
}
//Debug.Log("************ (5) ************");
}
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);
}
}
