Merge branch 'main' into pr-fix-servo-after-unpause

sjahr · web-flow · commit 1b3aea6428a5 · 2025-02-14T17:29:08.000+01:00
diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt
@@ -67,6 +67,10 @@ target_include_directories(moveit_ros_control_interface_empty_plugin
 ament_target_dependencies(moveit_ros_control_interface_empty_plugin
                           ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
 
+if(BUILD_TESTING)
+  add_subdirectory(test)
+endif()
+
 # ##############################################################################
 # Install ##
 # ##############################################################################
diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp
@@ -83,6 +83,44 @@ std::string parseJointNameFromResource(const std::string& claimed_interface)
   return claimed_interface.substr(0, index);
 }
 
+/**
+ * \brief  Modifies controller activation/deactivation lists to conform to ROS 2 control expectations.
+ * \detail Activation/deactivation is expected to be disjoint. For example, if controller B is a dependency of A (A
+ * chains to B) but controller B is also a dependency of C (B chains to B), then the switch from A->B to C->B would
+ * cause B to be in both the activation and deactivate list. This causes ROS 2 control to throw an error and reject the
+ * switch. This function adds the logic needed to avoid this from happening.
+ * @param[in] activate_controllers  controllers to activate
+ * @param[in] deactivate_controllers  controllers to deactivate
+ */
+void deconflictControllerActivationLists(std::vector<std::string>& activate_controllers,
+                                         std::vector<std::string>& deactivate_controllers)
+{
+  // Convert vectors to sets for uniqueness
+  std::unordered_set activate_set(activate_controllers.begin(), activate_controllers.end());
+  std::unordered_set deactivate_set(deactivate_controllers.begin(), deactivate_controllers.end());
+
+  // Find common elements
+  std::unordered_set<std::string> common_controllers;
+  for (const auto& str : activate_set)
+  {
+    if (deactivate_set.count(str))
+    {
+      common_controllers.insert(str);
+    }
+  }
+
+  // Remove common elements from both sets
+  for (const auto& controller_name : common_controllers)
+  {
+    activate_set.erase(controller_name);
+    deactivate_set.erase(controller_name);
+  }
+
+  // Convert sets back to vectors
+  activate_controllers.assign(activate_set.begin(), activate_set.end());
+  deactivate_controllers.assign(deactivate_set.begin(), deactivate_set.end());
+}
+
 MOVEIT_CLASS_FORWARD(Ros2ControlManager);  // Defines Ros2ControlManagerPtr, ConstPtr, WeakPtr... etc
 
 /**
@@ -119,8 +157,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
   rclcpp::Client<controller_manager_msgs::srv::ListControllers>::SharedPtr list_controllers_service_;
   rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_service_;
 
-  // Chained controllers have dependencies (other controllers which must be running)
+  // Chained controllers have dependent controllers (other controllers which must be started if the chained controller is started)
   std::unordered_map<std::string /* controller name */, std::vector<std::string> /* dependencies */> dependency_map_;
+  // Controllers may have preceding chained controllers (other chained controllers which must be shutdown if the controller is shutdown)
+  std::unordered_map<std::string /* controller name */, std::vector<std::string> /* reverse dependencies */>
+      dependency_map_reverse_;
 
   /**
    * \brief Check if given controller is active
@@ -373,32 +414,59 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
   /**
    * \brief Filter lists for managed controller and computes switching set.
    * Stopped list might be extended by unsupported controllers that claim needed resources
-   * @param activate vector of controllers to be activated
-   * @param deactivate vector of controllers to be deactivated
+   * @param activate_base vector of controllers to be activated
+   * @param deactivate_base vector of controllers to be deactivated
    * @return true if switching succeeded
    */
   bool switchControllers(const std::vector<std::string>& activate_base,
                          const std::vector<std::string>& deactivate_base) override
   {
-    // add controller dependencies
-    std::vector<std::string> activate = activate_base;
-    std::vector<std::string> deactivate = deactivate_base;
-    for (auto controllers : { &activate, &deactivate })
-    {
-      auto queue = *controllers;
+    // add_all_dependencies traverses the provided dependency map and appends the results to the controllers vector.
+    auto add_all_dependencies = [](const std::unordered_map<std::string, std::vector<std::string>>& dependencies,
+                                   const std::function<bool(const std::string&)>& should_include,
+                                   std::vector<std::string>& controllers) {
+      auto queue = controllers;
       while (!queue.empty())
       {
         auto controller = queue.back();
-        controller.erase(0, 1);
         queue.pop_back();
-        for (const auto& dependency : dependency_map_[controller])
+        if (controller.size() > 1 && controller[0] == '/')
+        {
+          // Remove leading / from controller name
+          controller.erase(0, 1);
+        }
+        if (dependencies.find(controller) == dependencies.end())
+        {
+          continue;
+        }
+        for (const auto& dependency : dependencies.at(controller))
         {
           queue.push_back(dependency);
-          controllers->push_back("/" + dependency);
+          if (should_include(dependency))
+          {
+            controllers.push_back("/" + dependency);
+          }
         }
       }
-    }
-    // activation dependencies must be started first, but they are processed last, so the order needs to be flipped
+    };
+
+    std::vector<std::string> activate = activate_base;
+    add_all_dependencies(
+        dependency_map_,
+        [this](const std::string& dependency) {
+          return active_controllers_.find(dependency) == active_controllers_.end();
+        },
+        activate);
+    std::vector<std::string> deactivate = deactivate_base;
+    add_all_dependencies(
+        dependency_map_reverse_,
+        [this](const std::string& dependency) {
+          return active_controllers_.find(dependency) != active_controllers_.end();
+        },
+        deactivate);
+
+    // The dependencies for chained controller activation must be started first, but they are processed last, so the
+    // order needs to be flipped
     std::reverse(activate.begin(), activate.end());
 
     std::scoped_lock<std::mutex> lock(controllers_mutex_);
@@ -471,6 +539,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
       }
     }
 
