From 33a9708c167a348d1af6d4a3c87b36174c7b40ca Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Mon, 10 Feb 2025 11:37:16 -0700 Subject: [PATCH 1/9] Add logic to Ros2ControlManager to match ros2_control Signed-off-by: Paul Gesel --- .../src/controller_manager_plugin.cpp | 100 ++++++++++++++---- 1 file changed, 82 insertions(+), 18 deletions(-) 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 a348859f5a..4922fbe8f8 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 @@ -119,8 +119,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 @@ -370,35 +373,91 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan return c; } + static void simplifyControllerActivationDeactivation(std::vector& activate_controllers, + std::vector& deactivate_controllers) + { + // Convert vectors to sets for uniqueness + std::unordered_set set1(activate_controllers.begin(), activate_controllers.end()); + std::unordered_set set2(deactivate_controllers.begin(), deactivate_controllers.end()); + + // Find common elements + std::unordered_set common; + for (const auto& str : set1) + { + if (set2.count(str)) + { + common.insert(str); + } + } + + // Remove common elements from both sets + for (const auto& str : common) + { + set1.erase(str); + set2.erase(str); + } + + // Convert sets back to vectors + activate_controllers.assign(set1.begin(), set1.end()); + deactivate_controllers.assign(set2.begin(), set2.end()); + } + /** * \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& 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); + if (controller.size() > 1 && controller[0] == '/') + { + // Remove leading / from controller name + controller.erase(0, 1); + } queue.pop_back(); - for (const auto& dependency : dependency_map_[controller]) + 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_); @@ -471,6 +530,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. + simplifyControllerActivationDeactivation(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 +566,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 +578,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)); From 704dd4ef4fc0b0c2062546ec5c6052a367951c8c Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 09:58:17 -0700 Subject: [PATCH 2/9] Add Ros2ControlManager test Signed-off-by: Paul Gesel --- .../CMakeLists.txt | 4 + .../src/controller_manager_plugin.cpp | 5 + .../test/CMakeLists.txt | 8 + .../test/test_controller_manager_plugin.cpp | 174 ++++++++++++++++++ 4 files changed, 191 insertions(+) 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 a23dac9f96..433f968200 100644 --- 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 index 4922fbe8f8..7dcfda8409 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 @@ -376,6 +376,11 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan static void simplifyControllerActivationDeactivation(std::vector& activate_controllers, std::vector& deactivate_controllers) { + // 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 ROS2 control to through an error and reject the + // switch. The simplifyControllerActivationDeactivation function adds the logic needed to avoid this from happening. + // Convert vectors to sets for uniqueness std::unordered_set set1(activate_controllers.begin(), activate_controllers.end()); std::unordered_set set2(deactivate_controllers.begin(), deactivate_controllers.end()); 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..3717aed647 --- /dev/null +++ b/moveit_plugins/moveit_ros_control_interface/test/test_controller_manager_plugin.cpp @@ -0,0 +1,174 @@ +/********************************************************************* + * 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", std::bind(&MockControllersManagerService::handle_list_controllers_service, + this, std::placeholders::_1, std::placeholders::_2)); + switch_controller_service_ = create_service( + "controller_manager/switch_controller", + std::bind(&MockControllersManagerService::handle_switch_controller_service, this, std::placeholders::_1, + std::placeholders::_2)); + } + + controller_manager_msgs::srv::SwitchController::Request last_request; + +private: + void handle_list_controllers_service( + const std::shared_ptr /*request*/, + 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 handle_switch_controller_service( + const std::shared_ptr request, + 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; +} From 6180eb62648a607c69dd29d1782e84c46b18aea9 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 15:52:30 -0700 Subject: [PATCH 3/9] move simplifyControllerActivationDeactivation to function and add doxygen Signed-off-by: Paul Gesel --- .../src/controller_manager_plugin.cpp | 72 ++++++++++--------- 1 file changed, 38 insertions(+), 34 deletions(-) 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 7dcfda8409..49834ff150 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 @@ -83,6 +83,44 @@ std::string parseJointNameFromResource(const std::string& claimed_interface) return claimed_interface.substr(0, index); } +/** + * \brief Modified Activation/deactivation 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 ROS2 control to through an error and reject the + * switch. The simplifyControllerActivationDeactivation 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 simplifyControllerActivationDeactivation(std::vector& activate_controllers, + std::vector& deactivate_controllers) +{ + // Convert vectors to sets for uniqueness + std::unordered_set set1(activate_controllers.begin(), activate_controllers.end()); + std::unordered_set set2(deactivate_controllers.begin(), deactivate_controllers.end()); + + // Find common elements + std::unordered_set common; + for (const auto& str : set1) + { + if (set2.count(str)) + { + common.insert(str); + } + } + + // Remove common elements from both sets + for (const auto& str : common) + { + set1.erase(str); + set2.erase(str); + } + + // Convert sets back to vectors + activate_controllers.assign(set1.begin(), set1.end()); + deactivate_controllers.assign(set2.begin(), set2.end()); +} + MOVEIT_CLASS_FORWARD(Ros2ControlManager); // Defines Ros2ControlManagerPtr, ConstPtr, WeakPtr... etc /** @@ -373,40 +411,6 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan return c; } - static void simplifyControllerActivationDeactivation(std::vector& activate_controllers, - std::vector& deactivate_controllers) - { - // 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 ROS2 control to through an error and reject the - // switch. The simplifyControllerActivationDeactivation function adds the logic needed to avoid this from happening. - - // Convert vectors to sets for uniqueness - std::unordered_set set1(activate_controllers.begin(), activate_controllers.end()); - std::unordered_set set2(deactivate_controllers.begin(), deactivate_controllers.end()); - - // Find common elements - std::unordered_set common; - for (const auto& str : set1) - { - if (set2.count(str)) - { - common.insert(str); - } - } - - // Remove common elements from both sets - for (const auto& str : common) - { - set1.erase(str); - set2.erase(str); - } - - // Convert sets back to vectors - activate_controllers.assign(set1.begin(), set1.end()); - deactivate_controllers.assign(set2.begin(), set2.end()); - } - /** * \brief Filter lists for managed controller and computes switching set. * Stopped list might be extended by unsupported controllers that claim needed resources From 77da98f62dd217a8c2a4c10b4428ed0b97c1ff8a Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 18:28:04 -0700 Subject: [PATCH 4/9] move queue.pop_back up Signed-off-by: Paul Gesel --- .../src/controller_manager_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 49834ff150..be1baec920 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 @@ -429,12 +429,12 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan while (!queue.empty()) { auto controller = queue.back(); + queue.pop_back(); if (controller.size() > 1 && controller[0] == '/') { // Remove leading / from controller name controller.erase(0, 1); } - queue.pop_back(); if (dependencies.find(controller) == dependencies.end()) { continue; From 2210564d2d7645c0106a6d73ea4ce10b9545be66 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 18:31:37 -0700 Subject: [PATCH 5/9] Update moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../src/controller_manager_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 be1baec920..2ab97c7901 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 @@ -84,7 +84,7 @@ std::string parseJointNameFromResource(const std::string& claimed_interface) } /** - * \brief Modified Activation/deactivation to conform to ROS 2 control expectations. + * \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 ROS2 control to through an error and reject the From 1fb2585227b80c2c63ec79307c082c2cf103333d Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 18:31:45 -0700 Subject: [PATCH 6/9] Update moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../src/controller_manager_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2ab97c7901..30cc01229f 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 @@ -87,7 +87,7 @@ std::string parseJointNameFromResource(const std::string& claimed_interface) * \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 ROS2 control to through an error and reject the + * cause B to be in both the activation and deactivate list. This causes ROS 2 control to throw an error and reject the * switch. The simplifyControllerActivationDeactivation function adds the logic needed to avoid this from happening. * @param[in] activate_controllers controllers to activate * @param[in] deactivate_controllers controllers to deactivate From 90c315c0c9c4b6b5683825a2b40be3861de7cb44 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 18:32:15 -0700 Subject: [PATCH 7/9] Update moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../src/controller_manager_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 30cc01229f..dd1ebd2514 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 @@ -88,7 +88,7 @@ std::string parseJointNameFromResource(const std::string& claimed_interface) * \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. The simplifyControllerActivationDeactivation function adds the logic needed to avoid this from happening. + * 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 */ From a2570ad3d1a259f3c24ef2ee338af0b894ca3a3b Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 18:34:43 -0700 Subject: [PATCH 8/9] pr feedback Signed-off-by: Paul Gesel --- .../src/controller_manager_plugin.cpp | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) 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 be1baec920..86cf0caf72 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 @@ -88,37 +88,37 @@ std::string parseJointNameFromResource(const std::string& claimed_interface) * \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 ROS2 control to through an error and reject the - * switch. The simplifyControllerActivationDeactivation function adds the logic needed to avoid this from happening. + * switch. The deconflictControllerActivationLists 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 simplifyControllerActivationDeactivation(std::vector& activate_controllers, - std::vector& deactivate_controllers) +void deconflictControllerActivationLists(std::vector& activate_controllers, + std::vector& deactivate_controllers) { // Convert vectors to sets for uniqueness - std::unordered_set set1(activate_controllers.begin(), activate_controllers.end()); - std::unordered_set set2(deactivate_controllers.begin(), deactivate_controllers.end()); + 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; - for (const auto& str : set1) + std::unordered_set common_controllers; + for (const auto& str : activate_set) { - if (set2.count(str)) + if (deactivate_set.count(str)) { - common.insert(str); + common_controllers.insert(str); } } // Remove common elements from both sets - for (const auto& str : common) + for (const auto& controller_name : common_controllers) { - set1.erase(str); - set2.erase(str); + activate_set.erase(controller_name); + deactivate_set.erase(controller_name); } // Convert sets back to vectors - activate_controllers.assign(set1.begin(), set1.end()); - deactivate_controllers.assign(set2.begin(), set2.end()); + 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 @@ -542,7 +542,7 @@ 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. - simplifyControllerActivationDeactivation(request->activate_controllers, request->deactivate_controllers); + 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. From 2d91d45451787a37db30abd93a460347d7917291 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Wed, 12 Feb 2025 07:58:36 -0700 Subject: [PATCH 9/9] clang fixes Signed-off-by: Paul Gesel --- .../test/test_controller_manager_plugin.cpp | 25 +++++++++++-------- 1 file changed, 15 insertions(+), 10 deletions(-) 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 index 3717aed647..9f73fa1542 100644 --- 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 @@ -48,20 +48,25 @@ class MockControllersManagerService final : public rclcpp::Node MockControllersManagerService() : Node("list_controllers_service") { list_controller_service_ = create_service( - "controller_manager/list_controllers", std::bind(&MockControllersManagerService::handle_list_controllers_service, - this, std::placeholders::_1, std::placeholders::_2)); + "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", - std::bind(&MockControllersManagerService::handle_switch_controller_service, this, std::placeholders::_1, - std::placeholders::_2)); + [this](const std::shared_ptr& request, + const std::shared_ptr& response) { + handleSwitchControllerService(request, response); + }); } controller_manager_msgs::srv::SwitchController::Request last_request; private: - void handle_list_controllers_service( - const std::shared_ptr /*request*/, - std::shared_ptr response) + 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"; @@ -84,9 +89,9 @@ class MockControllersManagerService final : public rclcpp::Node response->controller = { controller_a, controller_b }; } - void handle_switch_controller_service( - const std::shared_ptr request, - std::shared_ptr response) + void handleSwitchControllerService( + const std::shared_ptr& request, + const std::shared_ptr& response) { last_request = *request; for (const auto& deactivate : request->deactivate_controllers)