From 52a8387b6e36ce8607af337304d33def9ed23831 Mon Sep 17 00:00:00 2001 From: vincentrou Date: Mon, 3 Jul 2017 16:00:42 +0200 Subject: [PATCH 01/47] Remove useless character --- doc/HowToWriteYourFirstParamsFile.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/HowToWriteYourFirstParamsFile.md b/doc/HowToWriteYourFirstParamsFile.md index 363c5bc..c0a5ae3 100644 --- a/doc/HowToWriteYourFirstParamsFile.md +++ b/doc/HowToWriteYourFirstParamsFile.md @@ -54,7 +54,7 @@ global_scope=False, constant=False) # Add an enum: gen.add_enum("my_enum", description="My first self written enum", -entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium")) +entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium") # Add a subgroup my_group = gen.add_group("my_group") From 0f83e8e7d64a9c6bd71b881b0d1d690738a86847 Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Wed, 28 Dec 2016 20:38:37 +0100 Subject: [PATCH 02/47] param handler now generates python parameter files too --- cmake/rosparam_handler-macros.cmake | 48 +++++++++- .../parameter_generator_catkin.py | 33 ++++++- templates/Parameters.py.template | 94 +++++++++++++++++++ 3 files changed, 169 insertions(+), 6 deletions(-) create mode 100644 templates/Parameters.py.template diff --git a/cmake/rosparam_handler-macros.cmake b/cmake/rosparam_handler-macros.cmake index 9f4d8a0..10c7fb1 100644 --- a/cmake/rosparam_handler-macros.cmake +++ b/cmake/rosparam_handler-macros.cmake @@ -1,6 +1,5 @@ macro(generate_ros_parameter_files) - set(CFG_FILES "${ARGN}") set(ROSPARAM_HANDLER_ROOT_DIR "${ROSPARAM_HANDLER_CMAKE_DIR}/..") if (${PROJECT_NAME}_CATKIN_PACKAGE) @@ -33,6 +32,8 @@ macro(generate_ros_parameter_files) get_filename_component(_cfgonly ${_cfg} NAME_WE) set(_output_cfg ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg/${_cfgonly}.cfg) set(_output_cpp ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/${_cfgonly}Parameters.h) + set(_output_py ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/param/${_cfgonly}Parameters.py) + # Create command assert(CATKIN_ENV) @@ -42,17 +43,18 @@ macro(generate_ros_parameter_files) ${ROSPARAM_HANDLER_ROOT_DIR} ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} ) add_custom_command(OUTPUT - ${_output_cpp} ${_output_cfg} + ${_output_cpp} ${_output_cfg} ${_output_py} COMMAND ${_cmd} DEPENDS ${_input} ${genparam_build_files} COMMENT "Generating parameter files from ${_cfgonly}" ) list(APPEND ${PROJECT_NAME}_LOCAL_CFG_FILES "${_output_cfg}") - list(APPEND ${PROJECT_NAME}_params_generated ${_output_cpp} ${_output_cfg}) + list(APPEND ${PROJECT_NAME}_params_generated ${_output_cpp} ${_output_cfg} ${_output_py}) install( FILES ${_output_cpp} @@ -78,11 +80,13 @@ macro(generate_ros_parameter_files) list(APPEND ${PROJECT_NAME}_INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) # ensure that the folder exists file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) - #Require C++11 set_property(TARGET ${PROJECT_NAME}_genparam PROPERTY CXX_STANDARD 11) set_property(TARGET ${PROJECT_NAME}_genparam PROPERTY CXX_STANDARD_REQUIRED ON) + # install python files + install_ros_python_parameter_files() + # generate dynamic reconfigure files if(dynamic_reconfigure_FOUND_CATKIN_PROJECT) if(${PROJECT_NAME}_LOCAL_CFG_FILES) @@ -92,3 +96,39 @@ macro(generate_ros_parameter_files) message(WARNING "Dependency to dynamic_reconfigure is missing, or find_package(dynamic_reconfigure) was not called yet. Not building dynamic config files") endif() endmacro() + +macro(install_ros_python_parameter_files) + if(NOT install_ros_python_parameter_files_CALLED) + set(install_ros_python_parameter_files_CALLED TRUE) + + # mark that generate_dynamic_reconfigure_options() was called in order to detect wrong order of calling with catkin_python_setup() + set(${PROJECT_NAME}_GENERATE_DYNAMIC_RECONFIGURE TRUE) + + # check if catkin_python_setup() installs an __init__.py file for a package with the current project name + # in order to skip the installation of a generated __init__.py file + set(package_has_static_sources ${${PROJECT_NAME}_CATKIN_PYTHON_SETUP_HAS_PACKAGE_INIT}) + if(NOT EXISTS ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py) + file(WRITE ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py "") + endif() + if(NOT package_has_static_sources) + # install package __init__.py + install( + FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + ) + endif() + + # generate param module __init__.py + if(NOT EXISTS ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/param/__init__.py) + file(WRITE ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/param/__init__.py "") + endif() + + # compile python code before installing + find_package(PythonInterp REQUIRED) + install(CODE "execute_process(COMMAND \"${PYTHON_EXECUTABLE}\" -m compileall \"${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/param\")") + install( + DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/param + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + ) + endif() +endmacro() diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index d9457c8..eadb980 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -58,12 +58,13 @@ def __init__(self, parent=None, group=""): self.group = "gen" self.group_variable = filter(str.isalnum, self.group) - if len(sys.argv) != 4: + if len(sys.argv) != 5: eprint("ParameterGenerator: Unexpected amount of args, did you try to call this directly? You shouldn't do this!") self.dynconfpath = sys.argv[1] self.share_dir = sys.argv[2] self.cpp_gen_dir = sys.argv[3] + self.py_gen_dir = sys.argv[4] self.pkgname = None self.nodename = None @@ -93,7 +94,7 @@ def add_enum(self, name, description, entry_strings, default=None): entry_strings = [str(e) for e in entry_strings] # Make sure we only get strings if default is None: - default = entry_strings[0] + default = 0 else: default = entry_strings.index(default) self.add(name=name, paramtype="int", description=description, edit_method=name, default=default, @@ -330,6 +331,7 @@ def generate(self, pkgname, nodename, classname): self._generatecfg() self._generatecpp() + self._generatepy() return 0 @@ -459,6 +461,33 @@ def _generatecpp(self): with open(header_file, 'w') as f: f.write(content) + def _generatepy(self): + """ + Generate Python parameter file + :param self: + :return: + """ + params = self._get_parameters() + paramDescription = str(params) + + # Read in template file + templatefile = os.path.join(self.dynconfpath, "templates", "Parameters.py.template") + with open(templatefile, 'r') as f: + template = f.read() + + content = Template(template).substitute(pkgname=self.pkgname, ClassName=self.classname, + paramDescription=paramDescription) + + py_file = os.path.join(self.py_gen_dir, "param", self.classname + "Parameters.py") + try: + if not os.path.exists(os.path.dirname(py_file)): + os.makedirs(os.path.dirname(py_file)) + except OSError: + # Stupid error, sometimes the directory exists anyway + pass + with open(py_file, 'w') as f: + f.write(content) + def _get_parameters(self): """ Returns parameter of this and all childs diff --git a/templates/Parameters.py.template b/templates/Parameters.py.template new file mode 100644 index 0000000..2843eb5 --- /dev/null +++ b/templates/Parameters.py.template @@ -0,0 +1,94 @@ +# ********************************************************* +# +# File autogenerated for the ${pkgname} package +# by the rosparam_handler package. +# Please do not edit. +# +# ********************************************************* +import rospy + +param_description = $paramDescription + +defaults = {} + +for param in param_description: + defaults[param['name']] = param['default'] + + +class ${ClassName}Parameters(dict): + def __init__(self): + super(self.__class__, self).__init__(defaults) + self.from_param_server() + + __getattr__ = dict.__getitem__ + __setattr__ = dict.__setitem__ + + def from_param_server(self): + """ + Reads and initializes parameters with values from parameter server. + Called automatically at initialization. + """ + for k, v in self.iteritems(): + config = next((item for item in param_description if item["name"] == k), None) + if config['constant']: + self.test_const_param(k) + continue + + self[k] = self.get_param(k, config) + + def from_config(self, config): + """ + Reads parameter from a dynamic_reconfigure config file. + Should be called in the callback of dynamic_reconfigure. + :param config: config object from dynamic reconfigure + """ + for k, v in config.iteritems(): + # handle reserved name groups + if k == "groups": + continue + if not k in self: + raise TypeError("Element {} of config is not part of parameters.".format(k)) + self[k] = v + + @staticmethod + def test_const_param(param_name): + if rospy.has_param("~" + param_name): + rospy.logwarn( + "Parameter {} was set on the parameter server even though it was defined to be constant.".format( + param_name)) + + @staticmethod + def get_param(param_name, config): + full_name = "/" + param_name if config['global_scope'] else "~" + param_name + val = rospy.get_param(full_name, config['default']) + # test whether type is correct + try: + param_type = config['type'] + if config['is_vector']: + val = list(val) + elif config['is_map']: + val = dict(val) + elif param_type == 'std::string': + val = str(val) + elif param_type == 'int': + val = int(val) + elif param_type == 'bool': + val = bool(val) + elif param_type == 'float' or param_type == 'double': + val = float(val) + except ValueError: + rospy.logerr( + "Parameter {} is set, but has a different type. Using default value instead.".format(param_name)) + val = config['default'] + # test bounds + if config['min'] is not None and config['min'] > val: + rospy.logerr( + "Value of {} for {} is smaller than minimal allowed value. " + "Correcting value to min={}".format(val, param_name, config['min'])) + val = config['min'] + if config['max'] is not None and config['max'] < val: + rospy.logerr( + "Value of {} for {} is greater than maximal allowed value. " + "Correcting value to max={}".format(val,param_name, config['max'])) + val = config['max'] + return val From f34f612a016b7cbcf14148cb7af73f2b5d94cead Mon Sep 17 00:00:00 2001 From: Niels Ole Salscheider Date: Mon, 20 Mar 2017 09:30:14 +0100 Subject: [PATCH 03/47] Fix initialisation order in parameter class template --- templates/Parameters.h.template | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index ad97be9..78988b2 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -73,9 +73,9 @@ struct ${ClassName}Parameters { using Config = ${ClassName}Config; ${ClassName}Parameters(const ros::NodeHandle& private_node_handle) - : privateNamespace{private_node_handle.getNamespace() + "/"}, - nodeName{getNodeName(private_node_handle)}, - globalNamespace{"/"} {} + : globalNamespace{"/"}, + privateNamespace{private_node_handle.getNamespace() + "/"}, + nodeName{getNodeName(private_node_handle)} {} void fromParamServer(){ $fromParamServer From 9dd15bdbfcb06f2b4f5919fd38af5d5a29e60cdd Mon Sep 17 00:00:00 2001 From: Jan-Hendrik Pauls Date: Mon, 9 Jan 2017 17:18:21 +0100 Subject: [PATCH 04/47] Set default values on parameter server if no value is set --- templates/Parameters.h.template | 2 ++ 1 file changed, 2 insertions(+) diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index ad97be9..2c94a0a 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -122,6 +122,8 @@ $non_default_params ); void getParam(const std::string key, T& val, const T& defaultValue) { if (!ros::param::has(key)) { val = defaultValue; + ros::param::set(key, defaultValue); + ROS_INFO_STREAM("Parameter "< Date: Tue, 11 Jul 2017 17:35:57 +0200 Subject: [PATCH 05/47] Include python usage in documentation --- README.md | 1 + doc/HowToUseYourParameterStruct.md | 21 ++++++++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index fcb110a..534e581 100644 --- a/README.md +++ b/README.md @@ -25,6 +25,7 @@ The `rosparam_handler` let's you: - set default, min and max values - choose between global and private namespace - save a lot of time on specifying your parameters in several places. +- ...in both C++ and Python ## Usage See the Tutorials on diff --git a/doc/HowToUseYourParameterStruct.md b/doc/HowToUseYourParameterStruct.md index 664d51b..853bd8d 100644 --- a/doc/HowToUseYourParameterStruct.md +++ b/doc/HowToUseYourParameterStruct.md @@ -52,4 +52,23 @@ void reconfigureRequest(TutorialConfig& config, uint32_t level) { ``` 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 \ No newline at end of file +You can find a running version of this example code in the [rosparam_handler_tutorial](https://github.com/cbandera/rosparam_handler_tutorial)-Repository + +## Python +All your parameters are fully available in python nodes as well. Just import the parameter file: +```python +from rosparam_tutorials.param.TutorialParameters import TutorialParameters +``` + +Unlike in C++, the call to fromParamServer is not necessary: +```python +self.params = TutorialParameters() +``` + +And a dynamic_reconfigure callback might look like that: +```python +def reconfigure_callback(self, config, level): + self.params.from_config(config) + print("Parameter dummy changed to {}".format(self.params.dummy)) +``` + From 84ff1a471fa29847853cf560435556f0f7d258f9 Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Tue, 11 Jul 2017 18:54:39 +0200 Subject: [PATCH 06/47] Add min/max check for map and vector to be consistent with C++ --- templates/Parameters.py.template | 78 ++++++++++++++++++++++++-------- 1 file changed, 59 insertions(+), 19 deletions(-) diff --git a/templates/Parameters.py.template b/templates/Parameters.py.template index 2843eb5..bdc72f0 100644 --- a/templates/Parameters.py.template +++ b/templates/Parameters.py.template @@ -59,36 +59,76 @@ class ${ClassName}Parameters(dict): @staticmethod def get_param(param_name, config): + def get_type(type_string): + if type_string == 'std::string': + return str + elif type_string == 'int': + return int + elif type_string == 'bool': + return bool + elif type_string == 'float' or type_string == 'double': + return float + else: + raise ValueError() + full_name = "/" + param_name if config['global_scope'] else "~" + param_name val = rospy.get_param(full_name, config['default']) # test whether type is correct try: - param_type = config['type'] if config['is_vector']: val = list(val) + config_type = config['type'] + val_type = get_type(config_type[config_type.find("<")+1:config_type.find(">")]) + val = [ val_type(v) for v in val ] elif config['is_map']: val = dict(val) - elif param_type == 'std::string': - val = str(val) - elif param_type == 'int': - val = int(val) - elif param_type == 'bool': - val = bool(val) - elif param_type == 'float' or param_type == 'double': - val = float(val) + config_type = config['type'] + key_type = get_type(config_type[config_type.find("<")+1:config_type.find(",")]) + val_type = get_type(config_type[config_type.find(",")+1:config_type.find(">")]) + val = { key_type(key): val_type(v) for key, v in val.items() } + else: + val = get_type(config['type'])(val) except ValueError: rospy.logerr( "Parameter {} is set, but has a different type. Using default value instead.".format(param_name)) val = config['default'] # test bounds - if config['min'] is not None and config['min'] > val: - rospy.logerr( - "Value of {} for {} is smaller than minimal allowed value. " - "Correcting value to min={}".format(val, param_name, config['min'])) - val = config['min'] - if config['max'] is not None and config['max'] < val: - rospy.logerr( - "Value of {} for {} is greater than maximal allowed value. " - "Correcting value to max={}".format(val,param_name, config['max'])) - val = config['max'] + if config['min'] is not None: + if config['is_vector']: + if min(val) < config['min']: + rospy.logerr( + "Some values in {} for {} are smaller than minimal allowed value. " + "Correcting them to min={}".format(val, param_name, config['min'])) + val = [ v if v > config['min'] else config['min'] for v in val ] + elif config['is_map']: + if min(val.values()) < config['min']: + rospy.logerr( + "Some values in {} for {} are smaller than minimal allowed value. " + "Correcting them to min={}".format(val, param_name, config['min'])) + val = { k: (v if v > config['min'] else config['min']) for k, v in val.items() } + elif val < config['min']: + rospy.logerr( + "Value of {} for {} is smaller than minimal allowed value. " + "Correcting value to min={}".format(val, param_name, config['min'])) + val = config['min'] + + if config['max'] is not None: + if config['is_vector']: + if max(val) > config['max']: + rospy.logerr( + "Some values in {} for {} are greater than maximal allowed value. " + "Correcting them to max={}".format(val, param_name, config['max'])) + val = [ v if v < config['max'] else config['max'] for v in val ] + elif config['is_map']: + if max(val.values()) > config['max']: + rospy.logerr( + "Some values in {} for {} are greater than maximal allowed value. " + "Correcting them to max={}".format(val, param_name, config['max'])) + val = { k: (v if v < config['max'] else config['max']) for k, v in val.items() } + elif val > config['max']: + rospy.logerr( + "Value of {} for {} is greater than maximal allowed value. " + "Correcting value to max={}".format(val, param_name, config['max'])) + val = config['max'] return val + From 87c050ae256697b6e8405394b7c96898a001b789 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 10:05:55 +0200 Subject: [PATCH 07/47] Handler is now throwing an exception upon failure. Before, it was calling std::exit(EXIT_FAILURE) but it was causing problems during testing. --- templates/Parameters.h.template | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index ad97be9..0b7d4e5 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -22,8 +22,8 @@ struct ${ClassName}Config{}; namespace ${pkgname} { -#ifndef UTIL_FUNCTIONS_${pkgname} -#define UTIL_FUNCTIONS_${pkgname} +#ifndef ROSPARAM_HANDLER_UTIL_FUNCTIONS +#define ROSPARAM_HANDLER_UTIL_FUNCTIONS inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle){ std::string name_space = privateNodeHandle.getNamespace(); std::stringstream tempString(name_space); @@ -66,7 +66,15 @@ using is_map = std::is_same>; -#endif /* UTIL_FUNCTIONS_${pkgname} */ +class RosparamHandlerError : public std::runtime_error {}; + +void rosparam_handler_exit(const std::string msg = "Runtime Error in rosparam handler.") +{ + // std::exit(EXIT_FAILURE); + throw std::runtime_error(msg); +} + +#endif // ROSPARAM_HANDLER_UTIL_FUNCTIONS struct ${ClassName}Parameters { @@ -89,7 +97,7 @@ $test_limits $fromConfig #else ROS_FATAL_STREAM("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); - std::exit(EXIT_FAILURE); + rosparam_handler_exit("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); #endif } @@ -110,11 +118,11 @@ $non_default_params ); if (!ros::param::has(key)) { ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); missingParamsWarning(); - std::exit(EXIT_FAILURE); + rosparam_handler_exit("RosparamHandler: Undefined parameter."); } else if (!ros::param::get(key, val)) { ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); missingParamsWarning(); - std::exit(EXIT_FAILURE); + rosparam_handler_exit("RosparamHandler: GetParam could net retrieve parameter."); } } From 8a5bab04c7aab411b79003429ac659cd6800201b Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 10:24:31 +0200 Subject: [PATCH 08/47] Moved common functions to utilities.hpp. Fixes #11 --- include/rosparam_handler/utilities.hpp | 74 +++++++++++++++++++++----- templates/Parameters.h.template | 63 ++-------------------- 2 files changed, 67 insertions(+), 70 deletions(-) diff --git a/include/rosparam_handler/utilities.hpp b/include/rosparam_handler/utilities.hpp index f100a4f..10f6fe1 100644 --- a/include/rosparam_handler/utilities.hpp +++ b/include/rosparam_handler/utilities.hpp @@ -5,13 +5,9 @@ namespace rosparam_handler { -/** - * Sets the logger level according to a standardized parameter - * name 'verbosity'. - * - * @param nodeHandle The ROS node handle to search for the - * parameter 'verbosity'. - */ +/// \brief Sets the logger level according to a standardized parameter name 'verbosity'. +/// +/// @param nodeHandle The ROS node handle to search for the parameter 'verbosity'. inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) { std::string verbosity; @@ -44,10 +40,7 @@ inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) { } } -/** - * Show summary about node containing name, namespace, - * subscribed and advertised topics. - */ +/// \brief Show summary about node containing name, namespace, subscribed and advertised topics. inline void showNodeInfo() { using namespace ros::this_node; @@ -68,4 +61,61 @@ inline void showNodeInfo() { "Started '" << getName() << "' in namespace '" << getNamespace() << "'." << std::endl << "Subscribed topics: " << std::endl << msg_subscr.str() << "Advertised topics: " << std::endl << msg_advert.str()); } -} // rosparam_handler + +/// \brief Retrieve node name +/// +/// @param privateNodeHandle The private ROS node handle (i.e. ros::NodeHandle("~") ). +/// @return node name +inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle){ + std::string name_space = privateNodeHandle.getNamespace(); + std::stringstream tempString(name_space); + std::string name; + while(std::getline(tempString, name, '/')){;} + return name; +} + +/// \brief ExitFunction for rosparam_handler +inline void exit(const std::string msg = "Runtime Error in rosparam handler.") +{ + // std::exit(EXIT_FAILURE); + throw std::runtime_error(msg); +} + + +} // namespace rosparam_handler + +/// \brief Outstream helper for std:vector +template +std::ostream& operator<< (std::ostream& out, const std::vector& v) { + if ( !v.empty() ) { + out << '['; + std::copy (v.begin(), v.end(), std::ostream_iterator(out, ", ")); + out << "\b\b]"; + } + return out; +} + +/// \brief Outstream helper for std:map +template +std::ostream &operator<<(std::ostream &stream, const std::map& map) +{ + stream << '{'; + for (typename std::map::const_iterator it = map.begin(); + it != map.end(); + ++it) + { + stream << (*it).first << " --> " << (*it).second << ", "; + } + stream << '}'; + return stream; +} + +template +using is_vector = std::is_same>; + +template +using is_map = std::is_same>; \ No newline at end of file diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index 0b7d4e5..a8f65f3 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -13,6 +13,7 @@ #include #include #include +#include #ifdef DYNAMIC_RECONFIGURE_FOUND #include <${pkgname}/${ClassName}Config.h> #else @@ -22,67 +23,13 @@ struct ${ClassName}Config{}; namespace ${pkgname} { -#ifndef ROSPARAM_HANDLER_UTIL_FUNCTIONS -#define ROSPARAM_HANDLER_UTIL_FUNCTIONS -inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle){ - std::string name_space = privateNodeHandle.getNamespace(); - std::stringstream tempString(name_space); - std::string name; - while(std::getline(tempString, name, '/')){;} - return name; -} - -template -std::ostream& operator<< (std::ostream& out, const std::vector& v) { - if ( !v.empty() ) { - out << '['; - std::copy (v.begin(), v.end(), std::ostream_iterator(out, ", ")); - out << "\b\b]"; - } - return out; -} - -template -std::ostream &operator<<(std::ostream &stream, const std::map& map) -{ - stream << '{'; - for (typename std::map::const_iterator it = map.begin(); - it != map.end(); - ++it) - { - stream << (*it).first << " --> " << (*it).second << ", "; - } - stream << '}'; - return stream; -} - -template -using is_vector = std::is_same>; - -template -using is_map = std::is_same>; - -class RosparamHandlerError : public std::runtime_error {}; - -void rosparam_handler_exit(const std::string msg = "Runtime Error in rosparam handler.") -{ - // std::exit(EXIT_FAILURE); - throw std::runtime_error(msg); -} - -#endif // ROSPARAM_HANDLER_UTIL_FUNCTIONS - struct ${ClassName}Parameters { using Config = ${ClassName}Config; ${ClassName}Parameters(const ros::NodeHandle& private_node_handle) : privateNamespace{private_node_handle.getNamespace() + "/"}, - nodeName{getNodeName(private_node_handle)}, + nodeName{rosparam_handler::getNodeName(private_node_handle)}, globalNamespace{"/"} {} void fromParamServer(){ @@ -97,7 +44,7 @@ $test_limits $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."); + rosparam_handler::exit("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); #endif } @@ -118,11 +65,11 @@ $non_default_params ); if (!ros::param::has(key)) { ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); missingParamsWarning(); - rosparam_handler_exit("RosparamHandler: Undefined parameter."); + rosparam_handler::exit("RosparamHandler: Undefined parameter."); } else if (!ros::param::get(key, val)) { ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); missingParamsWarning(); - rosparam_handler_exit("RosparamHandler: GetParam could net retrieve parameter."); + rosparam_handler::exit("RosparamHandler: GetParam could net retrieve parameter."); } } From 5bbd0d56f6f5197d734287e4753413c6222de89e Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 10:29:57 +0200 Subject: [PATCH 09/47] Updated README to include release information. Closes #17. --- README.md | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/README.md b/README.md index fcb110a..61b1fb8 100644 --- a/README.md +++ b/README.md @@ -32,6 +32,15 @@ See the Tutorials on - [How to use your parameter struct](doc/HowToUseYourParameterStruct.md) - [rosparam_handler_tutorial](https://github.com/cbandera/rosparam_handler_tutorial) +## Installation +`rosparam_handler` has been released for +- indigo +- jade +- kinetic + +To get the package run +`rosdep update && rosdep install rosparam_handler` + ## Credits This project uses Open Source components. The code was, in large parts, built upon such existing open source software. You can find the source code of their projects along with license information below. We acknowledge and are grateful to these developers for their contributions to open source. From bbd4de2d25807dbcaacf7a614e033941db0403c2 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 10:36:09 +0200 Subject: [PATCH 10/47] Added LICENSE file. Closes #25 --- LICENSE | 26 ++++++++++++++++++++++++++ README.md | 2 +- 2 files changed, 27 insertions(+), 1 deletion(-) create mode 100644 LICENSE diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..6a5cfec --- /dev/null +++ b/LICENSE @@ -0,0 +1,26 @@ +Copyright (c) 2017, Claudio Bandera +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +The views and conclusions contained in the software and documentation are those +of the authors and should not be interpreted as representing official policies, +either expressed or implied, of the FreeBSD Project. diff --git a/README.md b/README.md index 61b1fb8..8a69364 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,7 @@ See the Tutorials on - [rosparam_handler_tutorial](https://github.com/cbandera/rosparam_handler_tutorial) ## Installation -`rosparam_handler` has been released for +`rosparam_handler` has been released in version `0.1.1` for - indigo - jade - kinetic From 32e2450690ae28e10923578971b7e246288fed34 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 11:52:12 +0200 Subject: [PATCH 11/47] Added first set of tests --- CMakeLists.txt | 32 ++++++++++++++++++- package.xml | 4 ++- .../parameter_generator_catkin.py | 14 ++++---- test/cfg/Defaults.params | 27 ++++++++++++++++ test/cfg/DefaultsAtLaunch.params | 27 ++++++++++++++++ test/cfg/DefaultsMissing.params | 14 ++++++++ test/cfg/MinMax.params | 21 ++++++++++++ .../launch/params/test_launch_parameters.yaml | 22 +++++++++++++ test/launch/rosparam_handler.test | 6 ++++ test/src/test_defaults.cpp | 26 +++++++++++++++ test/src/test_defaultsAtLaunch.cpp | 26 +++++++++++++++ test/src/test_defaultsMissing.cpp | 11 +++++++ test/src/test_main.cpp | 14 ++++++++ test/src/test_minMax.cpp | 21 ++++++++++++ 14 files changed, 256 insertions(+), 9 deletions(-) create mode 100755 test/cfg/Defaults.params create mode 100755 test/cfg/DefaultsAtLaunch.params create mode 100755 test/cfg/DefaultsMissing.params create mode 100755 test/cfg/MinMax.params create mode 100644 test/launch/params/test_launch_parameters.yaml create mode 100644 test/launch/rosparam_handler.test create mode 100644 test/src/test_defaults.cpp create mode 100644 test/src/test_defaultsAtLaunch.cpp create mode 100644 test/src/test_defaultsMissing.cpp create mode 100644 test/src/test_main.cpp create mode 100644 test/src/test_minMax.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0af89a7..587a81b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,25 @@ find_package(catkin REQUIRED) catkin_python_setup() +if (CATKIN_ENABLE_TESTING) + # set compiler flags + include(CheckCXXCompilerFlag) + CHECK_CXX_COMPILER_FLAG("-std=c++14" Cpp14CompilerFlag) + if (${Cpp14CompilerFlag}) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") + elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "17") + #no additional flag is required + else() + message(FATAL_ERROR "Compiler does not have c++14 support. Use at least g++4.9 or Visual Studio 2013 and newer.") + endif() + + find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure rostest roscpp) + set(ROSPARAM_HANDLER_CMAKE_DIR ${CMAKE_CURRENT_LIST_DIR}/cmake) + include(cmake/rosparam_handler-macros.cmake) + file(GLOB PROJECT_TEST_FILES_PARAMS RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/cfg/*.params") + generate_ros_parameter_files(${PROJECT_TEST_FILES_PARAMS}) +endif() + catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS catkin @@ -24,4 +43,15 @@ install( install( DIRECTORY include/rosparam_handler DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) \ No newline at end of file +) + +if (CATKIN_ENABLE_TESTING) + file(GLOB PROJECT_TEST_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/src/*.cpp") + set(TEST_TARGET_NAME "rosparam_handler_test") + add_rostest_gtest(${TEST_TARGET_NAME} test/launch/rosparam_handler.test ${PROJECT_TEST_FILES_SRC}) + target_link_libraries(${TEST_TARGET_NAME} ${catkin_LIBRARIES} gtest) + target_include_directories(${TEST_TARGET_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) + add_dependencies(${TEST_TARGET_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) + set_property(TARGET ${TEST_TARGET_NAME} PROPERTY CXX_STANDARD 11) + set_property(TARGET ${TEST_TARGET_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) +endif() diff --git a/package.xml b/package.xml index 6d3bbfe..4a2ce05 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,8 @@ catkin catkin - + rostest + roscpp + dynamic_reconfigure diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index d9457c8..787c0f9 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -93,7 +93,7 @@ def add_enum(self, name, description, entry_strings, default=None): entry_strings = [str(e) for e in entry_strings] # Make sure we only get strings if default is None: - default = entry_strings[0] + default = 0 else: default = entry_strings.index(default) self.add(name=name, paramtype="int", description=description, edit_method=name, default=default, @@ -110,7 +110,6 @@ def add(self, name, paramtype, description, level=0, edit_method='""', default=N - If no default value is given, you need to specify one in your launch file - Global parameters, vectors, maps and constant params can not be configurable - - Global parameters, vectors and maps can not have a default, min or max value :param self: :param name: The Name of you new parameter @@ -159,10 +158,11 @@ def _perform_checks(self, param): :return: """ - if param['type'].strip() == "std::string" and (param['max'] is not None or param['min'] is not None): - eprint(param['name'], "Max or min specified for for variable of type string") - in_type = param['type'].strip() + if param['max'] is not None or param['min'] is not None: + if in_type in ["std::string", "bool"]: + eprint(param['name'], "Max and min can not be specified for variable of type %s" % in_type) + if in_type.startswith('std::vector'): param['is_vector'] = True if in_type.startswith('std::map'): @@ -172,7 +172,7 @@ def _perform_checks(self, param): if (param['max'] is not None or param['min'] is not None): ptype = in_type[12:-1].strip() if ptype == "std::string": - eprint(param['name'], "Max and min can not be specified for variable of type %s" % param['type']) + eprint(param['name'], "Max and min can not be specified for variable of type %s" % in_type) if (param['is_map']): if (param['max'] is not None or param['min'] is not None): @@ -182,7 +182,7 @@ def _perform_checks(self, param): "parameter %s" % in_type) ptype = ptype[1].strip() if ptype == "std::string": - eprint(param['name'], "Max and min can not be specified for variable of type %s" % param['type']) + eprint(param['name'], "Max and min can not be specified for variable of type %s" % in_type) pattern = r'^[a-zA-Z][a-zA-Z0-9_]*$' if not re.match(pattern, param['name']): diff --git a/test/cfg/Defaults.params b/test/cfg/Defaults.params new file mode 100755 index 0000000..df3f0cf --- /dev/null +++ b/test/cfg/Defaults.params @@ -0,0 +1,27 @@ +#!/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_w_default", paramtype="int", description="An Integer parameter", default=1) +gen.add("double_param_w_default", paramtype="double",description="A double parameter", default=1.1) +gen.add("str_param_w_default", paramtype="std::string", description="A string parameter", default="Hello World") +gen.add("bool_param_w_default", paramtype="bool", description="A Boolean parameter", default=True) + +gen.add("vector_int_param_w_default", paramtype="std::vector", description="A vector of int parameter", default=[1,2,3]) +gen.add("vector_double_param_w_default", paramtype="std::vector", description="A vector of double parameter", default=[1.1, 1.2, 1.3]) +gen.add("vector_bool_param_w_default", paramtype="std::vector", description="A vector of bool parameter", default=[False, True]) +gen.add("vector_string_param_w_default", paramtype="std::vector", description="A vector of string parameter", default=["Hello", "World"]) + +gen.add("map_param_w_default", paramtype="std::map", description="A map parameter", default={"Hello": "World"}) + +gen.add_enum("enum_param_w_default", description="enum", entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium") + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "Defaults")) \ No newline at end of file diff --git a/test/cfg/DefaultsAtLaunch.params b/test/cfg/DefaultsAtLaunch.params new file mode 100755 index 0000000..a1727cd --- /dev/null +++ b/test/cfg/DefaultsAtLaunch.params @@ -0,0 +1,27 @@ +#!/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_wo_default", paramtype="int", description="An Integer parameter") +gen.add("double_param_wo_default", paramtype="double",description="A double parameter") +gen.add("str_param_wo_default", paramtype="std::string", description="A string parameter") +gen.add("bool_param_wo_default", paramtype="bool", description="A Boolean parameter") + +gen.add("vector_int_param_wo_default", paramtype="std::vector", description="A vector of int parameter") +gen.add("vector_double_param_wo_default", paramtype="std::vector", description="A vector of double parameter") +gen.add("vector_bool_param_wo_default", paramtype="std::vector", description="A vector of bool parameter") +gen.add("vector_string_param_wo_default", paramtype="std::vector", description="A vector of string parameter") + +gen.add("map_param_wo_default", paramtype="std::map", description="A map parameter") + +gen.add_enum("enum_param_wo_default", description="enum", entry_strings=["Small", "Medium", "Large", "ExtraLarge"]) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "DefaultsAtLaunch")) \ No newline at end of file diff --git a/test/cfg/DefaultsMissing.params b/test/cfg/DefaultsMissing.params new file mode 100755 index 0000000..d722a8c --- /dev/null +++ b/test/cfg/DefaultsMissing.params @@ -0,0 +1,14 @@ +#!/usr/bin/env python +####### workaround so that the module is found for test files ####### +import sys +import os +sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +###################################################### + +from rosparam_handler.parameter_generator_catkin import * +gen = ParameterGenerator() + +gen.add("int_param", paramtype="int", description="Handler should keep node from starting, when no default is given here or at launch.") + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "DefaultsMissing")) \ No newline at end of file diff --git a/test/cfg/MinMax.params b/test/cfg/MinMax.params new file mode 100755 index 0000000..b418d38 --- /dev/null +++ b/test/cfg/MinMax.params @@ -0,0 +1,21 @@ +#!/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_w_minmax", paramtype="int", description="An Integer parameter", default=3, min=0, max=2) +gen.add("double_param_w_minmax", paramtype="double",description="A double parameter", default=3.1, min=0., max=2.) + +gen.add("vector_int_param_w_minmax", paramtype="std::vector", description="A vector of int parameter", default=[-1,2,3], min=0, max=2) +gen.add("vector_double_param_w_minmax", paramtype="std::vector", description="A vector of double parameter", default=[-1.1, 1.2, 2.3], min=0., max=2.) + +gen.add("map_param_w_minmax", paramtype="std::map", description="A map parameter", default={"value1": -1.2,"value2": 1.2,"value3": 2.2}, min=0., max=2.) + +#Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "MinMax")) \ No newline at end of file diff --git a/test/launch/params/test_launch_parameters.yaml b/test/launch/params/test_launch_parameters.yaml new file mode 100644 index 0000000..f7fe217 --- /dev/null +++ b/test/launch/params/test_launch_parameters.yaml @@ -0,0 +1,22 @@ +# Use this file to specify parameters, that do not have default values. +# YAML Files can also be helpfull if you want to save parameters for different use-cases. +# In any way: Default values should go into the .params file! +# +# Use 'key: value' pairs, e.g. +# string: 'foo' +# integer: 1234 +# float: 1234.5 +# boolean: true +# vector: [1.0, 2.0, 3.4] +# map: {"a": "b", "c": "d"} + +int_param_wo_default: 1 +double_param_wo_default: 1.1 +str_param_wo_default: "Hello World" +bool_param_wo_default: true +vector_int_param_wo_default: [1,2,3] +vector_double_param_wo_default: [1.1, 1.2, 1.3] +vector_bool_param_wo_default: [false, true] +vector_string_param_wo_default: ["Hello","World"] +map_param_wo_default: {"Hello": "World"} +enum_param_wo_default: 1 \ No newline at end of file diff --git a/test/launch/rosparam_handler.test b/test/launch/rosparam_handler.test new file mode 100644 index 0000000..cfecce2 --- /dev/null +++ b/test/launch/rosparam_handler.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/test/src/test_defaults.cpp b/test/src/test_defaults.cpp new file mode 100644 index 0000000..9cc589b --- /dev/null +++ b/test/src/test_defaults.cpp @@ -0,0 +1,26 @@ +#include +#include +#include + +typedef rosparam_handler::DefaultsParameters ParamType; +typedef rosparam_handler::DefaultsConfig ConfigType; + +TEST(RosparamHandler, Defaults) { + ParamType testParams(ros::NodeHandle("~")); + ASSERT_NO_THROW(testParams.fromParamServer()); + + ASSERT_EQ(1, testParams.int_param_w_default); + ASSERT_DOUBLE_EQ(1.1, testParams.double_param_w_default); + ASSERT_EQ("Hello World", testParams.str_param_w_default); + ASSERT_EQ(true, testParams.bool_param_w_default); + + ASSERT_EQ(std::vector({1,2,3}), testParams.vector_int_param_w_default); + ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testParams.vector_double_param_w_default); + ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_w_default); + ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_w_default); + + std::map tmp{{"Hello","World"}}; + ASSERT_EQ(tmp, testParams.map_param_w_default); + + ASSERT_EQ(1, testParams.enum_param_w_default); +} \ No newline at end of file diff --git a/test/src/test_defaultsAtLaunch.cpp b/test/src/test_defaultsAtLaunch.cpp new file mode 100644 index 0000000..bdb9c26 --- /dev/null +++ b/test/src/test_defaultsAtLaunch.cpp @@ -0,0 +1,26 @@ +#include +#include +#include + +typedef rosparam_handler::DefaultsAtLaunchParameters ParamType; +typedef rosparam_handler::DefaultsAtLaunchConfig ConfigType; + +TEST(RosparamHandler, DefaultsAtLaunch) { + ParamType testParams(ros::NodeHandle("~")); + ASSERT_NO_THROW(testParams.fromParamServer()); + + ASSERT_EQ(1, testParams.int_param_wo_default); + ASSERT_DOUBLE_EQ(1.1, testParams.double_param_wo_default); + ASSERT_EQ("Hello World", testParams.str_param_wo_default); + ASSERT_EQ(true, testParams.bool_param_wo_default); + + ASSERT_EQ(std::vector({1,2,3}), testParams.vector_int_param_wo_default); + ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testParams.vector_double_param_wo_default); + ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_wo_default); + ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_wo_default); + + std::map tmp{{"Hello","World"}}; + ASSERT_EQ(tmp, testParams.map_param_wo_default); + + ASSERT_EQ(1, testParams.enum_param_wo_default); +} \ No newline at end of file diff --git a/test/src/test_defaultsMissing.cpp b/test/src/test_defaultsMissing.cpp new file mode 100644 index 0000000..39c5007 --- /dev/null +++ b/test/src/test_defaultsMissing.cpp @@ -0,0 +1,11 @@ +#include +#include +#include + +typedef rosparam_handler::DefaultsMissingParameters ParamType; +typedef rosparam_handler::DefaultsMissingConfig ConfigType; + +TEST(RosparamHandler, DefaultsMissing) { + ParamType testParams(ros::NodeHandle("~")); + ASSERT_THROW(testParams.fromParamServer(), std::runtime_error); +} diff --git a/test/src/test_main.cpp b/test/src/test_main.cpp new file mode 100644 index 0000000..72e1471 --- /dev/null +++ b/test/src/test_main.cpp @@ -0,0 +1,14 @@ +#include +#include + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "rosparam_handler_test"); + // The async spinner lets you publish and receive messages during the tests, + // no need to call spinOnce() + ros::AsyncSpinner spinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + ros::shutdown(); + return ret; +} diff --git a/test/src/test_minMax.cpp b/test/src/test_minMax.cpp new file mode 100644 index 0000000..38e6583 --- /dev/null +++ b/test/src/test_minMax.cpp @@ -0,0 +1,21 @@ +#include +#include +#include + +typedef rosparam_handler::MinMaxParameters ParamType; +typedef rosparam_handler::MinMaxConfig ConfigType; + +TEST(RosparamHandler, MinMax) { + ParamType testParams(ros::NodeHandle("~")); + ASSERT_NO_THROW(testParams.fromParamServer()); + + ASSERT_EQ(2, testParams.int_param_w_minmax); + ASSERT_DOUBLE_EQ(2., testParams.double_param_w_minmax); + + ASSERT_EQ(std::vector({0,2,2}), testParams.vector_int_param_w_minmax); + ASSERT_EQ(std::vector({0., 1.2, 2.}), testParams.vector_double_param_w_minmax); + + std::map tmp{{"value1",0.},{"value2",1.2},{"value3",2.}}; + ASSERT_EQ(tmp, testParams.map_param_w_minmax); + +} \ No newline at end of file From 233b48a3f886f87bbc2173de49f3188393186260 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 12:33:37 +0200 Subject: [PATCH 12/47] Added initial travis config --- .travis.yml | 120 ++++++++++++++++++++++++++++++++++++++++++++++++++++ README.md | 13 ++++++ 2 files changed, 133 insertions(+) create mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..b5d3aeb --- /dev/null +++ b/.travis.yml @@ -0,0 +1,120 @@ +# Generic .travis.yml file for running continuous integration on Travis-CI for +# any ROS package. +# +# Available here: +# - http://felixduvallet.github.io/ros-travis-integration +# - https://github.com/felixduvallet/ros-travis-integration +# +# This installs ROS on a clean Travis-CI virtual machine, creates a ROS +# workspace, resolves all listed dependencies, and sets environment variables +# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are +# no compilation errors), and runs all the tests. If any of the compilation/test +# phases fail, the build is marked as a failure. +# +# We handle two types of package dependencies specified in the package manifest: +# - system dependencies that can be installed using `rosdep`, including other +# ROS packages and system libraries. These dependencies must be known to +# `rosdistro` and get installed using apt-get. +# - package dependencies that must be checked out from source. These are handled by +# `wstool`, and should be listed in a file named dependencies.rosinstall. +# +# There are two variables you may want to change: +# - ROS_DISTRO (default is indigo). Note that packages must be available for +# ubuntu 14.04 trusty. +# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo +# root). This should list all necessary repositories in wstool format (see +# the ros wiki). If the file does not exists then nothing happens. +# +# See the README.md for more information. +# +# Author: Felix Duvallet + +# NOTE: The build lifecycle on Travis.ci is something like this: +# before_install +# install +# before_script +# script +# after_success or after_failure +# after_script +# OPTIONAL before_deploy +# OPTIONAL deploy +# OPTIONAL after_deploy + +################################################################################ + +# Use ubuntu trusty (14.04) with sudo privileges. +dist: trusty +sudo: required +language: + - generic +cache: + - apt + +# Configuration variables. All variables are global now, but this can be used to +# trigger a build matrix for different ROS distributions if desired. +env: + global: + - ROS_DISTRO=jade + - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] + - CI_SOURCE_PATH=$(pwd) + - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall + - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options + - ROS_PARALLEL_JOBS='-j8 -l6' + # Set the python path manually to include /usr/-/python2.7/dist-packages + # as this is where apt-get installs python packages. + - PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages + +################################################################################ + +# Install system dependencies, namely a very barebones ROS setup. +before_install: + - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" + - sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 + - sudo apt-get update -qq + - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin + - source /opt/ros/$ROS_DISTRO/setup.bash + # Prepare rosdep to install dependencies. + - sudo rosdep init + - rosdep update + +# Create a catkin workspace with the package under integration. +install: + - mkdir -p ~/catkin_ws/src + - cd ~/catkin_ws/src + - catkin_init_workspace + # Create the devel/setup.bash (run catkin_make with an empty workspace) and + # source it to set the path variables. + - cd ~/catkin_ws + - catkin_make + - source devel/setup.bash + # Add the package under integration to the workspace using a symlink. + - cd ~/catkin_ws/src + - ln -s $CI_SOURCE_PATH . + +# Install all dependencies, using wstool first and rosdep second. +# wstool looks for a ROSINSTALL_FILE defined in the environment variables. +before_script: + # source dependencies: install using wstool. + - cd ~/catkin_ws/src + - wstool init + - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi + - wstool up + # package depdencies: install using rosdep. + - cd ~/catkin_ws + - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO + +# Compile and test (mark the build as failed if any step fails). If the +# CATKIN_OPTIONS file exists, use it as an argument to catkin_make, for example +# to blacklist certain packages. +# +# NOTE on testing: `catkin_make run_tests` will show the output of the tests +# (gtest, nosetest, etc..) but always returns 0 (success) even if a test +# fails. Running `catkin_test_results` aggregates all the results and returns +# non-zero when a test fails (which notifies Travis the build failed). +script: + - source /opt/ros/$ROS_DISTRO/setup.bash + - cd ~/catkin_ws + - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) + # Run the tests, ensuring the path is set correctly. + - source devel/setup.bash + - catkin_make run_tests && catkin_test_results diff --git a/README.md b/README.md index 8a69364..a1f2cf3 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,6 @@ # rosparam_handler +[![Build Status](https://travis-ci.org/cbandera/rosparam_handler.svg?branch=master)](https://travis-ci.org/cbandera/rosparam_handler) + ## Package Summary A unified parameter handler for nodes with automatic code generation. Save time on defining your parameters. No more redundant code. Easy error checking. Make them dynamic with a single flag. @@ -41,6 +43,17 @@ See the Tutorials on To get the package run `rosdep update && rosdep install rosparam_handler` +## Contribution +`rosparam_handler` is developt according to Vincent Driessen's [Gitflow Workflow](http://nvie.com/posts/a-successful-git-branching-model/). +This means, +- the master branch is for releases only. +- development is done on feature branches +- finished features are integrated via PullRequests into develop. + +For a PullRequest to get merged into develop, it must pass +- Review by one of the maintainers. +- Compile / Test / Run on all target environments. + ## Credits This project uses Open Source components. The code was, in large parts, built upon such existing open source software. You can find the source code of their projects along with license information below. We acknowledge and are grateful to these developers for their contributions to open source. From 1d02859ccdec38ebcfb5a735f12a8742a5942c46 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 12:50:16 +0200 Subject: [PATCH 13/47] Added matrix job and removed C++14 check --- .travis.yml | 5 +++-- CMakeLists.txt | 10 ---------- 2 files changed, 3 insertions(+), 12 deletions(-) diff --git a/.travis.yml b/.travis.yml index b5d3aeb..a3eea25 100644 --- a/.travis.yml +++ b/.travis.yml @@ -54,7 +54,6 @@ cache: # trigger a build matrix for different ROS distributions if desired. env: global: - - ROS_DISTRO=jade - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] - CI_SOURCE_PATH=$(pwd) - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall @@ -63,7 +62,9 @@ env: # Set the python path manually to include /usr/-/python2.7/dist-packages # as this is where apt-get installs python packages. - PYTHONPATH=$PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages - + matrix: + - ROS_DISTRO=indigo + - ROS_DISTRO=jade ################################################################################ # Install system dependencies, namely a very barebones ROS setup. diff --git a/CMakeLists.txt b/CMakeLists.txt index 587a81b..f1f258c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,16 +7,6 @@ catkin_python_setup() if (CATKIN_ENABLE_TESTING) # set compiler flags - include(CheckCXXCompilerFlag) - CHECK_CXX_COMPILER_FLAG("-std=c++14" Cpp14CompilerFlag) - if (${Cpp14CompilerFlag}) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") - elseif(CMAKE_CXX_COMPILER_ID STREQUAL "MSVC" AND NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS "17") - #no additional flag is required - else() - message(FATAL_ERROR "Compiler does not have c++14 support. Use at least g++4.9 or Visual Studio 2013 and newer.") - endif() - find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure rostest roscpp) set(ROSPARAM_HANDLER_CMAKE_DIR ${CMAKE_CURRENT_LIST_DIR}/cmake) include(cmake/rosparam_handler-macros.cmake) From 158b22e1150c6962d4070d425317de2b781d91b6 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 12:59:29 +0200 Subject: [PATCH 14/47] Updated README contribution guide --- README.md | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index a1f2cf3..de5504f 100644 --- a/README.md +++ b/README.md @@ -44,7 +44,7 @@ To get the package run `rosdep update && rosdep install rosparam_handler` ## Contribution -`rosparam_handler` is developt according to Vincent Driessen's [Gitflow Workflow](http://nvie.com/posts/a-successful-git-branching-model/). +`rosparam_handler` is developed according to Vincent Driessen's [Gitflow Workflow](http://nvie.com/posts/a-successful-git-branching-model/). This means, - the master branch is for releases only. - development is done on feature branches @@ -52,6 +52,11 @@ This means, For a PullRequest to get merged into develop, it must pass - Review by one of the maintainers. + + Are the changes introduces in scope of the rosparam_handler? + + Is the documentation updated? + + Are enough reasonable tests added? + + Will these changes break the API? + + Do the new changes follow the current style of naming? - Compile / Test / Run on all target environments. ## Credits From 4bacbbf8447020c6b2f33bc39739bea00cca84b2 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sun, 20 Aug 2017 13:07:28 +0200 Subject: [PATCH 15/47] Changed build flag to display develop branch --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index de5504f..68b9cdd 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # rosparam_handler -[![Build Status](https://travis-ci.org/cbandera/rosparam_handler.svg?branch=master)](https://travis-ci.org/cbandera/rosparam_handler) +[![Build Status](https://travis-ci.org/cbandera/rosparam_handler.svg?branch=develop)](https://travis-ci.org/cbandera/rosparam_handler) ## Package Summary A unified parameter handler for nodes with automatic code generation. From 30eb393288f9f94c707b8bc59793595376f24ddb Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Sun, 27 Aug 2017 20:57:13 +0200 Subject: [PATCH 16/47] Add python unittests --- CMakeLists.txt | 1 + test/launch/rosparam_handler_python.test | 6 ++++ test/python/rosparam_handler_python_test.py | 13 ++++++++ test/python/tests/__init__.py | 22 +++++++++++++ test/python/tests/test_defaults.py | 18 +++++++++++ test/python/tests/test_defaultsAtLaunch.py | 18 +++++++++++ test/python/tests/test_defaultsMissing.py | 8 +++++ test/python/tests/test_minMax.py | 35 +++++++++++++++++++++ 8 files changed, 121 insertions(+) create mode 100644 test/launch/rosparam_handler_python.test create mode 100755 test/python/rosparam_handler_python_test.py create mode 100644 test/python/tests/__init__.py create mode 100644 test/python/tests/test_defaults.py create mode 100644 test/python/tests/test_defaultsAtLaunch.py create mode 100644 test/python/tests/test_defaultsMissing.py create mode 100644 test/python/tests/test_minMax.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f1f258c..3140a5b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,7 @@ if (CATKIN_ENABLE_TESTING) file(GLOB PROJECT_TEST_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/src/*.cpp") set(TEST_TARGET_NAME "rosparam_handler_test") add_rostest_gtest(${TEST_TARGET_NAME} test/launch/rosparam_handler.test ${PROJECT_TEST_FILES_SRC}) + add_rostest(test/launch/rosparam_handler_python.test) target_link_libraries(${TEST_TARGET_NAME} ${catkin_LIBRARIES} gtest) target_include_directories(${TEST_TARGET_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) add_dependencies(${TEST_TARGET_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) diff --git a/test/launch/rosparam_handler_python.test b/test/launch/rosparam_handler_python.test new file mode 100644 index 0000000..7c741d5 --- /dev/null +++ b/test/launch/rosparam_handler_python.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/test/python/rosparam_handler_python_test.py b/test/python/rosparam_handler_python_test.py new file mode 100755 index 0000000..3f9ae8b --- /dev/null +++ b/test/python/rosparam_handler_python_test.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +PKG = 'rosparam_handler' +import unittest +import rospy +import tests + +if __name__ == '__main__': + import rostest + unittests = tests.get_unittest_classes() + rospy.init_node(PKG) + for test_name, test in unittests.items(): + rostest.rosrun(PKG, test_name, test) diff --git a/test/python/tests/__init__.py b/test/python/tests/__init__.py new file mode 100644 index 0000000..4a76894 --- /dev/null +++ b/test/python/tests/__init__.py @@ -0,0 +1,22 @@ +from os.path import dirname, basename, isfile +import glob +# automatically import all files in this module +modules = glob.glob(dirname(__file__)+"/*.py") +__all__ = [ basename(f)[:-3] for f in modules if isfile(f) and not f.endswith('__init__.py')] + + +# determine the test classes +def get_unittest_classes(): + """ + Determines the unittests to be passed to rostest + :return: + """ + import unittest + unittests = {} + for test_module in __all__: + module = __import__("tests."+test_module, fromlist=['*']) + for name in dir(module): + obj = getattr(module, name) + if isinstance(obj, type) and issubclass(obj, unittest.TestCase): + unittests[name] = obj + return unittests diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py new file mode 100644 index 0000000..b499507 --- /dev/null +++ b/test/python/tests/test_defaults.py @@ -0,0 +1,18 @@ +import unittest +from rosparam_handler.param.DefaultsParameters import DefaultsParameters + +class TestDefaults(unittest.TestCase): + def test_defaults(self): + params = DefaultsParameters() + self.assertEqual(params.int_param_w_default, 1) + self.assertAlmostEqual(params.double_param_w_default, 1.1) + self.assertEqual(params.str_param_w_default, "Hello World") + self.assertEqual(params.bool_param_w_default, True) + + self.assertEqual(params.vector_int_param_w_default, [1, 2, 3]) + self.assertEqual(params.vector_double_param_w_default, [1.1, 1.2, 1.3]) + self.assertEqual(params.vector_string_param_w_default, ["Hello", "World"]) + + self.assertEqual(params.map_param_w_default, {"Hello": "World"}) + self.assertEqual(params.enum_param_w_default, 1) + diff --git a/test/python/tests/test_defaultsAtLaunch.py b/test/python/tests/test_defaultsAtLaunch.py new file mode 100644 index 0000000..313a52a --- /dev/null +++ b/test/python/tests/test_defaultsAtLaunch.py @@ -0,0 +1,18 @@ +import unittest +from rosparam_handler.param.DefaultsAtLaunchParameters import DefaultsAtLaunchParameters + + +class TestDefaultsAtLaunch(unittest.TestCase): + def test_defaults(self): + params = DefaultsAtLaunchParameters() + self.assertEqual(params.int_param_wo_default, 1) + self.assertAlmostEqual(params.double_param_wo_default, 1.1) + self.assertEqual(params.str_param_wo_default, "Hello World") + self.assertEqual(params.bool_param_wo_default, True) + + self.assertEqual(params.vector_int_param_wo_default, [1, 2, 3]) + self.assertEqual(params.vector_double_param_wo_default, [1.1, 1.2, 1.3]) + self.assertEqual(params.vector_string_param_wo_default, ["Hello", "World"]) + + self.assertEqual(params.map_param_wo_default, {"Hello": "World"}) + self.assertEqual(params.enum_param_wo_default, 1) diff --git a/test/python/tests/test_defaultsMissing.py b/test/python/tests/test_defaultsMissing.py new file mode 100644 index 0000000..b905ffd --- /dev/null +++ b/test/python/tests/test_defaultsMissing.py @@ -0,0 +1,8 @@ +import unittest +from rosparam_handler.param.DefaultsMissingParameters import DefaultsMissingParameters + + +class TestDefaultsMissingParameters(unittest.TestCase): + def test_defaults(self): + with self.assertRaises(TypeError): + params = DefaultsMissingParameters() diff --git a/test/python/tests/test_minMax.py b/test/python/tests/test_minMax.py new file mode 100644 index 0000000..ac2b999 --- /dev/null +++ b/test/python/tests/test_minMax.py @@ -0,0 +1,35 @@ +#include +#include +#include +# +# typedef rosparam_handler::MinMaxParameters ParamType; +# typedef rosparam_handler::MinMaxConfig ConfigType; +# +# TEST(RosparamHandler, MinMax) { +# ParamType testParams(ros::NodeHandle("~")); +# ASSERT_NO_THROW(testParams.fromParamServer()); +# +# ASSERT_EQ(2, testParams.int_param_w_minmax); +# ASSERT_DOUBLE_EQ(2., testParams.double_param_w_minmax); +# +# ASSERT_EQ(std::vector({0,2,2}), testParams.vector_int_param_w_minmax); +# ASSERT_EQ(std::vector({0., 1.2, 2.}), testParams.vector_double_param_w_minmax); +# +# std::map tmp{{"value1",0.},{"value2",1.2},{"value3",2.}}; +# ASSERT_EQ(tmp, testParams.map_param_w_minmax); +# +# } +import unittest +from rosparam_handler.param.MinMaxParameters import MinMaxParameters + + +class TestMinMaxParameters(unittest.TestCase): + def test_min_max(self): + params = MinMaxParameters() + self.assertEqual(params.int_param_w_minmax, 2) + self.assertAlmostEqual(params.double_param_w_minmax, 2.) + + self.assertEqual(params.vector_int_param_w_minmax, [0, 2, 2]) + self.assertEqual(params.vector_double_param_w_minmax, [0, 1.2, 2.]) + + self.assertEqual(params.map_param_w_minmax, {"value1": 0., "value2": 1.2, "value3": 2.}) From f0be113867d741a858ba532896c635ce4472b2c2 Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Sun, 27 Aug 2017 21:23:34 +0200 Subject: [PATCH 17/47] Throw more informative exception if parameters without default value are unset --- templates/Parameters.py.template | 3 +++ test/python/tests/test_defaultsMissing.py | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/templates/Parameters.py.template b/templates/Parameters.py.template index bdc72f0..492a37b 100644 --- a/templates/Parameters.py.template +++ b/templates/Parameters.py.template @@ -73,6 +73,9 @@ class ${ClassName}Parameters(dict): full_name = "/" + param_name if config['global_scope'] else "~" + param_name val = rospy.get_param(full_name, config['default']) + if val is None: + raise KeyError("Parameter {} is neither set on the parameter server nor " + "has it a default value".format(param_description)) # test whether type is correct try: if config['is_vector']: diff --git a/test/python/tests/test_defaultsMissing.py b/test/python/tests/test_defaultsMissing.py index b905ffd..5783e86 100644 --- a/test/python/tests/test_defaultsMissing.py +++ b/test/python/tests/test_defaultsMissing.py @@ -4,5 +4,5 @@ class TestDefaultsMissingParameters(unittest.TestCase): def test_defaults(self): - with self.assertRaises(TypeError): + with self.assertRaises(KeyError): params = DefaultsMissingParameters() From 2cf0e8b04bccadb4eeeaa39525e0bb844f362613 Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Sun, 27 Aug 2017 22:50:54 +0200 Subject: [PATCH 18/47] Name tests properly, add cmake dependency --- CMakeLists.txt | 2 +- src/rosparam_handler/parameter_generator_catkin.py | 1 - test/python/tests/test_defaultsAtLaunch.py | 2 +- test/python/tests/test_defaultsMissing.py | 2 +- 4 files changed, 3 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3140a5b..4d625bc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,7 +39,7 @@ if (CATKIN_ENABLE_TESTING) file(GLOB PROJECT_TEST_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/src/*.cpp") set(TEST_TARGET_NAME "rosparam_handler_test") add_rostest_gtest(${TEST_TARGET_NAME} test/launch/rosparam_handler.test ${PROJECT_TEST_FILES_SRC}) - add_rostest(test/launch/rosparam_handler_python.test) + add_rostest(test/launch/rosparam_handler_python.test DEPENDENCIES ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(${TEST_TARGET_NAME} ${catkin_LIBRARIES} gtest) target_include_directories(${TEST_TARGET_NAME} PUBLIC ${catkin_INCLUDE_DIRS} include) add_dependencies(${TEST_TARGET_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index ce35779..18da90b 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -346,7 +346,6 @@ def _generatecfg(self): template = f.read() param_entries = self._generate_param_entries() - print(param_entries) param_entries = "\n".join(param_entries) template = Template(template).substitute(pkgname=self.pkgname, nodename=self.nodename, diff --git a/test/python/tests/test_defaultsAtLaunch.py b/test/python/tests/test_defaultsAtLaunch.py index 313a52a..0d15b4f 100644 --- a/test/python/tests/test_defaultsAtLaunch.py +++ b/test/python/tests/test_defaultsAtLaunch.py @@ -3,7 +3,7 @@ class TestDefaultsAtLaunch(unittest.TestCase): - def test_defaults(self): + def test_defaults_at_launch(self): params = DefaultsAtLaunchParameters() self.assertEqual(params.int_param_wo_default, 1) self.assertAlmostEqual(params.double_param_wo_default, 1.1) diff --git a/test/python/tests/test_defaultsMissing.py b/test/python/tests/test_defaultsMissing.py index 5783e86..3f96ba6 100644 --- a/test/python/tests/test_defaultsMissing.py +++ b/test/python/tests/test_defaultsMissing.py @@ -3,6 +3,6 @@ class TestDefaultsMissingParameters(unittest.TestCase): - def test_defaults(self): + def test_defaults_missing_params(self): with self.assertRaises(KeyError): params = DefaultsMissingParameters() From 86796e235ce692f3c2371c390e77528609d7d715 Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Mon, 28 Aug 2017 18:11:05 +0200 Subject: [PATCH 19/47] Add unittests --- test/src/test_defaults.cpp | 62 +++++++++++++++++++++++++++++- test/src/test_defaultsAtLaunch.cpp | 4 +- 2 files changed, 62 insertions(+), 4 deletions(-) diff --git a/test/src/test_defaults.cpp b/test/src/test_defaults.cpp index 9cc589b..fe33377 100644 --- a/test/src/test_defaults.cpp +++ b/test/src/test_defaults.cpp @@ -19,8 +19,66 @@ TEST(RosparamHandler, Defaults) { ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_w_default); ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_w_default); - std::map tmp{{"Hello","World"}}; + std::map tmp{{"Hello","World"}}; ASSERT_EQ(tmp, testParams.map_param_w_default); ASSERT_EQ(1, testParams.enum_param_w_default); -} \ No newline at end of file +} + +TEST(RosparamHandler, DefaultsOnParamServer) { + ros::NodeHandle nh("~"); + ParamType testParams(nh); + ASSERT_NO_THROW(testParams.fromParamServer()); + + // values should now be set on param server + { + int int_param; + ASSERT_TRUE(nh.getParam("int_param_w_default", int_param)); + ASSERT_EQ(int_param, testParams.int_param_w_default); + } + { + double double_param; + ASSERT_TRUE(nh.getParam("double_param_w_default", double_param)); + EXPECT_EQ(double_param, testParams.double_param_w_default); + } + { + bool bool_param; + ASSERT_TRUE(nh.getParam("bool_param_w_default", bool_param)); + EXPECT_EQ(bool_param, testParams.bool_param_w_default); + } + { + bool bool_param; + ASSERT_TRUE(nh.getParam("bool_param_w_default", bool_param)); + EXPECT_EQ(bool_param, testParams.bool_param_w_default); + } + { + std::vector vector_int_param; + ASSERT_TRUE(nh.getParam("vector_int_param_w_default", vector_int_param)); + EXPECT_EQ(vector_int_param, testParams.vector_int_param_w_default); + } + { + std::vector vector_double_param; + ASSERT_TRUE(nh.getParam("vector_double_param_w_default", vector_double_param)); + EXPECT_EQ(vector_double_param, testParams.vector_double_param_w_default); + } + { + std::vector vector_bool_param; + ASSERT_TRUE(nh.getParam("vector_bool_param_w_default", vector_bool_param)); + EXPECT_EQ(vector_bool_param, testParams.vector_bool_param_w_default); + } + { + std::vector vector_string_param; + ASSERT_TRUE(nh.getParam("vector_string_param_w_default", vector_string_param)); + EXPECT_EQ(vector_string_param, testParams.vector_string_param_w_default); + } + { + std::map map_param_w_default; + ASSERT_TRUE(nh.getParam("map_param_w_default", map_param_w_default)); + EXPECT_EQ(map_param_w_default, testParams.map_param_w_default); + } + { + int enum_param; + ASSERT_TRUE(nh.getParam("enum_param_w_default", enum_param)); + EXPECT_EQ(enum_param, testParams.enum_param_w_default); + } +} diff --git a/test/src/test_defaultsAtLaunch.cpp b/test/src/test_defaultsAtLaunch.cpp index bdb9c26..3a87d1d 100644 --- a/test/src/test_defaultsAtLaunch.cpp +++ b/test/src/test_defaultsAtLaunch.cpp @@ -19,8 +19,8 @@ TEST(RosparamHandler, DefaultsAtLaunch) { ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_wo_default); ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_wo_default); - std::map tmp{{"Hello","World"}}; + std::map tmp{{"Hello","World"}}; ASSERT_EQ(tmp, testParams.map_param_wo_default); ASSERT_EQ(1, testParams.enum_param_wo_default); -} \ No newline at end of file +} From 83d352372dbdc899f1b3d834c39f50b071f06dce Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Mon, 28 Aug 2017 18:27:23 +0200 Subject: [PATCH 20/47] Remove duplicate test --- test/src/test_defaults.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/test/src/test_defaults.cpp b/test/src/test_defaults.cpp index fe33377..3364967 100644 --- a/test/src/test_defaults.cpp +++ b/test/src/test_defaults.cpp @@ -46,11 +46,6 @@ TEST(RosparamHandler, DefaultsOnParamServer) { ASSERT_TRUE(nh.getParam("bool_param_w_default", bool_param)); EXPECT_EQ(bool_param, testParams.bool_param_w_default); } - { - bool bool_param; - ASSERT_TRUE(nh.getParam("bool_param_w_default", bool_param)); - EXPECT_EQ(bool_param, testParams.bool_param_w_default); - } { std::vector vector_int_param; ASSERT_TRUE(nh.getParam("vector_int_param_w_default", vector_int_param)); From 07d52f8fa46a7f603dab57d991e41117ecb87d70 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Tue, 29 Aug 2017 20:16:10 +0200 Subject: [PATCH 21/47] Updated the documentation --- doc/HowToUseYourParameterStruct.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/HowToUseYourParameterStruct.md b/doc/HowToUseYourParameterStruct.md index 664d51b..5c18891 100644 --- a/doc/HowToUseYourParameterStruct.md +++ b/doc/HowToUseYourParameterStruct.md @@ -25,7 +25,7 @@ rosparam_tutorials::TutorialParameters params_; ## Initializing the struct. When initializing your node, the params struct must be initialized with a private NodeHandle. -The call to `fromParamServer()` 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. When min and max values are specified, these bounds will be checked as well. +The call to `fromParamServer()` 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() From 8cf6cca206e662806e62a0fcb0b269e2152657a8 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Tue, 29 Aug 2017 19:44:58 +0000 Subject: [PATCH 22/47] Fixed merge error in template --- templates/Parameters.h.template | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index 4396dca..c4c5747 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -30,7 +30,7 @@ struct ${ClassName}Parameters { ${ClassName}Parameters(const ros::NodeHandle& private_node_handle) : globalNamespace{"/"}, privateNamespace{private_node_handle.getNamespace() + "/"}, - nodeName{getNodeName(private_node_handle)} {} + nodeName{rosparam_handler::getNodeName(private_node_handle)} {} void fromParamServer(){ $fromParamServer From 8321c3d56b8006c4ace006c91f1dcf60fab94d59 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Tue, 29 Aug 2017 20:16:48 +0000 Subject: [PATCH 23/47] Removed verbose output during build --- src/rosparam_handler/parameter_generator_catkin.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 787c0f9..3980669 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -344,8 +344,7 @@ def _generatecfg(self): template = f.read() param_entries = self._generate_param_entries() - print(param_entries) - + param_entries = "\n".join(param_entries) template = Template(template).substitute(pkgname=self.pkgname, nodename=self.nodename, classname=self.classname, params=param_entries) From b75fae03221b8bd6657e49dee2db1d4cb0e04038 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Tue, 29 Aug 2017 22:33:30 +0200 Subject: [PATCH 24/47] Force travis to do a clean build --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index a3eea25..ac856f0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -115,6 +115,7 @@ before_script: script: - source /opt/ros/$ROS_DISTRO/setup.bash - cd ~/catkin_ws + - rm -rf build/ devel/ - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) # Run the tests, ensuring the path is set correctly. - source devel/setup.bash From 68c219e822b2f32dc4a84e28c53f44417329c22e Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Tue, 29 Aug 2017 22:45:29 +0200 Subject: [PATCH 25/47] Readded check for C++11 support. --- CMakeLists.txt | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f1f258c..fa64362 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,6 +36,14 @@ install( ) if (CATKIN_ENABLE_TESTING) + include(CheckCXXCompilerFlag) + CHECK_CXX_COMPILER_FLAG("-std=c++11" Cpp11CompilerFlag) + if (${Cpp11CompilerFlag}) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + else() + message(FATAL_ERROR "Compiler does not have c++11 support.") + endif() + file(GLOB PROJECT_TEST_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/src/*.cpp") set(TEST_TARGET_NAME "rosparam_handler_test") add_rostest_gtest(${TEST_TARGET_NAME} test/launch/rosparam_handler.test ${PROJECT_TEST_FILES_SRC}) From cc2703d95b5f504080bd3bd40ba8aa6af316ee1a Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Wed, 30 Aug 2017 15:56:06 +0200 Subject: [PATCH 26/47] Remove useless comment leftovers --- test/python/tests/test_minMax.py | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/test/python/tests/test_minMax.py b/test/python/tests/test_minMax.py index ac2b999..29b2244 100644 --- a/test/python/tests/test_minMax.py +++ b/test/python/tests/test_minMax.py @@ -1,24 +1,3 @@ -#include -#include -#include -# -# typedef rosparam_handler::MinMaxParameters ParamType; -# typedef rosparam_handler::MinMaxConfig ConfigType; -# -# TEST(RosparamHandler, MinMax) { -# ParamType testParams(ros::NodeHandle("~")); -# ASSERT_NO_THROW(testParams.fromParamServer()); -# -# ASSERT_EQ(2, testParams.int_param_w_minmax); -# ASSERT_DOUBLE_EQ(2., testParams.double_param_w_minmax); -# -# ASSERT_EQ(std::vector({0,2,2}), testParams.vector_int_param_w_minmax); -# ASSERT_EQ(std::vector({0., 1.2, 2.}), testParams.vector_double_param_w_minmax); -# -# std::map tmp{{"value1",0.},{"value2",1.2},{"value3",2.}}; -# ASSERT_EQ(tmp, testParams.map_param_w_minmax); -# -# } import unittest from rosparam_handler.param.MinMaxParameters import MinMaxParameters From 37c88d90294b59656cd1f9c8ee16046733bd2570 Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Wed, 30 Aug 2017 18:03:10 +0200 Subject: [PATCH 27/47] Set default values on param server, add python tests for that --- templates/Parameters.py.template | 14 ++++++++++---- test/python/tests/test_defaults.py | 16 ++++++++++++++++ 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/templates/Parameters.py.template b/templates/Parameters.py.template index 492a37b..5ac199a 100644 --- a/templates/Parameters.py.template +++ b/templates/Parameters.py.template @@ -72,10 +72,16 @@ class ${ClassName}Parameters(dict): raise ValueError() full_name = "/" + param_name if config['global_scope'] else "~" + param_name - val = rospy.get_param(full_name, config['default']) - if val is None: - raise KeyError("Parameter {} is neither set on the parameter server nor " - "has it a default value".format(param_description)) + try: + val = rospy.get_param(full_name) + except KeyError: + if config['default'] is None: + raise KeyError("Parameter {} is neither set on the parameter server nor " + "has it a default value".format(param_description)) + rospy.loginfo("Parameter {} is not yet set. Setting default value".format(param_name)) + rospy.set_param(full_name, config['default']) + val = config['default'] + # test whether type is correct try: if config['is_vector']: diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py index b499507..e6df69d 100644 --- a/test/python/tests/test_defaults.py +++ b/test/python/tests/test_defaults.py @@ -1,5 +1,7 @@ import unittest from rosparam_handler.param.DefaultsParameters import DefaultsParameters +import rospy + class TestDefaults(unittest.TestCase): def test_defaults(self): @@ -16,3 +18,17 @@ def test_defaults(self): self.assertEqual(params.map_param_w_default, {"Hello": "World"}) self.assertEqual(params.enum_param_w_default, 1) + def test_set_defaults_on_param_server(self): + params = DefaultsParameters() + # now all parameters should be set on param server + self.assertEqual(params.int_param_w_default, rospy.get_param("~int_param_w_default")) + self.assertAlmostEqual(params.double_param_w_default, rospy.get_param("~double_param_w_default")) + self.assertEqual(params.str_param_w_default, rospy.get_param("~str_param_w_default")) + self.assertEqual(params.bool_param_w_default, rospy.get_param("~bool_param_w_default")) + + self.assertEqual(params.vector_int_param_w_default, rospy.get_param("~vector_int_param_w_default")) + self.assertEqual(params.vector_double_param_w_default, rospy.get_param("~vector_double_param_w_default")) + self.assertEqual(params.vector_string_param_w_default, rospy.get_param("~vector_string_param_w_default")) + + self.assertEqual(params.map_param_w_default, rospy.get_param("~map_param_w_default")) + self.assertEqual(params.enum_param_w_default, rospy.get_param("~enum_param_w_default")) From 07c526a33b44441cc0767f926c289c1f2c699e37 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Thu, 31 Aug 2017 08:19:20 +0200 Subject: [PATCH 28/47] Made tests visible Replaced individual testcases with TestSuite --- test/python/rosparam_handler_python_test.py | 9 +++--- test/python/tests/__init__.py | 32 +++++++++++---------- test/python/tests/test_defaults.py | 6 ++-- test/python/tests/test_defaultsAtLaunch.py | 2 +- test/python/tests/test_defaultsMissing.py | 2 +- test/python/tests/test_minMax.py | 2 +- 6 files changed, 28 insertions(+), 25 deletions(-) diff --git a/test/python/rosparam_handler_python_test.py b/test/python/rosparam_handler_python_test.py index 3f9ae8b..c42e6f8 100755 --- a/test/python/rosparam_handler_python_test.py +++ b/test/python/rosparam_handler_python_test.py @@ -6,8 +6,7 @@ import tests if __name__ == '__main__': - import rostest - unittests = tests.get_unittest_classes() - rospy.init_node(PKG) - for test_name, test in unittests.items(): - rostest.rosrun(PKG, test_name, test) + import rostest + + rospy.init_node(PKG) + rostest.rosrun(PKG, "RosparamTestSuite", "tests.RosparamTestSuite") diff --git a/test/python/tests/__init__.py b/test/python/tests/__init__.py index 4a76894..47d6911 100644 --- a/test/python/tests/__init__.py +++ b/test/python/tests/__init__.py @@ -1,22 +1,24 @@ from os.path import dirname, basename, isfile import glob +import unittest # automatically import all files in this module modules = glob.glob(dirname(__file__)+"/*.py") __all__ = [ basename(f)[:-3] for f in modules if isfile(f) and not f.endswith('__init__.py')] +class RosparamTestSuite(unittest.TestSuite): -# determine the test classes -def get_unittest_classes(): - """ - Determines the unittests to be passed to rostest - :return: - """ - import unittest - unittests = {} - for test_module in __all__: - module = __import__("tests."+test_module, fromlist=['*']) - for name in dir(module): - obj = getattr(module, name) - if isinstance(obj, type) and issubclass(obj, unittest.TestCase): - unittests[name] = obj - return unittests + def __init__(self): + super(RosparamTestSuite, self).__init__() + + # Collect test cases + testcases = {} + for test_module in __all__: + module = __import__("tests."+test_module, fromlist=['*']) + for name in dir(module): + obj = getattr(module, name) + if isinstance(obj, type) and issubclass(obj, unittest.TestCase): + testcases[name] = obj + + # Add testcases + for test_name, test in testcases.items(): + self.addTest(test()) diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py index e6df69d..a35faaf 100644 --- a/test/python/tests/test_defaults.py +++ b/test/python/tests/test_defaults.py @@ -4,7 +4,7 @@ class TestDefaults(unittest.TestCase): - def test_defaults(self): + def runTest(self): params = DefaultsParameters() self.assertEqual(params.int_param_w_default, 1) self.assertAlmostEqual(params.double_param_w_default, 1.1) @@ -18,7 +18,9 @@ def test_defaults(self): self.assertEqual(params.map_param_w_default, {"Hello": "World"}) self.assertEqual(params.enum_param_w_default, 1) - def test_set_defaults_on_param_server(self): + +class TestDefaultsOnServer(unittest.TestCase): + def runTest(self): params = DefaultsParameters() # now all parameters should be set on param server self.assertEqual(params.int_param_w_default, rospy.get_param("~int_param_w_default")) diff --git a/test/python/tests/test_defaultsAtLaunch.py b/test/python/tests/test_defaultsAtLaunch.py index 0d15b4f..001b260 100644 --- a/test/python/tests/test_defaultsAtLaunch.py +++ b/test/python/tests/test_defaultsAtLaunch.py @@ -3,7 +3,7 @@ class TestDefaultsAtLaunch(unittest.TestCase): - def test_defaults_at_launch(self): + def runTest(self): params = DefaultsAtLaunchParameters() self.assertEqual(params.int_param_wo_default, 1) self.assertAlmostEqual(params.double_param_wo_default, 1.1) diff --git a/test/python/tests/test_defaultsMissing.py b/test/python/tests/test_defaultsMissing.py index 3f96ba6..fb090c9 100644 --- a/test/python/tests/test_defaultsMissing.py +++ b/test/python/tests/test_defaultsMissing.py @@ -3,6 +3,6 @@ class TestDefaultsMissingParameters(unittest.TestCase): - def test_defaults_missing_params(self): + def runTest(self): with self.assertRaises(KeyError): params = DefaultsMissingParameters() diff --git a/test/python/tests/test_minMax.py b/test/python/tests/test_minMax.py index 29b2244..d4244e1 100644 --- a/test/python/tests/test_minMax.py +++ b/test/python/tests/test_minMax.py @@ -3,7 +3,7 @@ class TestMinMaxParameters(unittest.TestCase): - def test_min_max(self): + def runTest(self): params = MinMaxParameters() self.assertEqual(params.int_param_w_minmax, 2) self.assertAlmostEqual(params.double_param_w_minmax, 2.) From 60642d8d07ebb1dec0255a439c58b09f1608b909 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Thu, 31 Aug 2017 08:20:10 +0200 Subject: [PATCH 29/47] Replaced errors with warnings For python, when min/max is corrected. The behaviour is now similar to cpp --- templates/Parameters.py.template | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/templates/Parameters.py.template b/templates/Parameters.py.template index 5ac199a..74ac7a3 100644 --- a/templates/Parameters.py.template +++ b/templates/Parameters.py.template @@ -105,18 +105,18 @@ class ${ClassName}Parameters(dict): if config['min'] is not None: if config['is_vector']: if min(val) < config['min']: - rospy.logerr( + rospy.logwarn( "Some values in {} for {} are smaller than minimal allowed value. " "Correcting them to min={}".format(val, param_name, config['min'])) val = [ v if v > config['min'] else config['min'] for v in val ] elif config['is_map']: if min(val.values()) < config['min']: - rospy.logerr( + rospy.logwarn( "Some values in {} for {} are smaller than minimal allowed value. " "Correcting them to min={}".format(val, param_name, config['min'])) val = { k: (v if v > config['min'] else config['min']) for k, v in val.items() } elif val < config['min']: - rospy.logerr( + rospy.logwarn( "Value of {} for {} is smaller than minimal allowed value. " "Correcting value to min={}".format(val, param_name, config['min'])) val = config['min'] @@ -124,18 +124,18 @@ class ${ClassName}Parameters(dict): if config['max'] is not None: if config['is_vector']: if max(val) > config['max']: - rospy.logerr( + rospy.logwarn( "Some values in {} for {} are greater than maximal allowed value. " "Correcting them to max={}".format(val, param_name, config['max'])) val = [ v if v < config['max'] else config['max'] for v in val ] elif config['is_map']: if max(val.values()) > config['max']: - rospy.logerr( + rospy.logwarn( "Some values in {} for {} are greater than maximal allowed value. " "Correcting them to max={}".format(val, param_name, config['max'])) val = { k: (v if v < config['max'] else config['max']) for k, v in val.items() } elif val > config['max']: - rospy.logerr( + rospy.logwarn( "Value of {} for {} is greater than maximal allowed value. " "Correcting value to max={}".format(val, param_name, config['max'])) val = config['max'] From f6d3fec063a2c0606934b38675a13d8b94bc404c Mon Sep 17 00:00:00 2001 From: Fabian Poggenhans Date: Thu, 31 Aug 2017 23:55:38 +0200 Subject: [PATCH 30/47] Display correct test names for each test --- test/python/tests/__init__.py | 4 ++-- test/python/tests/test_defaults.py | 10 ++++++---- test/python/tests/test_defaultsAtLaunch.py | 2 +- test/python/tests/test_defaultsMissing.py | 2 +- test/python/tests/test_minMax.py | 2 +- 5 files changed, 11 insertions(+), 9 deletions(-) diff --git a/test/python/tests/__init__.py b/test/python/tests/__init__.py index 47d6911..59577b6 100644 --- a/test/python/tests/__init__.py +++ b/test/python/tests/__init__.py @@ -17,8 +17,8 @@ def __init__(self): for name in dir(module): obj = getattr(module, name) if isinstance(obj, type) and issubclass(obj, unittest.TestCase): - testcases[name] = obj + testcases[name] = unittest.TestLoader().loadTestsFromTestCase(obj) # Add testcases for test_name, test in testcases.items(): - self.addTest(test()) + self.addTest(test) diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py index a35faaf..3500ddb 100644 --- a/test/python/tests/test_defaults.py +++ b/test/python/tests/test_defaults.py @@ -4,7 +4,11 @@ class TestDefaults(unittest.TestCase): - def runTest(self): + def test_defaults(self): + """ + tests defaults on server + :return: + """ params = DefaultsParameters() self.assertEqual(params.int_param_w_default, 1) self.assertAlmostEqual(params.double_param_w_default, 1.1) @@ -18,9 +22,7 @@ def runTest(self): self.assertEqual(params.map_param_w_default, {"Hello": "World"}) self.assertEqual(params.enum_param_w_default, 1) - -class TestDefaultsOnServer(unittest.TestCase): - def runTest(self): + def test_defaults_on_server(self): params = DefaultsParameters() # now all parameters should be set on param server self.assertEqual(params.int_param_w_default, rospy.get_param("~int_param_w_default")) diff --git a/test/python/tests/test_defaultsAtLaunch.py b/test/python/tests/test_defaultsAtLaunch.py index 001b260..0d15b4f 100644 --- a/test/python/tests/test_defaultsAtLaunch.py +++ b/test/python/tests/test_defaultsAtLaunch.py @@ -3,7 +3,7 @@ class TestDefaultsAtLaunch(unittest.TestCase): - def runTest(self): + def test_defaults_at_launch(self): params = DefaultsAtLaunchParameters() self.assertEqual(params.int_param_wo_default, 1) self.assertAlmostEqual(params.double_param_wo_default, 1.1) diff --git a/test/python/tests/test_defaultsMissing.py b/test/python/tests/test_defaultsMissing.py index fb090c9..60ce714 100644 --- a/test/python/tests/test_defaultsMissing.py +++ b/test/python/tests/test_defaultsMissing.py @@ -3,6 +3,6 @@ class TestDefaultsMissingParameters(unittest.TestCase): - def runTest(self): + def test_defaults_missing(self): with self.assertRaises(KeyError): params = DefaultsMissingParameters() diff --git a/test/python/tests/test_minMax.py b/test/python/tests/test_minMax.py index d4244e1..bd65ed9 100644 --- a/test/python/tests/test_minMax.py +++ b/test/python/tests/test_minMax.py @@ -3,7 +3,7 @@ class TestMinMaxParameters(unittest.TestCase): - def runTest(self): + def test_min_max_parameters(self): params = MinMaxParameters() self.assertEqual(params.int_param_w_minmax, 2) self.assertAlmostEqual(params.double_param_w_minmax, 2.) From 718d6179de0ddd7b09786d2927c7ecb1c05820ff Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Mon, 11 Sep 2017 22:00:46 +0200 Subject: [PATCH 31/47] First draft implementation of yaml generator for #30 --- CMakeLists.txt | 4 ++ scripts/generate_yaml | 21 +++++++++ .../parameter_generator_catkin.py | 47 +++++++++++++++++++ 3 files changed, 72 insertions(+) create mode 100755 scripts/generate_yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a98db2..ae134b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,10 @@ install( DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) +install(PROGRAMS scripts/generate_yaml + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + if (CATKIN_ENABLE_TESTING) include(CheckCXXCompilerFlag) CHECK_CXX_COMPILER_FLAG("-std=c++11" Cpp11CompilerFlag) diff --git a/scripts/generate_yaml b/scripts/generate_yaml new file mode 100755 index 0000000..2d6106c --- /dev/null +++ b/scripts/generate_yaml @@ -0,0 +1,21 @@ +#!/usr/bin/env python +import sys +import os +from rosparam_handler.parameter_generator_catkin import YamlGenerator + +if len(sys.argv) < 2: + sys.exit('Usage: %s <.params file>' % sys.argv[0]) + +if not os.path.exists(sys.argv[1]): + sys.exit('ERROR: Param file %s was not found!' % sys.argv[1]) + +# Read argument +inFile = sys.argv[1] # "/home/claudio/Documents/repos/GitHub/rosparam_ws/src/rosparam_handler/test/cfg/Defaults.params" +# Mock call arguments for call to ParamGenerator +sys.argv[1:4] = ["", "", "", ""] +# Redirect import statement +sys.modules['rosparam_handler.parameter_generator_catkin'] = __import__('sys') + +# Execute param file in mocked environment +globalVars = {"__file__": inFile, 'ParameterGenerator': YamlGenerator} +execfile(inFile, globalVars) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 18da90b..73ea076 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -329,6 +329,13 @@ def generate(self, pkgname, nodename, classname): if self.parent: eprint("You should not call generate on a group! Call it on the main parameter generator instead!") + return self._generateImpl() + + def _generateImpl(self): + """ + Implementation level function. Can be overwritten by derived classes. + :return: + """ self._generatecfg() self._generatecpp() self._generatepy() @@ -487,6 +494,39 @@ def _generatepy(self): with open(py_file, 'w') as f: f.write(content) + def _generateyml(self): + """ + Generate .yaml file for roslaunch + :param self: + :return: + """ + params = self._get_parameters() + paramDescription = str(params) + + content = "### This file was generated using the rosparam_handler generate_yaml script.\n" + + for entry in params: + if not entry["constant"]: + content += "\n" + content += "# Name:\t" + str(entry["name"]) + "\n" + content += "# Desc:\t" + str(entry["description"]) + "\n" + content += "# Type:\t" + str(entry["type"]) + "\n" + if entry['min'] or entry['max']: + content += "# [min,max]:\t[" + str(entry["min"]) + "/" + str(entry["max"]) + "]" + "\n" + if entry["global_scope"]: + content += "# Lives in global namespace!\n" + if entry["default"] is not None: + content += str(entry["name"]) + " = " + str(entry["default"]) + "\n" + else: + content += "# " + str(entry["name"]) + " = \n" + + from pprint import pprint + pprint(params) + yaml_file = os.path.join(os.getcwd(), self.classname + "Parameters.yaml") + + with open(yaml_file, 'w') as f: + f.write(content) + def _get_parameters(self): """ Returns parameter of this and all childs @@ -552,3 +592,10 @@ def _make_bool(param): else: # Pray and hope that it is a string return bool(param) + + +# Create derived class for yaml generation +class YamlGenerator(ParameterGenerator): + def _generateImpl(self): + self._generateyml() + return 0 From cba741b077408220269303e6af2d9164427b6b8f Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Tue, 12 Sep 2017 21:23:24 +0200 Subject: [PATCH 32/47] Removed debugging leftover --- scripts/generate_yaml | 2 +- src/rosparam_handler/parameter_generator_catkin.py | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/scripts/generate_yaml b/scripts/generate_yaml index 2d6106c..aa8644c 100755 --- a/scripts/generate_yaml +++ b/scripts/generate_yaml @@ -10,7 +10,7 @@ if not os.path.exists(sys.argv[1]): sys.exit('ERROR: Param file %s was not found!' % sys.argv[1]) # Read argument -inFile = sys.argv[1] # "/home/claudio/Documents/repos/GitHub/rosparam_ws/src/rosparam_handler/test/cfg/Defaults.params" +inFile = sys.argv[1] # Mock call arguments for call to ParamGenerator sys.argv[1:4] = ["", "", "", ""] # Redirect import statement diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 73ea076..d72fd72 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -520,8 +520,6 @@ def _generateyml(self): else: content += "# " + str(entry["name"]) + " = \n" - from pprint import pprint - pprint(params) yaml_file = os.path.join(os.getcwd(), self.classname + "Parameters.yaml") with open(yaml_file, 'w') as f: From 66c004eaa5a19311a7cbe68b606121a057ae0ffc Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Wed, 13 Sep 2017 12:01:33 +0200 Subject: [PATCH 33/47] Moved common function to util header --- include/rosparam_handler/utilities.hpp | 123 ++++++++++++++++++++++--- templates/Parameters.h.template | 84 ++--------------- 2 files changed, 118 insertions(+), 89 deletions(-) diff --git a/include/rosparam_handler/utilities.hpp b/include/rosparam_handler/utilities.hpp index 10f6fe1..96731ea 100644 --- a/include/rosparam_handler/utilities.hpp +++ b/include/rosparam_handler/utilities.hpp @@ -1,8 +1,22 @@ #pragma once +#include +#include +#include #include #include + +template +using is_vector = std::is_same>; + +template +using is_map = std::is_same>; + namespace rosparam_handler { /// \brief Sets the logger level according to a standardized parameter name 'verbosity'. @@ -64,7 +78,8 @@ inline void showNodeInfo() { /// \brief Retrieve node name /// -/// @param privateNodeHandle The private ROS node handle (i.e. ros::NodeHandle("~") ). +/// @param privateNodeHandle The private ROS node handle (i.e. +/// ros::NodeHandle("~") ). /// @return node name inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle){ std::string name_space = privateNodeHandle.getNamespace(); @@ -82,8 +97,104 @@ inline void exit(const std::string msg = "Runtime Error in rosparam handler.") } +template +inline void setParam(const std::string key, T val) { + ros::param::set(key, val); +} + +template +inline bool getParam(const std::string key, T& val) { + if (!ros::param::has(key)) { + ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); + return false; + } else if (!ros::param::get(key, val)) { + ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); + return false; + } else { + return true; + } +} + +template +inline bool getParam(const std::string key, T& val, const T& defaultValue) { +if (!ros::param::has(key)) { + val = defaultValue; + ros::param::set(key, defaultValue); + ROS_INFO_STREAM("Parameter "< +inline typename std::enable_if::value, void>::type testMin(const std::string key, T& val, T min = std::numeric_limits::min()){ +if (val < min){ + ROS_WARN_STREAM("Value of " << val << " for " + << key << " is smaller than minimal allowed value. Correcting value to min=" << min); + val = min; +} +} + +template +inline typename std::enable_if::value && std::is_arithmetic::value, void>::type testMin(const std::string key, T& val, typename T::value_type min = std::numeric_limits::min()){ +for (auto& v : val) testMin(key, v, min); +} + +template +inline typename std::enable_if::value && std::is_arithmetic::value, void>::type testMin(const std::string key, T& val, typename T::mapped_type min = std::numeric_limits::min()){ +for (auto& v : val) testMin(key, v.second, min); +} + +template +inline typename std::enable_if::value, void>::type testMax(const std::string key, T& val, T max = std::numeric_limits::max()){ +if (val > max){ + ROS_WARN_STREAM("Value of " << val << " for " + << key << " is greater than maximal allowed. Correcting value to max=" << max); + val = max; +} +} + +template +inline typename std::enable_if< + is_vector::value && std::is_arithmetic::value, + void>::type +testMax(const std::string key, T& val, + typename T::value_type min = + std::numeric_limits::max()) { + for (auto& v : val) testMax(key, v, min); +} + +template +inline typename std::enable_if< + is_map::value && std::is_arithmetic::value, + void>::type testMax(const std::string key, T& val, + typename T::mapped_type min = + std::numeric_limits::max()) { + for (auto& v : val) testMax(key, v.second, min); +} + } // namespace rosparam_handler + + + + /// \brief Outstream helper for std:vector template std::ostream& operator<< (std::ostream& out, const std::vector& v) { @@ -109,13 +220,3 @@ std::ostream &operator<<(std::ostream &stream, const std::map& map) stream << '}'; return stream; } - -template -using is_vector = std::is_same>; - -template -using is_map = std::is_same>; \ No newline at end of file diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index c4c5747..0ee6f24 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -8,9 +8,6 @@ #pragma once -#include -#include -#include #include #include #include @@ -33,9 +30,15 @@ struct ${ClassName}Parameters { nodeName{rosparam_handler::getNodeName(private_node_handle)} {} void fromParamServer(){ + bool success = true; $fromParamServer $test_limits + + if(!success){ + missingParamsWarning(); + rosparam_handler::exit("RosparamHandler: GetParam could net retrieve parameter."); + } ROS_DEBUG_STREAM(*this); } @@ -60,81 +63,6 @@ $string_representation; $non_default_params ); } - template - void getParam(const std::string key, T& val) { - if (!ros::param::has(key)) { - ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); - missingParamsWarning(); - rosparam_handler::exit("RosparamHandler: Undefined parameter."); - } else if (!ros::param::get(key, val)) { - ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); - missingParamsWarning(); - rosparam_handler::exit("RosparamHandler: GetParam could net retrieve parameter."); - } - } - - template - void getParam(const std::string key, T& val, const T& defaultValue) { - if (!ros::param::has(key)) { - val = defaultValue; - ros::param::set(key, defaultValue); - ROS_INFO_STREAM("Parameter "< - typename std::enable_if::value, void>::type - testMin(const std::string key, T& val, T min = std::numeric_limits::min()){ - if (val < min){ - ROS_WARN_STREAM("Value of " << val << " for " - << key << " is smaller than minimal allowed value. Correcting value to min=" << min); - val = min; - } - } - - template - typename std::enable_if::value && std::is_arithmetic::value, void>::type - testMin(const std::string key, T& val, typename T::value_type min = std::numeric_limits::min()){ - for (auto& v : val) testMin(key, v, min); - } - - template - typename std::enable_if::value && std::is_arithmetic::value, void>::type - testMin(const std::string key, T& val, typename T::mapped_type min = std::numeric_limits::min()){ - for (auto& v : val) testMin(key, v.second, min); - } - - template - typename std::enable_if::value, void>::type - testMax(const std::string key, T& val, T max = std::numeric_limits::max()){ - if (val > max){ - ROS_WARN_STREAM("Value of " << val << " for " - << key << " is greater than maximal allowed. Correcting value to max=" << max); - val = max; - } - } - - template - typename std::enable_if::value && std::is_arithmetic::value, void>::type - testMax(const std::string key, T& val, typename T::value_type min = std::numeric_limits::max()){ - for (auto& v : val) testMax(key, v, min); - } - - template - typename std::enable_if::value && std::is_arithmetic::value, void>::type - testMax(const std::string key, T& val, typename T::mapped_type min = std::numeric_limits::max()){ - for (auto& v : val) testMax(key, v.second, min); - } - $parameters private: From 6f68db6e3197d8ab27e04783adefbf5b2a4e8a77 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Thu, 14 Sep 2017 07:11:00 +0200 Subject: [PATCH 34/47] Added documentation to functions --- include/rosparam_handler/utilities.hpp | 120 ++++++++++++++++--------- templates/Parameters.h.template | 24 +++-- 2 files changed, 95 insertions(+), 49 deletions(-) diff --git a/include/rosparam_handler/utilities.hpp b/include/rosparam_handler/utilities.hpp index 96731ea..a4d76d9 100644 --- a/include/rosparam_handler/utilities.hpp +++ b/include/rosparam_handler/utilities.hpp @@ -6,22 +6,50 @@ #include #include - +/// \brief Helper function to test for std::vector template using is_vector = std::is_same>; +/// \brief Helper function to test for std::map template using is_map = std::is_same>; - + +/// \brief Outstream helper for std:vector +template +std::ostream& operator<< (std::ostream& out, const std::vector& v) { + if ( !v.empty() ) { + out << '['; + std::copy (v.begin(), v.end(), std::ostream_iterator(out, ", ")); + out << "\b\b]"; + } + return out; +} + +/// \brief Outstream helper for std:map +template +std::ostream &operator<<(std::ostream &stream, const std::map& map) +{ + stream << '{'; + for (typename std::map::const_iterator it = map.begin(); + it != map.end(); + ++it) + { + stream << (*it).first << " --> " << (*it).second << ", "; + } + stream << '}'; + return stream; +} + + namespace rosparam_handler { /// \brief Sets the logger level according to a standardized parameter name 'verbosity'. /// -/// @param nodeHandle The ROS node handle to search for the parameter 'verbosity'. +/// \param nodeHandle The ROS node handle to search for the parameter 'verbosity'. inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) { std::string verbosity; @@ -96,12 +124,19 @@ inline void exit(const std::string msg = "Runtime Error in rosparam handler.") throw std::runtime_error(msg); } - +/// \brief Set parameter on ROS parameter server +/// +/// \param key Parameter name +/// \param val Parameter value template inline void setParam(const std::string key, T val) { ros::param::set(key, val); } +/// \brief Get parameter from ROS parameter server +/// +/// \param key Parameter name +/// \param val Parameter value template inline bool getParam(const std::string key, T& val) { if (!ros::param::has(key)) { @@ -115,16 +150,19 @@ inline bool getParam(const std::string key, T& val) { } } + +/// \brief Get parameter from ROS parameter server or use default value +/// +/// If parameter does not exist on server yet, the default value is used and set on server. +/// \param key Parameter name +/// \param val Parameter value +/// \param defaultValue Parameter default value template inline bool getParam(const std::string key, T& val, const T& defaultValue) { -if (!ros::param::has(key)) { +if (!getParam(key, val)) { val = defaultValue; ros::param::set(key, defaultValue); - ROS_INFO_STREAM("Parameter "< inline typename std::enable_if::value, void>::type testMin(const std::string key, T& val, T min = std::numeric_limits::min()){ if (val < min){ @@ -151,16 +194,31 @@ if (val < min){ } } +/// \brief Limit parameter to lower bound if parameter is a vector. +/// +/// \param key Parameter name +/// \param val Parameter value +/// \param min Lower Threshold template inline typename std::enable_if::value && std::is_arithmetic::value, void>::type testMin(const std::string key, T& val, typename T::value_type min = std::numeric_limits::min()){ for (auto& v : val) testMin(key, v, min); } +/// \brief Limit parameter to lower bound if parameter is a map. +/// +/// \param key Parameter name +/// \param val Parameter value +/// \param min Lower Threshold template inline typename std::enable_if::value && std::is_arithmetic::value, void>::type testMin(const std::string key, T& val, typename T::mapped_type min = std::numeric_limits::min()){ for (auto& v : val) testMin(key, v.second, min); } +/// \brief Limit parameter to upper bound if parameter is a scalar. +/// +/// \param key Parameter name +/// \param val Parameter value +/// \param min Lower Threshold template inline typename std::enable_if::value, void>::type testMax(const std::string key, T& val, T max = std::numeric_limits::max()){ if (val > max){ @@ -170,6 +228,11 @@ if (val > max){ } } +/// \brief Limit parameter to upper bound if parameter is a vector. +/// +/// \param key Parameter name +/// \param val Parameter value +/// \param min Lower Threshold template inline typename std::enable_if< is_vector::value && std::is_arithmetic::value, @@ -180,6 +243,11 @@ testMax(const std::string key, T& val, for (auto& v : val) testMax(key, v, min); } +/// \brief Limit parameter to upper bound if parameter is a map. +/// +/// \param key Parameter name +/// \param val Parameter value +/// \param min Lower Threshold template inline typename std::enable_if< is_map::value && std::is_arithmetic::value, @@ -190,33 +258,3 @@ inline typename std::enable_if< } } // namespace rosparam_handler - - - - - -/// \brief Outstream helper for std:vector -template -std::ostream& operator<< (std::ostream& out, const std::vector& v) { - if ( !v.empty() ) { - out << '['; - std::copy (v.begin(), v.end(), std::ostream_iterator(out, ", ")); - out << "\b\b]"; - } - return out; -} - -/// \brief Outstream helper for std:map -template -std::ostream &operator<<(std::ostream &stream, const std::map& map) -{ - stream << '{'; - for (typename std::map::const_iterator it = map.begin(); - it != map.end(); - ++it) - { - stream << (*it).first << " --> " << (*it).second << ", "; - } - stream << '}'; - return stream; -} diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index 0ee6f24..1ebd5fc 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -20,6 +20,7 @@ struct ${ClassName}Config{}; namespace ${pkgname} { +/// \brief Parameter struct generated by rosparam_handler struct ${ClassName}Parameters { using Config = ${ClassName}Config; @@ -29,12 +30,13 @@ struct ${ClassName}Parameters { 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."); @@ -42,6 +44,10 @@ $test_limits ROS_DEBUG_STREAM(*this); } + /// \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 @@ -51,6 +57,7 @@ $fromConfig #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" @@ -58,17 +65,18 @@ $string_representation; return os; } + $parameters + +private: + /// \brief Issue a warning about missing default parameters. void missingParamsWarning(){ ROS_WARN_STREAM("[" << nodeName << "]\nThe following parameters do not have default values and need to be specified:\n" $non_default_params ); } - $parameters - - private: - const std::string globalNamespace; - const std::string privateNamespace; - const std::string nodeName; + const std::string globalNamespace; + const std::string privateNamespace; + const std::string nodeName; }; } // namespace ${pkgname} From 094ce230a8965b3daa490538d3877dc9b0f998cd Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Fri, 15 Sep 2017 07:47:04 +0200 Subject: [PATCH 35/47] Added functionality for setting parameters on server. This should resolve #31 --- doc/HowToUseYourParameterStruct.md | 11 +++ .../parameter_generator_catkin.py | 8 +- templates/Parameters.h.template | 7 +- templates/Parameters.py.template | 11 +++ test/python/tests/test_defaults.py | 30 ++++++++ test/src/test_defaults.cpp | 77 +++++++++++++++++++ 6 files changed, 141 insertions(+), 3 deletions(-) diff --git a/doc/HowToUseYourParameterStruct.md b/doc/HowToUseYourParameterStruct.md index 4801721..de471ba 100644 --- a/doc/HowToUseYourParameterStruct.md +++ b/doc/HowToUseYourParameterStruct.md @@ -54,6 +54,13 @@ This will update all values that were specified as configurable. At the same tim 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_.toParamServer(); +``` +This will set all non-const parameters with their current value on the ros parameter server. + ## Python All your parameters are fully available in python nodes as well. Just import the parameter file: ```python @@ -72,3 +79,7 @@ def reconfigure_callback(self, config, level): print("Parameter dummy changed to {}".format(self.params.dummy)) ``` +And a call to set the parameters on the server will look like this: +```python +self.params.to_param_server +``` diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 18da90b..ec78b79 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -377,6 +377,7 @@ def _generatecpp(self): param_entries = [] string_representation = [] from_server = [] + to_server = [] non_default_params = [] from_config = [] test_limits = [] @@ -418,8 +419,10 @@ def _generatecpp(self): else: param_entries.append(Template(' ${type} ${name}; /*!< ${description} */').substitute( type=param['type'], name=name, description=param['description'])) - from_server.append(Template(' getParam($paramname, $name$default);').substitute( + from_server.append(Template(' success &= getParam($paramname, $name$default);').substitute( paramname=full_name, name=name, default=default, description=param['description'])) + to_server.append(Template(' setParam(${paramname},${name});').substitute(paramname=full_name,name=name)) + # Test for configurable params if param['configurable']: @@ -441,6 +444,7 @@ def _generatecpp(self): string_representation = "".join(string_representation) non_default_params = "".join(non_default_params) from_server = "\n".join(from_server) + to_server = "\n".join(to_server) from_config = "\n".join(from_config) test_limits = "\n".join(test_limits) @@ -448,7 +452,7 @@ def _generatecpp(self): parameters=param_entries, fromConfig=from_config, fromParamServer=from_server, string_representation=string_representation, non_default_params=non_default_params, nodename=self.nodename, - test_limits=test_limits) + test_limits=test_limits, toParamServer=to_server) header_file = os.path.join(self.cpp_gen_dir, self.classname + "Parameters.h") try: diff --git a/templates/Parameters.h.template b/templates/Parameters.h.template index 1ebd5fc..a493f0d 100644 --- a/templates/Parameters.h.template +++ b/templates/Parameters.h.template @@ -41,7 +41,12 @@ $test_limits missingParamsWarning(); rosparam_handler::exit("RosparamHandler: GetParam could net retrieve parameter."); } - ROS_DEBUG_STREAM(*this); + ROS_DEBUG_STREAM(*this); + } + + /// \brief Set parameters on ROS parameter server. + void toParamServer(){ +$toParamServer } /// \brief Update configurable parameters. diff --git a/templates/Parameters.py.template b/templates/Parameters.py.template index 74ac7a3..882efde 100644 --- a/templates/Parameters.py.template +++ b/templates/Parameters.py.template @@ -36,6 +36,17 @@ class ${ClassName}Parameters(dict): self[k] = self.get_param(k, config) + + def to_param_server(self): + """ + Sets parameters with current values on the parameter server. + """ + for param_name, param_value in self.iteritems(): + config = next((item for item in param_description if item["name"] == param_name), None) + if not config['constant']: + full_name = "/" + param_name if config['global_scope'] else "~" + param_name + rospy.set_param(full_name, param_value) + def from_config(self, config): """ Reads parameter from a dynamic_reconfigure config file. diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py index 3500ddb..6fada5f 100644 --- a/test/python/tests/test_defaults.py +++ b/test/python/tests/test_defaults.py @@ -36,3 +36,33 @@ def test_defaults_on_server(self): self.assertEqual(params.map_param_w_default, rospy.get_param("~map_param_w_default")) self.assertEqual(params.enum_param_w_default, rospy.get_param("~enum_param_w_default")) + + def test_set_parameters_on_server(self): + params = DefaultsParameters() + + params.int_param_w_default = 2 + params.double_param_w_default = 2.2 + params.str_param_w_default = "World Hello" + params.bool_param_w_default = False + params.vector_int_param_w_default = [3,2,1]; + params.vector_double_param_w_default = [1.3,1.2,1.2]; + params.vector_bool_param_w_default = [True, False]; + params.vector_string_param_w_default = ["World","Hello"]; + params.map_param_w_default = {"World","Hello"}; + params.enum_param_w_default = 2; + + # Upload parameters + params.to_param_server() + + # now all parameters should be set on param server + self.assertEqual(params.int_param_w_default, rospy.get_param("~int_param_w_default")) + self.assertAlmostEqual(params.double_param_w_default, rospy.get_param("~double_param_w_default")) + self.assertEqual(params.str_param_w_default, rospy.get_param("~str_param_w_default")) + self.assertEqual(params.bool_param_w_default, rospy.get_param("~bool_param_w_default")) + + self.assertEqual(params.vector_int_param_w_default, rospy.get_param("~vector_int_param_w_default")) + self.assertEqual(params.vector_double_param_w_default, rospy.get_param("~vector_double_param_w_default")) + self.assertEqual(params.vector_string_param_w_default, rospy.get_param("~vector_string_param_w_default")) + + self.assertEqual(params.map_param_w_default, rospy.get_param("~map_param_w_default")) + self.assertEqual(params.enum_param_w_default, rospy.get_param("~enum_param_w_default")) diff --git a/test/src/test_defaults.cpp b/test/src/test_defaults.cpp index 3364967..390f6ba 100644 --- a/test/src/test_defaults.cpp +++ b/test/src/test_defaults.cpp @@ -46,6 +46,83 @@ TEST(RosparamHandler, DefaultsOnParamServer) { ASSERT_TRUE(nh.getParam("bool_param_w_default", bool_param)); EXPECT_EQ(bool_param, testParams.bool_param_w_default); } + { + std::string string_param; + ASSERT_TRUE(nh.getParam("str_param_w_default", string_param)); + EXPECT_EQ(string_param, testParams.str_param_w_default); + } + { + std::vector vector_int_param; + ASSERT_TRUE(nh.getParam("vector_int_param_w_default", vector_int_param)); + EXPECT_EQ(vector_int_param, testParams.vector_int_param_w_default); + } + { + std::vector vector_double_param; + ASSERT_TRUE(nh.getParam("vector_double_param_w_default", vector_double_param)); + EXPECT_EQ(vector_double_param, testParams.vector_double_param_w_default); + } + { + std::vector vector_bool_param; + ASSERT_TRUE(nh.getParam("vector_bool_param_w_default", vector_bool_param)); + EXPECT_EQ(vector_bool_param, testParams.vector_bool_param_w_default); + } + { + std::vector vector_string_param; + ASSERT_TRUE(nh.getParam("vector_string_param_w_default", vector_string_param)); + EXPECT_EQ(vector_string_param, testParams.vector_string_param_w_default); + } + { + std::map map_param_w_default; + ASSERT_TRUE(nh.getParam("map_param_w_default", map_param_w_default)); + EXPECT_EQ(map_param_w_default, testParams.map_param_w_default); + } + { + int enum_param; + ASSERT_TRUE(nh.getParam("enum_param_w_default", enum_param)); + EXPECT_EQ(enum_param, testParams.enum_param_w_default); + } +} + + +TEST(RosparamHandler, SetParamOnServer) { + ros::NodeHandle nh("~"); + ParamType testParams(nh); + ASSERT_NO_THROW(testParams.fromParamServer()); + + testParams.int_param_w_default = 2; + testParams.double_param_w_default = 2.2; + testParams.str_param_w_default = "World Hello"; + testParams.bool_param_w_default = false; + testParams.vector_int_param_w_default = std::vector{3,2,1}; + testParams.vector_double_param_w_default = std::vector{1.3,1.2,1.2}; + testParams.vector_bool_param_w_default = std::vector{true, false}; + testParams.vector_string_param_w_default = std::vector{"World","Hello"}; + testParams.map_param_w_default = std::map{{"World","Hello"}}; + testParams.enum_param_w_default = 2; + + testParams.toParamServer(); + + // values should now be set on param server + { + int int_param; + ASSERT_TRUE(nh.getParam("int_param_w_default", int_param)); + ASSERT_EQ(int_param, testParams.int_param_w_default); + } + { + double double_param; + ASSERT_TRUE(nh.getParam("double_param_w_default", double_param)); + EXPECT_EQ(double_param, testParams.double_param_w_default); + } + { + bool bool_param; + ASSERT_TRUE(nh.getParam("bool_param_w_default", bool_param)); + EXPECT_EQ(bool_param, testParams.bool_param_w_default); + } + { + std::string string_param; + ASSERT_TRUE(nh.getParam("str_param_w_default", string_param)); + EXPECT_EQ(string_param, testParams.str_param_w_default); + } { std::vector vector_int_param; ASSERT_TRUE(nh.getParam("vector_int_param_w_default", vector_int_param)); From 4e969e8ee87c38b5f62c0552da972ffbb553afe9 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Fri, 15 Sep 2017 17:39:25 +0200 Subject: [PATCH 36/47] Fix for failing tests --- src/rosparam_handler/parameter_generator_catkin.py | 5 +++++ test/python/tests/test_defaults.py | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index ec78b79..68293a2 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -490,6 +490,11 @@ def _generatepy(self): pass with open(py_file, 'w') as f: f.write(content) + init_file = os.path.join(self.py_gen_dir, "param", "__init__.py") + with open(init_file, 'wa') as f: + f.write("") + + def _get_parameters(self): """ diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py index 6fada5f..cf8c42f 100644 --- a/test/python/tests/test_defaults.py +++ b/test/python/tests/test_defaults.py @@ -48,7 +48,7 @@ def test_set_parameters_on_server(self): params.vector_double_param_w_default = [1.3,1.2,1.2]; params.vector_bool_param_w_default = [True, False]; params.vector_string_param_w_default = ["World","Hello"]; - params.map_param_w_default = {"World","Hello"}; + params.map_param_w_default = {"World":"Hello"}; params.enum_param_w_default = 2; # Upload parameters From 3f0b727b3eda0b77ffc524c01f632568b1383698 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Fri, 15 Sep 2017 23:55:38 +0200 Subject: [PATCH 37/47] Simplified utility functions --- include/rosparam_handler/utilities.hpp | 29 +++++++------------ .../parameter_generator_catkin.py | 4 +-- 2 files changed, 12 insertions(+), 21 deletions(-) diff --git a/include/rosparam_handler/utilities.hpp b/include/rosparam_handler/utilities.hpp index a4d76d9..38f91ed 100644 --- a/include/rosparam_handler/utilities.hpp +++ b/include/rosparam_handler/utilities.hpp @@ -186,7 +186,7 @@ if (ros::param::has(key)) { /// \param val Parameter value /// \param min Lower Threshold template -inline typename std::enable_if::value, void>::type testMin(const std::string key, T& val, T min = std::numeric_limits::min()){ +inline void testMin(const std::string key, T& val, T min = std::numeric_limits::min()){ if (val < min){ ROS_WARN_STREAM("Value of " << val << " for " << key << " is smaller than minimal allowed value. Correcting value to min=" << min); @@ -200,7 +200,7 @@ if (val < min){ /// \param val Parameter value /// \param min Lower Threshold template -inline typename std::enable_if::value && std::is_arithmetic::value, void>::type testMin(const std::string key, T& val, typename T::value_type min = std::numeric_limits::min()){ +inline void testMin(const std::string key, std::vector& val, T min = std::numeric_limits::min()){ for (auto& v : val) testMin(key, v, min); } @@ -209,8 +209,8 @@ for (auto& v : val) testMin(key, v, min); /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline typename std::enable_if::value && std::is_arithmetic::value, void>::type testMin(const std::string key, T& val, typename T::mapped_type min = std::numeric_limits::min()){ +template +inline void testMin(const std::string key, std::map& val, T min = std::numeric_limits::min()){ for (auto& v : val) testMin(key, v.second, min); } @@ -220,7 +220,7 @@ for (auto& v : val) testMin(key, v.second, min); /// \param val Parameter value /// \param min Lower Threshold template -inline typename std::enable_if::value, void>::type testMax(const std::string key, T& val, T max = std::numeric_limits::max()){ +inline void testMax(const std::string key, T& val, T max = std::numeric_limits::max()){ if (val > max){ ROS_WARN_STREAM("Value of " << val << " for " << key << " is greater than maximal allowed. Correcting value to max=" << max); @@ -234,13 +234,8 @@ if (val > max){ /// \param val Parameter value /// \param min Lower Threshold template -inline typename std::enable_if< - is_vector::value && std::is_arithmetic::value, - void>::type -testMax(const std::string key, T& val, - typename T::value_type min = - std::numeric_limits::max()) { - for (auto& v : val) testMax(key, v, min); +inline void testMax(const std::string key, std::vector& val, T max = std::numeric_limits::max()){ + for (auto& v : val) testMax(key, v, max); } /// \brief Limit parameter to upper bound if parameter is a map. @@ -248,13 +243,9 @@ testMax(const std::string key, T& val, /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline typename std::enable_if< - is_map::value && std::is_arithmetic::value, - void>::type testMax(const std::string key, T& val, - typename T::mapped_type min = - std::numeric_limits::max()) { - for (auto& v : val) testMax(key, v.second, min); +template +inline void testMax(const std::string key, std::map& val, T max = std::numeric_limits::max()){ + for (auto& v : val) testMax(key, v.second, max); } } // namespace rosparam_handler diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 68293a2..90ad997 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -430,10 +430,10 @@ def _generatecpp(self): # Test limits if param['min'] is not None: - test_limits.append(Template(' testMin<$type>($paramname, $name, $min);').substitute( + test_limits.append(Template(' testMin($paramname, $name, $min);').substitute( paramname=full_name, name=name, min=param['min'], type=param['type'])) if param['max'] is not None: - test_limits.append(Template(' testMax<$type>($paramname, $name, $max);').substitute( + test_limits.append(Template(' testMax($paramname, $name, $max);').substitute( paramname=full_name, name=name, max=param['max'], type=param['type'])) # Add debug output From 79c715f7c59060511fd4f2af838fd059aab65ea8 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Fri, 15 Sep 2017 23:59:43 +0200 Subject: [PATCH 38/47] ClangFormat files --- .clang-format | 92 +++++++++ include/rosparam_handler/utilities.hpp | 249 ++++++++++++------------- test/src/test_defaults.cpp | 21 +-- test/src/test_defaultsAtLaunch.cpp | 6 +- test/src/test_defaultsMissing.cpp | 2 +- test/src/test_main.cpp | 2 +- test/src/test_minMax.cpp | 11 +- 7 files changed, 235 insertions(+), 148 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..c45f471 --- /dev/null +++ b/.clang-format @@ -0,0 +1,92 @@ +Language: Cpp +BasedOnStyle: LLVM +AccessModifierOffset: -4 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlinesLeft: true +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +ColumnLimit: 120 +CommentPragmas: '^ IWYU pragma:' +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 8 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] +IncludeCategories: + - Regex: '^<.*\/.*\/.*>' + Priority: 3 + - Regex: '^<.*\/.*>' + Priority: 2 + - Regex: '^<.*>' + Priority: 1 + - Regex: '^".*\/.*\/.*"' + Priority: 6 + - Regex: '^".*\/.*"' + Priority: 5 + - Regex: '^".*"' + Priority: 4 + - Regex: '.*' + Priority: 7 +IndentCaseLabels: false +IndentWidth: 4 +IndentWrappedFunctionNames: false +KeepEmptyLinesAtTheStartOfBlocks: true +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 2000 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SpaceAfterCStyleCast: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 1 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 4 +UseTab: Never diff --git a/include/rosparam_handler/utilities.hpp b/include/rosparam_handler/utilities.hpp index 38f91ed..1bdba54 100644 --- a/include/rosparam_handler/utilities.hpp +++ b/include/rosparam_handler/utilities.hpp @@ -1,50 +1,42 @@ #pragma once +#include #include #include -#include #include #include /// \brief Helper function to test for std::vector template -using is_vector = std::is_same>; +using is_vector = std::is_same>; /// \brief Helper function to test for std::map template -using is_map = std::is_same>; +using is_map = std::is_same< + T, std::map>; /// \brief Outstream helper for std:vector template -std::ostream& operator<< (std::ostream& out, const std::vector& v) { - if ( !v.empty() ) { - out << '['; - std::copy (v.begin(), v.end(), std::ostream_iterator(out, ", ")); - out << "\b\b]"; - } - return out; +std::ostream& operator<<(std::ostream& out, const std::vector& v) { + if (!v.empty()) { + out << '['; + std::copy(v.begin(), v.end(), std::ostream_iterator(out, ", ")); + out << "\b\b]"; + } + return out; } /// \brief Outstream helper for std:map -template -std::ostream &operator<<(std::ostream &stream, const std::map& map) -{ - stream << '{'; - for (typename std::map::const_iterator it = map.begin(); - it != map.end(); - ++it) - { - stream << (*it).first << " --> " << (*it).second << ", "; +template +std::ostream& operator<<(std::ostream& stream, const std::map& map) { + stream << '{'; + for (typename std::map::const_iterator it = map.begin(); it != map.end(); ++it) { + stream << (*it).first << " --> " << (*it).second << ", "; } - stream << '}'; - return stream; + stream << '}'; + return stream; } - namespace rosparam_handler { /// \brief Sets the logger level according to a standardized parameter name 'verbosity'. @@ -52,76 +44,77 @@ namespace rosparam_handler { /// \param nodeHandle The ROS node handle to search for the parameter 'verbosity'. inline void setLoggerLevel(const ros::NodeHandle& nodeHandle) { - std::string verbosity; - if (!nodeHandle.getParam("verbosity", verbosity)) { - verbosity = "warning"; - } - - ros::console::Level level_ros; - bool valid_verbosity {true}; - if (verbosity == "debug") { - level_ros = ros::console::levels::Debug; - } else if (verbosity == "info") { - level_ros = ros::console::levels::Info; - } else if (verbosity == "warning") { - level_ros = ros::console::levels::Warn; - } else if (verbosity == "error") { - level_ros = ros::console::levels::Error; - } else if (verbosity == "fatal") { - level_ros = ros::console::levels::Fatal; - } else { - ROS_WARN_STREAM( - "Invalid verbosity level specified: " << verbosity << "! Falling back to INFO."); - valid_verbosity = false; - } - if (valid_verbosity) { - if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level_ros)) { - ros::console::notifyLoggerLevelsChanged(); - ROS_DEBUG_STREAM("Verbosity set to " << verbosity); - } - } + std::string verbosity; + if (!nodeHandle.getParam("verbosity", verbosity)) { + verbosity = "warning"; + } + + ros::console::Level level_ros; + bool valid_verbosity{true}; + if (verbosity == "debug") { + level_ros = ros::console::levels::Debug; + } else if (verbosity == "info") { + level_ros = ros::console::levels::Info; + } else if (verbosity == "warning") { + level_ros = ros::console::levels::Warn; + } else if (verbosity == "error") { + level_ros = ros::console::levels::Error; + } else if (verbosity == "fatal") { + level_ros = ros::console::levels::Fatal; + } else { + ROS_WARN_STREAM("Invalid verbosity level specified: " << verbosity << "! Falling back to INFO."); + valid_verbosity = false; + } + if (valid_verbosity) { + if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, level_ros)) { + ros::console::notifyLoggerLevelsChanged(); + ROS_DEBUG_STREAM("Verbosity set to " << verbosity); + } + } } /// \brief Show summary about node containing name, namespace, subscribed and advertised topics. inline void showNodeInfo() { - using namespace ros::this_node; + using namespace ros::this_node; - std::vector subscribed_topics, advertised_topics; - getSubscribedTopics(subscribed_topics); - getAdvertisedTopics(advertised_topics); + std::vector subscribed_topics, advertised_topics; + getSubscribedTopics(subscribed_topics); + getAdvertisedTopics(advertised_topics); - std::ostringstream msg_subscr, msg_advert; - for (auto const& t : subscribed_topics) { - msg_subscr << t << std::endl; - } - for (auto const& t : advertised_topics) { - msg_advert << t << std::endl; - } + std::ostringstream msg_subscr, msg_advert; + for (auto const& t : subscribed_topics) { + msg_subscr << t << std::endl; + } + for (auto const& t : advertised_topics) { + msg_advert << t << std::endl; + } - ROS_INFO_STREAM( - "Started '" << getName() << "' in namespace '" << getNamespace() << "'." << std::endl << "Subscribed topics: " << std::endl << msg_subscr.str() << "Advertised topics: " << std::endl << msg_advert.str()); + ROS_INFO_STREAM("Started '" << getName() << "' in namespace '" << getNamespace() << "'." << std::endl + << "Subscribed topics: " << std::endl + << msg_subscr.str() << "Advertised topics: " << std::endl + << msg_advert.str()); } - /// \brief Retrieve node name /// /// @param privateNodeHandle The private ROS node handle (i.e. /// ros::NodeHandle("~") ). /// @return node name -inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle){ +inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle) { std::string name_space = privateNodeHandle.getNamespace(); std::stringstream tempString(name_space); std::string name; - while(std::getline(tempString, name, '/')){;} + while (std::getline(tempString, name, '/')) { + ; + } return name; } /// \brief ExitFunction for rosparam_handler -inline void exit(const std::string msg = "Runtime Error in rosparam handler.") -{ - // std::exit(EXIT_FAILURE); - throw std::runtime_error(msg); +inline void exit(const std::string msg = "Runtime Error in rosparam handler.") { + // std::exit(EXIT_FAILURE); + throw std::runtime_error(msg); } /// \brief Set parameter on ROS parameter server @@ -130,7 +123,7 @@ inline void exit(const std::string msg = "Runtime Error in rosparam handler.") /// \param val Parameter value template inline void setParam(const std::string key, T val) { - ros::param::set(key, val); + ros::param::set(key, val); } /// \brief Get parameter from ROS parameter server @@ -139,18 +132,17 @@ inline void setParam(const std::string key, T val) { /// \param val Parameter value template inline bool getParam(const std::string key, T& val) { - if (!ros::param::has(key)) { - ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); - return false; - } else if (!ros::param::get(key, val)) { - ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); - return false; - } else { - return true; - } + if (!ros::param::has(key)) { + ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); + return false; + } else if (!ros::param::get(key, val)) { + ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); + return false; + } else { + return true; + } } - /// \brief Get parameter from ROS parameter server or use default value /// /// If parameter does not exist on server yet, the default value is used and set on server. @@ -159,25 +151,26 @@ inline bool getParam(const std::string key, T& val) { /// \param defaultValue Parameter default value template inline bool getParam(const std::string key, T& val, const T& defaultValue) { -if (!getParam(key, val)) { - val = defaultValue; - ros::param::set(key, defaultValue); - ROS_INFO_STREAM("Setting default value."); - return true; -} else { - // Param was already retrieved with last if statement. - return true; -} + if (!getParam(key, val)) { + val = defaultValue; + ros::param::set(key, defaultValue); + ROS_INFO_STREAM("Setting default value."); + return true; + } else { + // Param was already retrieved with last if statement. + return true; + } } /// \brief Tests that parameter is not set on the parameter server -inline bool testConstParam(const std::string key){ -if (ros::param::has(key)) { - ROS_WARN_STREAM("Parameter " << key << "' was set on the parameter server eventhough it was defined to be constant."); - return false; -} else { - return true; -} +inline bool testConstParam(const std::string key) { + if (ros::param::has(key)) { + ROS_WARN_STREAM("Parameter " << key + << "' was set on the parameter server eventhough it was defined to be constant."); + return false; + } else { + return true; + } } /// \brief Limit parameter to lower bound if parameter is a scalar. @@ -185,13 +178,13 @@ if (ros::param::has(key)) { /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline void testMin(const std::string key, T& val, T min = std::numeric_limits::min()){ -if (val < min){ - ROS_WARN_STREAM("Value of " << val << " for " - << key << " is smaller than minimal allowed value. Correcting value to min=" << min); - val = min; -} +template +inline void testMin(const std::string key, T& val, T min = std::numeric_limits::min()) { + if (val < min) { + ROS_WARN_STREAM("Value of " << val << " for " << key + << " is smaller than minimal allowed value. Correcting value to min=" << min); + val = min; + } } /// \brief Limit parameter to lower bound if parameter is a vector. @@ -199,9 +192,10 @@ if (val < min){ /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline void testMin(const std::string key, std::vector& val, T min = std::numeric_limits::min()){ -for (auto& v : val) testMin(key, v, min); +template +inline void testMin(const std::string key, std::vector& val, T min = std::numeric_limits::min()) { + for (auto& v : val) + testMin(key, v, min); } /// \brief Limit parameter to lower bound if parameter is a map. @@ -209,9 +203,10 @@ for (auto& v : val) testMin(key, v, min); /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline void testMin(const std::string key, std::map& val, T min = std::numeric_limits::min()){ -for (auto& v : val) testMin(key, v.second, min); +template +inline void testMin(const std::string key, std::map& val, T min = std::numeric_limits::min()) { + for (auto& v : val) + testMin(key, v.second, min); } /// \brief Limit parameter to upper bound if parameter is a scalar. @@ -219,13 +214,13 @@ for (auto& v : val) testMin(key, v.second, min); /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline void testMax(const std::string key, T& val, T max = std::numeric_limits::max()){ -if (val > max){ - ROS_WARN_STREAM("Value of " << val << " for " - << key << " is greater than maximal allowed. Correcting value to max=" << max); - val = max; -} +template +inline void testMax(const std::string key, T& val, T max = std::numeric_limits::max()) { + if (val > max) { + ROS_WARN_STREAM("Value of " << val << " for " << key + << " is greater than maximal allowed. Correcting value to max=" << max); + val = max; + } } /// \brief Limit parameter to upper bound if parameter is a vector. @@ -234,8 +229,9 @@ if (val > max){ /// \param val Parameter value /// \param min Lower Threshold template -inline void testMax(const std::string key, std::vector& val, T max = std::numeric_limits::max()){ - for (auto& v : val) testMax(key, v, max); +inline void testMax(const std::string key, std::vector& val, T max = std::numeric_limits::max()) { + for (auto& v : val) + testMax(key, v, max); } /// \brief Limit parameter to upper bound if parameter is a map. @@ -243,9 +239,10 @@ inline void testMax(const std::string key, std::vector& val, T max = std::num /// \param key Parameter name /// \param val Parameter value /// \param min Lower Threshold -template -inline void testMax(const std::string key, std::map& val, T max = std::numeric_limits::max()){ - for (auto& v : val) testMax(key, v.second, max); +template +inline void testMax(const std::string key, std::map& val, T max = std::numeric_limits::max()) { + for (auto& v : val) + testMax(key, v.second, max); } } // namespace rosparam_handler diff --git a/test/src/test_defaults.cpp b/test/src/test_defaults.cpp index 390f6ba..ef3312d 100644 --- a/test/src/test_defaults.cpp +++ b/test/src/test_defaults.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include typedef rosparam_handler::DefaultsParameters ParamType; @@ -14,12 +14,12 @@ TEST(RosparamHandler, Defaults) { ASSERT_EQ("Hello World", testParams.str_param_w_default); ASSERT_EQ(true, testParams.bool_param_w_default); - ASSERT_EQ(std::vector({1,2,3}), testParams.vector_int_param_w_default); + ASSERT_EQ(std::vector({1, 2, 3}), testParams.vector_int_param_w_default); ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testParams.vector_double_param_w_default); ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_w_default); ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_w_default); - std::map tmp{{"Hello","World"}}; + std::map tmp{{"Hello", "World"}}; ASSERT_EQ(tmp, testParams.map_param_w_default); ASSERT_EQ(1, testParams.enum_param_w_default); @@ -83,23 +83,22 @@ TEST(RosparamHandler, DefaultsOnParamServer) { } } - TEST(RosparamHandler, SetParamOnServer) { ros::NodeHandle nh("~"); ParamType testParams(nh); ASSERT_NO_THROW(testParams.fromParamServer()); testParams.int_param_w_default = 2; - testParams.double_param_w_default = 2.2; + testParams.double_param_w_default = 2.2; testParams.str_param_w_default = "World Hello"; testParams.bool_param_w_default = false; - testParams.vector_int_param_w_default = std::vector{3,2,1}; - testParams.vector_double_param_w_default = std::vector{1.3,1.2,1.2}; + testParams.vector_int_param_w_default = std::vector{3, 2, 1}; + testParams.vector_double_param_w_default = std::vector{1.3, 1.2, 1.2}; testParams.vector_bool_param_w_default = std::vector{true, false}; - testParams.vector_string_param_w_default = std::vector{"World","Hello"}; - testParams.map_param_w_default = std::map{{"World","Hello"}}; - testParams.enum_param_w_default = 2; - + testParams.vector_string_param_w_default = std::vector{"World", "Hello"}; + testParams.map_param_w_default = std::map{{"World", "Hello"}}; + testParams.enum_param_w_default = 2; + testParams.toParamServer(); // values should now be set on param server diff --git a/test/src/test_defaultsAtLaunch.cpp b/test/src/test_defaultsAtLaunch.cpp index 3a87d1d..4e935f9 100644 --- a/test/src/test_defaultsAtLaunch.cpp +++ b/test/src/test_defaultsAtLaunch.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include typedef rosparam_handler::DefaultsAtLaunchParameters ParamType; @@ -14,12 +14,12 @@ TEST(RosparamHandler, DefaultsAtLaunch) { ASSERT_EQ("Hello World", testParams.str_param_wo_default); ASSERT_EQ(true, testParams.bool_param_wo_default); - ASSERT_EQ(std::vector({1,2,3}), testParams.vector_int_param_wo_default); + ASSERT_EQ(std::vector({1, 2, 3}), testParams.vector_int_param_wo_default); ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testParams.vector_double_param_wo_default); ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_wo_default); ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_wo_default); - std::map tmp{{"Hello","World"}}; + std::map tmp{{"Hello", "World"}}; ASSERT_EQ(tmp, testParams.map_param_wo_default); ASSERT_EQ(1, testParams.enum_param_wo_default); diff --git a/test/src/test_defaultsMissing.cpp b/test/src/test_defaultsMissing.cpp index 39c5007..6910285 100644 --- a/test/src/test_defaultsMissing.cpp +++ b/test/src/test_defaultsMissing.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include typedef rosparam_handler::DefaultsMissingParameters ParamType; diff --git a/test/src/test_main.cpp b/test/src/test_main.cpp index 72e1471..d2e0020 100644 --- a/test/src/test_main.cpp +++ b/test/src/test_main.cpp @@ -1,5 +1,5 @@ -#include #include +#include int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/src/test_minMax.cpp b/test/src/test_minMax.cpp index 38e6583..4d21f96 100644 --- a/test/src/test_minMax.cpp +++ b/test/src/test_minMax.cpp @@ -1,5 +1,5 @@ -#include #include +#include #include typedef rosparam_handler::MinMaxParameters ParamType; @@ -11,11 +11,10 @@ TEST(RosparamHandler, MinMax) { ASSERT_EQ(2, testParams.int_param_w_minmax); ASSERT_DOUBLE_EQ(2., testParams.double_param_w_minmax); - - ASSERT_EQ(std::vector({0,2,2}), testParams.vector_int_param_w_minmax); + + ASSERT_EQ(std::vector({0, 2, 2}), testParams.vector_int_param_w_minmax); ASSERT_EQ(std::vector({0., 1.2, 2.}), testParams.vector_double_param_w_minmax); - - std::map tmp{{"value1",0.},{"value2",1.2},{"value3",2.}}; - ASSERT_EQ(tmp, testParams.map_param_w_minmax); + std::map tmp{{"value1", 0.}, {"value2", 1.2}, {"value3", 2.}}; + ASSERT_EQ(tmp, testParams.map_param_w_minmax); } \ No newline at end of file From 16e88a9c5194dc3914cd351e9b96d5ec3c9b4e34 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sat, 16 Sep 2017 00:13:57 +0200 Subject: [PATCH 39/47] Reformatted python files --- .../parameter_generator_catkin.py | 38 +++++++++---------- test/python/rosparam_handler_python_test.py | 8 ++-- test/python/tests/test_defaults.py | 10 ++--- 3 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 90ad997..95ec259 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -41,8 +41,6 @@ def eprint(*args, **kwargs): sys.exit(1) -# TODO add group - class ParameterGenerator(object): """Automatic config file and header generator""" @@ -59,7 +57,8 @@ def __init__(self, parent=None, group=""): self.group_variable = filter(str.isalnum, self.group) if len(sys.argv) != 5: - eprint("ParameterGenerator: Unexpected amount of args, did you try to call this directly? You shouldn't do this!") + eprint( + "ParameterGenerator: Unexpected amount of args, did you try to call this directly? You shouldn't do this!") self.dynconfpath = sys.argv[1] self.share_dir = sys.argv[2] @@ -179,7 +178,8 @@ def _perform_checks(self, param): if (param['max'] is not None or param['min'] is not None): ptype = in_type[9:-1].split(',') if len(ptype) != 2: - eprint(param['name'], "Wrong syntax used for setting up std::map<... , ...>: You provided '%s' with " + eprint(param['name'], + "Wrong syntax used for setting up std::map<... , ...>: You provided '%s' with " "parameter %s" % in_type) ptype = ptype[1].strip() if ptype == "std::string": @@ -188,16 +188,17 @@ def _perform_checks(self, param): pattern = r'^[a-zA-Z][a-zA-Z0-9_]*$' if not re.match(pattern, param['name']): eprint(param['name'], "The name of field does not follow the ROS naming conventions, " - "see http://wiki.ros.org/ROS/Patterns/Conventions") + "see http://wiki.ros.org/ROS/Patterns/Conventions") if param['configurable'] and ( - param['global_scope'] or param['is_vector'] or param['is_map'] or param['constant']): - eprint(param['name'], "Global Parameters, vectors, maps and constant params can not be declared configurable! ") + param['global_scope'] or param['is_vector'] or param['is_map'] or param['constant']): + eprint(param['name'], + "Global Parameters, vectors, maps and constant params can not be declared configurable! ") if param['global_scope'] and param['default'] is not None: eprint(param['name'], "Default values for global parameters should not be specified in node! ") if param['constant'] and param['default'] is None: eprint(param['name'], "Constant parameters need a default value!") if param['name'] in [p['name'] for p in self.parameters]: - eprint(param['name'],"Parameter with the same name exists already") + eprint(param['name'], "Parameter with the same name exists already") if param['edit_method'] == '': param['edit_method'] = '""' elif param['edit_method'] != '""': @@ -212,12 +213,12 @@ def _perform_checks(self, param): ptype = in_type[9:-1].split(',') if len(ptype) != 2: eprint(param['name'], "Wrong syntax used for setting up std::map<... , ...>: You provided '%s' with " - "parameter %s" % in_type) + "parameter %s" % in_type) ptype[0] = ptype[0].strip() ptype[1] = ptype[1].strip() if ptype[0] != "std::string": eprint(param['name'], "Can not setup map with %s as key type. Only std::map are allowed" % ptype[0]) + "...> are allowed" % ptype[0]) self._test_primitive_type(param['name'], ptype[0]) self._test_primitive_type(param['name'], ptype[1]) param['type'] = 'std::map<{},{}>'.format(ptype[0], ptype[1]) @@ -279,7 +280,7 @@ def _get_cvaluelist(param, field): :return: C++ compatible representation """ values = param[field] - assert(isinstance(values, list)) + assert (isinstance(values, list)) form = "" for value in values: if param['type'] == 'std::vector': @@ -300,7 +301,7 @@ def _get_cvaluedict(param, field): :return: C++ compatible representation """ values = param[field] - assert(isinstance(values, dict)) + assert (isinstance(values, dict)) form = "" for key, value in values.items(): if param['type'] == 'std::map': @@ -421,8 +422,8 @@ def _generatecpp(self): type=param['type'], name=name, description=param['description'])) from_server.append(Template(' success &= getParam($paramname, $name$default);').substitute( paramname=full_name, name=name, default=default, description=param['description'])) - to_server.append(Template(' setParam(${paramname},${name});').substitute(paramname=full_name,name=name)) - + to_server.append( + Template(' setParam(${paramname},${name});').substitute(paramname=full_name, name=name)) # Test for configurable params if param['configurable']: @@ -438,7 +439,7 @@ def _generatecpp(self): # Add debug output string_representation.append(Template(' << "\t" << p.$namespace << "$name:" << p.$name << ' - '"\\n"\n').substitute(namespace=namespace, name=name)) + '"\\n"\n').substitute(namespace=namespace, name=name)) param_entries = "\n".join(param_entries) string_representation = "".join(string_representation) @@ -450,7 +451,8 @@ def _generatecpp(self): content = Template(template).substitute(pkgname=self.pkgname, ClassName=self.classname, parameters=param_entries, fromConfig=from_config, - fromParamServer=from_server, string_representation=string_representation, + fromParamServer=from_server, + string_representation=string_representation, non_default_params=non_default_params, nodename=self.nodename, test_limits=test_limits, toParamServer=to_server) @@ -472,7 +474,7 @@ def _generatepy(self): """ params = self._get_parameters() paramDescription = str(params) - + # Read in template file templatefile = os.path.join(self.dynconfpath, "templates", "Parameters.py.template") with open(templatefile, 'r') as f: @@ -493,8 +495,6 @@ def _generatepy(self): init_file = os.path.join(self.py_gen_dir, "param", "__init__.py") with open(init_file, 'wa') as f: f.write("") - - def _get_parameters(self): """ diff --git a/test/python/rosparam_handler_python_test.py b/test/python/rosparam_handler_python_test.py index c42e6f8..62220ea 100755 --- a/test/python/rosparam_handler_python_test.py +++ b/test/python/rosparam_handler_python_test.py @@ -6,7 +6,7 @@ import tests if __name__ == '__main__': - import rostest - - rospy.init_node(PKG) - rostest.rosrun(PKG, "RosparamTestSuite", "tests.RosparamTestSuite") + import rostest + + rospy.init_node(PKG) + rostest.rosrun(PKG, "RosparamTestSuite", "tests.RosparamTestSuite") diff --git a/test/python/tests/test_defaults.py b/test/python/tests/test_defaults.py index cf8c42f..d3ad977 100644 --- a/test/python/tests/test_defaults.py +++ b/test/python/tests/test_defaults.py @@ -44,12 +44,12 @@ def test_set_parameters_on_server(self): params.double_param_w_default = 2.2 params.str_param_w_default = "World Hello" params.bool_param_w_default = False - params.vector_int_param_w_default = [3,2,1]; - params.vector_double_param_w_default = [1.3,1.2,1.2]; + params.vector_int_param_w_default = [3, 2, 1]; + params.vector_double_param_w_default = [1.3, 1.2, 1.2]; params.vector_bool_param_w_default = [True, False]; - params.vector_string_param_w_default = ["World","Hello"]; - params.map_param_w_default = {"World":"Hello"}; - params.enum_param_w_default = 2; + params.vector_string_param_w_default = ["World", "Hello"]; + params.map_param_w_default = {"World": "Hello"}; + params.enum_param_w_default = 2; # Upload parameters params.to_param_server() From 0f0af525e3dfef7374013606393a50e8dc65e3a1 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sat, 16 Sep 2017 00:55:10 +0200 Subject: [PATCH 40/47] Added documentation for generator script --- doc/HowToUseYourParameterStruct.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/doc/HowToUseYourParameterStruct.md b/doc/HowToUseYourParameterStruct.md index de471ba..74b223d 100644 --- a/doc/HowToUseYourParameterStruct.md +++ b/doc/HowToUseYourParameterStruct.md @@ -61,6 +61,14 @@ params_.toParamServer(); ``` This will set all non-const parameters with their current value on the ros parameter server. +## Setting parameters at launch time +If you want to run your node with parameters other then the default parameters, then they have to be set on the parameter server before the node starts. +To ease the burden of setting all parameters one after the other, roslaunch has the [rosparam](http://wiki.ros.org/roslaunch/XML/rosparam) argument to load a YAML file containing a whole set of key value pairs. +Rosparam handler provides a script, to automatically generates a YAML file for you to use. Calling it will generate a file in your current directory. +```sh +rosrun rosparam_handler generate_yaml +``` + ## Python All your parameters are fully available in python nodes as well. Just import the parameter file: ```python From 9134a50f33499ffce6def3329700e8e6f7d17f4c Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sat, 16 Sep 2017 00:59:13 +0200 Subject: [PATCH 41/47] Uncomment lines without default, to force user to set them --- src/rosparam_handler/parameter_generator_catkin.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 0a41ad3..61cd97d 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -527,7 +527,7 @@ def _generateyml(self): if entry["default"] is not None: content += str(entry["name"]) + " = " + str(entry["default"]) + "\n" else: - content += "# " + str(entry["name"]) + " = \n" + content += str(entry["name"]) + " = \n" yaml_file = os.path.join(os.getcwd(), self.classname + "Parameters.yaml") From ee68c646c50e96f47e41e52272dc8a2cb57faa53 Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sat, 16 Sep 2017 21:13:25 +0200 Subject: [PATCH 42/47] Bugfixes --- scripts/generate_yaml | 21 ------------ scripts/generate_yaml.py | 32 +++++++++++++++++++ .../parameter_generator_catkin.py | 29 ++++++++++------- 3 files changed, 49 insertions(+), 33 deletions(-) delete mode 100755 scripts/generate_yaml create mode 100755 scripts/generate_yaml.py diff --git a/scripts/generate_yaml b/scripts/generate_yaml deleted file mode 100755 index aa8644c..0000000 --- a/scripts/generate_yaml +++ /dev/null @@ -1,21 +0,0 @@ -#!/usr/bin/env python -import sys -import os -from rosparam_handler.parameter_generator_catkin import YamlGenerator - -if len(sys.argv) < 2: - sys.exit('Usage: %s <.params file>' % sys.argv[0]) - -if not os.path.exists(sys.argv[1]): - sys.exit('ERROR: Param file %s was not found!' % sys.argv[1]) - -# Read argument -inFile = sys.argv[1] -# Mock call arguments for call to ParamGenerator -sys.argv[1:4] = ["", "", "", ""] -# Redirect import statement -sys.modules['rosparam_handler.parameter_generator_catkin'] = __import__('sys') - -# Execute param file in mocked environment -globalVars = {"__file__": inFile, 'ParameterGenerator': YamlGenerator} -execfile(inFile, globalVars) diff --git a/scripts/generate_yaml.py b/scripts/generate_yaml.py new file mode 100755 index 0000000..2548c3d --- /dev/null +++ b/scripts/generate_yaml.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python +import sys +import os +from rosparam_handler.parameter_generator_catkin import YamlGenerator + + +def generate_yaml(in_file): + + if not os.path.exists(in_file): + sys.exit('ERROR: Param file %s was not found!' % in_file) + # Read argument + in_file = os.path.abspath(in_file) + basename = os.path.basename(in_file) + basename, _ = os.path.splitext(basename) + out_file = os.path.join(os.getcwd(), basename + "Parameters.yaml") + print("Yaml file is written to: {}".format(out_file)) + + # Mock call arguments for call to ParamGenerator + sys.argv[1:4] = ["", "", "", ""] + # Redirect import statement + sys.modules['rosparam_handler.parameter_generator_catkin'] = __import__('sys') + + # Execute param file in mocked environment + global_vars = {"__file__": in_file, 'ParameterGenerator': YamlGenerator} + return execfile(in_file, global_vars) + + +if __name__ == "__main__": + if len(sys.argv) < 2: + sys.exit('Usage: %s <.params file>' % sys.argv[0]) + + generate_yaml(sys.argv[1]) diff --git a/src/rosparam_handler/parameter_generator_catkin.py b/src/rosparam_handler/parameter_generator_catkin.py index 61cd97d..015b112 100644 --- a/src/rosparam_handler/parameter_generator_catkin.py +++ b/src/rosparam_handler/parameter_generator_catkin.py @@ -338,7 +338,7 @@ def _generateImpl(self): :return: """ self._generatecfg() - self._generatecpp() + self._generatehpp() self._generatepy() return 0 @@ -370,7 +370,7 @@ def _generatecfg(self): f.write(template) os.chmod(cfg_file, 509) # entspricht 775 (octal) - def _generatecpp(self): + def _generatehpp(self): """ Generate C++ Header file, holding the parameter struct. :param self: @@ -423,26 +423,32 @@ def _generatecpp(self): '*/').substitute(type=param['type'], name=name, description=param['description'], default=self._get_cvalue(param, "default"))) - from_server.append(Template(' testConstParam($paramname);').substitute(paramname=full_name)) + from_server.append(Template(' rosparam_handler::testConstParam($paramname);').substitute(paramname=full_name)) else: param_entries.append(Template(' ${type} ${name}; /*!< ${description} */').substitute( type=param['type'], name=name, description=param['description'])) - from_server.append(Template(' success &= getParam($paramname, $name$default);').substitute( + from_server.append(Template(' success &= rosparam_handler::getParam($paramname, $name$default);').substitute( paramname=full_name, name=name, default=default, description=param['description'])) to_server.append( - Template(' setParam(${paramname},${name});').substitute(paramname=full_name, name=name)) + Template(' rosparam_handler::setParam(${paramname},${name});').substitute(paramname=full_name, name=name)) # Test for configurable params if param['configurable']: from_config.append(Template(' $name = config.$name;').substitute(name=name)) # Test limits + if param['is_vector']: + ttype = param['type'][12:-1].strip() + elif param['is_map']: + ttype = param['type'][9:-1].strip() + else: + ttype = param['type'] if param['min'] is not None: - test_limits.append(Template(' testMin($paramname, $name, $min);').substitute( - paramname=full_name, name=name, min=param['min'], type=param['type'])) + test_limits.append(Template(' rosparam_handler::testMin<$type>($paramname, $name, $min);').substitute( + paramname=full_name, name=name, min=param['min'], type=ttype)) if param['max'] is not None: - test_limits.append(Template(' testMax($paramname, $name, $max);').substitute( - paramname=full_name, name=name, max=param['max'], type=param['type'])) + test_limits.append(Template(' rosparam_handler::testMax<$type>($paramname, $name, $max);').substitute( + paramname=full_name, name=name, max=param['max'], type=ttype)) # Add debug output string_representation.append(Template(' << "\t" << p.$namespace << "$name:" << p.$name << ' @@ -510,7 +516,6 @@ def _generateyml(self): :return: """ params = self._get_parameters() - paramDescription = str(params) content = "### This file was generated using the rosparam_handler generate_yaml script.\n" @@ -525,9 +530,9 @@ def _generateyml(self): if entry["global_scope"]: content += "# Lives in global namespace!\n" if entry["default"] is not None: - content += str(entry["name"]) + " = " + str(entry["default"]) + "\n" + content += str(entry["name"]) + ": " + str(entry["default"]) + "\n" else: - content += str(entry["name"]) + " = \n" + content += str(entry["name"]) + ": \n" yaml_file = os.path.join(os.getcwd(), self.classname + "Parameters.yaml") From 3fa3f47e87eb9b56af17ebc7060d9f358736b2ca Mon Sep 17 00:00:00 2001 From: Claudio Bandera Date: Sat, 16 Sep 2017 22:27:07 +0200 Subject: [PATCH 43/47] Renamed script again --- scripts/{generate_yaml.py => generate_yaml} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename scripts/{generate_yaml.py => generate_yaml} (100%) diff --git a/scripts/generate_yaml.py b/scripts/generate_yaml similarity index 100% rename from scripts/generate_yaml.py rename to scripts/generate_yaml From 0bd890ab0c843478b3912bae84cbf0128b3a4d49 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Wed, 10 Jan 2018 15:18:58 +0100 Subject: [PATCH 44/47] add dynamic_reconfigure test --- test/cfg/Defaults.params | 10 +-- test/src/test_dynamicReconfigure.cpp | 97 ++++++++++++++++++++++++++++ 2 files changed, 102 insertions(+), 5 deletions(-) create mode 100644 test/src/test_dynamicReconfigure.cpp diff --git a/test/cfg/Defaults.params b/test/cfg/Defaults.params index df3f0cf..632bebd 100755 --- a/test/cfg/Defaults.params +++ b/test/cfg/Defaults.params @@ -9,10 +9,10 @@ from rosparam_handler.parameter_generator_catkin import * gen = ParameterGenerator() # Parameters with different types -gen.add("int_param_w_default", paramtype="int", description="An Integer parameter", default=1) -gen.add("double_param_w_default", paramtype="double",description="A double parameter", default=1.1) -gen.add("str_param_w_default", paramtype="std::string", description="A string parameter", default="Hello World") -gen.add("bool_param_w_default", paramtype="bool", description="A Boolean parameter", default=True) +gen.add("int_param_w_default", paramtype="int", description="An Integer parameter", default=1, configurable=True) +gen.add("double_param_w_default", paramtype="double",description="A double parameter", default=1.1, configurable=True) +gen.add("str_param_w_default", paramtype="std::string", description="A string parameter", default="Hello World", configurable=True) +gen.add("bool_param_w_default", paramtype="bool", description="A Boolean parameter", default=True, configurable=True) gen.add("vector_int_param_w_default", paramtype="std::vector", description="A vector of int parameter", default=[1,2,3]) gen.add("vector_double_param_w_default", paramtype="std::vector", description="A vector of double parameter", default=[1.1, 1.2, 1.3]) @@ -24,4 +24,4 @@ gen.add("map_param_w_default", paramtype="std::map", de gen.add_enum("enum_param_w_default", description="enum", entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium") #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("rosparam_handler", "rosparam_handler_test", "Defaults")) \ No newline at end of file +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "Defaults")) diff --git a/test/src/test_dynamicReconfigure.cpp b/test/src/test_dynamicReconfigure.cpp new file mode 100644 index 0000000..eb71c66 --- /dev/null +++ b/test/src/test_dynamicReconfigure.cpp @@ -0,0 +1,97 @@ +#include +#include +#include + +#include + +typedef rosparam_handler::DefaultsParameters ParamType; +typedef rosparam_handler::DefaultsConfig ConfigType; + +class TestDynamicReconfigure : public testing::Test +{ +public: + + TestDynamicReconfigure() + : nh("~") + , testParams(nh) + , drSrv(nh) + { + auto cb = boost::bind(&TestDynamicReconfigure::configCallback, this, _1, _2); + drSrv.setCallback(cb); + } + + ~TestDynamicReconfigure() = default; + + void configCallback(ConfigType& config, uint32_t /*level*/) + { + testParams.fromConfig(config); + } + + ros::NodeHandle nh; + + ParamType testParams; + + dynamic_reconfigure::Server drSrv; +}; + +TEST_F(TestDynamicReconfigure, DynamicReconf) +{ + // Delete in case they are still on the server. + nh.deleteParam("int_param_w_default"); + nh.deleteParam("double_param_w_default"); + nh.deleteParam("str_param_w_default"); + nh.deleteParam("bool_param_w_default"); + + ASSERT_NO_THROW(testParams.fromParamServer()); + + ASSERT_EQ(1, testParams.int_param_w_default); + ASSERT_DOUBLE_EQ(1.1, testParams.double_param_w_default); + ASSERT_EQ("Hello World", testParams.str_param_w_default); + ASSERT_TRUE(testParams.bool_param_w_default); + + // ASSERT_EQ(std::vector({1, 2, 3}), testParams.vector_int_param_w_default); + // ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testParams.vector_double_param_w_default); + // ASSERT_EQ(std::vector({false, true}), testParams.vector_bool_param_w_default); + // ASSERT_EQ(std::vector({"Hello", "World"}), testParams.vector_string_param_w_default); + + // std::map tmp{{"Hello", "World"}}; + // ASSERT_EQ(tmp, testParams.map_param_w_default); + + // ASSERT_EQ(1, testParams.enum_param_w_default); + + dynamic_reconfigure::ReconfigureRequest srv_req; + dynamic_reconfigure::ReconfigureResponse srv_resp; + dynamic_reconfigure::Config conf; + + dynamic_reconfigure::IntParameter int_param; + int_param.name = "int_param_w_default"; + int_param.value = 2; + + dynamic_reconfigure::DoubleParameter double_param; + double_param.name = "double_param_w_default"; + double_param.value = 2.2; + + dynamic_reconfigure::StrParameter str_param; + str_param.name = "str_param_w_default"; + str_param.value = "Foo Bar"; + + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "bool_param_w_default"; + bool_param.value = false; + + conf.ints.push_back(int_param); + conf.doubles.push_back(double_param); + conf.strs.push_back(str_param); + conf.bools.push_back(bool_param); + + srv_req.config = conf; + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv_req, srv_resp)); + + ros::Duration(1).sleep(); + + EXPECT_EQ(2, testParams.int_param_w_default); + EXPECT_DOUBLE_EQ(2.2, testParams.double_param_w_default); + EXPECT_EQ("Foo Bar", testParams.str_param_w_default); + EXPECT_FALSE(testParams.bool_param_w_default); +} From 01f2c5e9415f0fa4d04ade8ed5a90a8de542dfc2 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Wed, 10 Jan 2018 15:21:12 +0100 Subject: [PATCH 45/47] std::endl -> \n --- include/rosparam_handler/utilities.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/include/rosparam_handler/utilities.hpp b/include/rosparam_handler/utilities.hpp index 1bdba54..21ae708 100644 --- a/include/rosparam_handler/utilities.hpp +++ b/include/rosparam_handler/utilities.hpp @@ -84,15 +84,15 @@ inline void showNodeInfo() { std::ostringstream msg_subscr, msg_advert; for (auto const& t : subscribed_topics) { - msg_subscr << t << std::endl; + msg_subscr << t << "\n"; } for (auto const& t : advertised_topics) { - msg_advert << t << std::endl; + msg_advert << t << "\n"; } - ROS_INFO_STREAM("Started '" << getName() << "' in namespace '" << getNamespace() << "'." << std::endl - << "Subscribed topics: " << std::endl - << msg_subscr.str() << "Advertised topics: " << std::endl + ROS_INFO_STREAM("Started '" << getName() << "' in namespace '" << getNamespace() << "'.\n" + << "Subscribed topics:\n" + << msg_subscr.str() << "Advertised topics:\n" << msg_advert.str()); } From a31e7be2cf3e1e08daf75272f21e6a077b1f9519 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Wed, 10 Jan 2018 16:06:07 +0100 Subject: [PATCH 46/47] update maintainer --- README.md | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ebb2419..77d546f 100644 --- a/README.md +++ b/README.md @@ -6,7 +6,7 @@ A unified parameter handler for nodes with automatic code generation. Save time on defining your parameters. No more redundant code. Easy error checking. Make them dynamic with a single flag. - Maintainer status: maintained -- Maintainer: Claudio Bandera +- Maintainer: Jeremie Deray - Author: Claudio Bandera - License: BSD - Bug / feature tracker: https://github.com/cbandera/rosparam_handler/issues diff --git a/package.xml b/package.xml index 4a2ce05..f868d0d 100644 --- a/package.xml +++ b/package.xml @@ -5,7 +5,7 @@ An easy wrapper for using parameters in ROS. BSD - Claudio Bandera + Jeremie Deray Claudio Bandera https://github.com/cbandera/rosparam_handler.git From 8a69587839c6fe55eddbd1a2f1b0a3dd6af9d3c1 Mon Sep 17 00:00:00 2001 From: Jeremie Deray Date: Sun, 4 Feb 2018 14:09:40 +0100 Subject: [PATCH 47/47] fix #34 --- test/cfg/Defaults.params | 2 +- test/cfg/DefaultsAtLaunch.params | 4 ++-- test/cfg/DefaultsMissing.params | 4 ++-- test/cfg/MinMax.params | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/test/cfg/Defaults.params b/test/cfg/Defaults.params index 632bebd..7585b40 100755 --- a/test/cfg/Defaults.params +++ b/test/cfg/Defaults.params @@ -2,7 +2,7 @@ ####### workaround so that the module is found ####### import sys import os -sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +sys.path.insert(0, os.path.join(os.path.dirname(__file__),"../../src")) ###################################################### from rosparam_handler.parameter_generator_catkin import * diff --git a/test/cfg/DefaultsAtLaunch.params b/test/cfg/DefaultsAtLaunch.params index a1727cd..ed759f0 100755 --- a/test/cfg/DefaultsAtLaunch.params +++ b/test/cfg/DefaultsAtLaunch.params @@ -2,7 +2,7 @@ ####### workaround so that the module is found ####### import sys import os -sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +sys.path.insert(0, os.path.join(os.path.dirname(__file__),"../../src")) ###################################################### from rosparam_handler.parameter_generator_catkin import * @@ -24,4 +24,4 @@ gen.add("map_param_wo_default", paramtype="std::map", d gen.add_enum("enum_param_wo_default", description="enum", entry_strings=["Small", "Medium", "Large", "ExtraLarge"]) #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("rosparam_handler", "rosparam_handler_test", "DefaultsAtLaunch")) \ No newline at end of file +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "DefaultsAtLaunch")) diff --git a/test/cfg/DefaultsMissing.params b/test/cfg/DefaultsMissing.params index d722a8c..0f8409d 100755 --- a/test/cfg/DefaultsMissing.params +++ b/test/cfg/DefaultsMissing.params @@ -2,7 +2,7 @@ ####### workaround so that the module is found for test files ####### import sys import os -sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +sys.path.insert(0, os.path.join(os.path.dirname(__file__),"../../src")) ###################################################### from rosparam_handler.parameter_generator_catkin import * @@ -11,4 +11,4 @@ gen = ParameterGenerator() gen.add("int_param", paramtype="int", description="Handler should keep node from starting, when no default is given here or at launch.") #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("rosparam_handler", "rosparam_handler_test", "DefaultsMissing")) \ No newline at end of file +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "DefaultsMissing")) diff --git a/test/cfg/MinMax.params b/test/cfg/MinMax.params index b418d38..c387e97 100755 --- a/test/cfg/MinMax.params +++ b/test/cfg/MinMax.params @@ -2,7 +2,7 @@ ####### workaround so that the module is found ####### import sys import os -sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) +sys.path.insert(0, os.path.join(os.path.dirname(__file__),"../../src")) ###################################################### from rosparam_handler.parameter_generator_catkin import * @@ -18,4 +18,4 @@ gen.add("vector_double_param_w_minmax", paramtype="std::vector", descrip gen.add("map_param_w_minmax", paramtype="std::map", description="A map parameter", default={"value1": -1.2,"value2": 1.2,"value3": 2.2}, min=0., max=2.) #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) -exit(gen.generate("rosparam_handler", "rosparam_handler_test", "MinMax")) \ No newline at end of file +exit(gen.generate("rosparam_handler", "rosparam_handler_test", "MinMax"))