Added test for the remapped service from the controller using the spa…

…wner args
ros-controls · Sep 11, 2024 · 912d1a7 · 912d1a7
1 parent 7b33265
commit 912d1a7
Show file tree

Hide file tree

Showing 5 changed files with 57 additions and 2 deletions.
diff --git 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
@@ -34,6 +34,7 @@
   <test_depend>python3-coverage</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
+  <test_depend>example_interfaces</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp
@@ -97,6 +97,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<example_interfaces::srv::SetBool>(
+    service_name,
+    [this](
+      const std::shared_ptr<example_interfaces::srv::SetBool::Request> request,
+      std::shared_ptr<example_interfaces::srv::SetBool::Response> 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
@@ -15,11 +15,14 @@
 #ifndef TEST_CONTROLLER__TEST_CONTROLLER_HPP_
 #define TEST_CONTROLLER__TEST_CONTROLLER_HPP_
 
+#include <memory>
 #include <string>
 #include <vector>
 
 #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,8 +71,7 @@ class TestController : public controller_interface::ControllerInterface
   CONTROLLER_MANAGER_PUBLIC
   std::vector<double> get_state_interface_data() const;
 
-  const std::string & getRobotDescription() const;
-
+  rclcpp::Service<example_interfaces::srv::SetBool>::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
@@ -425,6 +425,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<rclcpp::Node> node = rclcpp::Node::make_shared("set_bool_client");
+  auto set_bool_service = node->create_client<example_interfaces::srv::SetBool>("/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<example_interfaces::srv::SetBool>("/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<example_interfaces::srv::SetBool>("/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<controller_manager::ControllerManager>
 {