Fix update period for the first update after activation (#1551)

ros-controls · Jun 3, 2024 · 1ed61ef · 1ed61ef
1 parent e122f71
commit 1ed61ef
Show file tree

Hide file tree

Showing 4 changed files with 27 additions and 13 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -2147,6 +2147,17 @@ controller_interface::return_type ControllerManager::update(
  run_controller_at_cm_rate ? period
  : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
+ if (
+ *loaded_controller.next_update_cycle_time ==
+ rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
+ {
+ // it is zero after activation
+ RCLCPP_DEBUG(
+ get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
+ time.seconds(), loaded_controller.info.name.c_str());
+ *loaded_controller.next_update_cycle_time = time;
+ }
+
  bool controller_go =
  (time ==
  rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
@@ -2182,12 +2193,6 @@ controller_interface::return_type ControllerManager::update(
  controller_ret = controller_interface::return_type::ERROR;
  }
 
- if (
- *loaded_controller.next_update_cycle_time ==
- rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
- {
- *loaded_controller.next_update_cycle_time = time;
- }
  *loaded_controller.next_update_cycle_time += controller_period;
 
  if (controller_ret != controller_interface::return_type::OK)

diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
@@ -62,7 +62,7 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
 controller_interface::return_type TestController::update(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
 {
- update_period_ = period.seconds();
+ update_period_ = period;
  ++internal_counter;
 
  // set value to hardware to produce and test different behaviors there

diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp
@@ -80,7 +80,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;
- double update_period_ = 0;
+ rclcpp::Duration update_period_ = rclcpp::Duration::from_seconds(0.);
 };
 
 } // namespace test_controller

diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
@@ -374,9 +374,12 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
  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)
- ASSERT_GE(test_controller->update_period_, 1.0 / cm_update_rate);
- ASSERT_LT(test_controller->update_period_, 2.0 / cm_update_rate);
+ // [cm_update_rate, 2*cm_update_rate)
+ EXPECT_THAT(
+ test_controller->update_period_,
+ testing::AllOf(
+ testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)),
+ testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate))));
  loop_rate.sleep();
  }
  // if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -445,6 +448,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
 
  // Start controller, will take effect at the end of the update function
+ time_ = test_controller->get_node()->now(); // set to something nonzero
  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
  std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
  std::vector<std::string> stop_controllers = {};
@@ -472,6 +476,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
  const auto controller_update_rate = test_controller->get_update_rate();
 
  const auto initial_counter = test_controller->internal_counter;
+ // don't start with zero to check if the period is correct if controller is activated anytime
  rclcpp::Time time = time_;
  for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
  {
@@ -480,8 +485,12 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
  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)
- ASSERT_GE(test_controller->update_period_, 1.0 / controller_update_rate);
- ASSERT_LT(test_controller->update_period_, 2.0 / controller_update_rate);
+ EXPECT_THAT(
+ test_controller->update_period_,
+ testing::AllOf(
+ testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)),
+ testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate))))
+ << "update_counter: " << update_counter;
 
  time += rclcpp::Duration::from_seconds(0.01);
  if (update_counter % cm_update_rate == 0)