Transform coordinates x,y from base_link to odom
Hi guys, my algorithm calculates the position x, y (as double type) of an object scanned by the laser, but the problem is that these coordinates belongs to the base_link frame. Now I have to convert them to the odom reference triad, how can I do it? Do you have any TF code to implement?
Edit: this is what I'm trying to use
tf::StampedTransform base_link_to_odom_tf;
tf::TransformListener tfListener_;
tfListener_.waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(0.1));
try
{
tfListener_.lookupTransform("/odom", "/base_link", ros::Time(0), base_link_to_odom_tf);
} catch (tf::TransformException ex) {}
tf::Vector3 point, new_pos;
point.setZ(0.0);
point.setX(pos_x);
point.setY(pos_y);
new_pos = base_link_to_odom_tf * point;
obj_x.push_back(new_pos.x());
obj_y.push_back(new_pos.y());
...
obst_markers.markers[i].pose.position.x = obj_x[i];
obst_markers.markers[i].pose.position.y = obj_y[i];
where obj_x and obj_y are vectors of doubles used to load the objects as marker arrays
Edit2: after some tests moving the object in Gazebo I have these coordinates in output
x: 3.55343 new_pos.x(): 3.17272e-309
y: -0.375864 new_pos.y(): 2.97874e-309
x: 5.35993 new_pos.x(): 4.41872e-309
y: -0.338986 new_pos.y(): 4.01111e-309
x: 3.71522 new_pos.x(): 3.28127e-309
y: 1.33082 new_pos.y(): 3.65853e-309
The Using Stamped datatypes with tf2_ros::MessageFilter tutorial may help. There are many questions similar to this one and related tutorials. If you feel your question is unique, please provide links to resources you've found and explain what is different about your situation. If you've tried implementing a solution, please post your code (and any relevant output) along with links to reference material, so we can troubleshoot more easily.