Question

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: Robot failing to round a pretty easy corner

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.

Asked by hermanoid on 2022-08-16 16:18:43 UTC

Answer 1

Its difficult to tell from the information provided, its hard for me to tell scales of the costmap / minimum radius arc / etc. But how I'd start to debug this would be to tell how long it takes before a failure: is it immediate or is it after it tries to plan after N seconds of maximum planning time, or somewhere in the middle. These tell me very different reasons why it failed.

Your timestamps make it look absolutely immediate, which is strange. With ...752.3842 starting to ...752.3870 it failed super quick. I would think that would be pretty trivial path to find, unless your minimum turning radius was too large to make that turn at any given orientation (which is where the scale question comes up). Do you have an example of a path when this is successful?

Are you visualizing the global costmap's footprint or the local costmap's footprint? They're not necessarily the same thing unless you set them that way.

Asked by stevemacenski on 2022-08-17 16:24:01 UTC

Comments

The footprints are the same, they're actually both visualized in the picture and are just overlapping. I apologize, I don't have the robot handy to grab any successful planning operations. But, the videos I took over at https://answers.ros.org/question/405120/dwb-controller-struggles-with-navigation-in-tight-scenarios/ do show the robot planning successfully. Also, the minimum turning radius is set very low, to 0.1. Our robot doesn't mechanically need this radius, and it could be set lower if that's needed. However, the radius is set low enough that I suspect it's not why pathing is failing in this scenario.

Asked by hermanoid on 2022-08-18 06:06:38 UTC

Can you provide me with a map / configuration file / start / goal poses so I can reproduce?

That error https://github.com/ros-planning/navigation2/blob/7f0e08746915b7ad64caf1e2c008c4109a981367/nav2_smac_planner/src/smac_planner_hybrid.cpp#L320 appears when it literally exhausted all of its search branches before running out of things to try. I'd be really curious what num_iterations is in that situation when it fails.

The only reason I think that could fail, and so quickly, is if any primitive move allowable would be in collision (e.g. 1 step forward is in collision, 1 step left or right are both in collision). so that it can't actually move from its initial point anywhere.

Asked by stevemacenski on 2022-08-18 18:12:04 UTC

I no longer have access to the robots, anymore (just went off to college), but one of my old colleagues should be able to get that for you soon.

Asked by hermanoid on 2022-08-20 14:42:13 UTC

sorry for the delay and thank you so much for your help!

map https://github.com/johnjamesmiller/navigation2/blob/johnjamesmiller-smac-failing-to-get-path/nav2_system_tests/src/planning/empty_office_bedding.pgm
map yaml https://github.com/johnjamesmiller/navigation2/blob/johnjamesmiller-smac-failing-to-get-path/nav2_system_tests/src/planning/empty_office_bedding.yaml
config https://github.com/johnjamesmiller/navigation2/blob/johnjamesmiller-smac-failing-to-get-path/nav2_system_tests/src/planning/nav2_caretaker.yaml
start pose Translation: [1.481, 4.144, -0.133] Rotation: in Quaternion [0.110, -0.041, 0.993, -0.005]
goal pose 1.58, 5.08

Asked by johnjamesmiller on 2022-08-24 19:11:51 UTC

also enabled debug logging on planner. filtered out rcl

[DEBUG] [1661384552.929951989] [planner_server_rclcpp_node.rclcpp_action]: Accepted goal fe7214a53c8cdc52d117dbc5623acd99
[DEBUG] [1661384552.930027374] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Receiving a new goal
[DEBUG] [1661384552.930041344] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Executing goal asynchronously.
[DEBUG] [1661384552.930162692] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Executing the goal...
[DEBUG] [1661384552.933776567] [planner_server]: Attempting to a find path from (1.48, 4.14) to (1.58, 5.08).
[WARN] [1661384552.936535313] [planner_server]: GridBased: failed to create plan, no valid path found.
[WARN] [1661384552.936570935] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (1.58, 5.08)

Asked by johnjamesmiller on 2022-08-24 19:14:47 UTC

One other interesting thing. If the DWB controller is swapped out for RPP the planner will find an initial plan and controller will start to execute it until it turns into a lethal space.

Asked by johnjamesmiller on 2022-08-24 20:21:23 UTC

SmacPlannerHybrid cannot find path in tight environment

Comments

Answers

Comments