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>();
}
}
private nav_msgs.msg.Odometry CreateOdometryMsg()
{
var msg = new nav_msgs.msg.Odometry();
msg.Child_frame_id = "/base_footprint";
msg.Header.Frame_id = "/odom";
// TODO(sam): Add support for fixed size arrays to rcl_dotnet
// msg.Pose.Covariance = poseCovarianceDiagonal.CovarianceMatrixFromDiagonal();
// msg.Twist.Covariance = twistCovarianceDiagonal.CovarianceMatrixFromDiagonal();
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");
}
}
