Skip to content

Commit

Permalink
Robustify controller spawner and add integration test with many contr…
Browse files Browse the repository at this point in the history
…ollers (#1501)

---------

Co-authored-by: Dr. Denis <[email protected]>
(cherry picked from commit 80c264f)

# Conflicts:
#	controller_manager/test/test_spawner_unspawner.cpp
  • Loading branch information
fmauch authored and mergify[bot] committed Aug 16, 2024
1 parent 8b32c33 commit fb4eeec
Show file tree
Hide file tree
Showing 2 changed files with 131 additions and 7 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Expand All @@ -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):
Expand Down
84 changes: 84 additions & 0 deletions controller_manager/test/test_spawner_unspawner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -252,6 +252,90 @@ TEST_F(TestLoadController, unload_on_kill)
ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
}

<<<<<<< HEAD
=======
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);
}
}

>>>>>>> 80c264f (Robustify controller spawner and add integration test with many controllers (#1501))
class TestLoadControllerWithoutRobotDescription
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
Expand Down

0 comments on commit fb4eeec

Please sign in to comment.