From 0f42d78b86f81ba35a71eebd8564f64ead6b16e3 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 17 Aug 2022 11:16:34 +0800 Subject: [PATCH 1/4] support regex match for parameter client Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/parameter_map.hpp | 3 +- rclcpp/src/rclcpp/parameter_client.cpp | 23 ++------- rclcpp/src/rclcpp/parameter_map.cpp | 4 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 49 +++++++++++++++++++ .../load_complicated_parameters.yaml | 25 ++++++++++ .../test_node/no_valid_parameters.yaml | 4 ++ 6 files changed, 87 insertions(+), 21 deletions(-) create mode 100644 rclcpp/test/resources/test_node/load_complicated_parameters.yaml create mode 100644 rclcpp/test/resources/test_node/no_valid_parameters.yaml diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 303eac4a95..797aa00120 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -53,11 +53,12 @@ parameter_value_from(const rcl_variant_t * const c_value); /// Get the ParameterMap from a yaml file. /// \param[in] yaml_filename full name of the yaml file. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. /// \returns an instance of a parameter map /// \throws from rcl error of rcl_parse_yaml_file() RCLCPP_PUBLIC ParameterMap -parameter_map_from_yaml_file(const std::string & yaml_filename); +parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn = nullptr); } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 38ced0e1a5..41c97eb13d 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -290,7 +290,8 @@ std::shared_future> AsyncParametersClient::load_parameters( const std::string & yaml_filename) { - rclcpp::ParameterMap parameter_map = rclcpp::parameter_map_from_yaml_file(yaml_filename); + rclcpp::ParameterMap parameter_map = + rclcpp::parameter_map_from_yaml_file(yaml_filename, remote_node_name_.c_str()); return this->load_parameters(parameter_map); } @@ -298,25 +299,11 @@ std::shared_future> AsyncParametersClient::load_parameters( const rclcpp::ParameterMap & parameter_map) { - std::vector parameters; - std::string remote_name = remote_node_name_.substr(remote_node_name_.substr(1).find("/") + 2); - for (const auto & params : parameter_map) { - std::string node_full_name = params.first; - std::string node_name = node_full_name.substr(node_full_name.find("/*/") + 3); - if (node_full_name == remote_node_name_ || - node_full_name == "/**" || - (node_name == remote_name)) - { - for (const auto & param : params.second) { - parameters.push_back(param); - } - } - } - - if (parameters.size() == 0) { + auto iter = parameter_map.find(remote_node_name_); + if (iter == parameter_map.end() || iter->second.size() == 0) { throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); } - auto future_result = set_parameters(parameters); + auto future_result = set_parameters(iter->second); return future_result; } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index cb458e7db3..600fbec682 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -143,7 +143,7 @@ rclcpp::parameter_value_from(const rcl_variant_t * const c_param_value) } ParameterMap -rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) +rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); rcl_params_t * rcl_parameters = rcl_yaml_node_struct_init(allocator); @@ -153,5 +153,5 @@ rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename) rclcpp::exceptions::throw_from_rcl_error(RCL_RET_ERROR); } - return rclcpp::parameter_map_from(rcl_parameters); + return rclcpp::parameter_map_from(rcl_parameters, node_fqn); } diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 4cd9e671be..697a4ef5c8 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -948,3 +948,52 @@ TEST_F(TestParameterClient, sync_parameter_load_parameters) { auto list_parameters = synchronous_client->list_parameters({}, 3); ASSERT_EQ(list_parameters.names.size(), static_cast(5)); } + +/* + Coverage for async load_parameters with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_complicated_regex) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "load_complicated_parameters.yaml").string(); + auto load_future = asynchronous_client->load_parameters(parameters_filepath); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + ASSERT_EQ(param.get_value(), "last_one_win"); +} + +/* + Coverage for async load_parameters to load file without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY}; + const std::string parameters_filepath = ( + test_resources_path / "test_node" / "no_valid_parameters.yaml").string(); + EXPECT_THROW( + asynchronous_client->load_parameters(parameters_filepath), + rclcpp::exceptions::InvalidParametersException); +} diff --git a/rclcpp/test/resources/test_node/load_complicated_parameters.yaml b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml new file mode 100644 index 0000000000..7722f636c2 --- /dev/null +++ b/rclcpp/test/resources/test_node/load_complicated_parameters.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + bar: 5 + foo: 3.5 + a_value: "first" + +/*: + load_node: + ros__parameters: + bar_foo: "ok" + a_value: "second" + +namespace: + load_node: + ros__parameters: + foo_bar: true + a_value: "third" + +bar: + ros__parameters: + fatal: 10 + +/**/namespace/*: + ros__parameters: + a_value: "last_one_win" diff --git a/rclcpp/test/resources/test_node/no_valid_parameters.yaml b/rclcpp/test/resources/test_node/no_valid_parameters.yaml new file mode 100644 index 0000000000..a75356cd77 --- /dev/null +++ b/rclcpp/test/resources/test_node/no_valid_parameters.yaml @@ -0,0 +1,4 @@ +/no/valid/parameters/node: + ros__parameters: + bar: 5 + foo: 3.5 From 9196dc9cb548b77ad0f06624df70d8708013c83b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 20 Aug 2022 11:01:27 +0800 Subject: [PATCH 2/4] address review that change behavior of a public method Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/parameter_client.hpp | 3 + rclcpp/include/rclcpp/parameter_map.hpp | 9 ++ rclcpp/src/rclcpp/parameter_client.cpp | 17 +++- rclcpp/src/rclcpp/parameter_map.cpp | 27 +++++- rclcpp/test/rclcpp/test_parameter_client.cpp | 87 ++++++++++++++++++++ 5 files changed, 136 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/parameter_client.hpp b/rclcpp/include/rclcpp/parameter_client.hpp index 5e509bbf10..1bbe4c2ebc 100644 --- a/rclcpp/include/rclcpp/parameter_client.hpp +++ b/rclcpp/include/rclcpp/parameter_client.hpp @@ -185,6 +185,9 @@ class AsyncParametersClient /** * This function filters the parameters to be set based on the node name. * + * If two duplicate keys exist in node names belongs to one FQN, there is no guarantee + * which one could be set. + * * \param parameter_map named parameters to be loaded * \return the future of the set_parameter service used to load the parameters * \throw InvalidParametersException if there is no parameter to set diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 797aa00120..08f46a9291 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -60,6 +60,15 @@ RCLCPP_PUBLIC ParameterMap parameter_map_from_yaml_file(const std::string & yaml_filename, const char * node_fqn = nullptr); +/// Get the Parameters from ParameterMap. +/// \param[in] parameter_map a parameter map. +/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. +/// \returns a list of a parameter +/// \throws from rcl error of rcl_parse_yaml_file() +RCLCPP_PUBLIC +std::vector +parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn = nullptr); + } // namespace rclcpp #endif // RCLCPP__PARAMETER_MAP_HPP_ diff --git a/rclcpp/src/rclcpp/parameter_client.cpp b/rclcpp/src/rclcpp/parameter_client.cpp index 41c97eb13d..64e415e82f 100644 --- a/rclcpp/src/rclcpp/parameter_client.cpp +++ b/rclcpp/src/rclcpp/parameter_client.cpp @@ -292,18 +292,27 @@ AsyncParametersClient::load_parameters( { rclcpp::ParameterMap parameter_map = rclcpp::parameter_map_from_yaml_file(yaml_filename, remote_node_name_.c_str()); - return this->load_parameters(parameter_map); + + auto iter = parameter_map.find(remote_node_name_); + if (iter == parameter_map.end() || iter->second.size() == 0) { + throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); + } + auto future_result = set_parameters(iter->second); + + return future_result; } std::shared_future> AsyncParametersClient::load_parameters( const rclcpp::ParameterMap & parameter_map) { - auto iter = parameter_map.find(remote_node_name_); - if (iter == parameter_map.end() || iter->second.size() == 0) { + std::vector parameters = + rclcpp::parameters_from_map(parameter_map, remote_node_name_.c_str()); + + if (parameters.size() == 0) { throw rclcpp::exceptions::InvalidParametersException("No valid parameter"); } - auto future_result = set_parameters(iter->second); + auto future_result = set_parameters(parameters); return future_result; } diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp index 600fbec682..5ed67daae6 100644 --- a/rclcpp/src/rclcpp/parameter_map.cpp +++ b/rclcpp/src/rclcpp/parameter_map.cpp @@ -25,6 +25,13 @@ using rclcpp::exceptions::InvalidParameterValueException; using rclcpp::ParameterMap; using rclcpp::ParameterValue; +static bool is_node_name_matched(const std::string & node_name, const char * node_fqn) +{ + // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] + std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); + return std::regex_match(node_fqn, std::regex(regex)); +} + ParameterMap rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn) { @@ -53,9 +60,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * nod } if (node_fqn) { - // Update the regular expression ["/*" -> "(/\\w+)" and "/**" -> "(/\\w+)*"] - std::string regex = rcpputils::find_and_replace(node_name, "/*", "(/\\w+)"); - if (!std::regex_match(node_fqn, std::regex(regex))) { + if (!is_node_name_matched(node_name, node_fqn)) { // No need to parse the items because the user just care about node_fqn continue; } @@ -155,3 +160,19 @@ rclcpp::parameter_map_from_yaml_file(const std::string & yaml_filename, const ch return rclcpp::parameter_map_from(rcl_parameters, node_fqn); } + +std::vector +rclcpp::parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn) +{ + std::vector parameters; + std::string node_name_old; + for (auto & [node_name, node_parameters] : parameter_map) { + if (node_fqn && !is_node_name_matched(node_name, node_fqn)) { + // No need to parse the items because the user just care about node_fqn + continue; + } + parameters.insert(parameters.end(), node_parameters.begin(), node_parameters.end()); + } + + return parameters; +} diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 697a4ef5c8..15304f8f63 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include @@ -997,3 +998,89 @@ TEST_F(TestParameterClient, async_parameter_load_no_valid_parameter) { asynchronous_client->load_parameters(parameters_filepath), rclcpp::exceptions::InvalidParametersException); } + +/* + Coverage for async load_parameters from maps with complicated regex expression + */ +TEST_F(TestParameterClient, async_parameter_load_parameters_from_map) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/**", + { + {"bar", 5}, + {"foo", 3.5}, + {"a_value", "first"} + } + }, + {"/*/load_node", + { + {"bar_foo", "ok"}, + {"a_value", "second"} + } + }, + {"/namespace/load_node", + { + {"foo_bar", true}, + {"a_value", "third"} + } + }, + {"/bar", + { + {"fatal", 10} + } + }, + {"/**/namespace/*", + { + {"a_value", "not_win"} + } + } + }; + + auto load_future = asynchronous_client->load_parameters(parameter_map); + auto result_code = rclcpp::spin_until_future_complete( + load_node, load_future, std::chrono::milliseconds(100)); + ASSERT_EQ(result_code, rclcpp::FutureReturnCode::SUCCESS); + ASSERT_EQ(load_future.get()[0].successful, true); + // list parameters + auto list_parameters = asynchronous_client->list_parameters({}, 3); + rclcpp::spin_until_future_complete( + load_node, list_parameters, std::chrono::milliseconds(100)); + ASSERT_EQ(list_parameters.get().names.size(), static_cast(6)); + // to check the parameter "a_value" + std::string param_name = "a_value"; + auto param = load_node->get_parameter(param_name); + // rclcpp::ParameterMap is an unordered map, no guarantee which value will be set for `a_value`. + EXPECT_THAT( + (std::array{"first", "second", "third", "not_win"}), + testing::Contains(param.get_value())); +} + +/* + Coverage for async load_parameters from maps without valid parameters + */ +TEST_F(TestParameterClient, async_parameter_load_from_map_no_valid_parameter) { + auto load_node = std::make_shared( + "load_node", + "namespace", + rclcpp::NodeOptions().allow_undeclared_parameters(true)); + auto asynchronous_client = + std::make_shared(load_node, "/namespace/load_node"); + // load parameters + rclcpp::ParameterMap parameter_map = { + {"/no/valid/parameters/node", + { + {"bar", 5}, + {"bar", 3.5} + } + } + }; + EXPECT_THROW( + asynchronous_client->load_parameters(parameter_map), + rclcpp::exceptions::InvalidParametersException); +} From 7059eb2f4d41b0c256e62457486ca777d5560301 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 20 Aug 2022 11:06:15 +0800 Subject: [PATCH 3/4] remove an unnecessary comment Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/parameter_map.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp index 08f46a9291..17e2128a7b 100644 --- a/rclcpp/include/rclcpp/parameter_map.hpp +++ b/rclcpp/include/rclcpp/parameter_map.hpp @@ -64,7 +64,6 @@ parameter_map_from_yaml_file(const std::string & yaml_filename, const char * nod /// \param[in] parameter_map a parameter map. /// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr. /// \returns a list of a parameter -/// \throws from rcl error of rcl_parse_yaml_file() RCLCPP_PUBLIC std::vector parameters_from_map(const ParameterMap & parameter_map, const char * node_fqn = nullptr); From 437fc631218242a5a9aa05a226edd1458794e0bb Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 20 Aug 2022 11:20:25 +0800 Subject: [PATCH 4/4] update gmock compilation setting Signed-off-by: Chen Lihui --- rclcpp/test/rclcpp/CMakeLists.txt | 2 +- rclcpp/test/rclcpp/test_parameter_client.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 6f915feef5..d4e497f4bf 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -299,7 +299,7 @@ if(TARGET test_init_options) ament_target_dependencies(test_init_options "rcl") target_link_libraries(test_init_options ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_parameter_client test_parameter_client.cpp) +ament_add_gmock(test_parameter_client test_parameter_client.cpp) if(TARGET test_parameter_client) ament_target_dependencies(test_parameter_client "rcl_interfaces" diff --git a/rclcpp/test/rclcpp/test_parameter_client.cpp b/rclcpp/test/rclcpp/test_parameter_client.cpp index 15304f8f63..64ef2d903b 100644 --- a/rclcpp/test/rclcpp/test_parameter_client.cpp +++ b/rclcpp/test/rclcpp/test_parameter_client.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include #include #include