fix the sorting logic for complex and large use-cases (fixes #1170)

controller_manager/src/controller_manager.cpp

@@ -2448,7 +2448,15 @@ bool ControllerManager::controller_sorting(
         (is_controller_active(ctrl_b.c) || is_controller_inactive(ctrl_b.c))))
   {
     if (is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) return true;
-    return false;
+    // When both the controllers are inactive, do not change their initial order
+    auto ctrl_a_it = std::find_if(
+      controllers.begin(), controllers.end(),
+      std::bind(controller_name_compare, std::placeholders::_1, ctrl_a.info.name));
+    auto ctrl_b_it = std::find_if(
+      controllers.begin(), controllers.end(),
+      std::bind(controller_name_compare, std::placeholders::_1, ctrl_b.info.name));
+    return std::distance(controllers.begin(), ctrl_a_it) <
+           std::distance(controllers.begin(), ctrl_b_it);
   }
 
   const std::vector<std::string> cmd_itfs = ctrl_a.c->command_interface_configuration().names;
@@ -2457,8 +2465,25 @@ bool ControllerManager::controller_sorting(
   {
     // The case of the controllers that don't have any command interfaces. For instance,
     // joint_state_broadcaster
+    // If the controller b is also under the same condition, then maintain their initial order
+    if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable())
+    {
+      auto ctrl_a_it = std::find_if(
+        controllers.begin(), controllers.end(),
+        std::bind(controller_name_compare, std::placeholders::_1, ctrl_a.info.name));
+      auto ctrl_b_it = std::find_if(
+        controllers.begin(), controllers.end(),
+        std::bind(controller_name_compare, std::placeholders::_1, ctrl_b.info.name));
+      return std::distance(controllers.begin(), ctrl_a_it) <
+             std::distance(controllers.begin(), ctrl_b_it);
+    }
     return true;
   }
+  else if (ctrl_b.c->command_interface_configuration().names.empty() || !ctrl_b.c->is_chainable())
+  {
+    // If only the controller b is a broadcaster or non chainable type , then swap the controllers
+    return false;
+  }
   else
   {
     auto following_ctrls = get_following_controller_names(ctrl_a.info.name, controllers);
@@ -2516,7 +2541,7 @@ bool ControllerManager::controller_sorting(
       const auto ctrl_b_chain_first_controller = find_first_element(following_ctrls_b);
       if (ctrl_a_chain_first_controller < ctrl_b_chain_first_controller)
       {
-        return true;
+        return false;
       }
     }