Skip to content

Split controller switch test #2177

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 4 commits into
base: master
Choose a base branch
from
Draft

Split controller switch test #2177

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
18884b1
extract single_controller_lifecycle test from controller_lifecycle test
ashwinsnambiar Apr 11, 2025
0deb02b
refactor lifecycle_test to switch_controller_with_unknown_controller …
ashwinsnambiar Apr 11, 2025
1d0e078
Merge branch 'master' into split_controller_switch_test
saikishor Apr 18, 2025
d86cced
delete unused test_controller
ashwinsnambiar Apr 20, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
223 changes: 156 additions & 67 deletions controller_manager/test/test_controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,160 @@ TEST_F(TestControllerManagerRobotDescription, controller_robot_description_updat
test_controller2->get_robot_description());
}

TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
TEST_P(TestControllerManagerWithStrictness, single_controller_lifecycle)
{
const auto test_param = GetParam();
auto test_controller = std::make_shared<test_controller::TestController>();

// Check for the hardware component and no controllers
controller_manager_msgs::msg::ControllerManagerActivity cm_msg;
const std::string cm_activity_topic =
std::string("/") + std::string(TEST_CM_NAME) + std::string("/activity");
get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 0u);

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(1, test_controller.use_count());

get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 1u);
ASSERT_EQ(cm_msg.controllers[0].name, test_controller::TEST_CONTROLLER_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

// setup interface to claim from controllers
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
{
cmd_itfs_cfg.names.push_back(interface);
}
test_controller->set_command_interface_configuration(cmd_itfs_cfg);

controller_interface::InterfaceConfiguration state_itfs_cfg;
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
test_controller->set_state_interface_configuration(state_itfs_cfg);

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter)
<< "Update should not reach an unconfigured controller";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());

// configure controller
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
}
get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 1u);
ASSERT_EQ(cm_msg.controllers[0].name, test_controller::TEST_CONTROLLER_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_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, test_param.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(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, 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_lifecycle_state().id());

get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 1u);
ASSERT_EQ(cm_msg.controllers[0].name, test_controller::TEST_CONTROLLER_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_GE(test_controller->internal_counter, 1u);
size_t last_internal_counter = test_controller->internal_counter;

// Stop controller, will take effect at the end of the update function
start_controllers = {};
stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.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";

get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 1u);
ASSERT_EQ(cm_msg.controllers[0].name, test_controller::TEST_CONTROLLER_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter)
<< "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, switch_future.get());
}

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());
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());

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
test_controller->get_lifecycle_state().id());
}

TEST_P(TestControllerManagerWithStrictness, switch_controller_with_unknown_controller)
{
const auto test_param = GetParam();
auto test_controller = std::make_shared<test_controller::TestController>();
Expand Down Expand Up @@ -198,6 +351,8 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 2u);
// TODO: Confirm whether first controller is TEST_CONTROLLER2_NAME or test_controller??
// Inconsistent from the order of insertion.
ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);
ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
Expand Down Expand Up @@ -244,72 +399,6 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter);

// Start the real test controller, will take effect at the end of the update function
start_controllers = {test_controller::TEST_CONTROLLER_NAME};
stop_controllers = {};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.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(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, 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_lifecycle_state().id());

get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 2u);
ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, expected_ctrl2_state);
ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_GE(test_controller->internal_counter, 1u);
size_t last_internal_counter = test_controller->internal_counter;

// Stop controller, will take effect at the end of the update function
start_controllers = {};
stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.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";

get_cm_status_message(cm_activity_topic, cm_msg);
ASSERT_EQ(cm_msg.hardware_components.size(), 3u);
ASSERT_EQ(cm_msg.controllers.size(), 2u);
ASSERT_EQ(cm_msg.controllers[0].name, TEST_CONTROLLER2_NAME);
ASSERT_EQ(cm_msg.controllers[0].state.id, expected_ctrl2_state);
ASSERT_EQ(cm_msg.controllers[1].name, test_controller::TEST_CONTROLLER_NAME);
ASSERT_EQ(cm_msg.controllers[1].state.id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter)
<< "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, switch_future.get());
}

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());
auto unload_future = std::async(
std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
test_controller::TEST_CONTROLLER_NAME);
Expand Down