From e2e3536d72598b5696bbb25bb40c5bf9e606f947 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Matou=C5=A1=20Vrba?= Date: Mon, 4 Dec 2023 13:17:44 +0100 Subject: [PATCH] Static parameter loading from yaml files instead of ROSparam server (#48) * [ParamLoader]: updated documentation, added some tests, some minor polishing * [ParamLoader]: added some documentation, minor cleanup * [ParamLoader]: minifix of documentation --- include/mrs_lib/param_loader.h | 63 +++++++++++++++++++++++-- include/mrs_lib/param_provider.h | 69 ++++++++++++++++++++++++---- include/mrs_lib/ros_param_provider.h | 14 ------ src/param_loader/example.cpp | 39 +++++++++++++--- src/param_loader/param_loader.cpp | 1 - test/param_loader/test.cpp | 28 +++++++++++ test/param_loader/test.yaml | 2 + test/param_loader/test_static.yaml | 1 + 8 files changed, 181 insertions(+), 36 deletions(-) delete mode 100644 include/mrs_lib/ros_param_provider.h diff --git a/include/mrs_lib/param_loader.h b/include/mrs_lib/param_loader.h index 3ccf9493..9e9531f8 100644 --- a/include/mrs_lib/param_loader.h +++ b/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(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(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(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 diff --git a/include/mrs_lib/param_provider.h b/include/mrs_lib/param_provider.h index 49a0f2e6..2efb7025 100644 --- a/include/mrs_lib/param_provider.h +++ b/include/mrs_lib/param_provider.h @@ -1,28 +1,79 @@ +// clang: MatousFormat +/** \file + \brief Defines ParamProvider - a helper class for ParamLoader. + \author Matouš Vrba - vrbamato@fel.cvut.cz + */ + #ifndef PARAM_PROVIDER_H #define PARAM_PROVIDER_H -#include -#include -#include -#include #include #include 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 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 m_yamls; @@ -35,7 +86,7 @@ namespace mrs_lib std::optional findYamlNode(const std::string& param_name) const; }; - +//} } #include "mrs_lib/impl/param_provider.hpp" diff --git a/include/mrs_lib/ros_param_provider.h b/include/mrs_lib/ros_param_provider.h deleted file mode 100644 index 9cece40f..00000000 --- a/include/mrs_lib/ros_param_provider.h +++ /dev/null @@ -1,14 +0,0 @@ -#include - -namespace mrs_lib -{ - class RosParamProvider : public ParamProvider - { - public: - - template - bool getParam(const std::string& param_name, T& value_out); - - ParamProvider() = delete; - }; -} diff --git a/src/param_loader/example.cpp b/src/param_loader/example.cpp index fa8e6227..21a535ef 100644 --- a/src/param_loader/example.cpp +++ b/src/param_loader/example.cpp @@ -31,11 +31,15 @@ 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::quiet_NaN()); nh.setParam("test_param_vector", std::vector({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 */ @@ -43,27 +47,48 @@ int main(int argc, char **argv) pl.loadParam("test_param_double", test_param_double); /* Load a parameter and return it */ - const std::vector test_param_vector = pl.loadParam2>("test_param_vector"); + const auto test_param_vector = pl.loadParam2>("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("test_param_int", 4); + const auto test_param_int = pl.loadParam2("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("test_param_bool"); + const auto test_param_bool = pl.loadParam2("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("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; } diff --git a/src/param_loader/param_loader.cpp b/src/param_loader/param_loader.cpp index d31df0a2..be39ee41 100644 --- a/src/param_loader/param_loader.cpp +++ b/src/param_loader/param_loader.cpp @@ -15,7 +15,6 @@ template bool mrs_lib::ParamLoader::loadParamReusable(const std::string& template bool mrs_lib::ParamLoader::loadParam(const std::string& name, std::string& out_value, const std::string& default_value); template bool mrs_lib::ParamLoader::loadParamReusable(const std::string& name, std::string& out_value, const std::string& default_value); - template <> ros::Duration mrs_lib::ParamLoader::loadParam2(const std::string& name, const ros::Duration& default_value) { diff --git a/test/param_loader/test.cpp b/test/param_loader/test.cpp index da8400c2..eb7bcd14 100644 --- a/test/param_loader/test.cpp +++ b/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"); diff --git a/test/param_loader/test.yaml b/test/param_loader/test.yaml index f98dcd60..d27882f7 100644 --- a/test/param_loader/test.yaml +++ b/test/param_loader/test.yaml @@ -12,3 +12,5 @@ test_param_matrix: [] services: complex_structure: ["asd"] + +rosparam_xmlrpc_value: "somestuff" diff --git a/test/param_loader/test_static.yaml b/test/param_loader/test_static.yaml index 6194ee6b..40e4da34 100644 --- a/test/param_loader/test_static.yaml +++ b/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"