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 teb_local_planner 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 ...
Any updates?