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).

SteveMacenski · 2024-11-20T23:12:10Z

Yeah that sounds good to me. I generally agree that this is probably the issue, I just want to know for sure before we assume too much :-)

I'm trying to think of some strategies to speed this up (if this is the case): Check every-Nth bin rather than all 72? Full resolution granularity when you don't care about your orientation at all seems over kill. Then we could do a refinement step to check in the sliver that has the 2 lowest bounding costs. So for example, we check every 5th bin (drops the cost for checking during search when unsuccessful by 4/5ths). If any are valid, we check the bounding range only between the 2 shortest ones.

That way, normal searches that have no expanions possible completes N-1/Nx faster and we only do the dense checking for a smaller sliver once we know there is any valid solution. I'd aruge the probability that we have some single-orientation solution to the planning problem that its 2 neighbors on either side are not valid at all for is virtually non-existent in practice -- which in exchange we speed up our planning time again

stevedanomodolor · 2024-11-25T21:02:19Z

Yeah that sounds good to me. I generally agree that this is probably the issue, I just want to know for sure before we assume too much :-)

You are right on this. I assumed wrong. I did the following test for all the cases: default, bidirectional, and all_direction for the two planners. The start and end are on the left.

I measured the total time it took to call both the get_expansion and get_heuristic functions. The increase in execution time for both functions is more or less the same, but the get_heuristic function showed a significantly increase in comparison. This occurs in the all_direction mode, as the increase is not as pronounced in the bidirectional mode. By the way, I am using the default parameters provided on the Nav2 website. What I find strange is that the SMAC Hybrid planner performed much worse in all_direction mode compared to the SMAC Lattice planner.

<title></title>
<meta name="generator" content="LibreOffice 24.2.6.2 (Linux)"/>
<style type="text/css">
	body,div,table,thead,tbody,tfoot,tr,th,td,p { font-family:"Liberation Sans"; font-size:x-small }
	a.comment-indicator:hover + comment { background:#ffd; position:absolute; display:block; border:1px solid black; padding:0.5em;  } 
	a.comment-indicator { background:red; display:inline-block; border:1px solid black; width:0.5em; height:0.5em;  } 
	comment { display:none;  } 
</style>

	get heuristic time(ms)	get expansion time(ms)	?x heuristic	?x expansion
Default hybrid	0.841365	1.427716
Bidirectional hybrid	0.95309	1.517734	1.13279016835737	1.06305035455231
All direction hybrid	7.583123	12.685215	9.01288144859841	8.88497082052733
Default lattice	0.600979	1.27284
Bidirectional lattice	1.057823	1.427675	1.76016632860716	1.12164529713083
All direction lattice	2.145226	2.166438	3.569552347087	1.70205053266711

I'm trying to think of some strategies to speed this up (if this is the case): Check every-Nth bin rather than all 72? Full resolution granularity when you don't care about your orientation at all seems over kill. Then we could do a refinement step to check in the sliver that has the 2 lowest bounding costs. So for example, we check every 5th bin (drops the cost for checking during search when unsuccessful by 4/5ths). If any are valid, we check the bounding range only between the 2 shortest ones.

That way, normal searches that have no expanions possible completes N-1/Nx faster and we only do the dense checking for a smaller sliver once we know there is any valid solution. I'd aruge the probability that we have some single-orientation solution to the planning problem that its 2 neighbors on either side are not valid at all for is virtually non-existent in practice -- which in exchange we speed up our planning time again

Would try this, it should improve the overall I believe.

SteveMacenski · 2024-11-26T22:33:10Z

What I find strange is that the SMAC Hybrid planner performed much worse in all_direction mode compared to the SMAC Lattice planner.

The Hybrid-A* can have 64, 72, or other total bins it needs to check. The state lattice planner typically has ~16 since the primitives have to create a regularized lattice structure. You could do more, but typically 16 is sufficient. If you reduced Hybrid-A* to 16, do you see a similar result as the state lattice planner?

SteveMacenski · 2024-12-05T18:28:41Z

To summarize our call: We should do a course-to-fine search rather than going for all N (72) bins. We could introduce a new parameter for the analytic expansion that is the coarse search resolution (default: 4). If we find any valid path in that coarse search, then we do the full resolution. If we make it default to 4, it should have similar characteristics to the state lattice performance.

This should be relatively easy to implement using a queue and a while loop to replace the for loop, where it is initialized with every Nth goal orientation to start with. When the queue is empty at the end of the while loop, we can check if any expansion is valid. If so, then queue up the remaining non-Nth goal.

:-)

stevedanomodolor · 2024-12-15T18:32:00Z

What I find strange is that the SMAC Hybrid planner performed much worse in all_direction mode compared to the SMAC Lattice planner.

The Hybrid-A* can have 64, 72, or other total bins it needs to check. The state lattice planner typically has ~16 since the primitives have to create a regularized lattice structure. You could do more, but typically 16 is sufficient. If you reduced Hybrid-A* to 16, do you see a similar result as the state lattice planner?

yes, get the same result

SteveMacenski · 2024-12-17T19:30:33Z

Oh great - that means what we discussed will almost definitely improve performance to that level!

stevedanomodolor · 2024-12-18T07:37:31Z

I tried the improvement, and it did reduce the overall time. 🎉

Now, I am testing another idea to further reduce the time for the distance heuristic. What do you think:

The main issue with the current method of storing a loopup table that contains the minimum values for all directions is that the chosen minimum goal might still end up in collision. In addition during the isInputValid check, we discard goals that are in collision. As a result, the "minimum" value stored may not be valid.

I propose the following, the first lookup table stores the distance as is now. In addition to this, i create another lookup table that for each position (x, y, θ) to (x,y,theta) i store the index of the position of the loop up table but sorted from minimum to maximum distance.

During the isInputValid step, I create a boolean list that marks which goals are invalid (i.e., in collision) and gives this information to the distance heuristic.

Using the Second Table, when evaluating the getDistance heuristic, we iterate through the indices in the second lookup table for the given (x, y, θ). We pick the first index corresponding to a goal that is not marked as invalid in the boolean list.

I tried before to create another lookup table during the is input valid, but its takes a long time to do that loop.

SteveMacenski · 2024-12-19T19:15:13Z

I tried the improvement, and it did reduce the overall time. 🎉

Yay! What's it look now?

What do you think

I think I understand: You want to keep the current lookup table so that you can access any point, orientation to query (HLUT1). Then, create a second lookup table that contains for each X,Y cell a vector of indexes of HLUT1, sorted by distance (HLUT2). So the size would be the same as HLUT, but store a vector rather than a float.

This is where you lose me.

During the isInputValid step, I create a boolean list that marks which goals are invalid (i.e., in collision) and gives this information to the distance heuristic.

Using the Second Table, when evaluating the getDistance heuristic, we iterate through the indices in the second lookup table for the given (x, y, θ). We pick the first index corresponding to a goal that is not marked as invalid in the boolean list.

Even if you had this, HLUT2 is sorted by distance. How do you know which distance belongs to which orientation? They're no longer ordered by orientation.

You would probably need to store a struct that contains the distance and the bin/orientation information. If you had that, I don't think you'd need the bool list, since you could just match any input node information to the struct's information. You'd march in order until you find the first bool list that's valid, likely just a few. However, you're now storing more information.

Or, don't sort it at all and go through the vector and find the min that meets the condition that has its corresponding bool list value is true. Then the bool list is needed and would require a full iteration of the vector.

I'd consider the memory impact of such a lookup table. If we have a HLUT dimension of 200x200 (10m at 5cm resolution) of floats, that's 0.32Mb. If we have each orientation stored as HLUT2 would have, say 72 as default, thats now 23Mb. For a HLUT of 20m x 20m (our default), that's 1.28Mb and 92Mb respectively. Double all these numbers if you store a float or int for the distance and some cell index information in a struct.

Assuming that's an OK amount of memory to store, accessing a memory block of that size, then iterating through the vector may actually turn out to be slower than just iterating through HLUT1 72 times.

