SmacPlannerHybrid creates kinematically infeasible path #4271
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 22
- ROS2 Version:
- Humble
- Version or commit hash:
- It's a fork that tracks commit d509fbf
- DDS implementation:
- Fast-RTPS
Steps to reproduce issue
I tried it in my custom simulation but I reckon it should also happen on tb3 simulation. You could try to set the global footprint bigger than the local one and try navigating close to a lethal obstacle on the global costmap.
This is the planner/costmap config:
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: False
global_frame: map
map_topic: /no_global_map_topic
always_send_full_costmap: True
rolling_window: True
track_unknown_space: False
publish_frequency: 5.0
update_frequency: 10.0
resolution: 0.05
width: 50
height: 50
lethal_cost_threshold: 99
# footprint_padding: 0.5
robot_base_frame: base_footprint
footprint: '[ [0.215, 0.42], [0.215, -0.42], [-1.01, -0.42], [-1.01, 0.42] ]'
plugins:
- 'static_layer'
- 'inflation_layer'
static_layer:
plugin: "nav2_costmap_2d::StaticLayer"
map_subscribe_transient_local: True
map_topic: /mission/geofence/global_costmap
inflation_layer:
plugin: 'nav2_costmap_2d::InflationLayer'
cost_scaling_factor: 1.5
inflation_radius: 0.7
inscribe: false
planner_server:
ros__parameters:
use_sim_time: False
expected_planner_frequency: 20.0
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
tolerance: 0.2
downsample_costmap: false
downsampling_factor: 1
allow_unknown: true
max_iterations: 1000000
max_on_approach_iterations: 1000
max_planning_time: 1.5
motion_model_for_search: "REEDS_SHEPP"
cost_travel_multiplier: 2.0
angle_quantization_bins: 64
analytic_expansion_ratio: 3.5
minimum_turning_radius: 0.4
reverse_penalty: 2.0
change_penalty: 0.15
non_straight_penalty: 1.50
cost_penalty: 5.5
lookup_table_size: 10.0
cache_obstacle_heuristic: False
debug_visualizations: true
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1.0e-10
Expected behavior
Create kinematically feasible path
Actual behavior
Resulting path makes planned footprints collide with global costmap
red thin rectangle = global and local footprint
red thicker rectangle = TEB debug visualization of the infeasible projected footprint
red path = SmacPlannerHybrid global plan
I also tried with a circular footprint, same result