Autonomous Navigation with Octomap

[Andrew]

Hello,

We are attempting to integrate an autonomous exploration/navigation package (explore-lite) into our existing root configuration. We have the following setup:

Turtlebot 2
Structure Sensor (Depth Image)
RealSense R200 (RGB Image)
RPLidar (2D planar LIDAR)

Previously, we configured RTABMap with the RPLidar, to produce the 2D occupancy grid for navigation. We found that there were problems with obstacles which were above or below the plane scanned by the LIDAR, which would cause the robot to crash into objects (For instance, a chair with thin legs). We switched to using RTABMap's published octomap_grid, which solved the problems with crashing into obstacles.

However, whenever a loop closure occurs, some of the map is lost, and the robot tends to get "stuck" in a loop of entering an area, exploring, getting a loop closure, losing the map, and re-exploring the same area.

We are looking for a way to preserve the free space of the octomap on loop closure, so that the robot does not get stuck exploring previously explored locations.

Or, is there any alternative that we could use that generates a better grid map for autonomous navigation?

Thanks,
Andrew

[matlabbe]

Hi Andrew,

Which rtabmap version are you using? Latest from source tracks the empty cells and replaces them in OctoMap on loop closure, while previous versions moved only occupied cells (ground, obstacles).

To use less computation power, you may also consider using "grid_map" with Grid/FromDepth=true, possibly with Grid/RayTracing=true and Grid/3D=false to fill empty space. In some cases, empty space clearing (Grid/RayTracing=true) with this kind of map would be less safe than with octomap_grid.

To fill empty space in latest version with "octomap_grid", these parameters should be set: Grid/FromDepth=true and Grid/RayTracing=true. Beware that on loop closure, OctoMap updates can take a
 significant time. To avoid reconstructing the whole OctoMap after each loop closure, parameter GridGlobal/UpdateError (default 0.01 m) can be increased to reconstruct the OctoMap only if the corrected poses have moved more than this value.

Beside the maps, for your problem "We found that there were problems with obstacles which were above or below the plane scanned by the LIDAR, which would cause the robot to crash into objects", you could continue to use the 2D map from RPLidar in rtabmap for exploration, but setup a voxel layer in local costmap of move_base to avoid obstacles that lidar cannot see. That voxel layer would be subscribed from a point cloud generated from the depth image of Structure or R200 sensor. That could be more efficient for exploration as the field of view of RPLidar is a lot larger than the cameras, while being safe for obstacles that lidar cannot see.

#local_costmap_params.yaml
...
observation_sources: point_cloud_sensor

  # assuming receiving a cloud called "obstacles" from rtabmap_ros/obstacles_detection node
  point_cloud_sensor: {
    sensor_frame: base_footprint,
    data_type: PointCloud2, 
    topic: obstacles, 
    expected_update_rate: 0.1, 
    marking: true, 
    clearing: true,
    min_obstacle_height: -99999.0,
    max_obstacle_height: 99999.0}

Such example used in this stereo navigation tutorial.


cheers,
Mathieu

[Andrew]

Thanks for the fast response. I'll download the newest version of RTABMap, and look into adding a Voxel layer to the map.

Thanks for the help!

Andrew

[Andrew]

Hello again,

I have integrated the obstacle_detection node into the system. It works, but we receive the error:

[ERROR] [1523412279.639724606]: Lookup would require extrapolation into the future.  Requested time 1523412279.589668067 but the latest data is at time 1523412279.583657568, when looking up transform from frame [base_footprint] to frame [map]

from the obstacle_detection node.

I tried to increase the queue size on the node, and also throttled the cloud that the node subscribes to, but the error still occurs. Do you have any ideas why this issue might be happening?

I have attached my TF tree below.

Thanks in advance,
Andrew

frames.pdf

[matlabbe]

Hi,

well, map -> odom -> base_footprint tf seems published. How is obstacle_detection configured in the launch file? There is something weird in the tf tree though, the base_footprint transform is 1523411267.933 sec old. Is it running with a rosbag that "rosparam set use_sim_time true" is missing and rosbag doesn't publish "--clock"? (EDIT That is okay, these are static transforms, should not be a problem)

cheers,
Mathieu

[Andrew]

Here is the launch file we are using to launch the obstacle detection. We are throttling the point cloud to 5Hz.

structure_rectify_new.launch

[matlabbe]

Try setting wait_for_transform to true:

<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection structure_register_manager">
   ...
   <param name="wait_for_transform" type="bool" value="true"/>
</node>

cheers,
Mathieu

[Andrew]

I added that parameter to our launch file and tested for a bit, I haven't seen the error come up since, so I think that was the problem.

Thanks for the help,

Andrew