Merge branch 'master' of github.com:ctu-mrs/mrs_lib

include/mrs_lib/param_loader.h

@@ -585,10 +585,10 @@ class ParamLoader
   /* addYamlFileFromParam() function //{ */
 
   /*!
-    * \brief Adds the specified file as a source of static parameters.
+    * \brief Loads a filepath from a parameter loads that file as a YAML.
     *
-    * \param filepath The full path to the yaml file to be loaded.
-    * \return true if loading and parsing the file was successful, false otherwise.
+    * \param param_name Name of the parameter from which to load the YAML filename to be loaded.
+    * \return true      if loading and parsing the file was successful, false otherwise.
     */
   bool addYamlFileFromParam(const std::string& param_name)
   {
@@ -839,6 +839,59 @@ class ParamLoader
 
   //}
 
+  /* loadParam specializations for XmlRpc::Value type //{ */
+
+  /*!
+    * \brief An overload for loading an optional XmlRpc::XmlRpcValue.
+    *
+    * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+    * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+    * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+    *
+    * \param name          Name of the parameter in the rosparam server.
+    * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+    */
+  bool loadParam(const std::string& name, XmlRpc::XmlRpcValue& out, const XmlRpc::XmlRpcValue& default_value)
+  {
+    return loadParam<XmlRpc::XmlRpcValue>(name, out, default_value);
+  }
+
+  /*!
+    * \brief An overload for loading a compulsory XmlRpc::XmlRpcValue.
+    *
+    * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+    * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+    * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+    *
+    * \param name          Name of the parameter in the rosparam server.
+    * \param out_value     Reference to the variable to which the parameter value will be stored (such as a class member variable).
+    * \return              true if the parameter was loaded from \p rosparam, false if the default value was used.
+    */
+  bool loadParam(const std::string& name, XmlRpc::XmlRpcValue& out)
+  {
+    return loadParam<XmlRpc::XmlRpcValue>(name, out);
+  }
+
+  /*!
+    * \brief An overload for loading an optional XmlRpc::XmlRpcValue.
+    *
+    * This can be used if you want to manually parse some more complex parameter but still take advantage of ParamLoader.
+    * \warning  XmlRpc::XmlRpcValue must be loaded from a rosparam server and not directly from a YAML file
+    * (i.e. you cannot use it to load parameters from a file added using the addYamlFile() or addYamlFileFromParam() methods).
+    *
+    * \param name          Name of the parameter in the rosparam server.
+    * \param default_value This value will be used if the parameter name is not found in the rosparam server.
+    * \return              the loaded parameter.
+    */
+  XmlRpc::XmlRpcValue loadParam2(const std::string& name, const XmlRpc::XmlRpcValue& default_value)
+  {
+    return loadParam2<XmlRpc::XmlRpcValue>(name, default_value);
+  }
+
+  //}
+
   /* loadParam specializations and convenience functions for Eigen dynamic matrix type //{ */
 
   /*!
@@ -1153,8 +1206,8 @@ class ParamLoader
     * matrix_array:
     *   rows: 3
     *   cols: [1, 2]
-    *   data: [-5.0, Eigen::Dynamic0.0, 23.0,
-    *          -5.0, Eigen::Dynamic0.0, 12.0,
+    *   data: [-5.0, 0.0, 23.0,
+    *          -5.0, 0.0, 12.0,
     *           2.0,   4.0,  7.0]
     *
     * \endcode

include/mrs_lib/param_provider.h

@@ -1,28 +1,79 @@
+// clang: MatousFormat
+/**  \file
+     \brief Defines ParamProvider - a helper class for ParamLoader.
+     \author Matouš Vrba - [email protected]
+ */
+
 #ifndef PARAM_PROVIDER_H
 #define PARAM_PROVIDER_H
 
-#include <string>
-#include <fstream>
-#include <sstream>
-#include <algorithm>
 #include <yaml-cpp/yaml.h>
 #include <ros/node_handle.h>
 
 namespace mrs_lib
 {
+
+/*** ParamProvider CLASS //{ **/
+
+/**
+* \brief Helper class for ParamLoader.
+*
+* This class abstracts away loading of parameters from ROS parameter server and directly from
+* YAML files ("static" parameters). The user can specify a number of YAML files that will be
+* parsed and when a parameter value is requested, these are checked first before attempting
+* to load the parameter from the ROS server (which can be slow). The YAML files are searched
+* in FIFO order and when a matching name is found in a file, its value is returned.
+*
+*/
   class ParamProvider
   {
     public:
 
+  /*!
+    * \brief Main constructor.
+    *
+    * \param nh            The parameters will be loaded from rosparam using this node handle.
+    * \param node_name     Optional node name used when printing the loaded values or loading errors.
+    * \param use_rosparam  If true, parameters that weren't found in the YAML files will be attempted to be loaded from ROS.
+    */
+      ParamProvider(const ros::NodeHandle& nh, std::string node_name, const bool use_rosparam = true);
+
+  /*!
+    * \brief Add a YAML file to be parsed and used for loading parameters.
+    *
+    * The first file added will be the first file searched for a parameter when using getParam().
+    *
+    * \param filepath  Path to the YAML file.
+    */
+      bool addYamlFile(const std::string& filepath);
+
+  /*!
+    * \brief Gets the value of a parameter.
+    *
+    * Firstly, the parameter is attempted to be loaded from the YAML files added by the addYamlFile() method
+    * in the same order that they were added. If the parameter is not found in any YAML file, and the use_rosparam
+    * flag of the constructor is true, the ParamProvider will attempt to load it from the ROS parameter server.
+    *
+    * \param param_name   Name of the parameter to be loaded. Namespaces should be separated with a forward slash '/'.
+    * \param value_out    Output argument that will hold the value of the loaded parameter, if successfull. Not modified otherwise.
+    * \return             Returns true iff the parameter was successfully loaded.
+    */
       template <typename T>
       bool getParam(const std::string& param_name, T& value_out) const;
 
+  /*!
+    * \brief Specialization of getParam() for the XmlRpcValue type.
+    *
+    * The XmlRpc::XmlRpcValue can be useful for manual parsing of more complex types.
+    *
+    * \warning XmlRpc::XmlRpcValue parameters cannot be loaded from a YAML file - only from ROS!
+    *
+    * \param param_name   Name of the parameter to be loaded. Namespaces should be separated with a forward slash '/'.
+    * \param value_out    Output argument that will hold the value of the loaded parameter, if successfull. Not modified otherwise.
+    * \return             Returns true iff the parameter was successfully loaded.
+    */
       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);
-
     private:
 
       std::vector<YAML::Node> m_yamls;
@@ -35,7 +86,7 @@ namespace mrs_lib
 
       std::optional<YAML::Node> findYamlNode(const std::string& param_name) const;
   };
-
+//}
 }
 
 #include "mrs_lib/impl/param_provider.hpp"

