Add ispathvalid maxcost #4873

mini-1235 · 2025-01-25T08:07:56Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4756
Primary OS tested on	Ubuntu 24.04
Robotic platform tested on	Custom simulation environment
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Add max cost as described in the issue
Add corresponding test

Description of documentation updates required from your changes

Add new parameter description https://docs.nav2.org/configuration/packages/bt-plugins/conditions/IsPathValid.html

Description of how this change was tested

Wrote a new test to test this and also tested on custom simulation env

Future work that may be required in bullet points

For Maintainers:

Check that any new parameters added are updated in docs.nav2.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

Signed-off-by: mini-1235 <[email protected]>

SteveMacenski

Overall, looks very good to me, just small ~5 minute worth of nitpicks and we can merge this!

nav2_msgs/srv/IsPathValid.srv

nav2_behavior_tree/nav2_tree_nodes.xml

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

nav2_planner/src/planner_server.cpp

Signed-off-by: mini-1235 <[email protected]>

mini-1235 · 2025-01-28T16:52:55Z

Hi, thanks for reviewing @SteveMacenski, I have fixed the problem you have mentioned above and raised a PR on the documentation side, can you please take another look on it? Also the mppi test failed for some reason, but it passed on my machine, can you help on it 😄

SteveMacenski

Small changes for setting the limit to be inscribed inflated by default, but otherwise LGTM!

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

nav2_planner/src/planner_server.cpp

SteveMacenski · 2025-01-28T19:54:05Z

nav2_planner/src/planner_server.cpp

        response->is_valid = false;
        break;
-      } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
+      } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost > request->max_cost) {


Suggested change

} else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost > request->max_cost) {

} else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost >= request->max_cost) {

I think we treat NO_INFORMATION(255) as FREE_SPACE here, so I should convert 255 to 0 / do a simple && cost!=FREE_SAPCE here, am I correct?

This is a good point - I think if we add in max_cost, we should also add in a bool service argument for unknown_valid and we check if the cost is larger than the max cost and if unknown should be considered valid or invalid (collision). It would need to be exposed from the BT node and so forth as well - but easy enough!

nav2_system_tests/src/planning/test_planner_is_path_valid.cpp

Signed-off-by: mini-1235 <[email protected]>

nav2_planner/src/planner_server.cpp

Signed-off-by: Steve Macenski <[email protected]>

SteveMacenski · 2025-01-29T22:23:17Z

One sec - lets let CI turn over again, a bunch of weird unusual tests failed and I want to make sure it wasn't just a fluke. Retriggering

* Add ispathvalid maxcost Signed-off-by: mini-1235 <[email protected]> * Fix problems as requested Signed-off-by: mini-1235 <[email protected]> * Set default as 253, Add considered unknown as obstacle Signed-off-by: mini-1235 <[email protected]> * Edit comment Signed-off-by: mini-1235 <[email protected]> * Update nav2_planner/src/planner_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: mini-1235 <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]>

* Add ispathvalid maxcost Signed-off-by: mini-1235 <[email protected]> * Fix problems as requested Signed-off-by: mini-1235 <[email protected]> * Set default as 253, Add considered unknown as obstacle Signed-off-by: mini-1235 <[email protected]> * Edit comment Signed-off-by: mini-1235 <[email protected]> * Update nav2_planner/src/planner_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: mini-1235 <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: stevedanomodolor <[email protected]>

tonynajjar · 2025-06-11T15:22:24Z

nav2_planner/src/planner_server.cpp

        response->is_valid = false;
        break;
-      } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) {
+      } else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE || cost >= request->max_cost) {


I think this introduced a bug or at the very least an unexpected behavior compared to how it was.

In this clause, use_radius is false so cost is a footprintCost. The default max_cost that was set to 253. A footprintCost of 253 is acceptable (a vertex is intersecting a INSCRIBED_INFLATED_OBSTACLE cell) but according to this logic, this renders the path invalid.

@mini-1235 can you double-check my logic? Thank you

This is true, but that's why this is a configuration of the request. If you set use_radius = false then you should set your max_cost limit.

Do you recommend changing the default to 254? That seems sensible enough to me so it doesn't trigger anything unless the user meaningfully overrides it with another value of their selection.

Do you recommend changing the default to 254?

Yes and that would not change anything for the case of where use_radius is true because we already check cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)

This is true, but that's why this is a configuration of the request

I just didn't expect a change in behavior in my IsPathValid BT node, at least not without notes.

Add ispathvalid maxcost

969c34c

Signed-off-by: mini-1235 <[email protected]>

SteveMacenski requested changes Jan 27, 2025

View reviewed changes

mini-1235 mentioned this pull request Jan 28, 2025

Add new parameter description in ispathvalid ros-navigation/docs.nav2.org#637

Merged

Fix problems as requested

4a81119

Signed-off-by: mini-1235 <[email protected]>

mini-1235 requested a review from SteveMacenski January 28, 2025 17:43

SteveMacenski requested changes Jan 28, 2025

View reviewed changes

mini-1235 added 2 commits January 29, 2025 21:14

Set default as 253, Add considered unknown as obstacle

342378e

Signed-off-by: mini-1235 <[email protected]>

Edit comment

2c510d9

Signed-off-by: mini-1235 <[email protected]>

SteveMacenski approved these changes Jan 29, 2025

View reviewed changes

nav2_planner/src/planner_server.cpp Outdated Show resolved Hide resolved

Update nav2_planner/src/planner_server.cpp

4dc0a19

Signed-off-by: Steve Macenski <[email protected]>

SteveMacenski merged commit dcc5db5 into ros-navigation:main Jan 29, 2025
9 of 10 checks passed

tonynajjar reviewed Jun 11, 2025

View reviewed changes

	} else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE \|\| cost > request->max_cost) {
	} else if (cost == nav2_costmap_2d::LETHAL_OBSTACLE \|\| cost >= request->max_cost) {

Add ispathvalid maxcost #4873

Add ispathvalid maxcost #4873

Uh oh!

Conversation

mini-1235 commented Jan 25, 2025

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:

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

mini-1235 commented Jan 28, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski Jan 28, 2025

Choose a reason for hiding this comment

Uh oh!

mini-1235 Jan 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jan 29, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Jan 29, 2025

Uh oh!

Uh oh!

tonynajjar Jun 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Jun 11, 2025

Choose a reason for hiding this comment

Uh oh!

tonynajjar Jun 11, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

mini-1235 commented Jan 28, 2025 •

edited

Loading

mini-1235 Jan 29, 2025 •

edited

Loading

SteveMacenski Jan 29, 2025 •

edited

Loading

tonynajjar Jun 11, 2025 •

edited

Loading