From ab8fa8215ff30033e9d213f7ca9cc1dc7216b670 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 23 Aug 2024 17:03:30 -0700 Subject: [PATCH] Humble release 12: August 23 (#4644) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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 --- nav2_amcl/package.xml | 2 +- nav2_amcl/src/amcl_node.cpp | 1 + nav2_behavior_tree/package.xml | 2 +- nav2_behaviors/package.xml | 2 +- nav2_bringup/package.xml | 2 +- nav2_bt_navigator/package.xml | 2 +- nav2_collision_monitor/package.xml | 2 +- nav2_common/package.xml | 2 +- nav2_constrained_smoother/package.xml | 2 +- nav2_controller/package.xml | 2 +- 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 | 5 ++ 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 | 1 + nav2_costmap_2d/src/clear_costmap_service.cpp | 8 ++ 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 +- nav2_graceful_controller/package.xml | 2 +- nav2_lifecycle_manager/package.xml | 2 +- nav2_map_server/package.xml | 2 +- .../nav2_mppi_controller/motion_models.hpp | 4 +- nav2_mppi_controller/package.xml | 2 +- nav2_mppi_controller/src/optimizer.cpp | 2 +- nav2_mppi_controller/test/critics_tests.cpp | 2 +- .../test/motion_model_tests.cpp | 2 +- nav2_msgs/package.xml | 2 +- nav2_navfn_planner/package.xml | 2 +- nav2_planner/package.xml | 2 +- .../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_smoother/package.xml | 2 +- nav2_smoother/src/nav2_smoother.cpp | 8 +- nav2_system_tests/package.xml | 2 +- nav2_theta_star_planner/package.xml | 2 +- nav2_util/package.xml | 2 +- nav2_velocity_smoother/package.xml | 2 +- nav2_voxel_grid/package.xml | 2 +- nav2_waypoint_follower/package.xml | 2 +- navigation2/package.xml | 2 +- 65 files changed, 288 insertions(+), 232 deletions(-) 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 delete mode 100644 nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg create mode 100644 nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/tools/utils.hpp diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 3fe64c32ac..9964578d41 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.1.15 + 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 144f04976f..0fa2f94c54 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1358,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(); diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index e004823e5d..3cbef2156c 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.1.15 + 1.1.16 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index a6846b6e90..e6f70dec4c 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.1.15 + 1.1.16 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index 8ce24f007d..3f497d77e0 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.1.15 + 1.1.16 Bringup scripts and configurations for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index 76c24d865e..e592bcb141 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.1.15 + 1.1.16 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index b922b713b4..79bb4eeb90 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.1.15 + 1.1.16 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index d3c2803de4..b8b16cef0d 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.1.15 + 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 d7e24b12bf..d9eaeda346 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.1.15 + 1.1.16 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 0291eccae9..f2a069a271 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.1.15 + 1.1.16 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 45e1455e9b..a553283ace 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.1.15 + 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 2d74530b43..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 */ 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 bef5200bbc..d4d1a32acd 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.1.15 + 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 653c0885ca..4d5f3c717b 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -304,8 +304,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 729530d82b..25a10e3bd8 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -318,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; diff --git a/nav2_costmap_2d/src/clear_costmap_service.cpp b/nav2_costmap_2d/src/clear_costmap_service.cpp index 4a97b9ae1c..e11aca2ce5 100644 --- a/nav2_costmap_2d/src/clear_costmap_service.cpp +++ b/nav2_costmap_2d/src/clear_costmap_service.cpp @@ -59,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_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index a36dfbbe3c..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.15 + 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 b3988b191f..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.15 + 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 6c1815ddf6..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.15 + 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 e4caba5f2f..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.15 + 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 74b145a34f..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.15 + 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 24dbc7c54a..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.15 + 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 01aca5e822..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.15 + 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 920cee18a8..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.15 + 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/package.xml b/nav2_graceful_controller/package.xml index ac2c392044..a66797726d 100644 --- a/nav2_graceful_controller/package.xml +++ b/nav2_graceful_controller/package.xml @@ -2,7 +2,7 @@ nav2_graceful_controller - 1.1.15 + 1.1.16 Graceful motion controller Alberto Tudela Apache-2.0 diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index bc6c798690..02c71c3112 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.1.15 + 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/package.xml b/nav2_map_server/package.xml index a963cc5cd0..67f94ef8c6 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.1.15 + 1.1.16 Refactored map server for ROS2 Navigation 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..0c5671ea3e 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); } diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index 11c9c17972..82d37bb8b6 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.1.15 + 1.1.16 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 3c14d5f30e..ee6d708e5c 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -410,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 ac21de711a..ad227f6398 100644 --- a/nav2_mppi_controller/test/critics_tests.cpp +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -102,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); diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp index 6085896cfe..4238fa03fb 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}); diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 8bc5f59980..f024425756 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.1.15 + 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 4be8ab5ec0..4dec58b470 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.1.15 + 1.1.16 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 95faf82ed9..884eb2c33d 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.1.15 + 1.1.16 TODO Steve Macenski Apache-2.0 diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml index 003bedd132..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.15 + 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 d3f96dd25e..e5ecbad9e5 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 0b57f3cd37..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.15 + 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 acbf2d1bf7..9e2485af7c 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -20,6 +20,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; @@ -61,6 +62,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_); @@ -74,6 +77,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( @@ -140,6 +145,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())); @@ -202,6 +242,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 adfa2acf0c..b903d1d2c8 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.1.15 + 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 09b45d26b0..c3064d3b24 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.1.15 + 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 001f5df8d6..8b3b9808f9 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -235,6 +235,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 8ddf561517..10923a7f25 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.1.15 + 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 06bc0520d4..bb68fd8682 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_smoother/package.xml b/nav2_smoother/package.xml index 0f1fce5e1a..42e993c2b2 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.1.15 + 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 795fd1fb2f..bb68ee3821 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -260,7 +260,12 @@ void SmootherServer::smoothPlan() 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; @@ -270,7 +275,6 @@ void SmootherServer::smoothPlan() } // Perform smoothing - auto goal = action_server_->get_current_goal(); result->path = goal->path; result->was_completed = smoothers_[current_smoother_]->smooth( result->path, goal->max_smoothing_duration); diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index c4138c991d..67b55e15f4 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.1.15 + 1.1.16 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index 52c51d426d..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.15 + 1.1.16 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 1231956463..8102306c23 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.1.15 + 1.1.16 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index f9e0b1ff08..dabe9d46c5 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.1.15 + 1.1.16 Nav2's Output velocity smoother Steve Macenski Apache-2.0 diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml index 13d2d6f4cf..c5f568b2bf 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.1.15 + 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/package.xml b/nav2_waypoint_follower/package.xml index 83912ab944..f4a2e2cb22 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.1.15 + 1.1.16 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 0fc4695c86..b2b00936db 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.1.15 + 1.1.16 ROS2 Navigation Stack