Using obstacles detection nodelet from camera node point cloud

[bbeckvold]

We are using the StereoLabs Zed depth camera and attempting to feed it's generated point cloud into the obstacles detection nodelet. The Zed camera generates the depth cloud directly (using CUDA in our onboard GPU) and then published the PointCloud2 message. Rtabmap can read the image data from the Zed just fine but we need the ground part stripped out with the nodelet.

The nodelet is setup to subscribe to the correct "camera/point_cloud/cloud" topic and it publishes on a new topic to the move_base through "planner_cloud". The problem seems to be that the obstacle_detection nodelet isn't publishing any data at all on the planner_cloud topic. Here is the warning message we are receiving, which looks to be generated by costmap_2d:

[ WARN] [1459553569.486509890]: The /planner_cloud observation buffer has not been updated for 4.04 seconds, and it should be updated every 0.50 seconds.

We believe that the issue is with the nodelet because we can send the point cloud directly to move_base from the camera, but it has the ground piece which is causing navigation problems. We also assume that they need to be in the same process but are not sure of how this might be implemented.

Any help would be appreciated.
-Ben

[matlabbe]

Hi Ben,

Can you record a small rosbag (5-10 sec) with /tf and /camera/point_cloud/cloud topic? Also, are you using rtabmap binaries or latest from source?

cheers,
Mathieu

[bbeckvold]

Hi Mathieu,

Thanks for taking a look! We're building from the latest source.

https://drive.google.com/file/d/0Bw7IHhN8Mz-seVIxUWlKVVpJZWM/view?usp=sharing

[matlabbe]

Hi Ben,

There are three issues with the setup.

1) The frame_id in the point cloud header is /zed_optical_frame, though when shown in RVIZ under fixed frame /base_link, the point cloud top is not on z+ axis and it doesn't look like in front of the robot (maybe the header in the point cloud topic should be /zed_frame instead). If you connect the cloud like that to obstacles_detection nodelet, the left wall would be the ground!


2) The floor is very reflective!!! The ZED disparity algorithm doesn't give the right depth for the floor (note that most disparity algorithms would give similar results, the best is to not have any reflections). See the following images (in the second the points are colored according to down/top axis). The rtabmap_ros/obstacles_detection nodelet cannot work if the ground doesn't look like a plane.


3) For performance issue, the point cloud should be downsampled before connecting it to obstacles_detection nodelet. You can use pcl/VoxelGrid nodelet for convenience (using a leaf of 2.5 or 5 cm could be okay depending on your local costmap resolution).

Note: If the robot is always moving on an even ground (e.g., indoor floor), you may not need obstacles_detection nodelet, see voxel costmap plugin for obstacle layer. EDIT: The voxel plugin is automatically used when PointCloud type is used in "observation_sources" of the obstacle layer (see "Costmap Configuration" example here).

cheers

[bbeckvold]

We did notice that the reflective ground is causing issues, that's why we were looking heavily into the obstacle detection nodelet. Is there any reason for that nodelet to not publish anything?

[matlabbe]

Hi,

I tested obstacles_detection with your bag, and actually /ground_cloud and /obstacles_cloud are published, but at a very low rate (like 30 sec to process only one cloud).

To better test it, I put a VoxelFilter so that obstacles detection is done in ~20 ms:

<launch>

  <node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />

  <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->
  <node pkg="nodelet" type="nodelet" name="voxel_grid" args="load pcl/VoxelGrid pcl_manager" output="screen">
    <remap from="~input" to="/camera/point_cloud/cloud" />
    <rosparam>
      filter_field_name: z
      filter_limit_min: -10
      filter_limit_max: 10
      filter_limit_negative: False
      leaf_size: 0.05
    </rosparam>
  </node>

  <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection pcl_manager" output="screen">
      <remap from="cloud"     to="/voxel_grid/output"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_link"/>		
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="min_cluster_size"     type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.4"/>
  </node>  
</launch>

Output: