diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml index 27d1e917c7..3effde7d18 100644 --- a/nav2_amcl/package.xml +++ b/nav2_amcl/package.xml @@ -2,7 +2,7 @@ nav2_amcl - 1.2.8 + 1.2.9

amcl is a probabilistic localization system for a robot moving in diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 4354d1fc94..48c8bd2417 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -2,7 +2,7 @@ nav2_behavior_tree - 1.2.8 + 1.2.9 TODO Michael Jeronimo Carlos Orduno diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml index 51b422dbc6..9f2b9ad3ff 100644 --- a/nav2_behaviors/package.xml +++ b/nav2_behaviors/package.xml @@ -2,7 +2,7 @@ nav2_behaviors - 1.2.8 + 1.2.9 TODO Carlos Orduno Steve Macenski diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml index d9ac165a60..5bf57af203 100644 --- a/nav2_bringup/package.xml +++ b/nav2_bringup/package.xml @@ -2,7 +2,7 @@ nav2_bringup - 1.2.8 + 1.2.9 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 308a0ff0ad..4e708a9e39 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -2,7 +2,7 @@ nav2_bt_navigator - 1.2.8 + 1.2.9 TODO Michael Jeronimo Apache-2.0 diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml index 9c493ffac6..f59f2f7738 100644 --- a/nav2_collision_monitor/package.xml +++ b/nav2_collision_monitor/package.xml @@ -2,7 +2,7 @@ nav2_collision_monitor - 1.2.8 + 1.2.9 Collision Monitor Alexey Merzlyakov Steve Macenski diff --git a/nav2_common/package.xml b/nav2_common/package.xml index 19aa6d0bd1..615d8ce78a 100644 --- a/nav2_common/package.xml +++ b/nav2_common/package.xml @@ -2,7 +2,7 @@ nav2_common - 1.2.8 + 1.2.9 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 8c9c87484c..172572b73a 100644 --- a/nav2_constrained_smoother/package.xml +++ b/nav2_constrained_smoother/package.xml @@ -2,7 +2,7 @@ nav2_constrained_smoother - 1.2.8 + 1.2.9 Ceres constrained smoother Matej Vargovcik Steve Macenski diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml index 2d9416db32..4ebb396c7d 100644 --- a/nav2_controller/package.xml +++ b/nav2_controller/package.xml @@ -2,7 +2,7 @@ nav2_controller - 1.2.8 + 1.2.9 Controller action interface Carl Delsey Apache-2.0 diff --git a/nav2_core/package.xml b/nav2_core/package.xml index 535be634c6..2e6a02f648 100644 --- a/nav2_core/package.xml +++ b/nav2_core/package.xml @@ -2,7 +2,7 @@ nav2_core - 1.2.8 + 1.2.9 A set of headers for plugins core to the Nav2 stack Steve Macenski Carl Delsey diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index a49554cc44..fd720fd24f 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -2,7 +2,7 @@ nav2_costmap_2d - 1.2.8 + 1.2.9 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_dwb_controller/costmap_queue/package.xml b/nav2_dwb_controller/costmap_queue/package.xml index 0708195431..83c0619c52 100644 --- a/nav2_dwb_controller/costmap_queue/package.xml +++ b/nav2_dwb_controller/costmap_queue/package.xml @@ -1,7 +1,7 @@ costmap_queue - 1.2.8 + 1.2.9 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 0c1d9c2257..61e4679cdf 100644 --- a/nav2_dwb_controller/dwb_core/package.xml +++ b/nav2_dwb_controller/dwb_core/package.xml @@ -2,7 +2,7 @@ dwb_core - 1.2.8 + 1.2.9 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 ad4b5f0fec..491ee4ecb7 100644 --- a/nav2_dwb_controller/dwb_critics/package.xml +++ b/nav2_dwb_controller/dwb_critics/package.xml @@ -1,7 +1,7 @@ dwb_critics - 1.2.8 + 1.2.9 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 9ada111110..a987b73763 100644 --- a/nav2_dwb_controller/dwb_msgs/package.xml +++ b/nav2_dwb_controller/dwb_msgs/package.xml @@ -2,7 +2,7 @@ dwb_msgs - 1.2.8 + 1.2.9 Message/Service definitions specifically for the dwb_core David V. Lu!! BSD-3-Clause diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml index b0b9eb77b1..4449747736 100644 --- a/nav2_dwb_controller/dwb_plugins/package.xml +++ b/nav2_dwb_controller/dwb_plugins/package.xml @@ -1,7 +1,7 @@ dwb_plugins - 1.2.8 + 1.2.9 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 b7005a361a..f0e5f2832b 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.2.8 + 1.2.9 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 15c5520368..5e0a9e990c 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.2.8 + 1.2.9 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 ff94df6cb3..305ff7b75e 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.2.8 + 1.2.9 A handful of useful utility functions for nav_2d packages. David V. Lu!! BSD-3-Clause diff --git a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp index 8d302907c4..0728df7d07 100644 --- a/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp +++ b/nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp @@ -104,6 +104,16 @@ class LifecycleManager : public rclcpp::Node * @return true or false */ bool startup(); + /** + * @brief Configures the managed nodes. + * @return true or false + */ + bool configure(); + /** + * @brief Cleanups the managed nodes + * @return true or false + */ + bool cleanup(); /** * @brief Deactivate, clean up and shut down all the managed nodes. * @return true or false 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 076e848902..80e9305e9e 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 @@ -78,6 +78,16 @@ class LifecycleManagerClient * @return true or false */ bool reset(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /** + * @brief Make configure service call + * @return true or false + */ + bool configure(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); + /** + * @brief Make cleanup service call + * @return true or false + */ + bool cleanup(const std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)); /** * @brief Check if lifecycle node manager server is active * @return ACTIVE or INACTIVE or TIMEOUT diff --git a/nav2_lifecycle_manager/package.xml b/nav2_lifecycle_manager/package.xml index e222d526e8..85258fe230 100644 --- a/nav2_lifecycle_manager/package.xml +++ b/nav2_lifecycle_manager/package.xml @@ -2,7 +2,7 @@ nav2_lifecycle_manager - 1.2.8 + 1.2.9 A controller/manager for the lifecycle nodes of the Navigation 2 system Michael Jeronimo Apache-2.0 diff --git a/nav2_lifecycle_manager/src/lifecycle_manager.cpp b/nav2_lifecycle_manager/src/lifecycle_manager.cpp index cea3a18260..e03dfd4701 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager.cpp @@ -125,6 +125,12 @@ LifecycleManager::managerCallback( case ManageLifecycleNodes::Request::STARTUP: response->success = startup(); break; + case ManageLifecycleNodes::Request::CONFIGURE: + response->success = configure(); + break; + case ManageLifecycleNodes::Request::CLEANUP: + response->success = cleanup(); + break; case ManageLifecycleNodes::Request::RESET: response->success = reset(); break; @@ -319,6 +325,34 @@ LifecycleManager::startup() return true; } +bool +LifecycleManager::configure() +{ + message("Configuring managed nodes..."); + if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE)) { + RCLCPP_ERROR(get_logger(), "Failed to configure all requested nodes. Aborting bringup."); + managed_nodes_state_ = NodeState::UNKNOWN; + return false; + } + message("Managed nodes are now configured"); + managed_nodes_state_ = NodeState::INACTIVE; + return true; +} + +bool +LifecycleManager::cleanup() +{ + message("Cleaning up managed nodes..."); + if (!changeStateForAllNodes(Transition::TRANSITION_CLEANUP)) { + RCLCPP_ERROR(get_logger(), "Failed to cleanup all requested nodes. Aborting cleanup."); + managed_nodes_state_ = NodeState::UNKNOWN; + return false; + } + message("Managed nodes have been cleaned up"); + managed_nodes_state_ = NodeState::UNCONFIGURED; + return true; +} + bool LifecycleManager::shutdown() { diff --git a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp index 632eb1aa8e..123c69b58e 100644 --- a/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp +++ b/nav2_lifecycle_manager/src/lifecycle_manager_client.cpp @@ -73,6 +73,18 @@ LifecycleManagerClient::reset(const std::chrono::nanoseconds timeout) return callService(ManageLifecycleNodes::Request::RESET, timeout); } +bool +LifecycleManagerClient::configure(const std::chrono::nanoseconds timeout) +{ + return callService(ManageLifecycleNodes::Request::CONFIGURE, timeout); +} + +bool +LifecycleManagerClient::cleanup(const std::chrono::nanoseconds timeout) +{ + return callService(ManageLifecycleNodes::Request::CLEANUP, timeout); +} + SystemStatus LifecycleManagerClient::is_active(const std::chrono::nanoseconds timeout) { diff --git a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp index 96114f98a5..c2c0cb75f0 100644 --- a/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp +++ b/nav2_lifecycle_manager/test/test_lifecycle_manager.cpp @@ -100,6 +100,8 @@ TEST(LifecycleClientTest, BasicTest) client.is_active(std::chrono::nanoseconds(1000000000))); EXPECT_TRUE(client.resume()); EXPECT_TRUE(client.reset()); + EXPECT_TRUE(client.configure()); + EXPECT_TRUE(client.cleanup()); EXPECT_TRUE(client.shutdown()); } diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml index 98e08c8b80..a7ba7c6281 100644 --- a/nav2_map_server/package.xml +++ b/nav2_map_server/package.xml @@ -2,7 +2,7 @@ nav2_map_server - 1.2.8 + 1.2.9 Refactored map server for ROS2 Navigation diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml index ff0683cd40..33c9af4085 100644 --- a/nav2_mppi_controller/package.xml +++ b/nav2_mppi_controller/package.xml @@ -2,7 +2,7 @@ nav2_mppi_controller - 1.2.8 + 1.2.9 nav2_mppi_controller Steve Macenski Aleksei Budyakov diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index 2bfd8b716f..120a592e71 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -2,7 +2,7 @@ nav2_msgs - 1.2.8 + 1.2.9 Messages and service files for the Nav2 stack Michael Jeronimo Steve Macenski diff --git a/nav2_msgs/srv/ManageLifecycleNodes.srv b/nav2_msgs/srv/ManageLifecycleNodes.srv index 8f37cbf819..bb84f3e231 100644 --- a/nav2_msgs/srv/ManageLifecycleNodes.srv +++ b/nav2_msgs/srv/ManageLifecycleNodes.srv @@ -3,6 +3,8 @@ uint8 PAUSE = 1 uint8 RESUME = 2 uint8 RESET = 3 uint8 SHUTDOWN = 4 +uint8 CONFIGURE = 5 +uint8 CLEANUP = 6 uint8 command --- diff --git a/nav2_navfn_planner/package.xml b/nav2_navfn_planner/package.xml index 70f129d0ad..6fcdf170db 100644 --- a/nav2_navfn_planner/package.xml +++ b/nav2_navfn_planner/package.xml @@ -2,7 +2,7 @@ nav2_navfn_planner - 1.2.8 + 1.2.9 TODO Steve Macenski Carlos Orduno diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml index 667dac75c5..ed091d614b 100644 --- a/nav2_planner/package.xml +++ b/nav2_planner/package.xml @@ -2,7 +2,7 @@ nav2_planner - 1.2.8 + 1.2.9 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 047e5dc902..2b2a6ff96b 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.2.8 + 1.2.9 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 9129e9af61..6802ab7dc4 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: @@ -66,6 +69,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 cd9a540084..59bc9b0fda 100644 --- a/nav2_rotation_shim_controller/package.xml +++ b/nav2_rotation_shim_controller/package.xml @@ -2,7 +2,7 @@ nav2_rotation_shim_controller - 1.2.8 + 1.2.9 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 5097a1fc9c..dde73151fd 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 nav2_core::ControllerTFError("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())); @@ -198,6 +238,17 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() return current_path_.poses.back(); } +geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathGoal() +{ + if (current_path_.poses.empty()) { + throw nav2_core::InvalidPath("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) { @@ -302,6 +353,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 e1bc30f80e..307f4385d9 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -309,6 +309,67 @@ 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 = "fake_frame"; + path.poses.resize(4); + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "base_link"; + geometry_msgs::msg::Twist velocity; + nav2_controller::SimpleGoalChecker checker; + 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; + path.poses[3].pose.position.x = 0.20; + path.poses[3].pose.position.y = 0.20; + path.poses[3].header.frame_id = "base_link"; + + // this should make the goal checker to validated the fact that the robot is in range + // of the goal. The rotation shim controller should rotate toward the goal heading + // then it will throw an exception because the costmap is bogus + controller->setPlan(path); + EXPECT_THROW(controller->computeVelocityCommands(pose, velocity, &checker), std::runtime_error); +} + TEST(RotationShimControllerTest, testDynamicParameter) { auto node = std::make_shared("ShimControllerTest"); @@ -338,7 +399,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 +411,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 5512f763dd..7fb66af57a 100644 --- a/nav2_rviz_plugins/package.xml +++ b/nav2_rviz_plugins/package.xml @@ -2,7 +2,7 @@ nav2_rviz_plugins - 1.2.8 + 1.2.9 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 e6f908ce3d..f6c056fdd4 100644 --- a/nav2_simple_commander/package.xml +++ b/nav2_simple_commander/package.xml @@ -2,7 +2,7 @@ nav2_simple_commander - 1.2.8 + 1.2.9 An importable library for writing mobile robot applications in python3 steve Apache-2.0 diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml index e8c3d43625..c0b088c03c 100644 --- a/nav2_smac_planner/package.xml +++ b/nav2_smac_planner/package.xml @@ -2,7 +2,7 @@ nav2_smac_planner - 1.2.8 + 1.2.9 Smac global planning plugin: A*, Hybrid-A*, State Lattice Steve Macenski Apache-2.0 diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index 1fea5a11bc..93842a507e 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -2,7 +2,7 @@ nav2_smoother - 1.2.8 + 1.2.9 Smoother action interface Matej Vargovcik Steve Macenski diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 105446946a..87d6024c08 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -2,7 +2,7 @@ nav2_system_tests - 1.2.8 + 1.2.9 TODO Carlos Orduno Apache-2.0 diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml index c945c59494..167441d394 100644 --- a/nav2_theta_star_planner/package.xml +++ b/nav2_theta_star_planner/package.xml @@ -2,7 +2,7 @@ nav2_theta_star_planner - 1.2.8 + 1.2.9 Theta* Global Planning Plugin Steve Macenski Anshumaan Singh diff --git a/nav2_util/package.xml b/nav2_util/package.xml index 544360d5ad..3e922c515e 100644 --- a/nav2_util/package.xml +++ b/nav2_util/package.xml @@ -2,7 +2,7 @@ nav2_util - 1.2.8 + 1.2.9 TODO Michael Jeronimo Mohammad Haghighipanah diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml index 9928a77022..17def7f4b6 100644 --- a/nav2_velocity_smoother/package.xml +++ b/nav2_velocity_smoother/package.xml @@ -2,7 +2,7 @@ nav2_velocity_smoother - 1.2.8 + 1.2.9 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 1f602689c9..0b67699b73 100644 --- a/nav2_voxel_grid/package.xml +++ b/nav2_voxel_grid/package.xml @@ -2,7 +2,7 @@ nav2_voxel_grid - 1.2.8 + 1.2.9 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 d53e79afed..e96d215fcf 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -2,7 +2,7 @@ nav2_waypoint_follower - 1.2.8 + 1.2.9 A waypoint follower navigation server Steve Macenski Apache-2.0 diff --git a/navigation2/package.xml b/navigation2/package.xml index 9a0d7012b3..a4b681828f 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -2,7 +2,7 @@ navigation2 - 1.2.8 + 1.2.9 ROS2 Navigation Stack