From 16cd0870accf62eee050808d717cf8f6cc7be11a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 1 Jul 2023 21:29:09 +0200 Subject: [PATCH] Added test case to test the controllers with equal and higher update rates w.r.t controller manager --- .../test/test_controller_manager.cpp | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 22221b6e215..5797d690dea 100644 --- 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(); + + 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(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 start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + std::vector 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));