src/param_loader/example.cpp

@@ -31,39 +31,64 @@ int main(int argc, char **argv)
   ros::init(argc, argv, node_name);
   ros::NodeHandle nh;
 
-  ROS_INFO("[%s]: pushing testing parameters to the rosparam server", node_name.c_str());
+  /* Set up some parameters to be loaded by the example. In normal usage,
+   * you don't have to do this. Parameters are loaded from the rosparam
+   * server or directly from a YAML file. */
+  ROS_INFO_STREAM("[" << node_name << "]: pushing testing parameters to the rosparam server");
   nh.setParam("test_param_double", std::numeric_limits<double>::quiet_NaN());
   nh.setParam("test_param_vector", std::vector<int>({6, 6, 6}));
 
-  ROS_INFO("[%s]: loading parameters using ParamLoader", node_name.c_str());
+  /* Initialize the param loader with a NodeHandle and optionally the name of this node. */
+  ROS_INFO_STREAM("[" << node_name <<" ]: loading parameters using ParamLoader");
   mrs_lib::ParamLoader pl(nh, node_name);
 
   /* Most basic way to load a parameter to a variable, which could be a class member */ 
   double test_param_double;
   pl.loadParam("test_param_double", test_param_double);
 
   /* Load a parameter and return it */ 
-  const std::vector<int> test_param_vector = pl.loadParam2<std::vector<int>>("test_param_vector");
+  const auto test_param_vector = pl.loadParam2<std::vector<int>>("test_param_vector");
 
   /* Load a parameter with a default value, which will be used in this case
    * unless you manually push the parameter 'test_param_int' to the rosparam server
    * (e.g. using 'rosparam set test_param_int 15'). */ 
-  [[maybe_unused]] const int test_param_int = pl.loadParam2<int>("test_param_int", 4);
+  const auto test_param_int = pl.loadParam2<int>("test_param_int", 4);
 
   /* Load a compulsory parameter - without a default value. This will fail in this
    * case, unless you manually push the parameter to the server. */ 
