Merge branch 'master' into touchup

Author: christophfroehlich

controller_interface/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.28.1 (2025-04-17)
+-------------------
+
 4.28.0 (2025-04-10)
 -------------------
 * Make all packages use gmock, not gtest (`#2162 <https://github.com/ros-controls/ros2_control/issues/2162>`_)

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.28.0</version>
+  <version>4.28.1</version>
   <description>Base classes for controllers and syntax cookies for supporting common sensor types in controllers and broadcasters</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.28.1 (2025-04-17)
+-------------------
+* Fix the enforce_command_limits deactivation (`#2181 <https://github.com/ros-controls/ros2_control/issues/2181>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.28.0 (2025-04-10)
 -------------------
 * Integrate joint limit enforcement into `ros2_control` framework functional with Async controllers and components  (`#2047 <https://github.com/ros-controls/ros2_control/issues/2047>`_)

controller_manager/doc/controller_chaining.rst

@@ -48,8 +48,12 @@ This means the following:
 
 .. note::
 
-  Only the controllers that exposes the reference interfaces are switched to chained mode, when their reference interfaces are used by other controllers. When their reference interfaces are not used by the other controllers, they are switched back to get references from the subscriber.
-  However, the controllers that exposes the state interfaces are not triggered to chained mode, when their state interfaces are used by other controllers.
+  Controllers that expose the reference interfaces are switched to chained mode only when their reference interfaces are used by other controllers. When their reference interfaces are not used by other controllers, they are switched back to get references from the subscriber.
+  However, the controllers that expose the state interfaces are not triggered to chained mode when their state interfaces are used by other controllers.
+
+.. note::
+
+  This document uses terms *preceding* and *following* controller. These terms refer to such ordering of controllers that controller A *precedes* controller B if A claims (*connects its output to*) B's reference interfaces (*inputs*). In the example diagram at the beginning of this section, 'diff_drive_controller' *precedes* 'pid left wheel' and 'pid right wheel'. Consequently, 'pid left wheel' and 'pid right wheel' are controllers *following* after 'diff_drive_controller'.
 
 Implementation
 --------------

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.28.0</version>
+  <version>4.28.1</version>
   <description>The main runnable entrypoint of ros2_control and home of controller management and resource management.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

controller_manager/src/controller_manager.cpp

@@ -499,7 +499,10 @@ void ControllerManager::init_controller_manager()
   // Get parameters needed for RT "update" loop to work
   if (is_resource_manager_initialized())
   {
-    resource_manager_->import_joint_limiters(robot_description_);
+    if (enforce_command_limits_)
+    {
+      resource_manager_->import_joint_limiters(robot_description_);
+    }
     init_services();
   }
   else
@@ -621,7 +624,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
 
 void ControllerManager::init_resource_manager(const std::string & robot_description)
 {
-  resource_manager_->import_joint_limiters(robot_description_);
+  if (enforce_command_limits_)
+  {
+    resource_manager_->import_joint_limiters(robot_description_);
+  }
   if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
   {
     RCLCPP_WARN(
@@ -2880,11 +2886,7 @@ controller_interface::return_type ControllerManager::update(
     // To publish the activity of the failing controllers and the fallback controllers
     publish_activity();
   }
-
-  if (enforce_command_limits_)
-  {
-    resource_manager_->enforce_command_limits(period);
-  }
+  resource_manager_->enforce_command_limits(period);
 
   // there are controllers to (de)activate
   if (switch_params_.do_switch)

controller_manager_msgs/CHANGELOG.rst

@@ -2,6 +2,9 @@
 Changelog for package controller_manager_msgs
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.28.1 (2025-04-17)
+-------------------
+
 4.28.0 (2025-04-10)
 -------------------
 * [CM] Extend the list controller and hardware components messages (`#2102 <https://github.com/ros-controls/ros2_control/issues/2102>`_)

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.28.0</version>
+  <version>4.28.1</version>
   <description>Messages and services for the controller manager.</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

doc/introspection.rst

@@ -70,12 +70,19 @@ Data Visualization
 
 Data can be visualized with any tools that display ROS topics, but we recommend `PlotJuggler <https://plotjuggler.io/>`_ for viewing high resolution live data, or data in bags.
 
-1. Open ``PlotJuggler`` running ``ros2 run plotjuggler plotjuggler``.
+1. Open ``PlotJuggler`` by running ``ros2 run plotjuggler plotjuggler`` from the command line.
+
    .. image:: images/plotjuggler.png
-2. Visualize the data:
-   - Importing from the ros2bag
-   - Subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header.
+      :alt: PlotJuggler
+
+2. Visualize the data by importing from the ros2bag file or subscribing to the ROS2 topics live with the ``ROS2 Topic Subscriber`` option under ``Streaming`` header.
+
 3. Choose the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` from the popup window.
+
    .. image:: images/plotjuggler_select_topics.png
-4. Now, select the variables that are of your interest and drag them to the plot.
+      :alt: PlotJuggler Select Topics
+
+4. Then, select the variables that are of your interest and drag them to the plot.
+
    .. image:: images/plotjuggler_visualizing_data.png
+      :alt: PlotJuggler Visualizing Data

hardware_interface/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.28.1 (2025-04-17)
+-------------------
+* Avoid running joint limit enforcement during initialization (`#2188 <https://github.com/ros-controls/ros2_control/issues/2188>`_)
+* Use previous command to enforce the joint limits on position interfaces (`#2183 <https://github.com/ros-controls/ros2_control/issues/2183>`_)
+* Add log info for the type of joint limits being used (`#2186 <https://github.com/ros-controls/ros2_control/issues/2186>`_)
+* Fix the joint limits enforcement with `position` and `velocity` (`#2182 <https://github.com/ros-controls/ros2_control/issues/2182>`_)
+* Only log limiting error if something is limited. (`#2176 <https://github.com/ros-controls/ros2_control/issues/2176>`_)
+* Contributors: Felix Exner (fexner), Sai Kishor Kothakota
+
 4.28.0 (2025-04-10)
 -------------------
 * [HW Interface] Use new handle API inside the hardware components (`#2092 <https://github.com/ros-controls/ros2_control/issues/2092>`_)

hardware_interface/include/hardware_interface/handle.hpp

@@ -319,7 +319,7 @@ class Handle
     // END
   }
 
-  std::shared_mutex & get_mutex() { return handle_mutex_; }
+  std::shared_mutex & get_mutex() const { return handle_mutex_; }
 
   HandleDataType get_data_type() const { return data_type_; }
 

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -527,6 +527,7 @@ class ResourceManager
   mutable std::recursive_mutex resource_interfaces_lock_;
   mutable std::recursive_mutex claimed_command_interfaces_lock_;
   mutable std::recursive_mutex resources_lock_;
+  mutable std::recursive_mutex joint_limiters_lock_;
 
 private:
   bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;

hardware_interface/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface</name>
-  <version>4.28.0</version>
+  <version>4.28.1</version>
   <description>Base classes for hardware abstraction and tooling for them</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

hardware_interface/src/resource_manager.cpp

@@ -693,6 +693,15 @@ class ResourceStorage
         if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end())
         {
           soft_limits = {hw_info.soft_limits.at(joint_name)};
+          RCLCPP_INFO(
+            get_logger(), "Using SoftJointLimiter for joint '%s' in hardware '%s' : '%s'",
+            joint_name.c_str(), hw_info.name.c_str(), soft_limits[0].to_string().c_str());
+        }
+        else
+        {
+          RCLCPP_INFO(
+            get_logger(), "Using JointLimiter for joint '%s' in hardware '%s' : '%s'",
+            joint_name.c_str(), hw_info.name.c_str(), limits.to_string().c_str());
         }
         std::unique_ptr<
           joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>
@@ -731,14 +740,19 @@ class ResourceStorage
       const std::string interface_name = joint_name + "/" + interface_type;
       if (interface_map.find(interface_name) != interface_map.end())
       {
-        // If the command interface is not claimed, then the value is not set
-        if (is_command_itf && !claimed_command_interface_map_.at(interface_name))
+        // If the command interface is not claimed, then the value is not set (or) if the
+        // interface doesn't exist, then value is not set
+        if (
+          is_command_itf && (claimed_command_interface_map_.count(interface_name) == 0 ||
+                             !claimed_command_interface_map_.at(interface_name)))
         {
           value = std::nullopt;
         }
         else
         {
-          value = interface_map.at(interface_name)->get_value();
+          auto itf_handle = interface_map.at(interface_name);
+          std::shared_lock<std::shared_mutex> lock(itf_handle->get_mutex());
+          value = itf_handle->get_optional(lock).value();
         }
       }
     };
@@ -760,7 +774,9 @@ class ResourceStorage
       const std::string interface_name = limited_command.joint_name + "/" + interface_type;
       if (data.has_value() && interface_map.find(interface_name) != interface_map.end())
       {
-        interface_map.at(interface_name)->set_value(data.value());
+        auto itf_handle = interface_map.at(interface_name);
+        std::unique_lock<std::shared_mutex> lock(itf_handle->get_mutex());
+        std::ignore = itf_handle->set_value(lock, data.value());
       }
     };
     // update the command data of the limiters
