Robustify controller spawner and add integration test with many contr…

…ollers (#1501)

---------

Co-authored-by: Dr. Denis <denis@stoglrobotics.de>

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):

controller_manager/test/test_spawner_unspawner.cpp

@@ -374,6 +374,32 @@ TEST_F(TestLoadController, spawner_test_fallback_controllers)
  }
 }
 
+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<int>(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<controller_manager::ControllerManager>
 {