-
Notifications
You must be signed in to change notification settings - Fork 1.2k
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
[collision monitor] Select the observation sources used with each polygon #4227
base: main
Are you sure you want to change the base?
[collision monitor] Select the observation sources used with each polygon #4227
Conversation
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
f8d2bef
to
95174e9
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
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 think we can do this more efficiently and straight forward
- Create the unordered map
- Adjust the current
for (std::shared_ptr<Source> source : sources_) {
loop to contain (1) creating an entry for a source name key with an empty vector as the value. (2) Capture the return ofinsert
will give you an iterator to that pair in the map - Pass into
getData
the reference to the value (vector) to populate. That eliminates an entire data copy. It also eliminates multiple calls togetData
to populate both the vector and map separately - Adjust the
processStopSlowdownLimit
/processApproach
/ etc functions to take in this map. The vector of points should no longer exist - Pass the map to the
isPointInside
- When iterating through the sources, use the sources names to decide if you want to use that one or not
This eliminates any extra looping and a bunch of data copies (and not parallel populating a map and a vector of data). Speed here is really key and copying pointclouds and laserscans is a pretty worst-case situation
@@ -405,7 +405,14 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: | |||
} | |||
|
|||
// Points array collected from different data sources in a robot base frame | |||
std::vector<Point> collision_points; |
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.
This variable is no longer defined but appears to still be used. Does this compile / work?
std::unordered_map<std::string, std::vector<Point>> sources_collision_points_map; | ||
|
||
// Fill collision_points array from different data sources | ||
for (std::shared_ptr<Source> source : sources_) { |
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 source data population happens below, I don't think you should be adding a new duplicate but adjusting the old one, no?
If you create the map element, you can pass in the reference to that vector to getData()
so that it populates the map in-line rather than copying it twice (once from the source, another to the map).
// Fill collision_points array from different data sources
for (std::shared_ptr<Source> source : sources_) {
if (source->getEnabled()) {
if (!source->getData(curr_time, collision_points) && // <-- Right here populate an element of the map rather than the vector (that is no longer defined)
source->getSourceTimeout().seconds() != 0.0)
{
action_polygon = nullptr;
robot_action.polygon_name = "invalid source";
robot_action.action_type = STOP;
robot_action.req_vel.x = 0.0;
robot_action.req_vel.y = 0.0;
robot_action.req_vel.tw = 0.0;
break;
}
}
}
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.
It seems like this for loop should be deleted and the block of code below needs to be adjusted
@@ -468,6 +475,28 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in, const std_msgs::msg: | |||
// Update polygon coordinates | |||
polygon->updatePolygon(cmd_vel_in); | |||
|
|||
// Get collision points for sources associated to polygon | |||
std::vector<std::string> sources_names = polygon->getSourcesNames(); |
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.
It seems to me to be more sensible to pass in a reference to the map of collision points to isPointsInside
function for each of the polygons and let each of the polygons use its own internal getSourcesNames
when iterating through the map to decide if it wants to use it or not.
In that case, this block of code would be removed and we just pass in the map of collision points rather than the vector of collision points to the processXYZ()
functions
This pull request is in conflict. Could you fix it @anaelle-sw? |
@anaelle-sw looks like some small changes needed from merging your other PR 😄 Can we get this one in next? I do really appreciate your time and effort to contribute this work back to Nav2, it is very useful and thank you! |
@anaelle-sw Hi! Any update here? |
Sorry, I haven't had the time to work on this properly lately. I will rebase the branch on main and begin to apply to changes you requested by the end of the week. |
Thanks @anaelle-sw :-) |
Signed-off-by: asarazin <anaelle.sarazin@robocc.com>
95174e9
to
8264866
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
1 similar comment
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
The MR is now rebased on current main branch. I have also applied your optimization suggestions. But I have not tested yet the behaviour with these changes. I entirely agree with most of the optimizations you asked for, but actually I have a slightly better solution about function You suggested to pass the entire map (which associates the sources names to their vector of points) to function But in the case of Approach polygons, the function To do so, I have kept the already existing |
5af93f6
to
b46fc72
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
b46fc72
to
3a067b9
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
It does not appear to be building properly, but I love this idea and looks good to me! A couple of tests for this feature would be nice - since this is a pretty low-level system that we should be especially careful about. |
3a067b9
to
1c672fa
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
1c672fa
to
3b7f4e4
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
3b7f4e4
to
1f132cf
Compare
@anaelle-sw, your PR has failed to build. Please check CI outputs and resolve issues. |
Signed-off-by: asarazin <anaelle.sarazin@robocc.com>
1f132cf
to
8a036bd
Compare
Many of the collision monitor tests are now failing! |
Basic Info
Description of contribution in a few bullet points
New string vector parameter
<polygon_name>.sources_names
. Only the sources which names are specified in parameter are used to check collision with current polygon. If the parameter is not set, all the observation sources are used.Description of documentation updates required from your changes
Add new parameter
<polygon_name>.sources_names
to default configs, documentation page, and migration guideFuture work that may be required in bullet points
For Maintainers: