0
$\begingroup$

Rosanswers logo

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.

hector_slam

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"?>
  <launch>
  <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>
    
   <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"/>
</launch>

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 unconnected trees.

And indeed, the tf tree is disconnected:

image description

which wasn't before lauching it (camera_odom_frame and camera_pose_frame were connected). It seems that the tf between camera_odom_frame and camera_pose_frame is lost, although the camera package is still running and publishing the transformation:

---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1591188682
        nsecs: 892184019
      frame_id: "camera_odom_frame"
    child_frame_id: "camera_pose_frame"
    transform: 
      translation: 
        x: -0.000112432426249
        y: -8.70456551638e-06
        z: -6.21268409304e-05
      rotation: 
        x: -9.97320312308e-05
        y: 0.0114294840023
        z: 7.35611829441e-05
        w: 0.999934732914
---

As soon as I kill the hector_slam launched nodes, the transformation shows up again (as in the second picture of this post)

Does anybody know what is going on here??

gmapping

I have also tried gmapping, launched after launching the rplidar and the camera packages with the following launch file:

<launch>

  <arg name="scan_topic" default="scan" />

  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">

    <rosparam>
      odom_frame: camera_odom_frame
      base_frame: camera_pose_frame
      map_frame: map

      map_update_interval: 0.5 # Publish new map

      maxUrange: 5.5 # Should be just less than sensor range
      maxRange: 6.1 # Should be just greater than sensor range
      particles: 100 # Increased from 80

      # Update frequencies
      linearUpdate: 0.3
      angularUpdate: 0.5
      temporalUpdate: 2.0
      resampleThreshold: 0.5

      # Initial Map Size
      xmin: -100.0
      ymin: -100.0
      xmax: 100.0
      ymax: 100.0
      delta: 0.05

      # All default
      sigma: 0.05
      kernelSize: 1
      lstep: 0.05
      astep: 0.05
      iterations: 5
      lsigma: 0.075
      ogain: 3.0
      lskip: 0
      llsamplerange: 0.01
      llsamplestep: 0.01
      lasamplerange: 0.005
      lasamplestep: 0.005

    </rosparam>
    <remap from="scan" to="$(arg scan_topic)"/>
  </node>
</launch>

The tf tree seems to be ok:

image description

However, the node trows the following message:

[DEBUG] [1591185126.577717647]: MessageFilter [target=camera_odom_frame ]: Added message in frame laser at time 1591185126.417, count now 5
[ WARN] [1591185126.729478752]: MessageFilter [target=camera_odom_frame ]: Dropped 100.00% of messages so far. Please turn the [ros.gmapping.message_filter] rosconsole logger to DEBUG for more information.
[DEBUG] [1591185126.745662895]: MessageFilter [target=camera_odom_frame ]: Removed oldest message because buffer is full, count now 5 (frame_id=laser, stamp=1591185125.761198)
[DEBUG] [1591185126.745822135]: MessageFilter [target=camera_odom_frame ]: Added message in frame laser at time 1591185126.577, count now 5

Any ideas how to make it work?

Thanks a lot!!


Originally posted by pgarcia on ROS Answers with karma: 13 on 2020-06-03

Post score: 1


Original comments

Comment by billy on 2020-06-03:
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.

Comment by pgarcia on 2020-06-03:
Great, thanks!

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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

---try---->

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.


Originally posted by Dragonslayer with karma: 574 on 2020-06-03

This answer was ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.