Mapping and Navigation with Intel R200 depth camera

[andreacelani]

Hi Mathieu,

I'm trying to activate the obstacle_detection nodelet and fed it to the local costmap but I'm not able to map correctly the input/output of the nodelet.
I have an intel R200 depth camera and visual odometry activated.
Everything is fine with visual odometry, so the sensor and other stuff are working great.
I disabled the depthimage_to_laserscan nodelet and now the /map takes rtabmap/proj_map data

I have modified my local costmap params file to include obstacle_detection points:
***
observation_sources: point_cloud_sensor

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

Then I activated the nodelet in this way on my main .launch file:
***

      <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection nodelet">
         <remap from="cloud" to="rtabmap/cloud_map"/>
         <remap from="obstacles" to="/planner_cloud"/>

         
         
         
         
       </node>
***

But when I start the launch file the obstacle_detection nodelet is disconneted from everything else.

Basically it's a newbie question related to mapping in ROS.
I understand the output mapping, for example:
<remap from="obstacles" to="/planner_cloud"/>  will rename the obstacles stream as /planner_cloud stream.
But it's not clear to me the input remapping like:
<remap from="cloud" to="rtabmap/cloud_map"/> 

I suppose input stream of obstacle_detection called cloud (subscribed topic) is remapped as rtabmap/cloud_map (published topic of rtabmap) or it's the opposite?
I'm missing something.

Moreover I would like to understand the difference between proj_map topic from rtabmap and the obstacles topic from obstacle_detection nodelet.
It's not clear to me they seem very similar in terms of processed data.

Thank you
Andrea

[andreacelani]

Hi,

I successfully adapted the code written for Azimuth3 in order to process in the right way the obstacles and ground detection from obstacles_detection nodelet.

I put this on my main launch file under group "camera":

<group ns="camera">
    <!-- for the planner -->
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"            to="/camera/depth_registered/sw_registered/image_rect_raw"/>
      <remap from="depth/camera_info"      to="/camera/depth/camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="1"/>
      <param name="max_depth"  type="double" value="3.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>
  
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud" to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

      <param name="frame_id"             type="string" value="base_footprint"/>		
      <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>  
  </group>

And this on my planner:
     

<remap from="obstacles_cloud" to="/obstacles_cloud"/>
     <remap from="ground_cloud" to="/ground_cloud"/>
     <remap from="map" to="/map"/>

I also modified the costmap_common_params.yaml file adding the following lines:

observation_sources: point_cloud_sensorA point_cloud_sensorB

  point_cloud_sensorA: {
    sensor_frame: base_footprint,
    data_type: PointCloud2,
    topic: obstacles_cloud,
    expected_update_rate: 0.5,
    marking: true,
    clearing: true,
    min_obstacle_height: 0.04
  }

  point_cloud_sensorB: {
    sensor_frame: base_footprint,
    data_type: PointCloud2,
    topic: ground_cloud,
    expected_update_rate: 0.5,
    marking: false,
    clearing: true,
    min_obstacle_height: -1.0 # make sure the ground is not filtered
  }

Everything seems fine but 9 times over 10 the rtabmap_ros/point_cloud_xyz doesn't output any cloud points. This means no obstacles cloud and no ground cloud and then no global and local costmap.

Actually I see that everything works fine only 2 times. Both I saw the global and local (maybe) costmap were calculated very slowly.

What can I do to fix this strange issue related to rtabmap_ros/point_cloud_xyz and global/local costmap?
I checked the input of nodelet and the camera depth stream is ok.

NB: I didn't throttle the camera depth data and I didn't put any VoxelFilter (like here: https://github.com/introlab/rtabmap_ros/blob/master/launch/azimut3/az3_nav_kinect_odom.launch and here: http://official-rtab-map-forum.206.s1.nabble.com/Using-obstacles-detection-nodelet-from-camera-node-point-cloud-td1191.html#a1196)

Thank you

Andrea

[matlabbe]

Hi,

What is the rate of /camera/cloudXYZ topic? and compare it with the input depth:
­

$ rostopic hz /camera/cloudXYZ
$ rostopic hz /camera/depth_registered/sw_registered/image_rect_raw

As your voxel size is 2 cm, you may increase decimation to 2 or 4 to compute the point cloud a lot faster.

cheers

[matlabbe]

rtabmap node (/proj_map) and obstacles_detection nodelet compute the same thing. They segment the obstacles and ground using the same code under the hood. The "Grid/" parameters can be set to rtabmap node and obstacles_detection nodelet (>=0.11.11). rtabmap node does it only for nodes added to map (at its update rate, default 1 Hz).

$ rosrun rtabmap_ros rtabmap --params | grep Grid/

For "remap" tags in launch file, the "from" parameter is the hard-coded name of the topic set in the node/nodelet and the "to" is to which topic we want to rename/connect it.

[andreacelani]

Thank you for explanation.
Now everything is working fine since I throttled the R200 camera input from 30fps to 10 hz:

<node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle" output="screen">
      <param name="rate" type="double" value="10"/>
      <param name="decimation" type="int" value="1"/>

      <remap from="rgb/image_in"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image_in"     to="/camera/depth_registered/sw_registered/image_rect_raw"/>
      <remap from="rgb/camera_info_in" to="/camera/rgb/camera_info"/>

      <remap from="rgb/image_out"       to="data_throttled_image"/>
      <remap from="depth/image_out"     to="data_throttled_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_throttled_camera_info"/>
    </node> 

I also decimated a little bit the point cloud needed by obstacle detection

<node pkg="nodelet" type="nodelet" name="pcl_manager" args="manager" output="screen" />
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz pcl_manager">
      <remap from="depth/image"            to="/rtabmap/data_throttled_image_depth"/>
      <remap from="depth/camera_info"      to="/rtabmap/data_throttled_camera_info"/>
      <remap from="cloud"                  to="cloudXYZ" />
      <param name="decimation" type="int" value="2"/>
      <param name="max_depth"  type="double" value="4.0"/>
      <param name="voxel_size" type="double" value="0.02"/>
    </node>

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

      <param name="frame_id"             type="string" value="base_footprint"/>		
      <param name="map_frame_id"         type="string" value="map"/>
      <param name="wait_for_transform"   type="bool" value="true"/>
      <param name="ground_normal_angle"  type="double" value="0.785"/>
      <param name="min_cluster_size"     type="int" value="50"/>
      <param name="max_obstacles_height" type="double" value="2.2"/>
      <param name="queue_size"           type="int" value="100"/>
    </node>

I applied data reduction because I noticed that my pc is the bottleneck. When data is too much global costmap and local costmap stuck (even camera depth driver stuck and I have to re-enable the depth camera).

Running htop I noticed that there are two rgbd_odometry processes. Does it is supposed to be correct?

[matlabbe]

You should only have one rgbd_odometry.

[andreacelani]

mmh.. I'll try to understand which launch file starts the second rgbd_odomety