Add controller dependency graph for ::AUTO switch mode

.pre-commit-config.yaml

@@ -85,7 +85,7 @@ repos:
         entry: ament_cpplint
         language: system
         files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
-        args: ["--linelength=100", "--filter=-whitespace/newline"]
+        args: ["--linelength=100", "--filter=-whitespace/newline,-readability/fn_size"]
 
   # Cmake hooks
   - repo: local

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -366,6 +366,10 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 
 protected:
   pal_statistics::RegistrationsRAII stats_registrations_;
+
+public:
+  using SharedPtr = std::shared_ptr<ControllerInterfaceBase>;
+  using WeakPtr = std::weak_ptr<ControllerInterfaceBase>;
 };
 
 using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;

controller_manager/include/controller_manager/controller_manager.hpp

@@ -501,6 +501,14 @@ class ControllerManager : public rclcpp::Node
     const std::string & ctrl_name, std::vector<std::string>::iterator controller_iterator,
     bool append_to_controller);
 
+  /**
+   * @brief Build the controller chain topology information based on the provided controllers.
+   *  This method constructs a directed graph representing the dependencies between controllers.
+   *  It analyzes the relationships between controllers, such as which controllers depend on others,
+   *  and builds a directed graph to represent these dependencies.
+   */
+  void build_controllers_topology_info(const std::vector<ControllerSpec> & controllers);
+
   /**
    * @brief Method to publish the state of the controller manager.
    * The state includes the list of controllers and the list of hardware interfaces along with
@@ -635,6 +643,7 @@ class ControllerManager : public rclcpp::Node
   std::unique_ptr<rclcpp::PreShutdownCallbackHandle> preshutdown_cb_handle_{nullptr};
   RTControllerListWrapper rt_controllers_wrapper_;
   std::unordered_map<std::string, ControllerChainSpec> controller_chain_spec_;
+  ControllerChainDependencyGraph controller_chain_dependency_graph_;
   std::vector<std::string> ordered_controllers_names_;
   /// mutex copied from ROS1 Control, protects service callbacks
   /// not needed if we're guaranteed that the callbacks don't come from multiple threads

controller_manager/test/test_controller_manager.cpp

@@ -73,6 +73,139 @@ class TestControllerManagerWithStrictness
   }
 };
 
+TEST(ControllerManagerDependencyGraph, controller_chain_dependency_graph)
+{
+  {
+    controller_manager::ControllerChainDependencyGraph graph;
+    // Let's test the case of A -> B -> C -> D && B -> E -> F
+    controller_manager::ControllerPeerInfo A;
+    A.name = "A";
+    A.command_interfaces = {"B3", "B4"};
+    A.reference_interfaces = {"A3", "A4"};
+    A.state_interfaces = {"A5", "A6"};
+    controller_manager::ControllerPeerInfo B;
+    B.name = "B";
+    B.command_interfaces = {"C3", "C4", "E3", "E4"};
+    B.reference_interfaces = {"B3", "B4"};
+    B.state_interfaces = {"B5", "B6"};
+    controller_manager::ControllerPeerInfo C;
+    C.name = "C";
+    C.command_interfaces = {"D3", "D4"};
+    C.reference_interfaces = {"C3", "C4"};
+    C.state_interfaces = {"C5", "C6"};
+    controller_manager::ControllerPeerInfo D;
+    D.name = "D";
+    D.command_interfaces = {"D1", "D2"};
+    D.reference_interfaces = {"D3", "D4"};
+    D.state_interfaces = {"D5", "D6"};
+    controller_manager::ControllerPeerInfo E;
+    E.name = "E";
+    E.command_interfaces = {"F3", "F4"};
+    E.reference_interfaces = {"E3", "E4", "E7", "E8"};
+    E.state_interfaces = {"E5", "E6"};
+    controller_manager::ControllerPeerInfo F;
+    F.name = "F";
+    F.command_interfaces = {"F1", "F2"};
+    F.reference_interfaces = {"F3", "F4"};
+    F.state_interfaces = {"F5", "F6"};
+
+    graph.add_dependency(A, B);
+    graph.add_dependency(B, C);
+    graph.add_dependency(C, D);
+    graph.add_dependency(B, E);
+    graph.add_dependency(E, F);
+
+    EXPECT_THAT(
+      graph.get_dependencies_to_activate("A"),
+      testing::UnorderedElementsAre("A", "B", "C", "D", "E", "F"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_activate("B"),
+      testing::UnorderedElementsAre("B", "C", "D", "E", "F"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("C", "D"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre("D"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("E"), testing::UnorderedElementsAre("E", "F"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre("F"));
+
+    EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre("A"));
+    EXPECT_THAT(graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", "B"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("B", "A", "C"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("D"), testing::UnorderedElementsAre("C", "B", "A", "D"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("B", "A", "E"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("E", "B", "A", "F"));
+  }
+
+  {
+    RCLCPP_INFO(rclcpp::get_logger("test"), "Testing controller chain dependency graph");
+    RCLCPP_INFO(rclcpp::get_logger("test"), "+++++++++++++++++++++++++++++++++++++++++++++++++");
+    controller_manager::ControllerChainDependencyGraph graph;
+    // Let's test the case of A -> B -> C -> D && E -> B && E -> F
+    controller_manager::ControllerPeerInfo A;
+    A.name = "A";
+    A.command_interfaces = {"B3", "B4"};
+    A.reference_interfaces = {"A3", "A4"};
+    A.state_interfaces = {"A5", "A6"};
+    controller_manager::ControllerPeerInfo B;
+    B.name = "B";
+    B.command_interfaces = {"C3", "C4"};
+    B.reference_interfaces = {"B3", "B4", "B5", "B6"};
+    B.state_interfaces = {"B5", "B6"};
+    controller_manager::ControllerPeerInfo C;
+    C.name = "C";
+    C.command_interfaces = {"D3", "D4"};
+    C.reference_interfaces = {"C3", "C4"};
+    C.state_interfaces = {"C5", "C6"};
+    controller_manager::ControllerPeerInfo D;
+    D.name = "D";
+    D.command_interfaces = {"D1", "D2"};
+    D.reference_interfaces = {"D3", "D4"};
+    D.state_interfaces = {"D5", "D6"};
+    controller_manager::ControllerPeerInfo E;
+    E.name = "E";
+    E.command_interfaces = {"F3", "F4", "B5", "B6"};
+    E.reference_interfaces = {"E3", "E4"};
+    E.state_interfaces = {"E5", "E6"};
+    controller_manager::ControllerPeerInfo F;
+    F.name = "F";
+    F.command_interfaces = {"F1", "F2"};
+    F.reference_interfaces = {"F3", "F4"};
+    F.state_interfaces = {"F5", "F6"};
+
+    graph.add_dependency(A, B);
+    graph.add_dependency(B, C);
+    graph.add_dependency(C, D);
+    graph.add_dependency(E, B);
+    graph.add_dependency(E, F);
+
+    EXPECT_THAT(
+      graph.get_dependencies_to_activate("A"),
+      testing::UnorderedElementsAre("A", "B", "C", "D", "E", "F"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_activate("B"), testing::UnorderedElementsAre("B", "C", "D"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("C"), testing::UnorderedElementsAre("C", "D"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("D"), testing::UnorderedElementsAre("D"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_activate("E"),
+      testing::UnorderedElementsAre("E", "F", "B", "C", "D", "A"));
+    EXPECT_THAT(graph.get_dependencies_to_activate("F"), testing::UnorderedElementsAre("F"));
+
+    EXPECT_THAT(graph.get_dependencies_to_deactivate("A"), testing::UnorderedElementsAre("A"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("B"), testing::UnorderedElementsAre("A", "B", "E"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("C"), testing::UnorderedElementsAre("C", "B", "A", "E"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("D"),
+      testing::UnorderedElementsAre("D", "C", "B", "A", "E"));
+    EXPECT_THAT(graph.get_dependencies_to_deactivate("E"), testing::UnorderedElementsAre("E", "A"));
+    EXPECT_THAT(
+      graph.get_dependencies_to_deactivate("F"), testing::UnorderedElementsAre("F", "E", "A"));
+  }
+}
+
 class TestControllerManagerRobotDescription
 : public ControllerManagerFixture<controller_manager::ControllerManager>
 {