Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Improve tests for parameters #366

Merged
merged 3 commits into from
May 15, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
113 changes: 64 additions & 49 deletions test_rclcpp/test/parameter_fixtures.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,85 +27,100 @@

const double test_epsilon = 1e-6;

void declare_test_parameters(std::shared_ptr<rclcpp::Node> node)
std::vector<rclcpp::Parameter>
get_test_parameters()
{
node->declare_parameter("foo");
node->declare_parameter("bar");
node->declare_parameter("barstr");
node->declare_parameter("baz");
node->declare_parameter("foo.first");
node->declare_parameter("foo.second");
node->declare_parameter("foobar");
}

void set_test_parameters(
std::shared_ptr<rclcpp::SyncParametersClient> parameters_client)
{
printf("Setting parameters\n");
// Set several differnet types of parameters.
auto set_parameters_results = parameters_client->set_parameters({
return {
rclcpp::Parameter("foo", 2),
rclcpp::Parameter("bar", "hello"),
rclcpp::Parameter("barstr", std::string("hello_str")),
rclcpp::Parameter("baz", 1.45),
rclcpp::Parameter("foo.first", 8),
rclcpp::Parameter("foo.second", 42),
rclcpp::Parameter("foobar", true),
});
};
}

void declare_test_parameters(std::shared_ptr<rclcpp::Node> node, int declare_up_to = -1)
{
auto parameters = get_test_parameters();

if (declare_up_to < 0 || declare_up_to > static_cast<int>(parameters.size())) {
declare_up_to = static_cast<int>(parameters.size());
}

for (int i = 0u; i < declare_up_to; ++i) {
node->declare_parameter(parameters[i].get_name());
}
}

