How to use costmap_converter for dynamic obstacle avoidance in teb_local_planner?

asked 2023-01-05 17:23:07 -0500

Shivam gravatar image

updated 2023-01-09 16:10:13 -0500

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 ...
(more)
edit retag flag offensive close merge delete