ros-navigation · SteveMacenski · May 31, 2024 · May 28, 2024 · May 29, 2024 · May 31, 2024
diff --git a/nav2_amcl/package.xml b/nav2_amcl/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_amcl</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>
     <p>
       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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behavior_tree</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_behaviors</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>TODO</description>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bringup</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Bringup scripts and configurations for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_bt_navigator</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>TODO</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_collision_monitor</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Collision Monitor</description>
   <maintainer email="alexey.merzlyakov@samsung.com">Alexey Merzlyakov</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_common/package.xml b/nav2_common/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_common</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Common support functionality used throughout the navigation 2 stack</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_constrained_smoother</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Ceres constrained smoother</description>
   <maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_controller</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Controller action interface</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_core/package.xml b/nav2_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_core</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>A set of headers for plugins core to the Nav2 stack</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>

diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format ="3">
   <name>nav2_costmap_2d</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>
     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
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>costmap_queue</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>The costmap_queue package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_core/package.xml b/nav2_dwb_controller/dwb_core/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_core</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>TODO</description>
   <maintainer email="carl.r.delsey@intel.com">Carl Delsey</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_critics/package.xml b/nav2_dwb_controller/dwb_critics/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_critics</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>The dwb_critics package</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_msgs/package.xml b/nav2_dwb_controller/dwb_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dwb_msgs</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Message/Service definitions specifically for the dwb_core</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/dwb_plugins/package.xml b/nav2_dwb_controller/dwb_plugins/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>dwb_plugins</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>
       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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_dwb_controller</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>
     ROS2 controller (DWB) metapackage
   </description>

diff --git a/nav2_dwb_controller/nav_2d_msgs/package.xml b/nav2_dwb_controller/nav_2d_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_msgs</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Basic message types for two dimensional navigation, extending from geometry_msgs::Pose2D.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git a/nav2_dwb_controller/nav_2d_utils/package.xml b/nav2_dwb_controller/nav_2d_utils/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav_2d_utils</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>A handful of useful utility functions for nav_2d packages.</description>
   <maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
   <license>BSD-3-Clause</license>

diff --git 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
@@ -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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_lifecycle_manager</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>A controller/manager for the lifecycle nodes of the Navigation 2 system</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <license>Apache-2.0</license>

diff --git 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
@@ -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
@@ -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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_map_server</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>
     Refactored map server for ROS2 Navigation
   </description>

diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_mppi_controller</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>nav2_mppi_controller</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="budyakov.aleksei@gmail.com">Aleksei Budyakov</maintainer>

diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_msgs</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Messages and service files for the Nav2 stack</description>
   <maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>

diff --git 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
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_navfn_planner</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>TODO</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>

diff --git a/nav2_planner/package.xml b/nav2_planner/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_planner</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>TODO</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <license>Apache-2.0</license>

diff --git a/nav2_regulated_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>nav2_regulated_pure_pursuit_controller</name>
-  <version>1.2.8</version>
+  <version>1.2.9</version>
   <description>Regulated Pure Pursuit Controller</description>
   <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
   <maintainer email="shrijitsingh99@gmail.com">Shrijit Singh</maintainer>

diff --git 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/...n_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/...n_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_;