use regex for wildcard matching (backport #1839) (#1986)

* use regex for wildcard matching (#1839) * use regex for wildcard matching Co-authored-by: Aaron Lipinski <[email protected]> Signed-off-by: Chen Lihui <[email protected]> * use map to process the content of parameter file by order Signed-off-by: Chen Lihui <[email protected]> * add more test cases Signed-off-by: Chen Lihui <[email protected]> * try to not decrease the performance and make the param win last Signed-off-by: Chen Lihui <[email protected]> * update node name Signed-off-by: Chen Lihui <[email protected]> * update document comment Signed-off-by: Chen Lihui <[email protected]> * add more test for parameter_map_from Signed-off-by: Chen Lihui <[email protected]> Co-authored-by: Aaron Lipinski <[email protected]> (cherry picked from commit 6dd3a03) * not to break ABI Signed-off-by: Chen Lihui <[email protected]> Signed-off-by: Chen Lihui <[email protected]> Co-authored-by: Chen Lihui <[email protected]>
ros2 · Sep 9, 2022 · 7f57510 · 7f57510
1 parent 166007d
commit 7f57510
Show file tree

Hide file tree

Showing 8 changed files with 376 additions and 11 deletions.
diff --git a/rclcpp/include/rclcpp/parameter_map.hpp b/rclcpp/include/rclcpp/parameter_map.hpp
@@ -41,6 +41,16 @@ RCLCPP_PUBLIC
 ParameterMap
 parameter_map_from(const rcl_params_t * const c_params);
 
+/// Convert parameters from rcl_yaml_param_parser into C++ class instances.
+/// \param[in] c_params C structures containing parameters for multiple nodes.
+/// \param[in] node_fqn a Fully Qualified Name of node, default value is nullptr.
+///   If it's not nullptr, return the relative node parameters belonging to this node_fqn.
+/// \returns a map where the keys are fully qualified node names and values a list of parameters.
+/// \throws InvalidParametersException if the `rcl_params_t` is inconsistent or invalid.
+RCLCPP_PUBLIC
+ParameterMap
+parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn);
+
 /// Convert parameter value from rcl_yaml_param_parser into a C++ class instance.
 /// \param[in] c_value C structure containing a value of a parameter.
 /// \returns an instance of a parameter value

diff --git a/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp b/rclcpp/src/rclcpp/detail/resolve_parameter_overrides.cpp
@@ -51,18 +51,13 @@ rclcpp::detail::resolve_parameter_overrides(
         [params]() {
           rcl_yaml_node_struct_fini(params);
         });
-      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params);
+      rclcpp::ParameterMap initial_map = rclcpp::parameter_map_from(params, node_fqn.c_str());
 
-      // Enforce wildcard matching precedence
-      // TODO(cottsay) implement further wildcard matching
-      const std::array<std::string, 2> node_matching_names{"/**", node_fqn};
-      for (const auto & node_name : node_matching_names) {
-        if (initial_map.count(node_name) > 0) {
-          // Combine parameter yaml files, overwriting values in older ones
-          for (const rclcpp::Parameter & param : initial_map.at(node_name)) {
-            result[param.get_name()] =
-              rclcpp::ParameterValue(param.get_value_message());
-          }
+      if (initial_map.count(node_fqn) > 0) {
+        // Combine parameter yaml files, overwriting values in older ones
+        for (const rclcpp::Parameter & param : initial_map.at(node_fqn)) {
+          result[param.get_name()] =
+            rclcpp::ParameterValue(param.get_value_message());
         }
       }
     }

diff --git a/rclcpp/src/rclcpp/parameter_map.cpp b/rclcpp/src/rclcpp/parameter_map.cpp
@@ -13,8 +13,10 @@
 // limitations under the License.
 
 #include <string>
+#include <regex>
 #include <vector>
 
+#include "rcpputils/find_and_replace.hpp"
 #include "rclcpp/parameter_map.hpp"
 
 using rclcpp::exceptions::InvalidParametersException;
@@ -24,6 +26,12 @@ using rclcpp::ParameterValue;
 
 ParameterMap
 rclcpp::parameter_map_from(const rcl_params_t * const c_params)
+{
+  return parameter_map_from(c_params, nullptr);
+}
+
+ParameterMap
+rclcpp::parameter_map_from(const rcl_params_t * const c_params, const char * node_fqn)
 {
   if (NULL == c_params) {
     throw InvalidParametersException("parameters struct is NULL");
@@ -49,6 +57,17 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
       node_name = c_node_name;
     }
 
+    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))) {
+        // No need to parse the items because the user just care about node_fqn
+        continue;
+      }
+
+      node_name = node_fqn;
+    }
+
     const rcl_node_params_t * const c_params_node = &(c_params->params[n]);
 
     std::vector<Parameter> & params_node = parameters[node_name];
@@ -65,6 +84,7 @@ rclcpp::parameter_map_from(const rcl_params_t * const c_params)
       params_node.emplace_back(c_param_name, parameter_value_from(c_param_value));
     }
   }
+
   return parameters;
 }
 

diff --git a/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp b/rclcpp/test/rclcpp/node_interfaces/test_node_parameters.cpp
@@ -31,6 +31,8 @@
 #include "../../mocking_utils/patch.hpp"
 #include "../../utils/rclcpp_gtest_macros.hpp"
 
+#include "rcpputils/filesystem_helper.hpp"
+
 class TestNodeParameters : public ::testing::Test
 {
 public:
@@ -47,6 +49,7 @@ class TestNodeParameters : public ::testing::Test
       dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
       node->get_node_parameters_interface().get());
     ASSERT_NE(nullptr, node_parameters);
+    test_resources_path /= "test_node_parameters";
   }
 
   void TearDown()
@@ -57,6 +60,8 @@ class TestNodeParameters : public ::testing::Test
 protected:
   std::shared_ptr<rclcpp::Node> node;
   rclcpp::node_interfaces::NodeParameters * node_parameters;
+
+  rcpputils::fs::path test_resources_path{TEST_RESOURCES_DIRECTORY};
 };
 
 TEST_F(TestNodeParameters, construct_destruct_rcl_errors) {
@@ -199,3 +204,130 @@ TEST_F(TestNodeParameters, add_remove_parameters_callback) {
     node_parameters->remove_on_set_parameters_callback(handle.get()),
     std::runtime_error("Callback doesn't exist"));
 }
