diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index 1b0f308613..d13cb594b3 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -58,10 +58,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 5447040537..4bbd6306b1 100644
--- a/controller_manager/package.xml
+++ b/controller_manager/package.xml
@@ -36,6 +36,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 107c557dbb..a4159d7fbe 100644
--- a/controller_manager/test/test_spawner_unspawner.cpp
+++ b/controller_manager/test/test_spawner_unspawner.cpp
@@ -664,6 +664,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
{