From 3884d39d74487af84e753df5dda5333c22ac729f Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Thu, 30 May 2024 17:59:28 -0700
Subject: [PATCH] Offcycle Iron Sync 7: May 30, 2024 (#4383)
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 <joni.pollanen@karelics.fi>
* [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 <gennart.antoine@gmail.com>
* bump to 1.2.9 for release
---------
Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi>
Signed-off-by: Antoine Gennart <gennart.antoine@gmail.com>
Co-authored-by: Joni Pöllänen <jonipol@users.noreply.github.com>
Co-authored-by: Saitama <gennartan@users.noreply.github.com>
---
nav2_amcl/package.xml | 2 +-
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/package.xml | 2 +-
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 +-
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 +-
.../lifecycle_manager.hpp | 10 +++
.../lifecycle_manager_client.hpp | 10 +++
nav2_lifecycle_manager/package.xml | 2 +-
.../src/lifecycle_manager.cpp | 34 ++++++++++
.../src/lifecycle_manager_client.cpp | 12 ++++
.../test/test_lifecycle_manager.cpp | 2 +
nav2_map_server/package.xml | 2 +-
nav2_mppi_controller/package.xml | 2 +-
nav2_msgs/package.xml | 2 +-
nav2_msgs/srv/ManageLifecycleNodes.srv | 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 | 65 ++++++++++++++++++-
nav2_rviz_plugins/package.xml | 2 +-
nav2_simple_commander/package.xml | 2 +-
nav2_smac_planner/package.xml | 2 +-
nav2_smoother/package.xml | 2 +-
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 +-
49 files changed, 299 insertions(+), 39 deletions(-)
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 27d1e917c70..3effde7d189 100644
--- 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
index 4354d1fc946..48c8bd24176 100644
--- 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
index 51b422dbc62..9f2b9ad3ff4 100644
--- 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
index d9ac165a605..5bf57af203f 100644
--- 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
index 308a0ff0ad0..4e708a9e396 100644
--- 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
index 9c493ffac6a..f59f2f77381 100644
--- 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
index 19aa6d0bd15..615d8ce78ac 100644
--- 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
index 8c9c87484c4..172572b73ad 100644
--- 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
index 2d9416db329..4ebb396c7d4 100644
--- 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
index 535be634c6f..2e6a02f6483 100644
--- 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
index a49554cc440..fd720fd24fd 100644
--- 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
index 07081954314..83c0619c522 100644
--- 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
index 0c1d9c2257e..61e4679cdfb 100644
--- 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
index ad4b5f0fec8..491ee4ecb76 100644
--- 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
index 9ada111110d..a987b73763c 100644
--- 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
index b0b9eb77b1f..44497477362 100644
--- 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
index b7005a361a2..f0e5f2832bf 100644
--- 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
index 15c5520368f..5e0a9e990c0 100644
--- 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
index ff94df6cb32..305ff7b75e1 100644
--- 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
index 8d302907c4e..0728df7d071 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 076e848902c..80e9305e9e0 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 e222d526e84..85258fe2306 100644
--- 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
index cea3a182602..e03dfd4701f 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 632eb1aa8e6..123c69b58eb 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 96114f98a57..c2c0cb75f0b 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 98e08c8b80f..a7ba7c62815 100644
--- 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
index ff0683cd403..33c9af4085c 100644
--- 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
index 2bfd8b716f7..120a592e71a 100644
--- 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
index 8f37cbf819b..bb84f3e231a 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 70f129d0adf..6fcdf170dbf 100644
--- 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
index 667dac75c56..ed091d614b7 100644
--- 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
index 047e5dc9024..2b2a6ff96b1 100644
--- 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
index 9129e9af619..6802ab7dc42 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 c3812ec1d3c..6d4e8b86b33 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 00000000000..0a4ff4ac163
--- /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 cd9a5400843..59bc9b0fda9 100644
--- a/nav2_rotation_shim_controller/package.xml
+++ b/nav2_rotation_shim_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_rotation_shim_controller</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>Rotation Shim Controller</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
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 5097a1fc9cd..dde73151fda 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 <utility>
#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<std::mutex> 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<nav2_costmap_2d::Costmap2D::mutex_t> 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<rclcpp::Parameter>
} 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 e1bc30f80ec..307f4385d90 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<RotationShimShim>();
+ auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("ShimControllerTest");
+ std::string name = "PathFollower";
+ auto tf = std::make_shared<tf2_ros::Buffer>(node->get_clock());
+ auto listener = std::make_shared<tf2_ros::TransformListener>(*tf, node, true);
+ auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("fake_costmap");
+ rclcpp_lifecycle::State state;
+ costmap->on_configure(state);
+ auto tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(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<RotationShimShim>();
+ 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<rclcpp_lifecycle::LifecycleNode>("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 5512f763dde..7fb66af57ad 100644
--- a/nav2_rviz_plugins/package.xml
+++ b/nav2_rviz_plugins/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_rviz_plugins</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>Navigation 2 plugins for rviz</description>
<maintainer email="michael.jeronimo@intel.com">Michael Jeronimo</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_simple_commander/package.xml b/nav2_simple_commander/package.xml
index e6f908ce3d1..f6c056fdd4f 100644
--- a/nav2_simple_commander/package.xml
+++ b/nav2_simple_commander/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_simple_commander</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>An importable library for writing mobile robot applications in python3</description>
<maintainer email="stevenmacenski@gmail.com">steve</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_smac_planner/package.xml b/nav2_smac_planner/package.xml
index e8c3d43625a..c0b088c03ce 100644
--- a/nav2_smac_planner/package.xml
+++ b/nav2_smac_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_smac_planner</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>Smac global planning plugin: A*, Hybrid-A*, State Lattice</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml
index 1fea5a11bc6..93842a507e8 100644
--- a/nav2_smoother/package.xml
+++ b/nav2_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_smoother</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>Smoother action interface</description>
<maintainer email="vargovcik@robotechvision.com">Matej Vargovcik</maintainer>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index 105446946ae..87d6024c083 100644
--- a/nav2_system_tests/package.xml
+++ b/nav2_system_tests/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_system_tests</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>TODO</description>
<maintainer email="carlos.a.orduno@intel.com">Carlos Orduno</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_theta_star_planner/package.xml b/nav2_theta_star_planner/package.xml
index c945c59494f..167441d3945 100644
--- a/nav2_theta_star_planner/package.xml
+++ b/nav2_theta_star_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_theta_star_planner</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>Theta* Global Planning Plugin</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<maintainer email="anshumaan567@gmail.com">Anshumaan Singh</maintainer>
diff --git a/nav2_util/package.xml b/nav2_util/package.xml
index 544360d5ad3..3e922c515ef 100644
--- a/nav2_util/package.xml
+++ b/nav2_util/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_util</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="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
diff --git a/nav2_velocity_smoother/package.xml b/nav2_velocity_smoother/package.xml
index 9928a77022f..17def7f4b6d 100644
--- a/nav2_velocity_smoother/package.xml
+++ b/nav2_velocity_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_velocity_smoother</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>Nav2's Output velocity smoother</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
diff --git a/nav2_voxel_grid/package.xml b/nav2_voxel_grid/package.xml
index 1f602689c97..0b67699b733 100644
--- a/nav2_voxel_grid/package.xml
+++ b/nav2_voxel_grid/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_voxel_grid</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>
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.
</description>
diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml
index d53e79afed1..e96d215fcf7 100644
--- a/nav2_waypoint_follower/package.xml
+++ b/nav2_waypoint_follower/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_waypoint_follower</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>A waypoint follower navigation server</description>
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
<license>Apache-2.0</license>
diff --git a/navigation2/package.xml b/navigation2/package.xml
index 9a0d7012b36..a4b681828fc 100644
--- a/navigation2/package.xml
+++ b/navigation2/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>navigation2</name>
- <version>1.2.8</version>
+ <version>1.2.9</version>
<description>
ROS2 Navigation Stack
</description>