Feat/smac planner include orientation flexibility #4127
Conversation
|
@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. |
|
I think the analytic expansions might need to be rethought a bit. I think we should be taking all 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 So after You can use that best_score, store it for that particular angle to decide which to use. |
|
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 |
There was a problem hiding this comment.
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.
|
Any questions or anything I can unblock on? 😄 |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
2 similar comments
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
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. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
1 similar comment
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
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. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
9 similar comments
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
|
@stevedanomodolor, your PR has failed to build. Please check CI outputs and resolve issues. |
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
) Add header data to goal for short paths. Commit d8ae3c1 added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart <antoine.gennart@quimesis.be>
* Added load and save panel Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Improved dock_panel state machine Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Added loading dock plugins log Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Redo UI Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update tooltips Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix null-dereference Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com>
…on_r1803809978 Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
…ithub.com:stevedanomodolor/navigation2 into feat/smac_planner_include_orientation_flexibility
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
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.
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
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 dont think it is though because i did two tests.
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).
I think i can do just that, i will time the functions in the planning loop to see where rhe bottleneck is. |
|
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- That way, normal searches that have no expanions possible completes |
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.
Would try this, it should improve the overall I believe. |
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? |
|
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 :-) |
yes, get the same result |
|
Oh great - that means what we discussed will almost definitely improve performance to that level! |
|
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. |
Yay! What's it look now?
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 This is where you lose me.
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 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 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 |
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: