Merge branch 'static_params' of https://github.com/ctu-mrs/mrs_lib in…

…to static_params
ctu-mrs · Oct 13, 2023 · fcfad15 · fcfad15
2 parents ad3a201 + d331cae
commit fcfad15
Show file tree

Hide file tree

Showing 4 changed files with 74 additions and 72 deletions.
diff --git a/include/mrs_lib/impl/param_provider.hpp b/include/mrs_lib/impl/param_provider.hpp
@@ -19,24 +19,6 @@ namespace mrs_lib
     }
   }
 
-  bool ParamProvider::getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const
-  {
-    if (m_use_rosparam && m_nh.getParam(param_name, value_out))
-      return true;
-
-    try
-    {
-      const auto found_node = findYamlNode(param_name);
-      if (found_node.has_value())
-        ROS_WARN_STREAM("[" << m_node_name << "]: Parameter \"" << param_name << "\" of desired type XmlRpc::XmlRpcValue is only available as a static parameter, which doesn't support loading of this type.");
-    }
-    catch (const YAML::Exception& e)
-    {
-      ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what());
-    }
-    return false;
-  }
-
   template <typename T>
   bool ParamProvider::getParamImpl(const std::string& param_name, T& value_out) const
   {
@@ -61,59 +43,6 @@ namespace mrs_lib
 
     return false;
   }
-
-  std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
-  {
-    for (const auto& yaml : m_yamls)
-    {
-      // Try to load the parameter sequentially as a map.
-      auto cur_node_it = std::cbegin(yaml);
-      // The root should always be a pam
-      if (!cur_node_it->second.IsMap())
-        continue;
-
-      bool loaded = true;
-      {
-        constexpr char delimiter = '/';
-        auto substr_start = std::cbegin(param_name);
-        auto substr_end = substr_start;
-        do
-        {
-          substr_end = std::find(substr_start, std::cend(param_name), delimiter);
-          // why can't substr or string_view take iterators? :'(
-          const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
-          const auto count = std::distance(substr_start, substr_end);
-          const std::string param_substr = param_name.substr(start_pos, count);
-          substr_start = substr_end+1;
-
-          bool found = false;
-          for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
-          {
-            if (node_it->first.as<std::string>() == param_substr)
-            {
-              cur_node_it = node_it;
-              found = true;
-              break;
-            }
-          }
-
-          if (!found)
-          {
-            loaded = false;
-            break;
-          }
-        }
-        while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
-      }
-
-      if (loaded)
-      {
-        return cur_node_it->second;
-      }
-    }
-
-    return std::nullopt;
-  }
 }
 
 #endif // PARAM_PROVIDER_HPP
diff --git a/src/param_loader/param_provider.cpp b/src/param_loader/param_provider.cpp
@@ -43,4 +43,74 @@ namespace mrs_lib
     return false;
   }
 
+  bool ParamProvider::getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const
+  {
+    if (m_use_rosparam && m_nh.getParam(param_name, value_out))
+      return true;
+
+    try
+    {
+      const auto found_node = findYamlNode(param_name);
+      if (found_node.has_value())
+        ROS_WARN_STREAM("[" << m_node_name << "]: Parameter \"" << param_name << "\" of desired type XmlRpc::XmlRpcValue is only available as a static parameter, which doesn't support loading of this type.");
+    }
+    catch (const YAML::Exception& e)
+    {
+      ROS_ERROR_STREAM("[" << m_node_name << "]: YAML-CPP threw an unknown exception: " << e.what());
+    }
+    return false;
+  }
+
+  std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
+  {
+    for (const auto& yaml : m_yamls)
+    {
+      // Try to load the parameter sequentially as a map.
+      auto cur_node_it = std::cbegin(yaml);
+      // The root should always be a pam
+      if (!cur_node_it->second.IsMap())
+        continue;
+
+      bool loaded = true;
+      {
+        constexpr char delimiter = '/';
+        auto substr_start = std::cbegin(param_name);
+        auto substr_end = substr_start;
+        do
+        {
+          substr_end = std::find(substr_start, std::cend(param_name), delimiter);
+          // why can't substr or string_view take iterators? :'(
+          const auto start_pos = std::distance(std::cbegin(param_name), substr_start);
+          const auto count = std::distance(substr_start, substr_end);
+          const std::string param_substr = param_name.substr(start_pos, count);
+          substr_start = substr_end+1;
+
+          bool found = false;
+          for (auto node_it = std::cbegin(cur_node_it->second); node_it != std::cend(cur_node_it->second); ++node_it)
+          {
+            if (node_it->first.as<std::string>() == param_substr)
+            {
+              cur_node_it = node_it;
+              found = true;
+              break;
+            }
+          }
+
+          if (!found)
+          {
+            loaded = false;
+            break;
+          }
+        }
+        while (substr_end != std::end(param_name) && cur_node_it->second.IsMap());
+      }
+
+      if (loaded)
+      {
+        return cur_node_it->second;
+      }
+    }
+
+    return std::nullopt;
+  }
 }
diff --git a/test/param_loader/test.cpp b/test/param_loader/test.cpp
@@ -37,7 +37,7 @@ TEST(TESTSuite, main_test) {
   mrs_lib::ParamLoader pl(nh);
 
   XmlRpc::XmlRpcValue list;
-  pl.loadParam("services", list, XmlRpc::XmlRpcValue());
+  EXPECT_TRUE(pl.loadParam("services", list, XmlRpc::XmlRpcValue()));
 
   ROS_INFO("[%s]: testing ParamLoader", ros::this_node::getName().c_str());
   std::vector<Eigen::MatrixXd> loaded_nd_matrix = pl.loadMatrixArray2("test_param_nd_matrix");

diff --git a/test/param_loader/test.yaml b/test/param_loader/test.yaml
@@ -9,3 +9,6 @@ test_param_nd_matrix:
           2.0,   4.0]
 
 test_param_matrix: []
+
+services:
+  complex_structure: ["asd"]