Fix equal and higher controller update rate (ros-controls#1070)

controller_manager/src/controller_manager.cpp

@@ -610,6 +610,18 @@ controller_interface::return_type ControllerManager::configure_controller(
       controller_name, std::make_unique<ControllerThreadWrapper>(controller, update_rate_));
   }
 
+  const auto controller_update_rate = controller->get_update_rate();
+  const auto cm_update_rate = get_update_rate();
+  if (controller_update_rate > cm_update_rate)
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "The controller : %s update rate : %d Hz should be less than or equal to controller "
+      "manager's update rate : %d Hz!. The controller will be updated at controller_manager's "
+      "update rate.",
+      controller_name.c_str(), controller_update_rate, cm_update_rate);
+  }
+
   // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers
   if (controller->is_chainable())
   {
@@ -1873,8 +1885,9 @@ controller_interface::return_type ControllerManager::update(
     {
       const auto controller_update_rate = loaded_controller.c->get_update_rate();
 
-      bool controller_go =
-        controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0);
+      bool controller_go = controller_update_rate == 0 ||
+                           ((update_loop_counter_ % controller_update_rate) == 0) ||
+                           (controller_update_rate >= update_rate_);
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
         update_loop_counter_, controller_go ? "True" : "False",

controller_manager/test/test_controller/test_controller.hpp

@@ -65,7 +65,7 @@ class TestController : public controller_interface::ControllerInterface
   CONTROLLER_MANAGER_PUBLIC
   void set_state_interface_configuration(const controller_interface::InterfaceConfiguration & cfg);
 
-  size_t internal_counter = 0;
+  unsigned int internal_counter = 0;
   bool simulate_cleanup_failure = false;
   // Variable where we store when cleanup was called, pointer because the controller
   // is usually destroyed after cleanup

controller_manager/test/test_controller_manager.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <gtest/gtest.h>
+#include <gmock/gmock.h>
 #include <memory>
 #include <string>
 #include <utility>
@@ -27,13 +27,13 @@
 using ::testing::_;
 using ::testing::Return;
 
-class TestControllerManager
+class TestControllerManagerWithStrictness
 : public ControllerManagerFixture<controller_manager::ControllerManager>,
   public testing::WithParamInterface<Strictness>
 {
 };
 
-TEST_P(TestControllerManager, controller_lifecycle)
+TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
 {
   const auto test_param = GetParam();
   auto test_controller = std::make_shared<test_controller::TestController>();
@@ -159,7 +159,7 @@ TEST_P(TestControllerManager, controller_lifecycle)
     controller_interface::return_type::OK,
     cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
   EXPECT_GE(test_controller->internal_counter, 1u);
-  auto last_internal_counter = test_controller->internal_counter;
+  size_t last_internal_counter = test_controller->internal_counter;
 
   // Stop controller, will take effect at the end of the update function
   start_controllers = {};
@@ -196,7 +196,7 @@ TEST_P(TestControllerManager, controller_lifecycle)
   EXPECT_EQ(1, test_controller.use_count());
 }
 
-TEST_P(TestControllerManager, per_controller_update_rate)
+TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
 {
   auto strictness = GetParam().strictness;
   auto test_controller = std::make_shared<test_controller::TestController>();
@@ -254,4 +254,113 @@ TEST_P(TestControllerManager, per_controller_update_rate)
 }
 
 INSTANTIATE_TEST_SUITE_P(
-  test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort));
+  test_strict_best_effort, TestControllerManagerWithStrictness,
+  testing::Values(strict, best_effort));
+
+class TestControllerManagerWithUpdateRates
+: public ControllerManagerFixture<controller_manager::ControllerManager>,
+  public testing::WithParamInterface<unsigned int>
+{
+};
+
+TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_update_rate)
+{
+  const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+  const unsigned int ctrl_update_rate = GetParam();
+  auto test_controller = std::make_shared<test_controller::TestController>();
+
+  auto last_internal_counter = 0u;
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controller_manager"), "Testing update rate : %u Hz", ctrl_update_rate);
+  {
+    ControllerManagerRunner cm_runner(this);
+    cm_->add_controller(
+      test_controller, test_controller::TEST_CONTROLLER_NAME,
+      test_controller::TEST_CONTROLLER_CLASS_NAME);
+  }
+  EXPECT_EQ(1u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+    << "Update should not reach an unconfigured controller";
+
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());
+
+  rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(ctrl_update_rate));
+  test_controller->get_node()->set_parameter(update_rate_parameter);
+  // configure controller
+  cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+    << "Controller is not started";
+  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
+  std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
+  std::vector<std::string> stop_controllers = {};
+  auto switch_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    start_controllers, stop_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+  ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
+    << "switch_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+  EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
+    << "Controller is started at the end of update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
+  }
+
+  EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_state().id());
+
+  const auto pre_internal_counter = test_controller->internal_counter;
+  rclcpp::Rate loop_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(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
+    loop_rate.sleep();
+  }
+  // if we do 2 times of the controller_manager update rate, the internal counter should be
+  // similarly incremented
+  EXPECT_EQ(test_controller->internal_counter, pre_internal_counter + (2 * cm_->get_update_rate()));
+  EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
+
+  auto deactivate_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
+    stop_controllers, start_controllers, strictness, true, rclcpp::Duration(0, 0));
+
+  ASSERT_EQ(std::future_status::timeout, deactivate_future.wait_for(std::chrono::milliseconds(100)))
+    << "switch_controller should be blocking until next update cycle";
+
+  EXPECT_EQ(
+    controller_interface::return_type::OK,
+    cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)))
+    << "Controller is stopped at the end of update, so it should have done one more update";
+  {
+    ControllerManagerRunner cm_runner(this);
+    EXPECT_EQ(controller_interface::return_type::OK, deactivate_future.get());
+  }
+  auto unload_future = std::async(
+    std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
+    test_controller::TEST_CONTROLLER_NAME);
+  ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100)))
+    << "unload_controller should be blocking until next update cycle";
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
+  last_internal_counter = test_controller->internal_counter;
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  per_controller_equal_and_higher_update_rate, TestControllerManagerWithUpdateRates,
+  testing::Values(100, 232, 400));