Robustify spawner (backport #1501)

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

@@ -277,6 +277,87 @@ TEST_F(TestLoadController, unload_on_kill)
   ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
 }
 
+TEST_F(TestLoadController, spawner_test_fallback_controllers)
+{
+  const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
+                                     "/test/test_controller_spawner_with_fallback_controllers.yaml";
+
+  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));
+  cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
+
+  ControllerManagerRunner cm_runner(this);
+  EXPECT_EQ(
+    call_spawner(
+      "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
+      "-p " +
+      test_file_path),
+    0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  }
+
+  // Try to spawn now the controller with fallback controllers inside the yaml
+  EXPECT_EQ(
+    call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);
+
+  ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
+  {
+    auto ctrl_1 = cm_->get_loaded_controllers()[0];
+    ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
+    ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
+    ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+    auto ctrl_2 = cm_->get_loaded_controllers()[1];
+    ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
+    ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(
+      ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
+    ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+
+    auto ctrl_3 = cm_->get_loaded_controllers()[2];
+    ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
+    ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
+    ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
+    ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
+  }
+}
+
+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>
 {