Ask Your Question

hector_slam/gmapping with T265 (odometry) and RPLidar

asked 2020-06-03 08:03:32 -0500

pgarcia gravatar image

updated 2020-06-03 11:32:21 -0500

Hello everybody

I am trying to make SLAM using a 2D laser scanner (RPLidar A1) supporting it with a Realsense camera T265, which provides an accurate odometry.

The arrangement of both devices is as follows:

image description

The realsense ROS package provides two main frames: camera_odom_frame and camera_pose_frame. This last one is the one that moves around when the camera moves through the space. This is the tf tree created by the camera package:

image description

I have tried to use these two devices with two SLAM packages: hector_slam and gmapping.


This package works well when carrying out SLAM without odometry. My problem comes when I try to include the odometry information. After launching the LIDAR and the camera packages, I launch hector_slam using the following launch file:

<?xml version="1.0"?>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>

  <!-- <arg name="base_frame" default="base_footprint"/> -->
  <arg name="base_frame" default="base_link"/>

  <!-- <arg name="odom_frame" default="nav"/> -->
  <arg name="odom_frame" default="camera_odom_frame"/>

  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="scan"/>
  <arg name="map_size" default="2048"/>

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />

    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>

    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />

    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />    
    <param name="map_update_distance_thresh" value="0.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />

    <!-- Advertising config --> 
    <param name="advertise_map_service" value="true"/>

    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>

    <!-- Debug parameters -->
      <param name="output_timing" value="false"/>
      <param name="pub_drawings" value="true"/>
      <param name="pub_debug_output" value="true"/>
    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />

   <node pkg="tf" type="static_transform_publisher" name="map_to_camera_odom_broadcaster" args="0 0 0 0 0 0 map camera_odom_frame 100"/>
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera_broadcaster" args="0 0.05 -0.10 3.1415/2 0 0 base_link camera_pose_frame 100"/>
   <node pkg="tf" type="static_transform_publisher" name="base_to_laser_broadcaster" args="0 0 0 0 0 0 base_link laser 100"/>

Note the tf nodes launched at the end of the file. This launcher throws this error:

[ERROR] [1591184110.747014370]: Transform failed during publishing of map_odom transform: Could not find a connection between 'camera_odom_frame' and 'base_link' because they are not part of the same tree.Tf has two or more ...
edit retag flag offensive close merge delete


no access to your google drive to see TF trees - that may be an issue on my side but I've never run into before using these forums.

I upvoted the post so you may now have the karma needed to post TF trees directly.

billy gravatar image billy  ( 2020-06-03 11:02:40 -0500 )edit

Great, thanks!

pgarcia gravatar image pgarcia  ( 2020-06-03 11:32:41 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted

answered 2020-06-03 12:09:47 -0500

Dragonslayer gravatar image

updated 2020-06-03 12:27:25 -0500

Hector: You pass, thus create, base_link in the hector launchfile, but it should likely be the camera_base_frame.

<arg name="base_frame" default="base_link"/> ---try----> <arg name="base_frame" default="camera_base_frame"/>

I would also suggest, looking into changing the camera specific odom and base frame-IDs to more conventional (for the time beeing maybe even dummy ones with Zero static transform publishers) odom and base_link. Because if you develop further you might actually introduce a "real" robot base_link (thats not equal to camera_base) and sensorfusion-odometry. hector gives odometry as well, might make sense to fuse that with the camera odom in future upgrades.

edit flag offensive delete link more

answered 2020-06-04 02:50:55 -0500

pgarcia gravatar image

Thank you very much for the answer.

As you suggest, replacing base_link by camera_pose_frame makes it work.

Additionally, I have realized that actually the disconnection problem is that in the definition of the tf tree, it is important the definition parent->child, that is, the direction of the arrows (which I didn't know before). The tf tree cannot be defined with a frame with two inbound arrows. Changing the 2nd last tf in the launch file to this:

   <node pkg="tf" type="static_transform_publisher" name="base_to_camera_broadcaster" args="0 -0.05 0.10 -3.1415/2 0 0 camera_pose_frame base_link 100"/>

Now the base_link is the child of camera_pose_frame (not the other way around), and the tf between camera_odom_frame and camera_pose_frame does not break:

image description

Everything is well connected.


edit flag offensive delete link more



args="0 -0.05 0.10 -3.1415/2

does that /2 really work?

gvdhoorn gravatar image gvdhoorn  ( 2020-06-04 03:10:57 -0500 )edit

apparently, it does ;)

pgarcia gravatar image pgarcia  ( 2020-06-04 03:23:39 -0500 )edit

Glad that I could slay your dragon ;)

Dragonslayer gravatar image Dragonslayer  ( 2020-06-04 11:03:04 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2020-06-03 08:03:32 -0500

Seen: 642 times

Last updated: Jun 04 '20