Question

Hi all,

I am using the ROS navigation library on a robot running ROS melodic on debian 9.13

I am using standard GlobalPlanner and TEBlocal_planner.

I observe that the GlobalPlanner is not able to recalculate a path when the initial path turns out to be impossible. Typically I put a cardboard box in a hallway that it is impossible to bypass. There is an alternate path to the target. But GlobalPlanner does not reconsider the initial path even when I change target near the obstacle. So I understand the GlobalPlanner does not take into account the cumulative costs on the globalcostmap and the localcostmap? Is it this ? If so, is it possible to reconfigure the navigation so that GlobalPlanner can offer another path?

Thank you in advance for your help.

Capture video RVIZ is available with this link

The configuration file for Global_Planner is:

GlobalPlanner:
  old_navfn_behavior: false 
  use_quadratic: true
  use_dijkstra: true
  use_grid_path: false

  allow_unknown: false

  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 0.9

The configuration file for move_base is:

shutdown_costmaps: false

controller_frequency: 10.0

planner_patience: 5.0

controller_patience: 15.0

conservative_reset_dist: 3.0

planner_frequency: 5.0

oscillation_timeout: 10.0

oscillation_distance: 0.5

holonomic_robot: false

The (updated and working) configuration file comman for both global_costmap and local_costmap is:

footprint: [[-0.24, -0.27], [-0.24, 0.27], [0.24, 0.27], [0.24, -0.27]]
footprint_padding: 0.00

transform_tolerance: 0.2
map_type: costmap

always_send_full_costmap: true

obstacle_layer:
 enabled: true
 obstacle_range: 3.0
 raytrace_range: 4.0
 inflation_radius: 0.2
 track_unknown_space: true
 combination_method: 1

observation_sources: laser_scan_sensor
laser_scan_sensor: {data_type: LaserScan, topic: scanf, marking: true, clearing: true}

inflation_layer:
  enabled:              true
  cost_scaling_factor:  10.0 
  inflation_radius:     0.5

static_layer:
  enabled:              true
  map_topic:            "/map"

The (updated and working) configuration file for global_costmap is:

global_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 1.0
  publish_frequency: 0.5
  static_map: true

  transform_tolerance: 0.5
  plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: obstacle_layer,          type: "costmap_2d::VoxelLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

  inflation_layer: {cost_scaling_factor: 7.0, enabled: true, inflate_unknown: false,
    inflation_radius: 0.75}

The (updated and working) configuration file for local_costmap is:

local_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  update_frequency: 5.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 5.5
  height: 5.5
  resolution: 0.1
  transform_tolerance: 0.5

  plugins:
   - {name: static_layer,        type: "costmap_2d::StaticLayer"}
   - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
   - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}

  inflation_layer: {cost_scaling_factor: 7.0, enabled: true, inflate_unknown: false,
    inflation_radius: 0.5}
  #
  obstacle_layer:
    combination_method: 1
    enabled: true
    footprint_clearing_enabled: true
    max_obstacle_height: 2.0
    observation_sources: scan
    obstacle_range: 3.0
    raytrace_range: 3.5
    scan: {clearing: true, data_type: LaserScan, marking: true, max_obstacle_height: 2.0,
      min_obstacle_height: 0.0, sensor_frame: laser, topic: scanf}

The configuration file for TEB_local_planner is :

TebLocalPlannerROS:

 odom_topic: odom

 # Trajectory

 teb_autosize: True
 dt_ref: 0.3
 dt_hysteresis: 0.1
 max_samples: 500
 global_plan_overwrite_orientation: True
 allow_init_with_backwards_motion: False
 max_global_plan_lookahead_dist: 3.0
 global_plan_viapoint_sep: -1
 global_plan_prune_distance: 1
 exact_arc_length: False
 feasibility_check_no_poses: 5
 publish_feedback: False

 # Robot

 max_vel_x: 0.4
 max_vel_x_backwards: 0.2
 max_vel_y: 0.0
 max_vel_theta: 0.3
 acc_lim_x: 0.5
 acc_lim_theta: 1.0
 min_turning_radius: 0.0 # diff-drive robot (can turn on place!)

 footprint_model:
   type: "point"

 # GoalTolerance

 xy_goal_tolerance: 0.2
 yaw_goal_tolerance: 0.1
 free_goal_vel: False
 complete_global_plan: True

 # Obstacles

 min_obstacle_dist: 0.35 # This value must also include our robot radius, since footprint_model is set to "point".
 inflation_dist: 0.6
 include_costmap_obstacles: True
 costmap_obstacles_behind_robot_dist: 1.5
 obstacle_poses_affected: 15

 dynamic_obstacle_inflation_dist: 0.6
 include_dynamic_obstacles: True

 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5

 # Optimization

 no_inner_iterations: 5
 no_outer_iterations: 4
 optimization_activate: True
 optimization_verbose: False
 penalty_epsilon: 0.1
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2
 weight_max_vel_theta: 1
 weight_acc_lim_x: 1
 weight_acc_lim_theta: 1
 weight_kinematics_nh: 1000
 weight_kinematics_forward_drive: 1
 weight_kinematics_turning_radius: 1
 weight_optimaltime: 1 # must be > 0
 weight_shortest_path: 0
 weight_obstacle: 100
 weight_inflation: 0.2
 weight_dynamic_obstacle: 10
 weight_dynamic_obstacle_inflation: 0.2
 weight_viapoint: 1
 weight_adapt_factor: 2

 # Homotopy Class Planner

 enable_homotopy_class_planning: True
 enable_multithreading: True
 max_number_classes: 4
 selection_cost_hysteresis: 1.0
 selection_prefer_initial_plan: 0.9
 selection_obst_cost_scale: 100.0
 selection_alternative_time_cost: False

 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: 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

Asked by jchodorowski on 2020-11-25 08:48:17 UTC

Answer 1

Disclaimer: I do not want to request access to your video, so I did not watch it. If it contains important points, please make it readily available. Posting screenshots here could be useful, too, if you're able.

I'm not sure what you mean by "cumulative costs," but the global planner does use all of the costmap (total cost of its layers). Once a planner finds an acceptable path, its job is done unless you ask it to try again. That can happen based on timing or, as in your case, when the planned route becomes impossible.

The first thing to inspect is your move_base configuration file (move_base_params.yaml maybe). There is a planner_frequency parameter, which dictates how often in Hz the move_base node calls the global planner to retry. You may also be interested in recovery_behaviors, which controls how the robot tries to get itself out of trouble (can't find a path at all). The move_base documentation provides more details.

Although I answered your question as stated, I know what you're really after is functional navigation. If the above parameters are set appropriately, then move_base will call the planner until it has exhausted its options. Does the robot perform its recovery behaviors? Are you sure that the "alternate path" is viable and that the planner is able to see and evaluate it?

Asked by tryan on 2020-11-25 10:53:22 UTC

Comments

Thank you for your answer.

Sorry for the inconvenience about the video, I've updated the link and made a test with unconnected browser : the video is at least downloadable. I've updated my post with move_base params as well.

Total cost of all map layers was exactly what I called "cumulative costs", sorry if this term is inadequate.

The recovery behaviors set in move_base launchfile are those :

    <rosparam param="recovery_behaviors">
        [ { name: "aggressive_reset",      type: "clear_costmap_recovery/ClearCostmapRecovery" },
          { name: "geko_recovery_nav",     type: "geko_recovery_nav/GekoRecoveryNav"   } ]
    </rosparam>

The second one is a very simple move backward or forward, whatever cost less.

I hope you'll be able to have a look on a video, but yes, the "alternate path" is viable, the robot is able to go through it if adequate waypoints are set. What criteria applies to determine if the planner is able to see and evaluate a path ?

Asked by jchodorowski on 2020-11-25 11:14:50 UTC

The video definitely helped me. Thank you for updating it. Based on that, it looks like the global and local planners still think the path is viable when it isn't. I noticed that the lidar-detected obstacles are not inflated, so it's possible the planner thinks it can squeeze through (between lidar returns?). In costmap configuration files, the obstacles layer is usually before the inflation layer like this:

plugins:
    - {name: static_layer, type: "costmap_2d::StaticLayer"}
    - {name: obstacles_layer, type: "costmap_2d::VoxelLayer"}
    - {name: inflater_layer, type: "costmap_2d::InflationLayer"}

Can you post your costmap parameters for comparison?

Asked by tryan on 2020-11-25 11:49:47 UTC

There is a filter on lidar detection. In the video you can observe green points that are filtered points and white points that are all lidar detections. Only green points are used with navigation stack.

I've updated the original post with costmap configuration files

Asked by jchodorowski on 2020-11-26 06:46:02 UTC

OK, thanks for bearing with me, and sorry I didn't answer your previous question:

What criteria applies to determine if the planner is able to see and evaluate a path ?

I meant that the alternate path is entirely on the cost map, unobstructed, and you could make the robot take that path in some manner (e.g., extra waypoints), which you've already confirmed.

I notice that your global_costmap doesn't have an obstacle_layer. It seems the global planner doesn't know the new obstacle exists. You can check this by turning off the local_costmap display in RViz and seeing that the path looks clear. I recommend adding an obstacle_layer to your global configuration before your inflation_layer.

Also, your local_costmap doesn't have an inflation layer, but if you're happy with how the local planner handles things, that may be fine.

Asked by tryan on 2020-11-26 09:40:36 UTC

Thank you very much for helping ! I wasn't exhaustive in my params files since there is some common params to both costmaps. I've made some runs in a simulator where I observe the awaited behaviour : when a path is blocked and if there is an alternative path as you defined it, the robot is able to compute a new global path. The difference between simulation and reality is - among others - an introduction of costmap_prohibition_layer. I'll try to find the configuration that works and be back with results here. Again, I am very grateful to you for taking the time to look at this case.

Asked by jchodorowski on 2020-11-26 09:49:31 UTC

I've added costmap_common_params.yaml and updated global_costmap_params.yaml and local_costmap_params.yaml

In this empirical configuration the global planner makes updates with new paths that is no more going through obstacles.

I'll clean some redundancies and try to find the difference that make the global planner fail.

Asked by jchodorowski on 2020-11-30 05:53:38 UTC

The global_costmap params used these plugins before:

plugins:
    - {name: static_layer,            type: "costmap_2d::StaticLayer"}
    - {name: costmap_prohibition_layer,  type: "costmap_prohibition_layer_namespace::CostmapProhibitionLayer"}
    - {name: inflation_layer,         type: "costmap_2d::InflationLayer"}

Now, there's an obstacle layer (instead of the prohibition layer, but you could keep that, too). That is likely why the global planner now plans around new obstacles from sensor data when it did not before..

Asked by tryan on 2020-11-30 08:30:25 UTC

Thanks for all help provided tyran !

Asked by jchodorowski on 2020-12-03 05:53:34 UTC

" Dynamic " Global Planner ?

Comments

Answers

Comments