From 70468502a2e6bad988b7d93d206d78affffa9fd2 Mon Sep 17 00:00:00 2001 From: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com> Date: Wed, 10 Apr 2024 12:00:21 +0200 Subject: [PATCH] Nav2 Sync 3 2024 April 9 (#15) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fix #4001 (#4028) * Humble Sync 9: Jan 23, 2024 (#4064) * Update README.md: fix typo (#3842) * Update package.xml * fix typo (#3850) * adjust link to point to v3.8 of behavior tree docs (#3851) BT.CPP_v3 is used, thereby the correct docs should be linked * Fix bug in nav2_behavior_tree/bt_action_node (#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 * 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 --------- Signed-off-by: CihatAltiparmak * simplication of lattice_generator.py, fixes #3858 (#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 (#3892) Signed-off-by: Anton Kesy * fix use after free (#3910) * fix build mppi (#3927) Signed-off-by: kevin Co-authored-by: kevin * Removing old TODOs * Fixed links for install and build in README (#3963) Currently the readme is linking to an invalida page in the docs (404 error). * fix a few outdated comments in smac planners (#3978) * Update README.md * bump to 1.1.13 for release --------- Signed-off-by: CihatAltiparmak Signed-off-by: Anton Kesy Signed-off-by: kevin Co-authored-by: thandal Co-authored-by: Anton Kesy Co-authored-by: CihatAltiparmak 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 Co-authored-by: Abiel Fernandez Co-authored-by: Michael Ferguson * Fixes for compiling with clang on macOS (#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 * critical fix for path tracking MPPI (#4099) (#4101) (cherry picked from commit 8f7b6ceef943c99497c39da341219a67a4796162) Co-authored-by: Steve Macenski * backport graceful controller to humble (#4119) * Added cast to float to getClosestAngularBin return for behavior consistency (#4123) (#4125) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song * Revised test name Signed-off-by: Hunter Song --------- Signed-off-by: Hunter Song (cherry picked from commit c59e0f362a867244d589562ecff56a9a5f23df59) Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> * Fix goal handle was not freed correctly (#4137) (#4143) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Co-authored-by: Ziohang Co-authored-by: Steve Macenski (cherry picked from commit 2e491b18328ede064c75bfa024f57c7e49578eba) Co-authored-by: iceuw * bug fix (#4160) (#4162) Signed-off-by: Tianchu (cherry picked from commit 623434b09f25afac3872411b8fda786a6f6dce95) Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com> * Add uint suffix (#4213) (#4216) (cherry picked from commit 46ff0ecc4cc7d42cc92477e6159b29d69fd23050) Co-authored-by: Michael Wrock * Fix merge conflict in nav2_mppi_controller README (humble) (#4239) Signed-off-by: Stephanie Eng * Humble sync 10: April 4, 2024 (#4241) * 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 * 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 * completely shutdown inital_pose_sub_ (#4176) 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 * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM Signed-off-by: gg Signed-off-by: cybaol Signed-off-by: Brice Signed-off-by: Steve Macenski 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 Signed-off-by: Antonio Park Signed-off-by: Alexander Mock Signed-off-by: Kyungsik Park Signed-off-by: Tony Najjar Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM 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: BriceRenaudeau <48433002+BriceRenaudeau@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 Co-authored-by: Antonio Park Co-authored-by: Alexander Mock Co-authored-by: Tony Najjar * Fix graceful humble release version Signed-off-by: Steve Macenski * Fix graceful_controller exceptions --------- Signed-off-by: CihatAltiparmak Signed-off-by: Anton Kesy Signed-off-by: kevin Signed-off-by: Rhys Mainwaring Signed-off-by: Stephanie Eng Signed-off-by: GoesM Signed-off-by: gg Signed-off-by: cybaol Signed-off-by: Brice Signed-off-by: Steve Macenski Signed-off-by: Davide Faconti Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman Signed-off-by: Brice Renaudeau Signed-off-by: Antonio Park Signed-off-by: Alexander Mock Signed-off-by: Kyungsik Park Signed-off-by: Tony Najjar Co-authored-by: Steve Macenski Co-authored-by: thandal Co-authored-by: Anton Kesy Co-authored-by: CihatAltiparmak 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 Co-authored-by: Abiel Fernandez Co-authored-by: Michael Ferguson Co-authored-by: Rhys Mainwaring 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 Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com> Co-authored-by: Michael Wrock Co-authored-by: Stephanie Eng Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM 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: BriceRenaudeau <48433002+BriceRenaudeau@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: Antonio Park Co-authored-by: Alexander Mock Co-authored-by: Tony Najjar --- README.md | 6 +- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 11 + nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp | 4 +- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 26 +- nav2_amcl/src/pf/pf.c | 5 - nav2_amcl/src/pf/pf_vector.c | 4 +- .../sensors/laser/likelihood_field_model.cpp | 22 +- nav2_behavior_tree/CMakeLists.txt | 3 + nav2_behavior_tree/README.md | 2 +- .../nav2_behavior_tree/bt_action_node.hpp | 19 +- .../bt_cancel_action_node.hpp | 2 +- .../nav2_behavior_tree/bt_service_node.hpp | 4 +- .../action/progress_checker_selector_node.hpp | 96 ++ nav2_behavior_tree/nav2_tree_nodes.xml | 6 + nav2_behavior_tree/package.xml | 2 +- .../action/progress_checker_selector_node.cpp | 81 ++ .../plugins/condition/is_stuck_condition.cpp | 2 +- .../plugins/decorator/speed_controller.cpp | 2 +- .../test/plugins/action/CMakeLists.txt | 4 + .../plugins/action/test_bt_action_node.cpp | 133 +- .../test_progress_checker_selector_node.cpp | 187 +++ nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- .../navigators/navigate_through_poses.hpp | 2 +- .../navigators/navigate_to_pose.hpp | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- .../src/collision_monitor_node.cpp | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- .../test/test_constrained_smoother.cpp | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/controller_server.cpp | 2 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/CMakeLists.txt | 53 +- .../denoise/image_processing.hpp | 19 +- .../nav2_costmap_2d/inflation_layer.hpp | 2 +- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/denoise_layer.cpp | 2 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 6 +- .../test/integration/CMakeLists.txt | 44 +- nav2_costmap_2d/test/module_tests.cpp | 4 +- .../test/regression/CMakeLists.txt | 7 +- nav2_costmap_2d/test/unit/CMakeLists.txt | 27 +- .../test/unit/binary_filter_test.cpp | 18 +- .../test/unit/denoise_layer_test.cpp | 3 +- .../test/unit/image_processing_test.cpp | 77 +- .../test/unit/keepout_filter_test.cpp | 14 +- .../test/unit/speed_filter_test.cpp | 18 +- .../costmap_queue/CMakeLists.txt | 2 +- nav2_dwb_controller/costmap_queue/package.xml | 2 +- nav2_dwb_controller/dwb_core/package.xml | 2 +- nav2_dwb_controller/dwb_critics/package.xml | 2 +- nav2_dwb_controller/dwb_msgs/package.xml | 2 +- nav2_dwb_controller/dwb_plugins/package.xml | 2 +- .../nav2_dwb_controller/package.xml | 2 +- nav2_dwb_controller/nav_2d_msgs/package.xml | 2 +- nav2_dwb_controller/nav_2d_utils/package.xml | 2 +- .../nav_2d_utils/test/path_ops_test.cpp | 2 +- nav2_graceful_controller/CMakeLists.txt | 80 ++ nav2_graceful_controller/README.md | 42 + nav2_graceful_controller/doc/trajectories.png | Bin 0 -> 38333 bytes .../graceful_controller_plugin.xml | 7 + .../ego_polar_coords.hpp | 91 ++ .../graceful_controller.hpp | 180 +++ .../parameter_handler.hpp | 97 ++ .../nav2_graceful_controller/path_handler.hpp | 83 ++ .../smooth_control_law.hpp | 193 +++ .../nav2_graceful_controller/utils.hpp | 47 + nav2_graceful_controller/package.xml | 35 + .../src/graceful_controller.cpp | 365 +++++ .../src/parameter_handler.cpp | 171 +++ nav2_graceful_controller/src/path_handler.cpp | 126 ++ .../src/smooth_control_law.cpp | 124 ++ nav2_graceful_controller/src/utils.cpp | 51 + nav2_graceful_controller/test/CMakeLists.txt | 21 + .../test/test_egopolar.cpp | 83 ++ .../test/test_graceful_controller.cpp | 1214 +++++++++++++++++ nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- nav2_map_server/test/component/CMakeLists.txt | 3 - .../test/component/test_map_saver_node.cpp | 4 +- .../component/test_map_saver_publisher.cpp | 4 +- .../test/component/test_map_server_node.cpp | 4 +- .../test/map_saver_cli/CMakeLists.txt | 1 - .../test/map_saver_cli/test_map_saver_cli.cpp | 20 +- nav2_map_server/test/unit/CMakeLists.txt | 1 - nav2_map_server/test/unit/test_map_io.cpp | 4 +- nav2_mppi_controller/CMakeLists.txt | 7 +- nav2_mppi_controller/README.md | 47 +- nav2_mppi_controller/critics.xml | 4 + .../critics/cost_critic.hpp | 98 ++ .../nav2_mppi_controller/tools/utils.hpp | 3 +- nav2_mppi_controller/package.xml | 4 +- .../src/critics/cost_critic.cpp | 212 +++ nav2_mppi_controller/test/critics_tests.cpp | 1 + .../test/noise_generator_test.cpp | 2 + .../test/optimizer_smoke_test.cpp | 2 +- .../test/optimizer_unit_tests.cpp | 2 +- nav2_mppi_controller/test/utils/factory.hpp | 34 +- nav2_mppi_controller/test/utils/utils.hpp | 12 +- nav2_mppi_controller/test/utils_test.cpp | 4 +- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/package.xml | 2 +- nav2_msgs/srv/SetInitialPose.srv | 3 + nav2_navfn_planner/package.xml | 2 +- nav2_navfn_planner/src/navfn_planner.cpp | 5 - nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 5 +- .../package.xml | 2 +- nav2_rotation_shim_controller/package.xml | 2 +- .../test/test_shim_controller.cpp | 2 +- nav2_rviz_plugins/package.xml | 2 +- .../nav2_simple_commander/robot_navigator.py | 8 +- nav2_simple_commander/package.xml | 2 +- nav2_smac_planner/README.md | 2 +- .../include/nav2_smac_planner/a_star.hpp | 13 +- .../include/nav2_smac_planner/node_basic.hpp | 1 - .../nav2_smac_planner/node_lattice.hpp | 2 - .../nav2_smac_planner/thirdparty/robin_hood.h | 4 +- .../lattice_primitives/lattice_generator.py | 2 +- nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/collision_checker.cpp | 1 + nav2_smac_planner/src/node_hybrid.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 4 + .../src/smac_planner_lattice.cpp | 4 + nav2_smac_planner/test/test_nodehybrid.cpp | 30 + nav2_smoother/package.xml | 2 +- .../test/test_savitzky_golay_smoother.cpp | 7 +- nav2_smoother/test/test_smoother_server.cpp | 4 +- nav2_system_tests/package.xml | 2 +- .../behaviors/spin/spin_behavior_tester.cpp | 4 +- nav2_system_tests/src/planning/CMakeLists.txt | 2 +- .../src/planning/planner_tester.hpp | 3 + nav2_theta_star_planner/package.xml | 2 +- .../nav2_util/simple_action_server.hpp | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- .../include/nav2_voxel_grid/voxel_grid.hpp | 6 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/CMakeLists.txt | 2 - .../plugins/photo_at_waypoint.hpp | 6 +- nav2_waypoint_follower/package.xml | 2 +- .../plugins/photo_at_waypoint.cpp | 8 +- .../test/test_task_executors.cpp | 4 +- navigation2/package.xml | 2 +- 147 files changed, 4284 insertions(+), 337 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp create mode 100644 nav2_graceful_controller/CMakeLists.txt create mode 100644 nav2_graceful_controller/README.md create mode 100644 nav2_graceful_controller/doc/trajectories.png create mode 100644 nav2_graceful_controller/graceful_controller_plugin.xml create mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp create mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp create mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp create mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/path_handler.hpp create mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp create mode 100644 nav2_graceful_controller/include/nav2_graceful_controller/utils.hpp create mode 100644 nav2_graceful_controller/package.xml create mode 100644 nav2_graceful_controller/src/graceful_controller.cpp create mode 100644 nav2_graceful_controller/src/parameter_handler.cpp create mode 100644 nav2_graceful_controller/src/path_handler.cpp create mode 100644 nav2_graceful_controller/src/smooth_control_law.cpp create mode 100644 nav2_graceful_controller/src/utils.cpp create mode 100644 nav2_graceful_controller/test/CMakeLists.txt create mode 100644 nav2_graceful_controller/test/test_egopolar.cpp create mode 100644 nav2_graceful_controller/test/test_graceful_controller.cpp create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/cost_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/cost_critic.cpp create mode 100644 nav2_msgs/srv/SetInitialPose.srv diff --git a/README.md b/README.md index 488eb28042..fe0364bc3b 100644 --- a/README.md +++ b/README.md @@ -9,8 +9,8 @@ For detailed instructions on how to: - [Getting Started](https://navigation.ros.org/getting_started/index.html) - [Concepts](https://navigation.ros.org/concepts/index.html) -- [Build](https://navigation.ros.org/build_instructions/index.html#build) -- [Install](https://navigation.ros.org/build_instructions/index.html#install) +- [Build](https://navigation.ros.org/development_guides/build_docs/index.html#build) +- [Install](https://navigation.ros.org/development_guides/build_docs/index.html#install) - [General Tutorials](https://navigation.ros.org/tutorials/index.html) and [Algorithm Developer Tutorials](https://navigation.ros.org/plugin_tutorials/index.html) - [Configure](https://navigation.ros.org/configuration/index.html) - [Navigation Plugins](https://navigation.ros.org/plugins/index.html) @@ -20,6 +20,8 @@ For detailed instructions on how to: Please visit our [documentation site](https://navigation.ros.org/). [Please visit our community Slack here](https://join.slack.com/t/navigation2/shared_invite/zt-hu52lnnq-cKYjuhTY~sEMbZXL8p9tOw) (if this link does not work, please contact maintainers to reactivate). +If you need professional services related to Nav2, please contact Open Navigation at info@opennav.org. + ## Our Sponsors Please thank our amazing sponsors for their generous support of Nav2 on behalf of the community to allow the project to continue to be professionally maintained, developed, and supported for the long-haul! [Open Navigation LLC](https://www.opennav.org/) provides project leadership, maintenance, development, and support services to the Nav2 & ROS community. diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 2360d09c25..030a85f38c 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -35,6 +35,7 @@ #include "nav2_amcl/sensors/laser/laser.hpp" #include "nav2_msgs/msg/particle.hpp" #include "nav2_msgs/msg/particle_cloud.hpp" +#include "nav2_msgs/srv/set_initial_pose.hpp" #include "nav_msgs/srv/set_map.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "std_srvs/srv/empty.hpp" @@ -210,6 +211,16 @@ class AmclNode : public nav2_util::LifecycleNode const std::shared_ptr request, std::shared_ptr response); + // service server for providing an initial pose guess + rclcpp::Service::SharedPtr initial_guess_srv_; + /* + * @brief Service callback for an initial pose guess request + */ + void initialPoseReceivedSrv( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response); + // Let amcl update samples without requiring motion rclcpp::Service::SharedPtr nomotion_update_srv_; /* diff --git a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp index c3b406cfd4..713478d24d 100644 --- a/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp +++ b/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp @@ -49,7 +49,7 @@ typedef struct // Return a zero vector -pf_vector_t pf_vector_zero(); +pf_vector_t pf_vector_zero(void); // Check for NAN or INF in any component // int pf_vector_finite(pf_vector_t a); @@ -71,7 +71,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); // Return a zero matrix -pf_matrix_t pf_matrix_zero(); +pf_matrix_t pf_matrix_zero(void); // Check for NAN or INF in any component // int pf_matrix_finite(pf_matrix_t a); diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 5725a6e634..1f5199979a 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.12 + 1.1.14

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 65df56dac1..ba2010d507 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -325,13 +325,17 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Get rid of the inputs first (services and message filter input), so we // don't continue to process incoming messages 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 laser_scan_filter_.reset(); laser_scan_sub_.reset(); // Map + map_sub_.reset(); // map_sub_ may access map_, so it should be reset earlier if (map_ != NULL) { map_free(map_); map_ = nullptr; @@ -341,7 +345,6 @@ AmclNode::on_cleanup(const rclcpp_lifecycle::State & /*state*/) // Transforms tf_broadcaster_.reset(); - tf_listener_.reset(); tf_buffer_.reset(); // PubSub @@ -493,6 +496,15 @@ AmclNode::globalLocalizationCallback( pf_init_ = false; } +void +AmclNode::initialPoseReceivedSrv( + const std::shared_ptr/*request_header*/, + const std::shared_ptr req, + std::shared_ptr/*res*/) +{ + initialPoseReceived(std::make_shared(req->pose)); +} + // force nomotion updates (amcl updating without requiring motion) void AmclNode::nomotionUpdateCallback( @@ -1093,20 +1105,20 @@ AmclNode::initParameters() // Semantic checks if (laser_likelihood_max_dist_ < 0) { RCLCPP_WARN( - get_logger(), "You've set laser_likelihood_max_dist to be negtive," + get_logger(), "You've set laser_likelihood_max_dist to be negative," " this isn't allowed so it will be set to default value 2.0."); laser_likelihood_max_dist_ = 2.0; } if (max_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set max_particles to be negtive," + get_logger(), "You've set max_particles to be negative," " this isn't allowed so it will be set to default value 2000."); max_particles_ = 2000; } if (min_particles_ < 0) { RCLCPP_WARN( - get_logger(), "You've set min_particles to be negtive," + get_logger(), "You've set min_particles to be negative," " this isn't allowed so it will be set to default value 500."); min_particles_ = 500; } @@ -1120,7 +1132,7 @@ AmclNode::initParameters() if (resample_interval_ <= 0) { RCLCPP_WARN( - get_logger(), "You've set resample_interval to be zero or negtive," + get_logger(), "You've set resample_interval to be zero or negative," " this isn't allowed so it will be set to default value to 1."); resample_interval_ = 1; } @@ -1533,6 +1545,10 @@ AmclNode::initServices() "reinitialize_global_localization", std::bind(&AmclNode::globalLocalizationCallback, this, _1, _2, _3)); + initial_guess_srv_ = create_service( + "set_initial_pose", + std::bind(&AmclNode::initialPoseReceivedSrv, this, _1, _2, _3)); + nomotion_update_srv_ = create_service( "request_nomotion_update", std::bind(&AmclNode::nomotionUpdateCallback, this, _1, _2, _3)); diff --git a/nav2_amcl/src/pf/pf.c b/nav2_amcl/src/pf/pf.c index de4e3247d1..9d1f5a8289 100644 --- a/nav2_amcl/src/pf/pf.c +++ b/nav2_amcl/src/pf/pf.c @@ -463,7 +463,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) // Workspace double m[4], c[2][2]; - size_t count; double weight; // Cluster the samples @@ -474,7 +473,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) for (i = 0; i < set->cluster_max_count; i++) { cluster = set->clusters + i; - cluster->count = 0; cluster->weight = 0; cluster->mean = pf_vector_zero(); cluster->cov = pf_matrix_zero(); @@ -490,7 +488,6 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) } // Initialize overall filter stats - count = 0; weight = 0.0; set->mean = pf_vector_zero(); set->cov = pf_matrix_zero(); @@ -521,10 +518,8 @@ void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set) cluster = set->clusters + cidx; - cluster->count += 1; cluster->weight += sample->weight; - count += 1; weight += sample->weight; // Compute mean diff --git a/nav2_amcl/src/pf/pf_vector.c b/nav2_amcl/src/pf/pf_vector.c index a7a5cd39c6..00fa384060 100644 --- a/nav2_amcl/src/pf/pf_vector.c +++ b/nav2_amcl/src/pf/pf_vector.c @@ -35,7 +35,7 @@ // Return a zero vector -pf_vector_t pf_vector_zero() +pf_vector_t pf_vector_zero(void) { pf_vector_t c; @@ -130,7 +130,7 @@ pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b) // Return a zero matrix -pf_matrix_t pf_matrix_zero() +pf_matrix_t pf_matrix_zero(void) { int i, j; pf_matrix_t c; diff --git a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp index 491e64ecf1..1148c2828d 100644 --- a/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp +++ b/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp @@ -53,6 +53,17 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) self = reinterpret_cast(data->laser); + // Pre-compute a couple of things + double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; + double z_rand_mult = 1.0 / data->range_max; + + step = (data->range_count - 1) / (self->max_beams_ - 1); + + // Step size must be at least 1 + if (step < 1) { + step = 1; + } + total_weight = 0.0; // Compute the sample weights @@ -65,17 +76,6 @@ LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set) p = 1.0; - // Pre-compute a couple of things - double z_hit_denom = 2 * self->sigma_hit_ * self->sigma_hit_; - double z_rand_mult = 1.0 / data->range_max; - - step = (data->range_count - 1) / (self->max_beams_ - 1); - - // Step size must be at least 1 - if (step < 1) { - step = 1; - } - for (i = 0; i < data->range_count; i += step) { obs_range = data->ranges[i][0]; obs_bearing = data->ranges[i][1]; diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 944ca7bce4..29d0ecbca7 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -204,6 +204,9 @@ list(APPEND plugin_libs nav2_smoother_selector_bt_node) add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) +add_library(nav2_progress_checker_selector_bt_node SHARED plugins/action/progress_checker_selector_node.cpp) +list(APPEND plugin_libs nav2_progress_checker_selector_bt_node) + add_library(nav2_goal_updated_controller_bt_node SHARED plugins/decorator/goal_updated_controller.cpp) list(APPEND plugin_libs nav2_goal_updated_controller_bt_node) diff --git a/nav2_behavior_tree/README.md b/nav2_behavior_tree/README.md index b2c30c432f..84e16e7d4e 100644 --- a/nav2_behavior_tree/README.md +++ b/nav2_behavior_tree/README.md @@ -63,4 +63,4 @@ The BehaviorTree engine has a run method that accepts an XML description of a BT See the code in the [BT Navigator](../nav2_bt_navigator/src/bt_navigator.cpp) for an example usage of the BehaviorTreeEngine. -For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/learn-the-basics/bt_basics/ +For more information about the behavior tree nodes that are available in the default BehaviorTreeCPP library, see documentation here: https://www.behaviortree.dev/docs/3.8/learn-the-basics/BT_basics diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index d7cd7f0b15..8851ab660e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -163,7 +163,7 @@ class BtActionNode : public BT::ActionNodeBase } /** - * @brief Function to perform some user-defined operation whe the action is aborted. + * @brief Function to perform some user-defined operation when the action is aborted. * @return BT::NodeStatus Returns FAILURE by default, user may override return another value */ virtual BT::NodeStatus on_aborted() @@ -207,7 +207,7 @@ class BtActionNode : public BT::ActionNodeBase // if new goal was sent and action server has not yet responded // check the future goal handle if (future_goal_handle_) { - auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + auto elapsed = (node_->now() - time_goal_sent_).template to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { // return RUNNING if there is still some time before timeout happens if (elapsed < server_timeout_) { @@ -237,7 +237,7 @@ class BtActionNode : public BT::ActionNodeBase { goal_updated_ = false; send_new_goal(); - auto elapsed = (node_->now() - time_goal_sent_).to_chrono(); + auto elapsed = (node_->now() - time_goal_sent_).template to_chrono(); if (!is_future_goal_handle_complete(elapsed)) { if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; @@ -266,7 +266,7 @@ class BtActionNode : public BT::ActionNodeBase // Action related failure that should not fail the tree, but the node return BT::NodeStatus::FAILURE; } else { - // Internal exception to propogate to the tree + // Internal exception to propagate to the tree throw e; } } @@ -300,6 +300,7 @@ class BtActionNode : public BT::ActionNodeBase void halt() override { if (should_cancel_goal()) { + auto future_result = action_client_->async_get_result(goal_handle_); auto future_cancel = action_client_->async_cancel_goal(goal_handle_); if (callback_group_executor_.spin_until_future_complete(future_cancel, server_timeout_) != rclcpp::FutureReturnCode::SUCCESS) @@ -308,6 +309,16 @@ class BtActionNode : public BT::ActionNodeBase node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str()); } + + if (callback_group_executor_.spin_until_future_complete(future_result, server_timeout_) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR( + node_->get_logger(), + "Failed to get result for %s in node halt!", action_name_.c_str()); + } + + on_cancelled(); } setStatus(BT::NodeStatus::IDLE); diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index e87ffbfac4..42d27c94ec 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -116,7 +116,7 @@ class BtCancelActionNode : public BT::ActionNodeBase return basic; } - void halt() + void halt() override { } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index e050ab3038..3537f9e9a2 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -183,7 +183,7 @@ class BtServiceNode : public BT::ActionNodeBase */ virtual BT::NodeStatus check_future() { - auto elapsed = (node_->now() - sent_time_).to_chrono(); + auto elapsed = (node_->now() - sent_time_).template to_chrono(); auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { @@ -199,7 +199,7 @@ class BtServiceNode : public BT::ActionNodeBase if (rc == rclcpp::FutureReturnCode::TIMEOUT) { on_wait_for_result(); - elapsed = (node_->now() - sent_time_).to_chrono(); + elapsed = (node_->now() - sent_time_).template to_chrono(); if (elapsed < server_timeout_) { return BT::NodeStatus::RUNNING; } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp new file mode 100644 index 0000000000..e996bda55b --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp @@ -0,0 +1,96 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The ProgressCheckerSelector behavior is used to switch the progress checker + * of the controller server. It subscribes to a topic "progress_checker_selector" + * to get the decision about what progress_checker must be used. It is usually used before of + * the FollowPath. The selected_progress_checker output port is passed to progress_checker_id + * input port of the FollowPath + */ +class ProgressCheckerSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::ProgressCheckerSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + ProgressCheckerSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_progress_checker", + "the default progress_checker to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "progress_checker_selector", + "the input topic name to select the progress_checker"), + + BT::OutputPort( + "selected_progress_checker", + "Selected progress_checker by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the progress_checker_selector topic + * + * @param msg the message with the id of the progress_checker_selector + */ + void callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr progress_checker_selector_sub_; + + std::string last_selected_progress_checker_; + + rclcpp::Node::SharedPtr node_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__PROGRESS_CHECKER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index a96693112d..360a3bea60 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -181,6 +181,12 @@ Name of the selected goal checker received from the topic subcription + + Name of the topic to receive progress checker selection commands + Default progress checker of the controller selector + Name of the selected progress checker received from the topic subcription + + Spin distance Allowed time for spinning diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index c841446540..b0fe9257fd 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.12 + 1.1.14 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp new file mode 100644 index 0000000000..e8c418ddc1 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/progress_checker_selector_node.cpp @@ -0,0 +1,81 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +ProgressCheckerSelector::ProgressCheckerSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + progress_checker_selector_sub_ = node_->create_subscription( + topic_name_, qos, std::bind(&ProgressCheckerSelector::callbackProgressCheckerSelect, this, _1)); +} + +BT::NodeStatus ProgressCheckerSelector::tick() +{ + rclcpp::spin_some(node_); + + // This behavior always use the last selected progress checker received from the topic input. + // When no input is specified it uses the default goaprogressl checker. + // If the default progress checker is not specified then we work in + // "required progress checker mode": In this mode, the behavior returns failure if the progress + // checker selection is not received from the topic input. + if (last_selected_progress_checker_.empty()) { + std::string default_progress_checker; + getInput("default_progress_checker", default_progress_checker); + if (default_progress_checker.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_progress_checker_ = default_progress_checker; + } + } + + setOutput("selected_progress_checker", last_selected_progress_checker_); + + return BT::NodeStatus::SUCCESS; +} + +void +ProgressCheckerSelector::callbackProgressCheckerSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_progress_checker_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("ProgressCheckerSelector"); +} diff --git a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp index 211254dafe..a898dec947 100644 --- a/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp +++ b/nav2_behavior_tree/plugins/condition/is_stuck_condition.cpp @@ -95,7 +95,7 @@ void IsStuckCondition::logStuck(const std::string & msg) const return; } - RCLCPP_INFO(node_->get_logger(), msg.c_str()); + RCLCPP_INFO(node_->get_logger(), "%s", msg.c_str()); prev_msg = msg; } diff --git a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp index e5c96eba2c..1ab530dab1 100644 --- a/nav2_behavior_tree/plugins/decorator/speed_controller.cpp +++ b/nav2_behavior_tree/plugins/decorator/speed_controller.cpp @@ -43,7 +43,7 @@ SpeedController::SpeedController( if (min_rate_ <= 0.0 || max_rate_ <= 0.0) { std::string err_msg = "SpeedController node cannot have rate <= 0.0"; - RCLCPP_FATAL(node_->get_logger(), err_msg.c_str()); + RCLCPP_FATAL(node_->get_logger(), "%s" ,err_msg.c_str()); throw BT::BehaviorTreeException(err_msg); } diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index c640e82424..4aaf185306 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -107,3 +107,7 @@ ament_target_dependencies(test_smoother_selector_node ${dependencies}) ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp) target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node) ament_target_dependencies(test_goal_checker_selector_node ${dependencies}) + +ament_add_gtest(test_progress_checker_selector_node test_progress_checker_selector_node.cpp) +target_link_libraries(test_progress_checker_selector_node nav2_progress_checker_selector_bt_node) +ament_target_dependencies(test_progress_checker_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp index d95cbd4a84..12c25d30d6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_bt_action_node.cpp @@ -54,11 +54,17 @@ class FibonacciActionServer : public rclcpp::Node sleep_duration_ = sleep_duration; } + void setServerLoopRate(std::chrono::nanoseconds server_loop_rate) + { + server_loop_rate_ = server_loop_rate; + } + protected: rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID &, std::shared_ptr) { + RCLCPP_INFO(this->get_logger(), "Goal is received.."); if (sleep_duration_ > 0ms) { std::this_thread::sleep_for(sleep_duration_); } @@ -73,6 +79,13 @@ class FibonacciActionServer : public rclcpp::Node void handle_accepted( const std::shared_ptr> handle) + { + // this needs to return quickly to avoid blocking the executor, so spin up a new thread + std::thread{std::bind(&FibonacciActionServer::execute, this, _1), handle}.detach(); + } + + void execute( + const std::shared_ptr> handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (handle) { @@ -88,8 +101,17 @@ class FibonacciActionServer : public rclcpp::Node sequence.push_back(0); sequence.push_back(1); + rclcpp::Rate rate(server_loop_rate_); for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) { + if (handle->is_canceling()) { + RCLCPP_INFO(this->get_logger(), "Goal is canceling."); + handle->canceled(result); + return; + } + + RCLCPP_INFO(this->get_logger(), "Goal is feedbacking."); sequence.push_back(sequence[i] + sequence[i - 1]); + rate.sleep(); } handle->succeed(result); @@ -99,6 +121,7 @@ class FibonacciActionServer : public rclcpp::Node protected: rclcpp_action::Server::SharedPtr action_server_; std::chrono::milliseconds sleep_duration_; + std::chrono::nanoseconds server_loop_rate_; }; class FibonacciAction : public nav2_behavior_tree::BtActionNode @@ -121,6 +144,13 @@ class FibonacciAction : public nav2_behavior_tree::BtActionNodeset>("sequence", result_.result->sequence); + config().blackboard->set("on_cancelled_triggered", true); + return BT::NodeStatus::SUCCESS; + } + static BT::PortsList providedPorts() { return providedBasicPorts({BT::InputPort("order", "Fibonacci order")}); @@ -144,6 +174,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); config_->blackboard->set("initial_pose_received", false); + config_->blackboard->set("on_cancelled_triggered", false); BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) @@ -220,6 +251,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -255,15 +287,22 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle - // halt BT for a new execution cycle + // halt BT for a new execution cycle, + // get if the on_cancelled is triggered from blackboard and assert + // that the on_cancelled triggers after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a large action server goal handling duration action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -300,6 +339,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the action server will take 100ms before accepting the goal action_server_->setHandleGoalSleepDuration(100ms); + action_server_->setServerLoopRate(10ns); // to keep track of the number of ticks it took to reach a terminal result int ticks = 0; @@ -327,14 +367,21 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) // the new cycle // halt BT for a new execution cycle + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancelled never can trigger after halting node + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); tree_->haltTree(); + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, false); // setting a small action server goal handling duration action_server_->setHandleGoalSleepDuration(25ms); + action_server_->setServerLoopRate(10ns); // reset state variables ticks = 0; result = BT::NodeStatus::RUNNING; + config_->blackboard->set("on_cancelled_triggered", false); // main BT execution loop while (rclcpp::ok() && result == BT::NodeStatus::RUNNING) { @@ -348,6 +395,90 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::SUCCESS); } +TEST_F(BTActionNodeTestFixture, test_server_cancel) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + // setting a server timeout smaller than the time the action server will take to accept the goal + // to simulate a server timeout scenario + config_->blackboard->set("server_timeout", 100ms); + config_->blackboard->set("bt_loop_duration", 10ms); + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 50ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // to keep track of the number of ticks it took to reach expected tick count + int ticks = 0; + + BT::NodeStatus result = BT::NodeStatus::RUNNING; + + // BT loop execution rate + rclcpp::WallRate loopRate(100ms); + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 5) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + bool on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 5 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 5); + + // send new goal to the action server for a new execution cycle + + // the action server will take 2ms before accepting the goal + // and the feedback period of the action server will be 1000ms + action_server_->setHandleGoalSleepDuration(2ms); + action_server_->setServerLoopRate(50ms); + + // reset state variable + ticks = 0; + config_->blackboard->set("on_cancelled_triggered", false); + result = BT::NodeStatus::RUNNING; + + // main BT execution loop + while (rclcpp::ok() && result == BT::NodeStatus::RUNNING && ticks < 7) { + result = tree_->tickRoot(); + ticks++; + loopRate.sleep(); + } + + // halt BT for testing if the action node cancels the goal correctly + RCLCPP_INFO(node_->get_logger(), "Tree is halting."); + tree_->haltTree(); + + // get if the on_cancel is triggered from blackboard and assert + // that the on_cancel is triggered after halting node + on_cancelled_triggered = config_->blackboard->get("on_cancelled_triggered"); + EXPECT_EQ(on_cancelled_triggered, true); + + // ticks variable must be 7 because execution time of the action server + // is at least 1000000 x 50 ms + EXPECT_EQ(ticks, 7); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp new file mode 100644 index 0000000000..08d589ac03 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_progress_checker_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2024 Open Navigation LLC +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/progress_checker_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class ProgressCheckerSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("progress_checker_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "ProgressCheckerSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr ProgressCheckerSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * ProgressCheckerSelectorTestFixture::config_ = nullptr; +std::shared_ptr ProgressCheckerSelectorTestFixture::factory_ = nullptr; +std::shared_ptr ProgressCheckerSelectorTestFixture::tree_ = nullptr; + +TEST_F(ProgressCheckerSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_progress_checker_result; + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + + EXPECT_EQ(selected_progress_checker_result, "SimpleProgressCheck"); + + std_msgs::msg::String selected_progress_checker_cmd; + + selected_progress_checker_cmd.data = "AngularProgressChecker"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto progress_checker_selector_pub = + node_->create_publisher( + "progress_checker_selector_custom_topic_name", qos); + + // publish a few updates of the selected_progress_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + progress_checker_selector_pub->publish(selected_progress_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check progress_checker updated + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + EXPECT_EQ("AngularProgressChecker", selected_progress_checker_result); +} + +TEST_F(ProgressCheckerSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_progress_checker_result; + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + + EXPECT_EQ(selected_progress_checker_result, "GridBased"); + + std_msgs::msg::String selected_progress_checker_cmd; + + selected_progress_checker_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto progress_checker_selector_pub = + node_->create_publisher("progress_checker_selector", qos); + + // publish a few updates of the selected_progress_checker + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + progress_checker_selector_pub->publish(selected_progress_checker_cmd); + + rclcpp::spin_some(node_); + } + + // check goal_checker updated + config_->blackboard->get("selected_progress_checker", selected_progress_checker_result); + EXPECT_EQ("RRT", selected_progress_checker_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 898573b9a3..0d6ca68ffc 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.12 + 1.1.14 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 559608e61f..0340705620 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.12 + 1.1.14 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp index 2605fc8649..615ce5ef64 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp @@ -32,7 +32,7 @@ namespace nav2_bt_navigator { /** - * @class NavigateToPoseNavigator + * @class NavigateThroughPosesNavigator * @brief A navigator for navigating to a a bunch of intermediary poses */ class NavigateThroughPosesNavigator diff --git a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp index 98287b5be8..32dc1cd144 100644 --- a/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp +++ b/nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp @@ -72,7 +72,7 @@ class NavigateToPoseNavigator * @brief Get action name for this navigator * @return string Name of action server */ - std::string getName() {return std::string("navigate_to_pose");} + std::string getName() override {return std::string("navigate_to_pose");} /** * @brief Get navigator's default BT diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 2540ec06a7..f8209fcfc2 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.12 + 1.1.14 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 0672683c27..9ffc92422e 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.12 + 1.1.14 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index a6e3f86806..8e9914a400 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -389,7 +389,7 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) printAction(robot_action, action_polygon); } - // Publish requred robot velocity + // Publish required robot velocity publishVelocity(robot_action); // Publish polygons for better visualization diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 42b183c6e9..631a50e658 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.12 + 1.1.14 Common support functionality used throughout the navigation 2 stack Carl Delsey Apache-2.0 diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml index b1879c5743..1e456a608c 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.12 + 1.1.14 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_constrained_smoother/test/test_constrained_smoother.cpp b/nav2_constrained_smoother/test/test_constrained_smoother.cpp index 463af33fd0..1ccae77f77 100644 --- a/nav2_constrained_smoother/test/test_constrained_smoother.cpp +++ b/nav2_constrained_smoother/test/test_constrained_smoother.cpp @@ -100,7 +100,7 @@ class SmootherTest : public ::testing::Test SmootherTest() {SetUp();} ~SmootherTest() {} - void SetUp() + void SetUp() override { node_lifecycle_ = std::make_shared( diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 8c1208f9cc..efd36e841f 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.12 + 1.1.14 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index a0bd0cec14..6c96915c9b 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -534,7 +534,7 @@ void ControllerServer::computeAndPublishVelocity() // other types will not be resolved with more attempts } catch (nav2_core::NoValidControl & e) { if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) { - RCLCPP_WARN(this->get_logger(), e.what()); + RCLCPP_WARN(this->get_logger(), "%s", e.what()); cmd_vel_2d.twist.angular.x = 0; cmd_vel_2d.twist.angular.y = 0; cmd_vel_2d.twist.angular.z = 0; diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 8f09d1b42e..d1d580e439 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.12 + 1.1.14 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42..3be30b2853 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -26,17 +26,10 @@ find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) remove_definitions(-DDISABLE_LIBUSB-1.0) -find_package(Eigen3 REQUIRED) +find_package(Eigen3 3.3 REQUIRED) nav2_package() -include_directories( - include - ${EIGEN3_INCLUDE_DIRS} -) - -add_definitions(${EIGEN3_DEFINITIONS}) - add_library(nav2_costmap_2d_core SHARED src/array_parser.cpp src/costmap_2d.cpp @@ -52,6 +45,13 @@ add_library(nav2_costmap_2d_core SHARED src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_core ALIAS nav2_costmap_2d_core) + +target_include_directories(nav2_costmap_2d_core + PUBLIC + "$" + "$" +) set(dependencies geometry_msgs @@ -79,6 +79,7 @@ set(dependencies ament_target_dependencies(nav2_costmap_2d_core ${dependencies} ) +target_link_libraries(nav2_costmap_2d_core Eigen3::Eigen) add_library(layers SHARED plugins/inflation_layer.cpp @@ -89,11 +90,12 @@ add_library(layers SHARED plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp ) +add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers ${dependencies} ) target_link_libraries(layers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(filters SHARED @@ -101,11 +103,14 @@ add_library(filters SHARED plugins/costmap_filters/speed_filter.cpp plugins/costmap_filters/binary_filter.cpp ) +add_library(${PROJECT_NAME}::filters ALIAS filters) + + ament_target_dependencies(filters ${dependencies} ) target_link_libraries(filters - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_library(nav2_costmap_2d_client SHARED @@ -113,18 +118,19 @@ add_library(nav2_costmap_2d_client SHARED src/costmap_subscriber.cpp src/costmap_topic_collision_checker.cpp ) +add_library(${PROJECT_NAME}::nav2_costmap_2d_client ALIAS nav2_costmap_2d_client) ament_target_dependencies(nav2_costmap_2d_client ${dependencies} ) target_link_libraries(nav2_costmap_2d_client - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d_markers src/costmap_2d_markers.cpp) target_link_libraries(nav2_costmap_2d_markers - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) ament_target_dependencies(nav2_costmap_2d_markers @@ -133,7 +139,7 @@ ament_target_dependencies(nav2_costmap_2d_markers add_executable(nav2_costmap_2d_cloud src/costmap_2d_cloud.cpp) target_link_libraries(nav2_costmap_2d_cloud - nav2_costmap_2d_core + ${PROJECT_NAME}::nav2_costmap_2d_core ) add_executable(nav2_costmap_2d src/costmap_2d_node.cpp) @@ -142,26 +148,24 @@ ament_target_dependencies(nav2_costmap_2d ) target_link_libraries(nav2_costmap_2d - nav2_costmap_2d_core - layers - filters + ${PROJECT_NAME}::nav2_costmap_2d_core + ${PROJECT_NAME}::layers + ${PROJECT_NAME}::filters ) install(TARGETS - nav2_costmap_2d_core layers filters + nav2_costmap_2d_core nav2_costmap_2d_client + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) -install(TARGETS - nav2_costmap_2d - nav2_costmap_2d_markers - nav2_costmap_2d_cloud - RUNTIME DESTINATION lib/${PROJECT_NAME} +install(TARGETS nav2_costmap_2d_markers nav2_costmap_2d_cloud nav2_costmap_2d + DESTINATION lib/${PROJECT_NAME} ) install(FILES costmap_plugins.xml @@ -169,7 +173,7 @@ install(FILES costmap_plugins.xml ) install(DIRECTORY include/ - DESTINATION include/ + DESTINATION include/${PROJECT_NAME} ) if(BUILD_TESTING) @@ -184,8 +188,7 @@ if(BUILD_TESTING) pluginlib_export_plugin_description_file(nav2_costmap_2d test/regression/order_layer.xml) endif() -ament_export_include_directories(include) -ament_export_libraries(layers filters nav2_costmap_2d_core nav2_costmap_2d_client) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(${dependencies}) pluginlib_export_plugin_description_file(nav2_costmap_2d costmap_plugins.xml) ament_package() diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index ee79894847..75c720cbaa 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -986,9 +986,9 @@ class GroupsRemover const IsBg & is_background) const { // Creates an image labels in which each obstacles group is labeled with a unique code - auto components = connectedComponents(image, buffer, label_trees, is_background); - const Label groups_count = components.second; - const Image