+
+TEST_F(TestNodeParameters, wildcard_with_namespace)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "wildcards.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(7u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
+  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_another").get<std::string>(),
+    "namespace_wild_another");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_one_star").get<std::string>(),
+    "namespace_wild_one_star");
+  EXPECT_EQ(parameter_overrides.at("node_wild_in_ns").get<std::string>(), "node_wild_in_ns");
+  EXPECT_EQ(
+    parameter_overrides.at("node_wild_in_ns_another").get<std::string>(),
+    "node_wild_in_ns_another");
+  EXPECT_EQ(parameter_overrides.at("explicit_in_ns").get<std::string>(), "explicit_in_ns");
+  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
+}
+
+TEST_F(TestNodeParameters, wildcard_no_namespace)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "wildcards.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(5u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("full_wild").get<std::string>(), "full_wild");
+  EXPECT_EQ(parameter_overrides.at("namespace_wild").get<std::string>(), "namespace_wild");
+  EXPECT_EQ(
+    parameter_overrides.at("namespace_wild_another").get<std::string>(),
+    "namespace_wild_another");
+  EXPECT_EQ(parameter_overrides.at("node_wild_no_ns").get<std::string>(), "node_wild_no_ns");
+  EXPECT_EQ(parameter_overrides.at("explicit_no_ns").get<std::string>(), "explicit_no_ns");
+  EXPECT_EQ(parameter_overrides.count("should_not_appear"), 0u);
+  // "/*" match exactly one token, not expect to get `namespace_wild_one_star`
+  EXPECT_EQ(parameter_overrides.count("namespace_wild_one_star"), 0u);
+}
+
+TEST_F(TestNodeParameters, params_by_order)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "params_by_order.yaml").string()
+  });
+
+  std::shared_ptr<rclcpp::Node> node = std::make_shared<rclcpp::Node>("node2", "ns", opts);
+
+  auto * node_parameters =
+    dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+    node->get_node_parameters_interface().get());
+  ASSERT_NE(nullptr, node_parameters);
+
+  const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+  EXPECT_EQ(3u, parameter_overrides.size());
+  EXPECT_EQ(parameter_overrides.at("a_value").get<std::string>(), "last_one_win");
+  EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
+  EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
+}
+
+TEST_F(TestNodeParameters, complicated_wildcards)
+{
+  rclcpp::NodeOptions opts;
+  opts.arguments(
+  {
+    "--ros-args",
+    "--params-file", (test_resources_path / "complicated_wildcards.yaml").string()
+  });
+
+  {
+    // regex matched: /**/foo/*/bar
+    std::shared_ptr<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/d/bar", opts);
+
+    auto * node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+
+    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+    EXPECT_EQ(2u, parameter_overrides.size());
+    EXPECT_EQ(parameter_overrides.at("foo").get<std::string>(), "foo");
+    EXPECT_EQ(parameter_overrides.at("bar").get<std::string>(), "bar");
+  }
+
+  {
+    // regex not matched: /**/foo/*/bar
+    std::shared_ptr<rclcpp::Node> node =
+      std::make_shared<rclcpp::Node>("node2", "/a/b/c/foo/bar", opts);
+
+    auto * node_parameters =
+      dynamic_cast<rclcpp::node_interfaces::NodeParameters *>(
+      node->get_node_parameters_interface().get());
+    ASSERT_NE(nullptr, node_parameters);
+
+    const auto & parameter_overrides = node_parameters->get_parameter_overrides();
+    EXPECT_EQ(0u, parameter_overrides.size());
+  }
+}
diff --git a/rclcpp/test/rclcpp/test_parameter_map.cpp b/rclcpp/test/rclcpp/test_parameter_map.cpp
@@ -19,6 +19,7 @@
 
 #include <cstdio>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 #include "rclcpp/parameter_map.hpp"
@@ -353,3 +354,132 @@ TEST(Test_parameter_map_from, string_array_param_value)
   c_params->params[0].parameter_values[0].string_array_value = NULL;
   rcl_yaml_node_struct_fini(c_params);
 }
+
+TEST(Test_parameter_map_from, one_node_one_param_by_node_fqn)
+{
+  rcl_params_t * c_params = make_params({"foo"});
+  make_node_params(c_params, 0, {"string_param"});
+
+  std::string hello_world = "hello world";
+  char * c_hello_world = new char[hello_world.length() + 1];
+  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+  c_params->params[0].parameter_values[0].string_value = c_hello_world;
+
+  rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, "/foo");
+  const std::vector<rclcpp::Parameter> & params = map.at("/foo");
+  EXPECT_STREQ("string_param", params.at(0).get_name().c_str());
+  EXPECT_STREQ(hello_world.c_str(), params.at(0).get_value<std::string>().c_str());
+
+  c_params->params[0].parameter_values[0].string_value = NULL;
+  delete[] c_hello_world;
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, multi_nodes_same_param_name_by_node_fqn)
+{
+  std::vector<std::string> node_names_keys = {
+    "/**",              // index: 0
+    "/*",               // index: 1
+    "/**/node",         // index: 2
+    "/*/node",          // index: 3
+    "/ns/node"          // index: 4
+  };
+
+  rcl_params_t * c_params = make_params(node_names_keys);
+
+  std::vector<char *> param_values;
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    make_node_params(c_params, i, {"string_param"});
+    std::string hello_world = "hello world" + std::to_string(i);
+    char * c_hello_world = new char[hello_world.length() + 1];
+    std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+    c_params->params[i].parameter_values[0].string_value = c_hello_world;
+    param_values.push_back(c_hello_world);
+  }
+
+  std::unordered_map<std::string, std::vector<size_t>> node_fqn_expected = {
+    {"/ns/foo/another_node", {0}},
+    {"/another", {0, 1}},
+    {"/node", {0, 1, 2}},
+    {"/another_ns/node", {0, 2, 3}},
+    {"/ns/node", {0, 2, 3, 4}},
+  };
+
+  for (auto & kv : node_fqn_expected) {
+    rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str());
+    const std::vector<rclcpp::Parameter> & params = map.at(kv.first);
+
+    EXPECT_EQ(kv.second.size(), params.size());
+    for (size_t i = 0; i < params.size(); ++i) {
+      std::string param_value = "hello world" + std::to_string(kv.second[i]);
+      EXPECT_STREQ("string_param", params.at(i).get_name().c_str());
+      EXPECT_STREQ(param_value.c_str(), params.at(i).get_value<std::string>().c_str());
+    }
+  }
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    c_params->params[i].parameter_values[0].string_value = NULL;
+  }
+  for (auto c_hello_world : param_values) {
+    delete[] c_hello_world;
+  }
+  rcl_yaml_node_struct_fini(c_params);
+}
+
+TEST(Test_parameter_map_from, multi_nodes_diff_param_name_by_node_fqn)
+{
+  std::vector<std::string> node_names_keys = {
+    "/**",              // index: 0
+    "/*",               // index: 1
+    "/**/node",         // index: 2
+    "/*/node",          // index: 3
+    "/ns/**",           // index: 4
+    "/ns/*",            // index: 5
+    "/ns/**/node",      // index: 6
+    "/ns/*/node",       // index: 7
+    "/ns/**/a/*/node",  // index: 8
+    "/ns/node"          // index: 9
+  };
+
+  rcl_params_t * c_params = make_params(node_names_keys);
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    std::string param_name = "string_param" + std::to_string(i);
+    make_node_params(c_params, i, {param_name});
+  }
+
+  std::string hello_world = "hello world";
+  char * c_hello_world = new char[hello_world.length() + 1];
+  std::snprintf(c_hello_world, hello_world.size() + 1, "%s", hello_world.c_str());
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    c_params->params[i].parameter_values[0].string_value = c_hello_world;
+  }
+
+  std::unordered_map<std::string, std::vector<size_t>> node_fqn_expected = {
+    {"/ns/node", {0, 2, 3, 4, 5, 6, 9}},
+    {"/node", {0, 1, 2}},
+    {"/ns/foo/node", {0, 2, 4, 6, 7}},
+    {"/ns/foo/a/node", {0, 2, 4, 6}},
+    {"/ns/foo/a/bar/node", {0, 2, 4, 6, 8}},
+    {"/ns/a/bar/node", {0, 2, 4, 6, 8}},
+    {"/ns/foo/zoo/a/bar/node", {0, 2, 4, 6, 8}},
+  };
+
+  for (auto & kv : node_fqn_expected) {
+    rclcpp::ParameterMap map = rclcpp::parameter_map_from(c_params, kv.first.c_str());
+    const std::vector<rclcpp::Parameter> & params = map.at(kv.first);
+    EXPECT_EQ(kv.second.size(), params.size());
+    for (size_t i = 0; i < params.size(); ++i) {
+      std::string param_name = "string_param" + std::to_string(kv.second[i]);
+      EXPECT_STREQ(param_name.c_str(), params.at(i).get_name().c_str());
+      EXPECT_STREQ(hello_world.c_str(), params.at(i).get_value<std::string>().c_str());
+    }
+  }
+
+  for (size_t i = 0; i < node_names_keys.size(); ++i) {
+    c_params->params[i].parameter_values[0].string_value = NULL;
+  }
+  delete[] c_hello_world;
+  rcl_yaml_node_struct_fini(c_params);
+}
diff --git a/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml b/rclcpp/test/resources/test_node_parameters/complicated_wildcards.yaml
@@ -0,0 +1,5 @@
+/**/foo/*/bar:
+  node2:
+    ros__parameters:
+      foo: "foo"
+      bar: "bar"