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 controllers while configuring instead of while activating #1107

Merged
merged 7 commits into from
Nov 4, 2023
Merged

Sort controllers while configuring instead of while activating #1107

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
e654806
update tests to be able to sort on configuring the controller
saikishor Aug 27, 2023
e32476b
update the logic to sort controller upon configuring (fixes #1099)
saikishor Aug 27, 2023
998d111
use ControllerManagerRunner to avoid blocking when switching lists
saikishor Aug 27, 2023
6416002
remove the TODO from denis
saikishor Aug 28, 2023
0e9b0be
fix potential bug of matching substrings
saikishor Sep 6, 2023
e029104
fix another potential bug of matching substrings with controller name
saikishor Sep 6, 2023
6ffa209
Merge branch 'master' into sort_controllers_on_configure
bmagyar Oct 30, 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
62 changes: 33 additions & 29 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,9 +148,6 @@ std::vector<std::string> get_following_controller_names(

return following_controllers;
}
// If the controller is not configured, return empty
if (!(is_controller_active(controller_it->c) || is_controller_inactive(controller_it->c)))
return following_controllers;
const auto cmd_itfs = controller_it->c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
Expand Down Expand Up @@ -209,13 +206,11 @@ std::vector<std::string> get_preceding_controller_names(
}
for (const auto & ctrl : controllers)
{
// If the controller is not configured, return empty
if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c)))
return preceding_controllers;
auto cmd_itfs = ctrl.c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
if (itf.find(controller_name) != std::string::npos)
auto split_pos = itf.find_first_of('/');
if ((split_pos != std::string::npos) && (itf.substr(0, split_pos) == controller_name))
{
preceding_controllers.push_back(ctrl.info.name);
auto ctrl_names = get_preceding_controller_names(ctrl.info.name, controllers);
Expand Down Expand Up @@ -769,10 +764,35 @@ controller_interface::return_type ControllerManager::configure_controller(
return controller_interface::return_type::ERROR;
}
resource_manager_->import_controller_reference_interfaces(controller_name, interfaces);
}

// Now let's reorder the controllers
// lock controllers
std::lock_guard<std::recursive_mutex> guard(rt_controllers_wrapper_.controllers_lock_);
std::vector<ControllerSpec> & to = rt_controllers_wrapper_.get_unused_list(guard);
const std::vector<ControllerSpec> & from = rt_controllers_wrapper_.get_updated_list(guard);

// Copy all controllers from the 'from' list to the 'to' list
to = from;

// Reordering the controllers
std::sort(
to.begin(), to.end(),
std::bind(
&ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2,
to));

// TODO(destogl): check and resort controllers in the vector
RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:");
for (const auto & ctrl : to)
{
RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str());
}

// switch lists
rt_controllers_wrapper_.switch_updated_list(guard);
// clear unused list
rt_controllers_wrapper_.get_unused_list(guard).clear();

return controller_interface::return_type::OK;
}

Expand Down Expand Up @@ -1234,19 +1254,6 @@ controller_interface::return_type ControllerManager::switch_controller(
}
}

// Reordering the controllers
std::sort(
to.begin(), to.end(),
std::bind(
&ControllerManager::controller_sorting, this, std::placeholders::_1, std::placeholders::_2,
to));

RCLCPP_DEBUG(get_logger(), "Reordered controllers list is:");
for (const auto & ctrl : to)
{
RCLCPP_DEBUG(this->get_logger(), "\t%s", ctrl.info.name.c_str());
}

// switch lists
rt_controllers_wrapper_.switch_updated_list(guard);
// clear unused list
Expand Down Expand Up @@ -2423,13 +2430,6 @@ bool ControllerManager::controller_sorting(
const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
const std::vector<controller_manager::ControllerSpec> & controllers)
{
// If the controllers are not active, then should be at the end of the list
if (!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c))
{
if (is_controller_active(ctrl_a.c)) return true;
return false;
}

const std::vector<std::string> cmd_itfs = ctrl_a.c->command_interface_configuration().names;
const std::vector<std::string> state_itfs = ctrl_a.c->state_interface_configuration().names;
if (cmd_itfs.empty() || !ctrl_a.c->is_chainable())
Expand Down Expand Up @@ -2501,7 +2501,11 @@ bool ControllerManager::controller_sorting(
// TODO(saikishor): deal with the state interface chaining in the sorting algorithm
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
auto state_it = std::find_if(
state_itfs.begin(), state_itfs.end(),
[ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
[ctrl_b](auto itf)
{
auto index = itf.find_first_of('/');
return ((index != std::string::npos) && (itf.substr(0, index) == ctrl_b.info.name));
});
if (state_it != state_itfs.end())
{
return false;
Expand Down
24 changes: 2 additions & 22 deletions controller_manager/test/test_chainable_controller/test_chainable_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,33 +32,13 @@ TestChainableController::TestChainableController()
controller_interface::InterfaceConfiguration
TestChainableController::command_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured.");
}
return cmd_iface_cfg_;
}

controller_interface::InterfaceConfiguration
TestChainableController::state_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured.");
}
return state_iface_cfg_;
}

controller_interface::return_type TestChainableController::update_reference_from_subscribers(
Expand Down
24 changes: 2 additions & 22 deletions controller_manager/test/test_controller/test_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,32 +31,12 @@ TestController::TestController()

controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return cmd_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get command interface configuration until the controller is configured.");
}
return cmd_iface_cfg_;
}

controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
{
if (
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
{
return state_iface_cfg_;
}
else
{
throw std::runtime_error(
"Can not get state interface configuration until the controller is configured.");
}
return state_iface_cfg_;
}

controller_interface::return_type TestController::update(
Expand Down
22 changes: 17 additions & 5 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,8 +104,11 @@ TEST_P(TestControllerManagerWithStrictness, 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);
{
ControllerManagerRunner cm_runner(this);
destogl marked this conversation as resolved.
Show resolved Hide resolved
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
cm_->configure_controller(TEST_CONTROLLER2_NAME);
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)));
Expand Down Expand Up @@ -217,7 +220,10 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)

test_controller->get_node()->set_parameter({"update_rate", 4});
// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
{
ControllerManagerRunner cm_runner(this);
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)));
Expand Down Expand Up @@ -296,7 +302,10 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
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);
{
ControllerManagerRunner cm_runner(this);
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)));
Expand Down Expand Up @@ -390,7 +399,10 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)

test_controller->get_node()->set_parameter({"update_rate", static_cast<int>(ctrl_update_rate)});
// configure controller
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
{
ControllerManagerRunner cm_runner(this);
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)));
Expand Down
79 changes: 11 additions & 68 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 @@ -261,27 +261,28 @@ TEST_F(TestControllerManagerSrvs, list_chained_controllers_srv)
// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(2u, result->controller.size());
// At this stage, the controllers are already reordered
// 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_command_interfaces.size(), 3u);
ASSERT_EQ(result->controller[0].required_state_interfaces.size(), 2u);
ASSERT_EQ(result->controller[0].is_chainable, true);
ASSERT_EQ(result->controller[0].is_chainable, false);
ASSERT_EQ(result->controller[0].is_chained, false);
ASSERT_EQ(result->controller[0].reference_interfaces.size(), 2u);
ASSERT_EQ("joint1/position", result->controller[0].reference_interfaces[0]);
ASSERT_EQ("joint1/velocity", result->controller[0].reference_interfaces[1]);
ASSERT_EQ(result->controller[0].reference_interfaces.size(), 0u);
ASSERT_EQ(result->controller[0].chain_connections.size(), 1u);

ASSERT_EQ(result->controller[0].chain_connections.size(), 0u);
// check test 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);
ASSERT_EQ("joint1/position", result->controller[1].reference_interfaces[0]);
ASSERT_EQ("joint1/velocity", result->controller[1].reference_interfaces[1]);
// activate controllers
cm_->switch_controller(
{test_chainable_controller::TEST_CONTROLLER_NAME}, {},
Expand Down Expand Up @@ -603,22 +604,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(6u, result->controller.size());

// activate controllers
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_4},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);

ASSERT_EQ(6u, result->controller.size());
destogl marked this conversation as resolved.
Show resolved Hide resolved
// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_5);
Expand Down Expand Up @@ -776,25 +761,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(8u, result->controller.size());

// activate controllers
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_3}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_7},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
cm_->switch_controller(
{test_controller::TEST_CONTROLLER_NAME}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);

ASSERT_EQ(8u, result->controller.size());
// reordered controllers
ASSERT_EQ(result->controller[0].name, "test_controller_name");
ASSERT_EQ(result->controller[1].name, TEST_CHAINED_CONTROLLER_7);
Expand Down Expand Up @@ -1009,29 +975,6 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers)
result = call_service_and_wait(*client, request, srv_executor);
ASSERT_EQ(10u, result->controller.size());

// activate controllers
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_1}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_4}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_7}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5, TEST_CHAINED_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_6, TEST_CHAINED_CONTROLLER_8},
{}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true,
rclcpp::Duration(0, 0));
cm_->switch_controller(
{TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {},
controller_manager_msgs::srv::SwitchController::Request::STRICT, true, rclcpp::Duration(0, 0));
// get controller list after activate
result = call_service_and_wait(*client, request, srv_executor);

ASSERT_EQ(10u, result->controller.size());

auto get_ctrl_pos = [result](const std::string & controller_name) -> int
{
auto it = std::find_if(
Expand Down
15 changes: 12 additions & 3 deletions controller_manager/test/test_load_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -162,7 +162,10 @@ TEST_P(TestLoadedControllerParametrized, starting_and_stopping_a_controller)
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id());

// Activate configured controller
cm_->configure_controller(controller_name1);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(controller_name1);
}
start_test_controller(test_param.strictness);
ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, controller_if->get_state().id());
}
Expand Down Expand Up @@ -246,7 +249,10 @@ TEST_P(TestLoadedControllerParametrized, inactive_controller_cannot_be_configure
test_controller->cleanup_calls = &cleanup_calls;
// Configure from inactive state
test_controller->simulate_cleanup_failure = false;
EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK);
{
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(cm_->configure_controller(controller_name1), controller_interface::return_type::OK);
}
ASSERT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id());
EXPECT_EQ(1u, cleanup_calls);
}
Expand Down Expand Up @@ -421,7 +427,10 @@ TEST_P(TestTwoLoadedControllers, switch_multiple_controllers)
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if2->get_state().id());

cm_->configure_controller(controller_name2);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(controller_name2);
}

// Stop controller 1
RCLCPP_INFO(cm_->get_logger(), "Stopping controller #1");
Expand Down