From ff3427b8dcfc69e6e9b864f852c6be3ab39d18cc Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 12 Feb 2025 11:45:29 -0700 Subject: [PATCH] Add logic to Ros2ControlManager to match ros2_control (#3332) * Add logic to Ros2ControlManager to match ros2_control Signed-off-by: Paul Gesel * Add Ros2ControlManager test Signed-off-by: Paul Gesel * move simplifyControllerActivationDeactivation to function and add doxygen Signed-off-by: Paul Gesel * move queue.pop_back up Signed-off-by: Paul Gesel * Update moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Update moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Update moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * pr feedback Signed-off-by: Paul Gesel * clang fixes Signed-off-by: Paul Gesel --------- Signed-off-by: Paul Gesel Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: Sebastian Jahr (cherry picked from commit dbf07b1acdf452c1a7eba82988110948ddd0c1e3) # Conflicts: # moveit_plugins/moveit_ros_control_interface/CMakeLists.txt # moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp --- .../CMakeLists.txt | 10 + .../src/controller_manager_plugin.cpp | 110 +++++++++-- .../test/CMakeLists.txt | 8 + .../test/test_controller_manager_plugin.cpp | 179 ++++++++++++++++++ 4 files changed, 291 insertions(+), 16 deletions(-) create mode 100644 moveit_plugins/moveit_ros_control_interface/test/CMakeLists.txt create mode 100644 moveit_plugins/moveit_ros_control_interface/test/test_controller_manager_plugin.cpp diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 8194d7883d..b0ee9fca06 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -66,10 +66,20 @@ ament_target_dependencies(${PROJECT_NAME}_empty_plugin if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) +<<<<<<< HEAD # These don't pass yet, disable them for now set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) set(ament_cmake_uncrustify_FOUND TRUE) +======= +if(BUILD_TESTING) + add_subdirectory(test) +endif() + +# ############################################################################## +# Install ## +# ############################################################################## +>>>>>>> dbf07b1ac (Add logic to Ros2ControlManager to match ros2_control (#3332)) # Run all lint tests in package.xml except those listed above ament_lint_auto_find_test_dependencies() 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 index 7982b7dcb8..72b1b7ba66 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -74,6 +74,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& activate_controllers, + std::vector& 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 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 /** @@ -110,8 +148,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan rclcpp::Client::SharedPtr list_controllers_service_; rclcpp::Client::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 /* dependencies */> dependency_map_; + // Controllers may have preceding chained controllers (other chained controllers which must be shutdown if the controller is shutdown) + std::unordered_map /* reverse dependencies */> + dependency_map_reverse_; /** * \brief Check if given controller is active @@ -362,32 +403,64 @@ 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 +<<<<<<< HEAD * @param activate * @param deactivate +======= + * @param activate_base vector of controllers to be activated + * @param deactivate_base vector of controllers to be deactivated +>>>>>>> dbf07b1ac (Add logic to Ros2ControlManager to match ros2_control (#3332)) * @return true if switching succeeded */ bool switchControllers(const std::vector& activate_base, const std::vector& deactivate_base) override { - // add controller dependencies - std::vector activate = activate_base; - std::vector 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>& dependencies, + const std::function& should_include, + std::vector& 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 activate = activate_base; + add_all_dependencies( + dependency_map_, + [this](const std::string& dependency) { + return active_controllers_.find(dependency) == active_controllers_.end(); + }, + activate); + std::vector 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 lock(controllers_mutex_); @@ -442,6 +515,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; @@ -473,6 +551,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) @@ -481,14 +562,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 new file mode 100644 index 0000000000..363bdd7b34 --- /dev/null +++ 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 new file mode 100644 index 0000000000..9f73fa1542 --- /dev/null +++ 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 + +#include + +#include +#include +#include +#include +#include + +class MockControllersManagerService final : public rclcpp::Node +{ +public: + MockControllersManagerService() : Node("list_controllers_service") + { + list_controller_service_ = create_service( + "controller_manager/list_controllers", + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + handleListControllersService(request, response); + }); + switch_controller_service_ = create_service( + "controller_manager/switch_controller", + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + handleSwitchControllerService(request, response); + }); + } + + controller_manager_msgs::srv::SwitchController::Request last_request; + +private: + void handleListControllersService( + const std::shared_ptr& /*request*/, + const std::shared_ptr& 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& request, + const std::shared_ptr& 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 activate_set_; + rclcpp::Service::SharedPtr list_controller_service_; + rclcpp::Service::SharedPtr switch_controller_service_; +}; + +TEST(ControllerManagerPlugin, SwitchControllers) +{ + // GIVEN a ClassLoader for MoveItControllerManager + pluginlib::ClassLoader 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(); + 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({ "A", "B" })); + EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector({})); + // A and B should stop + instance->switchControllers({}, { "/B" }); + EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector({})); + EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector({ "A", "B" })); + // Only B should start + instance->switchControllers({ "/B" }, {}); + EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector({ "B" })); + EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector({})); + // Only B should stop + instance->switchControllers({}, { "/B" }); + EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector({})); + EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector({ "B" })); + // Multiple activations results in only 1 + instance->switchControllers({ "/B", "/B", "/B", "/B" }, {}); + EXPECT_EQ(mock_service->last_request.activate_controllers, std::vector({ "B" })); + EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector({})); + 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({ "B" })); + EXPECT_EQ(mock_service->last_request.deactivate_controllers, std::vector({})); + + executor.cancel(); + ros_thread.join(); +} + +TEST(ControllerManagerPlugin, LoadMoveItControllerManagerPlugin) +{ + // GIVEN a ClassLoader for MoveItControllerManager + pluginlib::ClassLoader 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; +}