Convert odometry to transformStamped?
I'm working on integrating MSCKF-VIO (slam) with voxblox_node from mav_voxblox_planner. There, I need to convert odometry (from slam module) to transformstamped for mapping. Right now, I tried like that but it is not correct. Anyone please help me!
void OdomToTransform::odomCallback(const nav_msgs::OdometryConstPtr& msg) {
// Converting and storing the transform
odometry_.header = msg->header;
odometry_.child_frame_id = msg->child_frame_id;
odometry_.pose = msg->pose;
odometry_.twist = msg->twist;
//tf::transformMsgToKindr(msg->transform, &transform_.transformation);
}
void OdomToTransform::publishTransformStamped(const ros::TimerEvent& event) {
// odom -> transformStamped (point)
geometry_msgs::TransformStamped transform;
transform.header = odometry_.header;
transform.child_frame_id = odometry_.child_frame_id;
transform.transform.translation.x = odometry_.pose.pose.position.x;
transform.transform.translation.y = odometry_.pose.pose.position.y;
transform.transform.translation.z = odometry_.pose.pose.position.z;
transform.transform.rotation = odometry_.pose.pose.orientation;
transform_pub_.publish(transform);
}