What to do next after using stereo mapping on RTABMAPVIZ?

[thisisyourmom]

Hi, I wanna achive outdoor navigation by using just a stereo, but after building a graphview(proj_map) and a 3D map(point cloud), what should I do next? Do I have to open rtabmapviz and run navigation stack at the same time? Is there a tutorial for this in details?(I have read this http://wiki.ros.org/rtabmap_ros/Tutorials/StereoOutdoorNavigation )

Its buiding map and navigating at the same time?(https://www.youtube.com/watch?v=X885QsH0szo)

P/S I'm using ZED+TX2

[matlabbe]

Hi,

In that video, navigation is done while mapping. You don't need rtabmapviz for navigation, rtabmap node publishes what you need for navigation: Tf /map -> /odom and occupancy grid map "proj_map". Compare with the original figure of this page:
 

In the  outdoor stereo navigation tutorial, the connection between move_base and rtabmap is established with this line of code just before setting up move_base:

<remap from="map" to="/rtabmap/proj_map"/>

You can also do navigation with rtabmap set localization mode. For all navigation stuff, I would refer you to official ros navigation tutorials.

cheers,
Mathieu

[thisisyourmom]

Thanks for your reply Mathieu! I got another question. When I control my robot to move around at 0.2m/s to map a new environment, it was ok at first but I will get this error on RVIZ very often(it works fine on RTABMAPVIZ).



I also get this error [pcl::VoxelGrid::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow. sometimes


My command(based on http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping):

roslaunch zed_wrapper zed_camera.launch publish_tf:=false

rosrun tf static_transform_publisher 0 0 0 -1.5707963267948966 0 -1.5707963267948966 camera_link zed_center 100

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start --Vis/CorFlowMaxLevel 5 --Stereo/MaxDisparity 200 --Odom/Strategy 1 --Odom/GuessMotion true --Vis/EstimationType 1 --Vis/CorType 1" right_image_topic:=/stereo_camera/right/image_rect_color stereo:=true rviz:=true rtabmapviz:=false

P/S ZED+VGA resolution+60 fps

[matlabbe]

Hi,
Maybe it is because of points_xyzrgb nodelet created when activating rviz agurment. See end of rtabmap.launch. You could remove the voxel_size parameter of the points_xyzrgb. Note that this nodelet is just to see the live point cloud from the camera in rviz.

The "not enough inliers" mean odometry is lost. See Lost odometry section on this page.

Cheers,
Mathieu

[thisisyourmom]

Yea I know it was the lost odometry. But it works ok on RTABMAPVIZ (same environment and same parameters). I only received these errors very often on RVIZ and have no idea why.

Are there any parameters that I should add to the launch file? Better use stereo_mapping.launch than rtabmap.launch?

[matlabbe]

rtabmapviz and rviz are independent of the mapping process, they should not affect the performance directly. Indirectly, they can use some CPU time that odometry could have used to estimate faster (thus getting less lost). I can see that odometry update time seems slow (about ~130/140 ms), you may compare this time between using rviz or rtabmapviz. If with rviz the update time is higher, then you may kill points_xyzrgb node to save some CPU time. You may not launch rtabmapviz or rviz on the TX2 to save CPU usage, but on a remote computer.

stereo_mapping.launch is using directly rtabmap.launch in last versions, stereo_mapping.launch is still there for backward compatibility. Normally, you would use rtabmap.launch directly with stereo:=true.

cheers,
Mathieu

[thisisyourmom]

It works better now...

BTW, I got another problem, the tf axes in RVIZ seem weird to me because my ZED cam is mounted flat on my robot, like how you mounted bumblebee2 on your azimut3 robot, but the weirdest thing is it seems ok in RTABMAPVIZ.






TF:


My commands:
roslaunch zed_wrapper zed_rtab.launch

rosrun tf static_transform_publisher 0.3 0 0.8 -1.5707963267948966 0 -1.5707963267948966 base_link zed_ceer 100

roslaunch rtabmap_ros rtabmap_modified.launch rtabmap_args:="--delete_db_on_start --Vis/CorFlowMaxLevel 5 --Stereo/MaxDisparity 200 --Odom/Strategy 1 --Odom/GuessMotion true --Vis/EstimationType 1 --Vis/CorType 1 --Odom/ResetCountdown 10" right_image_topic:=/stereo_camera/right/image_rect_color stereo:=true rviz:=true rtabmapviz:=false


rtabmap.launch

<launch>
   
       
  <arg name="stereo"          default="false"/>
 
 
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
 
 
  <arg name="localization"            default="false"/>
 
 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
 
       
  <arg name="frame_id"      default="base_link"/>
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="rtabmap_args"            default=""/>             
  <arg name="launch_prefix"           default=""/>             
 
 
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
   
 
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
 
 
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />     
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
 
  <arg name="compressed"              default="false"/>         
  <arg name="rgb_image_transport"     default="compressed"/>   
  <arg name="depth_image_transport"   default="compressedDepth"/> 
   
  <arg name="subscribe_scan"          default="false"/>
  <arg name="scan_topic"              default="/scan"/>
 
  <arg name="subscribe_scan_cloud"    default="false"/>
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
   
  <arg name="visual_odometry"         default="true"/>         
  <arg name="odom_topic"              default="/odom"/>         
  <arg name="odom_frame_id"           default="odom"/>             
  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
 
 
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
  <arg unless="$(arg compressed)" name="rgb_topic_relay"           default="$(arg rgb_topic)"/>
  <arg if="$(arg compressed)"     name="depth_topic_relay"         default="$(arg depth_topic)_relay"/>
  <arg unless="$(arg compressed)" name="depth_topic_relay"         default="$(arg depth_topic)"/>
  <arg if="$(arg compressed)"     name="left_image_topic_relay"    default="$(arg left_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="left_image_topic_relay"    default="$(arg left_image_topic)"/>
  <arg if="$(arg compressed)"     name="right_image_topic_relay"   default="$(arg right_image_topic)_relay"/>
  <arg unless="$(arg compressed)" name="right_image_topic_relay"   default="$(arg right_image_topic)"/>

 
  <group ns="$(arg namespace)">
 
   
    <group unless="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
      <node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
 
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
        <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
        <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
         
       
       
       
       
       
      </node>
    </group>
   
   
    <group if="$(arg stereo)">
      <node if="$(arg compressed)" name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
      <node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
 
      <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
        <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
        <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
        <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
        <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
         
       
       
       
       
       
      </node>
    </group>
 
   
   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
     
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
     
     
         
         
         
    </node>
 
   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
     
     
     
     
     
     
     
     
     
     
     
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="left/image_rect"        to="$(arg left_image_topic_relay)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic_relay)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap from="scan_cloud"             to="$(arg scan_cloud_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
 
  </group>
 
 
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="left/image"        to="$(arg left_image_topic_relay)"/>
    <remap from="right/image"       to="$(arg right_image_topic_relay)"/>
    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap from="rgb/image"         to="$(arg rgb_topic_relay)"/>
    <remap from="depth/image"       to="$(arg depth_topic_relay)"/>
    <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
    <remap from="cloud"             to="voxel_cloud" />

   
   
  </node>
</launch>


zed_rtab.launch

<launch>
    <arg name="svo_file"              default="" />
    <arg name="zed_id"                default="0" />
   
    <arg name="gpu_id"                default="-1" />
   
    <arg name="publish_tf"            default="false" />
    <arg name="odometry_frame"        default="odom" />
    <arg name="base_frame"            default="zed_center" />
    <arg name="camera_frame"          default="zed_left_camera" />
    <arg name="depth_frame"           default="zed_depth_camera" />
   
    <arg name="publish_urdf"          default="true" />

    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen">

       
       
           
       
       
       
       
   
       
       

       
       
       
       
       
       
       
       
       
       
       

       
       
       
       
       
       
       
       
       
       
       
       
       
       
       
       
       

    </node>
       
   
    <group if="$(arg publish_urdf)">
       
        <node name="zed_state_publisher" pkg="robot_state_publisher" type="state_publisher">
            <remap from="robot_description" to="zed_description" />
        </node>
    </group>

</launch>

[matlabbe]

The frame you are seeing in rtabmapviz is the base_link frame. To see the camera position according to base_link, you can right-click->Show frustum.

Are the point clouds correctly oriented in RVIZ and rtabmapviz? Based on this tutorial, zed_center frame would be already in correct orientation so you don't have to do the rotation between base_link and zed_center in the static transform publisher.

cheers,
Mathieu

[thisisyourmom]

The pointclouds are shown correctly, when I move ZED to right and left the pointclouds will connect to the previous one os nothing is wrong but the tf axes seem weird to me. I followed this http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping

If I dont do the rotation between zed_center and base_link, the cloud map will be shown upwards...

P/S my robot base is in parallel with ZED

[matlabbe]

Ah yes in the stereo tutorial there is a static transform publisher to rotate zed_center. I don't have my zed camera right know so I cannot test if it happens also with the zed ros wrapper version I have. I've put a note to check this in a week.

[thisisyourmom]

Thanks for your reply Mathieu! I will keep trying to see what I can do, also I wish you a Happy New Year

Please let me know if you test it with ZED :)

[matlabbe]

Hi,

I reproduced the TF bug. However, the mapping doesn't affected.

RGB-D topics:


Stereo topics:


I think this is because there is missing a rotation between zed_center and zed_left_camera frames (so we would not have to rotate zed_center):

rosrun tf tf_echo zed_center zed_left_camera
At time 0.000
- Translation: [0.000, 0.060, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

On RGB-D version, rgb image (which is also the left image) is set to zed_depth_camera, which includes the optical rotation:

rosrun tf tf_echo zed_center zed_depth_camera
At time 0.000
- Translation: [0.000, 0.060, 0.000]
- Rotation: in Quaternion [-0.500, 0.500, -0.500, 0.500]
            in RPY (radian) [-1.571, -0.000, -1.571]
            in RPY (degree) [-90.000, -0.000, -90.000]

Happy New Year too!

[thisisyourmom]

Thanks for testing this! Ive got another problem now...

I have no idea why there is nothing being publish by planner_cloud so I have to replace /rtabmap/cloud_obstacles in
costmap_common_params.yaml

obstacle_range: 2.5
raytrace_range: 3.0
robot_radius: 0.3 # distance a circular robot should be clear of the obstacle
inflation_radius: 0.45
transform_tolerance: 2

observation_sources: point_cloud_sensor

#assuming receiving a cloud from rtabmap_ros/obstacles_detection node
point_cloud_sensor: { 
sensor_frame: base_link, 
data_type: PointCloud2, 
topic: <b>/rtabmap/cloud_obstacles</b>,
marking: true, 
clearing: true,
min_obstacle_height: -99999.0,
max_obstacle_height: 99999.0
}

nav.launch

<?xml version="1.0"?>
<launch>
  <group ns="planner">
      <remap from="openni_points" to="/planner_cloud"/>
      <remap from="map" to="/rtabmap/grid_map"/>
      <remap from="move_base_simple/goal" to="/planner_goal"/>
        
      <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
         <rosparam file="$(find xxx_Package)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
		 <rosparam file="$(find xxx_Package)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
         <rosparam file="$(find xxx_Package)/config/local_costmap_params.yaml" command="load" />
         <rosparam file="$(find xxx_Package)/config/global_costmap_params.yaml" command="load" />
         <rosparam file="$(find xxx_Package)/config/base_local_planner_params.yaml" command="load" />
      </node>
  </group>
   
  <group ns="/stereo_camera" >
		<!-- Create point cloud for the planner -->
		<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="standalone rtabmap_ros/point_cloud_xyz">
			<remap from="disparity/image"       to="disparity"/>
			<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
			<remap from="cloud"                 to="cloudXYZ"/>
			
			<param name="voxel_size" type="double" value="0.05"/>
			<param name="decimation" type="int" value="4"/>
			<param name="max_depth" type="double" value="4"/>
			
		</node>
		<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="standalone rtabmap_ros/obstacles_detection">
    		<remap from="cloud" to="cloudXYZ"/>
			<remap from="obstacles" to="/planner_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="Grid/MinClusterSize" type="int" value="20"/>
			<param name="Grid/MaxObstacleHeight" type="double" value="0.0"/>
  		</node>
  </group>
</launch>

Also planner_goal is not working in RVIZ too when using 2D Nav Goal arrow, so I have to use rostopic pub to give a goal to movebase... No idea why the remapping is not working... Have I done anything wrong in the launch file?

[matlabbe]

Hi,

You should not use "/rtabmap/cloud_obstacles" topic coming from rtabmap node in move_base's local costmap. It should be the "/planner_cloud" published by rtabmap_ros/obstacles_detection nodelet. To know why /planner_cloud is not published, look if the input cloud "cloudXYZ" of obstacles_detection is also published. In this example, the cloud is created from the disparity image of the stereo camera. With the zed camera, you may subscribe to depth image if disparity is not available. If you have only left and right images, using rtabmap_ros/point_cloud_xyzrgb nodelet can generate a cloud for convenience:

<node pkg="nodelet" type="nodelet" name="depth2cloud" args="standalone rtabmap_ros/point_cloud_xyz">
         <remap from="depth/image"         to="/stereo_camera/depth/image"/>
         <remap from="depth/camera_info"   to="/stereo_camera/depth/camera_info"/>

         <remap from="cloud"               to="cloudXYZ"/>
         
         <param name="approx_sync" type="bool" value="false"/>
         <param name="voxel_size" type="double" value="0.05"/>
         <param name="decimation" type="int" value="4"/>
         <param name="max_depth" type="double" value="4"/>
      </node>

//or

<node pkg="nodelet" type="nodelet" name="stereo2cloud" args="standalone rtabmap_ros/point_cloud_xyzrgb">
         <remap from="left/image"          to="/stereo_camera/left/image_rect_color"/>
         <remap from="right/image"         to="/stereo_camera/right/image_rect_color"/>
         <remap from="left/camera_info"    to="/stereo_camera/left/camera_info"/>
         <remap from="right/camera_info"   to="/stereo_camera/right/camera_info"/>

         <remap from="cloud"               to="cloudXYZ"/>
         
         <param name="approx_sync" type="bool" value="false"/>
         <param name="voxel_size" type="double" value="0.05"/>
         <param name="decimation" type="int" value="4"/>
         <param name="max_depth" type="double" value="4"/>
      </node>

For the goal, "/planner_goal" is a remap of "planner/move_base_simple/goal" in that example. It is the goal you usualy send directly to move_base. To avoid confusion and use the standard 2D nav goal of move_base, just don't remap it. Also, be aware of the namespaces used in the launch file, that can change the global service name.

EDIT:"/rtabmap/cloud_obstacles" (from rtabmap node published at 1 Hz) should not be used in local costmap because the local costmap should be independent from the mapping process. It should be updated fast and with sensor data directly. Here the point_cloud_xyz and obstacles_detection nodelets take the raw point cloud from the camera, filter and downsample it, then segment ground and obstacles to help local costmap doing correctly the ray tracing.


cheers,
Mathieu

[thisisyourmom]

It works fine now! :)

BTW I have met 3 major problems now:

1) Sometimes there are shadows and skynoises projected on the grid_map while mapping, are there any ways to edit the map like using paint software to change the obstacle areas to free area so that the robot can make a path through it? (I have tried this in gmapping and it worked!)

2) How movebase receive the current vx and vtheta when we replace the RTAB VO with robot base odom(not publishing robot base odom->map and odom.odom.x ... )?


3) These errors happen a lot when I send a goal to move base to navigate the robot, and the local costmap does not rotate to align the static map... Any parameters to tune to improve the performace?

costmap_common_params.yaml:
obstacle_range: 2.5
raytrace_range: 3.0
#footprint: [[0.15, 0.1], [-0.15, 0.1], [-0.15, -0.1], [0.15, -0.1]] 
robot_radius: 0.3 # distance a circular robot should be clear of the obstacle
inflation_radius: 0.35
transform_tolerance: 2
controller_patience: 2.0

NavfnROS:
    allow_unknown: true
    track_unknown_space: true
     
observation_sources: point_cloud_sensor
# assuming receiving a cloud from rtabmap_ros/obstacles_detection node
point_cloud_sensor: {
  sensor_frame: base_link,
  data_type: PointCloud2,
  topic: /planner_cloud,
  marking: true,
  clearing: true,
  min_obstacle_height: -99999.0,
  max_obstacle_height: 99999.0}

global_costmap_params.yaml:
global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 0.5
  publish_frequency: 0.5
  static_map: true

<b>local_costmap_params.yaml:</b>
local_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 2.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 3.0
  height: 3.0
  resolution: 0.05

base_local_planner_params.yaml:
TrajectoryPlannerROS:
  max_vel_x: 0.30
  min_vel_x: 0.13
  max_vel_theta: 0.3
  min_in_place_vel_theta: 0.15

  acc_lim_theta: 1.6
  acc_lim_x: 1.5
  acc_lim_y: 1.5

  holonomic_robot: false
  
  xy_goal_tolerance:  0.20
  yaw_goal_tolerance: 0.20
  
  meter_scoring: true

[ WARN] [1515085166.431188179]: Control loop missed its desired rate of 20.0000Hz... the loop actually took 0.0739 seconds
[ WARN] [1515085166.499858583]: Control loop missed its desired rate of 20.0000Hz... the loop actually took 0.0925 seconds
	
---------------------------------

[ WARN] [1515085532.317085895]: Rotate recovery behavior started.
[ERROR] [1515085532.317439636]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ERROR] [1515085532.467114367]: Aborting because a valid control could not be found. Even after executing all recovery behaviors

[matlabbe]

Hi,

1) There is no way right now to manually edit the occupancy grid map created by rtabmap. You should use Grid/ parameters to try to filter most of the noise at the source. The same parameters can be used for obstacles_detection node (for local move_base's costmap) and rtabmap node (for "static" global move_base's costmap).

$ rosrun rtabmap_ros rtabmap --params | grep Grid/
Param: Grid/3D = "true"                                    [A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and "Grid/FromDepth" is false.]
Param: Grid/CellSize = "0.05"                              [Resolution of the occupancy grid.]
Param: Grid/ClusterRadius = "0.1"                          [[Grid/NormalsSegmentation=true] Cluster maximum radius.]
Param: Grid/DepthDecimation = "4"                          [[Grid/DepthDecimation=true] Decimation of the depth image before creating cloud. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).]
Param: Grid/DepthMax = "4.0"                               [[Grid/FromDepth=true] Maximum cloud's depth from sensor. 0=inf.]
Param: Grid/DepthMin = "0.0"                               [[Grid/FromDepth=true] Minimum cloud's depth from sensor.]
Param: Grid/DepthRoiRatios = "0.0 0.0 0.0 0.0"             [[Grid/FromDepth=true] Region of interest ratios [left, right, top, bottom].]
Param: Grid/FlatObstacleDetected = "true"                  [[Grid/NormalsSegmentation=true] Flat obstacles detected.]
Param: Grid/FootprintHeight = "0.0"                        [Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.]
Param: Grid/FootprintLength = "0.0"                        [Footprint length used to filter points over the footprint of the robot.]
Param: Grid/FootprintWidth = "0.0"                         [Footprint width used to filter points over the footprint of the robot. Footprint length should be set.]
Param: Grid/FromDepth = "true"                             [Create occupancy grid from depth image(s), otherwise it is created from laser scan.]
Param: Grid/GroundIsObstacle = "false"                     [[Grid/3D=true] Ground segmentation (Grid/NormalsSegmentation) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).]
Param: Grid/MapFrameProjection = "false"                   [Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.]
Param: Grid/MaxGroundAngle = "45"                          [[Grid/NormalsSegmentation=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.]
Param: Grid/MaxGroundHeight = "0.0"                        [Maximum ground height (0=disabled). Should be set if "Grid/NormalsSegmentation" is true.]
Param: Grid/MaxObstacleHeight = "0.0"                      [Maximum obstacles height (0=disabled).]
Param: Grid/MinClusterSize = "10"                          [[Grid/NormalsSegmentation=true] Minimum cluster size to project the points.]
Param: Grid/MinGroundHeight = "0.0"                        [Minimum ground height (0=disabled).]
Param: Grid/NoiseFilteringMinNeighbors = "5"               [Noise filtering minimum neighbors.]
Param: Grid/NoiseFilteringRadius = "0.0"                   [Noise filtering radius (0=disabled). Done after segmentation.]
Param: Grid/NormalK = "20"                                 [[Grid/NormalsSegmentation=true] K neighbors to compute normals.]
Param: Grid/NormalsSegmentation = "true"                   [Segment ground from obstacles using point normals, otherwise a fast passthrough is used.]
Param: Grid/ProjRayTracing = "true"                        [[Grid/3D=false] 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.]
Param: Grid/Scan2dMaxFilledRange = "4.0"                   [Unknown space filled maximum range. If 0, the laser scan maximum range is used.]
Param: Grid/Scan2dUnknownSpaceFilled = "false"             [Unknown space filled. Only used with 2D laser scans.]
Param: Grid/ScanDecimation = "1"                           [[Grid/FromDepth=false] Decimation of the laser scan before creating cloud.]

For example, assuming that the camera is parallel to ground looking forward and you want only points over 5 cm and under 40 cm to be used for obstacles, set Grid/NormalsSegmentation=false, Grid/MaxGroundHeight=0.05 and Grid/MaxObstacleHeight=0.4. To filter spurious points, you can activate the noise filtering by tuning Grid/NoiseFilteringRadius. Grid/ProjRayTracing can be nice to fill unknown spaces (and possibly clear some spurious obstacles), but can be used only if Grid/3D is false.

2) I think this topic is optional, as move_base uses TF to get position of the robot. rgbd_odometry or stereo_odometry will set velocity in Odometry.twist. If you want to use robot's odometry, then the node publishing it should set the twist itself. It should also publish corresponding TF (/odom -> /base_link). Note that this the job of the mapping/localization node to publish /map->/odom (it this case rtabmap). See REP105 for description of some standard frames.

3) move_base should only be used with 2d robots. You may want to force rtabmap's nodes to publish transforms in 2d (or in 3 degrees-of-freedom) by setting "Reg/Force3DoF=true" parameters for stereo_odometry/rgbd_odometry/rtabmap nodes so that current odometry and map correction don't have roll/pitch/z values.

cheers,
Mathieu

[thisisyourmom]

I will try tuning first and then let you know about the results!

By the way, I have realized that it wont avoid moving objects like people passing by in real time, and I wonder the stereo is only avoiding non free spaces. Do I need to add another sensors like lidar/sonars to the obstacle layer to avoid moving objects?

[matlabbe]

Hi,

To avoid "moving objects like people passing by in real time", this is the job of move_base's local costmap. If actual sensor data sent to local costmap cannot "see" these objects, it may have collisions. If you have a LiDAR or some other sensors that can detect these objects, they can be easily added to local costmap's "observation_sources" (by adding new sensor with right type). See this page for example of LaserScan type and a PointCloud type.

I think however that the stereo camera should be able to detect people though. You can compare the raw point cloud from the stereo camera with the one filtered with obstacles_detection to see if obstacles_detection filters a little too much.

cheers,
Mathieu

[thisisyourmom]

My latest rtabmap.launch:
     

...
      <param name="Reg/Force3DoF"    value="true" />
      <param name="Grid/3D"                         type="bool" value="false"/>
      <param name="Grid/NoiseFilteringRadius"       type="double" value="1.10"/>
      <param name="Grid/NoiseFilteringMinNeighbors" type="int" value="2"/>
      <param name="Grid/MaxGroundHeight"            type="double" value="0.05"/>
      <param name="Grid/MaxObstacleHeight"          type="double" value="0.4"/>
      ...

1) Using raw points means using the "clouds" topic instead of "obstacles" topic in rtabmap obstacle nodelet?
 
2) By the way what the colors represent?


3) How do I change this parameters in launch file? NodeFilterRadius and NodeFilterAngle?


4) Sometimes my robot's tf will "fly up" and I have no idea why...


5) Could this be ZED being too shaky while running on a uneven surface?

[thisisyourmom]

Hi Mathieu, have you forgotten my questions?