diff --git a/test_rclcpp/test/parameter_fixtures.hpp b/test_rclcpp/test/parameter_fixtures.hpp index ba32c424..0d581697 100644 --- a/test_rclcpp/test/parameter_fixtures.hpp +++ b/test_rclcpp/test/parameter_fixtures.hpp @@ -27,23 +27,10 @@ const double test_epsilon = 1e-6; -void declare_test_parameters(std::shared_ptr node) +std::vector +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 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")), @@ -51,61 +38,89 @@ void set_test_parameters( rclcpp::Parameter("foo.first", 8), rclcpp::Parameter("foo.second", 42), rclcpp::Parameter("foobar", true), - }); + }; +} + +void declare_test_parameters(std::shared_ptr node, int declare_up_to = -1) +{ + auto parameters = get_test_parameters(); + + if (declare_up_to < 0 || declare_up_to > static_cast(parameters.size())) { + declare_up_to = static_cast(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 parameters_client, + int successful_up_to = -1) +{ + printf("Setting parameters\n"); + std::vector 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(parameters.size())) { + successful_up_to = static_cast(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(parameters.size()); ++i) { + const auto & result = set_parameters_results[i]; + ASSERT_FALSE(result.successful); + } } -void set_test_parameters_atomically( - std::shared_ptr parameters_client) +void test_set_parameters_atomically_sync( + std::shared_ptr 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 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 node, - std::shared_ptr parameters_client) + std::shared_ptr 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 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(parameters.size())) { + successful_up_to = static_cast(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(parameters.size()); ++i) { + ASSERT_FALSE(results[i].successful); } } -void verify_test_parameters( +void test_get_parameters_sync( std::shared_ptr parameters_client) { printf("Listing parameters with recursive depth\n"); @@ -215,7 +230,7 @@ void verify_test_parameters( } } -void verify_get_parameters_async( +void test_get_parameters_async( std::shared_ptr node, std::shared_ptr parameters_client) { diff --git a/test_rclcpp/test/test_local_parameters.cpp b/test_rclcpp/test/test_local_parameters.cpp index d99cdc3e..56a6239d 100644 --- a/test_rclcpp/test/test_local_parameters.cpp +++ b/test_rclcpp/test/test_local_parameters.cpp @@ -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) { @@ -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); } } @@ -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 diff --git a/test_rclcpp/test/test_parameters_server.cpp b/test_rclcpp/test/test_parameters_server.cpp index 982e92e7..3488c81b 100644 --- a/test_rclcpp/test/test_parameters_server.cpp +++ b/test_rclcpp/test/test_parameters_server.cpp @@ -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; } diff --git a/test_rclcpp/test/test_remote_parameters.cpp b/test_rclcpp/test/test_remote_parameters.cpp index 3b06c52f..bd10b9e6 100644 --- a/test_rclcpp/test/test_remote_parameters.cpp +++ b/test_rclcpp/test/test_remote_parameters.cpp @@ -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]; @@ -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")); @@ -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")); @@ -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( + 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( + 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( + 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); }