diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e44fb5fb9b..e5dac58de6 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -49,10 +49,12 @@ target_link_libraries(ros2_control_node PRIVATE if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ros2_control_test_assets REQUIRED) + find_package(example_interfaces REQUIRED) # Plugin Libraries that are built and installed for use in testing add_library(test_controller SHARED test/test_controller/test_controller.cpp) target_link_libraries(test_controller PUBLIC controller_manager) + ament_target_dependencies(test_controller PUBLIC example_interfaces) target_compile_definitions(test_controller PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") pluginlib_export_plugin_description_file(controller_interface test/test_controller/test_controller.xml) install( diff --git a/controller_manager/package.xml b/controller_manager/package.xml index 030fcd3f7a..a57eabb980 100644 --- a/controller_manager/package.xml +++ b/controller_manager/package.xml @@ -34,6 +34,7 @@ python3-coverage hardware_interface_testing ros2_control_test_assets + example_interfaces ament_cmake diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 04ae8c02c2..d20e7598a9 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -101,6 +101,17 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + const std::string service_name = get_node()->get_name() + std::string("/set_bool"); + service_ = get_node()->create_service( + service_name, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); + response->success = request->data; + }); return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller/test_controller.hpp b/controller_manager/test/test_controller/test_controller.hpp index d57fd9ddd9..ee9e668cfa 100644 --- a/controller_manager/test/test_controller/test_controller.hpp +++ b/controller_manager/test/test_controller/test_controller.hpp @@ -15,11 +15,14 @@ #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_ #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_ +#include #include #include #include "controller_interface/controller_interface.hpp" #include "controller_manager/visibility_control.h" +#include "example_interfaces/srv/set_bool.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" namespace test_controller @@ -68,10 +71,9 @@ class TestController : public controller_interface::ControllerInterface CONTROLLER_MANAGER_PUBLIC std::vector get_state_interface_data() const; - const std::string & getRobotDescription() const; - void set_external_commands_for_testing(const std::vector & commands); + rclcpp::Service::SharedPtr service_; unsigned int internal_counter = 0; bool simulate_cleanup_failure = false; // Variable where we store when cleanup was called, pointer because the controller diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 74e1efeeed..afc93dc868 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -439,6 +439,45 @@ TEST_F(TestLoadController, spawner_with_many_controllers) } } +TEST_F(TestLoadController, test_spawner_parsed_controller_ros_args) +{ + ControllerManagerRunner cm_runner(this); + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + std::stringstream ss; + + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0); + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + + // Now as the controller is active, we can call check for the service + std::shared_ptr node = rclcpp::Node::make_shared("set_bool_client"); + auto set_bool_service = node->create_client("/set_bool"); + ASSERT_FALSE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_1_set_bool_service = + node->create_client("/ctrl_1/set_bool"); + ASSERT_TRUE(ctrl_1_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(ctrl_1_set_bool_service->service_is_ready()); + + // Now test the remapping of the service name with the controller_ros_args + EXPECT_EQ( + call_spawner( + "ctrl_2 -c test_controller_manager --controller-ros-args '-r /ctrl_2/set_bool:=/set_bool'"), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + // Now as the controller is active, we can call check for the remapped service + ASSERT_TRUE(set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_TRUE(set_bool_service->service_is_ready()); + // Now check the service availability in the controller namespace + auto ctrl_2_set_bool_service = + node->create_client("/ctrl_2/set_bool"); + ASSERT_FALSE(ctrl_2_set_bool_service->wait_for_service(std::chrono::seconds(2))); + ASSERT_FALSE(ctrl_2_set_bool_service->service_is_ready()); +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture {