[Controllers] Receive a valid period on the first update cycle (#2572) (#2602)

Author: mergify[bot]

controller_manager/src/controller_manager.cpp

@@ -2976,21 +2976,13 @@ controller_interface::return_type ControllerManager::update(
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
-      bool first_update_cycle = false;
+      const bool first_update_cycle =
+        (*loaded_controller.last_update_cycle_time ==
+         rclcpp::Time(0, 0, this->get_trigger_clock()->get_clock_type()));
       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_trigger_clock()->get_clock_type()))
-      {
-        // last_update_cycle_time is zero after activation
-        first_update_cycle = true;
-        *loaded_controller.last_update_cycle_time = current_time;
-        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);
+        first_update_cycle ? controller_period
+                           : (current_time - *loaded_controller.last_update_cycle_time);
 
       const double error_now =
         std::abs((controller_actual_period.seconds() * controller_update_rate) - 1.0);

controller_manager/test/test_controller_manager.cpp

@@ -472,8 +472,11 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
   std::this_thread::sleep_for(
     std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
   EXPECT_EQ(test_controller->internal_counter, 1u);
-  EXPECT_EQ(test_controller->update_period_.seconds(), 0.0)
-    << "The first trigger cycle should have zero period";
+  EXPECT_NEAR(
+    test_controller->update_period_.seconds(),
+    1.0 / (static_cast<double>(test_controller->get_update_rate())), 1.e-6)
+    << "The first trigger cycle should have non-zero period to allow for integration in the "
+       "controllers";
 
   const double exp_period = (cm_->get_trigger_clock()->now() - time_).seconds();
   time_ = cm_->get_trigger_clock()->now();
@@ -1064,8 +1067,11 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     }
     else
     {
-      // Check that the first cycle update period is zero
-      EXPECT_EQ(test_controller->update_period_.seconds(), 0.0);
+      EXPECT_NEAR(
+        test_controller->update_period_.seconds(),
+        1.0 / (static_cast<double>(test_controller->get_update_rate())), 1.e-6)
+        << "The first trigger cycle should have non-zero period to allow for integration in the "
+           "controllers";
     }
 
     if (update_counter > 0 && update_counter % cm_update_rate == 0)