Create service to get cost of a cell in the costmap #4302
Conversation
| { | ||
| RCLCPP_INFO( | ||
| get_logger(), "Received request to get cost at point (%f, %f)", request->x, request->y); | ||
| auto cost = static_cast<float>(layered_costmap_->getCostmap()->getCost( |
There was a problem hiding this comment.
I think some bounds checking needs to happen that the request is within the costmap's size
| float32 x | ||
| float32 y | ||
| --- | ||
| uint8 cost |
There was a problem hiding this comment.
You cast the output to a float in the cpp code but its a uint?
There was a problem hiding this comment.
good point, I just copied it from here why is it cast there?
There was a problem hiding this comment.
Because footprint cost is a float.
|
I'm not entirely sure this is a good thing to expose to users. I understand you want it to debug the current problem, but this will be crazy inefficient if misused in applications. I think perhaps this should just be a debugging tool for the current work unless you feel strongly about it otherwise |
|
I don't feel strongly about it but can you elaborate on
I was hoping this could also be the base for an rviz plugin with which you just click a point on the costmap and it gives you the cost |
|
I suppose if there's an rviz thing for it in nav2's config, that's fine with me |
That would be very nice to debug, and to be able to get the true value of the cost of a costmap cell, not the truncated occupancy grid msg value |
|
It would have to be the truncated occupancy grid value, because that’s what rviz displays if we did it only within rviz |
|
Is it possible to add a bool in the .srv to get costmap cost at pose or footprint cost at pose? |
The request would also require the |
|
Any update here @tonynajjar? |
|
Yeah I was wondering what to do with it. I don't need it anymore personally but if enough people want it I guess we can keep it in the backlog as a low-prio thing to implement? |
|
I guess you could close this PR, open a ticket as low priority / good first issue with a description: service to get cell/footprint cost + rviz panel for selecting and using it. You could link this branch as a starting point This would make a nice grad student / hobbyist project |
Basic Info
Description of contribution in a few bullet points
Draft PR created for https://github.com/open-navigation/navigation2/issues/4271
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: