ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Convert odometry to transformStamped?

asked 2020-08-04 06:55:49 -0500

wntun gravatar image

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); 
}
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2020-08-09 21:57:08 -0500

wntun gravatar image

I need to filter for a quaternion of zero length.

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;
    if (transform.transform.rotation.x !=0 && transform.transform.rotation.y!=0 && transform.transform.rotation.z!=0 && transform.transform.rotation.w!=0){
        transform_pub_.publish(transform);
    }
}
edit flag offensive delete link more
0

answered 2020-08-05 13:03:40 -0500

updated 2020-08-05 13:16:25 -0500

You can do everything inside your odomCallback function

void OdomToTransform::odomCallback(const nav_msgs::Odometry &msg) {
   odometry_ = msg;

   geometry_msgs::TransformStamped tf_stamped;

   tf_stamped.header = odometry_.header;
   tf_stamped.child_frame_id = odometry_.child_frame_id;
   tf_stamped.transform.translation.x = odometry_.pose.pose.position.x;
   tf_stamped.transform.translation.y = odometry_.pose.pose.position.y;
   tf_stamped.transform.translation.z = odometry_.pose.pose.position.z;
   tf_stamped.transform.rotation = odometry_.pose.pose.orientation;

   transform_pub_.publish(tf_stamped); 

}
edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2020-08-04 01:56:21 -0500

Seen: 1,125 times

Last updated: Aug 05 '20