Calculate the transformation from map to odom
Hi all,
For the last a few days, I am struggling with the correct way to calculate the transformation from map to odom. I have a robot system, which publish odom message. Since odom drift, I wrote my own algorithm. My algorithm can compute the location and orientation of the robot in the world (map) system. And I would like to publish a "map" frame and the transformation from map to odom.
I am working with a 2D robot and suppose geometry_msgs::Point position1
is the robot location in odom, geometry_msgs/Quaternion orientation1
is the robot orientation in odom, x, y, theta
is real location and orientation of the robot in the world system I calculated with my algorithm. theta
uses the x axis as 0 degree. What is the correct way to calculate the transformation from map to odom and publish it in tf?
Below is the code I am using to publish tf:
tf::StampedTransform tf_map_to_odom;
// set up parent and child frames
tf_map_to_odom.frame_id_ = string("map");
tf_map_to_odom.child_frame_id_ = string("odom");
// set up publish rate
ros::Rate loop_rate(100);
// set the time
tf_map_to_odom.stamp_ = ros::Time::now();
//set the transformation
tf_map_to_odom.setOrigin(***How to calculate this?***));
tf_map_to_odom.setRotation(***How to calculate this?***);
// broadcast transform
_tf_br.sendTransform(tf_map_to_odom);
Thank you very much for your help.
Anyone knows?
Did you did this? If yes can you show your code?