Merge branch 'master' into integrate/pal_statistics

ros-controls · Jan 29, 2025 · 9b750b1 · 9b750b1
2 parents a89b26e + 3755e03
commit 9b750b1
Show file tree

Hide file tree

Showing 30 changed files with 162 additions and 17 deletions.
diff --git a/controller_interface/CHANGELOG.rst b/controller_interface/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+* Use `target_compile_definitions` instead of installing test files (`#2009 <https://github.com/ros-controls/ros2_control/issues/2009>`_)
+* Add GPS semantic component (`#2000 <https://github.com/ros-controls/ros2_control/issues/2000>`_)
+* Contributors: Sai Kishor Kothakota, Wiktor Bajor
+
 4.24.0 (2025-01-13)
 -------------------
 * Trigger shutdown transition in destructor (`#1979 <https://github.com/ros-controls/ros2_control/issues/1979>`_)

diff --git a/controller_interface/package.xml b/controller_interface/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>Description of controller_interface</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/controller_manager/CHANGELOG.rst b/controller_manager/CHANGELOG.rst
@@ -2,6 +2,16 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+* Handle SIGINT properly in the controller manager (`#2014 <https://github.com/ros-controls/ros2_control/issues/2014>`_)
+* Fix the initial wrong periodicity reported by controller_manager (`#2018 <https://github.com/ros-controls/ros2_control/issues/2018>`_)
+* Use `target_compile_definitions` instead of installing test files (`#2009 <https://github.com/ros-controls/ros2_control/issues/2009>`_)
+* Fix a heading level (`#2007 <https://github.com/ros-controls/ros2_control/issues/2007>`_)
+* Update path of GPL (`#1994 <https://github.com/ros-controls/ros2_control/issues/1994>`_)
+* Fix: on_shutdown callback of controllers never get executed (`#1995 <https://github.com/ros-controls/ros2_control/issues/1995>`_)
+* Contributors: Christoph Fröhlich, Sai Kishor Kothakota, Wiktor Bajor
+
 4.24.0 (2025-01-13)
 -------------------
 * [CM] Remove obsolete ControllerMock from the tests (`#1990 <https://github.com/ros-controls/ros2_control/issues/1990>`_)

diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -82,6 +82,12 @@ class ControllerManager : public rclcpp::Node
 
   virtual ~ControllerManager();
 
+  /// Shutdown all controllers in the controller manager.
+  /**
+   * \return true if all controllers are successfully shutdown, false otherwise.
+   */
+  bool shutdown_controllers();
+
   void robot_description_callback(const std_msgs::msg::String & msg);
 
   void init_resource_manager(const std::string & robot_description);
@@ -531,6 +537,7 @@ class ControllerManager : public rclcpp::Node
     int used_by_realtime_controllers_index_ = -1;
   };
 
+  std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
   RTControllerListWrapper rt_controllers_wrapper_;
   std::unordered_map<std::string, ControllerChainSpec> controller_chain_spec_;
   std::vector<std::string> ordered_controllers_names_;

diff --git a/controller_manager/package.xml b/controller_manager/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_manager</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>Description of controller_manager</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -292,7 +292,46 @@ ControllerManager::ControllerManager(
   init_controller_manager();
 }
 
