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. |
nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp
Outdated
Show resolved
Hide resolved
nav2_smac_planner/src/a_star.cpp
Outdated
|
|
||
| if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { | ||
| if (!_search_info.cache_obstacle_heuristic || goals_coordinates != _goals_coordinates) { |
There was a problem hiding this comment.
We need to check if any goal coord matches any new goal coord. Both vectors don't have to match. The yaw doesn't have to match.
There was a problem hiding this comment.
Does really matter if the yaw matches if the goal is to reset the obstacle heurtistic which does not depend on the information on the yaw?
There was a problem hiding this comment.
I am not quite sure I understand.
The obstacle critic should only be reset when the map coords(costmap coords) are changed. Since all the goals should have the same x,y you can just check the initial of both sets to determine if the obstacle heuristic needs to be recalculated.
It doesn't matter if the yaw matches, but it does matter if there is a mismatch because we will end up recomputing the obstacle heuristic for changes in yaw which has no affect on the obstacle heuristic.
| !_goal->isNodeValid(_traverse_unknown, _collision_checker)) | ||
| { | ||
| throw nav2_core::GoalOccupied("Goal was in lethal cost"); | ||
| if (getToleranceHeuristic() < 0.001) { |
There was a problem hiding this comment.
What about the goal coordinates?
After this step there will be a mismatch better the goalSet and the goal_coordinates.
There was a problem hiding this comment.
Yes, I did that on purpose because I use the goal_coordinates for comparision when i call the setgoals function and I dont want to lose the order because i need it here getInitialGoalCoordinate.
There was a problem hiding this comment.
Yeah so the current flow is
- setGoal get called, initializes the goalSet and goals_coords
- CreatePath -> areInputsValid which adjusts goalSet, Note: this could invalidate the first goals_coord value i.e (goals_coords[0]). After this point goalSet.size() != goals_coords.size()
- Then initial coord get passing to try analytic expansion, the heading of this coord can be invalid.
In the analytic expansion, getHeuristicCost is calucalted which is a function of the heading.
As a result, you might up attempting analytic expansions in the wrong iterations. You need to calculate the heuristic cost of the valid set of all goal and take the min.
There was a problem hiding this comment.
You're correct. The change I implemented, which includes pruning, has led to additional problems. In addition to what you said, during the initial stage, I calculate the distance heuristic in advance based on the goal heading with the lowest cost, but there's a chance this goal might be invalid and the heuristic can led the wrong goal. The optimal solution would be to adjust the distance heuristic lookup to depend on the heading. I plan to apply this only for standard and bidirectional headings, considering the impact of the distance heuristic is relatively minor compared to the obstacle heuristic, what do you think?
There was a problem hiding this comment.
I think you should also do it for the all direction headings.
The easiest thing would be to create a set of lookup tables rather than just taking the min. Then based on the goals used you construct the final min lookup table used for the search.
|
Thanks Josh! |
|
This pull request is in conflict. Could you fix it @stevedanomodolor? |
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
I made some changes for simplicity when computing the distance heuristics. |
… and node2d Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
stevedanomodolor
force-pushed
the
feat/smac_planner_include_orientation_flexibility
branch
from
afa9489
to
c0abe18
Compare
|
@SteveMacenski locally, all my ctest are passing but for some reason the behavior tests are failing |
|
The spin/backup are periodically flaky. I wack them down and they come up on occasion. Given this doesn't touch the behavior server at all, I'm not concerned by those failures. It just means I need to play wackamole again or finally delete and rewrite them myself. I retriggered CI for kicks and giggles, it may pass this time. I'd just focus on the review comments and performance checking (and any test failures that could be plausibly caused by you, which I don't see) |
I update the readme with the latest performance check comparing before and after the changes |
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
…ility Signed-off-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
Codecov ReportAttention: Patch coverage is
Additional details and impacted files@@ Coverage Diff @@
## main #4127 +/- ##
==========================================
+ Coverage 89.55% 89.57% +0.02%
==========================================
Files 435 435
Lines 19848 19950 +102
==========================================
+ Hits 17775 17871 +96
- Misses 2073 2079 +6 ☔ View full report in Codecov by Sentry. |
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
stevedanomodolor
force-pushed
the
feat/smac_planner_include_orientation_flexibility
branch
from
deaa173
to
eff8859
Compare
I don't see that? Apologies in the delay in reviews, this is a big PR and I've been a bit underwater recently so this unfortunately didn't get the attention on the schedule I would have liked. Doing it today though. |
| getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); | ||
| node_coords, goals_coords[0], motion_table.cost_penalty); | ||
| float distance_heuristic = std::numeric_limits<float>::max(); | ||
| for (unsigned int i = 0; i < goals_coords.size(); i++) { |
There was a problem hiding this comment.
I'd be interested in the runtime impact of this over the all headings setting
There was a problem hiding this comment.
Included in the pr descrition
There was a problem hiding this comment.
@SteveMacenski What if during the initailzation fase, I construct 3 tables for each direction and then based on the goal, take the one the look up table value?. It similar to what @jwallace42 suggested but, I compute everything beforehand.
There was a problem hiding this comment.
That would be alot of memory, leaving this as a single table is best I think -- but I think this is moot now from the discussion below?
There was a problem hiding this comment.
@SteveMacenski I am just starting to see a lots of problem with the 3 table idea, because if i precompute the table, there is a possibility the minimum distance heuristic corresponding to a goal be prune because now i do goal path pruning. The other option is to create a default table in the beginning during the is path valid function, I can precompute a temporal table based on this default. This way in defautl goal heading mode, I dont influence the computation time, what do you think?. The only downside here is that during I will have two lookup tables in memory though.
There was a problem hiding this comment.
I don't quite understand what you're asking.
I think we should leave the table structure as it is. The content should just be set according to the heading policy
There was a problem hiding this comment.
The problem with setting it beforehand is that there is a change that one of the goals set to be minimum will be pruned later in the isinputvalid function making the lookup table invalid hence why I decided to go with the current approach. I am working on the following, still needs testing and benchmarking but for the last few days, I have been having issues building and running dockers in rolling(the comment from the other issue :). This is still wip
void NodeHybrid::resetDistanceHeuristic(
const CoordinateVector & goals_coords,
const GoalHeadingMode & goal_heading_mode)
{
dist_heuristic_lookup_table = initial_dist_heuristic_lookup_table;
if (goal_heading_mode == GoalHeadingMode::DEFAULT) {
return;
}
std::unordered_set<int> goal_heading_angles;
if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL ||
goal_heading_mode == GoalHeadingMode::ALL_DIRECTION)
{
goal_heading_angles.reserve(goals_coords.size());
for (const auto & coord : goals_coords) {
goal_heading_angles.insert(motion_table.getClosestAngularBin(coord.theta));
}
}
int ceiling_size = static_cast<int>(ceil(size_lookup / 2.0));
int total_size = static_cast<int>(size_lookup);
auto is_goal_heading = [&](int heading) {
return goal_heading_angles.find(heading) != goal_heading_angles.end();
};
int index = 0;
if (goal_heading_mode == GoalHeadingMode::BIDIRECTIONAL) {
for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
for (int heading = 0; heading < dim_3_size_int; ++heading) {
int heading_angle_180 = (heading + dim_3_size_int / 2) % dim_3_size_int;
int index_180 = (index-heading) + heading_angle_180;
// std::cout << "index: " << index << " index_180: " << index_180 << " size: " << dist_heuristic_lookup_table.size() << std::endl;
float dist_heuristic = initial_dist_heuristic_lookup_table[index];
float dist_heuristic_180 = initial_dist_heuristic_lookup_table[index_180];
if (dist_heuristic_180 < dist_heuristic && is_goal_heading(heading_angle_180)) {
dist_heuristic_lookup_table[index] = dist_heuristic_180;
}
index++;
}
}
}
} else if (goal_heading_mode == GoalHeadingMode::ALL_DIRECTION) {
for (int x = 0; x < size_lookup; ++x) {
for (int y = 0; y < ceiling_size; ++y) {
std::vector<float> dist_heuristic_angles(dim_3_size_int);
std::vector<int> indices(dim_3_size_int);
for (int heading = 0; heading < dim_3_size_int; ++heading) {
dist_heuristic_angles[heading] = dist_heuristic_lookup_table[index];
indices[heading] = index;
index ++;
}
// we want to get the minimum distance heuristic for all the goal heading angles
// and that is also one of the goal heading angles
auto min_it = std::min_element(
dist_heuristic_angles.begin(), dist_heuristic_angles.end(), [&](float a, float b) {
int a_index = &a - &dist_heuristic_angles[0];
int b_index = &b - &dist_heuristic_angles[0];
return (is_goal_heading(a_index) ? a : std::numeric_limits<float>::max()) <
(is_goal_heading(b_index) ? b : std::numeric_limits<float>::max());
});
// if we find one we update the distance heuristic for all the goal heading angles
if (min_it != dist_heuristic_angles.end()) {
for (int heading = 0; heading < dim_3_size_int; ++heading) {
dist_heuristic_lookup_table[indices[heading]] = *min_it;
}
}
}
}
}
}
| return std::max(obstacle_heuristic, dist_heuristic); | ||
| getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty); | ||
| float distance_heuristic = std::numeric_limits<float>::max(); | ||
| for (unsigned int i = 0; i < goals_coords.size(); i++) { |
There was a problem hiding this comment.
Ditto. It might be best to just use the main goal. If so, then this could be reverted to taking in the Coordinates.
There was a problem hiding this comment.
With the suggestion above, I can make this changes
There was a problem hiding this comment.
@SteveMacenski With the below approach, this way I can just pass the coordinates instead of the coordinatevector, the only problem I see with this is https://github.com/stevedanomodolor/navigation2/blob/1a34ea3f3789aa66955a389b8db17acef95a836b/nav2_smac_planner/src/node_lattice.cpp#L422. I am sure how much of an influence it would be.
dist_heuristic_lookup_table.resize(size_lookup_total);
dist_heuristic_lookup_table_bidirectional;
dist_heuristic_lookup_table_all_direction;
There was a problem hiding this comment.
The obstacle_heuristic == 0.0 case is defensive programming in case everything goes very wrong - this should not happen that both the cost is 0 and its outside of the window around the goal. This is a "just in case" case
There was a problem hiding this comment.
Then I will go with the approach I mentioned above and update the benchmarks again, this could reduce more time.
the pr description* |
Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
|
Ah got it, thanks! I was looking in the package's readme. I'm surprised its so much faster, that's a cool result! |
…entation_flexibility
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: