Allow path end pose deviation revive #4065

jwallace42 · 2024-01-25T00:31:20Z

This pr revives the work done here by @pepisg

Co-authored-by: Steve Macenski <[email protected]>

…/navigation2 into allow_path_end_pose_deviation

…ath_end_pose_deviation

SteveMacenski · 2024-01-25T17:26:56Z

I think we were talking about having the param removed and always using the path's final goal as the start to the new?

pepisg · 2024-01-25T19:01:06Z

I think we were talking about having the param removed and always using the path's final goal as the start to the new

Yeah, I think that was what we agreed on slack. Also, discussion was gravitating around possible artifacts happening on Lattice. Finally, I wasn't sure how test should look like for this change (sorry for taking that long to review this work again 😢 )

Signed-off-by: gg <[email protected]>

nav2_planner/include/nav2_planner/planner_server.hpp

nav2_planner/src/planner_server.cpp

* Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]>

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]>

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]>

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> Signed-off-by: stevedan <[email protected]>

Signed-off-by: stevedan <[email protected]> New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> [behavior_tree] support both ports and blackboard (ros-navigation#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti <[email protected]> * remove unused header Signed-off-by: Davide Faconti <[email protected]> * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti <[email protected]> * add description Signed-off-by: Davide Faconti <[email protected]> * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti <[email protected]> * updated tree XML Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> adding new Friends of Nav2 Sponsors! Update README.md (ros-navigation#4114) Signed-off-by: Steve Macenski <[email protected]> CI green P3 (ros-navigation#4117) Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (ros-navigation#4113). (ros-navigation#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue <[email protected]> Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <[email protected]> * Revised test name Signed-off-by: Hunter Song <[email protected]> --------- Signed-off-by: Hunter Song <[email protected]> Adding context to MPPI readme Signed-off-by: Steve Macenski <[email protected]> [behavior_tree] Add generate_nav2_tree_nodes_xml (ros-navigation#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti <[email protected]> * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti <[email protected]> * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti <[email protected]> * fix previous commit (uncrustify) Signed-off-by: Davide Faconti <[email protected]> * revert change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Fix compilation with clang (ros-navigation#4131) Signed-off-by: Ramon Wijnands <[email protected]> code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan <[email protected]> Fix goal handle was not freed correctly (ros-navigation#4137) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Ziohang <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Adding Nvidia as Gold Sponsor! (ros-navigation#4144) Prevent a possible segmentation fault (ros-navigation#4141) (ros-navigation#4147) * Prevent a possible segmentation fault Signed-off-by: Joni Pöllänen <[email protected]> * Cleanup Signed-off-by: Joni Pöllänen <[email protected]> --------- Signed-off-by: Joni Pöllänen <[email protected]> Updating stereolabs logo (ros-navigation#4158) Update README.md Signed-off-by: Steve Macenski <[email protected]> behavior_tree: migration to BT.CPP 4.5.x (ros-navigation#4128) * behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti <[email protected]> * fix formatting Signed-off-by: Davide Faconti <[email protected]> * fix lint warning Signed-off-by: Davide Faconti <[email protected]> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <[email protected]> * handle SKIPPED state Signed-off-by: Davide Faconti <[email protected]> * fix SequenceStar Signed-off-by: Davide Faconti <[email protected]> * add tests Signed-off-by: Davide Faconti <[email protected]> * fix Signed-off-by: Davide Faconti <[email protected]> * fix unit tests Signed-off-by: Davide Faconti <[email protected]> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <[email protected]> * fit test Signed-off-by: Davide Faconti <[email protected]> * fix Signed-off-by: Davide Faconti <[email protected]> * remove SequenceStar Signed-off-by: Davide Faconti <[email protected]> * remove SequenceStar Signed-off-by: Davide Faconti <[email protected]> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <[email protected]> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <[email protected]> * fix compilation Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> bug fix (ros-navigation#4160) Signed-off-by: Tianchu <[email protected]> Stop planner if the goal is cancelled (ros-navigation#4148) * Add support for stopping planners when the action is cancelled Signed-off-by: Kemal Bektas <[email protected]> * Support cancel in Smac planner Signed-off-by: Kemal Bektas <[email protected]> * Support cancel in Theta* planner Signed-off-by: Kemal Bektas <[email protected]> * Support cancel in Navfn planner Signed-off-by: Kemal Bektas <[email protected]> * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas <[email protected]> * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas <[email protected]> * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas <[email protected]> * Add system tests for cancel Signed-off-by: Kemal Bektas <[email protected]> --------- Signed-off-by: Kemal Bektas <[email protected]> Co-authored-by: Kemal Bektas <[email protected]> Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan <[email protected]> Fix BT.CPP import (ros-navigation#4165) Signed-off-by: Tony Najjar <[email protected]> Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <[email protected]> nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <[email protected]> * added current controller loop rate logging Signed-off-by: ARK3r <[email protected]> * linting Signed-off-by: ARK3r <[email protected]> * uncrustify lint Signed-off-by: ARK3r <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> improvement based on feedback Signed-off-by: stevedan <[email protected]>

* Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]>

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]>

* Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]>

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]>

* nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r <[email protected]> * added current controller loop rate logging Signed-off-by: ARK3r <[email protected]> * linting Signed-off-by: ARK3r <[email protected]> * uncrustify lint Signed-off-by: ARK3r <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix <[email protected]> Co-authored-by: Felix <[email protected]> * replace throw-error with error-log to avoid UAF mentioned in #4175 (#4180) * replace throw-error with error-log to avoid UAF Signed-off-by: GoesM <[email protected]> * fix typo Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * fix typos in description messages (#4188) Signed-off-by: Antonio Park <[email protected]> * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <[email protected]> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <[email protected]> * added whitespace Signed-off-by: Alexander Mock <[email protected]> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <[email protected]> --------- Signed-off-by: Alexander Mock <[email protected]> * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park <[email protected]> * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <[email protected]> * Update README.md --------- Signed-off-by: Tony Najjar <[email protected]> * Update footprint iif changed (#4193) Signed-off-by: Brice <[email protected]> * fix missing param declare (#4203) Signed-off-by: nelson <[email protected]> * Revert "nav2_controller: add loop rate log (#4171)" (#4210) This reverts commit 4737462. * add polygon_subscribe_transient_local parameter in collision monitor (#4207) Signed-off-by: asarazin <[email protected]> Co-authored-by: asarazin <[email protected]> * nav2_controller: add loop rate log (#4228) * added current loop rate printout Signed-off-by: ARK3r <[email protected]> * remove empty line Signed-off-by: ARK3r <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> * bump to 1.2.7 for iron sync * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in #4078 (#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <[email protected]> * reformat Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Fix typo (#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <[email protected]> * Add velocity based polygon (#3708) * add velocity based polygon * fix header, copyright and variable name change * optimise polygon update * optimise duplicated code with setPolygonShape * add warning log for uncovered speed * update feedback * rename polygon velocity to velocity polygon * cleanup * fix typo * add dynamic support for velocity polygon * wrap try catch for getting parameters * update naming and linting * use switch case * Revert "use switch case" This reverts commit 1230ede. * fix proper return for invalid parameters * remove topic parameter for velocity polygon * fix formatting manually * continue if points are not defined * rewrite velocity polygon with polygon base class Signed-off-by: nelson <[email protected]> * update review comments and description Signed-off-by: nelson <[email protected]> * add VelocityPolygon to detector node Signed-off-by: nelson <[email protected]> * review update Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: nelson <[email protected]> * add velocity polygon tests Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * add in-line comment Signed-off-by: nelson <[email protected]> * fix push back Signed-off-by: nelson <[email protected]> * minor change and update README Signed-off-by: nelson <[email protected]> * update README Signed-off-by: nelson <[email protected]> --------- Signed-off-by: nelson <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * avoid implicit type case (#4098) * adding progress checker selector BT node (#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> * Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski <[email protected]> * fix merge conflict * Revert "Add velocity based polygon (#3708)" This reverts commit a3ed745. * custom backported version of cost critic inscribed cost --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Felix <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: nelson <[email protected]> Signed-off-by: asarazin <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: nfriedrich00 <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: Reza Kermani <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: bi0ha2ard <[email protected]> Co-authored-by: Felix <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: nelson <[email protected]> Co-authored-by: anaelle-sw <[email protected]> Co-authored-by: asarazin <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: nfriedrich00 <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]>

