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

…to static_params
ctu-mrs · Oct 13, 2023 · ad3a201 · ad3a201
2 parents 9370bd1 + 8c0db74
commit ad3a201
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 13 deletions.
diff --git a/include/mrs_lib/impl/param_provider.hpp b/include/mrs_lib/impl/param_provider.hpp
@@ -19,8 +19,50 @@ 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
+  {
+    {
+      const auto found_node = findYamlNode(param_name);
+      if (found_node.has_value())
+      {
+        try
+        {
+          // try catch is the only type-generic option...
+          value_out = found_node.value().as<T>();
+          return true;
+        }
+        catch (const YAML::BadConversion& e)
+        {}
+      }
+
+    }
+
+    if (m_use_rosparam)
+      return m_nh.getParam(param_name, value_out);
+
+    return false;
+  }
+
+  std::optional<YAML::Node> ParamProvider::findYamlNode(const std::string& param_name) const
   {
     for (const auto& yaml : m_yamls)
     {
@@ -66,22 +108,11 @@ namespace mrs_lib
 
       if (loaded)
       {
-        try
-        {
-          // try catch is the only type-generic option...
-          value_out = cur_node_it->second.as<T>();
-          return true;
-        }
-        catch (const YAML::BadConversion& e)
-        {}
+        return cur_node_it->second;
       }
-
     }
 
-    if (m_use_rosparam)
-      return m_nh.getParam(param_name, value_out);
-
-    return false;
+    return std::nullopt;
   }
 }
 

diff --git a/include/mrs_lib/param_provider.h b/include/mrs_lib/param_provider.h
@@ -17,6 +17,8 @@ namespace mrs_lib
       template <typename T>
       bool getParam(const std::string& param_name, T& value_out) const;
 
+      bool getParam(const std::string& param_name, XmlRpc::XmlRpcValue& value_out) const;
+
       ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam = true);
 
       bool addYamlFile(const std::string& filepath);
@@ -30,6 +32,8 @@ namespace mrs_lib
 
       template <typename T>
       bool getParamImpl(const std::string& param_name, T& value_out) const;
+
+      std::optional<YAML::Node> findYamlNode(const std::string& param_name) const;
   };
 
 }

diff --git a/test/param_loader/test.cpp b/test/param_loader/test.cpp
@@ -36,6 +36,9 @@ TEST(TESTSuite, main_test) {
   ROS_INFO("[%s]: initializing ParamLoader", ros::this_node::getName().c_str());
   mrs_lib::ParamLoader pl(nh);
 
+  XmlRpc::XmlRpcValue list;
+  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");
   if (pl.loadedSuccessfully())