TF tree conflict when fusing ICP odometry with wheel encoders while deskewing LiDAR cloud

[BaptisteCarbillet]

Hello, I am trying to fuse LiDAR ICP odometry with wheel encoder odometry using robot_localization, and I’m running into a TF tree issue that breaks deskewing in RTAB-Map. My launch file is very similar to this one https://github.com/introlab/rtabmap_ros/blob/ros2/rtabmap_examples/launch/lidar3d.launch.py. If I only use the odometry coming from ICP, my tf tree is correct, with map -> odom -> base_link_stabilized -> base_link. When I want to use robot_localization to fuse my odometries, base_link_stabilized gets created and becomes a parent of base_link, and at the same time, the fused odometry frame (e.g. odom) also tries to be the parent of base_link, which results in a TF tree that is not a tree anymore. Here are my frames configuration in my ekf.yaml file. I was thinking to circumvent this, I could put base_link_stabilized instead of base_link in base_link frame which produce a correct tree, but it feels wrong conceptually and when the robot spins in place, the SLAM pose estimate drifts and the map becomes inconsistent.

map_frame: map              
odom_frame: odom            
base_link_frame: base_link
world_frame: odom    

Should LiDAR deskewing be disabled or avoided in this case? Is there a known best practice for combining LiDAR deskwing and odometry fusion with robot_localization?

[matlabbe]

You would probably need to disable tf published from icp_odometry by setting its parameter publish_tf to false. Make also sure that your wheel odometry node is also not publishing any TF, just the odom topic. Then you have to setup robot_localization to provide transform odom -> base_link_stabilized and not odom -> base_link. It should be possible to do it by configuring these parameters correctly:

map_frame: odom              
odom_frame: base_link_stabilized
base_link_frame: base_link
world_frame: odom    

Other option is to remove base_link_stabilized and let lidar_deskewing deskewing from odom frame directly. Note however that it may feel slightly weird to deskew from an odometry frame that includes output of icp_odometry. That may just not work.