Remove in collision goals BT node #4595

tonynajjar · 2024-08-05T14:15:07Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4587
Primary OS tested on	Ubuntu
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

Added BT node that removes goals if the footprint cost is below a configurable cost threshold
Fixed getCostCallback not handling out-of-bounds if use_footprint is true

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 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: Tony Najjar <[email protected]>

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

Signed-off-by: Tony Najjar <[email protected]>

codecov · 2024-08-05T15:29:28Z

Codecov Report

Attention: Patch coverage is 90.38462% with 5 lines in your changes missing coverage. Please review.

Files	Patch %	Lines
...lugins/action/remove_in_collision_goals_action.cpp	88.88%	3 Missing ⚠️
nav2_costmap_2d/src/costmap_2d_ros.cpp	88.23%	2 Missing ⚠️

Files	Coverage Δ
...lugins/action/remove_in_collision_goals_action.hpp	`100.00% <100.00%> (ø)`
...tree/plugins/action/remove_passed_goals_action.cpp	`100.00% <100.00%> (ø)`
...tmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp	`100.00% <ø> (ø)`
nav2_costmap_2d/src/costmap_2d_ros.cpp	`88.52% <88.23%> (+0.09%)`	⬆️
...lugins/action/remove_in_collision_goals_action.cpp	`88.88% <88.88%> (ø)`

... and 5 files with indirect coverage changes

SteveMacenski

Usual bits on tests and docs, but overall looks like a good initial prototype!

Thought: Adjust the GetCost service to a GetCosts service so that you don't call it repeatedly - populate 1 request with N goals and get their costs back in bulk. That's probably useful for a number of reasons

nav2_rviz_plugins/src/costmap_cost_tool.cpp

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

Signed-off-by: Tony Najjar <[email protected]>

mergify · 2024-08-07T09:03:02Z

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Signed-off-by: Tony Najjar <[email protected]>

mergify · 2024-08-07T09:10:23Z

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Signed-off-by: Tony Najjar <[email protected]>

mergify · 2024-08-07T10:41:21Z

@tonynajjar, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

Signed-off-by: Tony Najjar <[email protected]>

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp

tonynajjar · 2024-08-07T12:27:45Z

I graduated the PR from draft status. Docs and some manual testing are are missing, you can do a second round of review though

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar · 2024-08-07T13:17:50Z

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

+    should_send_request_ = false;
+    return;
+  }
+  request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();


The request_ is never recreated/reset in bt_service_node. It would make sense to add that there don't you think?

I think for this case since the goal vector length can change this is good!

Sorry I wasn't clear enough, I meant to recreate the request in the BTServiceNode parent instead of for every child. Seems like a generic enough thing to do to me.

Oh, I think some rely on it being the same by populating it once in initialization -- but if that's not the case, happy to have that change too. Some API change could come and make that break.

It's only used here in Nav2 but some people might rely on it in their private plugins. I'm happy either way, the benefit of keeping it as is, is at least to have the option to initialize once.

I'm ambivalent unless someone had a compelling argument either way.

mergify · 2024-08-07T15:58:48Z

This pull request is in conflict. Could you fix it @tonynajjar?

SteveMacenski

Otherwise LGTM, just doc updates:

Update migration guide that its GetCosts now
Add the new BT node to the plugins page, migration guide, configuration guide

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

SteveMacenski · 2024-08-07T16:08:03Z

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

+    should_send_request_ = false;
+    return;
+  }
+  request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();


I think for this case since the goal vector length can change this is good!

…-goals Signed-off-by: Tony Najjar <[email protected]>

Signed-off-by: Tony Najjar <[email protected]>

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

Signed-off-by: Tony Najjar <[email protected]>

SteveMacenski · 2024-08-08T17:07:51Z

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

+    should_send_request_ = false;
+    return;
+  }
+  request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();


I'm ambivalent unless someone had a compelling argument either way.

* Remove goals in collision Signed-off-by: Tony Najjar <[email protected]> * fix for main Signed-off-by: Tony Najjar <[email protected]> * Fix linting Signed-off-by: Tony Najjar <[email protected]> * linting fixes Signed-off-by: Tony Najjar <[email protected]> * PR fixes Signed-off-by: Tony Najjar <[email protected]> * remove second costmap Signed-off-by: Tony Najjar <[email protected]> * Minor fixes Signed-off-by: Tony Najjar <[email protected]> * getcosts instead of cost Signed-off-by: Tony Najjar <[email protected]> * get future once Signed-off-by: Tony Najjar <[email protected]> * replace with BTServiceNode Signed-off-by: Tony Najjar <[email protected]> * Add test Signed-off-by: Tony Najjar <[email protected]> * fix costmap tool Signed-off-by: Tony Najjar <[email protected]> * fix ament_uncrustify Signed-off-by: Tony Najjar <[email protected]> * remove duplicate Signed-off-by: Tony Najjar <[email protected]> * add return Signed-off-by: Tony Najjar <[email protected]> * fix typo Signed-off-by: Tony Najjar <[email protected]> * reset request before sending Signed-off-by: Tony Najjar <[email protected]> * pr comments fixes Signed-off-by: Tony Najjar <[email protected]> * fix ament cmake Signed-off-by: Tony Najjar <[email protected]> * fix test Signed-off-by: Tony Najjar <[email protected]> * fix Signed-off-by: Tony Najjar <[email protected]> --------- Signed-off-by: Tony Najjar <[email protected]> Signed-off-by: Joseph Duchesne <[email protected]>