@@ -948,7 +964,8 @@ class ResourceStorage
               interface_name.c_str());
             continue;
           }
-          const auto limiter_fn = [&, interface_name](double value, bool & is_limited) -> double
+          const auto limiter_fn = [this, joint_name, interface_name, desired_period, &limiters](
+                                    double value, bool & is_limited) -> double
           {
             is_limited = false;
             joint_limits::JointInterfacesCommandLimiterData data;
@@ -976,10 +993,14 @@ class ResourceStorage
             }
             data.limited = data.command;
             is_limited = limiters[joint_name]->enforce(data.actual, data.limited, desired_period);
-            RCLCPP_ERROR_THROTTLE(
-              get_logger(), *rm_clock_, 1000,
-              "Command '%s' for joint '%s' is out of limits. Command limited to %f - %d",
-              interface_name.c_str(), joint_name.c_str(), value, is_limited);
+            if (is_limited)
+            {
+              RCLCPP_ERROR_THROTTLE(
+                get_logger(), *rm_clock_, 1000,
+                "Command of at least one joint is out of limits (throttled log). %s with desired "
+                "period : %f sec.",
+                data.to_string().c_str(), desired_period.seconds());
+            }
             if (
               interface_name == hardware_interface::HW_IF_POSITION &&
               data.limited.position.has_value())
@@ -1428,6 +1449,7 @@ bool ResourceManager::load_and_initialize_components(
 
 void ResourceManager::import_joint_limiters(const std::string & urdf)
 {
+  std::lock_guard<std::recursive_mutex> guard(joint_limiters_lock_);
   const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
   resource_storage_->import_joint_limiters(hardware_info);
 }
@@ -1949,6 +1971,21 @@ bool ResourceManager::perform_command_mode_switch(
   const bool actuators_result = call_perform_mode_switch(resource_storage_->actuators_);
   const bool systems_result = call_perform_mode_switch(resource_storage_->systems_);
 
+  if (actuators_result && systems_result)
+  {
+    // Reset the internals of the joint limiters
+    for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_)
+    {
+      for (const auto & [joint_name, limiter] : limiters)
+      {
+        limiter->reset_internals();
+        RCLCPP_DEBUG(
+          get_logger(), "Resetting internals of joint limiter for joint '%s' in hardware '%s'",
+          joint_name.c_str(), hw_name.c_str());
+      }
+    }
+  }
+
   return actuators_result && systems_result;
 }
 
@@ -2039,6 +2076,12 @@ return_type ResourceManager::set_component_state(
 // CM API: Called in "update"-thread
 bool ResourceManager::enforce_command_limits(const rclcpp::Duration & period)
 {
+  std::unique_lock<std::recursive_mutex> limiters_guard(joint_limiters_lock_, std::try_to_lock);
+  if (!limiters_guard.owns_lock())
+  {
+    return false;
+  }
+
   bool enforce_result = false;
   // Joint Limiters operations
   for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_)

hardware_interface_testing/CHANGELOG.rst

@@ -2,6 +2,11 @@
 Changelog for package hardware_interface_testing
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+4.28.1 (2025-04-17)
+-------------------
+* Use previous command to enforce the joint limits on position interfaces (`#2183 <https://github.com/ros-controls/ros2_control/issues/2183>`_)
+* Contributors: Sai Kishor Kothakota
+
 4.28.0 (2025-04-10)
 -------------------
 * Integrate joint limit enforcement into `ros2_control` framework functional with Async controllers and components  (`#2047 <https://github.com/ros-controls/ros2_control/issues/2047>`_)

hardware_interface_testing/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>hardware_interface_testing</name>
-  <version>4.28.0</version>
+  <version>4.28.1</version>
   <description>Commonly used test fixtures for the ros2_control framework</description>
   <maintainer email="[email protected]">Bence Magyar</maintainer>
   <maintainer email="[email protected]">Denis Štogl</maintainer>

hardware_interface_testing/test/test_components/test_actuator.cpp

@@ -102,15 +102,15 @@ class TestActuator : public ActuatorInterface
     const std::vector<std::string> & /*start_interfaces*/,
     const std::vector<std::string> & /*stop_interfaces*/) override
   {
-    position_state_ += 1.0;
+    position_state_ += 0.001;
     return hardware_interface::return_type::OK;
   }
 
   hardware_interface::return_type perform_command_mode_switch(
     const std::vector<std::string> & /*start_interfaces*/,
     const std::vector<std::string> & /*stop_interfaces*/) override
   {
-    position_state_ += 100.0;
+    position_state_ += 0.1;
     return hardware_interface::return_type::OK;
   }