SmacPlannerHybrid cannot find path in tight environment
We're using ROS 2 Galactic with the SmacPlannerHybrid planner, DWB Controller, and some unusual constraints, such as our tight (barn) environment and elongated differential robot. We've been having all sorts of troubles with navigation, including the controller acting strangely (that'll be for another post), but one recurring issue is the SmacPlannerHybrid algorithm failing to come up with paths in fairly obvious scenarios.
Here is one such scenario:
Obviously I can tell you that I can physically drive the robot around this corner no-problem. It certainly takes some knowledge of kinematic feasibility - the robot needs to go straight forward for a little ways before it could start to turn - but that's why we chose this planner in the first place.
Here is our global planner config:
planner_server:
ros__parameters:
expected_planner_frequency: 20.0
use_sim_time: False
planner_plugins: ["GridBased"]
GridBased:
plugin: "nav2_smac_planner/SmacPlannerHybrid"
downsample_costmap: false
downsampling_factor: 1
allow_unknown: false
max_iterations: -1
max_planning_time: 5.0
motion_model_for_search: "DUBIN"
angle_quantization_bins: 72
analytic_expansion_ratio: 3.5
analytic_expansion_max_length: 3.
minimum_turning_radius: 0.1
reverse_penalty: 2.0
change_penalty: 0.0
non_straight_penalty: 1.0
cost_penalty: 1.0
retrospective_penalty: 0.015
lookup_table_size: 20.0
cache_obstacle_heuristic: false
tolerance: 0.5
max_on_approach_iterations: 1000
cost_travel_multiplier: 1.0
smooth_path: True
smoother:
max_iterations: 1000
w_smooth: 0.3
w_data: 0.2
tolerance: 1e-10
Log output:
robot_1 | [bt_navigator-6] [INFO] [1660685752.384208487] [bt_navigator]: Begin navigating from current location to (1.90, 5.05)
robot_1 | [planner_server-4] [WARN] [1660685752.387001041] [planner_server]: GridBased: failed to create plan, no valid path found.
robot_1 | [planner_server-4] [WARN] [1660685752.387070424] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (1.90, 5.05)
robot_1 | [planner_server-4] [WARN] [1660685752.387096038] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
robot_1 | [bt_navigator-6] [ERROR] [1660685752.423907120] [bt_navigator]: Goal failed
robot_1 | [bt_navigator-6] [WARN] [1660685752.423959355] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.