Ask Your Question

Very low rate of obstacles_detection rtabmap

asked 2019-06-25 06:12:00 -0500

EdwardNur gravatar image

updated 2019-06-25 06:43:30 -0500

gvdhoorn gravatar image

I am using the standalone nodelet obstacles_detection to get the obstacles from my stereo camera. When I run it, I get a slow rate of < 1 Hz and for the real-time obstacle detection that is not suitable. How can I speed up the process? Here is how I launched that nodelet:

<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet"-->
   <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="standalone rtabmap_ros/obstacles_detection">
      <remap from="cloud" to="/zed/zed_node/point_cloud/cloud_registered"/>
      <remap from="obstacles" to="/planner_cloud"/>

      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="map_frame_id" type="string" value="map"/>
      <param name="min_cluster_size" type="int" value="20"/>
      <param name="max_obstacles_height" type="double" value="0.0"/>
edit retag flag offensive close merge delete


I have turned on the "disparity2cloud" nodelet and supplied the disparity images with the regular camera_info but for some reason the cloudXYZ from that nodelet comes in a strange format (data is symmetrical) and the /planner_cloud data is empty

EdwardNur gravatar image EdwardNur  ( 2019-06-25 07:02:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2019-06-25 10:51:33 -0500

matlabbe gravatar image

I suggest to downsample the point clouds before feeding it to obstacles_detection. See pcl_ros's VoxelGrid filter for example:, try with a leaf size of 5 cm.

Another way is to use rtabmap_ros/point_cloud_xyz nodelet to convert depth image to point cloud using decimation for fast downsampling.

<node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_ros/point_cloud_xyz">
   <remap from="depth/image"         to="/zed/zed_node/depth/image"/>
   <remap from="depth/camera_info"   to="/zed/zed_node/depth/camera_info"/>
   <remap from="cloud"               to="voxel_cloud" />
   <param name="decimation"  type="double" value="4"/>
   <param name="voxel_size"  type="double" value="0.05"/>
   <param name="approx_sync" type="bool" value="false"/>
edit flag offensive delete link more


@matlabbe cheers, I will try it

EdwardNur gravatar image EdwardNur  ( 2019-06-25 10:52:55 -0500 )edit

@matlabbe yes it works but the question is, can I use the depth cloud produced by the rtabmap instead of my own cloud? I have used /rtabmap/cloud_map and I can see that obstacles_detection node subscribes to that but gives no data to /planner_cloud topic even though /rtabmap/cloud_map is not empty

EdwardNur gravatar image EdwardNur  ( 2019-07-09 05:39:41 -0500 )edit

I don't recommend passing /rtabmap/cloud_map to obstacles_detection, as you could use directly /rtabmap/cloud_obstacles. However, if you feed those obstacles to obstacle_layer from costmap_2d, you should use the voxel cloud of the answer above as input to obstacles_detection, then feed the resulting obstacles to obstacle_layer.

matlabbe gravatar image matlabbe  ( 2019-07-09 09:08:07 -0500 )edit

@matlabbe sorry, I did not follow your suggestion. Do you mean it is better to use /rtabmap/cloud_obstacles inside the obstacle_layer and use the output of the obstacle_layer (which is /planner_cloud) to the local_costmap_param?

EdwardNur gravatar image EdwardNur  ( 2019-07-09 14:01:32 -0500 )edit

zed_node -> (depth image) -> point_cloud_xyz -> (voxel cloud) -> obstacles_detection -> (obstacles cloud, which you call /planner_cloud) -> obstacle_layer

matlabbe gravatar image matlabbe  ( 2019-07-09 15:12:49 -0500 )edit

@matlabbe Yes I get that, this was my initial setup, I am just trying to avoid using the depth image

EdwardNur gravatar image EdwardNur  ( 2019-07-10 01:51:51 -0500 )edit

Not sure why you would want to avoid the depth image. Inputs to local costmap should be the sensor (or obstacles derived directly from sensor) directly, not the map from rtabmap.

matlabbe gravatar image matlabbe  ( 2019-07-10 11:44:58 -0500 )edit

@matlabbe I think you misunderstood. I do not want to avoid the depth image, I just want to avoid the depth image produced by ZED and use the depth image (or PCL) produced by RTABmap algorithm.

EdwardNur gravatar image EdwardNur  ( 2019-07-11 11:08:20 -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: 2019-06-25 06:12:00 -0500

Seen: 229 times

Last updated: Jun 25 '19