Add footprintAreaCost #5250
Add footprintAreaCost #5250
Conversation
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
| double max_cost = 0.0; | ||
|
|
||
| // Check if footprint is rectangular for optimization | ||
| bool is_rectangular = isRectangularFootprint(footprint, min_x, max_x, min_y, max_y); |
There was a problem hiding this comment.
optimization: doesn't need to be checked every time
| @@ -143,6 +243,41 @@ double FootprintCollisionChecker<CostmapT>::footprintCostAtPose( | |||
| return footprintCost(oriented_footprint); | |||
| } | |||
|
|
|||
| template<typename CostmapT> | |||
| bool FootprintCollisionChecker<CostmapT>::isPointInFootprint( | |||
There was a problem hiding this comment.
taken from collision_monitor. Libraries also exist for this e.g boost
| @@ -150,7 +148,8 @@ bool GridCollisionChecker::inCollision( | |||
| current_footprint.push_back(new_pt); | |||
| } | |||
|
|
|||
| float footprint_cost = static_cast<float>(footprintCost(current_footprint)); | |||
| // Check full area covered by footprint | |||
| float footprint_cost = static_cast<float>(footprintAreaCost(current_footprint)); | |||
There was a problem hiding this comment.
can be parametrized to allow user to tune for speed vs accuracy
Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
|
@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues. |
|
I asked my LLM to benchmark the 2 functions and this is the result. Take it with a grain of salt, I didn't sanity-check it but the results look reasonable. |
|
@SteveMacenski do you have time for this soon? I have about an hour still where I could finish this |
|
@tonynajjar I just commented in the issue - looking at this now |
|
|
||
| // Check all cells within bounding box | ||
| for (unsigned int mx = min_mx; mx <= max_mx; ++mx) { | ||
| for (unsigned int my = min_my; my <= max_my; ++my) { |
There was a problem hiding this comment.
I think for y; for x is actually faster, no?
|
I use Github Copilot in VScode with agent mode with the latest Claude Sonnet or GPT models. I still iterated quite a bit to get to this state but it's a huge help for sure... Anyway, you advocated for the other strategy (swept area), so what would be your plan for this PR? Complete it and merge it until someone can implement the other strategy? Or discard it? |
|
I just got a case where this approach fails (and it doesn't even involve single cells) because the angle change between 2 planned footprints is too big. The red footprint is TEB's feasibility checker kicking in showing where the collision would be if we try to follow SMAC's plan. |
|
I increased the turning radius to 0.4 to see if that was a problem only with small turning radii but even in this case I'm not confident that the interpolation between the planned footprint is collision-free but I guess it depends on the interpolation model
|
|
With those changes I'm noticing a weird behavior but I don't see how it relates: For goals moderatly close to lethal space (i don't know what the distance limit is), SMAC doesn't reach the desired orientation. Example: in the following image, pose 432 is sent as the goal from SMAC but the final pose does not match the orientation. (blue rectangle is the last planned footprint) I'm sure that it is because of this PR as I tested without and it reaches any orientation
Do you have a guess why this PR could cause that? Update1: I noticed that when that happens, there are many more arrows in the
Update2: Okay I think I'm slowly understanding better how SMAC works - I think the fact that there are expansion arrows up until the the goal means that SMAC never successfully used analytical expansion, which I assume would be responsible of reaching the right orientation accurately? If yes then next question: what causes the the analytical expansion to fail? |
|
Alot here to process, so I'll go in order
I think from your later findings you might be on Team Swept Area now? For right now, I think we can leave this open for discussion until we find the final solution and/or use this in the interim or permanently.
⬆️ So swept area might be better?
Its not that 0.4m is a magic number (though it did come from somewhere; I selected it based on the turning radius of common RC car toys that are able to get through tight spaces in houses, I figured if it can do it, then that's a good baseline for us. It also makes good turns in and out of aisles as a baseline that isn't just a point-turn but also isn't so arced out that its highly dependent on the width of aisles to successfully compute). It just interpolates more than the jumps for basically zero-radius turns. It doesn't surprise me that you could reproduce it, but I would expect it to be harder. You're right on the analytic expansion. Are you using a tolerance > 0? If so, then it could return a solution nearby if it was unable to compute an exact match.
I ... have no idea. Perhaps there's a bug in your Area check that is checking some wrong cells, so when you're near an obstacle, its checking part of the window it shouldn't. When analytic expansion tries to compute the exact path, then it is unable to do so because they're being reported in collision. The only relationship with the collision checking and analytic expansion is that the expansions are checked for collision using the collision checker. So, I think that would indicate to me that something about this is buggy. I don't see an immediate problem though, so I'd need to add in some prints to log in the analytic expansion to see what it thinks is going on. |
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: