PoseStamped vector specialization #4607

tonynajjar · 2024-08-08T13:29:31Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#4388
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

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 <tony.najjar.1997@gmail.com>

tonynajjar · 2024-08-08T13:30:55Z

PR is not done, will ping once ready for review. I just need to open a PR to remember finishing it :)

SteveMacenski · 2024-08-21T20:55:16Z

@tonynajjar hidy ho, any chance we can get this in soon? 🙃

tonynajjar · 2024-08-21T21:17:56Z

I thrive on pings, I'll try to resume it this week. I got stuck on something but forgot on what.

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar · 2024-08-23T15:21:09Z

Pretty much ready for a first review, one thing I couldn't get to the bottom of yet is why test_goal_updated_controller suddenly fails with these changes. (At least locally, let's see in CI)

SteveMacenski

I retriggered CI, if it fails again, then its a real issue -- those tests are pretty simple API use so if that's broken, there's a real possibility the actual BT node is broken too and not just a test update

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp

codecov · 2024-09-03T18:29:18Z

Codecov Report

All modified and coverable lines are covered by tests ✅

Files with missing lines	Coverage Δ
...avior_tree/include/nav2_behavior_tree/bt_utils.hpp	`100.00% <100.00%> (ø)`
...tree/plugins/action/remove_passed_goals_action.hpp	`100.00% <ø> (ø)`
.../plugins/condition/distance_traveled_condition.hpp	`100.00% <ø> (ø)`
...gins/condition/globally_updated_goal_condition.hpp	`100.00% <ø> (ø)`
..._tree/plugins/condition/goal_reached_condition.hpp	`100.00% <ø> (ø)`
..._tree/plugins/condition/goal_updated_condition.hpp	`100.00% <ø> (ø)`
...gins/condition/initial_pose_received_condition.hpp	`100.00% <ø> (ø)`
...lugins/condition/path_expiring_timer_condition.hpp	`100.00% <ø> (ø)`
...ior_tree/plugins/decorator/distance_controller.hpp	`100.00% <ø> (ø)`
...tree/plugins/decorator/goal_updated_controller.hpp	`100.00% <ø> (ø)`
... and 10 more

... and 3 files with indirect coverage changes

This reverts commit 51f54eb.

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar · 2024-09-03T18:48:22Z

nav2_behavior_tree/test/plugins/decorator/test_goal_updated_controller.cpp

@@ -36,8 +36,6 @@ class GoalUpdatedControllerTestFixture : public nav2_behavior_tree::BehaviorTree
    poses1.push_back(goal1);
    config_->blackboard->set("goal", goal1);
    config_->blackboard->set("goals", poses1);
-    config_->input_ports["goals"] = "";


@SteveMacenski these were causing the test to fail, I see they were added there by you I believe, do you remember what were you solving with these lines?

There were issues in the 24.04 migration about ports not being set, so I set them all to something in the tests. It doesn't need to be "", but needs to be initialized as something to stop BT.CPP update from throwing an uninitialized port error due to how the tests are structured

SteveMacenski

Is this good to go from your previous comments above?

tonynajjar · 2024-09-04T07:28:41Z

CI is happy I'm happy