void test_set_parameters_sync(
std::shared_ptr<rclcpp::SyncParametersClient> parameters_client,
int successful_up_to = -1)
{
printf("Setting parameters\n");
std::vector<rclcpp::Parameter> parameters = get_test_parameters();
auto set_parameters_results = parameters_client->set_parameters(parameters);
printf("Got set_parameters result\n");

ASSERT_EQ(set_parameters_results.size(), parameters.size());

// Check to see if they were set.
for (auto & result : set_parameters_results) {
if (successful_up_to < 0 || successful_up_to > static_cast<int>(parameters.size())) {
successful_up_to = static_cast<int>(parameters.size());
}

// Check success values
for (int i = 0u; i < successful_up_to; ++i) {
const auto & result = set_parameters_results[i];
ASSERT_TRUE(result.successful);
}
for (int i = successful_up_to; i < static_cast<int>(parameters.size()); ++i) {
const auto & result = set_parameters_results[i];
ASSERT_FALSE(result.successful);
}
}

void set_test_parameters_atomically(
std::shared_ptr<rclcpp::SyncParametersClient> parameters_client)
void test_set_parameters_atomically_sync(
std::shared_ptr<rclcpp::SyncParametersClient> parameters_client,
bool expect_result_successful = true)
{
printf("Setting parameters atomically\n");
// Set several differnet types of parameters.
auto set_parameters_result = parameters_client->set_parameters_atomically({
rclcpp::Parameter("foo", 2),
rclcpp::Parameter("bar", "hello"),
rclcpp::Parameter("barstr", std::string("hello_str")),
rclcpp::Parameter("baz", 1.45),
rclcpp::Parameter("foo.first", 8),
rclcpp::Parameter("foo.second", 42),
rclcpp::Parameter("foobar", true),
});
std::vector<rclcpp::Parameter> parameters = get_test_parameters();
auto set_parameters_result = parameters_client->set_parameters_atomically(parameters);
printf("Got set_parameters_atomically result\n");

// Check to see if they were set.
ASSERT_TRUE(set_parameters_result.successful);
ASSERT_EQ(set_parameters_result.successful, expect_result_successful);
}

void verify_set_parameters_async(
void test_set_parameters_async(
std::shared_ptr<rclcpp::Node> node,
std::shared_ptr<rclcpp::AsyncParametersClient> parameters_client)
std::shared_ptr<rclcpp::AsyncParametersClient> parameters_client,
int successful_up_to = -1)
{
printf("Setting parameters\n");
// Set several differnet types of parameters.
auto set_parameters_results = parameters_client->set_parameters({
rclcpp::Parameter("foo", 2),
rclcpp::Parameter("bar", "hello"),
rclcpp::Parameter("barstr", std::string("hello_str")),
rclcpp::Parameter("baz", 1.45),
rclcpp::Parameter("foo.first", 8),
rclcpp::Parameter("foo.second", 42),
rclcpp::Parameter("foobar", true),
});
std::vector<rclcpp::Parameter> parameters = get_test_parameters();
auto set_parameters_results = parameters_client->set_parameters(parameters);
rclcpp::spin_until_future_complete(node, set_parameters_results); // Wait for the results.
printf("Got set_parameters result\n");

// Check to see if they were set.
for (auto & result : set_parameters_results.get()) {
ASSERT_TRUE(result.successful);
if (successful_up_to < 0 || successful_up_to > static_cast<int>(parameters.size())) {
successful_up_to = static_cast<int>(parameters.size());
}

const auto results = set_parameters_results.get();
ASSERT_EQ(results.size(), parameters.size());

// Check success values
for (int i = 0u; i < successful_up_to; ++i) {
ASSERT_TRUE(results[i].successful);
}
for (int i = successful_up_to; i < static_cast<int>(parameters.size()); ++i) {
ASSERT_FALSE(results[i].successful);
}
}

void verify_test_parameters(
void test_get_parameters_sync(
std::shared_ptr<rclcpp::SyncParametersClient> parameters_client)
{
printf("Listing parameters with recursive depth\n");
Expand Down Expand Up @@ -215,7 +230,7 @@ void verify_test_parameters(
}
}

void verify_get_parameters_async(
void test_get_parameters_async(
std::shared_ptr<rclcpp::Node> node,
std::shared_ptr<rclcpp::AsyncParametersClient> parameters_client)
{
Expand Down
12 changes: 6 additions & 6 deletions test_rclcpp/test/test_local_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,8 @@ TEST(CLASSNAME(test_local_parameters, RMW_IMPLEMENTATION), local_synchronous) {
if (!parameters_client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
set_test_parameters(parameters_client);
verify_test_parameters(parameters_client);
test_set_parameters_sync(parameters_client);
test_get_parameters_sync(parameters_client);
}

TEST(CLASSNAME(test_local_parameters, RMW_IMPLEMENTATION), local_synchronous_repeated) {
Expand All @@ -85,10 +85,10 @@ TEST(CLASSNAME(test_local_parameters, RMW_IMPLEMENTATION), local_synchronous_rep
if (!parameters_client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
set_test_parameters(parameters_client);
test_set_parameters_sync(parameters_client);
for (int i = 0; i < 10; ++i) {
printf("iteration: %d\n", i);
verify_test_parameters(parameters_client);
test_get_parameters_sync(parameters_client);
}
}

Expand All @@ -100,8 +100,8 @@ TEST(CLASSNAME(test_local_parameters, RMW_IMPLEMENTATION), local_asynchronous) {
if (!parameters_client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}
verify_set_parameters_async(node, parameters_client);
verify_get_parameters_async(node, parameters_client);
test_set_parameters_async(node, parameters_client);
test_get_parameters_async(node, parameters_client);
}

class ParametersAsyncNode : public rclcpp::Node
Expand Down
14 changes: 11 additions & 3 deletions test_rclcpp/test/test_parameters_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,20 @@ int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);

auto node = rclcpp::Node::make_shared(
"test_parameters_server",
auto node_allow_undeclared = rclcpp::Node::make_shared(
"test_parameters_server_allow_undeclared",
"/",
rclcpp::NodeOptions().allow_undeclared_parameters(true));

rclcpp::spin(node);
auto node_must_declare = rclcpp::Node::make_shared(
"test_parameters_server_must_declare",
"/",
rclcpp::NodeOptions().allow_undeclared_parameters(false));

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node_allow_undeclared);
executor.add_node(node_must_declare);
executor.spin();

return 0;
}
69 changes: 60 additions & 9 deletions test_rclcpp/test/test_remote_parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ using namespace std::chrono_literals;

TEST(CLASSNAME(parameters, rmw_implementation), test_remote_parameters_async) {
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
std::string test_server_name = "test_parameters_server";
std::string test_server_name = "test_parameters_server_allow_undeclared";
// TODO(tfoote) make test_server name parameterizable
// if (argc >= 2) {
// test_server_name = argv[1];
Expand All @@ -47,14 +47,14 @@ TEST(CLASSNAME(parameters, rmw_implementation), test_remote_parameters_async) {
ASSERT_TRUE(false) << "service not available after waiting";
}

verify_set_parameters_async(node, parameters_client);
test_set_parameters_async(node, parameters_client);

verify_get_parameters_async(node, parameters_client);
test_get_parameters_async(node, parameters_client);
}

TEST(CLASSNAME(parameters, rmw_implementation), test_remote_parameters_sync) {
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
std::string test_server_name = "test_parameters_server";
std::string test_server_name = "test_parameters_server_allow_undeclared";

auto node = rclcpp::Node::make_shared(std::string("test_remote_parameters_sync"));

Expand All @@ -64,14 +64,14 @@ TEST(CLASSNAME(parameters, rmw_implementation), test_remote_parameters_sync) {
ASSERT_TRUE(false) << "service not available after waiting";
}

set_test_parameters(parameters_client);
test_set_parameters_sync(parameters_client);

verify_test_parameters(parameters_client);
test_get_parameters_sync(parameters_client);
}

TEST(CLASSNAME(parameters, rmw_implementation), test_set_remote_parameters_atomically_sync) {
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
std::string test_server_name = "test_parameters_server";
std::string test_server_name = "test_parameters_server_allow_undeclared";

auto node = rclcpp::Node::make_shared(std::string("test_set_remote_parameters_atomically_sync"));

Expand All @@ -81,7 +81,58 @@ TEST(CLASSNAME(parameters, rmw_implementation), test_set_remote_parameters_atomi
ASSERT_TRUE(false) << "service not available after waiting";
}

set_test_parameters_atomically(parameters_client);
test_set_parameters_atomically_sync(parameters_client);

verify_test_parameters(parameters_client);
test_get_parameters_sync(parameters_client);
}

TEST(CLASSNAME(parameters_must_declare, rmw_implementation), test_remote_parameters_async) {
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
std::string test_server_name = "test_parameters_server_must_declare";

auto node = rclcpp::Node::make_shared(std::string("test_remote_parameters_async"));

auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
node,
test_server_name);
if (!parameters_client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}

test_set_parameters_async(node, parameters_client, 0);
}

TEST(CLASSNAME(parameters_must_declare, rmw_implementation), test_remote_parameters_sync) {
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
std::string test_server_name = "test_parameters_server_must_declare";

auto node = rclcpp::Node::make_shared(std::string("test_remote_parameters_sync"));

auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(
node,
test_server_name);
if (!parameters_client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}

test_set_parameters_sync(parameters_client, 0);
}

TEST(
CLASSNAME(parameters_must_declare, rmw_implementation),
test_set_remote_parameters_atomically_sync)
{
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
std::string test_server_name = "test_parameters_server_must_declare";

auto node = rclcpp::Node::make_shared(std::string("test_set_remote_parameters_atomically_sync"));

auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(
node,
test_server_name);
if (!parameters_client->wait_for_service(20s)) {
ASSERT_TRUE(false) << "service not available after waiting";
}

test_set_parameters_atomically_sync(parameters_client, false);
}
5 changes: 4 additions & 1 deletion test_rclcpp/test/test_two_executables.py.in
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ import os
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch.actions import OpaqueFunction
from launch.substitutions import LaunchConfiguration

import launch_testing
import launch_testing.asserts
Expand All @@ -26,6 +27,7 @@ def generate_test_description(ready_fn):
cmd=cmd,
name='@TEST_EXECUTABLE1_NAME@',
env=env,
sigterm_timeout=LaunchConfiguration('sigterm_timeout', default=30)
)
launch_description.add_action(executable_1)

Expand All @@ -40,6 +42,7 @@ def generate_test_description(ready_fn):
cmd=cmd,
name='@TEST_EXECUTABLE2_NAME@',
env=env,
sigterm_timeout=LaunchConfiguration('sigterm_timeout', default=30)
)

launch_description.add_action(executable_2)
Expand All @@ -54,7 +57,7 @@ class TestTwoExecutables(unittest.TestCase):

def @TEST_NAME@(self, executable_2):
"""Test that the second executable terminates after a finite amount of time."""
self.proc_info.assertWaitForShutdown(process=executable_2, timeout=30)
self.proc_info.assertWaitForShutdown(process=executable_2, timeout=60)


@launch_testing.post_shutdown_test()
Expand Down