-ControllerManager::~ControllerManager() { CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES(); }
+ControllerManager::~ControllerManager()
+{
+  CLEAR_ALL_ROS2_CONTROL_INTROSPECTION_REGISTRIES();
+  if (preshutdown_cb_handle_)
+  {
+    rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+    context->remove_pre_shutdown_callback(*(preshutdown_cb_handle_.get()));
+    preshutdown_cb_handle_.reset();
+  }
+}
+
+bool ControllerManager::shutdown_controllers()
+{
+  RCLCPP_INFO(get_logger(), "Shutting down all controllers in the controller manager.");
+  // Shutdown all controllers
+  std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
+  std::vector<ControllerSpec> controllers_list = rt_controllers_wrapper_.get_updated_list(guard);
+  bool ctrls_shutdown_status = true;
+  for (auto & controller : controllers_list)
+  {
+    if (is_controller_active(controller.c))
+    {
+      RCLCPP_INFO(
+        get_logger(), "Deactivating controller '%s'", controller.c->get_node()->get_name());
+      controller.c->get_node()->deactivate();
+      controller.c->release_interfaces();
+    }
+    if (is_controller_inactive(*controller.c) || is_controller_unconfigured(*controller.c))
+    {
+      RCLCPP_INFO(
+        get_logger(), "Shutting down controller '%s'", controller.c->get_node()->get_name());
+      shutdown_controller(controller);
+    }
+    ctrls_shutdown_status &=
+      (controller.c->get_node()->get_current_state().id() ==
+       lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
+    executor_->remove_node(controller.c->get_node()->get_node_base_interface());
+  }
+  return ctrls_shutdown_status;
+}
 
 void ControllerManager::init_controller_manager()
 {
@@ -332,10 +371,34 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
+
   INITIALIZE_ROS2_CONTROL_INTROSPECTION_REGISTRY(
     this, hardware_interface::DEFAULT_INTROSPECTION_TOPIC,
     hardware_interface::DEFAULT_REGISTRY_KEY);
   START_ROS2_CONTROL_INTROSPECTION_PUBLISHER_THREAD(hardware_interface::DEFAULT_REGISTRY_KEY);
+
+  // Add on_shutdown callback to stop the controller manager
+  rclcpp::Context::SharedPtr context = this->get_node_base_interface()->get_context();
+  preshutdown_cb_handle_ =
+    std::make_unique<rclcpp::PreShutdownCallbackHandle>(context->add_pre_shutdown_callback(
+      [this]()
+      {
+        RCLCPP_INFO(get_logger(), "Shutdown request received....");
+        if (this->get_node_base_interface()->get_associated_with_executor_atomic().load())
+        {
+          executor_->remove_node(this->get_node_base_interface());
+        }
+        executor_->cancel();
+        if (!this->shutdown_controllers())
+        {
+          RCLCPP_ERROR(get_logger(), "Failed shutting down the controllers.");
+        }
+        if (!resource_manager_->shutdown_components())
+        {
+          RCLCPP_ERROR(get_logger(), "Failed shutting down hardware components.");
+        }
+        RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
+      }));
 }
 
 void ControllerManager::initialize_parameters()

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
@@ -93,6 +93,10 @@ int main(int argc, char ** argv)
     }
   }
 
