Integrate joint limit enforcement into ros2_control framework functional with Async controllers and components (ros-controls#2047)

Author: saikishor

controller_manager/doc/userdoc.rst

@@ -79,6 +79,11 @@ robot_description [std_msgs::msg::String]
 Parameters
 -----------
 
+enforce_command_limits (optional; bool; default: ``false`` for Jazzy and earlier distro and ``true`` for Rolling and newer distros)
+  Enforces the joint limits to the joint command interfaces.
+  If the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits in the URDF.
+  If the command is within the limits, the command is passed through without any changes.
+
 <controller_name>.type
   Name of a plugin exported using ``pluginlib`` for a controller.
   This is a class from which controller's instance with name "``controller_name``" is created.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -657,6 +657,7 @@ class ControllerManager : public rclcpp::Node
   std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
+  bool enforce_command_limits_;
 
   controller_manager::MovingAverageStatistics periodicity_stats_;
 

controller_manager/src/controller_manager.cpp

@@ -438,7 +438,8 @@ ControllerManager::ControllerManager(
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
-  cm_node_options_(options)
+  cm_node_options_(options),
+  robot_description_(resource_manager_->get_robot_description())
 {
   initialize_parameters();
   init_controller_manager();
@@ -498,6 +499,7 @@ 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_);
     init_services();
   }
   else
@@ -559,6 +561,13 @@ void ControllerManager::init_controller_manager()
         }
         RCLCPP_INFO(get_logger(), "Shutting down the controller manager.");
       }));
+
+  // Declare the enforce_command_limits parameter such a way that it is enabled by default for
+  // rolling and newer alone
+  enforce_command_limits_ =
+    this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false);
+  RCLCPP_INFO_EXPRESSION(
+    get_logger(), enforce_command_limits_, "Enforcing command limits is enabled...");
 }
 
 void ControllerManager::initialize_parameters()
@@ -612,6 +621,7 @@ 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 (!resource_manager_->load_and_initialize_components(robot_description, update_rate_))
   {
     RCLCPP_WARN(
@@ -2871,6 +2881,11 @@ controller_interface::return_type ControllerManager::update(
     publish_activity();
   }
 
+  if (enforce_command_limits_)
+  {
+    resource_manager_->enforce_command_limits(period);
+  }
+
   // there are controllers to (de)activate
   if (switch_params_.do_switch)
   {

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -66,6 +66,12 @@ class TestControllerManagerWithTestableCM
   public testing::WithParamInterface<Strictness>
 {
 public:
+  TestControllerManagerWithTestableCM()
+  : ControllerManagerFixture<TestableControllerManager>(
+      ros2_control_test_assets::minimal_robot_urdf_no_limits)
+  {
+  }
+
   void SetupAndConfigureControllers(int strictness)
   {
     test_controller_actuator = std::make_shared<test_controller::TestController>();

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <memory>
+#include <regex>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -106,9 +107,13 @@ class TestControllerChainingWithControllerManager
   void SetUp()
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    const std::regex velocity_pattern(R"(velocity\s*=\s*"-?[0-9]+(\.[0-9]+)?")");
+    const std::string velocity_replacement = R"(velocity="10000.0")";
+    const std::string diffbot_urdf_large_limits = std::regex_replace(
+      ros2_control_test_assets::diffbot_urdf, velocity_pattern, velocity_replacement);
     cm_ = std::make_shared<TestableControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(),
+        diffbot_urdf_large_limits, rm_node_->get_node_clock_interface(),
         rm_node_->get_node_logging_interface(), true),
       executor_, TEST_CM_NAME);
     run_updater_ = false;

doc/release_notes.rst

@@ -90,6 +90,7 @@ controller_manager
 * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 <https://github.com/ros-controls/ros2_control/pull/1915>`_).
 * The ``pal_statistics`` is now integrated into the controller_manager, so that the controllers, hardware components and the controller_manager can be easily introspected and monitored using the topics ``~/introspection_data/names`` and ``~/introspection_data/values`` (`#1918 <https://github.com/ros-controls/ros2_control/pull/1918>`_).
 * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 <https://github.com/ros-controls/ros2_control/pull/1955>`_).
+* A new parameter ``enforce_command_limits`` is introduced to be able to enable and disable the enforcement of the command limits (`#1989 <https://github.com/ros-controls/ros2_control/pull/1989>`_).
 * A latched topic ``~/activity`` has been added to the controller_manager to publish the activity of the controller_manager, where the change in states of the controllers and the hardware components are published. (`#2006 <https://github.com/ros-controls/ros2_control/pull/2006>`_).
 * The controller manager will use a monotonic clock for triggering read-update-write cycles, but when the ``use_sim_time`` parameter is set to true, it will use the ROS Clock for triggering. When monotonic clock is being used, all the hardware components will receive the monotonic time in their read and write method, instead the controllers will always receive the ROS time in their update method irrespective of the clock being used. (`#2046 <https://github.com/ros-controls/ros2_control/pull/2046>`_).
 

hardware_interface/include/hardware_interface/handle.hpp

@@ -16,6 +16,8 @@
 #define HARDWARE_INTERFACE__HANDLE_HPP_
 
 #include <algorithm>
+#include <atomic>
+#include <functional>
 #include <limits>
 #include <memory>
 #include <mutex>
@@ -379,13 +381,40 @@ class CommandInterface : public Handle
 
   CommandInterface(CommandInterface && other) = default;
 
+  void set_on_set_command_limiter(std::function<double(double, bool &)> on_set_command_limiter)
+  {
+    on_set_command_limiter_ = on_set_command_limiter;
+  }
+
+  /// A setter for the value of the command interface that triggers the limiter.
+  /**
+   * @param value The value to be set.
+   * @return True if the value was set successfully, false otherwise.
+   */
+  template <typename T>
+  [[nodiscard]] bool set_limited_value(const T & value)
+  {
+    if constexpr (std::is_same_v<T, double>)
+    {
+      return set_value(on_set_command_limiter_(value, is_command_limited_));
+    }
+    else
+    {
+      return set_value(value);
+    }
+  }
+
+  const bool & is_limited() const { return is_command_limited_; }
+
   void registerIntrospection() const
   {
     if (value_ptr_ || std::holds_alternative<double>(value_))
     {
       std::function<double()> f = [this]()
       { return value_ptr_ ? *value_ptr_ : std::get<double>(value_); };
       DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name(), f);
+      DEFAULT_REGISTER_ROS2_CONTROL_INTROSPECTION(
+        "command_interface." + get_name() + ".is_limited", &is_command_limited_);
     }
   }
 
@@ -394,12 +423,23 @@ class CommandInterface : public Handle
     if (value_ptr_ || std::holds_alternative<double>(value_))
     {
       DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION("command_interface." + get_name());
+      DEFAULT_UNREGISTER_ROS2_CONTROL_INTROSPECTION(
+        "command_interface." + get_name() + ".is_limited");
     }
   }
 
   using Handle::Handle;
 
   using SharedPtr = std::shared_ptr<CommandInterface>;
+
+private:
+  bool is_command_limited_ = false;
+  std::function<double(double, bool &)> on_set_command_limiter_ =
+    [](double value, bool & is_limited)
+  {
+    is_limited = false;
+    return value;
+  };
 };
 
 }  // namespace hardware_interface

hardware_interface/include/hardware_interface/loaned_command_interface.hpp

@@ -114,7 +114,7 @@ class LoanedCommandInterface
   {
     unsigned int nr_tries = 0;
     ++set_value_statistics_.total_counter;
-    while (!command_interface_.set_value(value))
+    while (!command_interface_.set_limited_value(value))
     {
       ++set_value_statistics_.failed_counter;
       ++nr_tries;

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -119,6 +119,12 @@ class ResourceManager
   virtual bool load_and_initialize_components(
     const std::string & urdf, const unsigned int update_rate = 100);
 
+  /**
+   * @brief Import joint limiters from the URDF.
+   * @param urdf string containing the URDF.
+   */
+  void import_joint_limiters(const std::string & urdf);
+
   /**
    * @brief if the resource manager load_and_initialize_components(...) function has been called
    * this returns true. We want to permit to loading the urdf later on, but we currently don't want
@@ -456,6 +462,14 @@ class ResourceManager
   return_type set_component_state(
     const std::string & component_name, rclcpp_lifecycle::State & target_state);
 
+  /**
+   * Enforce the command limits for the position, velocity, effort, and acceleration interfaces.
+   * @note This method is RT-safe
+   * @return true if the command interfaces are out of limits and the limits are enforced
+   * @return false if the command interfaces values are within limits
+   */
+  bool enforce_command_limits(const rclcpp::Duration & period);
+
   /// Reads all loaded hardware components.
   /**
    * Reads from all active hardware components.
@@ -493,6 +507,8 @@ class ResourceManager
    */
   void set_on_component_state_switch_callback(std::function<void()> callback);
 
+  const std::string & get_robot_description() const;
+
 protected:
   /// Gets the logger for the resource manager
   /**