TF error kinect+rplidar

[palopter]

Hello Matlabbe,

Im using a kinect sensor and a rplidar for 2D and 3D mapping on Rviz. The problem comes when launching the rtabmap.launch file and the kinect just works for a second. I think that the issue must be related to the tf configuration, but after looking for some solution on internet I havent found any solution. The error is the next:

[ WARN] [1598258060.021085182]: Could not get transform from hector_slam to base_link after 0.200000 seconds (for stamp=1598258059.668498)! Error="Could not find a connection between 'hector_slam' and 'base_link' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.20042 timeout was 0.2.".

[ WARN] [1598258060.021195745]: We received odometry message, but we cannot get the corresponding TF hector_slam->base_link at data stamp 1598258059.668498s (odom msg stamp is 1598258059.668498s). Make sure TF of odometry is also published to get more accurate pose estimation. This warning is only printed once.

[ WARN] [1598258060.225278256]: Could not get transform from base_link to laser after 0.200000 seconds (for stamp=1598258059.796376)! Error="Could not find a connection between 'base_link' and 'laser' because they are not part of the same tree.Tf has two or more unconnected trees.. canTransform returned after 0.201163 timeout was 0.2.".

[ERROR] [1598258060.225372926]: Could not convert laser scan msg! Aborting rtabmap update...





I did the following commands:

$ roslaunch rplidar_ros rplidar.launch
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_utils static_tfs_kinect2_rplidar.launch
$ roslaunch rtabmap_utils display_pushcart.launch
$ roslaunch rtabmap_utils rtabmap.launch hector:=true

The files are the next:

 static_tfs_kinect2_rplidar.launch:

<launch>
   
   
                           
   <node pkg="tf" type="static_transform_publisher" name="static_tf_base_kinect"
    args="0.0 0.0 0.0 -1.5707963267948966 0.0 -1.5707963267948966 /base_link /camera_link 100" />

   <node pkg="tf" type="static_transform_publisher" name="static_tf_kinect_laser" args="0 0 0 0.01 0 0.01 camera_link laser 100" respawn="true"/> 
   
   <node pkg="tf" type="static_transform_publisher" name="camera_to_optical_tf" args="0 0 0 -1.570796 0 -1.570796 camera camera_optical 10" />

   
   
    <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0 0 0 base_footprint base_link 10" />


</launch>

display_pushcart.launch:

<?xml version="1.0"?>
<launch>
        <arg name="model" default="$(find rtabmap_utils)/urdf/pushcart.urdf"/>
       
       
       
        <node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
           
        </node> 
       
   
    <node pkg="tf" type="static_transform_publisher" name="static_tf_base_link" args="-0.3 0.0 -0.425 0.0 0.0 0.0 laser base_link 100" respawn="true"/>

</launch>

rtabmap.launch:


<launch>
   
   <arg name="resolution" default="sd" />
   
   
   <arg name="frame_id" default="laser"/> 

 
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="true" /> 
   

   
   
   <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">
       
       
       
       
       
       
       
       
       
       
       
       
   </node>
   
   
   
   <group ns="rtabmap">   
        <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
           
           
                       
             
             
             
                   
       
            <remap from="rgb/image"            to="/camera/rgb/image_rect_color"/>
            <remap from="depth/image"          to="/camera/depth_registered/image_raw"/>
            <remap from="rgb/camera_info"      to="/camera/rgb/camera_info"/>
            <remap from="scan"                 to="/scan"/>
            <remap from="odom"                 to="/scanmatch_odom"/>

           
               
             
             
           
           
        </node>
   </group>
 
 
   
   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_utils)/rviz_configs/kinect_rtabmap_with_hector.rviz"/>
 
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
        <remap from="rgb/image"       to="data_odom_sync/image"/>
        <remap from="depth/image"     to="data_odom_sync/depth"/>
        <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
        <remap from="cloud"           to="voxel_cloud" />
       
   </node>


   
</launch>


What am I doing wrong? Could you help me?

[matlabbe]

Hi,

Try debugging your TF tree with:

$ rosrun tf view_frames
$ evince frames.pdf

Use rqt_console to see which nodes are complaining.

If you are using base_footprint frame, I guess you would have to change the base frame used by hector_mapping to base_footprint instead of base_link. Is hector_mapping publishing the corresponding TF of the odometry message?

cheers,
Mathieu

[palopter]

Hi Mathieu,

the tf tree is the next:



I have changed the hector_mapping frame_id to base_footprint but it doesnt work. I attach some captures from the console showing the errors:






Finally, rviz is even worse xd. I also attach a capture of it:



All the best,

Pablo

[matlabbe]

hector slam doesn't publish the TF, maybe this parameter is missing : https://github.com/introlab/rtabmap_ros/blob/e919f66640fb5ad9cd5335c8cc02e82992703b21/launch/demo/demo_hector_mapping.launch#L33

[palopter]

Hi Mathieu,
I finally solved by adding this lines into the rtabmap node:

   
   
   

    <remap unless="$(arg hector)" from="odom" to="/scanmatch_odom"/>
   

Thank you very much for your help :)