Skip to content
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

Sort chained controllers #877

Closed
wants to merge 5 commits into from
Closed

Sort chained controllers #877

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
de6bc93
Make sure that controllers are properly sorted and executed in a chai…
sgmurray Nov 14, 2022
c2d1910
Merge branch 'master' into issue-853
bmagyar Jan 10, 2023
4b37be7
Merge branch 'master' into issue-853
bmagyar Jan 18, 2023
dfeb600
move sorting alogorithm to separate function.
sgmurray Jan 23, 2023
923e6bd
Merge branch 'issue-853' of https://github.com/sgmurray/ros2_control …
sgmurray Jan 23, 2023
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
108 changes: 106 additions & 2 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -419,7 +419,8 @@ controller_interface::return_type ControllerManager::configure_controller(
{
RCLCPP_INFO(get_logger(), "Configuring controller '%s'", controller_name.c_str());

const auto & controllers = get_loaded_controllers();
std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
const std::vector<ControllerSpec> & controllers = rt_controllers_wrapper_.get_updated_list(guard);

auto found_it = std::find_if(
controllers.begin(), controllers.end(),
Expand All @@ -436,6 +437,7 @@ controller_interface::return_type ControllerManager::configure_controller(
auto controller = found_it->c;

auto state = controller->get_state();

if (
state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE ||
state.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED)
Expand Down Expand Up @@ -489,10 +491,112 @@ controller_interface::return_type ControllerManager::configure_controller(
return controller_interface::return_type::ERROR;
}
resource_manager_->import_controller_reference_interfaces(controller_name, interfaces);
}

std::vector<ControllerSpec> & unused_controller_list =
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this code is very complex as all entangled sorting algo implementations tend to be. I'd prefer if it was in a separate function so it can be unit tested.

rt_controllers_wrapper_.get_unused_list(guard);
unused_controller_list = controllers;

std::unordered_map<std::string, int> preceding_controller_count = {};
std::unordered_map<std::string, std::vector<std::string>> following_controllers = {};

for (const auto & controller1 : unused_controller_list)
{
if (
!controller1.c->is_chainable() ||
(!is_controller_active(controller1.c) && !is_controller_inactive(controller1.c)))
{
continue;
}
for (const auto & controller2 : unused_controller_list)
{
if (!is_controller_active(controller2.c) && !is_controller_inactive(controller2.c))
{
continue;
}
if (controller2.info.name == controller1.info.name)
{
continue;
}
for (const auto & reference1 :
resource_manager_->get_controller_reference_interface_names(controller1.info.name))
{
const auto controller2_commands = controller2.c->command_interface_configuration().names;

std::string commands_string = "";
for (auto command : controller2_commands)
{
commands_string += " " + command;
}
auto same = std::find(controller2_commands.begin(), controller2_commands.end(), reference1);
if (same != controller2_commands.end())
{
// controller 2 precedes controller1
if (following_controllers.find(controller2.info.name) != following_controllers.end())
{
following_controllers[controller2.info.name].push_back(controller1.info.name);
}
else
{
following_controllers.emplace(
controller2.info.name, std::initializer_list<std::string>{controller1.info.name});
}
if (
preceding_controller_count.find(controller1.info.name) !=
preceding_controller_count.end())
{
preceding_controller_count[controller1.info.name] += 1;
}
else
{
preceding_controller_count[controller1.info.name] = 1;
}
break;
}
}
}
}

// Use Kahn's algorithm for topological sorting
std::deque<std::string> controllers_to_add = {};
for (const auto & controller : unused_controller_list)
{
if (preceding_controller_count.find(controller.info.name) == preceding_controller_count.end())
{
controllers_to_add.push_back(controller.info.name);
}
}

// TODO(destogl): check and resort controllers in the vector
for (auto i = unused_controller_list.begin(); i != unused_controller_list.end(); i++)
{
std::string controller_name_to_find = controllers_to_add.front();
controllers_to_add.pop_front();
auto found_it = std::find_if(
unused_controller_list.begin(), unused_controller_list.end(),
std::bind(controller_name_compare, std::placeholders::_1, controller_name_to_find));
iter_swap(i, found_it);
if (following_controllers.find(controller_name_to_find) != following_controllers.end())
{
for (const auto & controller_name : following_controllers[controller_name_to_find])
{
preceding_controller_count[controller_name] -= 1;
if (preceding_controller_count[controller_name] == 0)
{
controllers_to_add.push_back(controller_name);
}
}
}
}

std::string controller_names = "";
for (auto & cont : unused_controller_list)
{
controller_names += cont.info.name + " ";
}

rt_controllers_wrapper_.switch_updated_list(guard);
rt_controllers_wrapper_.get_unused_list(guard).clear();

return controller_interface::return_type::OK;
}

Expand Down
24 changes: 21 additions & 3 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 @@ -104,11 +104,24 @@ TEST_P(TestControllerManager, controller_lifecycle)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());

// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
cm_->configure_controller(TEST_CONTROLLER2_NAME);
auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
test_controller::TEST_CONTROLLER_NAME);
configure_future.wait_for(std::chrono::milliseconds(100));
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);

configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
TEST_CONTROLLER2_NAME);
configure_future.wait_for(std::chrono::milliseconds(100));
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);

EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";

Expand Down Expand Up @@ -216,11 +229,16 @@ TEST_P(TestControllerManager, per_controller_update_rate)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller->get_state().id());

test_controller->get_node()->set_parameter({"update_rate", 4});

// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
auto configure_future = std::async(
std::launch::async, &controller_manager::ControllerManager::configure_controller, cm_,
test_controller::TEST_CONTROLLER_NAME);
configure_future.wait_for(std::chrono::milliseconds(100));
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(configure_future.get(), controller_interface::return_type::OK);
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";

EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id());
Expand Down
50 changes: 27 additions & 23 deletions controller_manager/test/test_controller_manager_srvs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -262,24 +262,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(2u, result->controller.size());
// check chainable controller
ASSERT_EQ(result->controller[0].state, "inactive");
ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u);
ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 1u);
ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
ASSERT_EQ(result->controller[0].is_chainable, true);
ASSERT_EQ(result->controller[0].is_chained, false);
ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u);
;
ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
// check test controller

// cm_->configure_controller() sorts the controllers
// test controller now precedes chainable controller
ASSERT_EQ(result->controller[1].name, "test_chainable_controller_name");
ASSERT_EQ(result->controller[1].type, "controller_manager/test_chainable_controller");
ASSERT_EQ(result->controller[1].state, "inactive");
ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 0u);
ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 3u);
ASSERT_EQ(result->controller[1].required_command_interfaces.size(), 1u);
ASSERT_EQ(result->controller[1].required_state_interfaces.size(), 2u);
ASSERT_EQ(result->controller[1].is_chainable, false);
ASSERT_EQ(result->controller[1].is_chainable, true);
ASSERT_EQ(result->controller[1].is_chained, false);
ASSERT_EQ(result->controller[1].reference_interfaces.size(), 0u);
ASSERT_EQ(result->controller[1].chain_connections.size(), 1u);
ASSERT_EQ(result->controller[1].reference_interfaces.size(), 2u);
ASSERT_EQ(result->controller[1].chain_connections.size(), 0u);
// check test controller
ASSERT_EQ(result->controller[0].state, "inactive");
ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 0u);
ASSERT_EQ(result->controller[0].required_command_interfaces.size(), 3u);
ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
ASSERT_EQ(result->controller[0].is_chainable, false);
ASSERT_EQ(result->controller[0].is_chained, false);
ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u);
ASSERT_EQ(result->controller[0].chain_connections.size(), 1u);
// activate controllers
cm_->switch_controller(
{test_chainable_controller::TEST_CONTROLLER_NAME}, {},
Expand All @@ -290,18 +294,18 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);
// check chainable controller
ASSERT_EQ(result->controller[0].state, "active");
ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 1u);
ASSERT_EQ(result->controller[0].is_chained, true);
// check test controller
ASSERT_EQ(result->controller[1].state, "active");
ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 3u);
ASSERT_EQ(result->controller[1].claimed_interfaces.size(), 1u);
ASSERT_EQ(result->controller[1].is_chained, true);
// check test controller
ASSERT_EQ(result->controller[0].state, "active");
ASSERT_EQ(result->controller[0].claimed_interfaces.size(), 3u);
ASSERT_EQ(
test_chainable_controller::TEST_CONTROLLER_NAME,
result->controller[1].chain_connections[0].name);
ASSERT_EQ(2u, result->controller[1].chain_connections[0].reference_interfaces.size());
ASSERT_EQ("joint1/position", result->controller[1].chain_connections[0].reference_interfaces[0]);
ASSERT_EQ("joint1/velocity", result->controller[1].chain_connections[0].reference_interfaces[1]);
result->controller[0].chain_connections[0].name);
ASSERT_EQ(2u, result->controller[0].chain_connections[0].reference_interfaces.size());
ASSERT_EQ("joint1/position", result->controller[0].chain_connections[0].reference_interfaces[0]);
ASSERT_EQ("joint1/velocity", result->controller[0].chain_connections[0].reference_interfaces[1]);
}

TEST_F(TestControllerManagerSrvs, reload_controller_libraries_srv)
Expand Down
87 changes: 85 additions & 2 deletions controller_manager/test/test_controllers_chaining_with_controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ class TestableTestChainableController : public test_chainable_controller::TestCh
FRIEND_TEST(
TestControllerChainingWithControllerManager,
test_chained_controllers_deactivation_error_handling);
FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_random_order);
};

class TestableControllerManager : public controller_manager::ControllerManager
Expand Down Expand Up @@ -84,6 +85,7 @@ class TestableControllerManager : public controller_manager::ControllerManager
FRIEND_TEST(
TestControllerChainingWithControllerManager,
test_chained_controllers_deactivation_error_handling);
FRIEND_TEST(TestControllerChainingWithControllerManager, test_chained_controllers_random_order);

public:
TestableControllerManager(
Expand Down Expand Up @@ -975,10 +977,91 @@ TEST_P(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, diff_drive_controller->get_state().id());
}

// TODO(destogl): Add test case with controllers added in "random" order
//
// new value: "START_DOWNSTREAM_CTRLS" --> start "downstream" controllers in a controllers chain
//
TEST_P(TestControllerChainingWithControllerManager, test_chained_controllers_random_order)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm sorry but I don't get what this test does. It feels like this is testing multiple components and various random things... We should clarify the implementation of this. I don't get why the internal counter check should be different, I don't get why a "random order test" starts with "add all controllers IN RESERVE EXECUTION ORDER" ?? so it's ordered, not random :D

{
SetupControllers();

// add all controllers - CONTROLLERS HAVE TO ADDED IN REVERSE EXECUTION ORDER
cm_->add_controller(
pid_left_wheel_controller, PID_LEFT_WHEEL,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
pid_right_wheel_controller, PID_RIGHT_WHEEL,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
diff_drive_controller, DIFF_DRIVE_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
diff_drive_controller_two, DIFF_DRIVE_CONTROLLER_TWO,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);

CheckIfControllersAreAddedCorrectly();

ConfigureAndCheckControllers();

SetToChainedModeAndMakeReferenceInterfacesAvailable(
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_REFERENCE_INTERFACES);
SetToChainedModeAndMakeReferenceInterfacesAvailable(
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_REFERENCE_INTERFACES);
SetToChainedModeAndMakeReferenceInterfacesAvailable(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_REFERENCE_INTERFACES);

EXPECT_THROW(
cm_->resource_manager_->make_controller_reference_interfaces_available(
POSITION_TRACKING_CONTROLLER),
std::out_of_range);

// Set ControllerManager into Debug-Mode output to have detailed output on updating controllers
cm_->get_logger().set_level(rclcpp::Logger::Level::Debug);

// activate controllers - CONTROLLERS HAVE TO ADDED REVERSE EXECUTION ORDER
// (otherwise, interface will be missing)
ActivateAndCheckController(
pid_left_wheel_controller, PID_LEFT_WHEEL, PID_LEFT_WHEEL_CLAIMED_INTERFACES, 1u);
ActivateAndCheckController(
pid_right_wheel_controller, PID_RIGHT_WHEEL, PID_RIGHT_WHEEL_CLAIMED_INTERFACES, 1u);
ASSERT_EQ(pid_left_wheel_controller->internal_counter, 3u);

// Diff-Drive Controller claims the reference interfaces of PID controllers
ActivateAndCheckController(
diff_drive_controller, DIFF_DRIVE_CONTROLLER, DIFF_DRIVE_CLAIMED_INTERFACES, 1u);
ASSERT_EQ(pid_right_wheel_controller->internal_counter, 3u);
ASSERT_EQ(pid_left_wheel_controller->internal_counter, 5u);

// Position-Tracking Controller uses reference interfaces of Diff-Drive Controller
ActivateAndCheckController(
position_tracking_controller, POSITION_TRACKING_CONTROLLER,
POSITION_CONTROLLER_CLAIMED_INTERFACES, 1u);
// 'rot_z' reference interface is not claimed
for (const auto & interface : {"diff_drive_controller/rot_z"})
{
EXPECT_TRUE(cm_->resource_manager_->command_interface_exists(interface));
EXPECT_TRUE(cm_->resource_manager_->command_interface_is_available(interface));
EXPECT_FALSE(cm_->resource_manager_->command_interface_is_claimed(interface));
}
ASSERT_EQ(diff_drive_controller->internal_counter, 3u);
ASSERT_EQ(pid_right_wheel_controller->internal_counter, 5u);
ASSERT_EQ(pid_left_wheel_controller->internal_counter, 7u);

// update controllers
std::vector<double> reference = {32.0, 128.0};

// update 'Position Tracking' controller
for (auto & value : diff_drive_controller->reference_interfaces_)
{
ASSERT_EQ(value, 0.0); // default reference values are 0.0
}

// update all controllers at once and see that all have expected values --> also checks the order
// of controller execution
reference = {1024.0, 4096.0};
UpdateAllControllerAndCheck(reference, 2u);
}

INSTANTIATE_TEST_SUITE_P(
test_strict_best_effort, TestControllerChainingWithControllerManager,
Expand Down
Loading