* change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in #4078 (#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <[email protected]> * reformat Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * adding progress checker selector BT node (#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> * Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski <[email protected]> * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * fix typos in description messages (#4188) Signed-off-by: Antonio Park <[email protected]> * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <[email protected]> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <[email protected]> * added whitespace Signed-off-by: Alexander Mock <[email protected]> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <[email protected]> --------- Signed-off-by: Alexander Mock <[email protected]> * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park <[email protected]> * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <[email protected]> * Update README.md --------- Signed-off-by: Tony Najjar <[email protected]> * Update footprint iif changed (#4193) Signed-off-by: Brice <[email protected]> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]>

* change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in ros-navigation#4078 (ros-navigation#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <[email protected]> * reformat Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * adding progress checker selector BT node (ros-navigation#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> * Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <[email protected]> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <[email protected]> * AMCL: Set an initial guess by service call (ros-navigation#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <[email protected]> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <[email protected]> * added whitespace Signed-off-by: Alexander Mock <[email protected]> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <[email protected]> --------- Signed-off-by: Alexander Mock <[email protected]> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <[email protected]> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <[email protected]> * Update README.md --------- Signed-off-by: Tony Najjar <[email protected]> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <[email protected]> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]>

* Fix ros-navigation#4001 (ros-navigation#4028) * Humble Sync 9: Jan 23, 2024 (ros-navigation#4064) * Update README.md: fix typo (ros-navigation#3842) * Update package.xml * fix typo (ros-navigation#3850) * adjust link to point to v3.8 of behavior tree docs (ros-navigation#3851) BT.CPP_v3 is used, thereby the correct docs should be linked * Fix bug in nav2_behavior_tree/bt_action_node (ros-navigation#3849) * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <[email protected]> * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <[email protected]> --------- Signed-off-by: CihatAltiparmak <[email protected]> * simplication of lattice_generator.py, fixes ros-navigation#3858 (ros-navigation#3859) * simplification of equation to compute the max_value/outer edge of the lattice based on number of headings. * Fix class doxygen * fix minor typos (ros-navigation#3892) Signed-off-by: Anton Kesy <[email protected]> * fix use after free (ros-navigation#3910) * fix build mppi (ros-navigation#3927) Signed-off-by: kevin <[email protected]> Co-authored-by: kevin <[email protected]> * Removing old TODOs * Fixed links for install and build in README (ros-navigation#3963) Currently the readme is linking to an invalida page in the docs (404 error). * fix a few outdated comments in smac planners (ros-navigation#3978) * Update README.md * bump to 1.1.13 for release --------- Signed-off-by: CihatAltiparmak <[email protected]> Signed-off-by: Anton Kesy <[email protected]> Signed-off-by: kevin <[email protected]> Co-authored-by: thandal <[email protected]> Co-authored-by: Anton Kesy <[email protected]> Co-authored-by: CihatAltiparmak <[email protected]> Co-authored-by: Anil Kumar Chavali <[email protected]> Co-authored-by: 정찬희 <[email protected]> Co-authored-by: kevin <[email protected]> Co-authored-by: Abiel Fernandez <[email protected]> Co-authored-by: Michael Ferguson <[email protected]> * Fixes for compiling with clang on macOS (ros-navigation#4051) amcl: declare void parameter for functions with no args amcl: remove unused variables behavior_tree: address clang compilation issues behaviors: add missing override specifier bt_navigator: add missing override specifier collision_monitor: address clang compilation issues constrained_smoother: address clang compilation issues controller: address clang compilation issues costmap_2d: add missing override specifier costmap_2d: address clang compilation issues costmap_2d: fix link issue for order_layer dwb_controller: fix clang compile issue, replace ulong with uint32_t map_server: replace std::experimental::filesystem map_server: remove dependency on stdc++fs waypoint_follower: address clang compilation issues waypoint_follower: remove dependency on stdc++fs waypoint_follower: replace std::experimental::filesystem smoother: address clang compilation issues smoother: remove unused variables system_tests: remove dependency on stdc++fs rotation_shim_controller: update percentage arg in setSpeedLimit to boolean planner: remove unused variables costmap_2d: address clang compilation issues mppi_controller: replace use of auto as function param with templates mppi_controller: address clang compilation issues costmap_2d: resolve clang issue with std::pair non-const copy Signed-off-by: Rhys Mainwaring <[email protected]> * critical fix for path tracking MPPI (ros-navigation#4099) (ros-navigation#4101) (cherry picked from commit 8f7b6ce) Co-authored-by: Steve Macenski <[email protected]> * backport graceful controller to humble (ros-navigation#4119) * Added cast to float to getClosestAngularBin return for behavior consistency (ros-navigation#4123) (ros-navigation#4125) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <[email protected]> * Revised test name Signed-off-by: Hunter Song <[email protected]> --------- Signed-off-by: Hunter Song <[email protected]> (cherry picked from commit c59e0f3) Co-authored-by: hsong-MLE <[email protected]> * Fix goal handle was not freed correctly (ros-navigation#4137) (ros-navigation#4143) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Ziohang <[email protected]> Co-authored-by: Steve Macenski <[email protected]> (cherry picked from commit 2e491b1) Co-authored-by: iceuw <[email protected]> * bug fix (ros-navigation#4160) (ros-navigation#4162) Signed-off-by: Tianchu <[email protected]> (cherry picked from commit 623434b) Co-authored-by: Chortine <[email protected]> * Add uint suffix (ros-navigation#4213) (ros-navigation#4216) (cherry picked from commit 46ff0ec) Co-authored-by: Michael Wrock <[email protected]> * Fix merge conflict in nav2_mppi_controller README (humble) (ros-navigation#4239) Signed-off-by: Stephanie Eng <[email protected]> * Humble sync 10: April 4, 2024 (ros-navigation#4241) * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in ros-navigation#4078 (ros-navigation#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <[email protected]> * reformat Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * adding progress checker selector BT node (ros-navigation#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> * Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <[email protected]> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <[email protected]> * AMCL: Set an initial guess by service call (ros-navigation#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <[email protected]> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <[email protected]> * added whitespace Signed-off-by: Alexander Mock <[email protected]> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <[email protected]> --------- Signed-off-by: Alexander Mock <[email protected]> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <[email protected]> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <[email protected]> * Update README.md --------- Signed-off-by: Tony Najjar <[email protected]> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <[email protected]> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]> * Fix graceful humble release version Signed-off-by: Steve Macenski <[email protected]> * Fix graceful_controller exceptions --------- Signed-off-by: CihatAltiparmak <[email protected]> Signed-off-by: Anton Kesy <[email protected]> Signed-off-by: kevin <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Stephanie Eng <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: thandal <[email protected]> Co-authored-by: Anton Kesy <[email protected]> Co-authored-by: CihatAltiparmak <[email protected]> Co-authored-by: Anil Kumar Chavali <[email protected]> Co-authored-by: 정찬희 <[email protected]> Co-authored-by: kevin <[email protected]> Co-authored-by: Abiel Fernandez <[email protected]> Co-authored-by: Michael Ferguson <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> Co-authored-by: hsong-MLE <[email protected]> Co-authored-by: iceuw <[email protected]> Co-authored-by: Chortine <[email protected]> Co-authored-by: Michael Wrock <[email protected]> Co-authored-by: Stephanie Eng <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]>

* Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: enricosutera <[email protected]>

* Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> Signed-off-by: enricosutera <[email protected]>

* nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <[email protected]> * added current controller loop rate logging Signed-off-by: ARK3r <[email protected]> * linting Signed-off-by: ARK3r <[email protected]> * uncrustify lint Signed-off-by: ARK3r <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (ros-navigation#4178) Signed-off-by: Felix <[email protected]> Co-authored-by: Felix <[email protected]> * replace throw-error with error-log to avoid UAF mentioned in ros-navigation#4175 (ros-navigation#4180) * replace throw-error with error-log to avoid UAF Signed-off-by: GoesM <[email protected]> * fix typo Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <[email protected]> * AMCL: Set an initial guess by service call (ros-navigation#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <[email protected]> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <[email protected]> * added whitespace Signed-off-by: Alexander Mock <[email protected]> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <[email protected]> --------- Signed-off-by: Alexander Mock <[email protected]> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <[email protected]> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <[email protected]> * Update README.md --------- Signed-off-by: Tony Najjar <[email protected]> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <[email protected]> * fix missing param declare (ros-navigation#4203) Signed-off-by: nelson <[email protected]> * Revert "nav2_controller: add loop rate log (ros-navigation#4171)" (ros-navigation#4210) This reverts commit 4737462. * add polygon_subscribe_transient_local parameter in collision monitor (ros-navigation#4207) Signed-off-by: asarazin <[email protected]> Co-authored-by: asarazin <[email protected]> * nav2_controller: add loop rate log (ros-navigation#4228) * added current loop rate printout Signed-off-by: ARK3r <[email protected]> * remove empty line Signed-off-by: ARK3r <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> * bump to 1.2.7 for iron sync * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in ros-navigation#4078 (ros-navigation#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <[email protected]> * reformat Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Fix typo (ros-navigation#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <[email protected]> * Add velocity based polygon (ros-navigation#3708) * add velocity based polygon * fix header, copyright and variable name change * optimise polygon update * optimise duplicated code with setPolygonShape * add warning log for uncovered speed * update feedback * rename polygon velocity to velocity polygon * cleanup * fix typo * add dynamic support for velocity polygon * wrap try catch for getting parameters * update naming and linting * use switch case * Revert "use switch case" This reverts commit 1230ede. * fix proper return for invalid parameters * remove topic parameter for velocity polygon * fix formatting manually * continue if points are not defined * rewrite velocity polygon with polygon base class Signed-off-by: nelson <[email protected]> * update review comments and description Signed-off-by: nelson <[email protected]> * add VelocityPolygon to detector node Signed-off-by: nelson <[email protected]> * review update Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: nelson <[email protected]> * add velocity polygon tests Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * add in-line comment Signed-off-by: nelson <[email protected]> * fix push back Signed-off-by: nelson <[email protected]> * minor change and update README Signed-off-by: nelson <[email protected]> * update README Signed-off-by: nelson <[email protected]> --------- Signed-off-by: nelson <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * avoid implicit type case (ros-navigation#4098) * adding progress checker selector BT node (ros-navigation#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> * Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <[email protected]> * fix merge conflict * Revert "Add velocity based polygon (ros-navigation#3708)" This reverts commit a3ed745. * custom backported version of cost critic inscribed cost --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Felix <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: nelson <[email protected]> Signed-off-by: asarazin <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: nfriedrich00 <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: Reza Kermani <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: bi0ha2ard <[email protected]> Co-authored-by: Felix <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: nelson <[email protected]> Co-authored-by: anaelle-sw <[email protected]> Co-authored-by: asarazin <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: nfriedrich00 <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]>

* nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <[email protected]> * added current controller loop rate logging Signed-off-by: ARK3r <[email protected]> * linting Signed-off-by: ARK3r <[email protected]> * uncrustify lint Signed-off-by: ARK3r <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (ros-navigation#4178) Signed-off-by: Felix <[email protected]> Co-authored-by: Felix <[email protected]> * replace throw-error with error-log to avoid UAF mentioned in ros-navigation#4175 (ros-navigation#4180) * replace throw-error with error-log to avoid UAF Signed-off-by: GoesM <[email protected]> * fix typo Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <[email protected]> * AMCL: Set an initial guess by service call (ros-navigation#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <[email protected]> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <[email protected]> * added whitespace Signed-off-by: Alexander Mock <[email protected]> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <[email protected]> --------- Signed-off-by: Alexander Mock <[email protected]> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <[email protected]> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <[email protected]> * Update README.md --------- Signed-off-by: Tony Najjar <[email protected]> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <[email protected]> * fix missing param declare (ros-navigation#4203) Signed-off-by: nelson <[email protected]> * Revert "nav2_controller: add loop rate log (ros-navigation#4171)" (ros-navigation#4210) This reverts commit 4737462. * add polygon_subscribe_transient_local parameter in collision monitor (ros-navigation#4207) Signed-off-by: asarazin <[email protected]> Co-authored-by: asarazin <[email protected]> * nav2_controller: add loop rate log (ros-navigation#4228) * added current loop rate printout Signed-off-by: ARK3r <[email protected]> * remove empty line Signed-off-by: ARK3r <[email protected]> --------- Signed-off-by: ARK3r <[email protected]> * bump to 1.2.7 for iron sync * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in ros-navigation#4078 (ros-navigation#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <[email protected]> * reformat Signed-off-by: GoesM <[email protected]> --------- Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> * Fix typo (ros-navigation#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <[email protected]> * Add velocity based polygon (ros-navigation#3708) * add velocity based polygon * fix header, copyright and variable name change * optimise polygon update * optimise duplicated code with setPolygonShape * add warning log for uncovered speed * update feedback * rename polygon velocity to velocity polygon * cleanup * fix typo * add dynamic support for velocity polygon * wrap try catch for getting parameters * update naming and linting * use switch case * Revert "use switch case" This reverts commit 1230ede. * fix proper return for invalid parameters * remove topic parameter for velocity polygon * fix formatting manually * continue if points are not defined * rewrite velocity polygon with polygon base class Signed-off-by: nelson <[email protected]> * update review comments and description Signed-off-by: nelson <[email protected]> * add VelocityPolygon to detector node Signed-off-by: nelson <[email protected]> * review update Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: nelson <[email protected]> * add velocity polygon tests Signed-off-by: nelson <[email protected]> * fix cpplint Signed-off-by: nelson <[email protected]> * add in-line comment Signed-off-by: nelson <[email protected]> * fix push back Signed-off-by: nelson <[email protected]> * minor change and update README Signed-off-by: nelson <[email protected]> * update README Signed-off-by: nelson <[email protected]> --------- Signed-off-by: nelson <[email protected]> Co-authored-by: Steve Macenski <[email protected]> * avoid implicit type case (ros-navigation#4098) * adding progress checker selector BT node (ros-navigation#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <[email protected]> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <[email protected]> * fix lint cpp - extra space Signed-off-by: Brice <[email protected]> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <[email protected]> * Fix tests Signed-off-by: Steve Macenski <[email protected]> * Update test_a_star.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (ros-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Signed-off-by: Brice <[email protected]> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (ros-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Update README.md Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <[email protected]> * misse change Signed-off-by: Davide Faconti <[email protected]> --------- Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Brice <[email protected]> * Allow path end pose deviation revive (ros-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <[email protected]> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <[email protected]> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Signed-off-by: Brice <[email protected]> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <[email protected]> * Fix linting Signed-off-by: Jose Faria <[email protected]> --------- Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Brice <[email protected]> * 1594 twist stamped publisher (ros-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice <[email protected]> * fix include of hpp Signed-off-by: Brice Renaudeau <[email protected]> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <[email protected]> * Update cost_critic.cpp Signed-off-by: Steve Macenski <[email protected]> --------- Signed-off-by: Brice <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Co-authored-by: Kino <[email protected]> * Use ament_export_targets for all targets (ros-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <[email protected]> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <[email protected]> * fix merge conflict * Revert "Add velocity based polygon (ros-navigation#3708)" This reverts commit a3ed745. * custom backported version of cost critic inscribed cost --------- Signed-off-by: ARK3r <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: GoesM <[email protected]> Signed-off-by: Felix <[email protected]> Signed-off-by: Antonio Park <[email protected]> Signed-off-by: Alexander Mock <[email protected]> Signed-off-by: Kyungsik Park <[email protected]> Signed-off-by: Tony Najjar <[email protected]> Signed-off-by: Brice <[email protected]> Signed-off-by: nelson <[email protected]> Signed-off-by: asarazin <[email protected]> Signed-off-by: gg <[email protected]> Signed-off-by: cybaol <[email protected]> Signed-off-by: nfriedrich00 <[email protected]> Signed-off-by: Davide Faconti <[email protected]> Signed-off-by: Jose Faria <[email protected]> Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice Renaudeau <[email protected]> Co-authored-by: Reza Kermani <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: GoesM <[email protected]> Co-authored-by: bi0ha2ard <[email protected]> Co-authored-by: Felix <[email protected]> Co-authored-by: Antonio Park <[email protected]> Co-authored-by: Alexander Mock <[email protected]> Co-authored-by: Tony Najjar <[email protected]> Co-authored-by: BriceRenaudeau <[email protected]> Co-authored-by: nelson <[email protected]> Co-authored-by: anaelle-sw <[email protected]> Co-authored-by: asarazin <[email protected]> Co-authored-by: Joshua Wallace <[email protected]> Co-authored-by: pepisg <[email protected]> Co-authored-by: Pedro Alejandro González <[email protected]> Co-authored-by: Kino <[email protected]> Co-authored-by: nfriedrich00 <[email protected]> Co-authored-by: Davide Faconti <[email protected]> Co-authored-by: jncfa <[email protected]> Co-authored-by: Ryan <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: MarcM0 <[email protected]>

pepisg and others added 9 commits November 2, 2023 01:22

Support stitching paths in compute path to poses

be2904e

Update nav2_planner/src/planner_server.cpp

a939eab

Co-authored-by: Steve Macenski <[email protected]>

Rename parameter to allow_path_through_poses_goal_deviation

f2efccd

Merge branch 'allow_path_end_pose_deviation' of github.com:kiwicampus…

92a443d

…/navigation2 into allow_path_end_pose_deviation

Fix description

0b64122

Merge branch 'main' of github.com:kiwicampus/navigation2 into allow_p…

6b1a8a2

…ath_end_pose_deviation

restore nav2_params

24b7a57

missing whitespace

409ead1

Merge branch 'main' into allow_path_end_pose_deviation_revive

9a88fe9

jwallace42 requested a review from SteveMacenski January 25, 2024 00:31

lint fix

ba23449

SteveMacenski mentioned this pull request Jan 25, 2024

Support starting on the last pose of planned path instead of the exact waypoint in compute path through poses action #3928

Closed

7 tasks

removed parameter

95ea0aa

Signed-off-by: gg <[email protected]>

SteveMacenski requested changes Jan 26, 2024

View reviewed changes

nav2_planner/include/nav2_planner/planner_server.hpp Outdated Show resolved Hide resolved

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

jwallace42 added 2 commits January 26, 2024 11:36

Update planner_server.hpp

58c00f6

Update planner_server.cpp

9cc61f4

SteveMacenski approved these changes Jan 26, 2024

View reviewed changes

SteveMacenski merged commit 8daece6 into ros-navigation:main Jan 26, 2024
9 of 11 checks passed

maksymdidukh mentioned this pull request Jun 24, 2024

Backport velocity polygons logivations/navigation2#71

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Allow path end pose deviation revive #4065

Allow path end pose deviation revive #4065

jwallace42 commented Jan 25, 2024

SteveMacenski commented Jan 25, 2024

pepisg commented Jan 25, 2024 •

edited

Loading

Allow path end pose deviation revive #4065

Allow path end pose deviation revive #4065

Conversation

jwallace42 commented Jan 25, 2024

SteveMacenski commented Jan 25, 2024

pepisg commented Jan 25, 2024 • edited Loading

pepisg commented Jan 25, 2024 •

edited

Loading