Feat/smac planner include orientation flexibility #4127

stevedanomodolor · 2024-02-20T19:11:15Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #4019)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)

Description of contribution in a few bullet points

tackles issue [Smac Planner] Enable goal orientation non-specificity #3789
Includes the possibility for the user to determine whether they want the goal heading should be default(the one defined in the goal), bidirectional (user defined + 180 degrees), or all_direction. This way, the user can save time calling the planner multiple times when the angle of the goal does not have to be fixed and has a bit of flexibility.
Results can be seen.

Success rate for  ALL_DIRECTION_SmacHybrid  is:  98.33333333333333 %
Success rate for  ALL_DIRECTION_SmacLattice  is:  97.66666666666667 %
Success rate for  ALL_DIRECTION_Smac2d  is:  100.0 %
Success rate for  BIDIRECTIONAL_SmacHybrid  is:  98.33333333333333 %
Success rate for  BIDIRECTIONAL_SmacLattice  is:  97.66666666666667 %
Success rate for  BIDIRECTIONAL_Smac2d  is:  100.0 %
Success rate for  DEFAULT_SmacHybrid  is:  96.66666666666667 %
Success rate for  DEFAULT_SmacLattice  is:  95.0 %
Success rate for  DEFAULT_Smac2d  is:  100.0 %
Success rate for  NORMAL_SmacHybrid  is:  96.66666666666667 %
Success rate for  NORMAL_SmacLattice  is:  95.0 %
Success rate for  NORMAL_Smac2d  is:  100.0 %

**********************Results Goal heading with new changes **********************
DEFAULT_Smac2d             48.463951551204566       0.1264367441153846    3.985572832589736   61.63986013986014
BIDIRECTIONAL_Smac2d       48.463951551204566       0.12651611789160838   3.985572832589736   61.63986013986014
ALL_DIRECTION_Smac2d       48.463951551204566       0.12622239968531468   3.985572832589736   61.63986013986014
DEFAULT_SmacHybrid         49.00072917359392        0.0657405418881119    1.6125494504062425  62.59090909090909
BIDIRECTIONAL_SmacHybrid   48.86220994748993        0.027206837828671327  1.465786467925095   60.33216783216783
ALL_DIRECTION_SmacHybrid   48.77456087576495        0.03619210418181818   1.6372313925886013  61.38461538461539
DEFAULT_SmacLattice        49.67700418147134        0.12979092083566435   1.6175072754351327  72.18181818181819
BIDIRECTIONAL_SmacLattice  49.83522326914245        0.027898774566433564  1.5455567606170904  70.31468531468532
ALL_DIRECTION_SmacLattice  49.91736393848793        0.03631584257342657   1.6489731848707427  71.43706293706293
-------------------------  -----------------------  --------------------  ------------------  -----------------


**********************Results Goal heading mode NO changes **********************
Read data
-----------  -----------------------  -------------------  ------------------  ------------------
Planner      Average path length (m)  Average Time (s)     Average cost        Max cost
SmacHybrid   48.59179172137297        0.0930492549330986   1.0656706399532687  54.59154929577465
SmacLattice  49.30762557367365        0.44674316980633805  0.8137733557552133  53.813380281690144
Smac2d       48.52958672840513        0.12973642600352114  0.7580233687132835  65.47183098591549
-----------  -----------------------  -------------------  ------------------  ------------------

Description of documentation updates required from your changes

How to run

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new features OR changes to existing behaviors are reflected in the tuning guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

SteveMacenski · 2024-02-21T18:49:21Z

@stevedanomodolor what's the status here - do you want me to review in detail, have gaps that are TODO, or have some questions to discuss? I don't want to go through and nitpick some small issues if you're really looking for feedback elsewhere right now.

SteveMacenski

I didn't review the analytic expansions yet, pending your answer to my above question. But overall from what I read so far, very few notes. This is very good and I couldn't have done it better myself!

nav2_smac_planner/include/nav2_smac_planner/constants.hpp

nav2_smac_planner/CMakeLists.txt

nav2_smac_planner/src/a_star.cpp

nav2_smac_planner/src/smac_planner_hybrid.cpp

nav2_smac_planner/src/smac_planner_lattice.cpp

stevedanomodolor · 2024-02-21T19:13:13Z

@stevedanomodolor what's the status here - do you want me to review in detail, have gaps that are TODO, or have some questions to discuss? I don't want to go through and nitpick some small issues if you're really looking for feedback elsewhere right now.

If you consider the general approach to be ok, then you can review it in detail. If the approach is good, what is just left is to modify the test to take into consideration these changes hench the todo in the CMakelists.

SteveMacenski · 2024-02-21T19:20:33Z

I think the analytic expansions might need to be rethought a bit. I think we should be taking all N of the goals and computing the analytic path, if valid. If any are valid, take the shortest one. There shouldn't be a loop surrounding the preamble which tells us if we want to do analytic expansions in this iteration:

      closest_distance = goal_distance_pair.second;
      NodePtr goal_node = goal_distance_pair.first;
      // We want to expand at a rate of d/expansion_ratio,
      // but check to see if we are so close that we would be expanding every iteration
      // If so, limit it to the expansion ratio (rounded up)
      int desired_iterations = std::max(
        static_cast<int>(closest_distance / _search_info.analytic_expansion_ratio),
        static_cast<int>(std::ceil(_search_info.analytic_expansion_ratio)));
      // If we are closer now, we should update the target number of iterations to go
      analytic_iterations =
        std::min(analytic_iterations, desired_iterations);

      // Always run the expansion on the first run in case there is a
      // trivial path to be found
      if (analytic_iterations <= 0) {

I think your logic is that if we sort by heuristic, then the first that comes back as a valid expansion will be the shortest. I think that would generally be true if the heuristic was a very purist implementation of a distance heuristic. But instead, we have the maximum of a few heuristics including cost information so the "closest" and the one with the "lowest travel cost" aren't necessarily the same thing. So I think largely these changes should be taken back to square one unfortunately and loop to find each of the N goals orientation's analytic expansion length (if valid) and then select the lowest cost one. Interestingly, you can use the new scoringFn to measure that for the final one to store. Luckily, there wasn't a huge number of changes you made to the analytic expansions, so its not a big setback at all and largely its just moving code around and measuring different things

So after

          while (min_turn_rad < max_min_turn_rad) {
            min_turn_rad += 0.5;  // In Grid Coords, 1/2 cell steps
            ompl::base::StateSpacePtr state_space;
            if (node->motion_table.motion_model == MotionModel::DUBIN) {
              state_space = std::make_shared<ompl::base::DubinsStateSpace>(min_turn_rad);
            } else {
              state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(min_turn_rad);
            }
            refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space);
            score = scoringFn(refined_analytic_nodes);
            if (score <= best_score) {
              analytic_nodes = refined_analytic_nodes;
              best_score = score;
            }
          }

You can use that best_score, store it for that particular angle to decide which to use.

SteveMacenski

I ran out of time this evening to review, but this is a few things -- also you have a number of linting issues I can see. Check CI for the full list of formatting problems

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

nav2_smac_planner/CMakeLists.txt

nav2_smac_planner/src/smac_planner_hybrid.cpp

SteveMacenski · 2024-02-28T02:56:11Z

Its also ready enough to update docs for the new variable for the mode to describe the mode, and the migration guide update to show this feature. An image/gif of this in action with the different modes would be great!

I looked through it and all looks good except the analytic expansions I didn't get to right now

jwallace42

Looks good!

I think you are missing some work for the distance heuristic.

The distance heuristic is pre computed based on free space. We current only calculate it for the the first goal. This means that we could artificially inflate the cost to go making the heuristic inadmissible.

My suggestion would be to remove the distance heuristic when we are in ALL_DIRECTION mode. For the BIDIRECTIONAL mode I would pre compute the distance for both angles and take the min of those two.

From what I have seen the distance heuristic is rarely greater than the obstacle heuristic so you probably haven't seen any issues.

nav2_smac_planner/src/a_star.cpp

SteveMacenski · 2024-03-08T00:58:27Z

Any questions or anything I can unblock on? 😄

mergify · 2024-03-09T19:22:11Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-09T21:54:27Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-10T04:25:16Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

stevedanomodolor · 2024-03-10T04:31:06Z

Any questions or anything I can unblock on? 😄

No blocking points, just making changes based on @jwallace42 feedback and testing them but after merging to the main and pulling the latest dockers, pr is not building.

mergify · 2024-03-10T04:36:36Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-10T19:15:18Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2024-03-11T19:52:15Z

Yeah there's a transient issue due to Rolling moving to 24.04 so a bunch of tooling and package indices are being messed with. Don't worry about it, its not your fault as long as it works locally. Just make sure to keep up on unit testing.

Want me to take a look again?

nav2_smac_planner/src/a_star.cpp

nav2_smac_planner/src/analytic_expansion.cpp

stevedanomodolor · 2024-03-12T07:47:16Z

Yeah there's a transient issue due to Rolling moving to 24.04 so a bunch of tooling and package indices are being messed with. Don't worry about it, its not your fault as long as it works locally. Just make sure to keep up on unit testing.

Want me to take a look again?

I will take advantage of the time to add more unit testing after making the modification you suggested. After the added unit tests, you can look into it.

mergify · 2024-03-23T04:32:48Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-24T13:46:14Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-24T14:08:44Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-24T15:04:30Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-24T17:49:25Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-25T19:17:28Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-25T19:33:33Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-25T19:41:28Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-25T19:59:18Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2024-03-25T20:09:28Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-03-11T21:57:17Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp

nav2_smac_planner/src/smac_planner_hybrid.cpp

nav2_smac_planner/src/analytic_expansion.cpp

SteveMacenski · 2025-03-21T22:49:57Z

I went through to summarize the remaining open items - most of which are very old:

Can you go through them and close them if they're not relevant anymore or highlight in a new comment (down here at the bottom) with anything remaining from them?

I'm also seeing for the new parameters added that they need to be added to the dynamic parameters test to exercise their lines

mergify · 2025-04-01T19:40:18Z

This pull request is in conflict. Could you fix it @stevedanomodolor?

stevedanomodolor · 2025-04-01T19:46:53Z

for #4127 (comment), i did made the changes but you mentioned in the following comment to use the current way #4127 (comment).

stevedanomodolor · 2025-04-01T19:51:15Z

I went through to summarize the remaining open items - most of which are very old:

Feat/smac planner include orientation flexibility #4127 (comment)

Feat/smac planner include orientation flexibility #4127 (comment)

Feat/smac planner include orientation flexibility #4127 (comment)

Feat/smac planner include orientation flexibility #4127 (comment)

Can you go through them and close them if they're not relevant anymore or highlight in a new comment (down here at the bottom) with anything remaining from them?

I'm also seeing for the new parameters added that they need to be added to the dynamic parameters test to exercise their lines

as for #4127 (comment), #4127 (comment) and #4127 (comment), we decided to include this in a different pr and not in this one so I will close them.

SteveMacenski

Small peanuts now, very close to being ready!

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp

nav2_smac_planner/include/nav2_smac_planner/types.hpp

nav2_smac_planner/src/smac_planner_hybrid.cpp

nav2_smac_planner/src/a_star.cpp

stevedanomodolor · 2025-04-23T03:54:21Z

@SteveMacenski just two question to finish the changes
Get rid of these 4x getters, they're unused
populate comment

SteveMacenski · 2025-04-23T19:06:39Z

@stevedanomodolor I think also the removeInvalidGoals comment and goal_manager.hpp are still open :-)

Please take a look at the linters as well

nav2_smac_planner/src/smac_planner_hybrid.cpp

nav2_smac_planner/src/smac_planner_lattice.cpp

nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp

nav2_smac_planner/src/a_star.cpp

nav2_smac_planner/src/analytic_expansion.cpp

stevedanomodolor · 2025-04-29T05:40:41Z

@SteveMacenski all changes made.

nav2_smac_planner/src/a_star.cpp

mergify · 2025-04-29T19:34:39Z

This pull request is in conflict. Could you fix it @stevedanomodolor?

mergify · 2025-04-29T20:05:25Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-04-29T20:06:25Z

This pull request is in conflict. Could you fix it @stevedanomodolor?

mergify · 2025-04-29T20:10:26Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

mergify · 2025-04-29T20:43:59Z

@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

stevedanomodolor · 2025-04-29T23:08:59Z

@SteveMacenski Updated in readme too

**********************Results Goal heading with new changes **********************
DEFAULT_Smac2d             48.463951551204566       0.1264367441153846    3.985572832589736   61.63986013986014
BIDIRECTIONAL_Smac2d       48.463951551204566       0.12651611789160838   3.985572832589736   61.63986013986014
ALL_DIRECTION_Smac2d       48.463951551204566       0.12622239968531468   3.985572832589736   61.63986013986014
DEFAULT_SmacHybrid         49.00072917359392        0.0657405418881119    1.6125494504062425  62.59090909090909
BIDIRECTIONAL_SmacHybrid   48.86220994748993        0.027206837828671327  1.465786467925095   60.33216783216783
ALL_DIRECTION_SmacHybrid   48.77456087576495        0.03619210418181818   1.6372313925886013  61.38461538461539
DEFAULT_SmacLattice        49.67700418147134        0.12979092083566435   1.6175072754351327  72.18181818181819
BIDIRECTIONAL_SmacLattice  49.83522326914245        0.027898774566433564  1.5455567606170904  70.31468531468532
ALL_DIRECTION_SmacLattice  49.91736393848793        0.03631584257342657   1.6489731848707427  71.43706293706293
-------------------------  -----------------------  --------------------  ------------------  -----------------


**********************Results Goal heading mode NO changes **********************
Read data
Planner              Average path length (m)  Average Time (s)     Average cost        Max cost
DEFAULT_Smac2d       48.463951551204566       0.1276387678706294   3.985572832589736   61.63986013986014
DEFAULT_SmacHybrid   48.47519141586789        0.06759979591608391  1.621306575384539   62.58391608391609
DEFAULT_SmacLattice  48.76214361923263        0.138069217          1.6300724505649906  72.18181818181819
-------------------  -----------------------  -------------------  ------------------  -----------------

stevedanomodolor marked this pull request as draft February 20, 2024 19:14

SteveMacenski requested changes Feb 21, 2024

View reviewed changes

SteveMacenski requested changes Feb 28, 2024

View reviewed changes

nav2_smac_planner/include/nav2_smac_planner/a_star.hpp Outdated Show resolved Hide resolved

nav2_smac_planner/CMakeLists.txt Outdated Show resolved Hide resolved

nav2_smac_planner/src/smac_planner_hybrid.cpp Outdated Show resolved Hide resolved

jwallace42 reviewed Mar 5, 2024

View reviewed changes

nav2_smac_planner/src/a_star.cpp Outdated Show resolved Hide resolved

stevedanomodolor requested review from SteveMacenski and jwallace42 March 10, 2024 22:39

SteveMacenski requested changes Mar 11, 2024

View reviewed changes

nav2_smac_planner/src/a_star.cpp Outdated Show resolved Hide resolved

nav2_smac_planner/src/a_star.cpp Outdated Show resolved Hide resolved

nav2_smac_planner/src/analytic_expansion.cpp Outdated Show resolved Hide resolved

stevedanomodolor requested a review from SteveMacenski March 15, 2025 07:01

SteveMacenski requested changes Mar 21, 2025

View reviewed changes

stevedanomodolor requested a review from SteveMacenski April 1, 2025 21:01

SteveMacenski requested changes Apr 22, 2025

View reviewed changes

SteveMacenski requested changes Apr 28, 2025

View reviewed changes

nav2_smac_planner/src/analytic_expansion.cpp Show resolved Hide resolved

nav2_smac_planner/src/analytic_expansion.cpp Outdated Show resolved Hide resolved

SteveMacenski approved these changes Apr 29, 2025

View reviewed changes

nav2_smac_planner/src/a_star.cpp Outdated Show resolved Hide resolved

nav2_smac_planner/src/a_star.cpp Outdated Show resolved Hide resolved

nav2_smac_planner/src/a_star.cpp Outdated Show resolved Hide resolved

stevedanomodolor force-pushed the feat/smac_planner_include_orientation_flexibility branch from 5548e07 to 119fbc5 Compare April 29, 2025 19:33

stevedanomodolor force-pushed the feat/smac_planner_include_orientation_flexibility branch from f417115 to d317abf Compare April 29, 2025 20:05

include functionality to allow multiple goal heading for smac planner

6eb58a9

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

stevedanomodolor force-pushed the feat/smac_planner_include_orientation_flexibility branch from 22f542f to 6eb58a9 Compare April 29, 2025 21:01

stevedanomodolor requested a review from SteveMacenski April 29, 2025 21:03

stevedanomodolor added 2 commits April 29, 2025 23:57

include missing parameter inclusion

cf8d443

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

increase test coverage

47901ad

Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

SteveMacenski approved these changes Apr 29, 2025

View reviewed changes

SteveMacenski merged commit f5543c3 into ros-navigation:main Apr 29, 2025
13 checks passed

Feat/smac planner include orientation flexibility #4127

Feat/smac planner include orientation flexibility #4127

Conversation

stevedanomodolor commented Feb 20, 2024 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

How to run

Future work that may be required in bullet points

For Maintainers:

SteveMacenski commented Feb 21, 2024 • edited Loading

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

stevedanomodolor commented Feb 21, 2024 • edited by SteveMacenski Loading

SteveMacenski commented Feb 21, 2024 • edited Loading

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski commented Feb 28, 2024

jwallace42 left a comment • edited Loading

Choose a reason for hiding this comment

SteveMacenski commented Mar 8, 2024

mergify bot commented Mar 9, 2024

mergify bot commented Mar 9, 2024

mergify bot commented Mar 10, 2024

stevedanomodolor commented Mar 10, 2024 • edited Loading

mergify bot commented Mar 10, 2024

mergify bot commented Mar 10, 2024

SteveMacenski commented Mar 11, 2024 • edited Loading

stevedanomodolor commented Mar 12, 2024

mergify bot commented Mar 23, 2024

mergify bot commented Mar 24, 2024

mergify bot commented Mar 24, 2024

mergify bot commented Mar 24, 2024

mergify bot commented Mar 24, 2024

mergify bot commented Mar 25, 2024

mergify bot commented Mar 25, 2024

mergify bot commented Mar 25, 2024

mergify bot commented Mar 25, 2024

mergify bot commented Mar 25, 2024

mergify bot commented Mar 11, 2025

SteveMacenski commented Mar 21, 2025

mergify bot commented Apr 1, 2025

stevedanomodolor commented Apr 1, 2025

stevedanomodolor commented Apr 1, 2025 • edited Loading

SteveMacenski left a comment

Choose a reason for hiding this comment

stevedanomodolor commented Apr 23, 2025

SteveMacenski commented Apr 23, 2025 • edited Loading

stevedanomodolor commented Apr 29, 2025

mergify bot commented Apr 29, 2025

mergify bot commented Apr 29, 2025

mergify bot commented Apr 29, 2025

mergify bot commented Apr 29, 2025

mergify bot commented Apr 29, 2025

stevedanomodolor commented Apr 29, 2025

stevedanomodolor commented Feb 20, 2024 •

edited

Loading

SteveMacenski commented Feb 21, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

stevedanomodolor commented Feb 21, 2024 •

edited by SteveMacenski

Loading

SteveMacenski commented Feb 21, 2024 •

edited

Loading

jwallace42 left a comment •

edited

Loading

stevedanomodolor commented Mar 10, 2024 •

edited

Loading

SteveMacenski commented Mar 11, 2024 •

edited

Loading

stevedanomodolor commented Apr 1, 2025 •

edited

Loading

SteveMacenski commented Apr 23, 2025 •

edited

Loading