From 9db62e512e45baa967366192a87ee8cc1f533d63 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Tue, 11 Feb 2025 09:58:17 -0700 Subject: [PATCH] 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..bbaf22e5fc --- /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; +}