* PoseStamped vector specialization Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * merge master Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix size check Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix test" This reverts commit 51f54eb. * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Adding non-charging dock support to docking server (for conveyers, pallots, etc) (#4627) * adding non-charging dock support to docking server Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * docs and linting * adding unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Publish optimal trajectory as a Path message (#4640) * Publish optimal trajectory as a Path message Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * move publish_optimal_path to TrajectoryVisualizer + minor refactoring Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * tests added for optimal path publication Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * populate optimal path message in add() Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * move path population in add_marker Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> --------- Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> * [collision monitor] Select the observation sources used with each polygon (#4227) * Collision monitor: select specific observation sources for polygon Signed-off-by: asarazin <anaelle.sarazin@robocc.com> * optimization Signed-off-by: asarazin <anaelle.sarazin@robocc.com> * add tests Signed-off-by: asarazin <anaelle.sarazin@robocc.com> --------- Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * Restore exported BT test utils header files after cmake revamp (#4652) (#4654) Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> * fix(bt_nodes): Correct default `server_timeout` behavior by using `getInputPortOrBlackboard()` (#4649) Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> * PoseStamped vector specialization (#4607) * PoseStamped vector specialization Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * merge master Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix size check Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix test" This reverts commit 51f54eb. * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * [DWB] Option to limit velocity commands in trajectory generator (#4663) * Option to limit vel cmd through traj generator Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Cleanup Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * fix linting Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Update linting Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * uncrustify Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * uncrustify Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> --------- Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> * Adding planner server timeout for costmap waiting (#4673) * Adding planner server timeout for costmap waiting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Adding controller server's costmap timeout as well Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing path longer on approach (#4622) * fixing path longer on approach Signed-off-by: Pradheep <padhupradheep@gmail.com> * removing the short circuit Signed-off-by: Pradheep <padhupradheep@gmail.com> * adding additional layer of check Signed-off-by: Pradheep <padhupradheep@gmail.com> --------- Signed-off-by: Pradheep <padhupradheep@gmail.com> * fix to bt action server logging before bt execution result being ready (#4677) Signed-off-by: DreamWest <sirjamestsao@gmail.com> * Correct paper name for graceful controller Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Added missing action clients in robot_navigator(BasicNavigator) to destroy_node (#4698) * fix: added assisted_teleop_client to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> * fix: added other missing action clients to robot_navigator(BasicNavigator) destroy_node Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> --------- Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> * Fixing SGF in MPPI and Smoother (#4669) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix: handle transition failures in all servers (#4708) * fix: handle transition failures in planner/controller/smoother servers Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * adding support for rest of servers + review comments Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Replacing throws with error and failed lifecycle transitions Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix vel smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server unit testing Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing last bits Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> * [RotationShimController] fix: rotate on short paths (#4716) Add header data to goal for short paths. Commit d8ae3c1 added the possibility to the rotation shim controller to rotate towards the goal when the goal was closer that the `forward_sampling_distance`. This feature was not fully working as the goal was missing proper header data, causing the rotation shim to give back control to the main controller. Co-authored-by: agennart <antoine.gennart@quimesis.be> * Added parameter `rotate_to_heading_once` (#4721) Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> * [RotationShimController] fix: rotate to goal heading (#4724) Add frame_id to goal when rotating towards goal heading, otherwise the transform would fail. This bug was introduced in 30e2cde by not setting the frame_id. Signed-off-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: agennart <antoine.gennart@quimesis.be> * [loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726) * Publish /clock from loopback sim Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t. This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data. In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS) Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * [nav2_smac_planner] fix typos Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Use single quotes Signed-off-by: Adi Vardi <adi.vardi@enway.ai> --------- Signed-off-by: Adi Vardi <adi.vardi@enway.ai> * Fix incorrect doxygen comment (#4741) Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> * Updating error logging in Smac collision detector object (#4743) * Updating error logging in Smac configs Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * linting Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * [map_io] Replace std logs by rclcpp logs (#4720) * replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * manual backport to Jazzy of 6b2e244 Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bump to 1.3.3 for jazzy sync Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing backport issue Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing backport of docking linking changes Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au> Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: huiyulhy <lhyleonghuiyu@gmail.com> Signed-off-by: Pradheep <padhupradheep@gmail.com> Signed-off-by: DreamWest <sirjamestsao@gmail.com> Signed-off-by: Tiwa Ojo <tiwa@reindeere.ca> Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Signed-off-by: Daniil Khaninaev <khaninaev@yahoo.com> Signed-off-by: agennart <antoine.gennart@quimesis.be> Signed-off-by: Adi Vardi <adi.vardi@enway.ai> Signed-off-by: Ryan Friedman <25047695+Ryanf55@users.noreply.github.com> Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: alyquantillion <158563995+alyquantillion@users.noreply.github.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: aosmw <116058035+aosmw@users.noreply.github.com> Co-authored-by: Alan <alan.xuefei@googlemail.com> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Huiyu Leong <26198479+huiyulhy@users.noreply.github.com> Co-authored-by: Pradheep Krishna <padhupradheep@gmail.com> Co-authored-by: DreamWest <sirjamestsao@gmail.com> Co-authored-by: Tiwa Ojo <55967921+tiwaojo@users.noreply.github.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Saitama <gennartan@users.noreply.github.com> Co-authored-by: agennart <antoine.gennart@quimesis.be> Co-authored-by: Daniil Khaninaev <khaninaev@yahoo.com> Co-authored-by: Adi Vardi <57910756+adivardi@users.noreply.github.com> Co-authored-by: Ryan <25047695+Ryanf55@users.noreply.github.com> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com>

* PoseStamped vector specialization Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * merge master Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * add path Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix size check Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Revert "fix test" This reverts commit 51f54eb. * fix test Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Joseph Duchesne <josephgeek@gmail.com>

PoseStamped vector specialization

f404906

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

SteveMacenski linked an issue Aug 8, 2024 that may be closed by this pull request

test_decorator_speed_controller complains about not finding template specialization (but succeeds) #4388

Closed

tonynajjar added 3 commits August 23, 2024 15:50

merge master

03c713f

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

Merge branch 'main' into fix-bt-specializations

dd57910

add path

9478d5b

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar marked this pull request as ready for review August 23, 2024 15:20

SteveMacenski reviewed Aug 23, 2024

View reviewed changes

nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp Outdated Show resolved Hide resolved

tonynajjar added 3 commits September 3, 2024 18:53

Merge remote-tracking branch 'upstream/main' into fix-bt-specializations

4b35740

fix size check

4d2b402

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

fix test

51f54eb

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar requested a review from SteveMacenski September 3, 2024 18:03

tonynajjar commented Sep 3, 2024

View reviewed changes

nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp Outdated Show resolved Hide resolved

tonynajjar commented Sep 3, 2024

View reviewed changes

nav2_behavior_tree/plugins/decorator/goal_updated_controller.cpp Outdated Show resolved Hide resolved

tonynajjar added 2 commits September 3, 2024 18:44

Revert "fix test"

3421529

This reverts commit 51f54eb.

fix test

8e4971e

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

tonynajjar commented Sep 3, 2024

View reviewed changes

SteveMacenski approved these changes Sep 3, 2024

View reviewed changes

tonynajjar merged commit 3ab6312 into ros-navigation:main Sep 4, 2024
11 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

PoseStamped vector specialization #4607

PoseStamped vector specialization #4607

tonynajjar commented Aug 8, 2024 •

edited

Loading

tonynajjar commented Aug 8, 2024

SteveMacenski commented Aug 21, 2024

tonynajjar commented Aug 21, 2024

tonynajjar commented Aug 23, 2024 •

edited

Loading

SteveMacenski left a comment

codecov bot commented Sep 3, 2024 •

edited

Loading

tonynajjar Sep 3, 2024

SteveMacenski Sep 3, 2024

SteveMacenski left a comment •

edited

Loading

tonynajjar commented Sep 4, 2024

PoseStamped vector specialization #4607

PoseStamped vector specialization #4607

Conversation

tonynajjar commented Aug 8, 2024 • edited Loading

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:

tonynajjar commented Aug 8, 2024

SteveMacenski commented Aug 21, 2024

tonynajjar commented Aug 21, 2024

tonynajjar commented Aug 23, 2024 • edited Loading

SteveMacenski left a comment

Choose a reason for hiding this comment

codecov bot commented Sep 3, 2024 • edited Loading

Codecov Report

tonynajjar Sep 3, 2024

Choose a reason for hiding this comment

SteveMacenski Sep 3, 2024

Choose a reason for hiding this comment

SteveMacenski left a comment • edited Loading

Choose a reason for hiding this comment

tonynajjar commented Sep 4, 2024

tonynajjar commented Aug 8, 2024 •

edited

Loading

tonynajjar commented Aug 23, 2024 •

edited

Loading

codecov bot commented Sep 3, 2024 •

edited

Loading

SteveMacenski left a comment •

edited

Loading