protected override void StartRos()
{
tfPublisher = node.CreatePublisher <tf2_msgs.msg.TFMessage>(tfTopic);
tfMessage = new tf2_msgs.msg.TFMessage();
tfMessage.Transforms = new TransformStamped[1];
tfMessage.Transforms[0] = new TransformStamped();
}
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 UpdateTransforms(tf2_msgs.msg.TFMessage msg)
{
foreach (var tfTransform in msg.transforms)
{
TfFrame tfFrame;
tfFrameDict.TryGetValue(tfTransform.child_frame_id, out tfFrame);
if (tfFrame != null)
{
tfFrame.TargetPosition = tfTransform.transform.translation.Ros2Unity();
tfFrame.TargetRotation = tfTransform.transform.rotation.Ros2Unity();
}
}
}
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);
}
//private List<double> poseCovarianceDiagonal = new List<double> { 0.001d, 0.001d, 0.001d, 0.001d, 0.001d, 0.03d };
//private List<double> twistCovarianceDiagonal = new List<double> { 0.001d, 0.001d, 0.001d, 0.001d, 0.001d, 0.03d };
protected override void StartRos()
{
odometryMsg = CreateOdometryMsg();
odometryPublisher = node.CreatePublisher <nav_msgs.msg.Odometry>(OdometryTopic);
tfMsg = CreateTfMsg();
tfPublisher = node.CreatePublisher <tf2_msgs.msg.TFMessage>("tf");
if (BaseRigidbody == null)
{
BaseRigidbody = GetComponent <Rigidbody>();
}
StartCoroutine("PublishOdometry");
if (PublishTf)
{
StartCoroutine("PublishTfRoutine");
}
}
protected override void StartRos()
{
if (ParentFrame == null)
{
ParentFrame = transform;
}
if (ParentFrame != null && ChildFrame != null)
{
tfMsg = CreateTfMsg();
tfPublisher = node.CreatePublisher <tf2_msgs.msg.TFMessage>("tf");
StartCoroutine("PublishTransform");
}
else
{
Debug.LogWarning("Tf publisher needs both child and parent frame!");
}
}
