[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings #4067

SteveMacenski · 2024-01-25T02:10:36Z

@doisyg please test on the map we did onsite in Verda Park to verify this still solves the problem for you after my refactor

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

doisyg · 2024-01-25T23:21:07Z

Tested, amazing speed and behavior improvement ! GAME CHANGER

SteveMacenski · 2024-01-25T23:54:18Z

All I needed to hear :-)

…king (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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> [behavior_tree] support both ports and blackboard (ros-navigation#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove unused header Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add description Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * updated tree XML Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> 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 <ryanfriedman5410+github@gmail.com> adding new Friends of Nav2 Sponsors! Update README.md (ros-navigation#4114) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> 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 <alan.xuefei@googlemail.com> 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 <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com> Adding context to MPPI readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> [behavior_tree] Add generate_nav2_tree_nodes_xml (ros-navigation#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix previous commit (uncrustify) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * revert change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Fix compilation with clang (ros-navigation#4131) Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> 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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ziohang <hang.zhou@agvsz.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> 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 <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Updating stereolabs logo (ros-navigation#4158) Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> 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 <davide.faconti@gmail.com> * fix formatting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix lint warning Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * handle SKIPPED state Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix unit tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fit test Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix compilation Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> bug fix (ros-navigation#4160) Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com> 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 <kemal.bektas@node-robotics.com> * Support cancel in Smac planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Theta* planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Navfn planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add system tests for cancel Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Fix BT.CPP import (ros-navigation#4165) Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> improvement based on feedback Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

* nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix <felix@fzeltner.de> Co-authored-by: Felix <felix@fzeltner.de> * 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 <GoesM@buaa.edu.cn> * fix typo Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <amock@uos.de> * - 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 <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix missing param declare (#4203) Signed-off-by: nelson <kaichie.lee@gmail.com> * 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 <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * nav2_controller: add loop rate log (#4228) * added current loop rate printout Signed-off-by: ARK3r <kermani.areza@gmail.com> * remove empty line Signed-off-by: ARK3r <kermani.areza@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> * 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 <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> * 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 <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Fix typo (#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> * 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 <kaichie.lee@gmail.com> * update review comments and description Signed-off-by: nelson <kaichie.lee@gmail.com> * add VelocityPolygon to detector node Signed-off-by: nelson <kaichie.lee@gmail.com> * review update Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> * add velocity polygon tests Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * add in-line comment Signed-off-by: nelson <kaichie.lee@gmail.com> * fix push back Signed-off-by: nelson <kaichie.lee@gmail.com> * minor change and update README Signed-off-by: nelson <kaichie.lee@gmail.com> * update README Signed-off-by: nelson <kaichie.lee@gmail.com> --------- Signed-off-by: nelson <kaichie.lee@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * 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 <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Felix <felix@fzeltner.de> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: Reza Kermani <kermani.areza@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com> Co-authored-by: Felix <felix@fzeltner.de> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: nelson <kaichie.lee@gmail.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>

* change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> * 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 <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * adding progress checker selector BT node (#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * 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 <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <amock@uos.de> * - 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 <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

* change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> * 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 <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * 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 <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * 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 <amock@uos.de> * - 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 <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

* 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 <cihataltiparmak1@gmail.com> * 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 <cihataltiparmak1@gmail.com> --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * 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 <antonkesy@gmail.com> * fix use after free (ros-navigation#3910) * fix build mppi (ros-navigation#3927) Signed-off-by: kevin <kevin@floatic.io> Co-authored-by: kevin <kevin@floatic.io> * 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 <cihataltiparmak1@gmail.com> Signed-off-by: Anton Kesy <antonkesy@gmail.com> Signed-off-by: kevin <kevin@floatic.io> Co-authored-by: thandal <than@timbrel.org> Co-authored-by: Anton Kesy <antonkesy@gmail.com> Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin <kevin@floatic.io> Co-authored-by: Abiel Fernandez <empoleom@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> * 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 <rhys.mainwaring@me.com> * critical fix for path tracking MPPI (ros-navigation#4099) (ros-navigation#4101) (cherry picked from commit 8f7b6ce) Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com> (cherry picked from commit c59e0f3) Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> * 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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ziohang <hang.zhou@agvsz.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> (cherry picked from commit 2e491b1) Co-authored-by: iceuw <m201770623@hust.edu.cn> * bug fix (ros-navigation#4160) (ros-navigation#4162) Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com> (cherry picked from commit 623434b) Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com> * Add uint suffix (ros-navigation#4213) (ros-navigation#4216) (cherry picked from commit 46ff0ec) Co-authored-by: Michael Wrock <michael.wrock@picknik.ai> * Fix merge conflict in nav2_mppi_controller README (humble) (ros-navigation#4239) Signed-off-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> * 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 <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> * 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 <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * 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 <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * 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 <amock@uos.de> * - 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 <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix graceful humble release version Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix graceful_controller exceptions --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Signed-off-by: Anton Kesy <antonkesy@gmail.com> Signed-off-by: kevin <kevin@floatic.io> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: thandal <than@timbrel.org> Co-authored-by: Anton Kesy <antonkesy@gmail.com> Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin <kevin@floatic.io> Co-authored-by: Abiel Fernandez <empoleom@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> Co-authored-by: iceuw <m201770623@hust.edu.cn> Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com> Co-authored-by: Michael Wrock <michael.wrock@picknik.ai> Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

…king (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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: enricosutera <enricosutera@outlook.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> Signed-off-by: enricosutera <enricosutera@outlook.com>

* nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (ros-navigation#4178) Signed-off-by: Felix <felix@fzeltner.de> Co-authored-by: Felix <felix@fzeltner.de> * 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 <GoesM@buaa.edu.cn> * fix typo Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * 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 <amock@uos.de> * - 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 <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix missing param declare (ros-navigation#4203) Signed-off-by: nelson <kaichie.lee@gmail.com> * 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 <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * nav2_controller: add loop rate log (ros-navigation#4228) * added current loop rate printout Signed-off-by: ARK3r <kermani.areza@gmail.com> * remove empty line Signed-off-by: ARK3r <kermani.areza@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> * 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 <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> * 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 <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Fix typo (ros-navigation#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> * 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 <kaichie.lee@gmail.com> * update review comments and description Signed-off-by: nelson <kaichie.lee@gmail.com> * add VelocityPolygon to detector node Signed-off-by: nelson <kaichie.lee@gmail.com> * review update Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> * add velocity polygon tests Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * add in-line comment Signed-off-by: nelson <kaichie.lee@gmail.com> * fix push back Signed-off-by: nelson <kaichie.lee@gmail.com> * minor change and update README Signed-off-by: nelson <kaichie.lee@gmail.com> * update README Signed-off-by: nelson <kaichie.lee@gmail.com> --------- Signed-off-by: nelson <kaichie.lee@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * 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 <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Felix <felix@fzeltner.de> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: Reza Kermani <kermani.areza@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com> Co-authored-by: Felix <felix@fzeltner.de> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: nelson <kaichie.lee@gmail.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>

…king (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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

* nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (ros-navigation#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (ros-navigation#4178) Signed-off-by: Felix <felix@fzeltner.de> Co-authored-by: Felix <felix@fzeltner.de> * 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 <GoesM@buaa.edu.cn> * fix typo Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (ros-navigation#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * 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 <amock@uos.de> * - 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 <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (ros-navigation#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (ros-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (ros-navigation#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix missing param declare (ros-navigation#4203) Signed-off-by: nelson <kaichie.lee@gmail.com> * 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 <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * nav2_controller: add loop rate log (ros-navigation#4228) * added current loop rate printout Signed-off-by: ARK3r <kermani.areza@gmail.com> * remove empty line Signed-off-by: ARK3r <kermani.areza@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> * 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 <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> * 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 <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Fix typo (ros-navigation#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> * 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 <kaichie.lee@gmail.com> * update review comments and description Signed-off-by: nelson <kaichie.lee@gmail.com> * add VelocityPolygon to detector node Signed-off-by: nelson <kaichie.lee@gmail.com> * review update Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> * add velocity polygon tests Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * add in-line comment Signed-off-by: nelson <kaichie.lee@gmail.com> * fix push back Signed-off-by: nelson <kaichie.lee@gmail.com> * minor change and update README Signed-off-by: nelson <kaichie.lee@gmail.com> * update README Signed-off-by: nelson <kaichie.lee@gmail.com> --------- Signed-off-by: nelson <kaichie.lee@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * 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 <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Felix <felix@fzeltner.de> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: Reza Kermani <kermani.areza@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com> Co-authored-by: Felix <felix@fzeltner.de> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: nelson <kaichie.lee@gmail.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: MarcM0 <marc.morcos9@gmail.com>

…king (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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> (cherry picked from commit 6bda3f3)

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> New MPPI Cost Critic (Contrib: Brice Renaudeau) (ros-navigation#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> [behavior_tree] support both ports and blackboard (ros-navigation#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove unused header Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add description Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * updated tree XML Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> 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 <ryanfriedman5410+github@gmail.com> adding new Friends of Nav2 Sponsors! Update README.md (ros-navigation#4114) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> 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 <alan.xuefei@googlemail.com> 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 <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com> Adding context to MPPI readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> [behavior_tree] Add generate_nav2_tree_nodes_xml (ros-navigation#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix previous commit (uncrustify) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * revert change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Fix compilation with clang (ros-navigation#4131) Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> 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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ziohang <hang.zhou@agvsz.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> 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 <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Updating stereolabs logo (ros-navigation#4158) Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> 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 <davide.faconti@gmail.com> * fix formatting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix lint warning Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * handle SKIPPED state Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix unit tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fit test Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix compilation Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> bug fix (ros-navigation#4160) Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com> 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 <kemal.bektas@node-robotics.com> * Support cancel in Smac planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Theta* planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Navfn planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add system tests for cancel Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Fix BT.CPP import (ros-navigation#4165) Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Update default recommendation from Obstacles to Cost critic in MPPI (ros-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> nav2_controller: add loop rate log (ros-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> improvement based on feedback Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Signed-off-by: stevedanomodolor <stevedan.o.omodolor@gmail.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (ros-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in ros-navigation#4068 (ros-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [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 <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (ros-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (ros-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * 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 <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * 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 <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (ros-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * 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 <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

SteveMacenski added 7 commits January 16, 2024 10:59

prototype to test SE2 footprint H improvements

5827038

some fixes

bf175d0

fixed

6266791

invert logic

916e8a8

Working final prototype to be tested

f68faf7

complete unit test conversions

2c789de

Update inflation_layer.hpp

07f81d1

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

SteveMacenski merged commit 6bda3f3 into main Jan 25, 2024

SteveMacenski deleted the dex_smac branch January 25, 2024 23:54

SteveMacenski mentioned this pull request Jan 25, 2024

Homologate Smac & MPPI: getInflationLayer (easy) #4066

Closed

papertowel123 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

Uh oh!

[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings #4067

[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings #4067

Uh oh!

SteveMacenski commented Jan 25, 2024

doisyg commented Jan 25, 2024

SteveMacenski commented Jan 25, 2024

Labels

3 participants

[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings #4067

[Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings #4067

Uh oh!

Conversation

SteveMacenski commented Jan 25, 2024

doisyg commented Jan 25, 2024

SteveMacenski commented Jan 25, 2024

Labels

3 participants