From 4e2b28f1c214a1c9897bb2ed93c81cdee5757b68 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Fri, 23 Jun 2023 00:48:32 +0200
Subject: [PATCH] Perform controller sorting at the end of switch_controller

---
 controller_manager/src/controller_manager.cpp | 56 +++++++++++++++++++
 1 file changed, 56 insertions(+)

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 69a3bbb5f34..df047089a07 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -1034,6 +1034,62 @@ controller_interface::return_type ControllerManager::switch_controller(
       controller.info.claimed_interfaces.clear();
     }
   }
+
+  // Reordering the controllers
+  /// @note The following conditions needs to be handled while ordering the controller list
+  /// 1. The controllers that do not use any state or command interfaces are updated first
+  /// 2. The controllers that use only the state system interfaces only are updated next
+  /// 3. The controllers that use any of an another controller's reference interface are updated
+  /// before the preceeding controller
+  /// 4. The controllers that use the controller's estimated interfaces are updated after the
+  /// preceeding controller
+  /// 5. The controllers that only use the system's command interfaces are updated last
+  /// 6. All inactive controllers go at the end of the list
+  auto sorting_lambda = [](ControllerSpec ctrl_a, ControllerSpec ctrl_b){
+    // If the controllers are not active, then go to the end of the list
+    if(!is_controller_active(ctrl_a.c) || !is_controller_active(ctrl_b.c))
+      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())
+    {
+      // The case of the controllers that don't have any command interfaces. For instance,
+      // joint_state_broadcaster
+      return true;
+    }
+    else
+    {
+      if (ctrl_b.c->is_chainable())
+      {
+        // If the ctrl_a's command interface is the one exported by the ctrl_b then ctrl_a should be
+        // infront of ctrl_b
+        auto it = std::find_if(
+          cmd_itfs.begin(), cmd_itfs.end(),
+          [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
+        if (it != cmd_itfs.end())
+        {
+          return true;
+        }
+        else
+        {
+          // If the ctrl_a's state interface is the one exported by the ctrl_b then ctrl_b should be
+          // infront of ctrl_a
+          auto state_it = std::find_if(
+            cmd_itfs.begin(), cmd_itfs.end(),
+            [ctrl_b](auto itf) { return (itf.find(ctrl_b.info.name) != std::string::npos); });
+          if (it != cmd_itfs.end())
+          {
+            return false;
+          }
+        }
+      }
+      // The rest of the cases, basically end up at the end of the list
+      return false;
+    }
+  };
+  std::sort(to.begin(), to.end(), sorting_lambda);
+
   // switch lists
   rt_controllers_wrapper_.switch_updated_list(guard);
   // clear unused list