Skip to content

Commit

Permalink
Merge branch 'master' into patch-1
Browse files Browse the repository at this point in the history
  • Loading branch information
destogl authored Dec 11, 2023
2 parents 059f8fd + 562f36d commit 2e0079b
Show file tree
Hide file tree
Showing 2 changed files with 22 additions and 4 deletions.
17 changes: 14 additions & 3 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -150,7 +150,9 @@ std::vector<std::string> get_following_controller_names(
}
// 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 @@ -210,7 +212,10 @@ std::vector<std::string> get_preceding_controller_names(
for (const auto & ctrl : controllers)
{
// If the controller is not configured, then continue
if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c))) continue;
if (!(is_controller_active(ctrl.c) || is_controller_inactive(ctrl.c)))
{
continue;
}
auto cmd_itfs = ctrl.c->command_interface_configuration().names;
for (const auto & itf : cmd_itfs)
{
Expand Down Expand Up @@ -2469,7 +2474,10 @@ bool ControllerManager::controller_sorting(
else
{
auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers);
if (following_ctrls.empty()) return false;
if (following_ctrls.empty())
{
return false;
}
// If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b
if (
std::find(following_ctrls.begin(), following_ctrls.end(), ctrl_b.info.name) !=
Expand Down Expand Up @@ -2509,7 +2517,10 @@ bool ControllerManager::controller_sorting(

// If there is no common parent, then they belong to 2 different sets
auto following_ctrls_b = get_following_controller_names(ctrl_b.info.name, controllers);
if (following_ctrls_b.empty()) return true;
if (following_ctrls_b.empty())
{
return true;
}
auto find_first_element = [&](const auto & controllers_list) -> size_t
{
auto it = std::find_if(
Expand Down
9 changes: 8 additions & 1 deletion controller_manager/test/test_controller_manager_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -639,7 +639,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_chained_controllers)
{TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5,
TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1,
test_controller::TEST_CONTROLLER_NAME})
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
Expand Down Expand Up @@ -796,7 +798,9 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers)
{TEST_CHAINED_CONTROLLER_4, TEST_CHAINED_CONTROLLER_3, TEST_CHAINED_CONTROLLER_5,
TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_1,
TEST_CHAINED_CONTROLLER_6, test_controller::TEST_CONTROLLER_NAME})
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
Expand Down Expand Up @@ -1010,7 +1014,10 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers)
TEST_CHAINED_CONTROLLER_4, TEST_CONTROLLER_2,
TEST_CHAINED_CONTROLLER_2, TEST_CHAINED_CONTROLLER_6,
TEST_CHAINED_CONTROLLER_7, TEST_CHAINED_CONTROLLER_8};
for (const auto & controller : ctrls_order) cm_->configure_controller(controller);
for (const auto & controller : ctrls_order)
{
cm_->configure_controller(controller);
}

// get controller list after configure
result = call_service_and_wait(*client, request, srv_executor);
Expand Down

0 comments on commit 2e0079b

Please sign in to comment.