+  // wait for the clock to be available
+  cm->get_clock()->wait_until_started();
+  cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate()));
+
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
   const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
   RCLCPP_INFO(
@@ -122,7 +126,7 @@ int main(int argc, char ** argv)
       auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
       auto const cm_now = std::chrono::nanoseconds(cm->now().nanoseconds());
       std::chrono::time_point<std::chrono::system_clock, std::chrono::nanoseconds>
-        next_iteration_time{cm_now};
+        next_iteration_time{cm_now - period};
 
       // for calculating the measured period of the loop
       rclcpp::Time previous_time = cm->now() - period;

diff --git a/controller_manager_msgs/CHANGELOG.rst b/controller_manager_msgs/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+
 4.24.0 (2025-01-13)
 -------------------
 

diff --git a/controller_manager_msgs/package.xml b/controller_manager_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>controller_manager_msgs</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>Messages and services for the controller manager.</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/hardware_interface/CHANGELOG.rst b/hardware_interface/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+* Handle SIGINT properly in the controller manager (`#2014 <https://github.com/ros-controls/ros2_control/issues/2014>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.24.0 (2025-01-13)
 -------------------
 * Add missing link of mock_components to hardware_interface (`#1992 <https://github.com/ros-controls/ros2_control/issues/1992>`_)

diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -76,6 +76,13 @@ class ResourceManager
 
   virtual ~ResourceManager();
 
+  /// Shutdown all hardware components, irrespective of their state.
+  /**
+   * The method is called when the controller manager is being shutdown.
+   * @return true if all hardware components are successfully shutdown, false otherwise.
+   */
+  bool shutdown_components();
+
   /// Load resources from on a given URDF.
   /**
    * The resource manager can be post-initialized with a given URDF.

diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>ros2_control hardware interface</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -573,7 +573,7 @@ class ResourceStorage
             result = shutdown_hardware(hardware);
             break;
           case State::PRIMARY_STATE_ACTIVE:
-            result = shutdown_hardware(hardware);
+            result = deactivate_hardware(hardware) && shutdown_hardware(hardware);
             break;
           case State::PRIMARY_STATE_FINALIZED:
             result = true;
@@ -1044,6 +1044,22 @@ ResourceManager::ResourceManager(
   }
 }
 
+bool ResourceManager::shutdown_components()
+{
+  std::unique_lock<std::recursive_mutex> guard(resource_interfaces_lock_);
+  bool shutdown_status = true;
+  for (auto const & hw_info : resource_storage_->hardware_info_map_)
+  {
+    rclcpp_lifecycle::State finalized_state(
+      lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED);
+    if (set_component_state(hw_info.first, finalized_state) != return_type::OK)
+    {
+      shutdown_status = false;
+    }
+  }
+  return shutdown_status;
+}
+
 // CM API: Called in "callback/slow"-thread
 bool ResourceManager::load_and_initialize_components(
   const std::string & urdf, const unsigned int update_rate)

diff --git a/hardware_interface_testing/CHANGELOG.rst b/hardware_interface_testing/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package hardware_interface_testing
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+
 4.24.0 (2025-01-13)
 -------------------
 

diff --git a/hardware_interface_testing/package.xml b/hardware_interface_testing/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface_testing</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>ros2_control hardware interface testing</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/joint_limits/CHANGELOG.rst b/joint_limits/CHANGELOG.rst
@@ -2,6 +2,12 @@
 Changelog for package joint_limits
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+* Define _USE_MATH_DEFINES in joint_soft_limiter.cpp to ensure that M_PI is defined (`#2001 <https://github.com/ros-controls/ros2_control/issues/2001>`_)
+* Use actual position when limiting desired position (`#1988 <https://github.com/ros-controls/ros2_control/issues/1988>`_)
+* Contributors: Sai Kishor Kothakota, Silvio Traversaro
+
 4.24.0 (2025-01-13)
 -------------------
 * Return strong type for joint_limits helpers (`#1981 <https://github.com/ros-controls/ros2_control/issues/1981>`_)

diff --git a/joint_limits/package.xml b/joint_limits/package.xml
@@ -1,6 +1,6 @@
 <package format="3">
   <name>joint_limits</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>Package with interfaces for handling of joint limits in controllers or in hardware. The package also implements Saturation Joint Limiter for position-velocity-acceleration set and other individual interfaces.</description>
 
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

diff --git a/ros2_control.jazzy.repos b/ros2_control.jazzy.repos
@@ -2,7 +2,7 @@ repositories:
   ros-controls/realtime_tools:
     type: git
     url: https://github.com/ros-controls/realtime_tools.git
-    version: master
+    version: jazzy
   ros-controls/control_msgs:
     type: git
     url: https://github.com/ros-controls/control_msgs.git

diff --git a/ros2_control/CHANGELOG.rst b/ros2_control/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package ros2_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+
 4.24.0 (2025-01-13)
 -------------------
 

diff --git a/ros2_control/package.xml b/ros2_control/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>ros2_control</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>Metapackage for ROS2 control related packages</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

diff --git a/ros2_control_test_assets/CHANGELOG.rst b/ros2_control_test_assets/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package ros2_control_test_assets
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+
 4.24.0 (2025-01-13)
 -------------------
 

diff --git a/ros2_control_test_assets/package.xml b/ros2_control_test_assets/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>ros2_control_test_assets</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>The package provides shared test resources for ros2_control stack
   </description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>

diff --git a/ros2controlcli/CHANGELOG.rst b/ros2controlcli/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package ros2controlcli
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+
 4.24.0 (2025-01-13)
 -------------------
 

diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>ros2controlcli</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>
     The ROS 2 command line tools for ROS2 Control.
   </description>

diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py
@@ -19,7 +19,7 @@
 
 setup(
     name=package_name,
-    version="4.24.0",
+    version="4.25.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/" + package_name, ["package.xml"]),

diff --git a/rqt_controller_manager/CHANGELOG.rst b/rqt_controller_manager/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rqt_controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.25.0 (2025-01-29)
+-------------------
+
 4.24.0 (2025-01-13)
 -------------------
 

diff --git a/rqt_controller_manager/package.xml b/rqt_controller_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>rqt_controller_manager</name>
-  <version>4.24.0</version>
+  <version>4.25.0</version>
   <description>Graphical frontend for interacting with the controller manager.</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>