From a3043bdbfc6434e543c1521660415f7f55308071 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 4 Apr 2024 14:53:58 -0700 Subject: [PATCH] Iron sync 5: April 4, 2024 (#4240) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r * added current controller loop rate logging Signed-off-by: ARK3r * linting Signed-off-by: ARK3r * uncrustify lint Signed-off-by: ARK3r * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: ARK3r Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM Co-authored-by: GoesM * chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix Co-authored-by: Felix * 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 * fix typo Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM * fix typos in description messages (#4188) Signed-off-by: Antonio Park * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock * - 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 * added whitespace Signed-off-by: Alexander Mock * renamed initial pose service in callback bind Signed-off-by: Alexander Mock --------- Signed-off-by: Alexander Mock * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar * Update README.md --------- Signed-off-by: Tony Najjar * Update footprint iif changed (#4193) Signed-off-by: Brice * fix missing param declare (#4203) Signed-off-by: nelson * Revert "nav2_controller: add loop rate log (#4171)" (#4210) This reverts commit 47374622dee01a27e5f9b8ae08f3d19a15de9b3a. * add polygon_subscribe_transient_local parameter in collision monitor (#4207) Signed-off-by: asarazin Co-authored-by: asarazin * nav2_controller: add loop rate log (#4228) * added current loop rate printout Signed-off-by: ARK3r * remove empty line Signed-off-by: ARK3r --------- Signed-off-by: ARK3r * 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 Co-authored-by: GoesM * 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 * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol * 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 * reformat Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM * 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 1230ede0dd9f168eb38ea63663f250b1bca2fdbf. * 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 * update review comments and description Signed-off-by: nelson * add VelocityPolygon to detector node Signed-off-by: nelson * review update Signed-off-by: nelson * fix cpplint Signed-off-by: nelson * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski Signed-off-by: nelson * add velocity polygon tests Signed-off-by: nelson * fix cpplint Signed-off-by: nelson * add in-line comment Signed-off-by: nelson * fix push back Signed-off-by: nelson * minor change and update README Signed-off-by: nelson * update README Signed-off-by: nelson --------- Signed-off-by: nelson Co-authored-by: Steve Macenski * 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 * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice * fix lint cpp - extra space Signed-off-by: Brice * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski * Fix tests Signed-off-by: Steve Macenski * Update test_a_star.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * 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 Signed-off-by: Brice * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM Co-authored-by: GoesM Signed-off-by: Brice * [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 --------- Signed-off-by: Steve Macenski Signed-off-by: Brice * Adding new Smac paper to readme Signed-off-by: Steve Macenski Signed-off-by: Brice * Update README.md Signed-off-by: Steve Macenski Signed-off-by: Brice * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti * misse change Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti Signed-off-by: Brice * 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 * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski Signed-off-by: Brice * 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 * 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 * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman * Remove stored node and assert Signed-off-by: Ryan Friedman * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman * Add full test coverage with subscriber Signed-off-by: Ryan Friedman * 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 * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman * Fix syntax errors Signed-off-by: Ryan Friedman * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman * Remove unused code Signed-off-by: Ryan Friedman * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman * Use pass-by-reference * Instead of std::move(std::unique_ptr) Signed-off-by: Ryan Friedman * Finish twist subscriber tests Signed-off-by: Ryan Friedman * Add other constructor and docs Signed-off-by: Ryan Friedman * Fix linter issues Signed-off-by: Ryan Friedman * Manually fix paren alignment Signed-off-by: Ryan Friedman * Remove GSoC reference Signed-off-by: Ryan Friedman * Document twist bool param in README Signed-off-by: Ryan Friedman * 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 * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman * More linter fixes Signed-off-by: Ryan Friedman * 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 * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski * 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 --------- Signed-off-by: Ryan Friedman Signed-off-by: Steve Macenski Signed-off-by: Rhys Mainwaring Co-authored-by: pedro-fuoco Co-authored-by: Steve Macenski Co-authored-by: Rhys Mainwaring Signed-off-by: Brice * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol Signed-off-by: Brice * fix include of hpp Signed-off-by: Brice Renaudeau * 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 * Update cost_critic.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Brice Signed-off-by: Steve Macenski Signed-off-by: GoesM Signed-off-by: Davide Faconti Signed-off-by: gg Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman Signed-off-by: Rhys Mainwaring Signed-off-by: cybaol Signed-off-by: Brice Renaudeau Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: Davide Faconti Co-authored-by: Joshua Wallace Co-authored-by: pepisg 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 Co-authored-by: pedro-fuoco Co-authored-by: Rhys Mainwaring Co-authored-by: Kino * 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 * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski * fix merge conflict * Revert "Add velocity based polygon (#3708)" This reverts commit a3ed7456745e38d36966e07182d9e5994143af91. * custom backported version of cost critic inscribed cost --------- Signed-off-by: ARK3r Signed-off-by: Steve Macenski Signed-off-by: GoesM Signed-off-by: Felix Signed-off-by: Antonio Park Signed-off-by: Alexander Mock Signed-off-by: Kyungsik Park Signed-off-by: Tony Najjar Signed-off-by: Brice Signed-off-by: nelson Signed-off-by: asarazin Signed-off-by: gg Signed-off-by: cybaol Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Signed-off-by: Davide Faconti Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman Signed-off-by: Rhys Mainwaring Signed-off-by: Brice Renaudeau Co-authored-by: Reza Kermani Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: bi0ha2ard Co-authored-by: Felix Co-authored-by: Antonio Park Co-authored-by: Alexander Mock Co-authored-by: Tony Najjar Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: nelson Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin Co-authored-by: Joshua Wallace Co-authored-by: pepisg Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino Co-authored-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Co-authored-by: Davide Faconti Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan Co-authored-by: pedro-fuoco Co-authored-by: Rhys Mainwaring Signed-off-by: MarcM0 --- nav2_amcl/src/amcl_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 1a10c7aa417..b6492cb4ef0 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -333,6 +333,7 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) global_loc_srv_.reset(); initial_guess_srv_.reset(); nomotion_update_srv_.reset(); + executor_thread_.reset(); // to make sure initial_pose_sub_ completely exit initial_pose_sub_.reset(); laser_scan_connection_.disconnect(); tf_listener_.reset(); // listener may access lase_scan_filter_, so it should be reset earlier