+    // ROS2 Control expects simplified controller activation/deactivation.
+    // E.g. a controller should not be stopped and started at the same time, rather it should be removed from both the
+    // activation and deactivation request.
+    deconflictControllerActivationLists(request->activate_controllers, request->deactivate_controllers);
+
     // Setting level to STRICT means that the controller switch will only be committed if all controllers are
     // successfully activated or deactivated.
     request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
@@ -502,6 +575,9 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
     {
       controller_name_map[result->controller[i].name] = i;
     }
+
+    dependency_map_.clear();
+    dependency_map_reverse_.clear();
     for (auto& controller : result->controller)
     {
       if (controller.chain_connections.size() > 1)
@@ -511,14 +587,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
                             "one controller is not supported.");
         return false;
       }
-      dependency_map_[controller.name].clear();
       for (const auto& chained_controller : controller.chain_connections)
       {
         auto ind = controller_name_map[chained_controller.name];
         dependency_map_[controller.name].push_back(chained_controller.name);
-        std::copy(result->controller[ind].required_command_interfaces.begin(),
-                  result->controller[ind].required_command_interfaces.end(),
-                  std::back_inserter(controller.required_command_interfaces));
+        dependency_map_reverse_[chained_controller.name].push_back(controller.name);
         std::copy(result->controller[ind].reference_interfaces.begin(),
                   result->controller[ind].reference_interfaces.end(),
                   std::back_inserter(controller.required_command_interfaces));
diff --git a/moveit_plugins/moveit_ros_control_interface/test/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/test/CMakeLists.txt
@@ -0,0 +1,8 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(pluginlib REQUIRED)
+ament_add_gtest(test_controller_manager_plugin
+                test_controller_manager_plugin.cpp TIMEOUT 20)
+target_link_libraries(test_controller_manager_plugin
+                      moveit_ros_control_interface_plugin)
+target_include_directories(test_controller_manager_plugin
+                           PRIVATE ${CMAKE_SOURCE_DIR}/include)
diff --git a/moveit_plugins/moveit_ros_control_interface/test/test_controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/test/test_controller_manager_plugin.cpp
@@ -0,0 +1,179 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include <vector>
+
+#include <gtest/gtest.h>
+
+#include <controller_manager_msgs/srv/detail/list_controllers__struct.hpp>
+#include <controller_manager_msgs/srv/detail/switch_controller__struct.hpp>
+#include <eigen3/Eigen/Eigen>
+#include <moveit/controller_manager/controller_manager.hpp>
+#include <pluginlib/class_loader.hpp>
+
+class MockControllersManagerService final : public rclcpp::Node
+{
+public:
+  MockControllersManagerService() : Node("list_controllers_service")
+  {
+    list_controller_service_ = create_service<controller_manager_msgs::srv::ListControllers>(
+        "controller_manager/list_controllers",
+        [this](const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>& request,
+               const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response>& response) {
+          handleListControllersService(request, response);
+        });
+    switch_controller_service_ = create_service<controller_manager_msgs::srv::SwitchController>(
+        "controller_manager/switch_controller",
+        [this](const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request>& request,
+               const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response>& response) {
+          handleSwitchControllerService(request, response);
+        });
+  }
+
+  controller_manager_msgs::srv::SwitchController::Request last_request;
+
+private:
+  void handleListControllersService(
+      const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request>& /*request*/,
+      const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response>& response)
+  {
+    controller_manager_msgs::msg::ChainConnection chain_connection_a;
+    chain_connection_a.name = "B";
+    chain_connection_a.reference_interfaces = { "ref_1", "ref_2", "ref_3" };
+    controller_manager_msgs::msg::ControllerState controller_a;
+    controller_a.chain_connections.push_back(chain_connection_a);
+    controller_a.name = "A";
+    controller_a.is_chained = true;
+    controller_a.required_command_interfaces = { "jnt_1", "jnt_2", "jnt_3" };
+    controller_a.type = "joint_trajectory_controller/JointTrajectoryController";
+    controller_a.state = activate_set_.find(controller_a.name) != activate_set_.end() ? "active" : "inactive";
+
+    controller_manager_msgs::msg::ControllerState controller_b;
+    controller_b.name = "B";
+    controller_b.required_command_interfaces = { "jnt_4", "jnt_5" };
+    controller_b.reference_interfaces = { "ref_1", "ref_2", "ref_3" };
+    controller_b.type = "joint_trajectory_controller/JointTrajectoryController";
+    controller_b.state = activate_set_.find(controller_b.name) != activate_set_.end() ? "active" : "inactive";
+
+    response->controller = { controller_a, controller_b };
+  }
+
+  void handleSwitchControllerService(
+      const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Request>& request,
+      const std::shared_ptr<controller_manager_msgs::srv::SwitchController::Response>& response)
+  {
+    last_request = *request;
+    for (const auto& deactivate : request->deactivate_controllers)
+    {
+      activate_set_.erase(deactivate);
+    }
+    for (const auto& activate : request->activate_controllers)
+    {
+      activate_set_.insert(activate);
+    }
+    response->ok = true;
+  }
+
+  std::unordered_set<std::string> activate_set_;
+  rclcpp::Service<controller_manager_msgs::srv::ListControllers>::SharedPtr list_controller_service_;
+  rclcpp::Service<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_controller_service_;
+};
+
+TEST(ControllerManagerPlugin, SwitchControllers)
+{
+  // GIVEN a ClassLoader for MoveItControllerManager
+  pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> loader(
+      "moveit_core", "moveit_controller_manager::MoveItControllerManager");
+
+  // WHEN we load the custom plugin defined in this package
+  // THEN loading succeeds
+  auto instance = loader.createUniqueInstance("moveit_ros_control_interface/Ros2ControlManager");
+
+  const auto mock_service = std::make_shared<MockControllersManagerService>();
+  rclcpp::executors::SingleThreadedExecutor executor;
+  std::thread ros_thread([&executor, &mock_service]() {
+    executor.add_node(mock_service);
+    executor.spin();
+  });
+  instance->initialize(mock_service);
+
+  // A and B should start
+  instance->switchControllers({ "/A" }, {});
+  EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector<std::string>({ "A", "B" }));
+  EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector<std::string>({}));
+  // A and B should stop
+  instance->switchControllers({}, { "/B" });
+  EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector<std::string>({}));
+  EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector<std::string>({ "A", "B" }));
+  // Only B should start
+  instance->switchControllers({ "/B" }, {});
+  EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector<std::string>({ "B" }));
+  EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector<std::string>({}));
+  // Only B should stop
+  instance->switchControllers({}, { "/B" });
+  EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector<std::string>({}));
+  EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector<std::string>({ "B" }));
+  // Multiple activations results in only 1
+  instance->switchControllers({ "/B", "/B", "/B", "/B" }, {});
+  EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector<std::string>({ "B" }));
+  EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector<std::string>({}));
+  instance->switchControllers({}, { "/B" });
+  // Multiple activation and deactivate of same controller results in empty list
+  instance->switchControllers({ "/B", "/A" }, { "/A", "/A", "/A" });
+  EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector<std::string>({ "B" }));
+  EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector<std::string>({}));
+
+  executor.cancel();
+  ros_thread.join();
+}
+
+TEST(ControllerManagerPlugin, LoadMoveItControllerManagerPlugin)
+{
+  // GIVEN a ClassLoader for MoveItControllerManager
+  pluginlib::ClassLoader<moveit_controller_manager::MoveItControllerManager> loader(
+      "moveit_core", "moveit_controller_manager::MoveItControllerManager");
+
+  // WHEN we load the custom plugin defined in this package
+  // THEN loading succeeds
+  EXPECT_NO_THROW(loader.createUniqueInstance("moveit_ros_control_interface/Ros2ControlManager"));
+}
+
+int main(int argc, char** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  const int result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}
