Re: Problem with global and local costmap
Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-with-global-and-local-costmap-tp1867p1872.html
Hi,
The local costmap should not subscribe to cloud_map. The global costmap should use static map plugin and subscribe to /proj_map. obstacles_detection plugin should subscribe to points from the camera, not cloud_map. Please compare carefully with configurations from the Stereo Outdoor Navigation tutorial.
Here some modifications I've done:
And the launch file:
The biggest issue here is the ZED images, I've added stereo_view node above to debug it. Note how the disparity on the ground is sparse and noisy (not sure if it is a calibration issue). If the ground cannot be correctly computed, you may have false obstacles appearing in the map or in obstacles_detection.
There are some image artifacts on the top, and some images are just wrong ( which cause odometry lost, note how no disparity values can be computed):
Did you try using depth images computed by ZED SDK for a RGB-D setup instead of a stereo setup?
cheers
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-with-global-and-local-costmap-tp1867p1872.html
Hi,
The local costmap should not subscribe to cloud_map. The global costmap should use static map plugin and subscribe to /proj_map. obstacles_detection plugin should subscribe to points from the camera, not cloud_map. Please compare carefully with configurations from the Stereo Outdoor Navigation tutorial.
Here some modifications I've done:
<common_costmap_params.yaml>
obstacle_range: 2.5 #actualiza info con obstaculos hasta 2.5 m
raytrace_range: 3.0
footprint: [[0.67, 0.51], [0.67, -0.51 ], [-0.67, -0.51], [-0.67,0.51]] #tamaño del robot
#robot_radius: ir_of_robot
inflation_radius: 0.20
<global_costmap_params.yaml>
global_costmap:
global_frame: /map
robot_base_frame: /base_footprint
update_frequency: 1.0
static_map: true
<local_costmap_params.yaml>
local_costmap:
global_frame: /odom
robot_base_frame: /base_footprint
update_frequency: 5
publish_frequency: 2
static_map: false
rolling_window: true
width: 6.0
height: 6.0
resolution: 0.05
origin_x: -3
origin_y: -3
transform_tolerance: 10
observation_sources: point_cloud_sensor
point_cloud_sensor: {sensor_frame: base_zed, data_type: PointCloud2, topic: /obstacles_cloud, marking: true, clearing: true, min_obstacle_height: -99999, max_obstacle_height: 99999}
And the launch file:
<launch>
<param name="use_sim_time" value="true"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<group ns="rtabmap">
<!-- Visual Odometry -->
<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" >
<remap from="left/image_rect" to="/zed/left/image_rect_throttle"/>
<remap from="right/image_rect" to="/zed/right/image_rect_throttle"/>
<remap from="left/camera_info" to="/zed/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/zed/right/camera_info_throttle"/>
<remap from="odom" to="/odometry"/>
<param name="Stereo/MaxDisparity" type="int" value="200"/>
<param name="frame_id" type="string" value="base_footprint"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="wait_for_transform_duration" type="double" value="1"/>
<param name="Vis/InlierDistance" type="string" value="0.5"/>
<param name="Vis/MinInliers" type="string" value="8"/>
<param name="Vis/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Vis/MaxDepth" type="string" value="0"/>
<param name="Vis/MaxCorners" type="string" value="500"/>
<param name="Vis/MinDistance" type="string" value="5"/>
<param name="queue_size" type="int" value="30"/>
</node>
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="wait_for_transform_duration" type="double" value="1"/>
<remap from="left/image_rect" to="/zed/left/image_rect_throttle"/>
<remap from="right/image_rect" to="/zed/right/image_rect_throttle"/>
<remap from="left/camera_info" to="/zed/left/camera_info_throttle"/>
<remap from="right/camera_info" to="/zed/right/camera_info_throttle"/>
<remap from="odom" to="/odometry"/>
<param name="queue_size" type="int" value="30"/>
<!-- RTAB-Map's parameters -->
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="Kp/MaxFeatures" type="string" value="200"/>
<param name="Kp/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="SURF/HessianThreshold" type="string" value="1000"/>
<param name="Vis/MaxDepth" type="string" value="0"/>
<param name="Vis/MinInliers" type="string" value="5"/>
<param name="Vis/InlierDistance" type="string" value="0.1"/>
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="true"/>
<param name="Vis/MaxFeatures" type="string" value="500"/>
</node>
</group>
<node pkg="nodelet" type="nodelet" name="stereo_nodelet" args="manager"/>
<group ns="zed">
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="left/image_rect_throttle"/>
<remap from="left/camera_info" to="left/camera_info_throttle"/>
<remap from="right/image_raw" to="right/image_rect_throttle"/>
<remap from="right/camera_info" to="right/camera_info_throttle"/>
<param name="disparity_range" value="200"/>
</node>
</group>
<!-- to debug disparity -->
<node pkg="image_view" type="stereo_view" name="stereo_view" output="screen">
<remap from="stereo" to="zed"/>
<remap from="image" to="image_rect_throttle"/>
</node>
rosrun image_view stereo_view stereo:=zed image:=image_rect_throttle
<!-- Generate a point cloud from the disparity image -->
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
<remap from="disparity/image" to="/zed/disparity"/>
<remap from="disparity/camera_info" to="/zed/right/camera_info_throttle"/>
<remap from="cloud" to="cloudXYZ"/>
<param name="voxel_size" type="double" value="0.05"/>
<param name="decimation" type="int" value="1"/>
<param name="max_depth" type="double" value="4"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet" output="screen">
<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="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 ns="planner">
<remap from="map" to="/rtabmap/proj_map"/>
<remap from="move_base_simple/goal" to="/planner_goal"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="5" />
<rosparam file="$(find zed)/launch/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find zed)/launch/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find zed)/launch/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find zed)/launch/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find zed)/launch/config/base_local_planner_params.yaml" command="load" />
</node>
</group>
</launch>
The biggest issue here is the ZED images, I've added stereo_view node above to debug it. Note how the disparity on the ground is sparse and noisy (not sure if it is a calibration issue). If the ground cannot be correctly computed, you may have false obstacles appearing in the map or in obstacles_detection.
There are some image artifacts on the top, and some images are just wrong ( which cause odometry lost, note how no disparity values can be computed):
Did you try using depth images computed by ZED SDK for a RGB-D setup instead of a stereo setup?
cheers
| Free forum by Nabble | Edit this page |