How to use costmap_converter for dynamic obstacle avoidance in teb_local_planner?
Hi, I am trying to implement dynamic obstacle avoidance using the costmap_converter package but my robot is still colliding with the dynamic obstacle. I followed the following tutorials: 1. costmap_conversion 2. track and include dynamic obstacle via costmap_converter
I am using 3 robots in simulation mmr1, mmr2, mmr3 and the navigation launch file is only for mmr3.
I am launching costmap_converter node like the following in my launch file:
<node name="standalone_converter" pkg="costmap_converter" type="standalone_converter" output="screen">
<param name="converter_plugin" value="costmap_converter::CostmapToDynamicObstacles" />
<param name="costmap_topic" value="/move_base/local_costmap/costmap" />
<param name="odom_topic" value="/odom" />
</node>
The parameter file for teblocalplanner and costmap_converter is as follows:
TebLocalPlannerROS:
odom_topic: /odom
map_frame: /map
# Trajectory
teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
global_plan_overwrite_orientation: True
max_global_plan_lookahead_dist: 3.0
feasibility_check_no_poses: 5
# Robot
max_vel_x: 0.25
max_vel_x_backwards: 0.15
max_vel_y: 0.0
max_vel_theta: 0.4
acc_lim_x: 0.25
acc_lim_y: 0.0
acc_lim_theta: 1
min_turning_radius: 0.0
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
# type: "circular"
# radius: 0.3 # for type "circular"
type: "polygon"
vertices: [[0.2, 0.27], [-0.45, 0.27], [-0.45, -0.27], [0.2, -0.27]]
# GoalTolerance
xy_goal_tolerance: 0.05
yaw_goal_tolerance: 0.17
free_goal_vel: Flase
complete_global_plan: True
# Obstacles
min_obstacle_dist: 0.5
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.0
# changing obstacle_poses_affected to true instead of 30 becasue it takes boolean True to avoid dynamic obstacles
obstacle_poses_affected: True
costmap_converter_plugin: "costmap_converter::CostmapToDynamicObstacles"
costmap_converter_spin_thread: True
costmap_converter_rate: 5
include_dynamic_obstacles: True
## Configure plugins (namespace move_base/CostmapToDynamicObstacles)
costmap_converter/CostmapToDynamicObstacles:
alpha_slow: 0.3
alpha_fast: 0.85
beta: 0.85
min_sep_between_slow_and_fast_filter: 80
min_occupancy_probability: 180
max_occupancy_neighbors: 100
morph_size: 1
filter_by_area: True
min_area: 3
max_area: 300
filter_by_circularity: True
min_circularity: 0.2
max_circularity: 1.0
filter_by_inertia: True
min_intertia_ratio: 0.2
max_inertia_ratio: 1.0
filter_by_convexity: False
min_convexity: 0.0
max_convexity: 1.0
dt: 0.2
dist_thresh: 60.0
max_allowed_skipped_frames: 3
max_trace_length: 10
static_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
# Optimization
no_inner_iterations: 2
no_outer_iterations: 2
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
obstacle_cost_exponent: 4
weight_max_vel_x: 2
weight_max_vel_y: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_y: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000 # WE HAVE A HOLONOMIC ROBOT, JUST ADD A SMALL PENALTY
weight_kinematics_forward_drive: 1000
weight_kinematics_turning_radius: 1
weight_optimaltime: 1
weight_obstacle: 50
weight_dynamic_obstacle: 10 # not in use yet
selection_alternative_time_cost: False # not in use yet
# Homotopy Class Planner
enable_homotopy_class_planning: True
enable_multithreading: True
simple_exploration: False
max_number_classes: 4
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_keypoint_offset: 0.1
obstacle_heading_threshold: 0.45
visualize_hc_graph: False
# Recovery
shrink_horizon_backup: True
shrink_horizon_min_duration: 10
oscillation_recovery: True
oscillation_v_eps: 0.1
oscillation_omega_eps: 0.1
oscillation_recovery_min_duration: 10
oscillation_filter_duration: 10
The following is some part of the output with warnings when I launch the navigation launch file:
NODES
/
map_server (map_server/map_server)
mmr1_amcl (amcl/amcl)
mmr1_move_base (move_base/move_base)
mmr2_amcl (amcl/amcl)
mmr2_move_base (move_base/move_base)
mmr3_amcl (amcl/amcl)
mmr3_move_base (move_base/move_base)
rviz (rviz/rviz)
standalone_converter (costmap_converter/standalone_converter)
ROS_MASTER_URI=http://localhost:11311
process[map_server-1]: started with pid [12415]
process[mmr1_amcl-2]: started with pid [12416]
[ WARN] [1672959666.879312777]: Request for map failed; trying again...
process[mmr2_amcl-3]: started with pid [12423]
process[mmr3_amcl-4]: started with pid [12430]
process[mmr1_move_base-5]: started with pid [12438]
[ WARN] [1672959667.167960978, 19.909000000]: No laser scan received (and thus no pose updates have been published) for 19.909000 seconds. Verify that data is being published on the /mmr2/scan topic.
process[mmr2_move_base-6]: started with pid [12447]
[ WARN] [1672959667.330220061, 20.062000000]: No laser scan received (and thus no pose updates have been published) for 20.062000 seconds. Verify that data is being published on the /mmr3/scan topic.
process[mmr3_move_base-7]: started with pid [12458]
[ WARN] [1672959667.452286430, 20.170000000]: Timed out waiting for transform from mmr1/base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist. canTransform: source_frame mmr1/base_footprint does not exist.. canTransform returned after 20.17 timeout was 0.1.
process[standalone_converter-8]: started with pid [12468]
[ WARN] [1672959667.558556945, 20.277000000]: Timed out waiting for transform from mmr2/base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 20.277 timeout was 0.1.
process[rviz-9]: started with pid [12478]
[ INFO] [1672959667.623855045]: Standalone costmap converter:costmap_converter::CostmapToDynamicObstacles loaded.
[ INFO] [1672959667.645261344]: CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles costmap_converter::CostmapToPolygonsDBSMCCH loaded.
QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[ WARN] [1672959667.663788209, 20.382000000]: Timed out waiting for transform from mmr3/base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 20.382 timeout was 0.1.
QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-root'
[ WARN] [1672959668.167266808, 20.881000000]: global_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1672959668.179723496, 20.892000000]: global_costmap: Using plugin "static_layer"
[ INFO] [1672959668.187549830, 20.900000000]: Requesting the map...
[ INFO] [1672959668.392923111, 21.103000000]: Resizing costmap to 576 X 800 at 0.050000 m/pix
[ INFO] [1672959668.489457749, 21.202000000]: Received a 576 X 800 map at 0.050000 m/pix
[ INFO] [1672959668.489501124, 21.202000000]: Subscribing to updates
[ INFO] [1672959668.493987454, 21.207000000]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1672959668.496463873, 21.210000000]: Subscribed to Topics: scan
[ INFO] [1672959668.507205333, 21.219000000]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1672959668.544800655, 21.257000000]: local_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1672959668.554212302, 21.264000000]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1672959668.555991509, 21.266000000]: Subscribed to Topics: scan
[ INFO] [1672959668.566107352, 21.277000000]: local_costmap: Using plugin "inflation_layer"
[ WARN] [1672959668.570944662, 21.282000000]: global_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1672959668.591097072, 21.303000000]: global_costmap: Using plugin "static_layer"
[ INFO] [1672959668.600209425, 21.312000000]: Requesting the map...
[ INFO] [1672959668.609404897, 21.322000000]: Created local_planner teb_local_planner/TebLocalPlannerROS
[ INFO] [1672959668.652348947, 21.366000000]: Footprint model 'polygon' loaded for trajectory optimization.
[ INFO] [1672959668.652441564, 21.366000000]: Parallel planning in distinctive topologies enabled.
[ INFO] [1672959668.753191247, 21.467000000]: CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles costmap_converter::CostmapToPolygonsDBSMCCH loaded.
[ WARN] [1672959668.767641014, 21.481000000]: global_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1672959668.786632081, 21.500000000]: global_costmap: Using plugin "static_layer"
[ INFO] [1672959668.800161197, 21.514000000]: Requesting the map...
[ INFO] [1672959668.801091207, 21.515000000]: Costmap conversion plugin costmap_converter::CostmapToDynamicObstacles loaded.
[ INFO] [1672959668.801976934, 21.516000000]: Resizing costmap to 576 X 800 at 0.050000 m/pix
[ INFO] [1672959668.903889707, 21.615000000]: Received a 576 X 800 map at 0.050000 m/pix
[ INFO] [1672959668.903935387, 21.615000000]: Subscribing to updates
[ INFO] [1672959668.910202740, 21.622000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1672959668.910346415, 21.622000000]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1672959668.913201194, 21.625000000]: Subscribed to Topics: scan
[ INFO] [1672959668.917512837, 21.629000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1672959668.933632641, 21.645000000]: global_costmap: Using plugin "inflation_layer"
[ INFO] [1672959668.967884256, 21.679000000]: CostmapToDynamicObstacles: odom received.
[ INFO] [1672959668.967949144, 21.679000000]: odom received!
[ WARN] [1672959668.989460144, 21.698000000]: local_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1672959668.999493148, 21.709000000]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1672959669.001154664, 21.710000000]: Subscribed to Topics: scan
[ INFO] [1672959669.009036177, 21.719000000]: Resizing costmap to 576 X 800 at 0.050000 m/pix
[ INFO] [1672959669.012272249, 21.722000000]: local_costmap: Using plugin "inflation_layer"
[ INFO] [1672959669.039202383, 21.749000000]: Created local_planner teb_local_planner/TebLocalPlannerROS
[ INFO] [1672959669.086703727, 21.794000000]: Footprint model 'polygon' loaded for trajectory optimization.
[ INFO] [1672959669.086781956, 21.794000000]: Parallel planning in distinctive topologies enabled.
[ INFO] [1672959669.110652171, 21.818000000]: Received a 576 X 800 map at 0.050000 m/pix
[ INFO] [1672959669.110702111, 21.818000000]: Subscribing to updates
[ INFO] [1672959669.115154007, 21.823000000]: global_costmap: Using plugin "obstacle_layer"
[ INFO] [1672959669.118431568, 21.826000000]: Subscribed to Topics: scan
[ INFO] [1672959669.131552360, 21.840000000]: global_costmap: Using plugin "inflation_layer"
[ WARN] [1672959669.171916339, 21.881000000]: local_costmap: Parameter "plugins" not provided, loading pre-Hydro parameters
[ INFO] [1672959669.185679721, 21.895000000]: local_costmap: Using plugin "obstacle_layer"
[ INFO] [1672959669.189087617, 21.898000000]: Subscribed to Topics: scan
[ INFO] [1672959669.212654557, 21.918000000]: local_costmap: Using plugin "inflation_layer"
[ INFO] [1672959669.222988531, 21.929000000]: CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles costmap_converter::CostmapToPolygonsDBSMCCH loaded.
[ INFO] [1672959669.265403212, 21.973000000]: Costmap conversion plugin costmap_converter::CostmapToDynamicObstacles loaded.
[ INFO] [1672959669.267112294, 21.975000000]: Created local_planner teb_local_planner/TebLocalPlannerROS
[ INFO] [1672959669.353606105, 22.064000000]: Footprint model 'polygon' loaded for trajectory optimization.
[ INFO] [1672959669.353683915, 22.064000000]: Parallel planning in distinctive topologies enabled.
[ INFO] [1672959669.446755758, 22.155000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1672959669.454321355, 22.163000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1672959669.474280638, 22.184000000]: CostmapToDynamicObstacles: underlying costmap conversion plugin for static obstacles costmap_converter::CostmapToPolygonsDBSMCCH loaded.
[ INFO] [1672959669.496216445, 22.206000000]: CostmapToDynamicObstacles: odom received.
[ INFO] [1672959669.496290204, 22.206000000]: odom received!
[ INFO] [1672959669.509153121, 22.219000000]: Costmap conversion plugin costmap_converter::CostmapToDynamicObstacles loaded.
[ INFO] [1672959669.660630653, 22.367000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1672959669.664839831, 22.371000000]: Recovery behavior will clear layer 'obstacles'
[ INFO] [1672959669.691279989, 22.396000000]: CostmapToDynamicObstacles: odom received.
[ INFO] [1672959669.691375400, 22.396000000]: odom received!
I have the parameters for the teblocalplanner and costmapcoverter in a YAML file. I am loading those parameters with the movebase nodes for each robot. For the costmap_converter I used the parameters provided in the Testing the obstacle tracker example
The teblocalplanner and the global planner is working fine for static obstacle.The problem is that mmr3 robot detects and avoids static obstacles but not dynamic obstacles. Please help me resolve this issue.
I am using ROS 1 Melodic on ubuntu 18.04
Asked by Shivam on 2023-01-05 18:23:07 UTC
Comments
Any updates?
Asked by alperenkeser on 2023-07-13 04:15:07 UTC