Posted by
Alexia on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Move-base-with-rtabmap-tp3048.html
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.84globa_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>