Fix Smac Planner confined collision checker #4055

SteveMacenski · 2024-01-17T12:28:30Z

Note: getCost has 0.5 cells added due to static_cast's flooring behavior to be more accurate in the continuous coordinate system. This is not due to the +0.5 offset in mapToWorld used for

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

* 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]>

* 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]>

* 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]>

* 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: 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]>

* 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]>

* 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]>

SteveMacenski added 3 commits January 17, 2024 04:28

Update collision_checker.cpp

8891a53

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

Fix tests

1f3745f

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

Update test_a_star.cpp

743a09d

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

SteveMacenski merged commit 6a540f1 into main Jan 20, 2024
9 of 11 checks passed

SteveMacenski deleted the SteveMacenski-patch-1 branch January 20, 2024 11:59

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

Fix Smac Planner confined collision checker #4055

Fix Smac Planner confined collision checker #4055

SteveMacenski commented Jan 17, 2024 •

edited

Loading

Fix Smac Planner confined collision checker #4055

Fix Smac Planner confined collision checker #4055

Conversation

SteveMacenski commented Jan 17, 2024 • edited Loading

SteveMacenski commented Jan 17, 2024 •

edited

Loading