Feat/smac planner include orientation flexibility

[stevedanomodolor]

---

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]

@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!

[stevedanomodolor]

@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]

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

[SteveMacenski]

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.

[SteveMacenski]

Any questions or anything I can unblock on? 😄

[mergify]

@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]

@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]

@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]

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]

@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]

@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]

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?

[stevedanomodolor]

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]

@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]

@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]

@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]

@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]

@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]

@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]

@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]

@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]

@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]

@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]

@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 :-)

[stevedanomodolor]

@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?

I just wanted to use the same goals to benchmark the current state of the stack and the changes I made.

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

I did some tests and got the following conclusions,

• Assuming we did the precomputation in the initial stage as is done now
  The issue with this is that we assume that all the possible angles do not collide with the goal which is not the case because during the isinput valid stage, we remove goals in collision. This means that this includes a level of complexity where, we have to store not the minimal but somehow the minimal and also the angle associated with it so that during the isinputvalid stage, we can assign this value. Before I did this, to see if its worth adding this level of complexity, I assumed there was no obstacle and tested with no obstacle. So I stored the distances as follows (see the other branch)

• bidirectional -> array of dim x,y,thetha/2
• All directions-> array of dim x,y
  There was no significant difference between the computational time which makes me believe that the bottleneck in all directions is not in the distance heuristic computation stage but most probably is in the expansion stage in the planning loop.

• I did try to compute the minimal during the isinputvalid stage but the computation time was too big so this option did not make any sense.

I would consider as a next task, maybe a different pr to consider techniques to optimize the expansion stage in the planning loop when using all directions because we don¡t have to expand to all the directions and just expand to the x best goals for example.

[SteveMacenski]

I just wanted to use the same goals to benchmark the current state of the stack and the changes I made.

But if you use a single goal, its not necessarily representative of randomness. If you have the same random seed at the start, then it should be deterministic.

The issue with this is that we assume that all the possible angles do not collide with the goal which is not the case because during the isinput valid stage, we remove goals in collision.

I guess this is where I'd handwave that the heuristic is an estimate, not an exact science - but this is a good point and a reason that if we don't see performance degradation we could do the for loop over each of the goal orientations for the distance heuristic, we can/should do that (which also simplifies code changes, which makes me happy 😄 ).

Why is that one ALL_DIRECTION_SmacHybrid so much higher than the others (20x)? This is also where maybe the goal non-randomness is working against you (the goal is at a really inconvenient location) rather than an actual problem in the software? I don't know though.

There was no significant difference between the computational time which makes me believe that the bottleneck in all directions is not in the distance heuristic computation stage but most probably is in the expansion stage in the planning loop.

It would be good to understand this to know that's the reason with some confidence. I don't disagree that could be a future improvement, but I'd like for us to at least understand if this is a correct assumption (and if not, maybe the issue is somewhere we can fix easily)

[stevedanomodolor]

I just wanted to use the same goals to benchmark the current state of the stack and the changes I made.

But if you use a single goal, its not necessarily representative of randomness. If you have the same random seed at the start, then it should be deterministic.

The issue with this is that we assume that all the possible angles do not collide with the goal which is not the case because during the isinput valid stage, we remove goals in collision.

I guess this is where I'd handwave that the heuristic is an estimate, not an exact science - but this is a good point and a reason that if we don't see performance degradation we could do the for loop over each of the goal orientations for the distance heuristic, we can/should do that (which also simplifies code changes, which makes me happy 😄 ).

Why is that one ALL_DIRECTION_SmacHybrid so much higher than the others (20x)? This is also where maybe the goal non-randomness is working against you (the goal is at a really inconvenient location) rather than an actual problem in the software? I don't know though.

I dont think it is though because i did two tests.

• generated lots of random start and goal position using the map that is currently used for benchmarking the planners and still got similar results where the all directions is high. This was the initial result i sent.
• the second one that i did was a known start and end goal with no obstacle. The reason i chose this to do the optimization is because in the previous case, there was a chance that the goalsset might be reduced because it might be in collision. This way I know that i am working in the worse case where there is no obstacle and i expand to all the bins. I did this just to have a reference. This is just for this test specifically.

I think the problem is we are trying to expand to multiple goals, we are jumping from 1 goals to the worst case 72 different goals(if we have 72 bins).
In both cases the all directions was significantly higher, the second is higher because no goal in collision and we are using all the goalset. This is what i suspect.

There was no significant difference between the computational time which makes me believe that the bottleneck in all directions is not in the distance heuristic computation stage but most probably is in the expansion stage in the planning loop.

It would be good to understand this to know that's the reason with some confidence. I don't disagree that could be a future improvement, but I'd like for us to at least understand if this is a correct assumption (and if not, maybe the issue is somewhere we can fix easily)

I think i can do just that, i will time the functions in the planning loop to see where rhe bottleneck is.

[SteveMacenski]

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]

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]

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?