tonynajjar added 3 commits August 5, 2024 15:22

Remove goals in collision

842de07

Signed-off-by: Tony Najjar <[email protected]>

fix for main

5411676

Signed-off-by: Tony Najjar <[email protected]>

Fix linting

f60d727

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar commented Aug 5, 2024

View reviewed changes

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp Outdated Show resolved Hide resolved

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp Outdated Show resolved Hide resolved

linting fixes

82e95fc

Signed-off-by: Tony Najjar <[email protected]>

SteveMacenski requested changes Aug 5, 2024

View reviewed changes

tonynajjar added 4 commits August 6, 2024 13:18

PR fixes

e11923a

Signed-off-by: Tony Najjar <[email protected]>

remove second costmap

384bccb

Signed-off-by: Tony Najjar <[email protected]>

Minor fixes

45b8a08

Signed-off-by: Tony Najjar <[email protected]>

getcosts instead of cost

640b0eb

Signed-off-by: Tony Najjar <[email protected]>

get future once

8eccb33

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar added 2 commits August 7, 2024 10:37

replace with BTServiceNode

c27c106

Signed-off-by: Tony Najjar <[email protected]>

Add test

29887dd

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar added 2 commits August 7, 2024 10:51

fix costmap tool

9465129

Signed-off-by: Tony Najjar <[email protected]>

fix ament_uncrustify

7fb4367

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar commented Aug 7, 2024

View reviewed changes

nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp Show resolved Hide resolved

tonynajjar marked this pull request as ready for review August 7, 2024 12:26

tonynajjar added 4 commits August 7, 2024 12:40

remove duplicate

facdb1e

Signed-off-by: Tony Najjar <[email protected]>

add return

135b19b

Signed-off-by: Tony Najjar <[email protected]>

fix typo

f0a46ee

Signed-off-by: Tony Najjar <[email protected]>

reset request before sending

9262583

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar commented Aug 7, 2024

View reviewed changes

SteveMacenski requested changes Aug 7, 2024

View reviewed changes

Merge remote-tracking branch 'upstream/main' into remove-in-collision…

dbd4565

…-goals Signed-off-by: Tony Najjar <[email protected]>

pr comments fixes

3f0c6c5

Signed-off-by: Tony Najjar <[email protected]>

tonynajjar mentioned this pull request Aug 8, 2024

Remove in collision goals docs ros-navigation/docs.nav2.org#579

Merged

tonynajjar requested a review from SteveMacenski August 8, 2024 08:56

tonynajjar added 2 commits August 8, 2024 09:06

fix ament cmake

a01b09b

Signed-off-by: Tony Najjar <[email protected]>

fix test

4c035ea

Signed-off-by: Tony Najjar <[email protected]>

SteveMacenski requested changes Aug 8, 2024

View reviewed changes

...behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp Show resolved Hide resolved

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp Show resolved Hide resolved

SteveMacenski requested changes Aug 8, 2024

View reviewed changes

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp Outdated Show resolved Hide resolved

fix

7fd9826

Signed-off-by: Tony Najjar <[email protected]>

SteveMacenski approved these changes Aug 8, 2024

View reviewed changes

SteveMacenski merged commit 47a1235 into ros-navigation:main Aug 8, 2024
11 checks passed

tonynajjar changed the title ~~Remove in collision goals~~ Remove in collision goals BT node Aug 8, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Remove in collision goals BT node #4595

Remove in collision goals BT node #4595

tonynajjar commented Aug 5, 2024

codecov bot commented Aug 5, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading

mergify bot commented Aug 7, 2024

mergify bot commented Aug 7, 2024

mergify bot commented Aug 7, 2024

tonynajjar commented Aug 7, 2024

tonynajjar Aug 7, 2024

SteveMacenski Aug 7, 2024

tonynajjar Aug 8, 2024

SteveMacenski Aug 8, 2024

tonynajjar Aug 8, 2024

SteveMacenski Aug 8, 2024

mergify bot commented Aug 7, 2024

SteveMacenski left a comment

SteveMacenski Aug 7, 2024

SteveMacenski Aug 8, 2024

Remove in collision goals BT node #4595

Remove in collision goals BT node #4595

Conversation

tonynajjar commented Aug 5, 2024

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:

codecov bot commented Aug 5, 2024 • edited Loading

Codecov Report

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

mergify bot commented Aug 7, 2024

mergify bot commented Aug 7, 2024

mergify bot commented Aug 7, 2024

tonynajjar commented Aug 7, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mergify bot commented Aug 7, 2024

SteveMacenski left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

codecov bot commented Aug 5, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading