Skip to content

Commit

Permalink
Make sure that controllers are properly sorted and executed in a chain (
Browse files Browse the repository at this point in the history 
#853)
  • Loading branch information
@sgmurray
sgmurray committed Dec 19, 2022
1 parent dbb6268 commit de6bc93
Show file tree
Hide file tree
Showing 5 changed files with 267 additions and 33 deletions.
108 changes: 106 additions & 2 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -411,7 +411,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 @@ -428,6 +429,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 @@ -481,10 +483,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 =
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
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
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
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)
{
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

0 comments on commit de6bc93

Please sign in to comment.