Offcycle Iron Sync 7: May 30, 2024 (#4383)

* Add configure and cleanup transitions to lifecycle manager and client (#4371) Signed-off-by: Joni Pöllänen <[email protected]> * [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 <[email protected]> * bump to 1.2.9 for release --------- Signed-off-by: Joni Pöllänen <[email protected]> Signed-off-by: Antoine Gennart <[email protected]> Co-authored-by: Joni Pöllänen <[email protected]> Co-authored-by: Saitama <[email protected]>
ros-navigation · May 31, 2024 · 3884d39 · 3884d39
1 parent ff88c48
commit 3884d39
Show file tree

Hide file tree

Showing 49 changed files with 299 additions and 39 deletions.
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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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="[email protected]">Carlos Orduno</maintainer>
   <maintainer email="[email protected]">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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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="[email protected]">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="[email protected]">Alexey Merzlyakov</maintainer>
   <maintainer email="[email protected]">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="[email protected]">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="[email protected]">Matej Vargovcik</maintainer>
   <maintainer email="[email protected]">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="[email protected]">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="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">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="[email protected]">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="[email protected]">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="[email protected]">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="[email protected]">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="[email protected]">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="[email protected]">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="[email protected]">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="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">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="[email protected]">Michael Jeronimo</maintainer>
   <maintainer email="[email protected]">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="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">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="[email protected]">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="[email protected]">Steve Macenski</maintainer>
   <maintainer email="[email protected]">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_;