The proper way to define a TF tree
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]
Can you paste your current TF tree here (with everything running)? You can use ros-blessed to get an ascii representation of your tree.
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!!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?
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.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...
What kind of odometry approach are you using? in the video it seems to be a scale problem.
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!
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.