Cost Critic better collision checking #5418
Conversation
Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
|
@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues. |
Signed-off-by: Tony Najjar <[email protected]>
|
@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues. |
| size_x_ = costmap->getSizeInCellsX(); | ||
| size_y_ = costmap->getSizeInCellsY(); | ||
|
|
||
| if (consider_footprint_) { |
There was a problem hiding this comment.
consider_footprint_ currently not used anymore -> need to reintroduce
| return; | ||
| } | ||
|
|
||
| // Store the unoriented footprint for on-the-fly transformation |
There was a problem hiding this comment.
We could consider bin-quantization like with SMAC if the tradeoff between tradeoff and accuracy is acceptable
| } | ||
|
|
||
| // Create convex hull from all collected points | ||
| nav2_costmap_2d::Footprint convex_hull = createConvexHull(all_points); |
There was a problem hiding this comment.
As a first approximation of a swept area I went with a convex hull because it's conservative and fast to compute. Maybe there are better ways
| return result; | ||
| } | ||
|
|
||
| nav2_costmap_2d::Footprint MPPICollisionChecker::createConvexHull( |
There was a problem hiding this comment.
This function is AI-generated, it needs to be replaced with some third-party implementation, maybe opencv
Signed-off-by: Tony Najjar <[email protected]>
|
@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues. |
There was a problem hiding this comment.
Can you actually run this in realtime doing this kind of checking?
Given the sheer number of checks we need to do here (2000x+ samples at 50x+ length each) and we have them all up front, rather than in Smac where we are breadcrumbing them along during search, I wonder if there are ways to improve this.
For example, we are scoring trajectories as a whole rather than individual poses, so we could find all the cells we need to check up-front and then check them all in some optimally-ordered fashion removing duplicates and minimizing cache misses.
blah blah blah GPU - but also now that we're using Eigen, maybe a way to better vectorize these checks.
We could also take some optimization from Smac Planner to have orientation footprints and/or orientation occupation masks precomputed at 1 deg resolutions or something.
A half baked thought since we're operating in the local costmap: I think we need to collision check in some way for obtaining the costs for the relative trajectory cost scoring. That's what incentivizes to stay away from obstacles even if not in actual collision. However I think those are all center pose costs if I read this right, which are the easy ones that we own no matter what for even circular robots.
However, for the binary 'am I actually in collision at this pose' using a footprint, maybe we can do something else. Get all the lethal cells in the costmap up front and check each pose-footprint using the polygon isPointInside check to check for each point if inside if its within the inscribed radius of the robot footprint. I think some elements of this can be vectorized with Eigen. If we're working with 100,000 footprints to collision check but we have a 5x5m window at 5cm resolution, the actual number of costmap cells is 10,000 (and the entire local costmap isn't going to be lethal). There may be a way to work that to our advantage. If we have a robot that is 1.0x0.5m box, that's a 3m perimeter at 5cm which is 60 checks. 60 checks by 100,000x footprints is far larger than the number of possible in-collision cells - which I don't know lets call it ~2,000 collision cells we actually care about.
The above obviously breaks down if the footprint is unusually small or the local costmap is much larger.
Another half baked thought would be to do diffs for dense checks. If we overlap the polygons from the last pose to the next pose, we should be able to reject some non-trivial number of cells that overlap from the last check which we should know is valid (because if its not, we should exit with a collision state). Then we only need to check the cells that diff. This would actually work for the Smac Planner as well.
For MPPI's case, it might make sense to compute all the cells for a given trajectory up front as I mention in a previous idea and that would be in effect the batch-processing version of this idea which is more incremental.
One last one... We do the normal checks that we own no matter what if its circular for point checks to compute the trajectory score (assuming not in collision) as I mentioned before:
A half baked thought since we're operating in the local costmap: I think we need to collision check in some way for obtaining the costs for the relative trajectory cost scoring. That's what incentivizes to stay away from obstacles even if not in actual collision. However I think those are all center pose costs if I read this right, which are the easy ones that we own no matter what for even circular robots.
We know we can do that on even a Jetson AGX Orin in real time at 20-30hz, while making this more optimal would be fabulous, that's a baseline of compute we know we can handle fine.
And now we have a new, slightly different problem, we just need to find if (a) a set of convex polyons or (b) a single but complicated polygon made up of all the poses, are in collision with discrete lethal cells. There may be easier ways to do this than when checking all possible cell costs to get the scoring cost.
For example: We recently had an update to map server that uses Eigen masks to massively improve the performance of the map server. If we made a zero-copy mask on the costmap thresholded by collision & the polygon mask, that intersection could be computed in a very optimal way. 8d69483. Maybe a workflow like make the polygon footprint mask in Eigen, give an Eigen matrix the costmap buffer (zero copy) and then apply the mask + reduce to get the highest value (or something). With precomputed orientation footprint masks that are translated into place, this could be even more optimal.
Perhaps there are other Eigen-y workflows that would be good to consider as well... Alot of these can leverage SIMD to do the checks which would be massively beneficial here.
This actually seems like a great idea to explore and could be used in all controllers since we know all the poses in a trajectory (or trajectories for DWB/MPPI) all at once we want to score.
PS something like this might be applicable to Smac Planner too, but on a smaller window scale for just a parent->child pose rather than here we can check an entire trajectory's 50x poses at once.
| } | ||
|
|
||
| nav2_costmap_2d::Footprint MPPICollisionChecker::createConvexHull( | ||
| const std::vector<geometry_msgs::msg::Point> & points) |
There was a problem hiding this comment.
PS if this was a set then you wouldnt need to remove duplications
I'd like to get your first impression on this draft. It contains the main idea; tests, docs, etc... not yet
Many of the concepts for MPPI's collision monitor are borrowed from SMAC's , maybe at some point "collision-checking" should be its own package/library common accross all controllers and planners
This PR depends on https://github.com/ros-navigation/navigation2/pull/5416/files
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Description of how this change was tested
Future work that may be required in bullet points
For Maintainers:
backport-*.