-
Notifications
You must be signed in to change notification settings - Fork 1.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Feat/smac planner include orientation flexibility #4127
base: main
Are you sure you want to change the base?
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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!
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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. |
Thanks Josh! |
This pull request is in conflict. Could you fix it @stevedanomodolor? |
Signed-off-by: stevedan <[email protected]>
Signed-off-by: stevedan <[email protected]>
Signed-off-by: stevedan <[email protected]>
Signed-off-by: stevedan <[email protected]>
I made some changes for simplicity when computing the distance heuristics. |
… and node2d Signed-off-by: stevedan <[email protected]>
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 <[email protected]>
…ility Signed-off-by: Stevedan Ogochukwu Omodolor <[email protected]>
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 <[email protected]>
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'd be interested in the runtime impact of this over the all headings setting
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Included in the pr descrition
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
With the suggestion above, I can make this changes
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Then I will go with the approach I mentioned above and update the benchmarks again, this could reduce more time.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Great!
the pr description* |
Signed-off-by: stevedan <[email protected]>
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
This pull request is in conflict. Could you fix it @stevedanomodolor? |
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: