Move_base with rtabmap

[Alexia]

Hello

I ran into a problem while stereo mapping using rtabmap and move_base. The main problem is the grid map and the cost maps.
Neither of the maps clears old obstacles when they have been removed from the field of view.
The projection of the pointcloud to the grid map is very noisy, is thee a filter to use with a stereo camera?
If a new obstacle appears while the camera is still the obstacle wont be mapped until the camera starts moving again. Is there a way to fix this?

Best Regards

Files used:
costmap_common_params:

obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[ 0.3,  0.3], [-0.3,  0.3], [-0.3, -0.3], [ 0.3, -0.3]]
footprint_padding: 0.03
#robot_radius: ir_of_robot
inflation_radius: 0.1
transform_tolerance: 1.0

recovery_behaviors: [
    {name: conservative_clear, type: clear_costmap_recovery/ClearCostmapRecovery},
    {name: aggressive_clear, type: clear_costmap_recovery/ClearCostmapRecovery}
]

conservative_clear: 
    reset_distance: 3.00
aggressive_clear:
    reset_distance: 1.84

globa_costmap_params:

global_costmap:
  global_frame: map
  robot_base_frame: base_link
  update_frequency: 0.5
  publish_frequency: 0.5
  static_map: true

local_costmap_params:

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: 4.0
  height: 4.0
  resolution: 0.025
  origin_x: -2.0
  origin_y: -2.0

  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: rtabmap/cloud_obstacles, 
    expected_update_rate: 0.5, 
    marking: true, 
    clearing: true,
    min_obstacle_height: -99999.0,
    max_obstacle_height: 99999.0}

move_base:

<launch>


<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="base_global_planner" value="navfn/NavfnROS"/> 
<param name="base_local_planner"  value="dwa_local_planner/DWAPlannerROS"/>
<rosparam file="$(find oscar_control)/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find oscar_control)/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find oscar_control)/local_costmap_params.yaml" command="load" />
<rosparam file="$(find oscar_control)/global_costmap_params.yaml" command="load" />
<rosparam file="$(find oscar_control)/base_local_planner_params.yaml" command="load" />
<!--<remap from="/odom" to="/odometry/filtered/local" />
<param name="base_local_planner"  value="base_local_planner/TrajectoryPlannerROS"/>
-->

<remap from="odom" to="/rtabmap/odom" />
<remap from="map"   to="/rtabmap/grid_map"/>

</node>


</launch>

stereo_mapping:

<launch>
   <!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->

   <!-- Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="zed_initial_frame"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default=""/>   <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>
  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the input topics are not synchronized -->
  
  <arg name="stereo_namespace"        default="/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" />      <!-- using grayscale image for efficiency -->
  <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="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
   
  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  
  <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
  
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
  
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="stereo"                  value="true"/>
    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
    <arg name="rviz"                    value="$(arg rviz)" />
    <arg name="localization"            value="$(arg localization)"/>
    <arg name="gui_cfg"                 value="$(arg rtabmapviz_cfg)" />
    <arg name="rviz_cfg"                value="$(arg rviz_cfg)" />
  
    <arg name="frame_id"                value="$(arg frame_id)"/>
    <arg name="namespace"               value="$(arg namespace)"/>
    <arg name="database_path"           value="$(arg database_path)"/>
    <arg name="wait_for_transform"      value="$(arg wait_for_transform)"/>
    <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>  
    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
    <arg name="approx_sync"             value="$(arg approx_sync)"/>

    <arg name="stereo_namespace"        value="$(arg stereo_namespace)"/>
    <arg name="left_image_topic"        value="$(arg left_image_topic)" />
    <arg name="right_image_topic"       value="$(arg right_image_topic)" />
    <arg name="left_camera_info_topic"  value="$(arg left_camera_info_topic)" />
    <arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
  
    <arg name="compressed"              value="$(arg compressed)"/>                                                                                
   
    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
    <arg name="scan_topic"              value="$(arg scan_topic)"/>
  
    <arg name="subscribe_scan_cloud"    value="$(arg subscribe_scan_cloud)"/>
    <arg name="scan_cloud_topic"        value="$(arg scan_cloud_topic)"/>
   
    <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
    <arg name="odom_topic"              value="$(arg odom_topic)"/>       
    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>   
    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
  </include>

</launch>

rtabmap:

<launch>
  <!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->

  <!-- For stereo:=false
        Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->
           
  <!-- For stereo:=true
        Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
     
  <!-- Choose between RGB-D and stereo -->      
  <arg name="stereo"          default="true"/>
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="false" /> 
  <arg name="rviz"                    default="true" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- Corresponding config files -->
  <arg name="cfg"                     default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
  <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"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="odom_frame_id"           default=""/>                <!-- If set, TF is used to get odometry instead of the topic -->
  <arg name="map_frame_id"            default="map"/>
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="30"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="rtabmap_args"            default="$(arg args)"/>   <!-- deprecated, use "args" argument -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
  
  <!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
  <arg     if="$(arg stereo)" name="approx_sync"  default="false"/>
  <arg unless="$(arg stereo)" name="approx_sync"  default="true"/>         
   
  <!-- RGB-D related topics -->
  <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" />
  
  <!-- stereo related topics -->
  <arg name="stereo_namespace"        default="/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" />      <!-- using grayscale image for efficiency -->
  <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"/>         <!-- If you want to subscribe to compressed image topics -->
  <arg name="rgb_image_transport"     default="compressed"/>    <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
  <arg name="depth_image_transport"   default="compressedDepth"/>  <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
   
  <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"/>          <!-- Launch rtabmap visual odometry node -->
  <arg name="odom_topic"               default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="vo_frame_id"              default="odom"/>
  <arg name="odom_tf_angular_variance" default="1"/>             <!-- If TF is used to get odometry, this is the default angular variance -->
  <arg name="odom_tf_linear_variance"  default="1"/>             <!-- If TF is used to get odometry, this is the default linear variance -->
  <arg name="odom_args"                default="$(arg rtabmap_args)"/>
  
  <arg name="subscribe_user_data"      default="false"/>             <!-- user data synchronized subscription -->
  <arg name="user_data_topic"          default="/user_data"/>
  <arg name="user_data_async_topic"    default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <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)"/>

  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <!-- RGB-D Odometry -->
    <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)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
        <param name="Odom/AlignWithGround"        type="bool"   value="true"/>
      </node>
    </group>
    
    <!-- Stereo Odometry -->
    <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)"/>
	  
        <param name="frame_id"                    type="string" value="$(arg frame_id)"/>
        <param name="odom_frame_id"               type="string" value="$(arg vo_frame_id)"/>
        <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
        <param name="approx_sync"                 type="bool"   value="$(arg approx_sync)"/>
        <param name="config_path"                 type="string" value="$(arg cfg)"/>
        <param name="queue_size"                  type="int"    value="$(arg queue_size)"/>
      </node>
    </group>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="true"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_user_data"  type="bool"   value="$(arg subscribe_user_data)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="map_frame_id"         type="string" value="$(arg map_frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
      <param name="odom_tf_linear_variance"  type="double" value="$(arg odom_tf_linear_variance)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"        type="string" value="$(arg database_path)"/>
      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      
      <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 from="user_data"              to="$(arg user_data_topic)"/>
      <remap from="user_data_async"        to="$(arg user_data_async_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
      
      <!-- localization mode -->
	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
      <param     if="$(arg stereo)" name="subscribe_depth"  type="bool" value="false"/>
      <param unless="$(arg stereo)" name="subscribe_depth"  type="bool" value="true"/>
      <param name="subscribe_stereo"     type="bool"   value="$(arg stereo)"/>
      <param name="subscribe_scan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_scan_cloud" type="bool"   value="$(arg subscribe_scan_cloud)"/>
      <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="odom_frame_id"        type="string" value="$(arg odom_frame_id)"/>
      <param name="wait_for_transform_duration" type="double"   value="$(arg wait_for_transform)"/>
      <param name="queue_size"           type="int"    value="$(arg queue_size)"/>
      <param name="approx_sync"          type="bool"   value="$(arg approx_sync)"/>
    
      <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>
  
  <!-- Visualization RVIZ -->
  <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" />

    <param name="decimation"  type="double" value="4"/>
    <param name="voxel_size"  type="double" value="0.0"/>
    <param name="approx_sync" type="bool"   value="$(arg approx_sync)"/>
  </node>

</launch>

[matlabbe]

Hi,

The "Grid/****" parameters can be used with obstacle_detection nodelet and with rtabmap node:

$ 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.]

Dynamic obstacles while standing still should be added temporary if "map_negative_poses_ignored" is false, which is the default. Which rtabmap version are you using? Is the obstacle added in local costmap or global costmap, or neither or both.

cheers,
Mathieu

[Alexia]

Hello Mathieu

We're using the latest version of RTABMAP.
Obstacles are added in both of the costmaps.
We got the octomap_grid cleaned up with the radius outlier removal implemented in rtabmap and it is much nicer now, but it still doesn't map new objects while stationary?

Output from rosrun params:

$ 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.]

We have changed the following in the launch file:

<param name="Grid/MaxGroundHeight" type="string" value="0.2"/>
<param name="Grid/MaxObstacleHeight" type="string" value="2"/>
<param name="Grid/NormalsSegmentation" type="string" value="false"/>
<param name="Grid/NoiseFilteringRadius" type="string" value="0.10" />
<param name="Grid/NoiseFilteringMinNeighbors" type="string" value="10" />
<param name="Grid/MapNegativePosesIgnored" type="string" value="false" />

Where can we make sure that "map_negative_poses_ignored" is still set to false?

Best regards
Alexia

PS: Thank you very much for your work on this program and the forum, it is a great help.

[matlabbe]

Hi Alexia,

map_negative_poses_ignored parameter is not a rtabmap library parameter, it is ros parameter. It is defined here:

pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_);
...
ROS_INFO("%s(maps): map_negative_poses_ignored = %s", name.c_str(), negativePosesIgnored_?"true":"false");

As shown above, you should see a ROS_INFO on terminal "map_negative_poses_ignored = false" on startup when rtabmap node is launched (as well as the modified Grid/** parameters):

ROS_MASTER_URI=http://localhost:11311

core service [/rosout] found
process[rtabmap/rgbd_odometry-1]: started with pid [9771]
process[rtabmap/rtabmap-2]: started with pid [9772]
process[rviz-3]: started with pid [9773]
process[points_xyzrgb-4]: started with pid [9774]
[ INFO] [1493997548.551457951]: Starting node...
[ INFO] [1493997548.672561895]: Initializing nodelet with 4 worker threads.
[ INFO] [1493997548.758212312]: Initializing nodelet with 4 worker threads.
[ INFO] [1493997549.557939392]: /rtabmap/rtabmap(maps): map_filter_radius          = 0.000000
[ INFO] [1493997549.558112791]: /rtabmap/rtabmap(maps): map_filter_angle           = 30.000000
[ INFO] [1493997549.558204065]: /rtabmap/rtabmap(maps): map_cleanup                = true
[ INFO] [1493997549.558296971]: /rtabmap/rtabmap(maps): map_negative_poses_ignored = false
[ INFO] [1493997549.558397691]: /rtabmap/rtabmap(maps): map_negative_scan_ray_tracing = true
[ INFO] [1493997549.558498432]: /rtabmap/rtabmap(maps): cloud_output_voxelized     = true
[ INFO] [1493997549.558600183]: /rtabmap/rtabmap(maps): cloud_subtract_filtering   = false
[ INFO] [1493997549.558701069]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1493997549.568427516]: /rtabmap/rtabmap(maps): octomap_tree_depth         = 16
[ INFO] [1493997549.568760110]: /rtabmap/rtabmap(maps): octomap_occupancy_thr      = 0.500000

[ INFO] [1493997551.366694495]: Update RTAB-Map parameter "Grid/MaxGroundHeight"="0.2" from arguments
[ INFO] [1493997551.366801615]: Update RTAB-Map parameter "Grid/MaxObstacleHeight"="2" from arguments
[ INFO] [1493997551.366845682]: Update RTAB-Map parameter "Grid/NoiseFilteringMinNeighbors"="10" from arguments
[ INFO] [1493997551.366884221]: Update RTAB-Map parameter "Grid/NoiseFilteringRadius"="0.1" from arguments
[ INFO] [1493997551.366920299]: Update RTAB-Map parameter "Grid/NormalsSegmentation"="false" from arguments
[ INFO] [1493997552.212241168]: RTAB-Map detection rate = 1.000000 Hz
[ INFO] [1493997552.217640733]: rtabmap: Deleted database "/home/mathieu/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1493997552.217741872]: rtabmap: Using database from "/home/mathieu/.ros/rtabmap.db".

I tried on my side using your parameters and objects are added. Command used:

$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Grid/MaxGroundHeight 0.2 --Grid/MaxObstacleHeight 2 --Grid/NormalsSegmentation false --Grid/NoiseFilteringRadius 0.1 --Grid/NoiseFilteringMinNeighbors 10" rtabmapviz:=false rviz:=true

Make sure you are using the latest code from source, not latest binaries as I am not sure if the latest data was added when not moving. I don't have a Kinetic machine right now, I'll try later with binaries if it should work too. EDIT it is okay with Kinetic binaries too.

cheers,
Mathieu