Added test case to test the controllers with equal and higher update …

…rates w.r.t controller manager
ros-controls · Jul 1, 2023 · 16cd087 · 16cd087
1 parent 99deef8
commit 16cd087
Showing 1 changed file with 104 additions and 0 deletions.
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
@@ -253,5 +253,109 @@ TEST_P(TestControllerManager, per_controller_update_rate)
   EXPECT_EQ(test_controller->get_update_rate(), 4u);
 }
 
+TEST_P(TestControllerManager, per_controller_equal_and_higher_update_rate)
+{
+  auto strictness = GetParam().strictness;
+  auto test_controller = std::make_shared<test_controller::TestController>();
+
+  long unsigned int last_internal_counter = 0u;
+  for (unsigned int ctrl_update_rate : {100, 232, 400})
+  {
+    RCLCPP_INFO(
+      rclcpp::get_logger("test_controller_manager"), "Testing update rate : %d 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(
   test_strict_best_effort, TestControllerManager, testing::Values(strict, best_effort));