Use monotonic clock for triggering read-update-write cycles + fix for overruns

controller_manager/include/controller_manager/controller_manager.hpp

@@ -210,6 +210,19 @@ class ControllerManager : public rclcpp::Node
    */
   unsigned int get_update_rate() const;
 
+  /// Get the trigger clock of the controller manager.
+  /**
+   * Get the trigger clock of the controller manager.
+   * The method is used to get the clock that is used for triggering the controllers and the
+   * hardware components.
+   *
+   * @note When the controller manager is used with real-time kernel, the trigger clock should be
+   * monotonic and not affected by the system time changes. Otherwise, the clock should be ROS Clock
+   *
+   * \returns trigger clock of the controller manager.
+   */
+  rclcpp::Clock::SharedPtr get_trigger_clock() const;
+
 protected:
   void init_services();
 
@@ -537,6 +550,7 @@ class ControllerManager : public rclcpp::Node
     int used_by_realtime_controllers_index_ = -1;
   };
 
+  rclcpp::Clock::SharedPtr trigger_clock_ = nullptr;
   std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
   RTControllerListWrapper rt_controllers_wrapper_;
   std::unordered_map<std::string, ControllerChainSpec> controller_chain_spec_;

controller_manager/src/controller_manager.cpp

@@ -235,8 +235,6 @@ ControllerManager::ControllerManager(
   std::shared_ptr<rclcpp::Executor> executor, const std::string & manager_node_name,
   const std::string & node_namespace, const rclcpp::NodeOptions & options)
 : rclcpp::Node(manager_node_name, node_namespace, options),
-  resource_manager_(std::make_unique<hardware_interface::ResourceManager>(
-    this->get_node_clock_interface(), this->get_node_logging_interface())),
   diagnostics_updater_(this),
   executor_(executor),
   loader_(std::make_shared<pluginlib::ClassLoader<controller_interface::ControllerInterface>>(
@@ -247,6 +245,8 @@ ControllerManager::ControllerManager(
   cm_node_options_(options)
 {
   initialize_parameters();
+  resource_manager_ =
+    std::make_unique<hardware_interface::ResourceManager>(trigger_clock_, this->get_logger());
   init_controller_manager();
 }
 
@@ -267,8 +267,7 @@ ControllerManager::ControllerManager(
 {
   initialize_parameters();
   resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(
-    urdf, this->get_node_clock_interface(), this->get_node_logging_interface(),
-    activate_all_hw_components, params_->update_rate);
+    urdf, trigger_clock_, this->get_logger(), activate_all_hw_components, params_->update_rate);
   init_controller_manager();
 }
 
@@ -403,6 +402,12 @@ void ControllerManager::initialize_parameters()
       this->get_node_parameters_interface(), this->get_logger());
     params_ = std::make_shared<controller_manager::Params>(cm_param_listener_->get_params());
     update_rate_ = static_cast<unsigned int>(params_->update_rate);
+    trigger_clock_ = realtime_tools::has_realtime_kernel()
+                       ? std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME)
+                       : this->get_clock();
+    RCLCPP_INFO(
+      get_logger(), "Using %s clock for triggering controller manager cycles.",
+      trigger_clock_->get_clock_type() == RCL_STEADY_TIME ? "Steady (Monotonic)" : "ROS");
   }
   catch (const std::exception & e)
   {
@@ -633,8 +638,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
-  controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(
-    0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
+  controller_spec.last_update_cycle_time =
+    std::make_shared<rclcpp::Time>(0, 0, this->get_trigger_clock()->get_clock_type());
   controller_spec.execution_time_statistics = std::make_shared<MovingAverageStatistics>();
   controller_spec.periodicity_statistics = std::make_shared<MovingAverageStatistics>();
 
@@ -1819,7 +1824,7 @@ void ControllerManager::activate_controllers(
     auto controller = found_it->c;
     // reset the last update cycle time for newly activated controllers
     *found_it->last_update_cycle_time =
-      rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
+      rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type());
 
     bool assignment_successful = true;
     // assign command interfaces to the controller
@@ -2489,7 +2494,7 @@ controller_interface::return_type ControllerManager::update(
   // Check for valid time
   if (!get_clock()->started())
   {
-    if (time == rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+    if (time == rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type()))
     {
       throw std::runtime_error(
         "No clock received, and time argument is zero. Check your controller_manager node's "
@@ -2530,10 +2535,10 @@ controller_interface::return_type ControllerManager::update(
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
       bool first_update_cycle = false;
-      const rclcpp::Time current_time = get_clock()->started() ? get_clock()->now() : time;
+      const rclcpp::Time current_time = get_clock()->started() ? get_trigger_clock()->now() : time;
       if (
         *loaded_controller.last_update_cycle_time ==
-        rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+        rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type()))
       {
         // last_update_cycle_time is zero after activation
         first_update_cycle = true;
@@ -2553,8 +2558,7 @@ controller_interface::return_type ControllerManager::update(
       // to keep up with the controller update rate (see issue #1769).
       const bool controller_go =
         run_controller_at_cm_rate ||
-        (time ==
-         rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
+        (time == rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type())) ||
         (controller_actual_period.seconds() * controller_update_rate >= 0.99) || first_update_cycle;
 
       RCLCPP_DEBUG(
@@ -2570,7 +2574,7 @@ controller_interface::return_type ControllerManager::update(
         try
         {
           const auto trigger_result =
-            loaded_controller.c->trigger_update(current_time, controller_actual_period);
+            loaded_controller.c->trigger_update(this->now(), controller_actual_period);
           trigger_status = trigger_result.successful;
           controller_ret = trigger_result.result;
           if (trigger_status && trigger_result.execution_time.has_value())
@@ -2801,6 +2805,8 @@ std::pair<std::string, std::string> ControllerManager::split_command_interface(
 
 unsigned int ControllerManager::get_update_rate() const { return update_rate_; }
 
+rclcpp::Clock::SharedPtr ControllerManager::get_trigger_clock() const { return trigger_clock_; }
+
 void ControllerManager::propagate_deactivation_of_chained_mode(
   const std::vector<ControllerSpec> & controllers)
 {

controller_manager/src/ros2_control_node.cpp

@@ -124,33 +124,50 @@ int main(int argc, char ** argv)
 
       // for calculating sleep time
       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 - period};
 
       // for calculating the measured period of the loop
-      rclcpp::Time previous_time = cm->now() - period;
+      rclcpp::Time previous_time = cm->get_trigger_clock()->now();
+      std::this_thread::sleep_for(period);
+
+      std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()};
 
       while (rclcpp::ok())
       {
         // calculate measured period
-        auto const current_time = cm->now();
+        auto const current_time = cm->get_trigger_clock()->now();
         auto const measured_period = current_time - previous_time;
         previous_time = current_time;
 
         // execute update loop
-        cm->read(cm->now(), measured_period);
-        cm->update(cm->now(), measured_period);
-        cm->write(cm->now(), measured_period);
+        cm->read(cm->get_trigger_clock()->now(), measured_period);
+        cm->update(cm->get_trigger_clock()->now(), measured_period);
+        cm->write(cm->get_trigger_clock()->now(), measured_period);
 
         // wait until we hit the end of the period
-        next_iteration_time += period;
         if (use_sim_time)
         {
           cm->get_clock()->sleep_until(current_time + period);
         }
         else
         {
+          next_iteration_time += period;
+          const auto time_now = std::chrono::steady_clock::now();
+          if (next_iteration_time < time_now)
+          {
+            const double time_diff =
+              static_cast<double>(
+                std::chrono::duration_cast<std::chrono::nanoseconds>(time_now - next_iteration_time)
+                  .count()) /
+              1.e6;
+            const double cm_period = 1.e3 / static_cast<double>(cm->get_update_rate());
+            const int overrun_count = static_cast<int>(std::ceil(time_diff / cm_period));
+            RCLCPP_WARN(
+              cm->get_logger(),
+              "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop "
+              "took %f ms (missed cycles : %d).",
+              cm->get_update_rate(), time_diff + cm_period, overrun_count + 1);
+            next_iteration_time += (overrun_count * period);
+          }
           std::this_thread::sleep_until(next_iteration_time);
         }
       }

doc/release_notes.rst

@@ -88,6 +88,7 @@ controller_manager
 * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 <https://github.com/ros-controls/ros2_control/pull/1808>`_).
 * 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>`_).
 * 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>`_).
+* When a realtime kernel is present, the controller manager will use a monotonic clock for triggering read-update-write cycles (`#2046 <https://github.com/ros-controls/ros2_control/pull/2046>`_).
 
 hardware_interface
 ******************

hardware_interface/include/hardware_interface/actuator.hpp

@@ -48,6 +48,9 @@ class Actuator final
     const HardwareInfo & actuator_info, rclcpp::Logger logger,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface);
 
+  const rclcpp_lifecycle::State & initialize(
+    const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
+
   const rclcpp_lifecycle::State & configure();
 
   const rclcpp_lifecycle::State & cleanup();

hardware_interface/include/hardware_interface/actuator_interface.hpp

@@ -101,16 +101,32 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   /// clock and logger interfaces.
   /**
    * \param[in] hardware_info structure with data from URDF.
+   * \param[in] logger Logger for the hardware component.
    * \param[in] clock_interface pointer to the clock interface.
-   * \param[in] logger_interface pointer to the logger interface.
    * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
    * \returns CallbackReturn::ERROR if any error happens or data are missing.
    */
+  [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]]
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
   {
-    clock_interface_ = clock_interface;
+    return this->init(hardware_info, logger, clock_interface->get_clock());
+  }
+
+  /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
+  /// clock and logger interfaces.
+  /**
+   * \param[in] hardware_info structure with data from URDF.
+   * \param[in] clock pointer to the resource manager clock.
+   * \param[in] logger Logger for the hardware component.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  CallbackReturn init(
+    const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
+  {
+    actuator_clock_ = clock;
     actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name);
     info_ = hardware_info;
     if (info_.is_async)
@@ -514,7 +530,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   /**
    * \return clock of the ActuatorInterface.
    */
-  rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); }
+  rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; }
 
   /// Get the hardware info of the ActuatorInterface.
   /**
@@ -542,7 +558,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod
   std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
 
 private:
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+  rclcpp::Clock::SharedPtr actuator_clock_;
   rclcpp::Logger actuator_logger_;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> actuator_states_;

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -52,6 +52,9 @@ class ResourceManager
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface);
 
+  /// Default constructor for the Resource Manager.
+  explicit ResourceManager(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger);
+
   /// Constructor for the Resource Manager.
   /**
    * The implementation loads the specified urdf and initializes the
@@ -64,14 +67,34 @@ class ResourceManager
    * \param[in] update_rate Update rate of the controller manager to calculate calling frequency
    * of async components.
    * \param[in] clock_interface reference to the clock interface of the CM node for getting time
-   * used for triggering async components.
+   * used for triggering async components and different read/write component rates.
+   * \param[in] logger_interface reference to the logger interface of the CM node for logging.
    */
   explicit ResourceManager(
     const std::string & urdf,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
     rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
     bool activate_all = false, const unsigned int update_rate = 100);
 
+  /// Constructor for the Resource Manager.
+  /**
+   * The implementation loads the specified urdf and initializes the
+   * hardware components listed within as well as populate their respective
+   * state and command interfaces.
+   *
+   * \param[in] urdf string containing the URDF.
+   * \param[in] activate_all boolean argument indicating if all resources should be immediately
+   * activated. Currently used only in tests.
+   * \param[in] update_rate Update rate of the controller manager to calculate calling frequency
+   * of async components.
+   * \param[in] clock reference to the clock of the CM node for getting time used for triggering
+   * async components and different read/write component rates.
+   * \param[in] logger logger of the CM node for logging.
+   */
+  explicit ResourceManager(
+    const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger,
+    bool activate_all = false, const unsigned int update_rate = 100);
+
   ResourceManager(const ResourceManager &) = delete;
 
   virtual ~ResourceManager();

hardware_interface/include/hardware_interface/sensor.hpp

@@ -48,6 +48,9 @@ class Sensor final
     const HardwareInfo & sensor_info, rclcpp::Logger logger,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface);
 
+  const rclcpp_lifecycle::State & initialize(
+    const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
+
   const rclcpp_lifecycle::State & configure();
 
   const rclcpp_lifecycle::State & cleanup();

hardware_interface/include/hardware_interface/sensor_interface.hpp

@@ -100,16 +100,32 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   /// clock and logger interfaces.
   /**
    * \param[in] hardware_info structure with data from URDF.
+   * \param[in] logger Logger for the hardware component.
    * \param[in] clock_interface pointer to the clock interface.
-   * \param[in] logger_interface pointer to the logger interface.
    * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
    * \returns CallbackReturn::ERROR if any error happens or data are missing.
    */
+  [[deprecated("Use init(HardwareInfo, rclcpp::Logger, rclcpp::Clock::SharedPtr) instead.")]]
   CallbackReturn init(
     const HardwareInfo & hardware_info, rclcpp::Logger logger,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
   {
-    clock_interface_ = clock_interface;
+    return this->init(hardware_info, logger, clock_interface->get_clock());
+  }
+
+  /// Initialization of the hardware interface from data parsed from the robot's URDF and also the
+  /// clock and logger interfaces.
+  /**
+   * \param[in] hardware_info structure with data from URDF.
+   * \param[in] clock pointer to the resource manager clock.
+   * \param[in] logger Logger for the hardware component.
+   * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed.
+   * \returns CallbackReturn::ERROR if any error happens or data are missing.
+   */
+  CallbackReturn init(
+    const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock)
+  {
+    sensor_clock_ = clock;
     sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name);
     info_ = hardware_info;
     if (info_.is_async)
@@ -318,7 +334,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   /**
    * \return clock of the SensorInterface.
    */
-  rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); }
+  rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; }
 
   /// Get the hardware info of the SensorInterface.
   /**
@@ -342,7 +358,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI
   std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> read_async_handler_;
 
 private:
-  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
+  rclcpp::Clock::SharedPtr sensor_clock_;
   rclcpp::Logger sensor_logger_;
   // interface names to Handle accessed through getters/setters
   std::unordered_map<std::string, StateInterface::SharedPtr> sensor_states_map_;

hardware_interface/include/hardware_interface/system.hpp

@@ -48,6 +48,9 @@ class System final
     const HardwareInfo & system_info, rclcpp::Logger logger,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface);
 
+  const rclcpp_lifecycle::State & initialize(
+    const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock);
+
   const rclcpp_lifecycle::State & configure();
 
   const rclcpp_lifecycle::State & cleanup();