[SmacPlanner2D] Unintuitive plan when away from obstacles

[doisyg]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• ROS2 Version:
  • source
• Version or commit hash:
  • latest
• DDS implementation:
  • Cyclone

Steps to reproduce issue

Plan with the following parameters

    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.5                      # tolerance for planning if unable to reach exact pose, in meters
      downsample_costmap: false             # whether or not to downsample the map
      downsampling_factor: 1                # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      allow_unknown: false                   # allow traveling in unknown space
      max_iterations: -1               # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000      # maximum number of iterations to attempt to reach goal once in tolerance
      max_planning_time: 5.0                # max time in s for planner to plan, smooth
      motion_model_for_search: "MOORE"      # 2D Moore, Von Neumann
      cost_travel_multiplier: 2.0           # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*.
      use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true)
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1e-10

Expected behavior

Shorter path when there is no cost constraint

Actual behavior

Non straight path when there is no cost constraint

For instance:

Additional information

---

[SteveMacenski]

Even without the costs, that is odd. Though, as part of the README I make pretty clear everything in Smac requires a potential field to be applied, it is important. If there is no cost, then the cost multiplier is irrelevant so all moves have a simple cost of 1 (or sqrt2). But that doesn't explain / excuse that laughable behavior.

If you can provide me with a minimum example (map, costmap settings, etc) and starting / goal poses you see this happen in, I can take a look. Looking over the code, I have a couple of places I'm suspicious about that I meant to update long ago and simply slipped my mind since the 2D planner was mostly just a toy example to show that things were working while building up the Hybrid-A* / State Lattice capabilities.

If you wanted to yourself if that's faster, they're all small but based on updates made to the other complimentary planners:

• https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/node_2d.cpp#L88-L89 this should be an L2 norm for Moore and the manhattan distance only for the 4-connected space
• That will slow things down, but since this is 2D infeasible anyway, enable costmap downsampling at 2x resolution https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/smac_planner_2d.cpp#L68-L72 that should put the pep back in the step
• I think perhaps https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/node_2d.cpp#L78 should actually be length * (1 + cost_travel_multiplier * normalized_cost)
• As aside: I've been thinking about just killing https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/node_2d.cpp#L104 the Von-Neumann (4-connected) option, its never really that much faster than 8 connected and the paths are markedly worse. May as well remove the opportunity for someone to get themselves into a bad situation

[doisyg]

Thanks for your detailed answer, I am slowly looking into all that.
In an attempt to test the behavior on a lot of path (and in order to provide a minimum example), I am looking at the system tests (https://github.com/ros-planning/navigation2/tree/main/nav2_system_tests/src/planning), do you you now how the example_result.png visualization was generated ? I can run the run the test_planner_random but I wonder where's the part that produces the result image with all the paths.

[SteveMacenski]

Via https://github.com/ros-planning/navigation2/blob/main/nav2_system_tests/src/planning/test_planner_random_node.cpp which has its map set in https://github.com/ros-planning/navigation2/blob/bcc9aff6bd0e60f6f887cde91b8d2f23f6a1dfa9/nav2_system_tests/src/planning/CMakeLists.txt#L46 which is set to https://github.com/ros-planning/navigation2/blob/bcc9aff6bd0e60f6f887cde91b8d2f23f6a1dfa9/nav2_system_tests/maps/map.pgm which is the map generated.

I'm not entirely sure where the code that was used to overlay the points on the map was done, that was probably done offline just for the purposes of visual demonstration of what was happening under the hood.

[doisyg]

Thx, could not find visualization related code but it was just a matter of adding 3 publishers (path, start point and end point) and increasing the number of tests to get to the same result in rviz (and adding buffer/history there):

We can already see some weird paths above, but this is for the default planner of the random test (NavFn, I believe), not SmacPlanner2D. I'll work on a setup that allow choosing the planner.

[SteveMacenski]

I'm fairly certain that the top 3 comments on my bulleted list will take care of the issue you mention for Smac2D. The heuristic would explain alot.

For the thing you show with NavFn, It uses the Navigation Function 2 for expanding the potential field which is smooth, but the backtracing is done seperately which can sometimes do odd things, usually towards the end of paths. Given this is only a binary field, not a costmap, there are a couple of odd "squiggles" but they're only as they're going up to / against hard walls at the start/end of goal so that is probably OK overall.

Navfn has been around essentially since the start of ROS Navigation and the codebase is relatively complex, I don't think it would be wise to make changes to it based on a benchmarking test if in practice we don't have issue with it when there is a potential field due to inflation. You didn't seem to have an issue with it before now ;-) so I wouldn't argue with it.

[SteveMacenski]

Any update?

[doisyg]

Slow progress because of other parallel tasks, but I am currently rewriting a test process directly interacting with the planner_server through its action (and with standard costmap with inflation)

[doisyg]

All with default parameters:

NavFn:

Smac2d:

Smac Hybrid:

[SteveMacenski]

I'm not sure what I should be pulling exactly from those images, but always nice to see things visualized!

I would still say that my bulleted changes to 2D are probably what you want to fix the behavior you mentioned

[doisyg]

Just for showing the difference between the planners and some weird paths with Smac2D. I ll investigate more and report here when I ll have a a bit of time.

[SteveMacenski]

Is there a couple paths in particular in Smac 2D that you think are odd? At first glance, I'm not sure which you mean.

[doisyg]

A few grossly highlighted

[SteveMacenski]

@doisyg I went ahead and made the changes I outlined in the comment. See https://github.com/ros-planning/navigation2/tree/smac_fix2

Let me know if this fixes your issues / comments. I can open a PR and merge it. I didn't make costmap downsampling to 2d default but that does give a nice 75% speed up if speed is a concern.

[doisyg]

Thx, I ll try ASAP and report

[SteveMacenski]

Any thoughts / updates?

[doisyg]

Review and test on a real platform planned for the end of the month

[SteveMacenski]

@doisyg we did some testing (see images in the PR) and I strongly believe this fixes your reported issue, but happy to reopen if it does not fully resolve what you're seeing

[doisyg]

Great! Sorry about opening the discussion without having the time nor the resources to further investigate it soon enough. I will try to report my conclusions on these changes at some point though.