diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index be11a4b782..3f5a820fae 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -32,7 +32,39 @@ class ServiceNotFoundError(Exception): pass -def service_caller(node, service_name, service_type, request, service_timeout=0.0): +def service_caller( + node, + service_name, + service_type, + request, + service_timeout=0.0, + call_timeout=10.0, + max_attempts=3, +): + """ + Abstraction of a service call. + + Has an optional timeout to find the service, receive the answer to a call + and a mechanism to retry a call of no response is received. + + @param node Node object to be associated with + @type rclpy.node.Node + @param service_name Service URL + @type str + @param request The request to be sent + @type service request type + @param service_timeout Timeout (in seconds) to wait until the service is available. 0 means + waiting forever, retrying every 10 seconds. + @type float + @param call_timeout Timeout (in seconds) for getting a response + @type float + @param max_attempts Number of attempts until a valid response is received. With some + middlewares it can happen, that the service response doesn't reach the client leaving it in + a waiting state forever. + @type int + @return The service response + + """ cli = node.create_client(service_type, service_name) while not cli.service_is_ready(): @@ -44,12 +76,20 @@ def service_caller(node, service_name, service_type, request, service_timeout=0. node.get_logger().warn(f"Could not contact service {service_name}") node.get_logger().debug(f"requester: making request: {request}\n") - future = cli.call_async(request) - rclpy.spin_until_future_complete(node, future) - if future.result() is not None: - return future.result() - else: - raise RuntimeError(f"Exception while calling service: {future.exception()}") + future = None + for attempt in range(max_attempts): + future = cli.call_async(request) + rclpy.spin_until_future_complete(node, future, timeout_sec=call_timeout) + if future.result() is None: + node.get_logger().warning( + f"Failed getting a result from calling {service_name} in " + f"{service_timeout}. (Attempt {attempt+1} of {max_attempts}.)" + ) + else: + return future.result() + raise RuntimeError( + f"Could not successfully call service {service_name} after {max_attempts} attempts." + ) def configure_controller(node, controller_manager_name, controller_name, service_timeout=0.0): diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index be261ece96..a22f7a4bde 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -277,6 +277,32 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +TEST_F(TestLoadController, spawner_with_many_controllers) +{ + std::stringstream ss; + const size_t num_controllers = 50; + const std::string controller_base_name = "ctrl_"; + for (size_t i = 0; i < num_controllers; i++) + { + const std::string controller_name = controller_base_name + std::to_string(static_cast(i)); + cm_->set_parameter( + rclcpp::Parameter(controller_name + ".type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + ss << controller_name << " "; + } + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner(ss.str() + " -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), num_controllers); + + for (size_t i = 0; i < num_controllers; i++) + { + auto ctrl = cm_->get_loaded_controllers()[i]; + ASSERT_EQ(ctrl.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ(ctrl.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } +} + class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture {