@@ -150,7 +150,9 @@ std::vector<std::string> get_following_controller_names(
150
150
}
151
151
// If the controller is not configured, return empty
152
152
if (!(is_controller_active (controller_it->c ) || is_controller_inactive (controller_it->c )))
153
+ {
153
154
return following_controllers;
155
+ }
154
156
const auto cmd_itfs = controller_it->c ->command_interface_configuration ().names ;
155
157
for (const auto & itf : cmd_itfs)
156
158
{
@@ -210,7 +212,10 @@ std::vector<std::string> get_preceding_controller_names(
210
212
for (const auto & ctrl : controllers)
211
213
{
212
214
// If the controller is not configured, then continue
213
- if (!(is_controller_active (ctrl.c ) || is_controller_inactive (ctrl.c ))) continue ;
215
+ if (!(is_controller_active (ctrl.c ) || is_controller_inactive (ctrl.c )))
216
+ {
217
+ continue ;
218
+ }
214
219
auto cmd_itfs = ctrl.c ->command_interface_configuration ().names ;
215
220
for (const auto & itf : cmd_itfs)
216
221
{
@@ -2443,7 +2448,10 @@ bool ControllerManager::controller_sorting(
2443
2448
if (!((is_controller_active (ctrl_a.c ) || is_controller_inactive (ctrl_a.c )) &&
2444
2449
(is_controller_active (ctrl_b.c ) || is_controller_inactive (ctrl_b.c ))))
2445
2450
{
2446
- if (is_controller_active (ctrl_a.c ) || is_controller_inactive (ctrl_a.c )) return true ;
2451
+ if (is_controller_active (ctrl_a.c ) || is_controller_inactive (ctrl_a.c ))
2452
+ {
2453
+ return true ;
2454
+ }
2447
2455
return false ;
2448
2456
}
2449
2457
@@ -2455,9 +2463,13 @@ bool ControllerManager::controller_sorting(
2455
2463
// joint_state_broadcaster
2456
2464
// If the controller b is also under the same condition, then maintain their initial order
2457
2465
if (ctrl_b.c ->command_interface_configuration ().names .empty () || !ctrl_b.c ->is_chainable ())
2466
+ {
2458
2467
return false ;
2468
+ }
2459
2469
else
2470
+ {
2460
2471
return true ;
2472
+ }
2461
2473
}
2462
2474
else if (ctrl_b.c ->command_interface_configuration ().names .empty () || !ctrl_b.c ->is_chainable ())
2463
2475
{
@@ -2467,12 +2479,17 @@ bool ControllerManager::controller_sorting(
2467
2479
else
2468
2480
{
2469
2481
auto following_ctrls = get_following_controller_names (ctrl_a.info .name , controllers);
2470
- if (following_ctrls.empty ()) return false ;
2482
+ if (following_ctrls.empty ())
2483
+ {
2484
+ return false ;
2485
+ }
2471
2486
// If the ctrl_b is any of the following controllers of ctrl_a, then place ctrl_a before ctrl_b
2472
2487
if (
2473
2488
std::find (following_ctrls.begin (), following_ctrls.end (), ctrl_b.info .name ) !=
2474
2489
following_ctrls.end ())
2490
+ {
2475
2491
return true ;
2492
+ }
2476
2493
else
2477
2494
{
2478
2495
auto ctrl_a_preceding_ctrls = get_preceding_controller_names (ctrl_a.info .name , controllers);
@@ -2505,7 +2522,10 @@ bool ControllerManager::controller_sorting(
2505
2522
2506
2523
// If there is no common parent, then they belong to 2 different sets
2507
2524
auto following_ctrls_b = get_following_controller_names (ctrl_b.info .name , controllers);
2508
- if (following_ctrls_b.empty ()) return true ;
2525
+ if (following_ctrls_b.empty ())
2526
+ {
2527
+ return true ;
2528
+ }
2509
2529
auto find_first_element = [&](const auto & controllers_list) -> size_t
2510
2530
{
2511
2531
auto it = std::find_if (
0 commit comments