-  [[maybe_unused]] const bool test_param_bool = pl.loadParam2<bool>("test_param_bool");
+  const auto test_param_bool = pl.loadParam2<bool>("test_param_bool");
+
+  /* So far, all the parameters were loaded from the ROS parameter server.
+   * These must be loaded to the server using the param or rosparam commands
+   * in the launchfile (or manually using the CLI tools). However, loading
+   * a large number of parameters from the ROS server in parallel can be slow,
+   * so we also offer the possibility to load parameters directly from a YAML file.
+   * To do this, firstly you tell the ParamLoader which file to use: */
+  pl.addYamlFile("/tmp/test.yaml");
+  /* (Note that this file will have to be created manually in this case for this */
+  /*  example to work.) */
+
+  /* Then, you can load parameters specified in the file normally. */
+  const auto string_from_yaml = pl.loadParam2<std::string>("string_from_yaml");
 
   /* Check if all parameters were loaded successfully */ 
   if (!pl.loadedSuccessfully())
   {
     /* If not, alert the user and shut the node down */ 
-    ROS_ERROR("[%s]: parameter loading failure", node_name.c_str());
+    ROS_ERROR_STREAM("[" << node_name << "]: parameter loading failure! Ending the node");
     ros::shutdown();
     return 1;
   }
 
-  /* Otherwise, continue the program (in this case it's the end) */ 
+  /* Otherwise, continue the program and happily use the loaded parameters! */ 
+  /* This is just some nonsense usage for demonstration. */
+  if (test_param_bool)
+  {
+    ROS_INFO_STREAM("[" << node_name << "]: Showing values of: " << string_from_yaml);
+    for (auto& el : test_param_vector)
+      ROS_INFO_STREAM("[" << node_name << "]: Value: " << test_param_double * el + test_param_int);
+  }
+
   return 0;
 
 }

src/param_loader/param_loader.cpp

@@ -15,7 +15,6 @@ template bool mrs_lib::ParamLoader::loadParamReusable<double>(const std::string&
 template bool mrs_lib::ParamLoader::loadParam<std::string>(const std::string& name, std::string& out_value, const std::string& default_value);
 template bool mrs_lib::ParamLoader::loadParamReusable<std::string>(const std::string& name, std::string& out_value, const std::string& default_value);
 
-
 template <>
 ros::Duration mrs_lib::ParamLoader::loadParam2<ros::Duration>(const std::string& name, const ros::Duration& default_value)
 {

test/param_loader/test.cpp

@@ -135,6 +135,34 @@ TEST(TESTSuite, static_params_test) {
 
 //}
 
+/* TEST(TESTSuite, weird_types_test) //{ */
+
+TEST(TESTSuite, weird_types_test) {
+
+  // Set up ROS.
+  ros::NodeHandle nh = ros::NodeHandle("~");
+
+  ros::Time::waitForValid();
+
+  ROS_INFO("[%s]: initializing ParamLoader", ros::this_node::getName().c_str());
+  mrs_lib::ParamLoader pl(nh);
+  XmlRpc::XmlRpcValue default_val;
+  XmlRpc::XmlRpcValue xml_val;
+  EXPECT_FALSE(pl.loadParam("XmlRpc_test", xml_val, default_val));
+  EXPECT_TRUE(pl.loadedSuccessfully());
+  EXPECT_FALSE(pl.loadParam("XmlRpc_test", xml_val));
+  EXPECT_FALSE(pl.loadedSuccessfully());
+  [[maybe_unused]] const auto ret = pl.loadParam2("XmlRpc_test", default_val);
+
+  EXPECT_TRUE(pl.loadParam("rosparam_xmlrpc_value", xml_val));
+  EXPECT_TRUE(pl.addYamlFileFromParam("static_params_file"));
+  EXPECT_FALSE(pl.loadParam("static_xmlrpc_value", xml_val));
+
+  EXPECT_FALSE(pl.loadedSuccessfully());
+}
+
+//}
+
 int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv) {
 
   ros::init(argc, argv, "param_loader_tests");

test/param_loader/test.yaml

@@ -12,3 +12,5 @@ test_param_matrix: []
 
 services:
   complex_structure: ["asd"]
+
+rosparam_xmlrpc_value: "somestuff"

test/param_loader/test_static.yaml

@@ -2,3 +2,4 @@ static_param: ["a", "b", "c", "d"]
 ns1:
   ns2:
     a: 5.0
+static_xmlrpc_value: "someotherstuff"