From 0254af69ab5e78f46afcc53bf12c9d2a7987359d Mon Sep 17 00:00:00 2001 From: Balint Rozgonyi <43723477+RBT22@users.noreply.github.com> Date: Fri, 27 Sep 2024 09:01:49 +0200 Subject: [PATCH] Nav2 Sync 4 2024 Sept 26 (#24) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * replace throw-error with error-log to avoid UAF mentioned in #4175 (#4180) (#4305) * replace throw-error with error-log to avoid UAF * fix typo --------- Signed-off-by: GoesM Co-authored-by: GoesM * Cherry-pick from 15c9be0aa5d09dd3be40bb46a66a5175464dd1a4 (#4317) Convert all wall timers and wall rates to ROS clock respecting rates and timers (#4000) * Convert all wall timers and wall rates to ROS clock respecting rates and timers * linty mclint face * WPF wait plugin respect time * move duration metrics to use local clocks * bumping version for cache to break it * complete timing refactor * remove old variable * Add dynamic parameter (#4319) To fix https://github.com/open-navigation/navigation2/issues/4315 Signed-off-by: Huy Nguyen Van <34271857+Huyhust13@users.noreply.github.com> * Humble release 11: May 23, 2024 (#4365) * Scale cost critic's weight when dynamically updated (#4246) * Scale cost critic's weight when dynamically updated Signed-off-by: pepisg * sign off Signed-off-by: pepisg --------- Signed-off-by: pepisg * Add expanding the ~/ to the full home dir of user in the path to the map yaml. (#4258) * Add user home expander of home sequence Signed-off-by: Wiktor Bajor * Add passing home dir as string instead of const char* Signed-off-by: Wiktor Bajor * Add docs Signed-off-by: Wiktor Bajor * Fix function declaration Signed-off-by: Wiktor Bajor * Fix linter issues Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter Signed-off-by: Wiktor Bajor * Uncrustify linter: remove remove whitespace Signed-off-by: Wiktor Bajor --------- Signed-off-by: Wiktor Bajor * Implement Critic for Velocity Deadband Hardware Constraints (#4256) * Adding new velocity deadband critic. - add some tests - cast double to float - add new features from "main" branch - fix formating - add cost test - fix linting issue - add README Signed-off-by: Denis Sokolov * Remove velocity deadband critic from defaults Signed-off-by: Denis Sokolov * remove old weight Signed-off-by: Denis Sokolov * fix velocity deadband critic tests Signed-off-by: Denis Sokolov --------- Signed-off-by: Denis Sokolov * removing clearable layer param (unused) (#4280) * provide message validation check API (#4276) * provide validation_message.hpp Signed-off-by: goes * fix typo Signed-off-by: goes * add test_validation_messages.cpp Signed-off-by: goes * change include-order Signed-off-by: goes * reformat Signed-off-by: goes * update test Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * Add footprint clearing for static layer (#4282) * Add footprint clearing for static layer Signed-off-by: Tony Najjar * fix flckering --------- Signed-off-by: Tony Najjar * Fix #4268 (#4296) Signed-off-by: Steve Macenski * Update README.md of nav2_bt_navigator (#4309) Update link to docs Signed-off-by: João Britto * Fix undefined symbols in `libpf_lib.so` (#4312) When I build `nav2_amcl` with `-Wl,--no-undefined` I noticed `libpf_lib.so` has undefined symbols. This PR correctly links `libpf_lib.so` to `libm` so all symbols can be found. You can verify this by executing the following command: ``` ldd -r ./build/nav2_amcl/src/pf/libpf_lib.so linux-vdso.so.1 (0x00007ffd1f8c0000) libc.so.6 => /lib/x86_64-linux-gnu/libc.so.6 (0x000074e909a00000) /lib64/ld-linux-x86-64.so.2 (0x000074e909e60000) undefined symbol: ceil (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: atan2 (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sin (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: hypot (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: cos (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: log (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: sqrt (./build/nav2_amcl/src/pf/libpf_lib.so) undefined symbol: floor (./build/nav2_amcl/src/pf/libpf_lib.so) ``` Signed-off-by: Ramon Wijnands * msg validation check for `/initialpose` in `nav2_amcl` (#4301) * add validation check for PoseWithCovarianceStamped Signed-off-by: goes * remove rebundant check before Signed-off-by: goes * reformat Signed-off-by: goes * typo fixed Signed-off-by: goes * change the type-name Signed-off-by: goes * update test Signed-off-by: goes * reformat Signed-off-by: goes * . Signed-off-by: goes * add comment Signed-off-by: goes * update comment Signed-off-by: goes * change header Signed-off-by: goes * update test Signed-off-by: goes * typo fixed Signed-off-by: goes --------- Signed-off-by: goes Co-authored-by: goes * 4320: Changed precision of calculations of the HybridNode MotionTable::getClosestAngularBin. (#4324) Signed-off-by: Krzysztof Pawełczyk Co-authored-by: Krzysztof Pawełczyk * [LifecycleNode] add bond_heartbeat_period (#4342) * add bond_heartbeat_period Signed-off-by: Guillaume Doisy * lint Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Update path_longer_on_approach.cpp (#4344) Fix the bug that isPathUpdated function will return false for the reason that it compare the timestaped between new path and old path's last pose Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> * [LifecycleManagerClient] clean set_initial_pose and navigate_to_pose (#4346) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy * Move projectState after getPointsInside (#4356) * Modify test to check fix Signed-off-by: Brice * Add static polygon check before simulation Signed-off-by: Brice --------- Signed-off-by: Brice * adding final pose in analytic expansion to check (#4353) * fix sync merge conflicts * bump humble to 1.1.15 for release * Revert "[LifecycleNode] add bond_heartbeat_period (#4342)" This reverts commit 6e44178bd44a74cc4c893040fd49ac303a93b8d9. --------- Signed-off-by: pepisg Signed-off-by: Wiktor Bajor Signed-off-by: Denis Sokolov Signed-off-by: goes Signed-off-by: Tony Najjar Signed-off-by: Steve Macenski Signed-off-by: João Britto Signed-off-by: Ramon Wijnands Signed-off-by: Krzysztof Pawełczyk Signed-off-by: Guillaume Doisy Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> Signed-off-by: Brice Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes Co-authored-by: Tony Najjar Co-authored-by: João Britto Co-authored-by: Ramon Wijnands Co-authored-by: AzaelCicero Co-authored-by: Krzysztof Pawełczyk Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> * adding mutex lock around map resizing due to dynamic parameter changes and associated processes (#4373) (#4378) (cherry picked from commit b0abc78ace48d21ca5b8b58afb0482d759733cb0) Co-authored-by: Steve Macenski * Make BT nodes have configurable wait times (Backport #3960 #4178 #4203) (#4409) * WIP: Make BT nodes have configurable wait times. (#3960) * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [#3920] Signed-off-by: Adam Galecki * Make BT nodes have configurable wait times. Previous solution provided hardcoded 1s value. Right now the value can be configured for BT Action, Cancel and Service nodes. [#3920] Signed-off-by: Adam Galecki * Fix typos, linting errors and value type from float to int * Fix extra underscores * Fix extra underscore * Update unit tests with blackboard parameter Signed-off-by: Adam Galecki * Fix formatting errors Signed-off-by: Adam Galecki * Update system tests to match new parameter Signed-off-by: Adam Galecki --------- Signed-off-by: Adam Galecki Signed-off-by: Christoph Froehlich * chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix Co-authored-by: Felix Signed-off-by: Christoph Froehlich * fix missing param declare (#4203) Signed-off-by: nelson Signed-off-by: Christoph Froehlich * Fix error messages (#4411) Signed-off-by: Christoph Froehlich --------- Signed-off-by: Adam Galecki Signed-off-by: Christoph Froehlich Signed-off-by: Felix Signed-off-by: nelson Co-authored-by: Adam Gałecki <73710544+embeddedadam@users.noreply.github.com> Co-authored-by: bi0ha2ard Co-authored-by: Felix Co-authored-by: nelson * Enable reloading BT xml file with same name (#4209) (#4422) * Let BtActionServer overwrite xml * Make a ROS parameter for it * Rename flag to always reload BT xml file --------- Signed-off-by: Johannes Huemer Signed-off-by: Christoph Froehlich Co-authored-by: Johannes Huemer * fix bug mentioned in #3958 (#3972) (#4463) * bug fixed * add space * Update planner_server.cpp * add space for code style * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add ChildLifecycleNode mode in costmap_2d_ros * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower * fit to NodeOption: is_lifecycle_follower * fit reorder Werror * fix wrong use of is_lifecycle_follower * remove blank line * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * Add files via upload * NodeOption: is_lifecycle_follower_ * NodeOption:is_lifecycle_follower_ * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * change default * add NodeOption for costmap_2d_ros * add node options for costmap2dros as an independent node * code style reformat * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * changes * comment changes * change get_parameter into =false * comment modification * missing line * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * delete last line * change lifecycle_test fit to NodeOption --------- Co-authored-by: GoesM Co-authored-by: Steve Macenski * bt_service_node and bt_action_node: Don't block BT loop (backport #4214) (#4408) (#4475) * bt_service_node and bt_action_node: Don't block BT loop (#4214) * Set smaller timeout for service node * Fix timeout calculation for service node * Add a feasible timeout also for action node --------- * Increasing test count from timeout handling changes (#4234) --------- Signed-off-by: Christoph Froehlich Signed-off-by: Steve Macenski Co-authored-by: Steve Macenski * Behavior tree node for extracting pose from path (#4518) (#4525) * add get pose from path action Signed-off-by: MarcM0 * cleanup from PR suggestions Signed-off-by: MarcM0 * Updates for main compatibility Signed-off-by: MarcM0 * Lint and build fix Signed-off-by: MarcM0 * More Lint and warnings Signed-off-by: MarcM0 * More Lint and build Signed-off-by: MarcM0 * remove code left over from older file Signed-off-by: MarcM0 * fix test blackboard var name Signed-off-by: MarcM0 * only populate pose frame if empty Signed-off-by: MarcM0 * lint Signed-off-by: MarcM0 --------- Signed-off-by: MarcM0 (cherry picked from commit 12a9c1d805847709e3b82f8dcfbb43c67b5b2937) Co-authored-by: Marc Morcos <30278842+MarcM0@users.noreply.github.com> * Make #4525 compile on humble (#4526) * Make it compile on humble Signed-off-by: Christoph Froehlich * Remove formatting Signed-off-by: Christoph Froehlich --------- Signed-off-by: Christoph Froehlich * Fix backward motion for graceful controller (#4527) (#4566) * Fix backward motion for graceful controller Signed-off-by: Alberto Tudela * Update smooth_control_law.cpp Signed-off-by: Alberto Tudela --------- Signed-off-by: Alberto Tudela (cherry picked from commit d1ad6401fa8939a8eee6c6d15b31cd656995ecb5) Co-authored-by: Alberto Tudela * nav2_collision_monitor dynamic parameters polygon and source enabled for Humble (#4615) * Copy modification from c2d84dfe671077bd76781ea356141b6d7ebb86f4 into humble collision_monitor for dynamic parameter enabled in polygon * Add the enabled dynamic parameter for source. Signed-off-by: Enzo Ghisoni * Start backport action_state_ declaration in collision_monitor_node_test.cpp Signed-off-by: EnzoGhisoni --------- Signed-off-by: EnzoGhisoni Co-authored-by: EnzoGhisoni * Humble release 12: August 23 (#4644) * Add configure and cleanup transitions to lifecycle manager and client (#4371) Signed-off-by: Joni Pöllänen * [RotationShimController] Rotate to goal heading (#4332) When arriving in the goal xy tolerance, the rotation shim controller takes back the control to command the robot to rotate in the goal heading orientation. The initial goal of the rotationShimController was to rotate the robot at the beginning of a navigation towards the paths orientation because some controllers are not good at performing in place rotations. For the same reason, the rotationShimController should be able to rotate the robot towards the goal heading. Signed-off-by: Antoine Gennart * [RotationShimController] Fix test for rotate to goal heading (#4289) (#4391) * Fix rotate to goal heading tests Signed-off-by: Antoine Gennart * reset laser_scan_filter before reinit (#4397) Signed-off-by: goes Co-authored-by: goes * Warn if inflation_radius_ < inscribed_radius_ (#4423) * Warn if inflation_radius_ < inscribed_radius_ Signed-off-by: Tony Najjar * convert to error Signed-off-by: Tony Najjar --------- Signed-off-by: Tony Najjar * chore: cleanup ros1 leftovers (#4446) Signed-off-by: Rein Appeldoorn * precomputeDistanceHeuristic is now computed once (#4451) Signed-off-by: Vincent Belpois Co-authored-by: SiddharthaUpase * shutdown services in destructor of `ClearCostmapService` (#4495) Signed-off-by: GoesM_server Co-authored-by: GoesM_server * fix(nav2_costmap_2d): make obstacle layer not current on enabled toggle (#4507) Signed-off-by: Kemal Bektas Co-authored-by: Kemal Bektas * min_turning_r_ getting param fix (#4510) * min_turning_r_ getting param fix Signed-off-by: Ivan Radionov * Update nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp Signed-off-by: Steve Macenski Signed-off-by: Ivan Radionov --------- Signed-off-by: Ivan Radionov Signed-off-by: Steve Macenski Co-authored-by: Ivan Radionov Co-authored-by: Steve Macenski * Return out of map update if frames mismatch. Signed-off-by Joey Yang (#4517) Signed-off-by: Joey Yang * check nullptr in smoothPlan() (#4544) * check nullptr in smoothPlan() Signed-off-by: GoesM * code-style Signed-off-by: GoesM * code-style Signed-off-by: GoesM * simple change Signed-off-by: GoesM --------- Signed-off-by: GoesM Co-authored-by: GoesM * bump to 1.1.15 Signed-off-by: Steve Macenski * Revert "Add configure and cleanup transitions to lifecycle manager and client (#4371)" This reverts commit 06ec9585ec5850e4bcaecdab007eca81bbbae4e9. * fix merge conflict with humble sync * fix merge conflict with humble sync --------- Signed-off-by: Joni Pöllänen Signed-off-by: Antoine Gennart Signed-off-by: Antoine Gennart Signed-off-by: goes Signed-off-by: Tony Najjar Signed-off-by: Rein Appeldoorn Signed-off-by: Vincent Belpois Signed-off-by: GoesM_server Signed-off-by: Kemal Bektas Signed-off-by: Ivan Radionov Signed-off-by: Steve Macenski Signed-off-by: Joey Yang Signed-off-by: GoesM Co-authored-by: Joni Pöllänen Co-authored-by: Saitama Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: goes Co-authored-by: Tony Najjar Co-authored-by: Rein Appeldoorn Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com> Co-authored-by: SiddharthaUpase Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Co-authored-by: Kemal Bektas Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com> Co-authored-by: Ivan Radionov Co-authored-by: Joey Yang * Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes (#4664) (#4668) * Fixing #4661: MPPI ackermann reversing taking incorrect sign sometimes Signed-off-by: Steve Macenski * fixing unit test for type implicit cast Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski (cherry picked from commit 7eb47d84012dd9e25988ce5c9c269280e6ee3dd1) Co-authored-by: Steve Macenski --------- Signed-off-by: GoesM Signed-off-by: Huy Nguyen Van <34271857+Huyhust13@users.noreply.github.com> Signed-off-by: pepisg Signed-off-by: Wiktor Bajor Signed-off-by: Denis Sokolov Signed-off-by: goes Signed-off-by: Tony Najjar Signed-off-by: Steve Macenski Signed-off-by: João Britto Signed-off-by: Ramon Wijnands Signed-off-by: Krzysztof Pawełczyk Signed-off-by: Guillaume Doisy Signed-off-by: StetroF <120172218+StetroF@users.noreply.github.com> Signed-off-by: Brice Signed-off-by: Adam Galecki Signed-off-by: Christoph Froehlich Signed-off-by: Felix Signed-off-by: nelson Signed-off-by: Johannes Huemer Signed-off-by: EnzoGhisoni Signed-off-by: Joni Pöllänen Signed-off-by: Antoine Gennart Signed-off-by: Antoine Gennart Signed-off-by: Rein Appeldoorn Signed-off-by: Vincent Belpois Signed-off-by: GoesM_server Signed-off-by: Kemal Bektas Signed-off-by: Ivan Radionov Signed-off-by: Joey Yang Signed-off-by: GoesM Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM Co-authored-by: Benjamin-Tan Co-authored-by: Huy Nguyen Van <34271857+Huyhust13@users.noreply.github.com> Co-authored-by: Steve Macenski Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Co-authored-by: Sokolov Denis <52282102+perchess@users.noreply.github.com> Co-authored-by: Tony Najjar Co-authored-by: João Britto Co-authored-by: Ramon Wijnands Co-authored-by: AzaelCicero Co-authored-by: Krzysztof Pawełczyk Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: StetroF <120172218+StetroF@users.noreply.github.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> Co-authored-by: Christoph Fröhlich Co-authored-by: Adam Gałecki <73710544+embeddedadam@users.noreply.github.com> Co-authored-by: bi0ha2ard Co-authored-by: Felix Co-authored-by: nelson Co-authored-by: Johannes Huemer Co-authored-by: Marc Morcos <30278842+MarcM0@users.noreply.github.com> Co-authored-by: Alberto Tudela Co-authored-by: Enzo Ghisoni <33607172+EnzoGhisoni@users.noreply.github.com> Co-authored-by: EnzoGhisoni Co-authored-by: Joni Pöllänen Co-authored-by: Saitama Co-authored-by: Rein Appeldoorn Co-authored-by: Vincent <46542431+VincidaB@users.noreply.github.com> Co-authored-by: SiddharthaUpase Co-authored-by: Kemal Bektas <34746077+bektaskemal@users.noreply.github.com> Co-authored-by: Kemal Bektas Co-authored-by: Ivan Radionov <45877502+JJRedmond@users.noreply.github.com> Co-authored-by: Ivan Radionov Co-authored-by: Joey Yang --- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 13 +- nav2_amcl/src/pf/CMakeLists.txt | 1 + nav2_behavior_tree/CMakeLists.txt | 3 + .../behavior_tree_engine.hpp | 4 +- .../nav2_behavior_tree/bt_action_node.hpp | 21 +- .../nav2_behavior_tree/bt_action_server.hpp | 6 + .../bt_action_server_impl.hpp | 22 +- .../bt_cancel_action_node.hpp | 10 +- .../nav2_behavior_tree/bt_service_node.hpp | 20 +- .../action/get_pose_from_path_action.hpp | 55 +++ nav2_behavior_tree/package.xml | 2 +- .../action/get_pose_from_path_action.cpp | 79 ++++ .../decorator/path_longer_on_approach.cpp | 2 +- .../src/behavior_tree_engine.cpp | 3 +- .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_assisted_teleop_action.cpp | 3 + .../test_assisted_teleop_cancel_node.cpp | 3 + .../plugins/action/test_back_up_action.cpp | 3 + .../action/test_back_up_cancel_node.cpp | 3 + .../plugins/action/test_bt_action_node.cpp | 9 +- .../action/test_clear_costmap_service.cpp | 9 + ...test_compute_path_through_poses_action.cpp | 3 + .../test_compute_path_to_pose_action.cpp | 3 + .../action/test_controller_cancel_node.cpp | 3 + .../action/test_drive_on_heading_action.cpp | 3 + .../test_drive_on_heading_cancel_node.cpp | 3 + .../action/test_follow_path_action.cpp | 3 + .../action/test_get_pose_from_path_action.cpp | 156 ++++++++ .../test_navigate_through_poses_action.cpp | 3 + .../action/test_navigate_to_pose_action.cpp | 3 + ...initialize_global_localization_service.cpp | 3 + .../action/test_smooth_path_action.cpp | 3 + .../test/plugins/action/test_spin_action.cpp | 3 + .../plugins/action/test_spin_cancel_node.cpp | 3 + .../test/plugins/action/test_wait_action.cpp | 3 + .../plugins/action/test_wait_cancel_node.cpp | 3 + .../plugins/drive_on_heading.hpp | 4 +- .../include/nav2_behaviors/timed_behavior.hpp | 15 +- nav2_behaviors/package.xml | 2 +- nav2_behaviors/plugins/assisted_teleop.cpp | 8 +- nav2_behaviors/plugins/back_up.cpp | 2 +- nav2_behaviors/plugins/spin.cpp | 4 +- nav2_bringup/package.xml | 2 +- .../params/nav2_multirobot_params_1.yaml | 1 + .../params/nav2_multirobot_params_2.yaml | 1 + .../params/nav2_multirobot_params_all.yaml | 1 + nav2_bringup/params/nav2_params.yaml | 1 + nav2_bt_navigator/README.md | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/CMakeLists.txt | 2 + .../nav2_collision_monitor/polygon.hpp | 19 +- .../include/nav2_collision_monitor/source.hpp | 23 ++ nav2_collision_monitor/package.xml | 2 +- .../params/collision_monitor_params.yaml | 5 + .../src/collision_monitor_node.cpp | 11 +- nav2_collision_monitor/src/pointcloud.cpp | 1 + nav2_collision_monitor/src/polygon.cpp | 41 +- nav2_collision_monitor/src/range.cpp | 1 + nav2_collision_monitor/src/scan.cpp | 1 + nav2_collision_monitor/src/source.cpp | 40 ++ nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- nav2_controller/src/controller_server.cpp | 13 +- nav2_core/package.xml | 2 +- nav2_costmap_2d/cfg/Costmap2D.cfg | 23 -- nav2_costmap_2d/cfg/GenericPlugin.cfg | 7 - nav2_costmap_2d/cfg/InflationPlugin.cfg | 12 - nav2_costmap_2d/cfg/ObstaclePlugin.cfg | 22 -- nav2_costmap_2d/cfg/VoxelPlugin.cfg | 22 -- .../nav2_costmap_2d/clear_costmap_service.hpp | 6 +- .../nav2_costmap_2d/costmap_2d_ros.hpp | 31 ++ .../include/nav2_costmap_2d/static_layer.hpp | 12 + nav2_costmap_2d/launch/example.launch | 21 - nav2_costmap_2d/launch/example_params.yaml | 43 -- nav2_costmap_2d/package.xml | 2 +- nav2_costmap_2d/plugins/inflation_layer.cpp | 10 + nav2_costmap_2d/plugins/obstacle_layer.cpp | 5 +- nav2_costmap_2d/plugins/static_layer.cpp | 36 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 10 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 49 ++- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 54 +++ 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 +- .../dwb_plugins/cfg/KinematicParams.cfg | 33 -- 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 +- .../ego_polar_coords.hpp | 16 - .../smooth_control_law.hpp | 6 +- nav2_graceful_controller/package.xml | 2 +- .../src/smooth_control_law.cpp | 5 +- .../test/test_egopolar.cpp | 18 +- .../lifecycle_manager_client.hpp | 17 - nav2_lifecycle_manager/package.xml | 2 +- .../include/nav2_map_server/map_io.hpp | 11 + nav2_map_server/package.xml | 2 +- nav2_map_server/src/map_io.cpp | 28 +- nav2_map_server/test/unit/test_map_io.cpp | 27 ++ nav2_mppi_controller/CMakeLists.txt | 1 + nav2_mppi_controller/README.md | 14 + nav2_mppi_controller/critics.xml | 4 + .../critics/velocity_deadband_critic.hpp | 54 +++ .../nav2_mppi_controller/motion_models.hpp | 8 +- nav2_mppi_controller/package.xml | 2 +- .../src/critics/cost_critic.cpp | 6 + .../src/critics/velocity_deadband_critic.cpp | 104 +++++ nav2_mppi_controller/src/optimizer.cpp | 4 +- nav2_mppi_controller/test/critics_tests.cpp | 52 ++- .../test/motion_model_tests.cpp | 80 +++- nav2_msgs/CMakeLists.txt | 1 + nav2_msgs/msg/CollisionMonitorState.msg | 9 + nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- .../include/nav2_planner/planner_server.hpp | 3 - nav2_planner/package.xml | 2 +- nav2_planner/src/planner_server.cpp | 24 +- .../package.xml | 2 +- nav2_rotation_shim_controller/README.md | 4 + .../nav2_rotation_shim_controller.hpp | 8 + .../tools/utils.hpp | 60 +++ nav2_rotation_shim_controller/package.xml | 2 +- .../src/nav2_rotation_shim_controller.cpp | 55 +++ .../test/test_shim_controller.cpp | 77 +++- nav2_rviz_plugins/package.xml | 2 +- nav2_simple_commander/package.xml | 2 +- .../include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/package.xml | 2 +- nav2_smac_planner/src/a_star.cpp | 6 +- nav2_smac_planner/src/analytic_expansion.cpp | 2 +- nav2_smac_planner/src/collision_checker.cpp | 2 +- nav2_smac_planner/src/node_hybrid.cpp | 3 +- nav2_smac_planner/test/test_nodehybrid.cpp | 15 +- .../include/nav2_smoother/nav2_smoother.hpp | 2 - nav2_smoother/package.xml | 2 +- nav2_smoother/src/nav2_smoother.cpp | 12 +- nav2_system_tests/package.xml | 2 +- .../behavior_tree/test_behavior_tree_node.cpp | 2 + .../backup/backup_behavior_tester.cpp | 4 +- .../backup/test_backup_behavior_launch.py | 5 +- .../drive_on_heading_behavior_tester.cpp | 4 +- .../spin/test_spin_behavior_launch.py | 5 +- .../wait/test_wait_behavior_launch.py | 5 +- .../behaviors/wait/wait_behavior_tester.cpp | 1 - nav2_theta_star_planner/package.xml | 2 +- .../nav2_util/simple_action_server.hpp | 4 +- .../include/nav2_util/validate_messages.hpp | 179 +++++++++ nav2_util/package.xml | 2 +- nav2_util/test/CMakeLists.txt | 4 + nav2_util/test/test_validation_messages.cpp | 368 ++++++++++++++++++ nav2_velocity_smoother/package.xml | 2 +- .../src/velocity_smoother.cpp | 4 +- nav2_voxel_grid/package.xml | 2 +- .../plugins/wait_at_waypoint.hpp | 1 + nav2_waypoint_follower/package.xml | 2 +- .../plugins/wait_at_waypoint.cpp | 3 +- navigation2/package.xml | 2 +- 161 files changed, 2078 insertions(+), 391 deletions(-) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp create mode 100644 nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp delete mode 100755 nav2_costmap_2d/cfg/Costmap2D.cfg delete mode 100755 nav2_costmap_2d/cfg/GenericPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/InflationPlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/ObstaclePlugin.cfg delete mode 100755 nav2_costmap_2d/cfg/VoxelPlugin.cfg delete mode 100644 nav2_costmap_2d/launch/example.launch delete mode 100644 nav2_costmap_2d/launch/example_params.yaml create mode 100644 nav2_costmap_2d/test/unit/lifecycle_test.cpp delete mode 100644 nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg create mode 100644 nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp create mode 100644 nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp create mode 100644 nav2_msgs/msg/CollisionMonitorState.msg create mode 100644 nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp create mode 100644 nav2_util/include/nav2_util/validate_messages.hpp create mode 100644 nav2_util/test/test_validation_messages.cpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 1f5199979a..9964578d41 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.14 + 1.1.16

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 ba2010d507..0fa2f94c54 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -49,6 +49,7 @@ #pragma GCC diagnostic pop #include "nav2_amcl/portable_utils.hpp" +#include "nav2_util/validate_messages.hpp" using namespace std::placeholders; using rcl_interfaces::msg::ParameterType; @@ -523,11 +524,8 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha RCLCPP_INFO(get_logger(), "initialPoseReceived"); - if (msg->header.frame_id == "") { - // This should be removed at some point - RCLCPP_WARN( - get_logger(), - "Received initial pose with empty frame_id. You should always supply a frame_id."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received initialpose message is malformed. Rejecting."); return; } if (nav2_util::strip_leading_slash(msg->header.frame_id) != global_frame_id_) { @@ -1360,6 +1358,7 @@ AmclNode::dynamicParametersCallback( lasers_update_.clear(); frame_to_laser_.clear(); laser_scan_connection_.disconnect(); + laser_scan_filter_.reset(); laser_scan_sub_.reset(); initMessageFilters(); @@ -1381,6 +1380,10 @@ void AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { RCLCPP_DEBUG(get_logger(), "AmclNode: A new map was received."); + if (!nav2_util::validateMsg(*msg)) { + RCLCPP_ERROR(get_logger(), "Received map message is malformed. Rejecting."); + return; + } if (first_map_only_ && first_map_received_) { return; } diff --git a/nav2_amcl/src/pf/CMakeLists.txt b/nav2_amcl/src/pf/CMakeLists.txt index c6f16e6a7d..3b4b2fa5ca 100644 --- a/nav2_amcl/src/pf/CMakeLists.txt +++ b/nav2_amcl/src/pf/CMakeLists.txt @@ -15,6 +15,7 @@ target_include_directories(pf_lib PRIVATE ../include) if(HAVE_DRAND48) target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48") endif() +target_link_libraries(pf_lib m) install(TARGETS pf_lib diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 29d0ecbca7..6ed628e050 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_navigate_through_poses_action_bt_node) add_library(nav2_remove_passed_goals_action_bt_node SHARED plugins/action/remove_passed_goals_action.cpp) list(APPEND plugin_libs nav2_remove_passed_goals_action_bt_node) +add_library(nav2_get_pose_from_path_action_bt_node SHARED plugins/action/get_pose_from_path_action.cpp) +list(APPEND plugin_libs nav2_get_pose_from_path_action_bt_node) + add_library(nav2_pipeline_sequence_bt_node SHARED plugins/control/pipeline_sequence.cpp) list(APPEND plugin_libs nav2_pipeline_sequence_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp index 6f0c9bfb91..49bb2efaa4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp @@ -25,6 +25,7 @@ #include "behaviortree_cpp_v3/xml_parsing.h" #include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h" +#include "rclcpp/rclcpp.hpp" namespace nav2_behavior_tree { @@ -46,7 +47,8 @@ class BehaviorTreeEngine * @brief A constructor for nav2_behavior_tree::BehaviorTreeEngine * @param plugin_libraries vector of BT plugin library names to load */ - explicit BehaviorTreeEngine(const std::vector & plugin_libraries); + explicit BehaviorTreeEngine( + const std::vector & plugin_libraries); virtual ~BehaviorTreeEngine() {} /** 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 8851ab660e..0a89dcfb0e 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 @@ -56,11 +56,16 @@ class BtActionNode : public BT::ActionNodeBase callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard - bt_loop_duration_ = + auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); + + // timeout should be less than bt_loop_duration to be able to finish the current tick + max_timeout_ = std::chrono::duration_cast(bt_loop_duration * 0.5); // Initialize the input and output messages goal_ = typename ActionT::Goal(); @@ -93,10 +98,11 @@ class BtActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(10s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", + action_name.c_str(), + wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); @@ -405,7 +411,7 @@ class BtActionNode : public BT::ActionNodeBase return false; } - auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; auto result = callback_group_executor_.spin_until_future_complete(*future_goal_handle_, timeout); elapsed += timeout; @@ -461,7 +467,10 @@ class BtActionNode : public BT::ActionNodeBase std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution - std::chrono::milliseconds bt_loop_duration_; + std::chrono::milliseconds max_timeout_; + + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; // To track the action server acknowledgement when a new goal is sent std::shared_ptr::SharedPtr>> diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 79b1df5bb4..cb67f45fac 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -242,6 +242,12 @@ class BtActionServer // Default timeout value while waiting for response from a server std::chrono::milliseconds default_server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; + + // should the BT be reloaded even if the same xml filename is requested? + bool always_reload_bt_xml_ = false; + // User-provided callbacks OnGoalReceivedCallback on_goal_received_callback_; OnLoopCallback on_loop_callback_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index 92526a7206..e640251e1a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -61,6 +61,12 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + if (!node->has_parameter("always_reload_bt_xml")) { + node->declare_parameter("always_reload_bt_xml", false); + } + if (!node->has_parameter("wait_for_service_timeout")) { + node->declare_parameter("wait_for_service_timeout", 1000); + } std::vector error_code_names = { "follow_path_error_code", @@ -90,7 +96,7 @@ BtActionServer::BtActionServer( error_codes_str += " " + error_code; } RCLCPP_INFO_STREAM(logger_, "Error_code parameters were set to:" << error_codes_str); - } + } } } @@ -144,6 +150,10 @@ bool BtActionServer::on_configure() bt_loop_duration_ = std::chrono::milliseconds(timeout); node->get_parameter("default_server_timeout", timeout); default_server_timeout_ = std::chrono::milliseconds(timeout); + int wait_for_service_timeout; + node->get_parameter("wait_for_service_timeout", wait_for_service_timeout); + wait_for_service_timeout_ = std::chrono::milliseconds(wait_for_service_timeout); + node->get_parameter("always_reload_bt_xml", always_reload_bt_xml_); // Get error code id names to grab off of the blackboard error_code_names_ = node->get_parameter("error_code_names").as_string_array(); @@ -158,6 +168,9 @@ bool BtActionServer::on_configure() blackboard_->set("node", client_node_); // NOLINT blackboard_->set("server_timeout", default_server_timeout_); // NOLINT blackboard_->set("bt_loop_duration", bt_loop_duration_); // NOLINT + blackboard_->set( + "wait_for_service_timeout", + wait_for_service_timeout_); return true; } @@ -200,8 +213,8 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena // Empty filename is default for backward compatibility auto filename = bt_xml_filename.empty() ? default_bt_xml_filename_ : bt_xml_filename; - // Use previous BT if it is the existing one - if (current_bt_xml_filename_ == filename) { + // Use previous BT if it is the existing one and always reload flag is not set to true + if (!always_reload_bt_xml_ && current_bt_xml_filename_ == filename) { RCLCPP_DEBUG(logger_, "BT will not be reloaded as the given xml is already loaded"); return true; } @@ -225,6 +238,9 @@ bool BtActionServer::loadBehaviorTree(const std::string & bt_xml_filena blackboard->set("node", client_node_); blackboard->set("server_timeout", default_server_timeout_); blackboard->set("bt_loop_duration", bt_loop_duration_); + blackboard->set( + "wait_for_service_timeout", + wait_for_service_timeout_); } } catch (const std::exception & e) { RCLCPP_ERROR(logger_, "Exception when loading BT: %s", e.what()); 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 42d27c94ec..5e9fa932a7 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 @@ -59,6 +59,8 @@ class BtCancelActionNode : public BT::ActionNodeBase server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); std::string remapped_action_name; if (getInput("server_name", remapped_action_name)) { @@ -89,10 +91,10 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - if (!action_client_->wait_for_action_server(1s)) { + if (!action_client_->wait_for_action_server(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", - action_name.c_str()); + node_->get_logger(), "\"%s\" action server not available after waiting for %.2fs", + action_name.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string("Action server ") + action_name + std::string(" not available")); @@ -168,6 +170,8 @@ class BtCancelActionNode : public BT::ActionNodeBase // The timeout value while waiting for response from a server when a // new action goal is canceled std::chrono::milliseconds server_timeout_; + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; }; } // namespace nav2_behavior_tree 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 3537f9e9a2..b33314fa47 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 @@ -57,11 +57,16 @@ class BtServiceNode : public BT::ActionNodeBase callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); // Get the required items from the blackboard - bt_loop_duration_ = + auto bt_loop_duration = config().blackboard->template get("bt_loop_duration"); server_timeout_ = config().blackboard->template get("server_timeout"); getInput("server_timeout", server_timeout_); + wait_for_service_timeout_ = + config().blackboard->template get("wait_for_service_timeout"); + + // timeout should be less than bt_loop_duration to be able to finish the current tick + max_timeout_ = std::chrono::duration_cast(bt_loop_duration * 0.5); // Now that we have node_ to use, create the service client for this BT service getInput("service_name", service_name_); @@ -77,10 +82,10 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - if (!service_client_->wait_for_service(1s)) { + if (!service_client_->wait_for_service(wait_for_service_timeout_)) { RCLCPP_ERROR( - node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", - service_node_name.c_str()); + node_->get_logger(), "\"%s\" service server not available after waiting for %.2fs", + service_name_.c_str(), wait_for_service_timeout_.count() / 1000.0); throw std::runtime_error( std::string( "Service server %s not available", @@ -187,7 +192,7 @@ class BtServiceNode : public BT::ActionNodeBase auto remaining = server_timeout_ - elapsed; if (remaining > std::chrono::milliseconds(0)) { - auto timeout = remaining > bt_loop_duration_ ? bt_loop_duration_ : remaining; + auto timeout = remaining > max_timeout_ ? max_timeout_ : remaining; rclcpp::FutureReturnCode rc; rc = callback_group_executor_.spin_until_future_complete(future_result_, timeout); @@ -247,7 +252,10 @@ class BtServiceNode : public BT::ActionNodeBase std::chrono::milliseconds server_timeout_; // The timeout value for BT loop execution - std::chrono::milliseconds bt_loop_duration_; + std::chrono::milliseconds max_timeout_; + + // The timeout value for waiting for a service to response + std::chrono::milliseconds wait_for_service_timeout_; // To track the server response when a new request is sent std::shared_future future_result_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp new file mode 100644 index 0000000000..4715b0eb22 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2024 Marc Morcos +// +// 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__GET_POSE_FROM_PATH_ACTION_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_ + +#include +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/robot_utils.hpp" +#include "behaviortree_cpp_v3/action_node.h" +#include "nav_msgs/msg/path.h" + +namespace nav2_behavior_tree +{ + +class GetPoseFromPath : public BT::ActionNodeBase +{ +public: + GetPoseFromPath( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("path", "Path to extract pose from"), + BT::OutputPort("pose", "Stamped Extracted Pose"), + BT::InputPort("index", 0, "Index of pose to extract from. -1 is end of list"), + }; + } + +private: + void halt() override {} + BT::NodeStatus tick() override; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__GET_POSE_FROM_PATH_ACTION_HPP_ diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index b0fe9257fd..3cbef2156c 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.14 + 1.1.16 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp new file mode 100644 index 0000000000..13b2da3140 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/get_pose_from_path_action.cpp @@ -0,0 +1,79 @@ +// Copyright (c) 2024 Marc Morcos +// +// 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 "nav_msgs/msg/path.hpp" +#include "nav2_util/geometry_utils.hpp" + +#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp" + +namespace nav2_behavior_tree +{ + +GetPoseFromPath::GetPoseFromPath( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::ActionNodeBase(name, conf) +{ +} + +inline BT::NodeStatus GetPoseFromPath::tick() +{ + setStatus(BT::NodeStatus::RUNNING); + + nav_msgs::msg::Path input_path; + getInput("path", input_path); + + int pose_index; + getInput("index", pose_index); + + if (input_path.poses.empty()) { + return BT::NodeStatus::FAILURE; + } + + // Account for negative indices + if(pose_index < 0) { + pose_index = input_path.poses.size() + pose_index; + } + + // out of bounds index + if(pose_index < 0 || static_cast(pose_index) >= input_path.poses.size()) { + return BT::NodeStatus::FAILURE; + } + + // extract pose + geometry_msgs::msg::PoseStamped output_pose; + output_pose = input_path.poses[pose_index]; + + // populate pose frame from path if necessary + if(output_pose.header.frame_id.empty()) { + output_pose.header.frame_id = input_path.header.frame_id; + } + + + setOutput("pose", output_pose); + + return BT::NodeStatus::SUCCESS; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("GetPoseFromPath"); +} diff --git a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp index d4b40b0964..b8c5bdbb46 100644 --- a/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp +++ b/nav2_behavior_tree/plugins/decorator/path_longer_on_approach.cpp @@ -36,7 +36,7 @@ bool PathLongerOnApproach::isPathUpdated( { return new_path != old_path && old_path.poses.size() != 0 && new_path.poses.size() != 0 && - old_path.poses.back() == new_path.poses.back(); + old_path.poses.back().pose == new_path.poses.back().pose; } bool PathLongerOnApproach::isRobotInGoalProximity( diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp index a5ee96af5e..0a3e100080 100644 --- a/nav2_behavior_tree/src/behavior_tree_engine.cpp +++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp @@ -25,7 +25,8 @@ namespace nav2_behavior_tree { -BehaviorTreeEngine::BehaviorTreeEngine(const std::vector & plugin_libraries) +BehaviorTreeEngine::BehaviorTreeEngine( + const std::vector & plugin_libraries) { BT::SharedLibrary loader; for (const auto & p : plugin_libraries) { diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 4aaf185306..513a19caec 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -92,6 +92,10 @@ ament_add_gtest(test_remove_passed_goals_action test_remove_passed_goals_action. target_link_libraries(test_remove_passed_goals_action nav2_remove_passed_goals_action_bt_node) ament_target_dependencies(test_remove_passed_goals_action ${dependencies}) +ament_add_gtest(test_get_pose_from_path_action test_get_pose_from_path_action.cpp) +target_link_libraries(test_get_pose_from_path_action nav2_get_pose_from_path_action_bt_node) +ament_target_dependencies(test_get_pose_from_path_action ${dependencies}) + ament_add_gtest(test_planner_selector_node test_planner_selector_node.cpp) target_link_libraries(test_planner_selector_node nav2_planner_selector_bt_node) ament_target_dependencies(test_planner_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp index d5b033184b..e293c845cb 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_action.cpp @@ -68,6 +68,9 @@ class AssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp index 006370b697..dd2deefbc0 100644 --- a/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_assisted_teleop_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelAssistedTeleopActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "assisted_teleop"); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index db97020212..436ff427d8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -68,6 +68,9 @@ class BackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp index e1a04c82a7..5f00476621 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelBackUpActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "back_up"); 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 12c25d30d6..86455a24a8 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 @@ -173,6 +173,7 @@ class BTActionNodeTestFixture : public ::testing::Test config_->blackboard->set("node", node_); config_->blackboard->set("server_timeout", 20ms); config_->blackboard->set("bt_loop_duration", 10ms); + config_->blackboard->set("wait_for_service_timeout", 1000ms); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("on_cancelled_triggered", false); @@ -315,8 +316,10 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_success) // the BT should have failed EXPECT_EQ(result, BT::NodeStatus::FAILURE); - // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should be 2 - EXPECT_EQ(ticks, 2); + // since the server timeout is 20ms and bt loop duration is 10ms, number of ticks should + // be at most 2, but it can be 1 too, because the tickOnce may execute two ticks. + EXPECT_LE(ticks, 3); + EXPECT_GE(ticks, 1); } TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) @@ -361,7 +364,7 @@ TEST_F(BTActionNodeTestFixture, test_server_timeout_failure) EXPECT_EQ(result, BT::NodeStatus::FAILURE); // since the server timeout is 90ms and bt loop duration is 10ms, number of ticks should be 9 - EXPECT_EQ(ticks, 9); + EXPECT_EQ(ticks, 10); // start a new execution cycle with the previous BT to ensure previous state doesn't leak into // the new cycle diff --git a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp index f10177ecff..76c2a83278 100644 --- a/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_clear_costmap_service.cpp @@ -53,6 +53,9 @@ class ClearEntireCostmapServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -139,6 +142,9 @@ class ClearCostmapExceptRegionServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); @@ -231,6 +237,9 @@ class ClearCostmapAroundRobotServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp index 122c764621..e9eade22c6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_through_poses_action.cpp @@ -76,6 +76,9 @@ class ComputePathThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index 5e35a1981d..f31fe55d4c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -74,6 +74,9 @@ class ComputePathToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp index cf754c5229..1ec9e545c6 100644 --- a/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_controller_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelControllerActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "follow_path"); diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp index 31f9af0427..49c7dd4de8 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_action.cpp @@ -69,6 +69,9 @@ class DriveOnHeadingActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp index 6c6eb57233..6b97a18b6f 100644 --- a/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_drive_on_heading_cancel_node.cpp @@ -65,6 +65,9 @@ class CancelDriveOnHeadingTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "drive_on_heading_cancel"); diff --git a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp index 1325070e00..b7fcb8ad43 100644 --- a/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_follow_path_action.cpp @@ -67,6 +67,9 @@ class FollowPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp new file mode 100644 index 0000000000..497a138f32 --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_get_pose_from_path_action.cpp @@ -0,0 +1,156 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2021 Samsung Research America +// Copyright (c) 2024 Marc Morcos +// +// 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 + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "behaviortree_cpp_v3/bt_factory.h" + +#include "../../test_action_server.hpp" +#include "nav2_behavior_tree/plugins/action/get_pose_from_path_action.hpp" +#include "../../test_behavior_tree_fixture.hpp" + +class GetPoseFromPathTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("get_pose_from_path_action_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( + "GetPoseFromPath", 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 GetPoseFromPathTestFixture::node_ = nullptr; +BT::NodeConfiguration * GetPoseFromPathTestFixture::config_ = nullptr; +std::shared_ptr GetPoseFromPathTestFixture::factory_ = nullptr; +std::shared_ptr GetPoseFromPathTestFixture::tree_ = nullptr; + +TEST_F(GetPoseFromPathTestFixture, test_tick) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new path and set it on blackboard + nav_msgs::msg::Path path; + std::vector goals; + goals.resize(2); + goals[0].pose.position.x = 1.0; + goals[1].pose.position.x = 2.0; + path.poses = goals; + path.header.frame_id = "test_frame_1"; + config_->blackboard->set("path", path); + + config_->blackboard->set("index", 0); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // check if returned pose is correct + geometry_msgs::msg::PoseStamped pose; + EXPECT_TRUE(config_->blackboard->get("pose", pose)); + EXPECT_EQ(pose.header.frame_id, "test_frame_1"); + EXPECT_EQ(pose.pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->haltTree(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // try last element + config_->blackboard->set("index", -1); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + // check if returned pose is correct + EXPECT_TRUE(config_->blackboard->get("pose", pose)); + EXPECT_EQ(pose.header.frame_id, "test_frame_1"); + EXPECT_EQ(pose.pose.position.x, 2.0); +} + +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_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp index c62e15798e..a6527c925e 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_through_poses_action.cpp @@ -70,6 +70,9 @@ class NavigateThroughPosesActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); std::vector poses; config_->blackboard->set>( diff --git a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp index 19b96d6f68..c457e77b90 100644 --- a/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_navigate_to_pose_action.cpp @@ -68,6 +68,9 @@ class NavigateToPoseActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp index 1dbf1a7e6d..b766e387f2 100644 --- a/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_reinitialize_global_localization_service.cpp @@ -53,6 +53,9 @@ class ReinitializeGlobalLocalizationServiceTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); factory_->registerNodeType( diff --git a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp index 5bbf108c8f..bc0d74b0dc 100644 --- a/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_smooth_path_action.cpp @@ -67,6 +67,9 @@ class SmoothPathActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); BT::NodeBuilder builder = diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp index 332d514941..7de1809a41 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_action.cpp @@ -68,6 +68,9 @@ class SpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp index 978dc2d78f..49d3330ddc 100644 --- a/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_spin_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelSpinActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "spin"); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp index 05cd388d7a..807c89ed0c 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_action.cpp @@ -59,6 +59,9 @@ class WaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); config_->blackboard->set("initial_pose_received", false); config_->blackboard->set("number_recoveries", 0); diff --git a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp index f1ed0750d8..cd7a938b49 100644 --- a/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_wait_cancel_node.cpp @@ -64,6 +64,9 @@ class CancelWaitActionTestFixture : public ::testing::Test config_->blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); + config_->blackboard->set( + "wait_for_service_timeout", + std::chrono::milliseconds(1000)); client_ = rclcpp_action::create_client( node_, "wait"); diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp index d430803d30..9609f78736 100644 --- a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp +++ b/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp @@ -74,7 +74,7 @@ class DriveOnHeading : public TimedBehavior command_speed_ = command->speed; command_time_allowance_ = command->time_allowance; - end_time_ = this->steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *this->tf_, this->global_frame_, this->robot_base_frame_, @@ -93,7 +93,7 @@ class DriveOnHeading : public TimedBehavior */ ResultStatus onCycleUpdate() { - rclcpp::Duration time_remaining = end_time_ - this->steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { this->stopRobot(); RCLCPP_WARN( diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index eb640780d3..a67e8d06f9 100644 --- a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp +++ b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp @@ -117,8 +117,8 @@ class TimedBehavior : public nav2_core::Behavior { node_ = parent; auto node = node_.lock(); - logger_ = node->get_logger(); + clock_ = node->get_clock(); RCLCPP_INFO(logger_, "Configuring %s", name.c_str()); @@ -184,7 +184,7 @@ class TimedBehavior : public nav2_core::Behavior rclcpp::Duration elasped_time_{0, 0}; // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; + rclcpp::Clock::SharedPtr clock_; // Logger rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")}; @@ -215,11 +215,12 @@ class TimedBehavior : public nav2_core::Behavior return; } - auto start_time = steady_clock_.now(); + auto start_time = clock_->now(); + rclcpp::WallRate loop_rate(cycle_frequency_); while (rclcpp::ok()) { - elasped_time_ = steady_clock_.now() - start_time; + elasped_time_ = clock_->now() - start_time; if (action_server_->is_cancel_requested()) { RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str()); stopRobot(); @@ -236,7 +237,7 @@ class TimedBehavior : public nav2_core::Behavior " however feature is currently not implemented. Aborting and stopping.", behavior_name_.c_str()); stopRobot(); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; action_server_->terminate_current(result); onActionCompletion(); return; @@ -248,14 +249,14 @@ class TimedBehavior : public nav2_core::Behavior RCLCPP_INFO( logger_, "%s completed successfully", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; action_server_->succeeded_current(result); onActionCompletion(); return; case Status::FAILED: RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str()); - result->total_elapsed_time = steady_clock_.now() - start_time; + result->total_elapsed_time = clock_->now() - start_time; result->error_code = on_cycle_update_result.error_code; action_server_->terminate_current(result); onActionCompletion(); diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 0d6ca68ffc..e6f70dec4c 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.14 + 1.1.16 TODO Carlos Orduno Steve Macenski diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp index d7db461a3a..e9b19b6726 100644 --- a/nav2_behaviors/plugins/assisted_teleop.cpp +++ b/nav2_behaviors/plugins/assisted_teleop.cpp @@ -67,7 +67,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptrtime_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, AssistedTeleopActionGoal::NONE}; } @@ -82,7 +82,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() feedback_->current_teleop_duration = elasped_time_; action_server_->publish_feedback(feedback_); - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN_STREAM( @@ -125,7 +125,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() if (time == simulation_time_step_) { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collided on first time step, setting velocity to zero"); scaled_twist.linear.x = 0.0f; @@ -135,7 +135,7 @@ ResultStatus AssistedTeleop::onCycleUpdate() } else { RCLCPP_DEBUG_STREAM_THROTTLE( logger_, - steady_clock_, + *clock_, 1000, behavior_name_.c_str() << " collision approaching in " << time << " seconds"); double scale_factor = time / projection_time_; diff --git a/nav2_behaviors/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp index 049e85a3cb..2bb0e8371e 100644 --- a/nav2_behaviors/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -31,7 +31,7 @@ ResultStatus BackUp::onRun(const std::shared_ptr comma command_speed_ = -std::fabs(command->speed); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; if (!nav2_util::getCurrentPose( initial_pose_, *tf_, global_frame_, robot_base_frame_, diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp index 72aa66014c..5b0d5cf254 100644 --- a/nav2_behaviors/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -91,14 +91,14 @@ ResultStatus Spin::onRun(const std::shared_ptr command) cmd_yaw_); command_time_allowance_ = command->time_allowance; - end_time_ = steady_clock_.now() + command_time_allowance_; + end_time_ = this->clock_->now() + command_time_allowance_; return ResultStatus{Status::SUCCEEDED, SpinActionGoal::NONE}; } ResultStatus Spin::onCycleUpdate() { - rclcpp::Duration time_remaining = end_time_ - steady_clock_.now(); + rclcpp::Duration time_remaining = end_time_ - this->clock_->now(); if (time_remaining.seconds() < 0.0 && command_time_allowance_.seconds() > 0.0) { stopRobot(); RCLCPP_WARN( diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 0340705620..3f497d77e0 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.14 + 1.1.16 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index f843ed9584..dbc1fb10a0 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -52,6 +52,7 @@ bt_navigator: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" navigate_through_poses: plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + wait_for_service_timeout: 1000 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index dac535be42..5070351f19 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -52,6 +52,7 @@ bt_navigator: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" navigate_through_poses: plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + wait_for_service_timeout: 1000 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml index 372dcdd3f6..5df764449e 100644 --- a/nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -45,6 +45,7 @@ bt_navigator: odom_topic: /odom bt_loop_duration: 10 default_server_timeout: 20 + wait_for_service_timeout: 1000 navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index bb1ce5d7b5..caf3e03cee 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -52,6 +52,7 @@ bt_navigator: plugin: "nav2_bt_navigator/NavigateToPoseNavigator" navigate_through_poses: plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + wait_for_service_timeout: 1000 # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml diff --git a/nav2_bt_navigator/README.md b/nav2_bt_navigator/README.md index 8dd51a6c6e..48787a9473 100644 --- a/nav2_bt_navigator/README.md +++ b/nav2_bt_navigator/README.md @@ -2,7 +2,7 @@ The BT Navigator (Behavior Tree Navigator) module implements the NavigateToPose and NavigateThroughPoses task interfaces. It is a [Behavior Tree](https://github.com/BehaviorTree/BehaviorTree.CPP/blob/master/docs/BT_basics.md)-based implementation of navigation that is intended to allow for flexibility in the navigation task and provide a way to easily specify complex robot behaviors. -See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://navigation.ros.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. +See its [Configuration Guide Page](https://docs.nav2.org/configuration/packages/configuring-bt-navigator.html) for additional parameter descriptions, as well as the [Nav2 Behavior Tree Explanation](https://docs.nav2.org/behavior_trees/index.html) pages explaining more context on the default behavior trees and examples provided in this package. ## Overview diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index f8209fcfc2..2fba14b666 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.14 + 1.1.16 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/CMakeLists.txt b/nav2_collision_monitor/CMakeLists.txt index 546d080790..35cba4a2c9 100644 --- a/nav2_collision_monitor/CMakeLists.txt +++ b/nav2_collision_monitor/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(nav2_common REQUIRED) find_package(nav2_util REQUIRED) find_package(nav2_costmap_2d REQUIRED) +find_package(nav2_msgs REQUIRED) ### Header ### @@ -35,6 +36,7 @@ set(dependencies tf2_geometry_msgs nav2_util nav2_costmap_2d + nav2_msgs ) set(monitor_executable_name collision_monitor) diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 97423dc411..d5843dd606 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -86,6 +86,11 @@ class Polygon * @return Action type for current polygon */ ActionType getActionType() const; + /** + * @brief Obtains polygon enabled state + * @return Whether polygon is enabled + */ + bool getEnabled() const; /** * @brief Obtains polygon maximum points to enter inside polygon causing no action * @return Maximum points to enter to current polygon and take no action @@ -161,6 +166,14 @@ class Polygon * @param point Given point to check * @return True if given point is inside polygon, otherwise false */ + + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + bool isPointInside(const Point & point) const; // ----- Variables ----- @@ -169,7 +182,9 @@ class Polygon nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; - + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + // Basic parameters /// @brief Name of polygon std::string polygon_name_; @@ -185,6 +200,8 @@ class Polygon double simulation_time_step_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; + /// @brief Whether polygon is enabled + bool enabled_; // Global variables /// @brief TF buffer diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index 8bc750cc71..30d58d53bc 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -74,7 +74,19 @@ class Source const rclcpp::Time & curr_time, std::vector & data) const = 0; + /** + * @brief Obtains source enabled state + * @return Whether source is enabled + */ + bool getEnabled() const; + protected: + /** + * @brief Source configuration routine. + * @return True in case of everything is configured correctly, or false otherwise + */ + bool configure(); + /** * @brief Supporting routine obtaining ROS-parameters common for all data sources * @param source_topic Output name of source subscription topic @@ -91,12 +103,21 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + // ----- Variables ----- /// @brief Collision Monitor node nav2_util::LifecycleNode::WeakPtr node_; /// @brief Collision monitor node logger stored for further usage rclcpp::Logger logger_{rclcpp::get_logger("collision_monitor")}; + /// @brief Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Basic parameters /// @brief Name of data source @@ -116,6 +137,8 @@ class Source /// @brief Whether to correct source data towards to base frame movement, /// considering the difference between current time and latest source time bool base_shift_correction_; + /// @brief Whether source is enabled + bool enabled_; }; // class Source } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 9ffc92422e..79bb4eeb90 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.14 + 1.1.16 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml index 1b0c36529e..a84d74c659 100644 --- a/nav2_collision_monitor/params/collision_monitor_params.yaml +++ b/nav2_collision_monitor/params/collision_monitor_params.yaml @@ -22,6 +22,7 @@ collision_monitor: max_points: 3 visualize: True polygon_pub_topic: "polygon_stop" + enabled: True PolygonSlow: type: "polygon" points: [0.4, 0.4, 0.4, -0.4, -0.4, -0.4, -0.4, 0.4] @@ -30,6 +31,7 @@ collision_monitor: slowdown_ratio: 0.3 visualize: True polygon_pub_topic: "polygon_slowdown" + enabled: True FootprintApproach: type: "polygon" action_type: "approach" @@ -38,12 +40,15 @@ collision_monitor: simulation_time_step: 0.1 max_points: 5 visualize: False + enabled: True observation_sources: ["scan"] scan: type: "scan" topic: "/scan" + enabled: True pointcloud: type: "pointcloud" topic: "/intel_realsense_r200_depth/points" min_height: 0.1 max_height: 0.5 + enabled: True diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 8e9914a400..390220d8fe 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -356,7 +356,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) // Fill collision_points array from different data sources for (std::shared_ptr source : sources_) { - source->getData(curr_time, collision_points); + if (source->getEnabled()) { + source->getData(curr_time, collision_points); + } } // By default - there is no action @@ -365,6 +367,9 @@ void CollisionMonitor::process(const Velocity & cmd_vel_in) std::shared_ptr action_polygon; for (std::shared_ptr polygon : polygons_) { + if (!polygon->getEnabled()) { + continue; + } if (robot_action.action_type == STOP) { // If robot already should stop, do nothing break; @@ -482,7 +487,9 @@ void CollisionMonitor::printAction( void CollisionMonitor::publishPolygons() const { for (std::shared_ptr polygon : polygons_) { - polygon->publish(); + if (polygon->getEnabled()) { + polygon->publish(); + } } } diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index d4d2ea1adf..7e84d8890a 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -48,6 +48,7 @@ PointCloud::~PointCloud() void PointCloud::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 99856eb0b0..b756c98afc 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -44,6 +44,7 @@ Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); poly_.clear(); + dyn_params_handler_.reset(); } bool Polygon::configure() @@ -82,6 +83,10 @@ bool Polygon::configure() polygon_pub_topic, polygon_qos); } + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Polygon::dynamicParametersCallback, this, std::placeholders::_1)); + return true; } @@ -109,6 +114,11 @@ ActionType Polygon::getActionType() const return action_type_; } +bool Polygon::getEnabled() const +{ + return enabled_; +} + int Polygon::getMaxPoints() const { return max_points_; @@ -171,7 +181,12 @@ double Polygon::getCollisionTime( Velocity vel = velocity; // Array of points transformed to the frame concerned with pose on each simulation step - std::vector points_transformed; + std::vector points_transformed = collision_points; + + // Check static polygon + if (getPointsInside(points_transformed) >= max_points_) { + return 0.0; + } // Robot movement simulation for (double time = 0.0; time <= time_before_collision_; time += simulation_time_step_) { @@ -241,6 +256,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return false; } + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(polygon_name_ + ".enabled").as_bool(); + nav2_util::declare_parameter_if_not_declared( node, polygon_name_ + ".max_points", rclcpp::ParameterValue(3)); max_points_ = node->get_parameter(polygon_name_ + ".max_points").as_int(); @@ -347,6 +366,26 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return true; } +rcl_interfaces::msg::SetParametersResult +Polygon::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == polygon_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + inline bool Polygon::isPointInside(const Point & point) const { // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index ae15bd488c..8f75049096 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -48,6 +48,7 @@ Range::~Range() void Range::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index 50f670cb13..8ac45904a2 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -45,6 +45,7 @@ Scan::~Scan() void Scan::configure() { + Source::configure(); auto node = node_.lock(); if (!node) { throw std::runtime_error{"Failed to lock node"}; diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index 6ad41224b5..41b6f647ee 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -46,6 +46,17 @@ Source::~Source() { } +bool Source::configure() +{ + auto node = node_.lock(); + + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind(&Source::dynamicParametersCallback, this, std::placeholders::_1)); + + return true; +} + void Source::getCommonParameters(std::string & source_topic) { auto node = node_.lock(); @@ -57,6 +68,10 @@ void Source::getCommonParameters(std::string & source_topic) node, source_name_ + ".topic", rclcpp::ParameterValue("scan")); // Set deafult topic for laser scanner source_topic = node->get_parameter(source_name_ + ".topic").as_string(); + + nav2_util::declare_parameter_if_not_declared( + node, source_name_ + ".enabled", rclcpp::ParameterValue(true)); + enabled_ = node->get_parameter(source_name_ + ".enabled").as_bool(); } bool Source::sourceValid( @@ -78,4 +93,29 @@ bool Source::sourceValid( return true; } +bool Source::getEnabled() const +{ + return enabled_; +} + +rcl_interfaces::msg::SetParametersResult +Source::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (param_name == source_name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + } + result.successful = true; + return result; +} + } // namespace nav2_collision_monitor diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 631a50e658..b8b16cef0d 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.14 + 1.1.16 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 1e456a608c..d9eaeda346 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.14 + 1.1.16 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index ebde190d30..6f909649d5 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.14 + 1.1.16 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 487a6e1ccc..6705b386b7 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -264,11 +264,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); intermediate_planner_->deactivate(); @@ -296,11 +292,8 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_checkers_.clear(); - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + costmap_ros_->cleanup(); + intermediate_planner_->cleanup(); diff --git a/nav2_core/package.xml b/nav2_core/package.xml index d1d580e439..88c0e90f46 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.14 + 1.1.16 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg deleted file mode 100755 index 07c4a1628b..0000000000 --- a/nav2_costmap_2d/cfg/Costmap2D.cfg +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t - -gen = ParameterGenerator() - -gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10) -gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100) -gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100) - -#map params -gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0) -gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0) -gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50) -gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0) -gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0) - -# robot footprint shape -gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]") -gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10) -gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D")) diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg deleted file mode 100755 index 555e2b5415..0000000000 --- a/nav2_costmap_2d/cfg/GenericPlugin.cfg +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t - -gen = ParameterGenerator() -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin")) diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg deleted file mode 100755 index 5c11eaf791..0000000000 --- a/nav2_costmap_2d/cfg/InflationPlugin.cfg +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100) -gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50) -gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin")) diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg deleted file mode 100755 index a94e98fbbe..0000000000 --- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum) - - -# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50) -# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50) -# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50) -# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50) -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin")) diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg deleted file mode 100755 index 977fcb99d4..0000000000 --- a/nav2_costmap_2d/cfg/VoxelPlugin.cfg +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python - -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t - -gen = ParameterGenerator() - -gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True) -gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True) -gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50) -gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0) -gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50) -gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16) -gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16) -gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16) - -combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"), - gen.const("Maximum", int_t, 1, "Take the maximum of the values"), - gen.const("Nothing", int_t, 99, "Do nothing")], - "Method for combining layers enum") -gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum) - -exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin")) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp index 46d79bd9df..844902dd10 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/clear_costmap_service.hpp @@ -48,6 +48,11 @@ class ClearCostmapService */ ClearCostmapService() = delete; + /** + * @brief A destructor + */ + ~ClearCostmapService(); + /** * @brief Clears the region outside of a user-specified area reverting to the static map */ @@ -67,7 +72,6 @@ class ClearCostmapService // Clearing parameters unsigned char reset_value_; - std::vector clearable_layers_; // Server for clearing the costmap rclcpp::Service::SharedPtr clear_except_service_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index af93f4c7e0..599f7c6f69 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -73,6 +73,12 @@ namespace nav2_costmap_2d class Costmap2DROS : public nav2_util::LifecycleNode { public: + /** + * @brief Constructor for the wrapper + * @param options Additional options to control creation of the node. + */ + Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name @@ -121,6 +127,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief as a child-LifecycleNode : + * Costmap2DROS may be launched by another Lifecycle Node as a composed module + * If composed, its parents will handle the shutdown, which includes this module + */ + void on_rcl_preshutdown() override + { + if (is_lifecycle_follower_) { + // Transitioning handled by parent node + return; + } + + // Else, if this is an independent node, this node needs to handle itself. + RCLCPP_INFO( + get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)", + this->get_name()); + + runCleanups(); + + destroyBond(); + } + /** * @brief Subscribes to sensor topics if necessary and starts costmap * updates, can be called to restart the costmap after calls to either @@ -333,6 +361,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::atomic stop_updates_{false}; std::atomic initialized_{false}; std::atomic stopped_{true}; + std::mutex _dynamic_parameter_mutex; std::unique_ptr map_update_thread_; ///< @brief A thread for updating the map rclcpp::Time last_publish_{0, 0, RCL_ROS_TIME}; rclcpp::Duration publish_cycle_{1, 0}; @@ -365,6 +394,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool track_unknown_space_{false}; double transform_tolerance_{0}; ///< The timeout before transform errors + bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node + // Derived parameters bool use_radius_{false}; std::vector unpadded_footprint_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index dd1a65cef0..967a6c3789 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -48,6 +48,7 @@ #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/footprint.hpp" namespace nav2_costmap_2d { @@ -160,6 +161,17 @@ class StaticLayer : public CostmapLayer rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector parameters); + std::vector transformed_footprint_; + bool footprint_clearing_enabled_; + /** + * @brief Clear costmap layer info below the robot's footprint + */ + void updateFootprint( + double robot_x, double robot_y, double robot_yaw, double * min_x, + double * min_y, + double * max_x, + double * max_y); + std::string global_frame_; ///< @brief The global frame for the costmap std::string map_frame_; /// @brief frame that map is located in diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch deleted file mode 100644 index ac089abfba..0000000000 --- a/nav2_costmap_2d/launch/example.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml deleted file mode 100644 index 72bf695fad..0000000000 --- a/nav2_costmap_2d/launch/example_params.yaml +++ /dev/null @@ -1,43 +0,0 @@ -global_frame: map -robot_base_frame: base_link -update_frequency: 5.0 -publish_frequency: 1.0 - -#set if you want the voxel map published -publish_voxel_map: true - -#set to true if you want to initialize the costmap from a static map -static_map: false - -#begin - COMMENT these lines if you set static_map to true -rolling_window: true -width: 6.0 -height: 6.0 -resolution: 0.025 -#end - COMMENT these lines if you set static_map to true - -#START VOXEL STUFF -map_type: voxel -origin_z: 0.0 -z_resolution: 0.2 -z_voxels: 10 -unknown_threshold: 10 -mark_threshold: 0 -#END VOXEL STUFF - -transform_tolerance: 0.3 -obstacle_max_range: 2.5 -obstacle_min_range: 0.0 -max_obstacle_height: 2.0 -raytrace_max_range: 3.0 -raytrace_min_range: 0.0 - -footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]] -#robot_radius: 0.46 -footprint_padding: 0.01 -inflation_radius: 0.55 -cost_scaling_factor: 10.0 -lethal_cost_threshold: 100 -observation_sources: base_scan -base_scan: {data_type: LaserScan, expected_update_rate: 0.4, - observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08} diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 39e29574ec..d4d1a32acd 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.14 + 1.1.16 This package provides an implementation of a 2D costmap that takes in sensor data from the world, builds a 2D or 3D occupancy grid of the data (depending diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 796c2fd62f..8cbe8dacc9 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged() computeCaches(); need_reinflation_ = true; + if (inflation_radius_ < inscribed_radius_) { + RCLCPP_ERROR( + logger_, + "The configured inflation radius (%.3f) is smaller than " + "the computed inscribed radius (%.3f) of your footprint, " + "it is highly recommended to set inflation radius to be at " + "least as big as the inscribed radius to avoid collisions", + inflation_radius_, inscribed_radius_); + } + RCLCPP_DEBUG( logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu," " inscribed_radius_ = %.3f, inflation_radius_ = %.3f", diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index cccae4d393..057acb125d 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -307,8 +307,11 @@ ObstacleLayer::dynamicParametersCallback( max_obstacle_height_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == name_ + "." + "enabled") { + if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { enabled_ = parameter.as_bool(); + if (enabled_) { + current_ = false; + } } else if (param_name == name_ + "." + "footprint_clearing_enabled") { footprint_clearing_enabled_ = parameter.as_bool(); } diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 18cd51164f..25a10e3bd8 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -45,6 +45,7 @@ #include "pluginlib/class_list_macros.hpp" #include "tf2/convert.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "nav2_util/validate_messages.hpp" PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer) @@ -132,6 +133,7 @@ StaticLayer::getParameters() declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); declareParameter("map_topic", rclcpp::ParameterValue("")); + declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(false)); auto node = node_.lock(); if (!node) { @@ -140,6 +142,7 @@ StaticLayer::getParameters() node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); + node->get_parameter(name_ + "." + "footprint_clearing_enabled", footprint_clearing_enabled_); std::string private_map_topic, global_map_topic; node->get_parameter(name_ + "." + "map_topic", private_map_topic); node->get_parameter("map_topic", global_map_topic); @@ -277,6 +280,10 @@ StaticLayer::interpretValue(unsigned char value) void StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) { + if (!nav2_util::validateMsg(*new_map)) { + RCLCPP_ERROR(logger_, "Received map message is malformed. Rejecting."); + return; + } if (!map_received_) { processMap(*new_map); map_received_ = true; @@ -311,6 +318,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u "StaticLayer: Map update ignored. Current map is in frame %s " "but update was in frame %s", map_frame_.c_str(), update->header.frame_id.c_str()); + return; } unsigned int di = 0; @@ -328,7 +336,7 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u void StaticLayer::updateBounds( - double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/, double * min_x, + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, double * max_x, double * max_y) @@ -366,6 +374,24 @@ StaticLayer::updateBounds( *max_y = std::max(wy, *max_y); has_updated_data_ = false; + + updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); +} + +void +StaticLayer::updateFootprint( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, + double * max_x, + double * max_y) +{ + if (!footprint_clearing_enabled_) {return;} + + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); + + for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { + touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y); + } } void @@ -387,6 +413,10 @@ StaticLayer::updateCosts( return; } + if (footprint_clearing_enabled_) { + setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE); + } + if (!layered_costmap_->isRolling()) { // if not rolling, the layered costmap (master_grid) has same coordinates as this layer if (!use_maximum_) { @@ -469,6 +499,10 @@ StaticLayer::dynamicParametersCallback( has_updated_data_ = true; current_ = false; } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == name_ + "." + "footprint_clearing_enabled") { + footprint_clearing_enabled_ = parameter.as_bool(); + } } } result.successful = true; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 61c2388112..e11aca2ce5 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -40,8 +40,6 @@ ClearCostmapService::ClearCostmapService( logger_ = node->get_logger(); reset_value_ = costmap_.getCostmap()->getDefaultValue(); - node->get_parameter("clearable_layers", clearable_layers_); - clear_except_service_ = node->create_service( "clear_except_" + costmap_.getName(), std::bind( @@ -61,6 +59,14 @@ ClearCostmapService::ClearCostmapService( std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); } +ClearCostmapService::~ClearCostmapService() +{ + // make sure services shutdown. + clear_except_service_.reset(); + clear_around_service_.reset(); + clear_entire_service_.reset(); +} + void ClearCostmapService::clearExceptRegionCallback( const shared_ptr/*request_header*/, const shared_ptr request, diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5fbd1beea8..96c32f955e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,6 +61,47 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name) : Costmap2DROS(name, "/", name) {} +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", "", options), + name_("costmap"), + default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, + default_types_{ + "nav2_costmap_2d::StaticLayer", + "nav2_costmap_2d::ObstacleLayer", + "nav2_costmap_2d::InflationLayer"} +{ + is_lifecycle_follower_ = false; + + RCLCPP_INFO(get_logger(), "Creating Costmap"); + + declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); + declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); + declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); + declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); + declare_parameter("height", rclcpp::ParameterValue(5)); + declare_parameter("width", rclcpp::ParameterValue(5)); + declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + declare_parameter( + "map_topic", rclcpp::ParameterValue( + (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); + declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); + declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); + declare_parameter("plugins", rclcpp::ParameterValue(default_plugins_)); + declare_parameter("filters", rclcpp::ParameterValue(std::vector())); + declare_parameter("publish_frequency", rclcpp::ParameterValue(1.0)); + declare_parameter("resolution", rclcpp::ParameterValue(0.1)); + declare_parameter("robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter("robot_radius", rclcpp::ParameterValue(0.1)); + declare_parameter("rolling_window", rclcpp::ParameterValue(false)); + declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); + declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); + declare_parameter("use_maximum", rclcpp::ParameterValue(false)); +} + Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, @@ -85,8 +126,6 @@ Costmap2DROS::Costmap2DROS( { RCLCPP_INFO(get_logger(), "Creating Costmap"); - std::vector clearable_layers{"obstacle_layer", "voxel_layer", "range_layer"}; - declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); @@ -113,7 +152,6 @@ Costmap2DROS::Costmap2DROS( declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); declare_parameter("use_maximum", rclcpp::ParameterValue(false)); - declare_parameter("clearable_layers", rclcpp::ParameterValue(clearable_layers)); } Costmap2DROS::~Costmap2DROS() @@ -433,6 +471,9 @@ Costmap2DROS::mapUpdateLoop(double frequency) // Execute after start() will complete plugins activation if (!stopped_) { + // Lock while modifying layered costmap and publishing values + std::scoped_lock lock(_dynamic_parameter_mutex); + // Measure the execution time of the updateMap method timer.start(); updateMap(); @@ -622,6 +663,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter { auto result = rcl_interfaces::msg::SetParametersResult(); bool resize_map = false; + std::lock_guard lock_reinit(_dynamic_parameter_mutex); for (auto parameter : parameters) { const auto & type = parameter.get_type(); @@ -717,6 +759,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter layered_costmap_->resizeMap( (unsigned int)(map_width_meters_ / resolution_), (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_); + updateMap(); } result.successful = true; diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp new file mode 100644 index 0000000000..a8ab817df3 --- /dev/null +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -0,0 +1,54 @@ +// 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 "gtest/gtest.h" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" + + +TEST(LifecylceTest, CheckInitialTfTimeout) { + rclcpp::init(0, nullptr); + + auto costmap = std::make_shared(rclcpp::NodeOptions()); + costmap->set_parameter({"initial_transform_timeout", 0.0}); + + std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; + + { + const auto state_after_configure = costmap->configure(); + ASSERT_EQ(state_after_configure.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // Without providing the transform from global to robot base the activation should fail + // and the costmap should transition into the inactive state. + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + + // Set a dummy transform from global to robot base + geometry_msgs::msg::TransformStamped transform_global_to_robot{}; + transform_global_to_robot.header.frame_id = costmap->getGlobalFrameID(); + transform_global_to_robot.child_frame_id = costmap->getBaseFrameID(); + costmap->getTfBuffer()->setTransform(transform_global_to_robot, "test", true); + // Now the costmap should successful transition into the active state + { + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } +} diff --git a/nav2_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index eb9ecc1aeb..a2f4254bb6 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.1.14 + 1.1.16 The costmap_queue package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml index e4d7b92256..848aae5e26 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.1.14 + 1.1.16 TODO Carl Delsey BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml index 3193de8981..01c7f80baf 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.1.14 + 1.1.16 The dwb_critics package David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml index 9dbea374a3..a7e740b4c5 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.1.14 + 1.1.16 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg b/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg deleted file mode 100644 index 51dab28aa4..0000000000 --- a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg +++ /dev/null @@ -1,33 +0,0 @@ -#!/usr/bin/env python -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t - -gen = ParameterGenerator() - -# velocities -gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0) -gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55) -gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1) -gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1) -gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. ' - 'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0) - -# acceleration -gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5) -gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5) -gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2) - -gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5) -gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5) -gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2) - -gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0. ' - 'Previously called min_trans_vel', 0.1) -gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. ' - 'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). ' - 'Previously called max_trans_vel', 0.55) -gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. ' - 'If the value is negative, then the min speed will be arbitrarily close to 0.0.' - ' Previously called min_rot_vel', 0.4) - -exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams')) diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index 92efb2a7cc..06b078c4ae 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.1.14 + 1.1.16 Standard implementations of the GoalChecker and TrajectoryGenerators for dwb_core diff --git a/nav2_dwb_controller/nav2_dwb_controller/package.xml b/nav2_dwb_controller/nav2_dwb_controller/package.xml index fd08166dd9..dea815479d 100644 --- a/nav2_dwb_controller/nav2_dwb_controller/package.xml +++ b/nav2_dwb_controller/nav2_dwb_controller/package.xml @@ -2,7 +2,7 @@ nav2_dwb_controller - 1.1.14 + 1.1.16 ROS2 controller (DWB) metapackage diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml index 486b587949..1f983a54f2 100644 --- a/nav2_dwb_controller/nav_2d_msgs/package.xml +++ b/nav2_dwb_controller/nav_2d_msgs/package.xml @@ -2,7 +2,7 @@ nav_2d_msgs - 1.1.14 + 1.1.16 Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D. David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml index d1b16c2212..54a58337ce 100644 --- a/nav2_dwb_controller/nav_2d_utils/package.xml +++ b/nav2_dwb_controller/nav_2d_utils/package.xml @@ -2,7 +2,7 @@ nav_2d_utils - 1.1.14 + 1.1.16 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp index c9123c510f..eadf608844 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/ego_polar_coords.hpp @@ -33,7 +33,6 @@ namespace nav2_graceful_controller struct EgocentricPolarCoordinates { float r; // Radial distance between the robot pose and the target pose. - // Negative value if the robot is moving backwards. float phi; // Orientation of target with respect to the line of sight // from the robot to the target. float delta; // Steering angle of the robot with respect to the line of sight. @@ -68,21 +67,6 @@ struct EgocentricPolarCoordinates r = sqrt(dX * dX + dY * dY); phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight); delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight); - // If the robot is moving backwards, flip the sign of the radial distance - r *= backward ? -1.0 : 1.0; - } - - /** - * @brief Construct a new egocentric polar coordinates for the target pose. - * - * @param target Target pose. - * @param backward If true, the robot is moving backwards. Defaults to false. - */ - explicit EgocentricPolarCoordinates( - const geometry_msgs::msg::Pose & target, - bool backward = false) - { - EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward); } }; diff --git a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp index ab68f75f68..eb553d3927 100644 --- a/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp +++ b/nav2_graceful_controller/include/nav2_graceful_controller/smooth_control_law.hpp @@ -79,8 +79,7 @@ class SmoothControlLaw * @param v_angular_max The maximum absolute velocity in the angular direction. */ void setSpeedLimit( - const double v_linear_min, const double v_linear_max, - const double v_angular_max); + const double v_linear_min, const double v_linear_max, const double v_angular_max); /** * @brief Compute linear and angular velocities command using the curvature. @@ -103,8 +102,7 @@ class SmoothControlLaw * @return Velocity command. */ geometry_msgs::msg::Twist calculateRegularVelocity( - const geometry_msgs::msg::Pose & target, - const bool & backward = false); + const geometry_msgs::msg::Pose & target, const bool & backward = false); /** * @brief Calculate the next pose of the robot by generating a velocity command using the diff --git a/nav2_graceful_controller/package.xml b/nav2_graceful_controller/package.xml index b9f4817e70..a66797726d 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.1.14 + 1.1.16 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_graceful_controller/src/smooth_control_law.cpp b/nav2_graceful_controller/src/smooth_control_law.cpp index 545a7dd6a8..7a04fce71e 100644 --- a/nav2_graceful_controller/src/smooth_control_law.cpp +++ b/nav2_graceful_controller/src/smooth_control_law.cpp @@ -43,8 +43,7 @@ void SmoothControlLaw::setSlowdownRadius(double slowdown_radius) } void SmoothControlLaw::setSpeedLimit( - const double v_linear_min, const double v_linear_max, - const double v_angular_max) + const double v_linear_min, const double v_linear_max, const double v_angular_max) { v_linear_min_ = v_linear_min; v_linear_max_ = v_linear_max; @@ -59,6 +58,8 @@ geometry_msgs::msg::Twist SmoothControlLaw::calculateRegularVelocity( auto ego_coords = EgocentricPolarCoordinates(target, current, backward); // Calculate the curvature double curvature = calculateCurvature(ego_coords.r, ego_coords.phi, ego_coords.delta); + // Invert the curvature if the robot is moving backwards + curvature = backward ? -curvature : curvature; // Adjust the linear velocity as a function of the path curvature to // slowdown the controller as it approaches its target diff --git a/nav2_graceful_controller/test/test_egopolar.cpp b/nav2_graceful_controller/test/test_egopolar.cpp index 2bd3c4c244..db01e26a88 100644 --- a/nav2_graceful_controller/test/test_egopolar.cpp +++ b/nav2_graceful_controller/test/test_egopolar.cpp @@ -31,8 +31,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorWithValues) { float phi_value = 1.2; float delta_value = -0.5; - nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value, - delta_value); + nav2_graceful_controller::EgocentricPolarCoordinates coords(r_value, phi_value, delta_value); EXPECT_FLOAT_EQ(r_value, coords.r); EXPECT_FLOAT_EQ(phi_value, coords.phi); @@ -57,6 +56,19 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPoses) { EXPECT_FLOAT_EQ(-1.1827937483787536, coords.delta); } +TEST(EgocentricPolarCoordinatesTest, constructorFromPose) { + geometry_msgs::msg::Pose target; + target.position.x = 3.0; + target.position.y = 3.0; + target.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), M_PI)); + + nav2_graceful_controller::EgocentricPolarCoordinates coords(target); + + EXPECT_FLOAT_EQ(4.2426405, coords.r); + EXPECT_FLOAT_EQ(2.3561945, coords.phi); + EXPECT_FLOAT_EQ(-M_PI / 4, coords.delta); +} + TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) { geometry_msgs::msg::Pose target; target.position.x = -3.0; @@ -70,7 +82,7 @@ TEST(EgocentricPolarCoordinatesTest, constructorFromPosesBackward) { nav2_graceful_controller::EgocentricPolarCoordinates coords(target, current, true); - EXPECT_FLOAT_EQ(-6.4031243, coords.r); + EXPECT_FLOAT_EQ(6.4031243, coords.r); EXPECT_FLOAT_EQ(-0.096055523, coords.phi); EXPECT_FLOAT_EQ(-1.0960555, coords.delta); } diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp index c5f2be8bb6..076e848902 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp @@ -84,23 +84,6 @@ class LifecycleManagerClient */ SystemStatus is_active(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); - // A couple convenience methods to facilitate scripting tests - /** - * @brief Set initial pose with covariance - * @param x X position - * @param y Y position - * @param theta orientation - */ - void set_initial_pose(double x, double y, double theta); - /** - * @brief Send goal pose to NavigationToPose action server - * @param x X position - * @param y Y position - * @param theta orientation - * @return true or false - */ - bool navigate_to_pose(double x, double y, double theta); - protected: using ManageLifecycleNodes = nav2_msgs::srv::ManageLifecycleNodes; diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index e04a65d0a7..02c71c3112 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.14 + 1.1.16 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_map_server/include/nav2_map_server/map_io.hpp b/nav2_map_server/include/nav2_map_server/map_io.hpp index bee7df7313..97dc81821c 100644 --- a/nav2_map_server/include/nav2_map_server/map_io.hpp +++ b/nav2_map_server/include/nav2_map_server/map_io.hpp @@ -98,6 +98,17 @@ bool saveMapToFile( const nav_msgs::msg::OccupancyGrid & map, const SaveParameters & save_parameters); +/** + * @brief Expand ~/ to home user dir. + * @param yaml_filename Name of input YAML file. + * @param home_dir Expanded `~/`home dir or empty string if HOME not set + * + * @return Expanded string or input string if `~/` not expanded + */ +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_dir); + } // namespace nav2_map_server #endif // NAV2_MAP_SERVER__MAP_IO_HPP_ diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 13c58a5726..67f94ef8c6 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.14 + 1.1.16 Refactored map server for ROS2 Navigation diff --git a/nav2_map_server/src/map_io.cpp b/nav2_map_server/src/map_io.cpp index 4803b6d3f6..85428490ed 100644 --- a/nav2_map_server/src/map_io.cpp +++ b/nav2_map_server/src/map_io.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include "Magick++.h" #include "nav2_util/geometry_utils.hpp" @@ -115,9 +116,33 @@ T yaml_get_value(const YAML::Node & node, const std::string & key) } } +std::string get_home_dir() +{ + if (const char * home_dir = std::getenv("HOME")) { + return std::string{home_dir}; + } + return std::string{}; +} + +std::string expand_user_home_dir_if_needed( + std::string yaml_filename, + std::string home_variable_value) +{ + if (yaml_filename.size() < 2 || !(yaml_filename[0] == '~' && yaml_filename[1] == '/')) { + return yaml_filename; + } + if (home_variable_value.empty()) { + std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n" + << "[INFO] [map_io] User home dir will be not expanded \n"; + return yaml_filename; + } + const std::string prefix{home_variable_value}; + return yaml_filename.replace(0, 1, prefix); +} + LoadParameters loadMapYaml(const std::string & yaml_filename) { - YAML::Node doc = YAML::LoadFile(yaml_filename); + YAML::Node doc = YAML::LoadFile(expand_user_home_dir_if_needed(yaml_filename, get_home_dir())); LoadParameters load_parameters; auto image_file_name = yaml_get_value(doc, "image"); @@ -295,7 +320,6 @@ LOAD_MAP_STATUS loadMapFromYaml( " for reason: " << e.what() << std::endl; return INVALID_MAP_METADATA; } - try { loadMapFromFile(load_parameters, map); } catch (std::exception & e) { diff --git a/nav2_map_server/test/unit/test_map_io.cpp b/nav2_map_server/test/unit/test_map_io.cpp index acc8d9debd..31269cf2d4 100644 --- a/nav2_map_server/test/unit/test_map_io.cpp +++ b/nav2_map_server/test/unit/test_map_io.cpp @@ -311,3 +311,30 @@ TEST_F(MapIOTester, loadInvalidYAML) LoadParameters loadParameters; ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml"))); } + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenShorterThanTwo) +{ + const std::string emptyFileName{}; + ASSERT_EQ(emptyFileName, expand_user_home_dir_if_needed(emptyFileName, "/home/user")); +} + +TEST( + HomeUserExpanderTestSuite, + homeUserExpanderShouldNotChangeInputStringWhenInputStringDoesNotStartWithHomeSequence) +{ + const std::string fileName{"valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenHomeVariableNotFound) +{ + const std::string fileName{"~/valid_file.yaml"}; + ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "")); +} + +TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHomeVariableSet) +{ + const std::string fileName{"~/valid_file.yaml"}; + const std::string expectedOutputFileName{"/home/user/valid_file.yaml"}; + ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user")); +} diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt index d2bb6d9c9d..3ca6735e40 100644 --- a/nav2_mppi_controller/CMakeLists.txt +++ b/nav2_mppi_controller/CMakeLists.txt @@ -89,6 +89,7 @@ add_library(mppi_critics SHARED src/critics/prefer_forward_critic.cpp src/critics/twirling_critic.cpp src/critics/constraint_critic.cpp + src/critics/velocity_deadband_critic.cpp ) set(libraries mppi_controller mppi_critics) diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md index 58de7b7983..e80f679219 100644 --- a/nav2_mppi_controller/README.md +++ b/nav2_mppi_controller/README.md @@ -174,6 +174,15 @@ Note: There is a "Legacy" version of this critic also available with the same pa | cost_weight | double | Default 10.0. Weight to apply to critic term. | | cost_power | int | Default 1. Power order to apply to term. | + +#### Velocity Deadband Critic + | Parameter | Type | Definition | + | --------------- | ------ | ----------------------------------------------------------------------------------------------------------- | + | cost_weight | double | Default 35.0. Weight to apply to critic term. | + | cost_power | int | Default 1. Power order to apply to term. | + | deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. | + + ### XML configuration example ``` controller_server: @@ -264,6 +273,11 @@ controller_server: threshold_to_consider: 0.5 max_angle_to_furthest: 1.0 forward_preference: true + # VelocityDeadbandCritic: + # enabled: true + # cost_power: 1 + # cost_weight: 35.0 + # deadband_velocities: [0.05, 0.05, 0.05] # TwirlingCritic: # enabled: true # twirling_cost_power: 1 diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml index d578d23a9e..6085dbec88 100644 --- a/nav2_mppi_controller/critics.xml +++ b/nav2_mppi_controller/critics.xml @@ -45,5 +45,9 @@ mppi critic for incentivizing moving within kinematic and dynamic bounds + + mppi critic for restricting command velocities in deadband range + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp new file mode 100644 index 0000000000..76e1dbd670 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/velocity_deadband_critic.hpp @@ -0,0 +1,54 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ + +#include + +#include "nav2_mppi_controller/critic_function.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include "nav2_mppi_controller/tools/utils.hpp" + +namespace mppi::critics +{ + +/** + * @class mppi::critics::VelocityDeadbandCritic + * @brief Critic objective function for enforcing feasible constraints + */ +class VelocityDeadbandCritic : public CriticFunction +{ +public: + /** + * @brief Initialize critic + */ + void initialize() override; + + /** + * @brief Evaluate cost related to goal following + * + * @param costs [out] add reference cost values to this tensor + */ + void score(CriticData & data) override; + +protected: + unsigned int power_{0}; + float weight_{0}; + std::vector deadband_velocities_{0.0f, 0.0f, 0.0f}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp index 4fe85bfb05..7d84ba29df 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp @@ -88,9 +88,9 @@ class AckermannMotionModel : public MotionModel /** * @brief Constructor for mppi::AckermannMotionModel */ - explicit AckermannMotionModel(ParametersHandler * param_handler) + explicit AckermannMotionModel(ParametersHandler * param_handler, const std::string & name) { - auto getParam = param_handler->getParamGetter("AckermannConstraints"); + auto getParam = param_handler->getParamGetter(name + ".AckermannConstraints"); getParam(min_turning_r_, "min_turning_r", 0.2); } @@ -112,8 +112,8 @@ class AckermannMotionModel : public MotionModel auto & vx = control_sequence.vx; auto & wz = control_sequence.wz; - auto view = xt::masked_view(wz, xt::fabs(vx) / xt::fabs(wz) < min_turning_r_); - view = xt::sign(wz) * vx / min_turning_r_; + auto view = xt::masked_view(wz, (xt::fabs(vx) / xt::fabs(wz)) < min_turning_r_); + view = xt::sign(wz) * xt::fabs(vx) / min_turning_r_; } /** diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index f5a3211036..82d37bb8b6 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.14 + 1.1.16 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index b91ab075b6..a9dd9e9e81 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -33,6 +33,12 @@ void CostCritic::initialize() // Normalized by cost value to put in same regime as other weights weight_ /= 254.0f; + // Normalize weight when parameter is changed dynamically as well + auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + weight_ = weight.as_double() / 254.0f; + }; + parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + collision_checker_.setCostmap(costmap_); possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_); diff --git a/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp new file mode 100644 index 0000000000..84d3aba303 --- /dev/null +++ b/nav2_mppi_controller/src/critics/velocity_deadband_critic.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// +// 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 "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" + +namespace mppi::critics +{ + +void VelocityDeadbandCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam(power_, "cost_power", 1); + getParam(weight_, "cost_weight", 35.0); + + // Recast double to float + std::vector deadband_velocities{0.0, 0.0, 0.0}; + getParam(deadband_velocities, "deadband_velocities", std::vector{0.0, 0.0, 0.0}); + std::transform( + deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(), + [](double d) {return static_cast(d);}); + + RCLCPP_INFO_STREAM( + logger_, "VelocityDeadbandCritic instantiated with " + << power_ << " power, " << weight_ << " weight, deadband_velocity [" + << deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << "," + << deadband_velocities_.at(2) << "]"); +} + +void VelocityDeadbandCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + + if (!enabled_) { + return; + } + + auto & vx = data.state.vx; + auto & wz = data.state.wz; + + if (data.motion_model->isHolonomic()) { + auto & vy = data.state.vy; + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; + } + + if (power_ > 1u) { + data.costs += xt::pow( + xt::sum( + std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) * + data.model_dt, + {1}, immediate) * + weight_, + power_); + } else { + data.costs += xt::sum( + (std::move( + xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) + + xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) * + data.model_dt, + {1}, immediate) * + weight_; + } + return; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 5ad3aadc56..3de703bcc4 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -122,6 +122,8 @@ void Optimizer::reset() control_history_[2] = {0.0, 0.0, 0.0}; control_history_[3] = {0.0, 0.0, 0.0}; + settings_.constraints = settings_.base_constraints; + costs_ = xt::zeros({settings_.batch_size}); generated_trajectories_.reset(settings_.batch_size, settings_.time_steps); @@ -408,7 +410,7 @@ void Optimizer::setMotionModel(const std::string & model) } else if (model == "Omni") { motion_model_ = std::make_shared(); } else if (model == "Ackermann") { - motion_model_ = std::make_shared(parameters_handler_); + motion_model_ = std::make_shared(parameters_handler_, name_); } else { throw std::runtime_error( std::string( diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp index 8b26aad774..ad227f6398 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -30,6 +30,7 @@ #include "nav2_mppi_controller/critics/path_follow_critic.hpp" #include "nav2_mppi_controller/critics/prefer_forward_critic.hpp" #include "nav2_mppi_controller/critics/twirling_critic.hpp" +#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp" #include "utils_test.cpp" // NOLINT // Tests the various critic plugin functions @@ -101,7 +102,7 @@ TEST(CriticTests, ConstraintsCritic) // Now with ackermann, all in constraint so no costs to score state.vx = 0.40 * xt::ones({1000, 30}); state.wz = 1.5 * xt::ones({1000, 30}); - data.motion_model = std::make_shared(¶m_handler); + data.motion_model = std::make_shared(¶m_handler, node->get_name()); critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); @@ -639,3 +640,52 @@ TEST(CriticTests, PathAlignLegacyCritic) critic.score(data); EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); } + +TEST(CriticTests, VelocityDeadbandCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap"); + ParametersHandler param_handler(node); + auto getParam = param_handler.getParamGetter("critic"); + std::vector deadband_velocities_; + getParam(deadband_velocities_, "deadband_velocities", std::vector{0.08, 0.08, 0.08}); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + + models::State state; + models::ControlSequence control_sequence; + models::Trajectories generated_trajectories; + models::Path path; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + + // Initialization testing + + // Make sure initializes correctly and that defaults are reasonable + VelocityDeadbandCritic critic; + critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler); + EXPECT_EQ(critic.getName(), "critic"); + + // Scoring testing + + // provide velocities out of deadband bounds, should not have any costs + state.vx = 0.80 * xt::ones({1000, 30}); + state.vy = 0.60 * xt::ones({1000, 30}); + state.wz = 0.80 * xt::ones({1000, 30}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // Test cost value + state.vx = 0.01 * xt::ones({1000, 30}); + state.vy = 0.02 * xt::ones({1000, 30}); + state.wz = 0.021 * xt::ones({1000, 30}); + critic.score(data); + // 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7 + EXPECT_NEAR(costs(1), 19.845, 0.01); +} diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 6085896cfe..f22424d226 100644 --- a/nav2_mppi_controller/test/motion_model_tests.cpp +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -136,7 +136,7 @@ TEST(MotionModelTests, AckermannTest) auto node = std::make_shared("my_node"); ParametersHandler param_handler(node); std::unique_ptr model = - std::make_unique(¶m_handler); + std::make_unique(¶m_handler, node->get_name()); // Check that predict properly populates the trajectory velocities with the control velocities state.cvx = 10 * xt::ones({batches, timesteps}); @@ -164,6 +164,9 @@ TEST(MotionModelTests, AckermannTest) // VX equal since this doesn't change, the WZ is reduced if breaking the constraint EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } // Now, check the specifics of the minimum curvature constraint EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); @@ -177,3 +180,78 @@ TEST(MotionModelTests, AckermannTest) // Check it cleanly destructs model.reset(); } + +TEST(MotionModelTests, AckermannReversingTest) +{ + models::ControlSequence control_sequence; + models::ControlSequence control_sequence2; + models::State state; + int batches = 1000; + int timesteps = 50; + control_sequence.reset(timesteps); // populates with zeros + control_sequence2.reset(timesteps); // populates with zeros + state.reset(batches, timesteps); // populates with zeros + auto node = std::make_shared("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler, node->get_name()); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({batches, timesteps}); + + // Manually set state index 0 from initial conditions which would be the speed of the robot + xt::view(state.vx, xt::all(), 0) = 10; + xt::view(state.wz, xt::all(), 0) = 1; + + model->predict(state); + + EXPECT_EQ(state.vx, state.cvx); + EXPECT_EQ(state.vy, xt::zeros({batches, timesteps})); // non-holonomic + EXPECT_EQ(state.wz, state.cwz); + + // Check that application of constraints are non-empty for Ackermann Drive + for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence.vx(i) = -idx * idx * idx; // now reversing + control_sequence.wz(i) = idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence = control_sequence; + model->applyConstraints(control_sequence); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx); + EXPECT_NE(initial_control_sequence.wz, control_sequence.wz); + for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) { + EXPECT_GT(control_sequence.wz(i), 0.0); + } + + // Repeat with negative rotation direction + for (unsigned int i = 0; i != control_sequence2.vx.shape(0); i++) { + float idx = static_cast(i); + control_sequence2.vx(i) = -idx * idx * idx; // now reversing + control_sequence2.wz(i) = -idx * idx * idx * idx; + } + + models::ControlSequence initial_control_sequence2 = control_sequence2; + model->applyConstraints(control_sequence2); + // VX equal since this doesn't change, the WZ is reduced if breaking the constraint + EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx); + EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz); + for (unsigned int i = 1; i != control_sequence2.wz.shape(0); i++) { + EXPECT_LT(control_sequence2.wz(i), 0.0); + } + + // Now, check the specifics of the minimum curvature constraint + EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6); + for (unsigned int i = 1; i != control_sequence2.vx.shape(0); i++) { + EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2); + } + + // Check that Ackermann Drive is properly non-holonomic and parameterized + EXPECT_EQ(model->isHolonomic(), false); + + // Check it cleanly destructs + model.reset(); +} diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2aa446f47f..4e52c24c6b 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ nav2_package() rosidl_generate_interfaces(${PROJECT_NAME} "msg/CollisionDetectorState.msg" + "msg/CollisionMonitorState.msg" "msg/Costmap.msg" "msg/CostmapMetaData.msg" "msg/CostmapFilterInfo.msg" diff --git a/nav2_msgs/msg/CollisionMonitorState.msg b/nav2_msgs/msg/CollisionMonitorState.msg new file mode 100644 index 0000000000..e470df57eb --- /dev/null +++ b/nav2_msgs/msg/CollisionMonitorState.msg @@ -0,0 +1,9 @@ +# Action type for robot in Collision Monitor +uint8 DO_NOTHING=0 # No action +uint8 STOP=1 # Stop the robot +uint8 SLOWDOWN=2 # Slowdown in percentage from current operating speed +uint8 APPROACH=3 # Keep constant time interval before collision +uint8 action_type + +# Name of triggered polygon +string polygon_name \ No newline at end of file diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index ad356d640d..f024425756 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.14 + 1.1.16 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 9f235683d0..4dec58b470 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.14 + 1.1.16 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index fee711f974..ec48b83184 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -241,9 +241,6 @@ class PlannerServer : public nav2_util::LifecycleNode double max_planner_duration_; std::string planner_ids_concat_; - // Clock - rclcpp::Clock steady_clock_{RCL_STEADY_TIME}; - // TF buffer std::shared_ptr tf_; diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 0934f6e6e7..884eb2c33d 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.14 + 1.1.16 TODO Steve Macenski Apache-2.0 diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 930d5b4ee3..9e41863ccd 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -207,11 +207,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger * ordering assumption: https://github.com/ros2/rclcpp/issues/2096 */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - costmap_ros_->deactivate(); - } + costmap_ros_->deactivate(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -236,15 +232,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) plan_publisher_.reset(); tf_.reset(); - /* - * Double check whether something else transitioned it to INACTIVE - * already, e.g. the rcl preshutdown callback. - */ - if (costmap_ros_->get_current_state().id() == - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - costmap_ros_->cleanup(); - } + costmap_ros_->cleanup(); PlannerMap::iterator it; for (it = planners_.begin(); it != planners_.end(); ++it) { @@ -362,7 +350,7 @@ void PlannerServer::computePlanThroughPoses() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); @@ -425,7 +413,7 @@ void PlannerServer::computePlanThroughPoses() result->path = concat_path; publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { @@ -484,7 +472,7 @@ PlannerServer::computePlan() { std::lock_guard lock(dynamic_params_lock_); - auto start_time = steady_clock_.now(); + auto start_time = this->now(); // Initialize the ComputePathToPose goal and result auto goal = action_server_pose_->get_current_goal(); @@ -521,7 +509,7 @@ PlannerServer::computePlan() // Publish the plan for visualization purposes publishPlan(result->path); - auto cycle_duration = steady_clock_.now() - start_time; + auto cycle_duration = this->now() - start_time; result->planning_time = cycle_duration; if (max_planner_duration_ && cycle_duration.seconds() > max_planner_duration_) { diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index df20eda09b..3ca8d52e07 100644 --- a/nav2_regulated_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -2,7 +2,7 @@ nav2_regulated_pure_pursuit_controller - 1.1.14 + 1.1.16 Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh diff --git a/nav2_rotation_shim_controller/README.md b/nav2_rotation_shim_controller/README.md index 15185bfa96..fd1081ad93 100644 --- a/nav2_rotation_shim_controller/README.md +++ b/nav2_rotation_shim_controller/README.md @@ -12,6 +12,8 @@ This is useful for situations when working with plugins that are either too spec As such, this controller will check the rough heading difference with respect to the robot and a newly received path. If within a threshold, it will pass the request onto the controller to execute. If it is outside of the threshold, this controller will rotate the robot towards that path heading. Once it is within the tolerance, it will then pass off control-execution from this rotation shim controller onto the primary controller plugin. At this point, the robot is still going to be rotating, allowing the current plugin to take control for a smooth hand off into path tracking. It is recommended to be more generous than strict in the angular threshold to allow for a smoother transition, but should be tuned for a specific application's desired behaviors. +When the `rotate_to_goal_heading` parameter is set to true, this controller is also able to take back control of the robot when reaching the XY goal tolerance of the goal checker. In this case, the robot will rotate towards the goal heading until the goal checker validate the goal and ends the current navigation task. + The Rotation Shim Controller is suitable for: - Robots that can rotate in place, such as differential and omnidirectional robots. - Preference to rotate in place rather than 'spiral out' when starting to track a new path that is at a significantly different heading than the robot's current heading. @@ -35,6 +37,7 @@ See its [Configuration Guide Page](https://navigation.ros.org/configuration/pack | `primary_controller` | Internal controller plugin to use for actual control behavior after rotating to heading | | `max_angular_accel` | Maximum angular acceleration for rotation to heading | | `simulate_ahead_time` | Time in seconds to forward simulate a rotation command to check for collisions. If a collision is found, forwards control back to the primary controller plugin. | +| `rotate_to_goal_heading` | If true, the rotationShimController will take back control of the robot when in XY tolerance of the goal and start rotating to the goal heading | Example fully-described XML with default parameter values: @@ -67,6 +70,7 @@ controller_server: rotate_to_heading_angular_vel: 1.8 max_angular_accel: 3.2 simulate_ahead_time: 1.0 + rotate_to_goal_heading: false # DWB parameters ... diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index c3812ec1d3..6d4e8b86b3 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -115,6 +115,13 @@ class RotationShimController : public nav2_core::Controller */ geometry_msgs::msg::PoseStamped getSampledPathPt(); + /** + * @brief Find the goal point in path + * May throw exception if the path is empty + * @return pt location of the output point + */ + geometry_msgs::msg::PoseStamped getSampledPathGoal(); + /** * @brief Uses TF to find the location of the sampled path point in base frame * @param pt location of the sampled path point @@ -168,6 +175,7 @@ class RotationShimController : public nav2_core::Controller double forward_sampling_distance_, angular_dist_threshold_; double rotate_to_heading_angular_vel_, max_angular_accel_; double control_duration_, simulate_ahead_time_; + bool rotate_to_goal_heading_; // Dynamic parameters handler std::mutex mutex_; diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp new file mode 100644 index 0000000000..0a4ff4ac16 --- /dev/null +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov +// Copyright (c) 2023 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_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ +#define NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ + +#include "nav2_core/goal_checker.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace nav2_rotation_shim_controller::utils +{ + +/** +* @brief get the current pose of the robot +* @param goal_checker goal checker to get tolerances +* @param robot robot pose +* @param goal goal pose +* @return bool Whether the robot is in the distance tolerance ignoring rotation and speed +*/ +inline bool withinPositionGoalTolerance( + nav2_core::GoalChecker * goal_checker, + const geometry_msgs::msg::Pose & robot, + const geometry_msgs::msg::Pose & goal) +{ + if (goal_checker) { + geometry_msgs::msg::Pose pose_tolerance; + geometry_msgs::msg::Twist velocity_tolerance; + goal_checker->getTolerances(pose_tolerance, velocity_tolerance); + + const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x; + + auto dx = robot.position.x - goal.position.x; + auto dy = robot.position.y - goal.position.y; + + auto dist_sq = dx * dx + dy * dy; + + if (dist_sq < pose_tolerance_sq) { + return true; + } + } + + return false; +} + +} // namespace nav2_rotation_shim_controller::utils + +#endif // NAV2_ROTATION_SHIM_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_rotation_shim_controller/package.xml b/nav2_rotation_shim_controller/package.xml index 6f3c821431..130c589fde 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.1.14 + 1.1.16 Rotation Shim Controller Steve Macenski Apache-2.0 diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index afd3238145..9b7ca3b656 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -19,6 +19,7 @@ #include #include "nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp" +#include "nav2_rotation_shim_controller/tools/utils.hpp" using rcl_interfaces::msg::ParameterType; @@ -60,6 +61,8 @@ void RotationShimController::configure( node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0)); nav2_util::declare_parameter_if_not_declared( node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); @@ -73,6 +76,8 @@ void RotationShimController::configure( node->get_parameter("controller_frequency", control_frequency); control_duration_ = 1.0 / control_frequency; + node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); + try { primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); RCLCPP_INFO( @@ -139,6 +144,41 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands const geometry_msgs::msg::Twist & velocity, nav2_core::GoalChecker * goal_checker) { + // Rotate to goal heading when in goal xy tolerance + if (rotate_to_goal_heading_) { + std::lock_guard lock_reinit(mutex_); + + try { + geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal(); + + if (!nav2_util::transformPoseInTargetFrame( + sampled_pt_goal, sampled_pt_goal, *tf_, + pose.header.frame_id)) + { + throw std::runtime_error("Failed to transform pose to base frame!"); + } + + if (utils::withinPositionGoalTolerance( + goal_checker, + pose.pose, + sampled_pt_goal.pose)) + { + double pose_yaw = tf2::getYaw(pose.pose.orientation); + double goal_yaw = tf2::getYaw(sampled_pt_goal.pose.orientation); + + double angular_distance_to_heading = angles::shortest_angular_distance(pose_yaw, goal_yaw); + + return computeRotateToHeadingCommand(angular_distance_to_heading, pose, velocity); + } + } catch (const std::runtime_error & e) { + RCLCPP_INFO( + logger_, + "Rotation Shim Controller was unable to find a goal point," + " a rotational collision was detected, or TF failed to transform" + " into base frame! what(): %s", e.what()); + } + } + if (path_updated_) { nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); @@ -201,6 +241,17 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() "passing off to primary controller plugin.", forward_sampling_distance_)); } +geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() +{ + if (current_path_.poses.empty()) { + throw std::runtime_error("Path is empty - cannot find a goal point"); + } + + auto goal = current_path_.poses.back(); + goal.header.stamp = clock_->now(); + return goal; +} + geometry_msgs::msg::Pose RotationShimController::transformPoseToBaseFrame(const geometry_msgs::msg::PoseStamped & pt) { @@ -305,6 +356,10 @@ RotationShimController::dynamicParametersCallback(std::vector } else if (name == plugin_name_ + ".simulate_ahead_time") { simulate_ahead_time_ = parameter.as_double(); } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".rotate_to_goal_heading") { + rotate_to_goal_heading_ = parameter.as_bool(); + } } } diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index 1c172436e6..1160a5a98a 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -309,6 +309,79 @@ TEST(RotationShimControllerTest, computeVelocityTests) EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); } +TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { + auto ctrl = std::make_shared(); + auto node = std::make_shared("ShimControllerTest"); + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto listener = std::make_shared(*tf, node, true); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + auto tf_broadcaster = std::make_shared(node); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "base_link"; + transform.child_frame_id = "odom"; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + tf_broadcaster->sendTransform(transform); + + // set a valid primary controller so we can do lifecycle + node->declare_parameter( + "PathFollower.primary_controller", + std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); + node->declare_parameter( + "PathFollower.rotate_to_goal_heading", + true); + + auto controller = std::make_shared(); + controller->configure(node, name, tf, costmap); + controller->activate(); + + // Test state update and path setting + nav_msgs::msg::Path path; + path.header.frame_id = "base_link"; + path.poses.resize(4); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "base_link"; + geometry_msgs::msg::Twist velocity; + nav2_controller::SimpleGoalChecker checker; + node->declare_parameter( + "checker.xy_goal_tolerance", + 1.0); + checker.initialize(node, "checker", costmap); + + path.header.frame_id = "base_link"; + path.poses[0].pose.position.x = 0.0; + path.poses[0].pose.position.y = 0.0; + path.poses[1].pose.position.x = 0.05; + path.poses[1].pose.position.y = 0.05; + path.poses[2].pose.position.x = 0.10; + path.poses[2].pose.position.y = 0.10; + // goal position within checker xy_goal_tolerance + path.poses[3].pose.position.x = 0.20; + path.poses[3].pose.position.y = 0.20; + // goal heading 45 degrees to the left + path.poses[3].pose.orientation.z = -0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; + path.poses[3].header.frame_id = "base_link"; + + controller->setPlan(path); + auto cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, -1.8); + + // goal heading 45 degrees to the right + path.poses[3].pose.orientation.z = 0.3826834; + path.poses[3].pose.orientation.w = 0.9238795; + controller->setPlan(path); + cmd_vel = controller->computeVelocityCommands(pose, velocity, &checker); + EXPECT_EQ(cmd_vel.twist.angular.z, 1.8); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -338,7 +411,8 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0), rclcpp::Parameter("test.max_angular_accel", 7.0), rclcpp::Parameter("test.simulate_ahead_time", 7.0), - rclcpp::Parameter("test.primary_controller", std::string("HI"))}); + rclcpp::Parameter("test.primary_controller", std::string("HI")), + rclcpp::Parameter("test.rotate_to_goal_heading", true)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -349,4 +423,5 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.rotate_to_heading_angular_vel").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.max_angular_accel").as_double(), 7.0); EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); + EXPECT_EQ(node->get_parameter("test.rotate_to_goal_heading").as_bool(), true); } diff --git a/nav2_rviz_plugins/package.xml b/nav2_rviz_plugins/package.xml index 25b640ca9a..b903d1d2c8 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.14 + 1.1.16 Navigation 2 plugins for rviz Michael Jeronimo Apache-2.0 diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml index 9dae3590e1..c3064d3b24 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.14 + 1.1.16 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index 17a13cee61..b44da9acdf 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -236,6 +236,7 @@ class AStarAlgorithm int _timing_interval = 5000; bool _traverse_unknown; + bool _is_initialized; int _max_iterations; int _max_on_approach_iterations; double _max_planning_time; diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index 617a3d570b..10923a7f25 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.14 + 1.1.16 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 841b43e357..344099fa2f 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -36,6 +36,7 @@ AStarAlgorithm::AStarAlgorithm( const MotionModel & motion_model, const SearchInfo & search_info) : _traverse_unknown(true), + _is_initialized(false), _max_iterations(0), _max_planning_time(0), _x_size(0), @@ -67,7 +68,10 @@ void AStarAlgorithm::initialize( _max_iterations = max_iterations; _max_on_approach_iterations = max_on_approach_iterations; _max_planning_time = max_planning_time; - NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + if(!_is_initialized) { + NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info); + } + _is_initialized = true; _dim3_size = dim_3_size; _expander = std::make_unique>( _motion_model, _search_info, _traverse_unknown, _dim3_size); diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 831bccbd9e..d0e068c634 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -161,7 +161,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionmotion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); reals = s.reals(); // Make sure in range [0, 2PI) diff --git a/nav2_smac_planner/src/collision_checker.cpp b/nav2_smac_planner/src/collision_checker.cpp index 549570fb26..59e7f6760b 100644 --- a/nav2_smac_planner/src/collision_checker.cpp +++ b/nav2_smac_planner/src/collision_checker.cpp @@ -97,7 +97,7 @@ bool GridCollisionChecker::inCollision( if (outsideRange(costmap_->getSizeInCellsX(), x) || outsideRange(costmap_->getSizeInCellsY(), y)) { - return false; + return true; } // Assumes setFootprint already set diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index 73f774bf73..d0a32bd3a6 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -250,7 +250,8 @@ MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node) unsigned int HybridMotionTable::getClosestAngularBin(const double & theta) { - return static_cast(floor(static_cast(theta) / bin_size)); + return static_cast(floor(theta / static_cast(bin_size))) % + num_angle_quantization; } float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx) diff --git a/nav2_smac_planner/test/test_nodehybrid.cpp b/nav2_smac_planner/test/test_nodehybrid.cpp index 5580e64d04..0050b0a0ab 100644 --- a/nav2_smac_planner/test/test_nodehybrid.cpp +++ b/nav2_smac_planner/test/test_nodehybrid.cpp @@ -13,6 +13,7 @@ // limitations under the License. Reserved. #include +#include #include #include #include @@ -312,6 +313,7 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = 3.1415926; + motion_table.num_angle_quantization = 2; double test_theta = 3.1415926; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); @@ -320,17 +322,28 @@ TEST(NodeHybridTest, basic_get_closest_angular_bin_test) { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; double test_theta = M_PI; - unsigned int expected_angular_bin = 1; + unsigned int expected_angular_bin = 0; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } { motion_table.bin_size = M_PI; + motion_table.num_angle_quantization = 2; float test_theta = M_PI; unsigned int expected_angular_bin = 1; unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); EXPECT_EQ(expected_angular_bin, calculated_angular_bin); } + + { + motion_table.bin_size = 0.0872664675; + motion_table.num_angle_quantization = 72; + double test_theta = 6.28318526567925; + unsigned int expected_angular_bin = 71; + unsigned int calculated_angular_bin = motion_table.getClosestAngularBin(test_theta); + EXPECT_EQ(expected_angular_bin, calculated_angular_bin); + } } diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index b52ebf6521..7fdd54aa48 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -166,8 +166,6 @@ class SmootherServer : public nav2_util::LifecycleNode std::shared_ptr costmap_sub_; std::shared_ptr footprint_sub_; std::shared_ptr collision_checker_; - - rclcpp::Clock steady_clock_; }; } // namespace nav2_smoother diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 38a352d0be..42e993c2b2 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.14 + 1.1.16 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 07c49ed8a3..219204e05f 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -252,13 +252,18 @@ bool SmootherServer::findSmootherId( void SmootherServer::smoothPlan() { - auto start_time = steady_clock_.now(); + auto start_time = this->now(); RCLCPP_INFO(get_logger(), "Received a path to smooth."); auto result = std::make_shared(); try { - std::string c_name = action_server_->get_current_goal()->smoother_id; + auto goal = action_server_->get_current_goal(); + if (!goal) { + return; // if action_server_ is inactivate, goal would be a nullptr + } + + std::string c_name = goal->smoother_id; std::string current_smoother; if (findSmootherId(c_name, current_smoother)) { current_smoother_ = current_smoother; @@ -267,7 +272,6 @@ void SmootherServer::smoothPlan() } // Perform smoothing - auto goal = action_server_->get_current_goal(); result->path = goal->path; if (!validate(result->path)) { @@ -276,7 +280,7 @@ void SmootherServer::smoothPlan() result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); - result->smoothing_duration = steady_clock_.now() - start_time; + result->smoothing_duration = this->now() - start_time; if (!result->was_completed) { RCLCPP_INFO( diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index a94a5989d5..c0b779291c 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.14 + 1.1.16 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp index 48ba35038c..48b97848f8 100644 --- a/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp +++ b/nav2_system_tests/src/behavior_tree/test_behavior_tree_node.cpp @@ -138,6 +138,8 @@ class BehaviorTreeHandler "server_timeout", std::chrono::milliseconds(20)); // NOLINT blackboard->set( "bt_loop_duration", std::chrono::milliseconds(10)); // NOLINT + blackboard->set( + "wait_for_service_timeout", std::chrono::milliseconds(1000)); // NOLINT blackboard->set>("tf_buffer", tf_); // NOLINT blackboard->set("initial_pose_received", false); // NOLINT blackboard->set("number_recoveries", 0); // NOLINT diff --git a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index a270c8ab08..974c2d60b2 100644 --- a/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp @@ -35,7 +35,9 @@ BackupBehaviorTester::BackupBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("backup_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("backup_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index bf7f4f04f1..440e6e49af 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -88,9 +88,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_backup_behavior_node', - output='screen') + cmd=[testExecutable], name='test_backup_behavior_node', output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp index 8db6ef6406..967c6beb04 100644 --- a/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/drive_on_heading/drive_on_heading_behavior_tester.cpp @@ -35,7 +35,9 @@ DriveOnHeadingBehaviorTester::DriveOnHeadingBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test"); + rclcpp::NodeOptions options; + options.parameter_overrides({{"use_sim_time", true}}); + node_ = rclcpp::Node::make_shared("DriveOnHeading_behavior_test", options); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index d26795834b..76cdcd1fc1 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -88,9 +88,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_spin_behavior_node', - output='screen') + cmd=[testExecutable], name='test_spin_behavior_node', output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 37f2ebd34c..112955bec4 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -88,9 +88,8 @@ def main(argv=sys.argv[1:]): testExecutable = os.getenv('TEST_EXECUTABLE') test1_action = ExecuteProcess( - cmd=[testExecutable], - name='test_wait_behavior_node', - output='screen') + cmd=[testExecutable], name='test_wait_behavior_node', output='screen', + ) lts = LaunchTestService() lts.add_test_action(ld, test1_action) diff --git a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index ea6e58afb7..dffd52f911 100644 --- a/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -164,7 +164,6 @@ bool WaitBehaviorTester::behaviorTest( RCLCPP_INFO(node_->get_logger(), "result received"); - if ((node_->now() - start_time).seconds() < static_cast(wait_time)) { return false; } diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 7fa8d96b54..db5092cb24 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.1.14 + 1.1.16 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index 4ee84d3b1d..391369e300 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -290,8 +290,8 @@ class SimpleActionServer info_msg("Waiting for async process to finish."); if (steady_clock::now() - start_time >= server_timeout_) { terminate_all(); - completion_callback_(); - throw std::runtime_error("Action callback is still running and missed deadline to stop"); + if (completion_callback_) {completion_callback_();} + error_msg("Action callback is still running and missed deadline to stop"); } } diff --git a/nav2_util/include/nav2_util/validate_messages.hpp b/nav2_util/include/nav2_util/validate_messages.hpp new file mode 100644 index 0000000000..58594f1ecd --- /dev/null +++ b/nav2_util/include/nav2_util/validate_messages.hpp @@ -0,0 +1,179 @@ +// Copyright (c) 2024 GoesM +// +// 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_UTIL__VALIDATE_MESSAGES_HPP_ +#define NAV2_UTIL__VALIDATE_MESSAGES_HPP_ + +#include +#include + +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" + + +// @brief Validation Check +// Check recieved message is safe or not for the nav2-system +// For each msg-type known in nav2, we could check it as following: +// if(!validateMsg()) RCLCPP_ERROR(,"malformed msg. Rejecting.") +// +// Workflow of validateMsg(): +// if here's a sub-msg-type in the recieved msg, +// the content of sub-msg would be checked as sub-msg-type +// then, check the whole recieved msg. +// +// Following conditions are involved in check: +// 1> Value Check: to avoid damaged value like like `nan`, `INF`, empty string and so on +// 2> Logic Check: to avoid value with bad logic, +// like the size of `map` should be equal to `height*width` +// 3> Any other needed condition could be joint here in future + +namespace nav2_util +{ + + +bool validateMsg(const double & num) +{ + /* @brief double/float value check + * if here'a need to check message validation + * it should be avoid to use double value like `nan`, `inf` + * otherwise, we regard it as an invalid message + */ + if (std::isinf(num)) {return false;} + if (std::isnan(num)) {return false;} + return true; +} + +template +bool validateMsg(const std::array & msg) +{ + /* @brief value check for double-array + * like the field `covariance` used in the msg-type: + * geometry_msgs::msg::PoseWithCovarianceStamped + */ + for (const auto & element : msg) { + if (!validateMsg(element)) {return false;} + } + + return true; +} + +const int NSEC_PER_SEC = 1e9; // 1 second = 1e9 nanosecond +bool validateMsg(const builtin_interfaces::msg::Time & msg) +{ + if (msg.nanosec >= NSEC_PER_SEC) { + return false; // invalid nanosec-stamp + } + return true; +} + +bool validateMsg(const std_msgs::msg::Header & msg) +{ + // check sub-type + if (!validateMsg(msg.stamp)) {return false;} + + /* @brief frame_id check + * if here'a need to check message validation + * it should at least have a non-empty frame_id + * otherwise, we regard it as an invalid message + */ + if (msg.frame_id.empty()) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::Point & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + return true; +} + +const double epsilon = 1e-4; +bool validateMsg(const geometry_msgs::msg::Quaternion & msg) +{ + // check sub-type + if (!validateMsg(msg.x)) {return false;} + if (!validateMsg(msg.y)) {return false;} + if (!validateMsg(msg.z)) {return false;} + if (!validateMsg(msg.w)) {return false;} + + if (abs(msg.x * msg.x + msg.y * msg.y + msg.z * msg.z + msg.w * msg.w - 1.0) >= epsilon) { + return false; + } + + return true; +} + +bool validateMsg(const geometry_msgs::msg::Pose & msg) +{ + // check sub-type + if (!validateMsg(msg.position)) {return false;} + if (!validateMsg(msg.orientation)) {return false;} + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovariance & msg) +{ + // check sub-type + if (!validateMsg(msg.pose)) {return false;} + if (!validateMsg(msg.covariance)) {return false;} + + return true; +} + +bool validateMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + if (!validateMsg(msg.pose)) {return false;} + return true; +} + + +// Function to verify map meta information +bool validateMsg(const nav_msgs::msg::MapMetaData & msg) +{ + // check sub-type + if (!validateMsg(msg.origin)) {return false;} + if (!validateMsg(msg.resolution)) {return false;} + + // logic check + // 1> we don't need an empty map + if (msg.height == 0 || msg.width == 0) {return false;} + return true; +} + +// for msg-type like map, costmap and others as `OccupancyGrid` +bool validateMsg(const nav_msgs::msg::OccupancyGrid & msg) +{ + // check sub-type + if (!validateMsg(msg.header)) {return false;} + // msg.data : @todo any check for it ? + if (!validateMsg(msg.info)) {return false;} + + // check logic + if (msg.data.size() != msg.info.width * msg.info.height) { + return false; // check map-size + } + return true; +} + + +} // namespace nav2_util + + +#endif // NAV2_UTIL__VALIDATE_MESSAGES_HPP_ diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 19e690c777..8102306c23 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.14 + 1.1.16 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index 9f1ae99bbc..ad0a5c0c41 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,3 +41,7 @@ target_link_libraries(test_odometry_utils ${library_name}) ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) + +ament_add_gtest(test_validation_messages test_validation_messages.cpp) +ament_target_dependencies(test_validation_messages rclcpp_lifecycle) +target_link_libraries(test_validation_messages ${library_name}) diff --git a/nav2_util/test/test_validation_messages.cpp b/nav2_util/test/test_validation_messages.cpp new file mode 100644 index 0000000000..a7d8ed8904 --- /dev/null +++ b/nav2_util/test/test_validation_messages.cpp @@ -0,0 +1,368 @@ +// Copyright (c) 2024 GoesM +// +// 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 "nav2_util/validate_messages.hpp" + +TEST(ValidateMessagesTest, DoubleValueCheck) { + // Test valid double value + EXPECT_TRUE(nav2_util::validateMsg(3.14)); + // Test invalid double value (infinity) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::infinity())); + // Test invalid double value (NaN) + EXPECT_FALSE(nav2_util::validateMsg(std::numeric_limits::quiet_NaN())); +} + +TEST(ValidateMessagesTest, TimeStampCheck) +{ + // Test valid time stamp + builtin_interfaces::msg::Time valid_time_stamp; + valid_time_stamp.sec = 123; + valid_time_stamp.nanosec = 456789; + EXPECT_TRUE(nav2_util::validateMsg(valid_time_stamp)); + // Test invalid time stamp (nanosec out of range) + builtin_interfaces::msg::Time invalid_time_stamp; + invalid_time_stamp.sec = 123; + invalid_time_stamp.nanosec = 1e9; // 1 second = 1e9 nanoseconds + EXPECT_FALSE(nav2_util::validateMsg(invalid_time_stamp)); +} + +TEST(ValidateMessagesTest, HeaderCheck) +{ + // Test valid header with non-empty frame_id + std_msgs::msg::Header valid_header; + valid_header.stamp.sec = 123; + valid_header.stamp.nanosec = 456789; + valid_header.frame_id = "map"; + EXPECT_TRUE(nav2_util::validateMsg(valid_header)); + // Test invalid header with empty frame_id + std_msgs::msg::Header invalid_header; + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 456789; + invalid_header.frame_id = ""; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); + invalid_header.stamp.sec = 123; + invalid_header.stamp.nanosec = 1e9; + invalid_header.frame_id = "map"; + EXPECT_FALSE(nav2_util::validateMsg(invalid_header)); +} + +TEST(ValidateMessagesTest, PointCheck) +{ + // Test valid Point message + geometry_msgs::msg::Point valid_point; + valid_point.x = 1.0; + valid_point.y = 2.0; + valid_point.z = 3.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_point)); + // Test invalid Point message with NaN value + geometry_msgs::msg::Point invalid_point; + invalid_point.x = 1.0; + invalid_point.y = std::numeric_limits::quiet_NaN(); + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = std::numeric_limits::quiet_NaN(); + invalid_point.y = 2.0; + invalid_point.z = 3.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); + // Test invalid Point message with NaN value + invalid_point.x = 1.0; + invalid_point.y = 2.0; + invalid_point.z = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_point)); +} + +TEST(ValidateMessagesTest, QuaternionCheck) +{ + // Test valid Quaternion message + geometry_msgs::msg::Quaternion valid_quaternion; + valid_quaternion.x = 0.0; + valid_quaternion.y = 0.0; + valid_quaternion.z = 0.0; + valid_quaternion.w = 1.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_quaternion)); + // Test invalid Quaternion message with invalid magnitude + geometry_msgs::msg::Quaternion invalid_quaternion; + invalid_quaternion.x = 0.1; + invalid_quaternion.y = 0.2; + invalid_quaternion.z = 0.3; + invalid_quaternion.w = 0.5; // Invalid magnitude (should be 1.0) + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + + // One NaN value + invalid_quaternion.x = 0.0; + invalid_quaternion.y = std::numeric_limits::quiet_NaN(); + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = std::numeric_limits::quiet_NaN(); + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 0.0; + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = std::numeric_limits::quiet_NaN(); + invalid_quaternion.w = 1.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); + invalid_quaternion.x = 0.0; + invalid_quaternion.y = 0.0; + invalid_quaternion.z = 1.0; + invalid_quaternion.w = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(nav2_util::validateMsg(invalid_quaternion)); +} + +TEST(ValidateMessagesTest, PoseCheck) +{ + // Test valid Pose message + geometry_msgs::msg::Pose valid_pose; + valid_pose.position.x = 1.0; + valid_pose.position.y = 2.0; + valid_pose.position.z = 3.0; + valid_pose.orientation.x = 1.0; + valid_pose.orientation.y = 0.0; + valid_pose.orientation.z = 0.0; + valid_pose.orientation.w = 0.0; + EXPECT_TRUE(nav2_util::validateMsg(valid_pose)); + // Test invalid Pose message with invalid position + geometry_msgs::msg::Pose invalid_pose; + invalid_pose.position.x = 1.0; + invalid_pose.position.y = std::numeric_limits::quiet_NaN(); + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 1.0; + invalid_pose.orientation.y = 0.0; + invalid_pose.orientation.z = 0.0; + invalid_pose.orientation.w = 0.0; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); + // Test invalid Pose message with invalid orientation + invalid_pose.position.x = 1.0; + invalid_pose.position.y = 2.0; + invalid_pose.position.z = 3.0; + invalid_pose.orientation.x = 0.1; + invalid_pose.orientation.y = 0.2; + invalid_pose.orientation.z = 0.3; + invalid_pose.orientation.w = 0.4; + EXPECT_FALSE(nav2_util::validateMsg(invalid_pose)); +} + + +TEST(ValidateMessagesTest, MapMetaDataCheck) { + // Test valid MapMetaData message + nav_msgs::msg::MapMetaData valid_map_meta_data; + valid_map_meta_data.resolution = 0.05; + valid_map_meta_data.width = 100; + valid_map_meta_data.height = 100; + geometry_msgs::msg::Pose valid_origin; + valid_origin.position.x = 0.0; + valid_origin.position.y = 0.0; + valid_origin.position.z = 0.0; + valid_origin.orientation.x = 0.0; + valid_origin.orientation.y = 0.0; + valid_origin.orientation.z = 0.0; + valid_origin.orientation.w = 1.0; + valid_map_meta_data.origin = valid_origin; + EXPECT_TRUE(nav2_util::validateMsg(valid_map_meta_data)); + + // Test invalid origin message + nav_msgs::msg::MapMetaData invalid_map_meta_data; + invalid_map_meta_data.resolution = 100.0; + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + geometry_msgs::msg::Pose invalid_origin; + invalid_origin.position.x = 0.0; + invalid_origin.position.y = 0.0; + invalid_origin.position.z = 0.0; + invalid_origin.orientation.x = 0.0; + invalid_origin.orientation.y = 0.0; + invalid_origin.orientation.z = 1.0; + invalid_origin.orientation.w = 1.0; + invalid_map_meta_data.origin = invalid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid resolution message + invalid_map_meta_data.resolution = std::numeric_limits::quiet_NaN(); + invalid_map_meta_data.width = 100; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); + + // Test invalid MapMetaData message with zero width + invalid_map_meta_data.resolution = 0.05; + invalid_map_meta_data.width = 0; + invalid_map_meta_data.height = 100; + invalid_map_meta_data.origin = valid_origin; + EXPECT_FALSE(nav2_util::validateMsg(invalid_map_meta_data)); +} + +TEST(ValidateMessagesTest, OccupancyGridCheck) { + // Test valid OccupancyGrid message + nav_msgs::msg::OccupancyGrid valid_occupancy_grid; + valid_occupancy_grid.header.frame_id = "map"; + valid_occupancy_grid.info.resolution = 0.05; + valid_occupancy_grid.info.width = 100; + valid_occupancy_grid.info.height = 100; + std::vector data(100 * 100, 0); // Initialize with zeros + valid_occupancy_grid.data = data; + EXPECT_TRUE(nav2_util::validateMsg(valid_occupancy_grid)); + + // Test invalid header message with wrong data size + nav_msgs::msg::OccupancyGrid invalid_occupancy_grid; + invalid_occupancy_grid.header.frame_id = ""; // Incorrect id + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid info message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 0; // Incorrect width + invalid_occupancy_grid.info.height = 100; + invalid_occupancy_grid.data = data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); + + // Test invalid OccupancyGrid message with wrong data size + invalid_occupancy_grid.header.frame_id = "map"; + invalid_occupancy_grid.info.resolution = 0.05; + invalid_occupancy_grid.info.width = 100; + invalid_occupancy_grid.info.height = 100; + std::vector invalid_data(100 * 99, 0); // Incorrect data size + invalid_occupancy_grid.data = invalid_data; + EXPECT_FALSE(nav2_util::validateMsg(invalid_occupancy_grid)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovariance validate_msg; + validate_msg.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.covariance[35] = 0.06853891909122467; + + validate_msg.pose.position.x = 0.50010401010515571; + validate_msg.pose.position.y = 1.7468730211257935; + validate_msg.pose.position.z = 0.0; + + validate_msg.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.orientation.y = 0.0; + validate_msg.pose.orientation.z = 0.0; + validate_msg.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovariance invalidate_msg1; + invalidate_msg1.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.covariance[7] = NAN; + invalidate_msg1.covariance[9] = NAN; + invalidate_msg1.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.position.z = 0.0; + + invalidate_msg1.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.orientation.y = 0.0; + invalidate_msg1.pose.orientation.z = 0.0; + invalidate_msg1.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovariance invalidate_msg2; + invalidate_msg2.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.position.x = NAN; + invalidate_msg2.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.position.z = 0.0; + + invalidate_msg2.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.orientation.y = 0.0; + invalidate_msg2.pose.orientation.z = 0.0; + invalidate_msg2.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + +TEST(ValidateMessagesTest, PoseWithCovarianceStampedCheck) { + // Valid message + geometry_msgs::msg::PoseWithCovarianceStamped validate_msg; + validate_msg.header.frame_id = "map"; + validate_msg.header.stamp.sec = 1711029956; + validate_msg.header.stamp.nanosec = 146734875; + + validate_msg.pose.covariance[0] = 0.25; + // assign other covariance values... + validate_msg.pose.covariance[35] = 0.06853891909122467; + + validate_msg.pose.pose.position.x = 0.50010401010515571; + validate_msg.pose.pose.position.y = 1.7468730211257935; + validate_msg.pose.pose.position.z = 0.0; + + validate_msg.pose.pose.orientation.x = 0.9440542194053062; + validate_msg.pose.pose.orientation.y = 0.0; + validate_msg.pose.pose.orientation.z = 0.0; + validate_msg.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_TRUE(nav2_util::validateMsg(validate_msg)); + + // Invalid messages + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg1; + invalidate_msg1.header.frame_id = "map"; + invalidate_msg1.header.stamp.sec = 1711029956; + invalidate_msg1.header.stamp.nanosec = 146734875; + + invalidate_msg1.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg1.pose.covariance[7] = NAN; + invalidate_msg1.pose.covariance[9] = NAN; + invalidate_msg1.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg1.pose.pose.position.x = 0.50010401010515571; + invalidate_msg1.pose.pose.position.y = 1.7468730211257935; + invalidate_msg1.pose.pose.position.z = 0.0; + + invalidate_msg1.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg1.pose.pose.orientation.y = 0.0; + invalidate_msg1.pose.pose.orientation.z = 0.0; + invalidate_msg1.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg1)); + + geometry_msgs::msg::PoseWithCovarianceStamped invalidate_msg2; + invalidate_msg2.header.frame_id = ""; + invalidate_msg2.header.stamp.sec = 1711029956; + invalidate_msg2.header.stamp.nanosec = 146734875; + + invalidate_msg2.pose.covariance[0] = 0.25; + // assign other covariance values... + invalidate_msg2.pose.covariance[35] = 0.06853891909122467; + + invalidate_msg2.pose.pose.position.x = 0.50010401010515571; + invalidate_msg2.pose.pose.position.y = 1.7468730211257935; + invalidate_msg2.pose.pose.position.z = 0.0; + + invalidate_msg2.pose.pose.orientation.x = 0.9440542194053062; + invalidate_msg2.pose.pose.orientation.y = 0.0; + invalidate_msg2.pose.pose.orientation.z = 0.0; + invalidate_msg2.pose.pose.orientation.w = -0.32979028309372299; + + EXPECT_FALSE(nav2_util::validateMsg(invalidate_msg2)); +} + + +// Add more test cases for other validateMsg functions if needed diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 9fa7e99540..dabe9d46c5 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.14 + 1.1.16 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index bc101fd732..cb2ca19916 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -134,7 +134,7 @@ VelocitySmoother::on_activate(const rclcpp_lifecycle::State &) RCLCPP_INFO(get_logger(), "Activating"); smoothed_cmd_pub_->on_activate(); double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); @@ -342,7 +342,7 @@ VelocitySmoother::dynamicParametersCallback(std::vector param } double timer_duration_ms = 1000.0 / smoothing_frequency_; - timer_ = create_wall_timer( + timer_ = this->create_wall_timer( std::chrono::milliseconds(static_cast(timer_duration_ms)), std::bind(&VelocitySmoother::smootherTimer, this)); } else if (name == "velocity_timeout") { diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 7a2d8dd89f..c5f568b2bf 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.14 + 1.1.16 voxel_grid provides an implementation of an efficient 3D voxel grid. The occupancy grid can support 3 different representations for the state of a cell: marked, free, or unknown. Due to the underlying implementation relying on bitwise and and or integer operations, the voxel grid only supports 16 different levels per voxel column. However, this limitation yields raytracing and cell marking performance in the grid comparable to standard 2D structures making it quite fast compared to most 3D structures. diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp index 911ae441a2..cbd2700246 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/wait_at_waypoint.hpp @@ -73,6 +73,7 @@ class WaitAtWaypoint : public nav2_core::WaypointTaskExecutor int waypoint_pause_duration_; bool is_enabled_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_waypoint_follower")}; + rclcpp::Clock::SharedPtr clock_; }; } // namespace nav2_waypoint_follower diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 2669d720b8..f4a2e2cb22 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.14 + 1.1.16 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp index 917655e21a..77e20bcf76 100644 --- a/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp +++ b/nav2_waypoint_follower/plugins/wait_at_waypoint.cpp @@ -42,6 +42,7 @@ void WaitAtWaypoint::initialize( throw std::runtime_error{"Failed to lock node in wait at waypoint plugin!"}; } logger_ = node->get_logger(); + clock_ = node->get_clock(); nav2_util::declare_parameter_if_not_declared( node, plugin_name + ".waypoint_pause_duration", @@ -77,7 +78,7 @@ bool WaitAtWaypoint::processAtWaypoint( logger_, "Arrived at %i'th waypoint, sleeping for %i milliseconds", curr_waypoint_index, waypoint_pause_duration_); - rclcpp::sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); + clock_->sleep_for(std::chrono::milliseconds(waypoint_pause_duration_)); return true; } } // namespace nav2_waypoint_follower diff --git a/navigation2/package.xml b/navigation2/package.xml index d546039a2c..b2b00936db 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.14 + 1.1.16 ROS2 Navigation Stack