diff --git a/doc/HowToUseYourParameterPtr.md b/doc/HowToUseYourParameterPtr.md new file mode 100644 index 0000000..49e58e2 --- /dev/null +++ b/doc/HowToUseYourParameterPtr.md @@ -0,0 +1,89 @@ +# How to Use Your `rosparam_handler::ParameterPtr` +**Description**: This tutorial will familiarize you with how you can use the autogenerated parameter structs in your nodes from a base class smart-pointer. It is thus recommended to read the tutorial [How to use your parameters struct file](HowToUseYourParametersStruct.md) if you haven't yet. +**Tutorial Level**: INTERMEDIATE + +## Generated files +When creating params files as described in [How to write your first .params file](HowToWriteYourFirstParamsFile.md), you will end up with the following two files: +- *'include/rosparam_tutorials/TutorialParameters.h'* +- *'include/rosparam_tutorials/TutorialConfig.h'* + +The 'Parameters.h' file will hold a struct called `Parameters`. +The 'Config.h' file will hold the normal dynamic_reconfigure Config struct. + +For your code it is enough to include the \*Parameters.h file, e.g. + +```cpp +#include "rosparam_tutorials/TutorialParameters.h" +``` + +You can now add an instance of the base parameter pointer to your class: + +```cpp +rosparam_tutorials::ParametersPtr params_ptr_; +``` + +## Initializing the pointer. +When initializing your node, the params pointer `params_ptr_` must be instantiated to the appropriate parameter type with a private `NodeHandle`. + +```cpp +MyNodeClass::MyNodeClass() + : params_ptr_{boost::make_shared(ros::NodeHandle("~"))} +{ + ... +} +``` + +In case your node has several classes, each using a different `ParameterPtr` object, the private `NodeHandle` must have a sub-namespace that is unique to your object in order to avoid parameters name collision. + +```cpp +MyNodeClass::MyClass() + : params_ptr_{boost::make_shared(ros::NodeHandle("~/my_class"))} +{ + ... +} + +MyNodeClass::MyOtherClass() + : params_ptr_{boost::make_shared(ros::NodeHandle("~/my_other_class"))} +{ + ... +} +``` + +## Initializing the struct. + +The call to `fromParamServer()` is done the very same manner as for a normal parameter object. +It will take care of getting all parameter values from the parameter server, checking their type, and checking that a default value is set, if you haven't provided one on your own. If you have specified a default value, but the parameter is not yet present on the parameter server, it will be set. When min and max values are specified, these bounds will be checked as well. + +```cpp +MyNodeClass::MyNodeClass() + : params_ptr_{boost::make_shared(ros::NodeHandle("~"))} +{ + params_ptr_->fromParamServer(); +} +``` + +If you do not use a class (which you should do though in my opinion), you can create it like so: +```cpp +rosparam_tutorials::ParametersPtr params_ptr{boost::make_shared(ros::NodeHandle("~"))} +params_ptr->fromParamServer(); +``` +Note: If you have set the logger level for your node to debug, you will get information on which values have been retrieved. +Note: If you use nodelets, you have to use the `getPrivateNodeHandle()` function instead. + +## Using dynamic_reconfigure +Your dynamic_reconfigure callback can now look as simple as: +```cpp +void reconfigureRequest(TutorialConfig& config, uint32_t level) { + params_ptr_->fromConfig(config); +} +``` +This will update all values that were specified as configurable. At the same time, it assures that all dynamic_reconfigure parameters live in the same namespace as those on the parameter server to avoid problems with redundant parameters. + +You can find a running version of this example code in the [rosparam_handler_tutorial](https://github.com/cbandera/rosparam_handler_tutorial)-Repository + +## Setting parameters on the server +If you change your parameters at runtime from within the code, you can upload the current state of the parameters with +```cpp +params_ptr_->toParamServer(); +``` +This will set all non-const parameters with their current value on the ros parameter server. diff --git a/include/rosparam_handler/ParametersBase.h b/include/rosparam_handler/ParametersBase.h new file mode 100644 index 0000000..f8fdf40 --- /dev/null +++ b/include/rosparam_handler/ParametersBase.h @@ -0,0 +1,90 @@ +/** + * \file ParametersBase.h + * + * Created on: Oct 28, 2017 + * + * \authors: Claudio Bandera + * Sascha Wirges + * Niels Ole Salscheider + * Jeremie Deray + */ + +#ifndef _ROSPARAM_HANDLER_PARAMETERS_BASE_H_ +#define _ROSPARAM_HANDLER_PARAMETERS_BASE_H_ + +#include +#include +#include +#include + +namespace rosparam_handler { + +/// \brief ParametersBase An abstract base struct for struct generated by rosparam_handler. +struct ParametersBase { + + ParametersBase(const ros::NodeHandle& private_node_handle) + : globalNamespace{"/"}, + privateNamespace{private_node_handle.getNamespace() + "/"}, + nodeName{rosparam_handler::getNodeName(private_node_handle)} {} + + virtual ~ParametersBase() = default; + + /// \brief Get values from parameter server + /// + /// Will fail if a value can not be found and no default value is given. + inline void fromParamServer(){ + if(!fromParamServerImpl()){ + missingParamsWarning(); + rosparam_handler::exit("RosparamHandler: GetParam could net retrieve parameter."); + } + ROS_DEBUG_STREAM(*this); + } + + /// \brief Set parameters on ROS parameter server. + virtual void toParamServer() = 0; + + /// \brief Update configurable parameters. + /// + /// \param config dynamic reconfigure struct + /// \level ? + template + inline void fromConfig(const T& config, const uint32_t level = 0){ +#ifdef DYNAMIC_RECONFIGURE_FOUND + fromConfigImpl(config, level); +#else + ROS_FATAL_STREAM("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); + rosparam_handler::exit("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); +#endif + } + + /// \brief Stream operator for printing parameter struct + inline friend std::ostream& operator<<(std::ostream& os, const ParametersBase& p) + { + return p.print(os); + } + +protected: + + /// \brief The actual implementation of 'fromParamServer' + /// overriden by the derived class + virtual bool fromParamServerImpl() = 0; + + /// \brief The actual implementation os 'fromConfig' specialized + /// for the 'DerivedConfig' type. + template + void fromConfigImpl(const T& config, const uint32_t level); + + /// \brief A helper function for ostram operator << + virtual std::ostream& print(std::ostream& os) const = 0; + + /// \brief Issue a warning about missing default parameters. + virtual void missingParamsWarning() = 0; + + const std::string globalNamespace = {"/"}; + const std::string privateNamespace; + const std::string nodeName; +}; + +} // namespace rosparam_handler + +#endif /* _ROSPARAM_HANDLER_PARAMETERS_BASE_H_ */ diff --git a/include/rosparam_handler/pointer.hpp b/include/rosparam_handler/pointer.hpp new file mode 100644 index 0000000..3db8e6b --- /dev/null +++ b/include/rosparam_handler/pointer.hpp @@ -0,0 +1,36 @@ +/** + * \file pointer.h + * + * Created on: Oct 28, 2017 + * + * \authors: Jeremie Deray + */ + +#ifndef _ROSPARAM_HANDLER_POINTER_H_ +#define _ROSPARAM_HANDLER_POINTER_H_ + +#include + +namespace rosparam_handler { + +/// \brief forward declaration +struct ParametersBase; + +/// \brief base pointer declaration +using ParametersPtr = boost::shared_ptr; + +template +boost::shared_ptr static_parameters_cast(const rosparam_handler::ParametersPtr& ptr) +{ + return boost::static_pointer_cast(ptr); +} + +template +boost::shared_ptr dynamic_parameters_cast(const rosparam_handler::ParametersPtr& ptr) +{ + return boost::dynamic_pointer_cast(ptr); +} + +} /* namespace rosparam_handler */ + +#endif /* _ROSPARAM_HANDLER_POINTER_H_ */ diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 015b112..5602f49 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -434,7 +434,7 @@ def _generatehpp(self): # Test for configurable params if param['configurable']: - from_config.append(Template(' $name = config.$name;').substitute(name=name)) + from_config.append(Template(' casted_ref.$name = config.$name;').substitute(name=name)) # Test limits if param['is_vector']: @@ -451,7 +451,7 @@ def _generatehpp(self): paramname=full_name, name=name, max=param['max'], type=ttype)) # Add debug output - string_representation.append(Template(' << "\t" << p.$namespace << "$name:" << p.$name << ' + string_representation.append(Template(' << "\t" << $namespace << "$name:" << $name << ' '"\\n"\n').substitute(namespace=namespace, name=name)) param_entries = "\n".join(param_entries) diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index a493f0d..5c4a59e 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -6,82 +6,94 @@ // // ********************************************************/ -#pragma once +#ifndef _${pkgname}_ROSPARAM_HANDLER_${ClassName}Parameters_H_ +#define _${pkgname}_ROSPARAM_HANDLER_${ClassName}Parameters_H_ + +#include -#include -#include -#include #ifdef DYNAMIC_RECONFIGURE_FOUND #include <${pkgname}/${ClassName}Config.h> #else struct ${ClassName}Config{}; #endif - namespace ${pkgname} { /// \brief Parameter struct generated by rosparam_handler -struct ${ClassName}Parameters { +struct ${ClassName}Parameters : public rosparam_handler::ParametersBase { using Config = ${ClassName}Config; - ${ClassName}Parameters(const ros::NodeHandle& private_node_handle) - : globalNamespace{"/"}, - privateNamespace{private_node_handle.getNamespace() + "/"}, - nodeName{rosparam_handler::getNodeName(private_node_handle)} {} - - /// \brief Get values from parameter server - /// - /// Will fail if a value can not be found and no default value is given. - void fromParamServer(){ - bool success = true; -$fromParamServer -$test_limits - if(!success){ - missingParamsWarning(); - rosparam_handler::exit("RosparamHandler: GetParam could net retrieve parameter."); - } - ROS_DEBUG_STREAM(*this); - } + inline ${ClassName}Parameters(const ros::NodeHandle& private_node_handle) : + ParametersBase(private_node_handle) {} /// \brief Set parameters on ROS parameter server. - void toParamServer(){ -$toParamServer + void toParamServer() override { +$toParamServer } - /// \brief Update configurable parameters. - /// - /// \param config dynamic reconfigure struct - /// \level ? - void fromConfig(const Config& config, const uint32_t level = 0){ -#ifdef DYNAMIC_RECONFIGURE_FOUND -$fromConfig -#else - ROS_FATAL_STREAM("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); - rosparam_handler::exit("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); -#endif - } - - /// \brief Stream operator for printing parameter struct - friend std::ostream& operator<<(std::ostream& os, const ${ClassName}Parameters& p) - { - os << "[" << p.nodeName << "]\nNode " << p.nodeName << " has the following parameters:\n" -$string_representation; - return os; - } + /// \brief the parameters members. $parameters -private: +protected: + + /// \brief Get values from parameter server + /// + /// Will fail if a value can not be found and no default value is given. + inline bool fromParamServerImpl() override { + + bool success = true; + + $fromParamServer + $test_limits + + return success; + + } + + /// \brief print. A helper function to be called from base stream operator for printing parameter struct + inline std::ostream& print(std::ostream& os) const override + { + os << "[" << nodeName << "]\nNode " << nodeName << " has the following parameters:\n" + $string_representation; + return os; + } + /// \brief Issue a warning about missing default parameters. - void missingParamsWarning(){ + inline void missingParamsWarning() override { ROS_WARN_STREAM("[" << nodeName << "]\nThe following parameters do not have default values and need to be specified:\n" $non_default_params ); } - const std::string globalNamespace; - const std::string privateNamespace; - const std::string nodeName; }; } // namespace ${pkgname} + +#ifdef DYNAMIC_RECONFIGURE_FOUND +namespace rosparam_handler +{ + + /// \brief Specialize base class fromConfigImpl. + /// to handle ${pkgname}::${ClassName}Config + + template<> + inline void ParametersBase::fromConfigImpl<${pkgname}::${ClassName}Config>( + const ${pkgname}::${ClassName}Config& config, const uint32_t /*level*/ ) + { + try { + ${pkgname}::${ClassName}Parameters& casted_ref = dynamic_cast<${pkgname}::${ClassName}Parameters&>(*this); + + $fromConfig + } + catch (const std::bad_cast& /*e*/) + { + ROS_FATAL("In ParametersBase::fromConfig, bad cast to type ${pkgname}::${ClassName}Parameters !"); + rosparam_handler::exit("In ParametersBase::fromConfig, bad cast to type ${pkgname}::${ClassName}Parameters !"); + } + } + +} +#endif + +#endif /* _${pkgname}_ROSPARAM_HANDLER_${ClassName}Parameters_H_ */ diff --git a/test/cfg/Bar.params b/test/cfg/Bar.params new file mode 100755 index 0000000..ef452e5 --- /dev/null +++ b/test/cfg/Bar.params @@ -0,0 +1,16 @@ +#!/usr/bin/env python +####### workaround so that the module is found ####### +import sys +import os +sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +###################################################### + +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +# Parameters with different types +gen.add("str_param", paramtype="std::string", description="A string parameter", default="param b", configurable=True) +gen.add("vector_int_param", paramtype="std::vector", description="A vector of int parameter", default=[1,2,3]) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "Bar")) diff --git a/test/cfg/Foo.params b/test/cfg/Foo.params new file mode 100755 index 0000000..08d8c07 --- /dev/null +++ b/test/cfg/Foo.params @@ -0,0 +1,16 @@ +#!/usr/bin/env python +####### workaround so that the module is found ####### +import sys +import os +sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +###################################################### + +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +# Parameters with different types +gen.add("int_param", paramtype="int", description="An Integer parameter", default=1, configurable=True) +gen.add("str_param", paramtype="std::string", description="A string parameter", default="param a", configurable=True) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "Foo")) diff --git a/test/src/test_base_param.cpp b/test/src/test_base_param.cpp new file mode 100644 index 0000000..9292822 --- /dev/null +++ b/test/src/test_base_param.cpp @@ -0,0 +1,240 @@ +#include +#include +#include +#include +#include + +typedef rosparam_handler::FooParameters FooParameters; +typedef rosparam_handler::BarParameters BarParameters; + +typedef rosparam_handler::FooConfig ConfigA; +typedef rosparam_handler::BarConfig ConfigB; + +class TestRosparamHandlerBase : public testing::Test +{ +public: + + TestRosparamHandlerBase() = default; + ~TestRosparamHandlerBase() = default; + + struct DummyBase + { + DummyBase(const std::string& nh_namespace) : + nh_(nh_namespace) { } + + virtual ~DummyBase() = default; + + template + void init() + { + params_ptr_ = boost::make_shared(nh_); + } + + void fromParamServer() + { + params_ptr_->fromParamServer(); + } + + void toParamServer() + { + params_ptr_->toParamServer(); + } + + std::string getNamespace() + { + return nh_.getNamespace(); + } + + rosparam_handler::ParametersPtr params_ptr_; + + ros::NodeHandle nh_ = ros::NodeHandle("~"); + }; + + struct Foo : public DummyBase + { + Foo() : DummyBase("foo"), dr_srv_(nh_) + { + init(); + + dynamic_reconfigure::Server::CallbackType cb; + cb = boost::bind(&Foo::configCallback, this, _1, _2); + dr_srv_.setCallback(cb); + } + + void configCallback(ConfigA &config, uint32_t level) + { + params_ptr_->fromConfig(config); + } + + dynamic_reconfigure::Server dr_srv_; + }; + + struct Bar : public DummyBase + { + Bar() : DummyBase("bar"), dr_srv_(nh_) + { + init(); + + dynamic_reconfigure::Server::CallbackType cb; + cb = boost::bind(&Bar::configCallback, this, _1, _2); + dr_srv_.setCallback(cb); + } + + void configCallback(ConfigB &config, uint32_t level) + { + params_ptr_->fromConfig(config); + } + + dynamic_reconfigure::Server dr_srv_; + }; + + Foo foo_; + Bar bar_; +}; + +TEST_F(TestRosparamHandlerBase, Defaults) { + + ASSERT_NO_THROW(foo_.fromParamServer()); + ASSERT_NO_THROW(bar_.fromParamServer()); + + boost::shared_ptr foo_param_ptr; + boost::shared_ptr bar_param_ptr; + + ASSERT_NO_THROW(foo_param_ptr = boost::dynamic_pointer_cast(foo_.params_ptr_)); + ASSERT_NO_THROW(bar_param_ptr = boost::dynamic_pointer_cast(bar_.params_ptr_)); + + ASSERT_EQ(1, foo_param_ptr->int_param); + ASSERT_EQ("param a", foo_param_ptr->str_param); + + ASSERT_EQ("param b", bar_param_ptr->str_param); + ASSERT_EQ(std::vector({1, 2, 3}), bar_param_ptr->vector_int_param); +} + +TEST_F(TestRosparamHandlerBase, DefaultsOnParamServer) { + + ASSERT_NO_THROW(foo_.fromParamServer()); + ASSERT_NO_THROW(bar_.fromParamServer()); + + boost::shared_ptr foo_param_ptr; + boost::shared_ptr bar_param_ptr; + + ASSERT_NO_THROW(foo_param_ptr = boost::dynamic_pointer_cast(foo_.params_ptr_)); + ASSERT_NO_THROW(bar_param_ptr = boost::dynamic_pointer_cast(bar_.params_ptr_)); + + ros::NodeHandle nh("~"); + + // values should now be set on param server + { + int value; + ASSERT_TRUE(nh.getParam(foo_.getNamespace() + "/int_param", value)); + ASSERT_EQ(value, foo_param_ptr->int_param); + } + { + std::string value; + ASSERT_TRUE(nh.getParam(foo_.getNamespace() + "/str_param", value)); + ASSERT_EQ(value, foo_param_ptr->str_param); + } + { + std::vector value; + ASSERT_TRUE(nh.getParam(bar_.getNamespace() + "/vector_int_param", value)); + ASSERT_EQ(value, bar_param_ptr->vector_int_param); + } + { + std::string value; + ASSERT_TRUE(nh.getParam(bar_.getNamespace() + "/str_param", value)); + ASSERT_EQ(value, bar_param_ptr->str_param); + } +} + +TEST_F(TestRosparamHandlerBase, SetParamOnServer) { + + ASSERT_NO_THROW(foo_.fromParamServer()); + ASSERT_NO_THROW(bar_.fromParamServer()); + + boost::shared_ptr foo_param_ptr; + boost::shared_ptr bar_param_ptr; + + ASSERT_NO_THROW(foo_param_ptr = boost::dynamic_pointer_cast(foo_.params_ptr_)); + ASSERT_NO_THROW(bar_param_ptr = boost::dynamic_pointer_cast(bar_.params_ptr_)); + + foo_param_ptr->int_param = 9; + foo_param_ptr->str_param = "Hello"; + + bar_param_ptr->vector_int_param = {4,2}; + bar_param_ptr->str_param = "World"; + + ASSERT_NO_THROW(foo_.toParamServer()); + ASSERT_NO_THROW(bar_.toParamServer()); + + ros::NodeHandle nh("~"); + + // values should now be set on param server + { + int value; + ASSERT_TRUE(nh.getParam(foo_.getNamespace() + "/int_param", value)); + ASSERT_EQ(value, foo_param_ptr->int_param); + } + { + std::string value; + ASSERT_TRUE(nh.getParam(foo_.getNamespace() + "/str_param", value)); + ASSERT_EQ(value, foo_param_ptr->str_param); + } + { + std::vector value; + ASSERT_TRUE(nh.getParam(bar_.getNamespace() + "/vector_int_param", value)); + ASSERT_EQ(value, bar_param_ptr->vector_int_param); + } + { + std::string value; + ASSERT_TRUE(nh.getParam(bar_.getNamespace() + "/str_param", value)); + ASSERT_EQ(value, bar_param_ptr->str_param); + } +} + +TEST_F(TestRosparamHandlerBase, DynamicReconf) +{ + ASSERT_NO_THROW(foo_.fromParamServer()); + ASSERT_NO_THROW(bar_.fromParamServer()); + + boost::shared_ptr foo_param_ptr; + boost::shared_ptr bar_param_ptr; + + ASSERT_NO_THROW(foo_param_ptr = boost::dynamic_pointer_cast(foo_.params_ptr_)); + ASSERT_NO_THROW(bar_param_ptr = boost::dynamic_pointer_cast(bar_.params_ptr_)); + + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::Config conf; + + dynamic_reconfigure::IntParameter int_param; + int_param.name = "int_param"; + int_param.value = 0; + + dynamic_reconfigure::StrParameter str_param; + str_param.name = "str_param"; + str_param.value = "dynamic foo string"; + + conf.ints.push_back(int_param); + conf.strs.push_back(str_param); + + srv_req.config = conf; + + ros::service::call(foo_.getNamespace() + "/set_parameters", srv_req, srv_resp); + + conf = dynamic_reconfigure::Config(); + str_param.name = "str_param"; + str_param.value = "dynamic bar string"; + + conf.strs.push_back(str_param); + + srv_req.config = conf; + + ros::service::call(bar_.getNamespace() + "/set_parameters", srv_req, srv_resp); + + ros::Duration(1).sleep(); + + ASSERT_EQ(0, foo_param_ptr->int_param); + ASSERT_EQ("dynamic foo string", foo_param_ptr->str_param); + + ASSERT_EQ("dynamic bar string", bar_param_ptr->str_param); +}