The proper way to define a TF tree

asked 2020-10-21 12:04:56 -0500

mirella melo gravatar image

updated 2020-11-02 19:38:42 -0500

Hi. I have two nodes: (1) the SLAM node, and (2) the Rtab-map node, which requires - for my case - (a) stereo images msg and (b) its respective configs msg; (c) odometry msg and (d) the transformation tree map->odom->base_link->camera_link. I'm sending all from:

Node (1): publishing odometry msg, images msg, and configs msg

odom_msg_->header.stamp = node->now();
odom_msg_->header.frame_id = "odom";
odom_msg_->child_frame_id = "base_link";
//...data...
odometry_pub_->publish(odom_msg_);
// ...data...
left_image_msg->header.frame_id = "camera_link";
right_image_msg->header.frame_id = "camera_link";
left_config.header.frame_id = "camera_link";
right_config.header.frame_id = "camera_link";

Node (1): publishing the TF (odom -> base_link)

tf2_ros::TransformBroadcaster odom_broadcaster(node);
odom_trans.header.stamp = odom_msg_->header.stamp;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
//...data...
odom_broadcaster.sendTransform(odom_trans);

Node (2): in the launch file I relate base_link->camera_link

 Node(
        package='tf2_ros', node_executable='static_transform_publisher',
        arguments=["0.0", "0.0", "0.0", -1.57", "0.0", "-1.57", "base_link", "camera_link"),

It is important to say that frame_id is set to base_link. And in RVIZ, I set map as the global fixed frame.

When I run the node (1) and (2) I can see (via RVIZ) the transformation tree as supposed to be. Then, I also debbuged used tf2_echo:

 ros2 run tf2_ros tf2_echo map odom
 At time 1603749986.677481295
 - Translation: [0.000, 0.000, 0.000]
 - Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

 ros2 run tf2_ros tf2_echo odom base_link
 At time 1603750227.448060361
 - Translation: [-0.005, 0.511, 0.000]
 - Rotation: in Quaternion [0.004, -0.003, 0.704, 0.710]

 ros2 run tf2_ros tf2_echo base_link camera_link
 At time 0.0
 - Translation: [0.000, 0.000, 0.000]
 - Rotation: in Quaternion [0.500, -0.500, 0.500, 0.500]

Node (2) does not show errors, just this warning:

 [ WARN] (2020-10-26 15:10:34.345) MsgConversion.cpp:1385::getTransform() (getting transform odom -> base_link) Lookup would require extrapolation into the future.  Requested time 1603750229.45627 but the latest data is at time 1603750227.74125, when looking up transform from frame [base_link] to frame [odom]
edit retag flag offensive close merge delete

Comments

Can you paste your current TF tree here (with everything running)? You can use ros-blessed to get an ascii representation of your tree.

chfritz gravatar image chfritz  ( 2020-10-24 11:30:37 -0500 )edit

Sorry for my late reply and thanks for answering me! Does ros-blessed work for ROS2? I installed it, but it asked me to source the environment even after I had done it. The only debug I could make was using tf2_echo. Please, let me know any tool for debugging TF in ROS2! I just updated the question with new info. Thanks!!

mirella melo gravatar image mirella melo  ( 2020-10-26 23:31:56 -0500 )edit

ah, sorry, you are right. ros-blessed only works for ROS1 right now. Thanks for adding the tf echo. Seems like your node1 is not publishing the odom->base_link transform often enough. What's the frequency?

chfritz gravatar image chfritz  ( 2020-10-27 10:34:53 -0500 )edit

It is inside the odometry node, more specifically inside the callback function, this is, every time a new frame arrives (because it is visual SLAM), node (1) computes the odometry and then I publish the odometry info and odom->base_link, together with the frames to the node 2 (Rtab-map) - they also have the same timestamp.So, the frequency follows the FPS rate, which is approximate 3 fps.

mirella melo gravatar image mirella melo  ( 2020-10-27 11:16:32 -0500 )edit

Here in this link, I put a video with the two nodes running. I`m starting to believe that the problem is in the odometry info because while the camera goes forward, the grid map seems nice, but once it starts to turn, it goes crazy...

mirella melo gravatar image mirella melo  ( 2020-10-27 11:53:48 -0500 )edit

What kind of odometry approach are you using? in the video it seems to be a scale problem.

matlabbe gravatar image matlabbe  ( 2020-11-06 20:19:21 -0500 )edit

Thanks for replying! I'm using OpenvSlam. And here is my odometry node. I detected/did few improvements and I'm not getting any more warnings. I have better described my current situation/observations on Rtabmap's github. While rotating seems to work fine, during translation, the point cloud has been duplicating. Thanks!

mirella melo gravatar image mirella melo  ( 2020-11-06 20:39:51 -0500 )edit
1

Note also that the first pose sent to rtabmap was not invertible (wrong rotation), making all next poses ignored. I added a check in this commit to fix that problem.

matlabbe gravatar image matlabbe  ( 2020-11-06 20:40:24 -0500 )edit