Support Chained Controllers in Moveit

moveit_plugins/moveit_ros_control_interface/CMakeLists.txt

@@ -63,6 +63,17 @@ if(BUILD_TESTING)
 
   # Run all lint tests in package.xml except those listed above
   ament_lint_auto_find_test_dependencies()
+
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(controller_manager_plugin_test
+          test/controller_manager_plugin_test.cpp)
+  ament_target_dependencies(controller_manager_plugin_test
+      ${THIS_PACKAGE_INCLUDE_DEPENDS}
+  )
+
+  target_include_directories(controller_manager_plugin_test PRIVATE src)
+  target_include_directories(controller_manager_plugin_test PRIVATE include)
+
 endif()
 
 #############

moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp

@@ -52,6 +52,7 @@
 #include <rclcpp/time.hpp>
 #include <map>
 #include <memory>
+#include <queue>
 
 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.plugins.ros_control_interface");
 static const rclcpp::Duration CONTROLLER_INFORMATION_VALIDITY_AGE = rclcpp::Duration::from_seconds(1.0);
@@ -146,7 +147,8 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll
     managed_controllers_.clear();
     active_controllers_.clear();
 
-    const auto& result = result_future.get();
+    auto result = result_future.get();
+    MoveItControllerManager::fixChainedControllers(result);
     for (const controller_manager_msgs::msg::ControllerState& controller : result->controller)
     {
       // If the controller is active, add it to the map of active controllers.
@@ -419,6 +421,101 @@ class MoveItControllerManager : public moveit_controller_manager::MoveItControll
     }
     return true;
   }
+  /**
+   * \brief fixChainedControllers modifies ListControllers service response if it contains chained controllers.
+   * Since chained controllers cannot be written to directly, they are removed from the response and their interface
+   * are propagated back to the first controller with a non-chained input
+   */
+  static void fixChainedControllers(std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response>& result)
+  {
+    // chained_to maps controllers to a set of controllers that it feeds into
+    std::unordered_map<controller_manager_msgs::msg::ControllerState*,
+                       std::unordered_set<controller_manager_msgs::msg::ControllerState*>>
+        chained_to;
+    // chained_from maps a controller to a set of controller it receives from
+    std::unordered_map<controller_manager_msgs::msg::ControllerState*,
+                       std::unordered_set<controller_manager_msgs::msg::ControllerState*>>
+        chained_from;
+    // controller_name_map maps controller names to the controller pointer
+    std::unordered_map<std::string, controller_manager_msgs::msg::ControllerState*> controller_name_map;
+    for (auto& c : result->controller)
+    {
+      controller_name_map[c.name] = &c;
+    }
+    for (auto& c : result->controller)
+    {
+      std::vector<std::string> non_chained_interfaces;
+      for (auto it = c.claimed_interfaces.begin(); it != c.claimed_interfaces.end(); it++)
+      {
+        std::string interface = *it;
+        auto name = interface.substr(0, interface.find('/'));
+        // if the controller interface begins with a controller name, then it is chained
+        if (controller_name_map.find(name) != controller_name_map.end())
+        {
+          chained_to[&c].insert(controller_name_map[name]);
+          chained_from[controller_name_map[name]].insert(&c);
+        }
+        else
+        {
+          non_chained_interfaces.push_back(interface);
+        }
+      }
+      // only use non-chained interfaces, these interfaces will accumulate and
+      // propagate to the controller chain input controller
+      c.claimed_interfaces = non_chained_interfaces;
+    }
+
+    // create a queue of all controllers that are chained to
+    // these need to be processed first
+    std::queue<controller_manager_msgs::msg::ControllerState*> chained_to_queue;
+    for (const auto& chained_controllers : chained_to)
+    {
+      for (auto chained_to_controllers : chained_controllers.second)
+      {
+        chained_to_queue.push(chained_to_controllers);
+      }
+    }
+
+    while (!chained_to_queue.empty())
+    {
+      auto c = chained_to_queue.front();
+      chained_to_queue.pop();
+      // if c is chained_from then it depends on another controller,
+      // so push to back of queue
+      if (chained_to.find(c) != chained_to.end())
+      {
+        chained_to_queue.push(c);
+        continue;
+      }
+      auto dependent_set = chained_from[c];
+      // add command interface to chained controllers
+      for (auto c2 : dependent_set)
+      {
+        for (const auto& interface : c->claimed_interfaces)
+        {
+          c2->claimed_interfaces.push_back(interface);
+        }
+        c2->required_command_interfaces = c2->claimed_interfaces;
+        chained_to[c2].erase(c);
+        // enable any controller that depended on c
+        if (chained_to[c2].empty())
+        {
+          chained_to.erase(c2);
+        }
+      }
+    }
+
+    std::vector<controller_manager_msgs::msg::ControllerState> cs_final;
+    // if controller is in the chained from map, then it is an intermediate and should not be included
+    for (auto& c : result->controller)
+    {
+      if (chained_from.find(&c) == chained_from.end())
+      {
+        cs_final.push_back(c);
+      }
+    }
+    result->controller = cs_final;
+  }
 };
 /**
  *  \brief MoveItMultiControllerManager discovers all running ros_control node and delegates member function to the

moveit_plugins/moveit_ros_control_interface/test/controller_manager_plugin_test.cpp

@@ -0,0 +1,87 @@
+#include "controller_manager_plugin.cpp"
+
+// Testing
+#include <gtest/gtest.h>
+
+std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> generate_response(int num_chained_controller,
+                                                                                           int rotation)
+{
+  auto result = std::make_shared<controller_manager_msgs::srv::ListControllers::Response>();
+  controller_manager_msgs::msg::ControllerState cs;
+  result->controller.resize(2 + num_chained_controller);
+
+  for (int i = 0; i <= num_chained_controller; i++)
+  {
+    cs = result->controller[(i + rotation) % (num_chained_controller + 2)];
+    cs.name = "chained_controller_" + std::to_string(i);
+    std::string chained_to_name;
+    if (i == num_chained_controller - 1)
+    {
+      chained_to_name = "admittance_controller";
+    }
+    else
+    {
+      chained_to_name = "chained_controller_" + std::to_string(i + 1);
+    }
+
+    cs.claimed_interfaces = {
+      chained_to_name + "/shoulder_pan_joint/position",  chained_to_name + "/shoulder_pan_joint/velocity",
+      chained_to_name + "/shoulder_lift_joint/position", chained_to_name + "/shoulder_lift_joint/velocity",
+      chained_to_name + "/elbow_joint/position",         chained_to_name + "/elbow_joint/velocity",
+      chained_to_name + "/wrist_1_joint/position",       chained_to_name + "/wrist_1_joint/velocity",
+      chained_to_name + "/wrist_2_joint/position",       chained_to_name + "/wrist_2_joint/velocity",
+      chained_to_name + "/wrist_3_joint/position",       chained_to_name + "/wrist_3_joint/velocity"
+    };
+
+    cs.claimed_interfaces.push_back("physical_interface" + std::to_string(i));
+
+    result->controller[(i + rotation) % (num_chained_controller + 2)] = cs;
+  }
+
+  cs = result->controller[(num_chained_controller + rotation) % (num_chained_controller + 2)];
+  cs.name = "admittance_controller";
+  cs.claimed_interfaces = { "shoulder_pan_joint/position", "shoulder_lift_joint/position", "elbow_joint/position",
+                            "wrist_1_joint/position",      "wrist_2_joint/position",       "wrist_3_joint/position" };
+  cs.required_command_interfaces = cs.claimed_interfaces;
+  result->controller[(num_chained_controller + rotation) % (num_chained_controller + 2)] = cs;
+
+  cs = result->controller[(num_chained_controller + 1 + rotation) % (num_chained_controller + 2)];
+  cs.name = "faked_forces_controller";
+  cs.claimed_interfaces = { "tcp_fts_sensor/force.x",  "tcp_fts_sensor/force.y",  "tcp_fts_sensor/force.z",
+                            "tcp_fts_sensor/torque.x", "tcp_fts_sensor/torque.y", "tcp_fts_sensor/torque.z" };
+  cs.required_command_interfaces = cs.claimed_interfaces;
+  result->controller[(num_chained_controller + 1 + rotation) % (num_chained_controller + 2)] = cs;
+  return result;
+}
+
+TEST(ControllerManagerPluginTest, FixChainedCntrollerInControllerListTest)
+{
+  std::vector<std::string> expected_interfaces = { "physical_interface0",         "physical_interface1",
+                                                   "shoulder_pan_joint/position", "shoulder_lift_joint/position",
+                                                   "elbow_joint/position",        "wrist_1_joint/position",
+                                                   "wrist_2_joint/position",      "wrist_3_joint/position" };
+
+  int num_chained_controller = 2;
+  for (int rotation = 0; rotation < 4; rotation++)
+  {
+    auto result = generate_response(num_chained_controller, rotation);
+    moveit_ros_control_interface::MoveItControllerManager::fixChainedControllers(result);
+    auto chained_controller = result->controller[0];
+    if (chained_controller.name != "chained_controller_0")
+    {
+      chained_controller = result->controller[1];
+    }
+
+    ASSERT_EQ(expected_interfaces, chained_controller.claimed_interfaces);
+  }
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+
+  int result = RUN_ALL_TESTS();
+
+  return result;
+}