Or not, we can't know without trying! Its a good idea and alot of these kind of things in Smac were done by 'just trying it out' and seeing what the results were

Separately, depending on the metrics from the change you made that worked, we may not need to pull out additional performance improvements by pushing more into lookup tables. But, we certainly could.

I think if we did something like that, it should be in a follow up PR as to not muddle up the great work done here and decouple the complexity a bit. The for each goal orientation -> find minimum distance heuristic loop seems performant enough for a first stab.

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

stevedanomodolor · 2025-01-21T21:42:02Z

I tried the improvement, and it did reduce the overall time. 🎉

Yay! What's it look now?

What do you think

I think I understand: You want to keep the current lookup table so that you can access any point, orientation to query (HLUT1). Then, create a second lookup table that contains for each X,Y cell a vector of indexes of HLUT1, sorted by distance (HLUT2). So the size would be the same as HLUT, but store a vector rather than a float.

This is where you lose me.

During the isInputValid step, I create a boolean list that marks which goals are invalid (i.e., in collision) and gives this information to the distance heuristic.

Using the Second Table, when evaluating the getDistance heuristic, we iterate through the indices in the second lookup table for the given (x, y, θ). We pick the first index corresponding to a goal that is not marked as invalid in the boolean list.

Even if you had this, HLUT2 is sorted by distance. How do you know which distance belongs to which orientation? They're no longer ordered by orientation.

You would probably need to store a struct that contains the distance and the bin/orientation information. If you had that, I don't think you'd need the bool list, since you could just match any input node information to the struct's information. You'd march in order until you find the first bool list that's valid, likely just a few. However, you're now storing more information.

Or, don't sort it at all and go through the vector and find the min that meets the condition that has its corresponding bool list value is true. Then the bool list is needed and would require a full iteration of the vector.

I'd consider the memory impact of such a lookup table. If we have a HLUT dimension of 200x200 (10m at 5cm resolution) of floats, that's 0.32Mb. If we have each orientation stored as HLUT2 would have, say 72 as default, thats now 23Mb. For a HLUT of 20m x 20m (our default), that's 1.28Mb and 92Mb respectively. Double all these numbers if you store a float or int for the distance and some cell index information in a struct.

Assuming that's an OK amount of memory to store, accessing a memory block of that size, then iterating through the vector may actually turn out to be slower than just iterating through HLUT1 72 times.

Or not, we can't know without trying! Its a good idea and alot of these kind of things in Smac were done by 'just trying it out' and seeing what the results were

Separately, depending on the metrics from the change you made that worked, we may not need to pull out additional performance improvements by pushing more into lookup tables. But, we certainly could.

I think if we did something like that, it should be in a follow up PR as to not muddle up the great work done here and decouple the complexity a bit. The for each goal orientation -> find minimum distance heuristic loop seems performant enough for a first stab.

will sync with latest and test again and show the results.

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

…ility

stevedanomodolor · 2025-01-23T19:24:52Z

@SteveMacenski Finally got some good results.

Before optimization

-------------------------  -----------------------  -------------------  ------------------  -----------------
Planner                    Average path length (m)  Average Time (s)     Average cost        Max cost
ALL_DIRECTION_SmacLattice  48.108264861507294       0.12122226572108843  2.300340138637709   71.61904761904762
ALL_DIRECTION_SmacHybrid   47.91328895624761        0.05004659965986395  1.7206390472833961  62.42176870748299
-------------------------  -----------------------  -------------------  ------------------  -----------------

After Optimization

------------------------  -----------------------  -------------------  ------------------  -----------------
Planner                    Average path length (m)  Average Time (s)     Average cost        Max cost
ALL_DIRECTION_SmacLattice  48.2876931460346         0.07347638301020408  2.606052742605017   77.02721088435374
ALL_DIRECTION_SmacHybrid   48.12612569861988        0.04729291806122449  2.4812420091811043  68.2312925170068
-------------------------  -----------------------  -------------------  ------------------  -----------------

Will cleanup and push for review

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

SteveMacenski

Overall looks really good to me, I have a couple of small changes and after that the next review I'll go into detail line by line to make sure we didn't miss anything subtle, but its largely code I've already reviewed pre-coarse search and approved, so its more just for my piece of mind (and don't expect to find any issues).

Please pull in the updates from the main branch (some smac optimizations were made) and benchmark against your changes with main pulled in and the new main itself. If they are similar/same/better, then this is good to go!

SteveMacenski · 2025-01-29T19:21:42Z

nav2_smac_planner/src/smac_planner_lattice.cpp

+  _goal_heading_mode = fromStringToGH(goal_heading_type);
+
+  nav2_util::declare_parameter_if_not_declared(
+    node, name + ".coarse_search_resolution", rclcpp::ParameterValue(4));


for both planners: check that this is an exact increment of the lattice or hybrid primitive set (i.e. equally divisible so that we don't end up with like prim_set.size() / coarse_resolution = 2.3535. It should be 2 or 3). Otherwise, in the code, we need to explicitly set a policy for handling this. overall though, I think its a misconfiguration if someone selects something non-exact, so throwing and failing to configure / do a param update seems OK

Also for the lattice planner: I think this should default to the lattice's size and not do coarse resolution (so default = 1 whereas hybrid = 4). The doc PR should be updated accordingly

SteveMacenski · 2025-01-29T19:27:34Z

nav2_smac_planner/src/smac_planner_lattice.cpp

+      _logger, "coarse iteration resolution selected as <= 0, "
+      "disabling coarse iteration resolution search for goal heading"
+    );
+    _coarse_search_resolution = 0;


Here and in the other file (and in the dynamic parameters): Shouldn't this be 1? I don't think 0 is a valid configuration. I know you error check for 0 to replace with 1, but I think we should still set 1 (and have the 0 check throw an exception?)

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 added 2 commits December 24, 2024 02:17

include setup for optimization

2e598e6

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

wip

37211a2

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

stevedanomodolor added 2 commits January 21, 2025 22:42

wip

8be1add

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

Merge branch 'main' into feat/smac_planner_include_orientation_flexib…

8f3c4b9

…ility

stevedanomodolor added 3 commits January 23, 2025 20:28

wip

5ca6dfe

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

revert back to main

5ff4de0

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

cleanup code after optimization

4310427

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

stevedanomodolor force-pushed the feat/smac_planner_include_orientation_flexibility branch from 970dc17 to 4310427 Compare January 25, 2025 08:01

stevedanomodolor added 9 commits January 25, 2025 09:43

add more converage

42f796d

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

linter fixes

74c6dc8

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

add tests

ffabd23

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

fix test

d24246e

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

include more tests

8fc82b0

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

fix test

4fee5f9

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

fix test

a88928e

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

update test file

f358765

cahnge to reszie

b81f52e

stevedanomodolor requested a review from SteveMacenski January 25, 2025 21:02

stevedanomodolor marked this pull request as ready for review January 25, 2025 21:03

SteveMacenski reviewed Jan 29, 2025

View reviewed changes

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

SteveMacenski commented Nov 20, 2024 • edited Loading

stevedanomodolor commented Nov 25, 2024

SteveMacenski commented Nov 26, 2024 • edited Loading

SteveMacenski commented Dec 5, 2024

stevedanomodolor commented Dec 15, 2024

SteveMacenski commented Dec 17, 2024

stevedanomodolor commented Dec 18, 2024

SteveMacenski commented Dec 19, 2024 • edited Loading

stevedanomodolor commented Jan 21, 2025

stevedanomodolor commented Jan 23, 2025

SteveMacenski left a comment

Choose a reason for hiding this comment

SteveMacenski Jan 29, 2025 • edited Loading

Choose a reason for hiding this comment

SteveMacenski Jan 29, 2025 • edited Loading

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

SteveMacenski commented Nov 20, 2024 •

edited

Loading

SteveMacenski commented Nov 26, 2024 •

edited

Loading

SteveMacenski commented Dec 19, 2024 •

edited

Loading

SteveMacenski Jan 29, 2025 •

edited

Loading

SteveMacenski Jan 29, 2025 •

edited

Loading