Re: Mapping and Navigation with Intel R200 depth camera

Posted by andreacelani on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Mapping-and-Navigation-with-Intel-R200-depth-camera-tp2583p2584.html

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