Create service to get cost of a cell in the costmap

tonynajjar commented 3 months ago

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #1)
Primary OS tested on	(Ubuntu, MacOS, Windows)
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	(No; Yes and it is marked inline in the code)

Description of contribution in a few bullet points

Draft PR created for https://github.com/open-navigation/navigation2/issues/4271

Added getCost service

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

[ ] Check that any new parameters added are updated in navigation.ros.org
[ ] Check that any significant change is added to the migration guide
[ ] Check that any new features OR changes to existing behaviors are reflected in the tuning guide
[ ] Check that any new functions have Doxygen added
[ ] Check that any new features have test coverage
[ ] Check that any new plugins is added to the plugins page
[ ] If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

SteveMacenski commented 3 months ago

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

tonynajjar commented 3 months ago

I don't feel strongly about it but can you elaborate on

but this will be crazy inefficient if misused in applications

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

SteveMacenski commented 3 months ago

I suppose if there's an rviz thing for it in nav2's config, that's fine with me

doisyg commented 3 months ago

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

SteveMacenski commented 3 months ago

It would have to be the truncated occupancy grid value, because that’s what rviz displays if we did it only within rviz

BriceRenaudeau commented 3 months ago

Is it possible to add a bool in the .srv to get costmap cost at pose or footprint cost at pose?

SteveMacenski commented 3 months ago

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 theta then too

SteveMacenski commented 2 months ago

Any update here @tonynajjar?

tonynajjar commented 2 months ago

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?

SteveMacenski commented 2 months ago

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

ros-navigation / navigation2