SmacPlannerHybrid cannot find path in tight environment

asked 2022-08-16 16:18:43 -0500

hermanoid
176 ●7 ●9 ●16

updated 2022-08-16 16:36:44 -0500

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.

answered 2022-08-17 16:24:01 -0500

stevemacenski

8272 ●34 ●503 ●129 https://www.linkedin.c...

updated 2022-08-17 16:25:25 -0500

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.

edit flag offensive delete link

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/4051... 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.

hermanoid ( 2022-08-18 06:06:38 -0500 )edit

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

That error https://github.com/ros-planning/navig... 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.

stevemacenski ( 2022-08-18 18:12:04 -0500 )edit

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.

hermanoid ( 2022-08-20 14:42:13 -0500 )edit

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

map https://github.com/johnjamesmiller/na...
map yaml https://github.com/johnjamesmiller/na...
config https://github.com/johnjamesmiller/na...
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

johnjamesmiller ( 2022-08-24 19:11:51 -0500 )edit

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)

johnjamesmiller ( 2022-08-24 19:14:47 -0500 )edit

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.

johnjamesmiller ( 2022-08-24 20:21:23 -0500 )edit

add a comment

SmacPlannerHybrid cannot find path in tight environment

1 Answer

Comments

Question Tools

Stats

Related questions

SmacPlannerHybrid cannot find path in tight environment edit

1 Answer

Comments

Question Tools

Stats

Related questions

SmacPlannerHybrid cannot find path in tight environment