From 75e7efd1ae2ce5316114f73345b182f600717aa3 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Tue, 14 Nov 2023 07:57:14 +0100
Subject: [PATCH] Fix the controller sorting bug when the interfaces are not
 configured (fixes #1164) (#1165)

---
 controller_manager/src/controller_manager.cpp | 13 ++++++++++
 .../test_chainable_controller.cpp             | 24 +++++++++++++++++--
 .../test/test_controller/test_controller.cpp  | 24 +++++++++++++++++--
 3 files changed, 57 insertions(+), 4 deletions(-)

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index b6c277126e..e61e3c769b 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -148,6 +148,9 @@ 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)
   {
@@ -206,6 +209,8 @@ 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;
     auto cmd_itfs = ctrl.c->command_interface_configuration().names;
     for (const auto & itf : cmd_itfs)
     {
@@ -2436,6 +2441,14 @@ bool ControllerManager::controller_sorting(
   const ControllerSpec & ctrl_a, const ControllerSpec & ctrl_b,
   const std::vector<controller_manager::ControllerSpec> & controllers)
 {
+  // If the neither of the controllers are configured, then return false
+  if (!((is_controller_active(ctrl_a.c) || is_controller_inactive(ctrl_a.c)) &&
+        (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;
+  }
+
   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())
diff --git a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp
index b02c2e65ef..d21957a0b4 100644
--- a/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp
+++ b/controller_manager/test/test_chainable_controller/test_chainable_controller.cpp
@@ -32,13 +32,33 @@ TestChainableController::TestChainableController()
 controller_interface::InterfaceConfiguration
 TestChainableController::command_interface_configuration() const
 {
-  return cmd_iface_cfg_;
+  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.");
+  }
 }
 
 controller_interface::InterfaceConfiguration
 TestChainableController::state_interface_configuration() const
 {
-  return state_iface_cfg_;
+  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.");
+  }
 }
 
 controller_interface::return_type TestChainableController::update_reference_from_subscribers(
diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
index 97670e9a7a..06f76bd044 100644
--- a/controller_manager/test/test_controller/test_controller.cpp
+++ b/controller_manager/test/test_controller/test_controller.cpp
@@ -31,12 +31,32 @@ TestController::TestController()
 
 controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
 {
-  return cmd_iface_cfg_;
+  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.");
+  }
 }
 
 controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
 {
-  return state_iface_cfg_;
+  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.");
+  }
 }
 
 controller_interface::return_type TestController::update(