Robotics StackExchange | Archived questions

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

Answers