nav2_costmap_2d: static_layer: add grid value interpretation for inscribed inflated obstacles #5561
Conversation
|
@DylanDeCoeyer-Quimesis I need much more detailed explanation of this, possibly with a diagram to meaningfully review it. I believe you that something may benefit from this, but I need to understand in detail like you do with references 😉 This touches a very old and very established piece of software, so some explanation for myself and future readers is important |
|
@SteveMacenski sure! The class To recover the original Costmap2D from the OccupancyGrid on the subscription side, the subscribers must use the same (reciprocal) conversion table. However, in the case of the StaticLayer subscription, the special value attributed to the inscribed inflated obstacle was ignored. The usual scaling factor was applied: So, this costmap layer initially supposed to copy the incoming costmap was transforming every inscribed inflated obstacle cells (253) into non lethal obstacle cells with cost of 251. If this is the expected behavior for any historical reason, then I'll have at least to make the |
|
Thanks! LGTM. CI had an unexpected test fail, so running again to make sure it wasn't just a rare one off. |
|
@DylanDeCoeyer-Quimesis please rebase or pull in main to acquire #5569 to fix CI |
DylanDeCoeyer-Quimesis
force-pushed
the
fix_static_layer_interpretation
branch
5 times, most recently
from
2d67d9b to
6a58990
Compare
|
@SteveMacenski there is still an issue with the unit tests, but I don't understand it. It says the number of INSCRIBED_INFLATED_OBSTACLE cells is 28 while 78 was expected by the |
|
I'm not sure - I'd look at printing the map (uncommenting |
|
@DylanDeCoeyer-Quimesis I have one question about this. For the e.g. the smac planner to work we need a inflation layer. Will inflation as is work with an already inflated static layer properly? I guess yes because it starts with the lethal pixels and builds the map from it or do I miss anything? |
|
@kira-oli Yes that's right, if my understanding is correct. The inflation layer starts from the lethal pixels and does not mind about the inscribed inflated pixels: https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp#L159 When setting the pixel cost, it takes the highest value between the new and the old cost: https://github.com/ros-navigation/navigation2/blob/main/nav2_costmap_2d/plugins/inflation_layer.cpp#L297 |
|
@DylanDeCoeyer-Quimesis I'm retriggering CI to see if that fixes things. If you could pull in main / rebase, that would be appreciated. We fixed some stuff recently that should make this pass if that is indeed a flaky issue |
…ribed inflated obstacles Until now, the special OccupancyGrid value "99" attributed to the inscribed inflated obstacles was mapped to 251 on conversion to Costmap2D, using the default linear relation. This is incorrect since a special value exists in the Costmap2D for inscribed obstacles: 253. This commit makes sure that the correct value is used in case of inflated obstacle. Signed-off-by: Dylan De Coeyer <[email protected]>
DylanDeCoeyer-Quimesis
force-pushed
the
fix_static_layer_interpretation
branch
from
6a58990 to
056485c
Compare
There was a problem hiding this comment.
Final test failed:
[costmap_tests-4] [ RUN ] TestNode.testInflation
[costmap_tests-4] [INFO] [1759998159.472056047] [inflation_test_node]:
[costmap_tests-4] inflation_test_node lifecycle node launched.
[costmap_tests-4] Waiting on external lifecycle transitions to activate
[costmap_tests-4] See https://design.ros2.org/articles/node_lifecycle.html for more information.
[costmap_tests-4] [INFO] [1759998159.472279993] [inflation_test_node]: Subscribing to the map topic (/map) with transient local durability
[costmap_tests-4] [INFO] [1759998159.474496863] [inflation_test_node]: Subscribed to Topics:
[costmap_tests-4] [INFO] [1759998159.475486091] [inflation_test_node]: StaticLayer: Resizing costmap to 10 X 10 at 1.000000 m/pix
[costmap_tests-4] /opt/overlay_ws/src/navigation2/nav2_costmap_2d/test/integration/inflation_tests.cpp:465: Failure
[costmap_tests-4] Expected equality of these values:
[costmap_tests-4] countValues(*costmap, nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
[costmap_tests-4] Which is: 28
[costmap_tests-4] 78u
[costmap_tests-4] Which is: 78
[costmap_tests-4]
[costmap_tests-4] [INFO] [1759998159.475806143] [inflation_test_node]: Destroying
[costmap_tests-4] [WARN] [1759998159.475823890] [rclcpp_lifecycle]: LifecycleNode is not shut down: Node still in state(1) in destructor
[costmap_tests-4] [ FAILED ] TestNode.testInflation (207 ms)
da70a3e to
a67f218
Compare
@SteveMacenski As I said above, the output is very weird. I've set the expected value to 28 following your comment, and here is the output that I get now for the same test: |
|
I agree its very weird. Try busting the cache to see if that helps. In the .circleci directory config, change the |
DylanDeCoeyer-Quimesis
force-pushed
the
fix_static_layer_interpretation
branch
2 times, most recently
from
3272a15 to
23a3aa5
Compare
Signed-off-by: Dylan De Coeyer <[email protected]>
Signed-off-by: Dylan De Coeyer <[email protected]>
DylanDeCoeyer-Quimesis
force-pushed
the
fix_static_layer_interpretation
branch
from
23a3aa5 to
cd74efa
Compare
|
Still failing that test :( Do you see this issue locally too? |
3 participants
Until now, the special OccupancyGrid value "99" attributed to the inscribed inflated obstacles was mapped to 251 on conversion to Costmap2D, using the default linear relation. This is incorrect since a special value exists in the Costmap2D for inscribed obstacles: 253.
This commit makes sure that the correct value is used in case of inflated obstacle.
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-*.