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 mode Default **********************
Read data
-------------------  -----------------------  -------------------  ------------------  ------------------
Planner              Average path length (m)  Average Time (s)     Average cost        Max cost
DEFAULT_SmacHybrid   48.59179172137297        0.08409778965140845  1.0656706399532687  54.59154929577465
DEFAULT_SmacLattice  49.30762557367365        0.4135887128204226   0.8137733557552133  53.813380281690144
DEFAULT_Smac2d       48.52958672840513        0.12046826587323943  0.7580233687132835  65.47183098591549
-------------------  -----------------------  -------------------  ------------------  ------------------
**********************Results Goal heading mode bidrectional **********************
Read data
-------------------------  -----------------------  -------------------  ------------------  -----------------
Planner                    Average path length (m)  Average Time (s)     Average cost        Max cost
BIDIRECTIONAL_SmacHybrid   48.72823963695479        0.03030190022535211  1.0223910481559262  54.13028169014085
BIDIRECTIONAL_SmacLattice  49.50773050762388        0.17851250328169013  0.7209227860091986  51.23943661971831
BIDIRECTIONAL_Smac2d       48.52960037283171        0.12235441819718311  0.7580233687132835  65.47183098591549
-------------------------  -----------------------  -------------------  ------------------  -----------------
**********************Results Goal heading mode all direction **********************
Read data
-------------------------  -----------------------  --------------------  ------------------  -----------------
Planner                    Average path length (m)  Average Time (s)      Average cost        Max cost
ALL_DIRECTION_SmacHybrid   48.72823963695479        0.031168896616197185  1.0223910481559262  54.13028169014085
ALL_DIRECTION_SmacLattice  49.50773050762388        0.18361958839788733   0.7209227860091986  51.23943661971831
ALL_DIRECTION_Smac2d       48.52960037283171        0.12754024430633804   0.7580233687132835  65.47183098591549


**********************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 · 2024-06-11T23:44:17Z

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

Signed-off-by: stevedan <[email protected]>

stevedanomodolor · 2024-10-10T11:28:27Z

Ah got it, thanks! I was looking in the package's readme. I'm surprised its so much faster, that's a cool result!

The one on one comparison is slightly different, but there is more gap between goal heading bidrectional vs default because it most likely found a faster solution.

codecov · 2024-10-10T11:28:33Z

Welcome to Codecov 🎉

Once you merge this PR into your default branch, you're all set! Codecov will compare coverage reports and display results in all future pull requests.

ℹ️ You can also turn on project coverage checks and project coverage reporting on Pull Request comment

Thanks for integrating Codecov - We've got you covered ☂️

stevedanomodolor · 2024-10-10T11:37:32Z

@SteveMacenski There is an issue in simulation where when you launch the simulation tb3/tb4 and you include the docking plugin(

navigation2/nav2_bringup/rviz/nav2_default_view.rviz

Line 28 in 6311978

- Class: nav2_rviz_plugins/Docking

) in the rviz param, rviz crashes on launch. So i tested without that. I guess #4717 is working on a solution. It has nothing to do with this pr, but so you are aware of that.

SteveMacenski · 2024-10-16T21:14:19Z

That PR was merged, can you try then with the updates?

SteveMacenski

What is the performance change to doing the for loop across all goals? The current status of main would be analog to the performance of storing the distances in the distance lookup table with the orientation policy that we had previously discussed. If its not a major departure in performance for bidirectionality or all angles - then I like this option as the last intrusive!

nav2_smac_planner/src/a_star.cpp

SteveMacenski · 2024-10-16T21:17:19Z

nav2_smac_planner/src/a_star.cpp

+  // we just have to check whether the x and y are the same because the dim3 is not used
+  // in the computation of the obstacle heuristic
+  if (!_search_info.cache_obstacle_heuristic ||
+    (goals_coordinates[0].x != _goals_coordinates[0].x &&


I believe we have the == operator for this?

It did not make sense to do the yaw comparison because it is not necessary for the obstacle heuristic.

This is just checking is the goal is updated at all to recompute based on a new request. Surely you're right that the yaw isn't an important characteristic, but it makes the code more ergonomic and understandable to compare the goals directly.

I think this was the initial comment that let to this decision. #4127 (comment). I do understand where you are coming from though, I can change it

@stevedanomodolor is this done?

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Stevedan Ogochukwu Omodolor <[email protected]>

Remove need for void

stevedanomodolor · 2024-10-17T06:43:07Z

What is the performance change to doing the for loop across all goals? The current status of main would be analog to the performance of storing the distances in the distance lookup table with the orientation policy that we had previously discussed. If its not a major departure in performance for bidirectionality or all angles - then I like this option as the last intrusive!

So far in the planner tests, I haven't observed a significant increase in computation cost, as mentioned in the PR description. This might be because the planner is finding the shortest path more efficiently. For example, in a scenario where a robot is moving from (0,0,0) to (10,0,180), a straight path, bidirectional and all-direction planners are likely faster. However, I haven't yet conducted a direct head-to-head comparison where I fix all goals to the shortest path and measure the computation time. I plan to do that and will update you with the results. I believe that would provide a more meaningful comparison.

SteveMacenski · 2024-10-17T16:33:20Z

You can do this with the tools/planner_benchmark scripts actually pretty easily too :-) Adding in obstacles and complications rather than open or largely-open spaces changes the compute characteristics alot since it needs to explore more directions and evaluate the heuristic's efficiency

Are all the metrics in the PR description updated for the current state? I know we explored another direction earlier with caching the distances based on the mode in the LUT, but now we're looping over the N goals. The compute time difference for each request is low, but since this is called hundreds of thousands of times in a sub-1s iteration, that could potentially be a meaningful contribution. This is the kind of thing that makes Smac, MPPI, and DWB difficult - the initial code was done relatively quickly (~1 month) but then several months in optimizing-benchmarking-profiling loops

…entation_flexibility

stevedanomodolor · 2024-10-18T06:31:30Z

You can do this with the tools/planner_benchmark scripts actually pretty easily too :-) Adding in obstacles and complications rather than open or largely-open spaces changes the compute characteristics alot since it needs to explore more directions and evaluate the heuristic's efficiency

Are all the metrics in the PR description updated for the current state? I know we explored another direction earlier with caching the distances based on the mode in the LUT, but now we're looping over the N goals. The compute time difference for each request is low, but since this is called hundreds of thousands of times in a sub-1s iteration, that could potentially be a meaningful contribution. This is the kind of thing that makes Smac, MPPI, and DWB difficult - the initial code was done relatively quickly (~1 month) but then several months in optimizing-benchmarking-profiling loops

They are not in the current state as in I did some minor changes, not any change that would affect the computation time although I admit I should do the tests again. I noticed that the last time I did the test, the computation time in both the all-direction and the bidirectional was less. I have the feeling this is because in most cases the goal with these two is so short that the overall computation is less. This does not tell me what we want to know, whether it is worth storing in memory as the other alternative. When I get the head-to-head comparison, I will let you know. I think that would make more sense to decide whether it make sense to store it in memory or not.

stevedanomodolor · 2024-10-18T12:33:39Z

@SteveMacenski Ok, so this is the direct-to-direct comparison to check that the changes do not influence the current implementation. Btw I use the same generated random goal to test everything, so I do not generate random goal everytime I run a time but use an already generated random goal.

Without my change

-----------  -----------------------  -------------------  ------------------  -----------------
Planner      Average path length (m)  Average Time (s)     Average cost        Max cost
Smac2d       48.47634783071699        0.14292506476842104  4.675870855208096   64.52631578947368
SmacHybrid   48.54287554354138        0.0894882033859649   1.9459911638993865  66.07368421052631
SmacLattice  48.78772473657533        0.2229827250982456   2.377393125487587   74.50175438596492
-----------  -----------------------  -------------------  ------------------  -----------------

With my change

-------------------  -----------------------  -------------------  ------------------  -----------------
Planner              Average path length (m)  Average Time (s)     Average cost        Max cost
DEFAULT_Smac2d       48.47634783071699        0.14277166064912283  4.675870855208096   64.52631578947368
DEFAULT_SmacHybrid   48.54287554354138        0.08836804374385963  1.9459911638993865  66.07368421052631
DEFAULT_SmacLattice  48.78772473657533        0.22034927409122806  2.377393125487587   74.50175438596492
-------------------  -----------------------  -------------------  ------------------  -----------------

The following does the comparison with the default and the others with changes made.

-------------------------  -----------------------  -------------------  -------------------  ------------------
Planner                    Average path length (m)  Average Time (s)     Average cost         Max cost
DEFAULT_Smac2d             48.49937498442165        0.15631528099650352  0.05049418373739123  13.381118881118882
BIDIRECTIONAL_Smac2d       48.49937498442165        0.15650485366433567  0.05049418373739123  13.381118881118882
ALL_DIRECTION_Smac2d       48.49937498442165        0.15685676974825175  0.05049418373739123  13.381118881118882
DEFAULT_SmacHybrid         48.563366720247004       0.09627746137762239  0.2945215688400506   21.594405594405593
BIDIRECTIONAL_SmacHybrid   48.256336211432114       0.03466704658041958  0.3090170694824764   20.44055944055944
ALL_DIRECTION_SmacHybrid   48.26032337153972        0.10581961293006992  0.14706941671424442  18.3986013986014
DEFAULT_SmacLattice        48.813321606346285       0.24365637090909092  0.2288637531266722   21.821678321678323
BIDIRECTIONAL_SmacLattice  48.428891891564554       0.04885632574125874  0.16030897287741855  19.346153846153847
ALL_DIRECTION_SmacLattice  48.361663423426066       0.05500744572727272  0.1408372526650894   18.346153846153847
-------------------------  -----------------------  -------------------  -------------------  ------------------

The only big influence is the hybrid where the all direction is slightly higher than default. It still missing the test head to head with the solution been the optimal for all the cases as I mentioned previously

stevedanomodolor · 2024-10-18T12:57:12Z

Did the head to head comparision as I said with no obstacle just pure straight line optimal for all.

Read data
-------------------------  -----------------------  ----------------  ------------  --------
Planner                    Average path length (m)  Average Time (s)  Average cost  Max cost
DEFAULT_Smac2d             2.088489204723648        0.00126962        255.0         255.0
BIDIRECTIONAL_Smac2d       2.088489204723648        0.001302083       255.0         255.0
ALL_DIRECTION_Smac2d       2.088489204723648        0.001256431       255.0         255.0
DEFAULT_SmacHybrid         2.0000000298023224       0.001611635       255.0         255.0
BIDIRECTIONAL_SmacHybrid   2.0000000298023224       0.001359679       255.0         255.0
ALL_DIRECTION_SmacHybrid   2.0861568852255252       0.026456966       255.0         255.0
DEFAULT_SmacLattice        2.0000000298023224       0.004581189       255.0         255.0
BIDIRECTIONAL_SmacLattice  2.0000000298023224       0.001838556       255.0         255.0
ALL_DIRECTION_SmacLattice  2.047192655617898        0.007660778       255.0         255.0

THe "all directions increased" did increase as expected. I will try caching based on orientation to compare the computation time at least and let you know.

SteveMacenski · 2024-11-08T19:26:59Z

@stevedanomodolor I'm back from ROSCon / ROS-I now and will give this a final, complete review. I went through our thread above and found 5 open comments that I tagged you in as needing some update or confirmation that the item was completed and can be marked as resolved (or needs to be still addressed).

Btw I use the same generated random goal to test everything, so I do not generate random goal everytime I run a time but use an already generated random goal.

Why?

THe "all directions increased" did increase as expected. I will try caching based on orientation to compare the computation time at least and let you know.

Ok :-) That is a pretty big 10x jump that's worth at least understanding

SteveMacenski

Some details:

I'm seeing some blocks that are untested in the annotations. It might be worth reviewing the code coverage results and adding some tests to cover any glaring gaps
But overall, this looks pretty much perfect to me now. Doing the final benchmarking (with changing the goal locations too on that random obstacle map) to sanity check finally that this all works well and doesn't mess up run-time implementations
Add migration guide updates for this new feature LGTM! And configuration guide for the new parameter :-)

SteveMacenski · 2024-11-08T19:34:46Z

nav2_smac_planner/src/smac_planner_hybrid.cpp

+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT"));
+  node->get_parameter(name + ".goal_heading_mode", goal_heading_type);
+  GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);


Suggested change

GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);

_goal_heading_mode = fromStringToGH(goal_heading_type);

Then you can remove the else and still check _goal_heading_mode for the UNKOWN condition. Its more concise

SteveMacenski · 2024-11-08T19:35:07Z

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -648,6 +661,22 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
            "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
            _motion_model_for_search.c_str());
        }
+      } else if (name == _name + ".goal_heading_mode") {


Keep as-is here, having the local variable makes sense :-)

SteveMacenski · 2024-11-08T19:35:14Z

nav2_smac_planner/src/smac_planner_lattice.cpp

+  node->get_parameter(name + ".goal_heading_mode", goal_heading_type);
+  GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);
+  goal_heading_mode = fromStringToGH(goal_heading_type);
+  if (goal_heading_mode == GoalHeadingMode::UNKNOWN) {


Ditto in this file

SteveMacenski · 2024-11-08T19:37:49Z

nav2_smac_planner/src/a_star.cpp

+      break;
+    case GoalHeadingMode::UNKNOWN:
+      throw std::runtime_error("Goal heading is UNKNOWN.");
+      break;


Don't need a break after a throw

SteveMacenski · 2024-11-08T19:39:55Z

nav2_smac_planner/src/a_star.cpp

    throw std::runtime_error("Failed to compute path, no valid start or goal given.");
  }
-


readd the line for separating the code blocks

SteveMacenski · 2024-11-08T19:41:49Z

nav2_smac_planner/src/analytic_expansion.cpp

    // 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)));
-


Readd code block separation line

SteveMacenski · 2024-11-08T19:43:56Z

nav2_smac_planner/src/analytic_expansion.cpp

-            if (refined_analytic_nodes.empty()) {
+      for (auto goal_node : goals_node) {
+        // Reset the counter and try the analytic path expansion
+        analytic_iterations = desired_iterations;


I don't think this reset has to be in the for loop?

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 Show resolved Hide resolved

nav2_smac_planner/src/analytic_expansion.cpp Show resolved Hide resolved

wip

232a1a7

Signed-off-by: stevedan <[email protected]>

stevedanomodolor marked this pull request as draft October 8, 2024 21:43

stevedanomodolor added 3 commits October 10, 2024 12:34

fix bidirectional goal

05262f5

Signed-off-by: stevedan <[email protected]>

return config back to normal

14a1441

Signed-off-by: stevedan <[email protected]>

remove duplicate vector

5ce2c7e

Signed-off-by: stevedan <[email protected]>

stevedanomodolor force-pushed the feat/smac_planner_include_orientation_flexibility branch from aa01e64 to 5ce2c7e Compare October 10, 2024 10:45

stevedanomodolor added 4 commits October 10, 2024 12:57

code cleanup

fe5dc61

Signed-off-by: stevedan <[email protected]>

missing param

3607b00

Signed-off-by: stevedan <[email protected]>

insert missing end of file newline

5939bf2

Signed-off-by: stevedan <[email protected]>

remove extra space

cd631f5

Signed-off-by: stevedan <[email protected]>

stevedanomodolor closed this Oct 10, 2024

stevedanomodolor reopened this Oct 10, 2024

stevedanomodolor marked this pull request as ready for review October 10, 2024 11:28

stevedanomodolor requested a review from SteveMacenski October 10, 2024 11:31

SteveMacenski requested changes Oct 16, 2024

View reviewed changes

stevedanomodolor and others added 2 commits October 17, 2024 08:30

Update nav2_smac_planner/src/a_star.cpp

c0ac181

Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Stevedan Ogochukwu Omodolor <[email protected]>

Update a_star.cpp

b18ba84

Remove need for void

Merge branch 'ros-navigation:main' into feat/smac_planner_include_ori…

92e2ee8

…entation_flexibility

SteveMacenski requested changes Nov 8, 2024

View reviewed changes

	GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type);
	_goal_heading_mode = fromStringToGH(goal_heading_type);

		throw std::runtime_error("Failed to compute path, no valid start or goal given.");
		}

Feat/smac planner include orientation flexibility #4127

Are you sure you want to change the base?

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 Jun 11, 2024

stevedanomodolor commented Oct 10, 2024 • edited Loading

codecov bot commented Oct 10, 2024 • edited Loading

Welcome to Codecov 🎉

stevedanomodolor commented Oct 10, 2024

SteveMacenski commented Oct 16, 2024

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

stevedanomodolor commented Oct 17, 2024

SteveMacenski commented Oct 17, 2024 • edited Loading

stevedanomodolor commented Oct 18, 2024

stevedanomodolor commented Oct 18, 2024 • edited Loading

stevedanomodolor commented Oct 18, 2024 • edited Loading

SteveMacenski commented Nov 8, 2024 • edited Loading

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

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 Oct 10, 2024 •

edited

Loading

codecov bot commented Oct 10, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

SteveMacenski commented Oct 17, 2024 •

edited

Loading

stevedanomodolor commented Oct 18, 2024 •

edited

Loading

stevedanomodolor commented Oct 18, 2024 •

edited

Loading

SteveMacenski commented Nov 8, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading