tf2 get position on map, map->base_link

asked 2022-05-10 13:57:35 -0600

PaddyCube gravatar image

Hi there,

I want to publish a point each time a button gets pressed on a joystick. The point I want to publish should match the actual position of the robot on map. The result should be the same as we get with rviz when publishing a point

To do so, I created a callback function of my button. Each time it gets pressed, it does a lookup of tf2 transform between map and base_link. Then, I publish the translation as new point.

    tf2_ros::TransformListener tfListener(tfBuffer);
geometry_msgs::TransformStamped transformStamped;
try
{
    transformStamped = tfBuffer.lookupTransform("map", "base_link",
                                                ros::Time(0), ros::Duration(3.0));
    geometry_msgs::PointStamped point;
    point.header.stamp = ros::Time::now();
    point.header.frame_id = "map";
    point.point.x = transformStamped.transform.translation.x;
    point.point.y = transformStamped.transform.translation.y;
    point.point.z = 0;
    pub_point.publish(point);
}
catch (tf2::TransformException &ex)
{
    ROS_WARN("%s", ex.what());
}

It all runs without error, but the point doesn't change its position even if the robot does.

This is the result with a moving robot

    ---
header: 
  seq: 9
  stamp: 
    secs: 1652208716
    nsecs: 504496873
  frame_id: "map"
point: 
  x: -0.104017204362
  y: 0.0015879370678
  z: 0.0
---
header: 
  seq: 10
  stamp: 
    secs: 1652208718
    nsecs: 871817450
  frame_id: "map"
point: 
  x: -0.104017204362
  y: 0.0015879370678
  z: 0.0
---
header: 
  seq: 11
  stamp: 
    secs: 1652208729
    nsecs: 184493917
  frame_id: "map"
point: 
  x: -0.104017204362
  y: 0.0015879370678
  z: 0.0
---

This is the corresponding output of rosrun tf tf_echo /map /base_link

At time 1652208536.761
- Translation: [-0.108, 0.001, -0.105]
- Rotation: in Quaternion [0.000, 0.000, 0.002, 1.000]
            in RPY (radian) [0.000, 0.000, 0.005]
            in RPY (degree) [0.000, 0.000, 0.268]
At time 1652208537.383
- Translation: [-0.107, 0.002, -0.105]
- Rotation: in Quaternion [0.000, -0.000, 0.002, 1.000]
            in RPY (radian) [0.000, -0.000, 0.005]
            in RPY (degree) [0.000, -0.007, 0.266]
At time 1652208628.816
- Translation: [-0.526, -0.001, -0.105]
- Rotation: in Quaternion [0.000, -0.000, 0.002, 1.000]
            in RPY (radian) [0.000, -0.000, 0.004]
            in RPY (degree) [0.002, -0.000, 0.226]
At time 1652208629.510
- Translation: [-0.524, -0.004, -0.105]
- Rotation: in Quaternion [0.000, 0.000, 0.002, 1.000]
            in RPY (radian) [0.000, 0.000, 0.004]
            in RPY (degree) [0.007, 0.000, 0.242]
edit retag flag offensive close merge delete

Comments

1

Your overall approach looks fine. However, for this to work, the tfBuffer and tfListener have to exist long-term, or you have to wait for that transform to update. And since this is a ros node, of course you need to ros::spin().

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-05-10 15:53:00 -0600 )edit

Ah thanks Mike. You're right, I now defined the listener as member variable and now it gets filled with correct values. Regarding ros::spin(), I use a asynchronuous spinner and only posted the relevant code for my issue.

PaddyCube gravatar image PaddyCube  ( 2022-05-13 05:17:06 -0600 )edit