[CM] Fix controller missing update cycles in a real setup (#1774)

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -134,7 +134,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * **The method called in the (real-time) control loop.**
    *
    * \param[in] time The time at the start of this control loop iteration
-   * \param[in] period The measured time taken by the last control loop iteration
+   * \param[in] period The measured time since the last control loop iteration
    * \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
    */
   CONTROLLER_INTERFACE_PUBLIC

controller_manager/CMakeLists.txt

@@ -85,6 +85,7 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_controller_manager
     test/test_controller_manager.cpp
+    TIMEOUT 180
   )
   target_link_libraries(test_controller_manager
     controller_manager

controller_manager/include/controller_manager/controller_manager.hpp

@@ -122,7 +122,7 @@ class ControllerManager : public rclcpp::Node
     controller_spec.c = controller;
     controller_spec.info.name = controller_name;
     controller_spec.info.type = controller_type;
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     return add_controller_impl(controller_spec);
   }
 

controller_manager/include/controller_manager/controller_spec.hpp

@@ -38,7 +38,7 @@ struct ControllerSpec
 {
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
-  std::shared_ptr<rclcpp::Time> next_update_cycle_time;
+  std::shared_ptr<rclcpp::Time> last_update_cycle_time;
 };
 
 }  // namespace controller_manager

controller_manager/src/controller_manager.cpp

@@ -596,7 +596,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
-  controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
+  controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(
     0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
   // We have to fetch the parameters_file at the time of loading the controller, because this way we
@@ -1545,8 +1545,8 @@ void ControllerManager::activate_controllers(
     }
     auto controller = found_it->c;
     auto controller_name = found_it->info.name;
-    // reset the next update cycle time for newly activated controllers
-    *found_it->next_update_cycle_time =
+    // 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());
 
     bool assignment_successful = true;
@@ -2139,10 +2139,31 @@ controller_interface::return_type ControllerManager::update(
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
-      bool controller_go =
+      const rclcpp::Time current_time = get_clock()->now();
+      if (
+        *loaded_controller.last_update_cycle_time ==
+        rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+      {
+        // it is zero after activation
+        *loaded_controller.last_update_cycle_time = current_time - controller_period;
+        RCLCPP_DEBUG(
+          get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
+          loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
+      }
+      const auto controller_actual_period =
+        (current_time - *loaded_controller.last_update_cycle_time);
+
+      /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
+      /// jitter in the system sleep cycles.
+      // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have
+      // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we
+      // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue
+      // 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.seconds() >= loaded_controller.next_update_cycle_time->seconds());
+        (controller_actual_period.seconds() * controller_update_rate >= 0.99);
 
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
@@ -2151,15 +2172,28 @@ controller_interface::return_type ControllerManager::update(
 
       if (controller_go)
       {
-        auto controller_ret = loaded_controller.c->update(time, controller_period);
-
-        if (
-          *loaded_controller.next_update_cycle_time ==
-          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+        auto controller_ret = controller_interface::return_type::OK;
+        // Catch exceptions thrown by the controller update function
+        try
         {
-          *loaded_controller.next_update_cycle_time = time;
+          controller_ret = loaded_controller.c->update(time, controller_period);
         }
-        *loaded_controller.next_update_cycle_time += controller_period;
+        catch (const std::exception & e)
+        {
+          RCLCPP_ERROR(
+            get_logger(), "Caught exception of type : %s while updating controller '%s': %s",
+            typeid(e).name(), loaded_controller.info.name.c_str(), e.what());
+          controller_ret = controller_interface::return_type::ERROR;
+        }
+        catch (...)
+        {
+          RCLCPP_ERROR(
+            get_logger(), "Caught unknown exception while updating controller '%s'",
+            loaded_controller.info.name.c_str());
+          controller_ret = controller_interface::return_type::ERROR;
+        }
+
+        *loaded_controller.last_update_cycle_time = current_time;
 
         if (controller_ret != controller_interface::return_type::OK)
         {

controller_manager/test/test_controller/test_controller.cpp

@@ -60,8 +60,9 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
 }
 
 controller_interface::return_type TestController::update(
-  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
+  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
+  update_period_ = period;
   ++internal_counter;
 
   // set value to hardware to produce and test different behaviors there

controller_manager/test/test_controller/test_controller.hpp

@@ -77,6 +77,7 @@ class TestController : public controller_interface::ControllerInterface
   // enables external setting of values to command interfaces - used for simulation of hardware
   // errors
   double set_first_command_interface_value_to;
+  rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.);
 };
 
 }  // namespace test_controller

controller_manager/test/test_controller_manager.cpp

@@ -337,11 +337,17 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
 
   const auto pre_internal_counter = test_controller->internal_counter;
   rclcpp::Rate loop_rate(cm_->get_update_rate());
+  const auto cm_update_rate = cm_->get_update_rate();
   for (size_t i = 0; i < 2 * cm_->get_update_rate(); i++)
   {
     EXPECT_EQ(
       controller_interface::return_type::OK,
       cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
+    // In case of a non perfect divisor, the update period should respect the rule
+    // [cm_update_rate, 2*cm_update_rate)
+    EXPECT_THAT(
+      test_controller->update_period_.seconds(),
+      testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate))));
     loop_rate.sleep();
   }
   // if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -402,6 +408,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     ControllerManagerRunner cm_runner(this);
     cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
   }
+  time_ = test_controller->get_node()->now();  // set to something nonzero
+  cm_->get_clock()->sleep_until(time_ + PERIOD);
+  time_ = cm_->get_clock()->now();
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -420,6 +429,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
+  cm_->get_clock()->sleep_until(time_ + PERIOD);
+  time_ = cm_->get_clock()->now();
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -434,16 +445,28 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
   const auto cm_update_rate = cm_->get_update_rate();
   const auto controller_update_rate = test_controller->get_update_rate();
+  const double controller_period = 1.0 / controller_update_rate;
 
   const auto initial_counter = test_controller->internal_counter;
   rclcpp::Time time = time_;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
+    rclcpp::Time old_time = time;
+    cm_->get_clock()->sleep_until(old_time + PERIOD);
+    time = cm_->get_clock()->now();
     EXPECT_EQ(
       controller_interface::return_type::OK,
       cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
+    // In case of a non perfect divisor, the update period should respect the rule
+    // [controller_update_rate, 2*controller_update_rate)
+    EXPECT_THAT(
+      test_controller->update_period_.seconds(),
+      testing::AllOf(
+        testing::Gt(0.99 * controller_period),
+        testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
+      << "update_counter: " << update_counter << " desired controller period: " << controller_period
+      << " actual controller period: " << test_controller->update_period_.seconds();
 
-    time += rclcpp::Duration::from_seconds(0.01);
     if (update_counter % cm_update_rate == 0)
     {
       const auto no_of_secs_passed = update_counter / cm_update_rate;
@@ -453,13 +476,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
       // For instance, a controller with update rate 37 Hz, seems to have 36 in the first update
       // cycle and then on accumulating 37 on every other update cycle so at the end of the 10
       // cycles it will have 369 instead of 370.
-      EXPECT_NEAR(
+      EXPECT_THAT(
         test_controller->internal_counter - initial_counter,
-        (controller_update_rate * no_of_secs_passed), 1);
+        testing::AnyOf(
+          testing::Ge((controller_update_rate - 1) * no_of_secs_passed),
+          testing::Lt((controller_update_rate * no_of_secs_passed))));
     }
   }
 }
 
 INSTANTIATE_TEST_SUITE_P(
   per_controller_update_rate_check, TestControllerUpdateRates,
-  testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));
+  testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90));