nav2_map_saver: map_io: properly handle special cost values in raw mode #4713
Conversation
Values -1, 0, 99, and 100 of the OccupancyGrid have special meanings [1]. When converting the grid to an RGB image, those values should be properly mapped. In our case, the -1 and 0 are already well managed. However, the 99 and 100 values are currently mapped to 252 and 255 while they should map to 253 and 254 [2]. [1] https://github.com/ros-navigation/navigation2/blob/main/nav2_map_server/src/map_io.cpp#L505 [2] https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp#L71
|
You have a variety of linting, test, and DCO failures that need to be resolved for review. Please look at those :-) |
nav2_map_server/src/map_io.cpp
Outdated
| @@ -504,7 +504,18 @@ void tryWriteMapToFile( | |||
| Magick::Quantum q; | |||
| if (map_cell < 0 || 100 < map_cell) { | |||
| q = MaxRGB; | |||
| } else { | |||
| } | |||
| else if (map_cell == 100) | |||
There was a problem hiding this comment.
What's the context you ran into this issue? I don't object to the change on face value, but I also don't feel that I appreciate the possible ramifications this could have on others once this becomes part of a released distribution, so it makes me a little weary.
I'm looking at ROS 1's version of this for reference (https://github.com/ros-planning/navigation/blob/noetic-devel/map_server/src/image_loader.cpp#L135) since that was in play for 10+ years and this should be more or less a refactor of that. I'm not seeing conditions like this in play there either.
There was a problem hiding this comment.
Perhaps this is dumb, but I'm not overly familar with this refactor.
If the others are map_cell / 255 * MaxRGB --> we have map_cell which is 0-100, no? In that case, the normal operation is [0-100] * 255 / MaxRGB. What your change does is add in 254 and 255 into that calculation, which is far above the [0-100] range of possible map_cell values. Is this mapping correct?
There was a problem hiding this comment.
Hi @SteveMacenski , I'm using the map_saver to save raw costmaps to .pgm files in order to work (offline) on a costmap processing program, and implement unit tests. This processing includes some thresholding using nav2_costmap_2d constants:
static constexpr unsigned char NO_INFORMATION = 255;
static constexpr unsigned char LETHAL_OBSTACLE = 254;
static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static constexpr unsigned char MAX_NON_OBSTACLE = 252;
static constexpr unsigned char FREE_SPACE = 0;
So, I was expecting the .pgm pixels intensity [0-255] to match the original costmap costs [0-255], at least for the aforementioned special values. The current situation is quite different:
- NO_INFORMATION = 255 (OK)
- LETHAL = 255 (NOK) -> indistinguishable from NO_INFORMATION
- INSCRIBED_INFLATED_OBSTACLE = 252 (NOK)
- MAX_NON_OBSTACLE = 249 (NOK)
- FREE_SPACE = 0 (OK)
This is partially due to the fact that the conversion rules from Costmap2D to OccupancyGrid [1] do not match the rules from OccupancyGrid to RGB image [1]. In addition, the casting from Costmap2D [0-255] to OccupancyGrid [0 100] induces a loss of information, and thus a drift when reconstructing the RGB image on a [0-255] scale.
I could not say if it was the expected behavior, and I could deal with the discrepancies if the risk on ramifications is too high. But at least, NO_INFORMATION and LETHAL should be distinguishable on the .pgm file.
[1] https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d_publisher.cpp#L94
[2] https://github.com/ros-navigation/navigation2/blob/main/nav2_map_server/src/map_io.cpp#L505
There was a problem hiding this comment.
Perhaps this is dumb, but I'm not overly familar with this refactor.
If the others are
map_cell / 255 * MaxRGB--> we havemap_cellwhich is 0-100, no? In that case, the normal operation is[0-100] * 255 / MaxRGB. What your change does is add in254and255into that calculation, which is far above the[0-100]range of possiblemap_cellvalues. Is this mapping correct?
See my other PR: #4712
In my understanding, 255 should be replaced by 100. map_cell / 100 thus represents a ratio [0-1]. And so are 254 / 255.0 and 253 / 255.0.
There was a problem hiding this comment.
EDIT: I just discovered how the OccupancyGrid is converted to a Costmap2D: https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d.cpp#L77 . For consistency, I think that the map_saver should probably use the same logic. So I will abandon this PR.
Still, it would have been nice to be able to
- differentiate UNKNOWN and LETHAL areas,
- use the nav2_costmap_2d original enum (https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp#L70) on the reconstructed costmap.
There was a problem hiding this comment.
I reviewed this one before your other PR, so take some of my previous remarks with a grain of salt.
For consistency, I think that the map_saver should probably use the same logic. So I will abandon this PR.
Is your plan to open a new PR to mirror this?
differentiate UNKNOWN and LETHAL areas,
I entirely agree.
Values -1, 0, 99, and 100 of the OccupancyGrid have special meanings [1]. When converting the grid to an RGB image, those values should be properly mapped.
In our case, the -1 and 0 are already well managed. However, the 99 and 100 values are currently mapped to 252 and 255 while they should map to 253 and 254 [2]. As a consequence, the LETHAL (100 -> 255) and the UNKNOWN (-1 -> 255) areas share the same pixel intensity.
[1] https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/src/costmap_2d_publisher.cpp#L94
[2] https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp#L71
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: