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