├── .clang-format ├── .gitignore ├── .gitlab-ci.yml ├── CMakeLists.txt ├── CODEOWNERS ├── LICENSE ├── README.md ├── cmake ├── rosinterface_handler-extras.cmake.em ├── rosinterface_handler-macros.cmake └── setup_custom_pythonpath_rosif.sh.in ├── doc ├── HowToUseLongParameters.md ├── HowToUseYourInterfaceStruct.md └── HowToWriteYourFirstInterfaceFile.md ├── include └── rosinterface_handler │ ├── console_bridge_compatibility.hpp │ ├── diagnostic_subscriber.hpp │ ├── simple_node_status.hpp │ ├── smart_subscriber.hpp │ └── utilities.hpp ├── package.xml ├── scripts └── generate_yaml ├── setup.py ├── src └── rosinterface_handler │ ├── __init__.py │ └── interface_generator_catkin.py ├── templates ├── ConfigType.h.template ├── Interface.h.template └── Interface.py.template └── test ├── cfg ├── Defaults.rosif ├── DefaultsAtLaunch.rosif ├── DefaultsMissing.rosif └── MinMax.rosif ├── launch ├── params │ └── test_launch_parameters.yaml ├── rosinterface_handler.test └── rosinterface_handler_python.test ├── python ├── rosinterface_handler_python_test.py └── tests │ ├── __init__.py │ ├── test_defaults.py │ ├── test_defaultsAtLaunch.py │ ├── test_defaultsMissing.py │ └── test_minMax.py └── src ├── test_defaults.cpp ├── test_defaultsAtLaunch.cpp ├── test_defaultsMissing.cpp ├── test_diagnosed_subscriber.cpp ├── test_main.cpp ├── test_minMax.cpp └── test_smart_subscriber.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: LLVM 3 | AccessModifierOffset: -4 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignEscapedNewlinesLeft: true 8 | AlignOperands: true 9 | AlignTrailingComments: true 10 | AllowAllParametersOfDeclarationOnNextLine: true 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortCaseLabelsOnASingleLine: false 13 | AllowShortFunctionsOnASingleLine: None 14 | AllowShortIfStatementsOnASingleLine: false 15 | AllowShortLoopsOnASingleLine: false 16 | AlwaysBreakAfterDefinitionReturnType: None 17 | AlwaysBreakAfterReturnType: None 18 | AlwaysBreakBeforeMultilineStrings: false 19 | AlwaysBreakTemplateDeclarations: true 20 | BinPackArguments: true 21 | BinPackParameters: true 22 | BraceWrapping: 23 | AfterClass: false 24 | AfterControlStatement: false 25 | AfterEnum: false 26 | AfterFunction: false 27 | AfterNamespace: false 28 | AfterObjCDeclaration: false 29 | AfterStruct: false 30 | AfterUnion: false 31 | BeforeCatch: false 32 | BeforeElse: false 33 | IndentBraces: false 34 | BreakBeforeBinaryOperators: None 35 | BreakBeforeBraces: Attach 36 | BreakBeforeTernaryOperators: true 37 | BreakConstructorInitializersBeforeComma: false 38 | ColumnLimit: 120 39 | CommentPragmas: '^ IWYU pragma:' 40 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 41 | ConstructorInitializerIndentWidth: 8 42 | ContinuationIndentWidth: 4 43 | Cpp11BracedListStyle: true 44 | DerivePointerAlignment: false 45 | DisableFormat: false 46 | ExperimentalAutoDetectBinPacking: false 47 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 48 | IncludeCategories: 49 | - Regex: '^<.*\/.*\/.*>' 50 | Priority: 3 51 | - Regex: '^<.*\/.*>' 52 | Priority: 2 53 | - Regex: '^<.*>' 54 | Priority: 1 55 | - Regex: '^".*\/.*\/.*"' 56 | Priority: 6 57 | - Regex: '^".*\/.*"' 58 | Priority: 5 59 | - Regex: '^".*"' 60 | Priority: 4 61 | - Regex: '.*' 62 | Priority: 7 63 | IndentCaseLabels: false 64 | IndentWidth: 4 65 | IndentWrappedFunctionNames: false 66 | KeepEmptyLinesAtTheStartOfBlocks: true 67 | MacroBlockBegin: '' 68 | MacroBlockEnd: '' 69 | MaxEmptyLinesToKeep: 1 70 | NamespaceIndentation: None 71 | PenaltyBreakBeforeFirstCallParameter: 19 72 | PenaltyBreakComment: 300 73 | PenaltyBreakFirstLessLess: 120 74 | PenaltyBreakString: 1000 75 | PenaltyExcessCharacter: 1000000 76 | PenaltyReturnTypeOnItsOwnLine: 2000 77 | PointerAlignment: Left 78 | ReflowComments: true 79 | SortIncludes: true 80 | SpaceAfterCStyleCast: false 81 | SpaceBeforeAssignmentOperators: true 82 | SpaceBeforeParens: ControlStatements 83 | SpaceInEmptyParentheses: false 84 | SpacesBeforeTrailingComments: 1 85 | SpacesInAngles: false 86 | SpacesInContainerLiterals: true 87 | SpacesInCStyleCastParentheses: false 88 | SpacesInParentheses: false 89 | SpacesInSquareBrackets: false 90 | Standard: Cpp11 91 | TabWidth: 4 92 | UseTab: Never 93 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | .idea 3 | CMakeLists.txt.user 4 | *~ 5 | *.autosave 6 | 7 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | #version=1.6 2 | include: 3 | - project: 'pub/mrt_build_config' 4 | ref: master 5 | file: '/ci_templates/default_catkin_project.yml' 6 | 7 | deps: 8 | # we only test one single dependency. Otherwise this would test hundreds of packages. 9 | script: 10 | - mrt ci test-deps -f --no-status -i example_package_ros_tool --only --release $CI_PROJECT_NAME 11 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.0) 2 | project(rosinterface_handler) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | # export compile commands 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS YES) 8 | 9 | catkin_python_setup() 10 | 11 | if (CATKIN_ENABLE_TESTING) 12 | # set compiler flags 13 | find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure diagnostic_updater rostest roscpp message_filters std_msgs) 14 | set(ROSINTERFACE_HANDLER_CMAKE_DIR ${CMAKE_CURRENT_LIST_DIR}/cmake) 15 | include(cmake/rosinterface_handler-macros.cmake) 16 | file(GLOB PROJECT_TEST_FILES_INTERFACE RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/cfg/*.rosif") 17 | generate_ros_interface_files(${PROJECT_TEST_FILES_INTERFACE}) 18 | endif() 19 | 20 | catkin_package( 21 | INCLUDE_DIRS include 22 | CATKIN_DEPENDS catkin 23 | CFG_EXTRAS rosinterface_handler-extras.cmake 24 | ) 25 | 26 | install( 27 | DIRECTORY cmake 28 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 29 | USE_SOURCE_PERMISSIONS 30 | ) 31 | 32 | install( 33 | DIRECTORY templates 34 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 35 | ) 36 | 37 | install( 38 | DIRECTORY include/rosinterface_handler/ 39 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 40 | ) 41 | 42 | install(PROGRAMS scripts/generate_yaml 43 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 44 | ) 45 | 46 | if (CATKIN_ENABLE_TESTING) 47 | file(GLOB PROJECT_TEST_FILES_SRC RELATIVE "${CMAKE_CURRENT_LIST_DIR}" "test/src/*.cpp") 48 | set(TEST_TARGET_NAME "rosinterface_handler_test") 49 | add_rostest_gtest(${TEST_TARGET_NAME} test/launch/rosinterface_handler.test ${PROJECT_TEST_FILES_SRC}) 50 | add_rostest(test/launch/rosinterface_handler_python.test DEPENDENCIES ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 51 | find_package(tf2_ros REQUIRED) 52 | find_package(image_transport REQUIRED) 53 | target_link_libraries(${TEST_TARGET_NAME} ${catkin_LIBRARIES} ${tf2_ros_LIBRARIES} ${image_transport_LIBRARIES} gtest) 54 | target_include_directories(${TEST_TARGET_NAME} PUBLIC include) 55 | target_include_directories(${TEST_TARGET_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ${image_transport_INCLUDE_DIRS}) 56 | # put dir of generated headers at the front. dynamic_reconfigure messes this up (see #173). 57 | target_include_directories(${TEST_TARGET_NAME} SYSTEM BEFORE PUBLIC ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 58 | add_dependencies(${TEST_TARGET_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) 59 | set_property(TARGET ${TEST_TARGET_NAME} PROPERTY CXX_STANDARD 17) 60 | set_property(TARGET ${TEST_TARGET_NAME} PROPERTY CXX_STANDARD_REQUIRED ON) 61 | endif() 62 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * fabian.poggenhans@kit.edu 2 | * kevin.roesch@kit.edu 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017, Claudio Bandera 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 2. Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | 13 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 14 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 15 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 16 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 17 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 18 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 19 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 20 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 21 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 | 24 | The views and conclusions contained in the software and documentation are those 25 | of the authors and should not be interpreted as representing official policies, 26 | either expressed or implied, of the FreeBSD Project. 27 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rosinterface_handler 2 | 3 | ## Package Summary 4 | A unified parameter handler for nodes with automatic code generation. 5 | Save time on defining your parameters, publishers and subscribers. No more redundant code. Easy error checking. Make them dynamic with a single flag. 6 | 7 | - Maintainer status: maintained 8 | - Maintainer: Kevin Rösch 9 | - Author: Claudio Bandera , Fabian Poggenhans 10 | - License: BSD 11 | 12 | 13 | ## Unified Interface Handling for ROS 14 | When working with ROS, there usually a lot unneccessary and error-prone boilerplate code involved when writing new nodes. There are also a couple of tools to to be handled correctly, e.g. [Parameter Server](http://wiki.ros.org/Parameter%20Server) and [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure/). 15 | 16 | With the multitude of options on where to specify your parameters and how to configure your publishers and subscribers, we often face the problem that we get a redundancy in our code and config files. 17 | 18 | The `rosinterface_handler` let's you: 19 | - specify your whole interface to the ROS world in a single file 20 | - automatically create publishers and subscribers 21 | - use a generated struct to hold your parameters 22 | - use member method for grabbing the parameters from the parameter server 23 | - use member method for updating them from dynamic_reconfigure. 24 | - make your parameters configurable with a single flag. 25 | - set default, min and max values 26 | - choose between global and private namespace 27 | - use parameters of type long (64 bit integers) 28 | - save a lot of time on specifying your parameters in several places. 29 | - ...in both C++ and Python 30 | 31 | ## Usage 32 | See the Tutorials on 33 | - [How to write your first .rosif file](doc/HowToWriteYourFirstInterfaceFile.md) 34 | - [How to use your parameter struct](doc/HowToUseYourInterfaceStruct.md) 35 | - [How to use `long` parameters](doc/HowToUseLongParameters.md) 36 | 37 | ## Installation 38 | Download and build this package in your catkin workspace. 39 | 40 | ## Credits 41 | This project is built upon a fork from the great [rosparam_handler](https://github.com/cbandera/rosparam_handler). 42 | -------------------------------------------------------------------------------- /cmake/rosinterface_handler-extras.cmake.em: -------------------------------------------------------------------------------- 1 | # Generated from: rosinterface_handler/cmake/rosinterface_handler-extras.cmake.em 2 | 3 | if (_ROSINTERFACE_HANDLER_EXTRAS_INCLUDED_) 4 | return() 5 | endif () 6 | set(_ROSINTERFACE_HANDLER_EXTRAS_INCLUDED_ TRUE) 7 | 8 | @[if DEVELSPACE]@ 9 | # cmake dir in develspace 10 | set(ROSINTERFACE_HANDLER_CMAKE_DIR "@(CMAKE_CURRENT_SOURCE_DIR)/cmake") 11 | @[else]@ 12 | # cmake dir in installspace 13 | set(ROSINTERFACE_HANDLER_CMAKE_DIR "@(PKG_CMAKE_DIR)") 14 | @[end if]@ 15 | 16 | include(${ROSINTERFACE_HANDLER_CMAKE_DIR}/rosinterface_handler-macros.cmake) 17 | -------------------------------------------------------------------------------- /cmake/rosinterface_handler-macros.cmake: -------------------------------------------------------------------------------- 1 | 2 | macro(generate_ros_interface_files) 3 | set(CFG_FILES "${ARGN}") 4 | set(ROSINTERFACE_HANDLER_ROOT_DIR "${ROSINTERFACE_HANDLER_CMAKE_DIR}/..") 5 | if (${PROJECT_NAME}_CATKIN_PACKAGE) 6 | message(FATAL_ERROR "generate_interface_files() must be called before catkin_package() in project '${PROJECT_NAME}'") 7 | endif () 8 | 9 | # ensure that package destination variables are defined 10 | catkin_destinations() 11 | if(dynamic_reconfigure_FOUND_CATKIN_PROJECT) 12 | add_definitions(-DDYNAMIC_RECONFIGURE_FOUND) 13 | endif() 14 | if(message_filters_FOUND_CATKIN_PROJECT) 15 | add_definitions(-DMESSAGE_FILTERS_FOUND) 16 | endif() 17 | if(diagnostic_updater_FOUND_CATKIN_PROJECT) 18 | add_definitions(-DDIAGNOSTIC_UPDATER_FOUND) 19 | endif() 20 | 21 | set(_autogen "") 22 | foreach (_cfg ${CFG_FILES}) 23 | # Construct the path to the .cfg file 24 | set(_input ${_cfg}) 25 | if (NOT IS_ABSOLUTE ${_input}) 26 | set(_input ${PROJECT_SOURCE_DIR}/${_input}) 27 | endif () 28 | 29 | get_filename_component(_cfgext ${_cfg} EXT) 30 | if( _cfgext STREQUAL ".rosif") 31 | # Define required input files 32 | set(geninterface_build_files 33 | ${ROSINTERFACE_HANDLER_ROOT_DIR}/templates/ConfigType.h.template 34 | ${ROSINTERFACE_HANDLER_ROOT_DIR}/templates/Interface.h.template 35 | ) 36 | 37 | # Define output files 38 | get_filename_component(_cfgonly ${_cfg} NAME_WE) 39 | set(_output_cfg ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/cfg/${_cfgonly}.cfg) 40 | set(_output_cpp ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/${_cfgonly}Interface.h) 41 | set(_output_py ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/interface/${_cfgonly}Interface.py) 42 | 43 | # We need to explicitly add the devel space to the PYTHONPATH 44 | # since it might contain dynamic_reconfigure or Python code of the current package. 45 | set("_CUSTOM_PYTHONPATH_ENV") 46 | if(EXISTS "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION}") 47 | configure_file( 48 | "${ROSINTERFACE_HANDLER_CMAKE_DIR}/setup_custom_pythonpath_rosif.sh.in" 49 | "setup_custom_pythonpath_rosif.sh" 50 | @ONLY 51 | ) 52 | set("_CUSTOM_PYTHONPATH_ENV" "${CMAKE_CURRENT_BINARY_DIR}/setup_custom_pythonpath_rosif.sh") 53 | endif() 54 | 55 | # Create command 56 | assert(CATKIN_ENV) 57 | set(_cmd 58 | ${CATKIN_ENV} 59 | 60 | ${_CUSTOM_PYTHONPATH_ENV} 61 | ${PYTHON_EXECUTABLE} 62 | ${_input} 63 | ${ROSINTERFACE_HANDLER_ROOT_DIR} 64 | ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} 65 | ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION} 66 | ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} 67 | ) 68 | 69 | add_custom_command(OUTPUT 70 | ${_output_cpp} ${_output_cfg} ${_output_py} 71 | COMMAND ${_cmd} 72 | DEPENDS ${_input} ${geninterface_build_files} 73 | COMMENT "Generating interface files from ${_cfgonly}" 74 | ) 75 | 76 | list(APPEND ${PROJECT_NAME}_LOCAL_CFG_FILES "${_output_cfg}") 77 | list(APPEND ${PROJECT_NAME}_interfaces_generated ${_output_cpp} ${_output_cfg} ${_output_py}) 78 | 79 | # make file show up in ides 80 | STRING(REGEX REPLACE "/" "-" IDE_TARGET_NAME ${PROJECT_NAME}-show-cfg-${_cfgonly}) 81 | add_custom_target(${IDE_TARGET_NAME} SOURCES ${_input}) 82 | 83 | install( 84 | FILES ${_output_cpp} 85 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 86 | ) 87 | elseif( _cfgext STREQUAL ".cfg" ) 88 | list(APPEND ${PROJECT_NAME}_LOCAL_CFG_FILES "${_cfg}") 89 | elseif( _cfgext STREQUAL ".param" ) 90 | message(WARNING "Found old .param Files. Rosparam handler and rosinterface handler should not be used in combination!") 91 | else() 92 | message(WARNING "Unknown file ending : ${_cfgext}. Skipping") 93 | endif() 94 | 95 | endforeach (_cfg) 96 | 97 | # geninterface target for hard dependency on generate_interface generation 98 | add_custom_target(${PROJECT_NAME}_geninterface ALL DEPENDS ${${PROJECT_NAME}_interfaces_generated}) 99 | 100 | # register target for catkin_package(EXPORTED_TARGETS) 101 | list(APPEND ${PROJECT_NAME}_EXPORTED_TARGETS ${PROJECT_NAME}_geninterface) 102 | 103 | # make sure we can find generated headers and that they overlay all other includes 104 | include_directories(BEFORE ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 105 | # pass the include directory to catkin_package() 106 | list(APPEND ${PROJECT_NAME}_INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 107 | # ensure that the folder exists 108 | file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}) 109 | #Require C++11 110 | set_property(TARGET ${PROJECT_NAME}_geninterface PROPERTY CXX_STANDARD 11) 111 | set_property(TARGET ${PROJECT_NAME}_geninterface PROPERTY CXX_STANDARD_REQUIRED ON) 112 | 113 | # install python files 114 | install_ros_python_interface_files() 115 | 116 | # generate dynamic reconfigure files 117 | if(dynamic_reconfigure_FOUND_CATKIN_PROJECT) 118 | if(${PROJECT_NAME}_LOCAL_CFG_FILES) 119 | generate_dynamic_reconfigure_options(${${PROJECT_NAME}_LOCAL_CFG_FILES}) 120 | endif() 121 | else() 122 | message(WARNING "Dependency to dynamic_reconfigure is missing, or find_package(dynamic_reconfigure) was not called yet. Not building dynamic config files") 123 | endif() 124 | endmacro() 125 | 126 | macro(install_ros_python_interface_files) 127 | if(NOT install_ros_python_interface_files_CALLED) 128 | set(install_ros_python_interface_files_CALLED TRUE) 129 | 130 | # mark that generate_dynamic_reconfigure_options() was called in order to detect wrong order of calling with catkin_python_setup() 131 | set(${PROJECT_NAME}_GENERATE_DYNAMIC_RECONFIGURE TRUE) 132 | 133 | # check if catkin_python_setup() installs an __init__.py file for a package with the current project name 134 | # in order to skip the installation of a generated __init__.py file 135 | set(package_has_static_sources ${${PROJECT_NAME}_CATKIN_PYTHON_SETUP_HAS_PACKAGE_INIT}) 136 | if(NOT EXISTS ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py) 137 | file(WRITE ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py "") 138 | endif() 139 | if(NOT package_has_static_sources) 140 | # install package __init__.py 141 | install( 142 | FILES ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/__init__.py 143 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} 144 | ) 145 | endif() 146 | 147 | # generate interface module __init__.py 148 | if(NOT EXISTS ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/interface/__init__.py) 149 | file(WRITE ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/interface/__init__.py "") 150 | endif() 151 | 152 | # compile python code before installing 153 | find_package(PythonInterp REQUIRED) 154 | install(CODE "execute_process(COMMAND \"${PYTHON_EXECUTABLE}\" -m compileall \"${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/interface\")") 155 | install( 156 | DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}/interface 157 | DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} 158 | ) 159 | endif() 160 | endmacro() 161 | -------------------------------------------------------------------------------- /cmake/setup_custom_pythonpath_rosif.sh.in: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env sh 2 | # generated from rosinterface_handler/cmake/setup_custom_pythonpath_rosif.sh.in 3 | 4 | PYTHONPATH=@CATKIN_DEVEL_PREFIX@/@CATKIN_GLOBAL_PYTHON_DESTINATION@:$PYTHONPATH 5 | exec "$@" 6 | -------------------------------------------------------------------------------- /doc/HowToUseLongParameters.md: -------------------------------------------------------------------------------- 1 | # How to use `long` parameters 2 | **Description**: This tutorial will tell you how to use `long` parameters (which are not supported by the parameter server). 3 | 4 | **Tutorial Level**: EASY 5 | 6 | 7 | ## Problem 8 | `xmlrpclib.py` as used by `roslaunch` does not support type `long` (64 bit integers, i.e. in `(-2**63,2**63-1)` but outside `(-2**31,2**31-1)`): 9 | 10 | ```xml 11 | 12 | ``` 13 | results in an error. 14 | 15 | ## Background 16 | 17 | see 18 | * https://github.com/python/cpython/blob/3.7/Lib/xmlrpc/client.py#L158 19 | * https://docs.python.org/3.6/library/xmlrpc.client.html#module-xmlrpc.client 20 | * http://xmlrpc.com/spec.md (search for "32-bit") 21 | 22 | ## Workaround 23 | We use the following workaround: 24 | - Add the substring `L` to the `long` parameter, making it a string (in case it is outside the valid integer range), e.g. 25 | ```python 26 | gen.add("long_param", paramtype="int64_t", description="A long parameter", default="12345678910111213L") 27 | # or 28 | gen.add("long_param", paramtype="int64_t", description="A long parameter", default="1L") 29 | # or 30 | gen.add("long_param", paramtype="int64_t", description="A long parameter", default=1) 31 | ``` 32 | 33 | - when setting it via an launchfile argument: 34 | ```xml 35 | 36 | 37 | 38 | 39 | 40 | ``` 41 | 42 | - when setting it via a yaml: 43 | ```yaml 44 | long_param: "12345678910111213L" 45 | # or 46 | long_param: "1L" 47 | # or 48 | long_param: 1 49 | ``` 50 | 51 | - For `long` parameters, a string is automatically cast to a 64bit integer when reading from the parameter server 52 | - Any `long` parameter is stored as `string` when writing to the parameter server 53 | -------------------------------------------------------------------------------- /doc/HowToUseYourInterfaceStruct.md: -------------------------------------------------------------------------------- 1 | # How to Use Your Parameter Struct 2 | **Description**: This tutorial will familiarize you with how you can use the autogenerated interface structs in your nodes. 3 | 4 | **Tutorial Level**: INTERMEDIATE 5 | 6 | ## Generated files 7 | When creating interface files as described in [How to write your first .rosif file](HowToWriteYourFirstInterfaceFile.md), you will end up with the following two files: 8 | - *'include/rosinterface_tutorials/TutorialInterface.h'* 9 | - *'include/rosinterface_tutorials/TutorialConfig.h'* 10 | 11 | The 'Interface.h' file will hold a struct called `Interface`. 12 | The 'Config.h' file will hold the normal dynamic_reconfigure Config struct. 13 | 14 | For your code it is enough to include the \*Interface.h file, e.g. 15 | 16 | ```cpp 17 | #include "rosinterface_tutorials/TutorialInterface.h" 18 | ``` 19 | 20 | You can now add an instance of the interface struct to your class: 21 | 22 | ```cpp 23 | rosinterface_tutorials::TutorialInterface interface_; 24 | ``` 25 | 26 | ## Initializing the struct. 27 | When initializing your node, the interface struct must be initialized with a private NodeHandle. 28 | 29 | 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. 30 | 31 | ```cpp 32 | MyNodeClass::MyNodeClass() 33 | : interface_{ros::NodeHandle("~")} 34 | { 35 | interface_.fromParamServer(); 36 | } 37 | ``` 38 | 39 | If you do not use a class (which you should do though in my opinion), you can create it like so: 40 | ```cpp 41 | rosinterface_tutorials::TutorialInterface interface_{ros::NodeHandle("~")} 42 | interface_.fromParamServer(); 43 | ``` 44 | Note: If you have set the logger level for your node to debug, you will get information on which values have been retrieved. 45 | Note: If you use nodelets, you have to use the `getPrivateNodeHandle()` function instead. 46 | 47 | ## Using dynamic_reconfigure 48 | Your dynamic_reconfigure callback can now look as simple as: 49 | ```cpp 50 | void reconfigureRequest(TutorialConfig& config, uint32_t level) { 51 | interface_.fromConfig(config); 52 | } 53 | ``` 54 | 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. 55 | 56 | You can find a running version of this example code in the [rosinterface_handler_tutorial](https://github.com/cbandera/rosinterface_handler_tutorial)-Repository 57 | 58 | ## Setting parameters on the server 59 | If you change your parameters at runtime from within the code, you can upload the current state of the parameters with 60 | ```cpp 61 | interface_.toParamServer(); 62 | ``` 63 | This will set all non-const parameters with their current value on the ros parameter server. 64 | 65 | ## Setting parameters at launch time 66 | 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. 67 | 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. 68 | Rosinterface handler provides a script, to automatically generates a YAML file for you to use. Calling it will generate a file in your current directory. 69 | ```sh 70 | rosrun rosinterface_handler generate_yaml 71 | ``` 72 | 73 | ## Publisher and subscriber 74 | Publishers and subscribers are already initialized and ready to use. If they are defined to be configruable, the `fromParamServer()` function takes care of updating the topic. 75 | In order to actually use the subscriber, you need to register your message callback(s) once on startup. Keep in mind that subscribers are actually shared pointers: 76 | ```cpp 77 | interface_.my_subscriber->registerCallback(&myCallback); 78 | ``` 79 | 80 | No work is required for publishers. Just use them: 81 | ```cpp 82 | std_msgs::Header my_msg; 83 | interface_.my_publisher.publish(my_msg); 84 | ``` 85 | 86 | ### Synchronized subscribers 87 | Because the subscribers are actually `message_filters::Subscriber`, you can directly use them to create a synchronized subscriber: 88 | ```cpp 89 | message_filters::TimeSynchronizer sync(*interface_.my_subscriber1, *interface_.my_subscriber2, 10); 90 | sync.registerCallback(boost::bind(&callback, _1, _2)); 91 | ``` 92 | 93 | 94 | ### Diagnosed publishers/subscribers 95 | Diagnosed publishers and subscribers work similar to the nomal publisher/subsribers and update their diagnostic status by themselves. 96 | No more action is required on them. However, the `updater` member inside your Interface object has an `update()` function 97 | that needs to be called regularly so that updates are published. Refer to the documentation of `diagnostic_updater` for more information. 98 | 99 | 100 | Calling the `update()` function is **not** necessary if you are using diagnostics with `simplified_status=True`. In that case, the 101 | `nodeStatus` member of the Interface object will do the job for you. 102 | 103 | You are suppoed to use this member in order to share the current state of the node. This could look like this: 104 | ```c++ 105 | interface_.nodeStatus.set(rosinterface_handler::NodeStatus::ERROR, "Something happened!"); 106 | ``` 107 | Remember to also clear the error once your node has recovered by calling `set` with `NodeStatus::OK`. 108 | 109 | ## Python 110 | All your interface definitions are fully available in python nodes as well. Just import the interface file: 111 | ```python 112 | from rosinterface_tutorials.interface.TutorialInterface import TutorialInterface 113 | ``` 114 | 115 | Unlike in C++, the call to fromParamServer is not necessary: 116 | ```python 117 | self.interface = TutorialInterface() 118 | ``` 119 | 120 | And a dynamic_reconfigure callback might look like that: 121 | ```python 122 | def reconfigure_callback(self, config, level): 123 | self.interface.from_config(config) 124 | print("Parameter dummy changed to {}".format(self.interface.dummy)) 125 | ``` 126 | 127 | And a call to set the parameters on the server will look like this: 128 | ```python 129 | self.interface.to_param_server() 130 | ``` 131 | -------------------------------------------------------------------------------- /doc/HowToWriteYourFirstInterfaceFile.md: -------------------------------------------------------------------------------- 1 | # How to Write Your First .rosif File 2 | **Description**: This tutorial will familiarize you with .rosif files that allow you to use rosinterface_handler. 3 | 4 | **Tutorial Level**: INTERMEDIATE 5 | 6 | ## Basic Setup 7 | 8 | To begin lets create a package called rosinterface_tutorials which depends on rospy, roscpp, rosinterface_handler and dynamic_reconfigure. 9 | ```shell 10 | catkin_create_pkg --rosdistro ROSDISTRO rospy roscpp rosinterface_handler dynamic_reconfigure 11 | ``` 12 | Where ROSDISTRO is the ROS distribution you are using. 13 | 14 | Now in your package create a cfg directory, this is where all cfg and rosif files live: 15 | ```shell 16 | mkdir cfg 17 | ``` 18 | Lastly you will need to create a rosif file, for this example we'll call it Tutorials.rosif, and open it in your favorite editor. 19 | 20 | ## The .rosif File 21 | 22 | Add the following to your Tutorials.rosif file: 23 | ```python 24 | #!/usr/bin/env python 25 | from rosinterface_handler.interface_generator_catkin import * 26 | gen = InterfaceGenerator() 27 | 28 | # Parameters with different types 29 | gen.add("int_param", paramtype="int", description="An Integer parameter") 30 | gen.add("double_param", paramtype="double",description="A double parameter") 31 | gen.add("str_param", paramtype="std::string", description="A string parameter", default="Hello World") 32 | gen.add("bool_param", paramtype="bool", description="A Boolean parameter") 33 | gen.add("vector_param", paramtype="std::vector", description="A vector parameter") 34 | gen.add("map_param", paramtype="std::map", description="A map parameter") 35 | 36 | # Default min and max values 37 | gen.add("weight", paramtype="double",description="Weight can not be negative", min=0.0) 38 | gen.add("age", paramtype="int",description="Normal age of a human is inbetween 0 and 100", min=0, max=100) 39 | gen.add("default_param", paramtype="std::string",description="Parameter with default value", default="Hello World") 40 | 41 | # Default vector/map 42 | gen.add("vector_bool", paramtype="std::vector", description="A vector of boolean with default value.", default=[False, True, True, False, True]) 43 | gen.add("map_string_float", paramtype="std::map", description="A map of with default value.", default={"a":0.1, "b":1.2, "c":2.3, "d":3.4, "e":4.5}, min=0, max=5) 44 | 45 | # Constant and configurable parameters 46 | gen.add("optimal_parameter", paramtype="double", description="Optimal parameter, can not be set via rosinterface", default=10, constant=True) 47 | gen.add("configurable_parameter", paramtype="double", description="This parameter can be set via dynamic_reconfigure", configurable=True) 48 | 49 | # Defining the namespace 50 | gen.add("global_parameter", paramtype="std::string", description="This parameter is defined in the global namespace", global_scope=True) 51 | 52 | # Full signature 53 | gen.add("dummy", paramtype="double", description="My Dummy parameter", level=0, 54 | edit_method="", default=5.2, min=0, max=10, configurable=True, 55 | global_scope=False, constant=False) 56 | 57 | # Add an enum: 58 | gen.add_enum("my_enum", description="My first self written enum", 59 | entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium") 60 | 61 | # Add a subgroup 62 | my_group = gen.add_group("my_group") 63 | my_group.add("subparam", paramtype="std::string", description="This parameter is part of a group", configurable=True) 64 | 65 | # Create subscribers/publishers 66 | gen.add_subscriber("my_subscriber", message_type="std_msgs::Header", description="subscriber", configurable=True) 67 | gen.add_publisher("my_publisher", message_type="std_msgs::Header", description="publisher", default_topic="publisher_topic") 68 | 69 | # Logging 70 | gen.add_verbosity_param(configurable=True) 71 | 72 | # TF 73 | gen.add_tf(buffer_name="tf_buffer", listener_name="tf_listener", broadcaster_name="tf_broadcaster") 74 | 75 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 76 | exit(gen.generate("rosinterface_tutorials", "example_node", "Tutorial")) 77 | ``` 78 | 79 | ## Line by line 80 | Now lets break the code down line by line. 81 | 82 | ### Initialization 83 | ```python 84 | #!/usr/bin/env python 85 | from rosinterface_handler.interface_generator_catkin import * 86 | gen = InterfaceGenerator() 87 | ``` 88 | These first lines are pretty simple, they just initialize ros, import and instantiate the parameter generator. 89 | 90 | ### Adding parameters 91 | Now that we have a generator we can start to define parameters. The add function adds a parameter to the list of parameters. It takes a the following mandatory arguments: 92 | 93 | - **name**: a string which specifies the name under which this parameter should be stored 94 | - **paramtype**: defines the type of value stored, and can be any of the primitive types: "int", "double", "std::string", "bool" or a container type using one of the primitive types: "std::vector<...>", "std::map" 95 | - **description**: string which describes the parameter 96 | 97 | Furthermore, following optional arguments can be passed: 98 | - **configurable**: Make this parameter reconfigurable at run time. Default: False 99 | - **global_scope**: Make this parameter live in the global namespace. Default: False 100 | - **level**: A bitmask which will later be passed to the dynamic reconfigure callback. When the callback is called all of the level values for parameters that have been changed are ORed together and the resulting value is passed to the callback. This is only used when *configurable* is set to True. 101 | - **edit_method**: An optional string that is directly passed to dynamic reconfigure. This is only used when *configurable* is set to True. 102 | - **default**: specifies the default value. Can not be set for global parameters. 103 | - **min**: specifies the min value (optional and does not apply to strings and bools) 104 | - **max**: specifies the max value (optional and does not apply to strings and bools) 105 | 106 | ```python 107 | # Parameters with different types 108 | gen.add("int_param", paramtype="int", description="An Integer parameter") 109 | gen.add("double_param", paramtype="double",description="A double parameter") 110 | gen.add("str_param", paramtype="std::string", description="A string parameter", "Hello World") 111 | gen.add("bool_param", paramtype="bool", description="A Boolean parameter") 112 | gen.add("vector_param", paramtype="std::vector", description="A vector parameter") 113 | gen.add("map_param", paramtype="std::map", description="A map parameter") 114 | ``` 115 | 116 | These lines simply define parameters of the different types. Their values will be retrieved from the ros parameter server. 117 | 118 | ```python 119 | # Default min and max values 120 | gen.add("weight", paramtype="double",description="Weight can not be negative", min=0.0) 121 | gen.add("age", paramtype="int",description="Normal age of a human is inbetween 0 and 100", min=0, max=100) 122 | gen.add("default_param", paramtype="std::string",description="Parameter with default value", default="Hello World") 123 | ``` 124 | 125 | These lines add parameters, which either have default values, which means that they wont throw an error if the parameters are not set on the server, or they have constraints on min/max values for the retrieved parameters. These bounds will be enforced, when fetching parameters from the server. 126 | 127 | ```python 128 | # Constant and configurable parameters 129 | gen.add("optimal_parameter", paramtype="double", description="Optimal parameter, can not be set via rosinterface", default=10, constant=True) 130 | gen.add("configurable_parameter", paramtype="double", description="This parameter can be set via dynamic_reconfigure", configurable_parameter=True) 131 | ``` 132 | 133 | These lines define a parameter that is configurable and one that is constant. Configurable means, that the entry will be added to the dynamic_reconfigure config file and can later be changed at runtime. Vectors, maps and global parameters can not be set configurable. 134 | A constant parameter can not be set through the parameter server anymore. This can be useful, if you have found optimal parameters for your setup, which should not be changed by users anymore. 135 | 136 | ```python 137 | # Defining the namespace 138 | gen.add("global_parameter", paramtype="std::string", description="This parameter is defined in the global namespace", global_scope=True) 139 | ``` 140 | 141 | With the global_scope flag, parameters can be defined to live in the global namespace. Normally you should always keep your parameters in the private namespace of a node. Only exception is when several nodes need to get the exact same value for a parameter. (E.g. a common map file) 142 | 143 | ### Adding enums 144 | ```python 145 | # Add an enum: 146 | gen.add_enum("my_enum", description="My first self written enum", 147 | entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium")) 148 | ``` 149 | 150 | By using the add_enum function, an enum for the dynamic_reconfigure window can be easily defined. The entry_strings will also be static parameters of the resulting parameter struct. 151 | 152 | ### Creating groups 153 | ```python 154 | # Add a subgroup 155 | my_group = gen.add_group("my_group") 156 | my_group.add("subparam", paramtype="std::string", description="This parameter is part of a group", configurable=True) 157 | ``` 158 | 159 | With the add_group function, you can sort parameters into groups in the dynamic_reconfigure window. This obviously only makes sense for configurable parameters. 160 | 161 | ### Adding publishers/subscribers 162 | ```python 163 | # Create subscribers/publishers 164 | gen.add_subscriber("my_subscriber", message_type="std_msgs::Header", description="subscriber", configurable=True) 165 | gen.add_publisher("my_publisher", message_type="std_msgs::Header", description="publisher", default_topic="publisher_topic") 166 | ``` 167 | 168 | Using these commands, the rosinterface handler can even do the subscribing and advertising for you. The rosinterface handler makes sure the node is always connected to the right topic. A parameter for the topic and the queue size will be automatically generated for you. 169 | The signature for both commands are very similar. They take the following mandatory elements: 170 | - **name**: Base name of the subscriber/publisher. Will be name of the object in the parameter struct. The topic parameter is then *name*_topic and the queue size *name*_queue_size (unless overriden). 171 | - **message_type**: Type of message including its namespace (e.g. std_msgs::Header). This will also be used to generate the name of the header/module to include (unless overriden). 172 | - **description**: Chose an informative documentation string for this subscriber/publisher. 173 | 174 | The following parameters are optional. Many of them will be automatically deduced from the mandatory parameters: 175 | - **default_topic**: Default topic to subscribe to. If this is an empty string, the subscriber/publisher will initialized in an invalid state. If it is `None` (default), the node will report an error if the topic is not defined by a parameter. 176 | - **default_queue_size**: Default queue size of the subscriber/publisher. 177 | - **no_delay** _(only for add_subscriber)_: Set the tcp_no_delay parameter for subscribing. Recommended for larger messages. 178 | - **topic_param**: Name of the param configuring the topic. Will be "*name*_topic" if None. 179 | - **queue_size_param**: Name of param configuring the queue size. Defaults to "*name*_queue_size". 180 | - **header**: Header name to include. Will be deduced for message type if None. 181 | - **module**: Module to import from (e.g. std_msgs.msg). Will be automatically deduced if None. 182 | - **configurable**: Should the subscribed topic and message queue size be dynamically configurable? 183 | - **scope**: Either *global*, *public* or *private*. A *global* subscriber/publisher will subscribe to `/topic`, 184 | a *public* one to `/namespace/topic` and a *private* one to `/namespace/node_name/topic`. If the topic starts with "/", 185 | the node will always subscribe globally. 186 | - **constant**: If this is true, the parameters will not be fetched from param server, 187 | but the default value is kept. 188 | - **diagnosed**: Enables diagnostics for the subscriber/publisher. Can be configured with the params below. 189 | The message must have a header. Not yet supported for python. 190 | - **min_frequency**: Sets the default minimum frequency for the subscriber/publisher 191 | - **min_frequency_param**: Sets the parameter for the minimum frequency. Defaults to _min_frequency 192 | - **max_delay**: Sets the default maximal header delay for the topics in seconds. 193 | - **max_delay_param**: Parameter for the maximal delay. Defaults to _max_delay. 194 | 195 | To define the topic, just set the topic parameter (usually _topic) to the topic of your dreams in your launch or config file. 196 | 197 | 198 | ### Defining verbosity 199 | ```python 200 | # Logging 201 | gen.add_verbosity_param(configurable=True) 202 | ``` 203 | 204 | Changing the verbosity of nodes to debug (or silence) something can be annoying. 205 | By calling `add_verbosity_param()`, *rosinterface_handler* adds a verbosity parameter (named *verbosity* by default) for you and will automatically set 206 | the verbosity of the node. If you make the parameter *configurable*, you can comfortably select the verbosity in the 207 | *rqt_reconfigure* window. The following parameters are optional: 208 | - **name**: Name of the verbosity parameter. 209 | - **configurable**: Show the verbosity in the *rqt_reconfigure* window. 210 | - **default**: Initial verbosity (can be `debug`, `info`, `warning`, `error` or `fatal`). 211 | ### TF 212 | ```python 213 | gen.add_tf(buffer_name="tf_buffer", listener_name="tf_listener", broadcaster_name="tf_broadcaster") 214 | ``` 215 | 216 | The rosinterface handler can also generate the tf objects for you (your package must depend on tf2_ros, of course). 217 | In the end, your interface object will have three more members: A tf_buffer, a tf_listener and a tf_broadcaster that you can use for handling transformations. These are the supported parameters: 218 | - **buffer_name**: Optional: Name of the tf2_ros::Buffer member in the interface object. Required if listener_name is not `None`. 219 | - **listener_name**: Optional: Name of the tf2_ros::TransformListener member in the interface object. Will not be created if `None` 220 | - **broadcaster_name**: Optional: Name of the tf2_ros::TransformBroadcaster member in the interface object. Will not be created if `None` 221 | 222 | ### Diagnostics 223 | Diagnostics allow you to monitor the status of your node in the context of the whole system without spamming the console (e.g. via `rqt_runtime_monitor`). 224 | 225 | This is enabled via `gen.add_diagnostic_updater(simplified_status=True)` and by adding `diagnostic_updater` as dependency to the package.xml. 226 | 227 | With `simplified_status` enabled, the node will autmatically share its current state. If it is *False*, you have to do that for yourself, but have the opportunity to implement a more fine granular status report. 228 | 229 | #### Diagnosed publishers/subsrcibers 230 | Diagnosed publisher/subscriber are created by passing `diagnosed=True` to the add_subscriber/publisher definition in the interface file. 231 | Before you do this, you must add a line `gen.add_diagnostic_updater()` to your file and not forget to add _diagnostic_updater_ as a dependency to your package. 232 | You can control the expected minimal frequency by setting the respective parameter. The delay of the messages can be monitored like this as well. 233 | 234 | Currently this is not supported for python (the flag is ignored). 235 | 236 | 237 | 238 | ### The final step 239 | 240 | ```python 241 | exit(gen.generate("rosinterface_tutorials", "example_node", "Tutorial")) 242 | ``` 243 | 244 | The last line simply tells the generator to generate the necessary files and exit the program. The second parameter is the name of a node this could run in (used to generate documentation only), the third parameter is a name prefix the generated files will get (e.g. "Config.h" for the dynamic_reconfigure struct and "Parameters.h" for the parameter struct. 245 | 246 | NOTE: The third parameter should be equal to the .rosif file name, without extension. Otherwise the libraries will be generated in every build, forcing a recompilation of the nodes which use them. 247 | 248 | ## Add rosif file to CMakeLists 249 | 250 | In order to make this rosif file usable it must be executable, so lets use the following command to make it excecutable 251 | 252 | ```shell 253 | chmod a+x cfg/Tutorials.rosif 254 | ``` 255 | 256 | Next we need to add the following lines to our CMakeLists.txt. 257 | 258 | ```cmake 259 | #add rosinterface_handler api 260 | find_package(catkin REQUIRED rosinterface_handler dynamic_reconfigure) 261 | 262 | generate_ros_interface_files( 263 | cfg/Tutorials.rosif 264 | cfg/SomeOtherCfgFile.cfg 265 | #... 266 | ) 267 | 268 | # make sure configure headers are built before any node using them 269 | add_dependencies(example_node ${PROJECT_NAME}_gencfg) # For dynamic_reconfigure 270 | add_dependencies(example_node ${PROJECT_NAME}_geninterface) # For rosinterface_handler 271 | ``` 272 | Note: You need a node example_node that is already built, before the add_dependencies line is reached (ref Create a node in C++). 273 | 274 | This will run our rosif file when the package is built. The last thing to do is to build the package and we're done! 275 | 276 | Note: It should be noted here, that you have to pass **all** '.rosif' **and all** '.cfg' files to the generate_parameter_files call. This is because dynamic_reconfigure can only be called once per package. Your normal cfg files will be passed on together with the newly created cfg files. 277 | 278 | For information on how to use the resulting parameter struct in your code, see the next tutorial on [How to use your interface struct](HowToUseYourInterfaceStruct.md). 279 | -------------------------------------------------------------------------------- /include/rosinterface_handler/console_bridge_compatibility.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // delete deprecated, conflicting macros lodDebug, logInfo, logWarn, logError 4 | // see also https://github.com/ros/class_loader/pull/66 5 | #ifdef CLASS_LOADER__CONSOLE_BRIDGE_COMPATIBILITY_HPP_ 6 | #undef logDebug 7 | #undef logInfo 8 | #undef logWarn 9 | #undef logError 10 | #endif 11 | -------------------------------------------------------------------------------- /include/rosinterface_handler/diagnostic_subscriber.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #define IF_HANDLER_DIAGNOSTICS_INCLUDED 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace rosinterface_handler { 10 | //! TopicDiagnostic does not clean up after itself. This wrapper does just that. 11 | class TopicDiagnosticWrapper { 12 | public: 13 | TopicDiagnosticWrapper(std::string name, diagnostic_updater::Updater& diag, 14 | const diagnostic_updater::FrequencyStatusParam& freq, 15 | const diagnostic_updater::TimeStampStatusParam& stamp) 16 | : updater_{diag}, diag_(std::move(name), diag, freq, stamp) { 17 | } 18 | TopicDiagnosticWrapper(TopicDiagnosticWrapper&& rhs) noexcept = delete; 19 | TopicDiagnosticWrapper& operator=(TopicDiagnosticWrapper&& rhs) noexcept = delete; 20 | TopicDiagnosticWrapper(const TopicDiagnosticWrapper& rhs) = delete; 21 | TopicDiagnosticWrapper& operator=(const TopicDiagnosticWrapper& rhs) = delete; 22 | 23 | ~TopicDiagnosticWrapper() { 24 | updater_.removeByName(diag_.getName()); // this is the line we actually need.. 25 | } 26 | 27 | void tick() { 28 | diag_.tick(); 29 | } 30 | 31 | void tick(const ros::Time& stamp) { 32 | diag_.tick(stamp); 33 | } 34 | 35 | const std::string& name() { 36 | return diag_.getName(); 37 | } 38 | 39 | private: 40 | diagnostic_updater::Updater& updater_; 41 | diagnostic_updater::TopicDiagnostic diag_; 42 | }; 43 | 44 | //! Like a message_filters::Subscriber, but also manages diagnostics. 45 | template > 46 | class DiagnosedSubscriber : public SubscriberBase { 47 | static_assert(ros::message_traits::HasHeader::value, 48 | "DiagnosedSubscriber can only be used on messages with a header!"); 49 | using SubscriberT = SubscriberBase; 50 | using MsgPtrT = boost::shared_ptr; 51 | 52 | public: 53 | template 54 | // NOLINTNEXTLINE(readability-identifier-naming) 55 | explicit DiagnosedSubscriber(diagnostic_updater::Updater& updater, Args&&... args) 56 | : SubscriberBase(std::forward(args)...), updater_{updater} { 57 | SubscriberT::registerCallback([this](const MsgPtrT& msg) { this->onMessage(msg); }); 58 | } 59 | 60 | DiagnosedSubscriber& minFrequency(double minFrequency) { 61 | this->minFreq_ = minFrequency; 62 | return *this; 63 | } 64 | DiagnosedSubscriber& maxTimeDelay(double maxTimeDelay) { 65 | this->maxTimeDelay_ = maxTimeDelay; 66 | initDiagnostic(this->getTopic()); 67 | return *this; 68 | } 69 | 70 | void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queueSize, 71 | const ros::TransportHints& transportHints = ros::TransportHints(), 72 | ros::CallbackQueueInterface* callbackQueue = nullptr) override { 73 | SubscriberT::subscribe(nh, topic, queueSize, transportHints, callbackQueue); 74 | initDiagnostic(topic); 75 | } 76 | 77 | void subscribe() override { 78 | SubscriberT::subscribe(); 79 | initDiagnostic(this->getTopic()); 80 | } 81 | 82 | void unsubscribe() override { 83 | SubscriberT::unsubscribe(); 84 | initDiagnostic(""); 85 | } 86 | 87 | private: 88 | void onMessage(const MsgPtrT& msg) { 89 | diagnostic_->tick(msg->header.stamp); 90 | } 91 | 92 | void initDiagnostic(const std::string& name) { 93 | diagnostic_.reset(); 94 | if (name.empty()) { 95 | return; 96 | } 97 | using namespace diagnostic_updater; 98 | // we allow messages from the near future because rosbag play sometimes creates those 99 | constexpr double MinTimeDelay = -0.01; 100 | diagnostic_ = std::make_unique(name + " subscriber", updater_, 101 | FrequencyStatusParam(&minFreq_, &maxFreq_, 0), 102 | TimeStampStatusParam(MinTimeDelay, maxTimeDelay_)); 103 | } 104 | double minFreq_{0.}; 105 | double maxFreq_{std::numeric_limits::infinity()}; 106 | double maxTimeDelay_{0.}; 107 | diagnostic_updater::Updater& updater_; 108 | std::unique_ptr diagnostic_; 109 | }; 110 | 111 | //! Similar to diagnostic_updater::DiagnosedPublisher, but with less segfaults and a simpler interface. Low frequency 112 | //! and delay are only treated as warnings/errors if there are actually subscribers on the advertised topic. 113 | template 114 | class DiagnosedPublisher { 115 | static_assert(ros::message_traits::HasHeader::value, 116 | "DiagnosedPublisher can only be used on messages with a header!"); 117 | using Publisher = diagnostic_updater::DiagnosedPublisher; 118 | class PublisherData { 119 | public: 120 | PublisherData(diagnostic_updater::Updater& updater, const ros::Publisher& publisher, double minFreq, 121 | double maxTimeDelay) 122 | : minFreq_{minFreq}, updater_{&updater}, 123 | publisher_{publisher, updater, diagnostic_updater::FrequencyStatusParam(&minFreq_, &maxFreq_, 0.), 124 | diagnostic_updater::TimeStampStatusParam(0., maxTimeDelay)} { 125 | // We want to control the result of the updater ourselves. Therefore we remove the callback registered by 126 | // the Publisher and replace it with our own callback. 127 | auto name = publisher_.getName(); 128 | updater.removeByName(name); 129 | updater.add(name, [&](diagnostic_updater::DiagnosticStatusWrapper& msg) { 130 | publisher_.run(msg); 131 | if (getNumSubscribers() == 0) { 132 | msg.level = diagnostic_msgs::DiagnosticStatus::OK; 133 | msg.message = "No subscribers; " + msg.message; 134 | } 135 | }); 136 | } 137 | ~PublisherData() { 138 | updater_->removeByName(publisher_.getName()); 139 | } 140 | PublisherData() noexcept = delete; 141 | PublisherData(PublisherData&& rhs) noexcept = delete; 142 | PublisherData& operator=(PublisherData&& rhs) noexcept = delete; 143 | PublisherData(const PublisherData& rhs) = delete; 144 | PublisherData& operator=(const PublisherData& rhs) = delete; 145 | 146 | template 147 | void publish(const T& msg) { 148 | publisher_.publish(msg); 149 | } 150 | 151 | uint32_t getNumSubscribers() const { 152 | return publisher_.getPublisher().getNumSubscribers(); 153 | } 154 | ros::Publisher publisher() const { 155 | return publisher_.getPublisher(); 156 | } 157 | 158 | private: 159 | double minFreq_{0.}; 160 | double maxFreq_{1.e8}; 161 | diagnostic_updater::Updater* updater_{nullptr}; 162 | Publisher publisher_; 163 | }; 164 | 165 | public: 166 | explicit DiagnosedPublisher(diagnostic_updater::Updater& updater) : updater_{&updater} { 167 | } 168 | DiagnosedPublisher() noexcept = default; 169 | DiagnosedPublisher(DiagnosedPublisher&& rhs) noexcept = default; 170 | DiagnosedPublisher& operator=(DiagnosedPublisher&& rhs) noexcept = default; 171 | DiagnosedPublisher(const DiagnosedPublisher& rhs) = default; 172 | DiagnosedPublisher& operator=(const DiagnosedPublisher& rhs) = default; 173 | ~DiagnosedPublisher() noexcept = default; 174 | 175 | DiagnosedPublisher& operator=(const ros::Publisher& publisher) { 176 | reset(publisher); 177 | return *this; 178 | } 179 | 180 | void publish(const boost::shared_ptr& message) { 181 | if (!!publisherData_) { 182 | publisherData_->publish(message); 183 | } 184 | } 185 | 186 | void publish(const MsgT& message) { 187 | if (!!publisherData_) { 188 | publisherData_->publish(message); 189 | } 190 | } 191 | 192 | DiagnosedPublisher& minFrequency(double minFrequency) { 193 | minFreq_ = minFrequency; 194 | if (!!publisherData_) { 195 | reset(publisherData_->publisher()); 196 | } 197 | return *this; 198 | } 199 | 200 | DiagnosedPublisher& maxTimeDelay(double maxTimeDelay) { 201 | maxTimeDelay_ = maxTimeDelay; 202 | if (!!publisherData_) { 203 | reset(publisherData_->publisher()); 204 | } 205 | return *this; 206 | } 207 | 208 | ros::Publisher publisher() const { 209 | if (!!publisherData_) { 210 | return publisherData_->publisher(); 211 | } 212 | return ros::Publisher(); 213 | } 214 | 215 | std::string getTopic() const { 216 | return publisher().getTopic(); 217 | } 218 | 219 | uint32_t getNumSubscribers() const { 220 | return !publisherData_ ? uint32_t() : publisherData_->getNumSubscribers(); 221 | } 222 | 223 | private: 224 | void reset(const ros::Publisher& publisher) { 225 | publisherData_ = std::make_shared(*updater_, publisher, minFreq_, maxTimeDelay_); 226 | } 227 | double minFreq_{0.}; 228 | double maxTimeDelay_{0.}; 229 | diagnostic_updater::Updater* updater_{nullptr}; 230 | std::shared_ptr publisherData_; 231 | }; 232 | } // namespace rosinterface_handler 233 | -------------------------------------------------------------------------------- /include/rosinterface_handler/simple_node_status.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "utilities.hpp" 9 | 10 | namespace rosinterface_handler { 11 | //! Mirrors diagnostic_updater::DiagnosticStatus, but with strong typing 12 | enum class NodeStatus : std::uint8_t { 13 | OK = 0, //!< Everything is fine 14 | WARN = 1, //!< Might be a problem 15 | ERROR = 2, //!< Something is clearly wrong 16 | STALE = 3, //! State unclear (e.g. during initialization) 17 | }; 18 | 19 | //! Simplifies the task of reporting the status of a node. All you have to do is initialize this object and from time to 20 | //! time set the current status 21 | class SimpleNodeStatus { 22 | using StatusWrapper = diagnostic_updater::DiagnosticStatusWrapper; 23 | struct Status { 24 | NodeStatus s; 25 | std::string msg; 26 | bool operator==(const Status& rhs) const { 27 | return s == rhs.s && msg == rhs.msg; 28 | } 29 | bool operator!=(const Status& rhs) const { 30 | return !(*this == rhs); 31 | } 32 | }; 33 | 34 | public: 35 | SimpleNodeStatus(const std::string& statusDescription, const ros::NodeHandle& privNh, 36 | diagnostic_updater::Updater& updater) 37 | // update() only has an effect every second, we need a slightly higher value to get a guaranteed update 38 | : updateStatus_{privNh.createTimer(ros::Duration(1.01), [&](const auto& /*s*/) { updater_->update(); })}, 39 | updater_{&updater} { 40 | updater_->add(statusDescription, [this](StatusWrapper& w) { this->getStatus(w); }); 41 | updater_->force_update(); 42 | } 43 | 44 | //! Lightweight way to set or report a new status. The status remains until overwritten by a new status. 45 | template 46 | void set(NodeStatus s, const Arg& arg, const Args&... Args_) { // NOLINT 47 | bool modified = false; 48 | { 49 | std::lock_guard g{statusMutex_}; 50 | Status newStatus{s, asString(arg, Args_...)}; 51 | modified = status_ != newStatus; 52 | status_ = newStatus; 53 | } 54 | if (s == NodeStatus::ERROR && modified) { 55 | // new errors are reported asap 56 | updater_->force_update(); 57 | } 58 | } 59 | 60 | //! Add/overwrite extra information about the status in form of key/value pairs. The information will be shared 61 | //! along with the overall node status. It remains until explicitly cleared or overwritten. 62 | template 63 | void info(const std::string& name, const Arg& arg, const Args&... Args_) { // NOLINT 64 | std::lock_guard g{statusMutex_}; 65 | extraInfo_[name] = asString(arg, Args_...); 66 | } 67 | 68 | //! Clears previously set information. Returns true on success. 69 | bool clearInfo(const std::string& name) { 70 | std::lock_guard g{statusMutex_}; 71 | auto it = extraInfo_.find(name); 72 | if (it != extraInfo_.end()) { 73 | extraInfo_.erase(it); 74 | return true; 75 | } 76 | return false; 77 | } 78 | 79 | private: 80 | void getStatus(StatusWrapper& w) const { 81 | std::lock_guard g{statusMutex_}; 82 | w.summary(static_cast(status_.s), status_.msg); 83 | for (const auto& info : extraInfo_) { 84 | w.add(info.first, info.second); 85 | } 86 | } 87 | ros::Timer updateStatus_; 88 | diagnostic_updater::Updater* updater_; 89 | mutable std::mutex statusMutex_; 90 | Status status_{NodeStatus::STALE, "Initializing"}; 91 | std::map extraInfo_; 92 | }; 93 | } // namespace rosinterface_handler 94 | -------------------------------------------------------------------------------- /include/rosinterface_handler/smart_subscriber.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rosinterface_handler { 13 | namespace detail { 14 | template 15 | struct Dereference { 16 | static inline constexpr decltype(auto) get(const T& t) { 17 | return t; 18 | } 19 | }; 20 | 21 | template 22 | struct Dereference { 23 | static inline constexpr decltype(auto) get(const T*& t) { 24 | return *t; 25 | } 26 | }; 27 | 28 | template 29 | struct Dereference> { 30 | static constexpr decltype(auto) get(const std::shared_ptr& t) { 31 | return *t; 32 | } 33 | }; 34 | } // namespace detail 35 | /** 36 | * @brief Subscriber that only actually subscribes to a topic if someone subscribes to a publisher 37 | * This is useful to avoid overhead for computing results that no one actually cares for. 38 | * Because this subscriber internally unsubscribes from a topic, upstream nodes are able to stop 39 | * publishing useless results as well. 40 | * 41 | * The smart subscriber can also be used for synchronized subscription via message_filters::TimeSynchronizer or similar. 42 | * 43 | * Set the environment variable NO_SMART_SUBSCRIBE to 1 to disable smart subscriptions. 44 | * 45 | * Usage example: 46 | * @code 47 | * void messageCallback(const std_msgs::Header::ConstPtr& msg) { 48 | * // do the work 49 | * } 50 | * 51 | * // subscribe in your main() or nodelet 52 | * ros::NodeHandle nh; 53 | * ros::Publisher myPub = nh.advertise("/output_topic", 5); 54 | * utils_ros::SmartSubscriber subscriber(myPub); 55 | * subscriber.subscribe(nh, "/header_topic", 5); 56 | * subscriber.addCallback(messageCallback); 57 | * @endcode 58 | */ 59 | template 60 | class SmartSubscriber : public message_filters::Subscriber { 61 | public: 62 | using Publishers = std::vector; 63 | 64 | template 65 | // NOLINTNEXTLINE(readability-identifier-naming) 66 | explicit SmartSubscriber(const PublishersT&... trackedPublishers) { 67 | // check for always-on-mode 68 | const auto* smartSubscribe = std::getenv("NO_SMART_SUBSCRIBE"); 69 | try { 70 | if (smartSubscribe && std::stoi(smartSubscribe) > 0) { 71 | setSmart(false); 72 | } 73 | } catch (const std::invalid_argument&) { 74 | } 75 | ros::SubscriberStatusCallback cb = [this](const ros::SingleSubscriberPublisher& /*s*/) { subscribeCallback(); }; 76 | callback_ = boost::make_shared(cb, cb, alivePtr_, ros::getGlobalCallbackQueue()); 77 | 78 | publisherInfo_.reserve(sizeof...(trackedPublishers)); 79 | using Workaround = int[]; // NOLINT 80 | Workaround{(addPublisher(trackedPublishers), 0)...}; 81 | } 82 | SmartSubscriber(SmartSubscriber&& rhs) noexcept = delete; 83 | SmartSubscriber& operator=(SmartSubscriber&& rhs) noexcept = delete; 84 | SmartSubscriber(const SmartSubscriber& rhs) = delete; 85 | SmartSubscriber& operator=(const SmartSubscriber& rhs) = delete; 86 | ~SmartSubscriber() override { 87 | // void the callback 88 | alivePtr_.reset(); // makes sure no callbacks are called while destructor is running 89 | std::lock_guard m(callbackLock_); 90 | for (auto& pub : publisherInfo_) { 91 | removeCallback(pub.topic); 92 | } 93 | callback_->disconnect_ = +[](const ros::SingleSubscriberPublisher& /*s*/) {}; 94 | callback_->connect_ = +[](const ros::SingleSubscriberPublisher& /*s*/) {}; 95 | } 96 | 97 | /** 98 | * @brief Subscribe to a topic. 99 | * 100 | * Calls the message_filtes::Subscriber's subscribe internally. 101 | * 102 | * @param nh The ros::NodeHandle to use to subscribe. 103 | * @param topic The topic to subscribe to. 104 | * @param queueSize The subscription queue size 105 | * @param transportHints The transport hints to pass along 106 | * @param callbackQueue The callback queue to pass along 107 | */ 108 | void subscribe(ros::NodeHandle& nh, const std::string& topic, uint32_t queueSize, 109 | const ros::TransportHints& transportHints = ros::TransportHints(), 110 | ros::CallbackQueueInterface* callbackQueue = nullptr) override { 111 | message_filters::Subscriber::subscribe(nh, topic, queueSize, transportHints, callbackQueue); 112 | subscribeCallback(); 113 | } 114 | 115 | using message_filters::Subscriber::subscribe; 116 | 117 | /** 118 | * @brief Adds a new publisher to monitor 119 | * @param publisher to look after 120 | * Requires that the publisher has "getTopic" and a "getNumSubscribers" function. 121 | * The SmartSubscriber does *not* manage the publisher and keeps a reference to it. If it goes out of scope, there 122 | * will be trouble. 123 | */ 124 | template 125 | void addPublisher(const Publisher& publisher) { 126 | publisherInfo_.push_back({[&]() { return detail::Dereference::get(publisher).getTopic(); }, 127 | [&]() { return detail::Dereference::get(publisher).getNumSubscribers(); }, 128 | detail::Dereference::get(publisher).getTopic()}); 129 | addCallback(publisherInfo_.back().topic); 130 | 131 | // check for subscribe 132 | if (!this->getTopic().empty()) { 133 | subscribeCallback(); 134 | } 135 | } 136 | 137 | /** 138 | * @brief stops tracking a publisher. 139 | * Does nothing if the publisher does not exist. 140 | * @return true if publisher existed and was removed 141 | */ 142 | bool removePublisher(const std::string& topic) { 143 | // remove from vector 144 | auto found = std::find_if(publisherInfo_.begin(), publisherInfo_.end(), 145 | [&](const auto& pubInfo) { return topic == pubInfo.getTopic(); }); 146 | if (found == publisherInfo_.end()) { 147 | return false; 148 | } 149 | publisherInfo_.erase(found); 150 | removeCallback(topic); 151 | return true; 152 | } 153 | 154 | /** 155 | * @brief updates the topics of the tracked subscribers 156 | * This can be necessary if these have changed through a reconfigure request 157 | */ 158 | // NOLINTNEXTLINE(readability-function-size) 159 | void updateTopics() { 160 | for (auto& publisher : publisherInfo_) { 161 | const auto currTopic = publisher.getTopic(); 162 | if (currTopic != publisher.topic) { 163 | ROS_DEBUG_STREAM("Publication moved from " << publisher.topic << " to " << currTopic); 164 | addCallback(currTopic); 165 | removeCallback(publisher.topic); 166 | publisher.topic = currTopic; 167 | } 168 | } 169 | subscribeCallback(); 170 | } 171 | 172 | /** 173 | * @brief returns whether this subsciber is currently subscribed to something 174 | * @return true if subscribed 175 | */ 176 | bool isSubscribed() const { 177 | return bool(this->getSubscriber()); 178 | } 179 | 180 | //! Since this subscriber subscribes automatically, it can not be disabled using unsubscribe(). This function 181 | //! disables him so that the message callback will no longer be called, no matter how many subscribers there are. 182 | void disable() { 183 | disabled_ = true; 184 | if (isSubscribed()) { 185 | this->unsubscribe(); 186 | } 187 | } 188 | 189 | //! Puts a disabled subscriber back into normal mode. 190 | void enable() { 191 | disabled_ = false; 192 | subscribeCallback(); 193 | } 194 | 195 | //! Returns whether this subscriber has been disabled via disable() 196 | bool isDisabled() const { 197 | return disabled_; 198 | } 199 | 200 | /** 201 | * @brief returns whether this subscriber is currently in smart mode 202 | * @return true if in smart mode 203 | * If the subscriber is not in smart mode, it will behave like a normal ros publisher and will always be subscribed 204 | * unless isDisabled is true. 205 | */ 206 | bool smart() const { 207 | return smart_; 208 | } 209 | 210 | /** 211 | * @brief enable/disable smart mode 212 | * @param smart new mode for subscriber 213 | */ 214 | void setSmart(bool smart) { 215 | smart_ = smart; 216 | subscribeCallback(); 217 | } 218 | 219 | /** 220 | * @brief pass this callback to all non-standard publisher that you have 221 | * @return subscriber callback of this SmartSubscriber 222 | */ 223 | const ros::SubscriberCallbacksPtr& callback() const { 224 | return callback_; 225 | } 226 | 227 | /** 228 | * @brief checks for new subscribers and subscribes or unsubscribes if anything changed. 229 | * This function is not supposed to be called actively, it is only here so that you can pass it as callback to any 230 | * special publisher 231 | * (like image transport) 232 | */ 233 | // NOLINTNEXTLINE(readability-function-size) 234 | void subscribeCallback() { 235 | std::lock_guard m(callbackLock_); 236 | if (disabled_ || !alivePtr_) { 237 | return; 238 | } 239 | const auto subscribed = isSubscribed(); 240 | bool subscribe = !smart() || std::any_of(publisherInfo_.begin(), publisherInfo_.end(), 241 | [](auto& p) { return p.getNumSubscriber() > 0; }); 242 | 243 | if (subscribe && !subscribed) { 244 | ROS_DEBUG_STREAM("Got new subscribers. Subscribing to " << this->getSubscriber().getTopic()); 245 | this->subscribe(); 246 | } 247 | if (!subscribe && subscribed) { 248 | ROS_DEBUG_STREAM("No subscribers found. Unsubscribing from " << this->getSubscriber().getTopic()); 249 | this->unsubscribe(); 250 | } 251 | } 252 | 253 | private: 254 | // NOLINTNEXTLINE(readability-function-size) 255 | void addCallback(const std::string& topic) { 256 | if (topic.empty()) { 257 | return; 258 | } 259 | auto pub = ros::TopicManager::instance()->lookupPublication(topic); 260 | if (!!pub) { 261 | pub->addCallbacks(callback_); 262 | } else { 263 | ROS_DEBUG_STREAM("Publication not found for topic " << topic); 264 | } 265 | } 266 | 267 | void removeCallback(const std::string& topic) { 268 | auto pub = ros::TopicManager::instance()->lookupPublication(topic); 269 | if (!!pub) { 270 | pub->removeCallbacks(callback_); 271 | } 272 | } 273 | 274 | struct PublisherInfo { 275 | std::function getTopic; 276 | std::function getNumSubscriber; 277 | std::string topic; 278 | }; 279 | std::vector publisherInfo_; 280 | boost::shared_ptr alivePtr_{boost::make_shared()}; 281 | ros::SubscriberCallbacksPtr callback_; 282 | std::mutex callbackLock_{}; 283 | bool smart_{true}; 284 | bool disabled_{false}; 285 | }; 286 | 287 | template 288 | using SmartSubscriberPtr = std::shared_ptr>; 289 | } // namespace rosinterface_handler 290 | -------------------------------------------------------------------------------- /include/rosinterface_handler/utilities.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | /// \brief Helper function to test for std::vector 11 | template 12 | using IsVector = std::is_same>; 13 | 14 | /// \brief Helper function to test for std::map 15 | template 16 | using IsMap = std::is_same< 17 | T, std::map>; 18 | 19 | /// \brief Outstream helper for std:vector 20 | template 21 | std::ostream& operator<<(std::ostream& out, const std::vector& v) { 22 | if (!v.empty()) { 23 | out << '['; 24 | std::copy(v.begin(), v.end(), std::ostream_iterator(out, ", ")); 25 | out << "\b\b]"; 26 | } 27 | return out; 28 | } 29 | 30 | /// \brief Outstream helper for std:map 31 | template 32 | std::ostream& operator<<(std::ostream& stream, const std::map& map) { 33 | stream << '{'; 34 | for (auto it = map.begin(); it != map.end(); ++it) { 35 | stream << (*it).first << " --> " << (*it).second << ", "; 36 | } 37 | stream << '}'; 38 | return stream; 39 | } 40 | 41 | namespace rosinterface_handler { 42 | 43 | /// \brief Retrieve node name 44 | /// 45 | /// @param privateNodeHandle The private ROS node handle (i.e. 46 | /// ros::NodeHandle("~") ). 47 | /// @return node name 48 | inline std::string getNodeName(const ros::NodeHandle& privateNodeHandle) { 49 | std::stringstream tempString(privateNodeHandle.getNamespace()); 50 | std::string name; 51 | while (std::getline(tempString, name, '/')) { 52 | ; 53 | } 54 | return name; 55 | } 56 | 57 | /// \brief Retrieve the parent node handle from a node handle 58 | /// 59 | /// @param privateNodeHandle Any ROS node handle (e.g. 60 | /// ros::NodeHandle("~") ). 61 | /// @return parent namespace (or empty, is there is no parent) 62 | inline std::string getParentNamespace(const ros::NodeHandle& nodeHandle) { 63 | const auto& nameSpace = nodeHandle.getNamespace(); 64 | return nameSpace.substr(0, nameSpace.find_last_of('/')); 65 | } 66 | 67 | /// \brief Sets the logger level according to a standardized parameter name 'verbosity'. 68 | /// 69 | /// \param nodeHandle The ROS node handle to search for the parameter 'verbosity'. 70 | // NOLINTNEXTLINE 71 | inline void setLoggerLevel(const ros::NodeHandle& nodeHandle, const std::string& verbosityParam = "verbosity", 72 | const std::string& loggerName = "") { 73 | 74 | std::string verbosity; 75 | if (!nodeHandle.getParam(verbosityParam, verbosity)) { 76 | verbosity = "warning"; 77 | } 78 | 79 | auto levelRos = ros::console::levels::Info; 80 | auto validVerbosity = true; 81 | if (verbosity == "debug") { 82 | levelRos = ros::console::levels::Debug; 83 | } else if (verbosity == "info") { 84 | levelRos = ros::console::levels::Info; 85 | } else if (verbosity == "warning" || verbosity == "warn") { 86 | levelRos = ros::console::levels::Warn; 87 | } else if (verbosity == "error") { 88 | levelRos = ros::console::levels::Error; 89 | } else if (verbosity == "fatal") { 90 | levelRos = ros::console::levels::Fatal; 91 | } else { 92 | ROS_WARN_STREAM("Invalid verbosity level specified: " << verbosity << "! Falling back to INFO."); 93 | validVerbosity = false; 94 | } 95 | if (validVerbosity) { 96 | if (!loggerName.empty() && 97 | ros::console::set_logger_level(std::string(ROSCONSOLE_NAME_PREFIX) + "." + loggerName, levelRos)) { 98 | ros::console::notifyLoggerLevelsChanged(); 99 | ROS_DEBUG_STREAM_NAMED(loggerName, "Verbosity set to " << verbosity); 100 | } 101 | // If this is a node, additionally set the default logger, so that ROS_LOG works 102 | if ((loggerName.empty() || ros::NodeHandle("~").getNamespace() == loggerName) && 103 | ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, levelRos)) { 104 | ros::console::notifyLoggerLevelsChanged(); 105 | ROS_DEBUG_STREAM("Verbosity set to " << verbosity); 106 | } 107 | } 108 | } 109 | 110 | /// \brief Show summary about node containing name, namespace, subscribed and advertised topics. 111 | [[deprecated("It doesn't work well on nodelets. Use interfaceObject.showNodeInfo() instead!")]] inline void 112 | showNodeInfo() { 113 | 114 | using namespace ros::this_node; 115 | 116 | std::vector subscribedTopics; 117 | std::vector advertisedTopics; 118 | getSubscribedTopics(subscribedTopics); 119 | getAdvertisedTopics(advertisedTopics); 120 | 121 | std::ostringstream msgSubscr; 122 | std::ostringstream msgAdvert; 123 | for (auto const& t : subscribedTopics) { 124 | msgSubscr << t << std::endl; 125 | } 126 | for (auto const& t : advertisedTopics) { 127 | msgAdvert << t << std::endl; 128 | } 129 | 130 | ROS_INFO_STREAM("Started '" << getName() << "' in namespace '" << getNamespace() << "'." << std::endl 131 | << "Subscribed topics: " << std::endl 132 | << msgSubscr.str() << "Advertised topics: " << std::endl 133 | << msgAdvert.str()); 134 | } 135 | 136 | /// \brief Retrieve the topic to subscribe to (aware of global topic names) 137 | /// 138 | /// @param name_space Parent namespace (with trailing "/") 139 | /// @param topic Global or local topic 140 | /// @return name_space + topic or topic if topic is global 141 | inline std::string getTopic(const std::string& nameSpace, const std::string& topic) { 142 | if (topic.empty() || topic[0] == '/') { 143 | return topic; 144 | } 145 | return nameSpace + topic; 146 | } 147 | 148 | /// \brief ExitFunction for rosinterface_handler 149 | inline void exit(const std::string& msg = "Runtime Error in rosinterface handler.") { 150 | // std::exit(EXIT_FAILURE); 151 | throw std::runtime_error(msg); 152 | } 153 | 154 | /// \brief Set parameter on ROS parameter server 155 | /// 156 | /// \param key Parameter name 157 | /// \param val Parameter value 158 | template 159 | inline void setParam(const std::string& key, T val) { 160 | ros::param::set(key, val); 161 | } 162 | 163 | /// \brief Set parameter on ROS parameter server (Overload for long, since long cannot be saved to the parameterserver 164 | /// directly, but a workaround with strings is needed) 165 | inline void setParam(const std::string& key, int64_t val) { 166 | std::string valString = std::to_string(val) + std::string("L"); 167 | ros::param::set(key, valString); 168 | } 169 | 170 | /// \brief Get parameter from ROS parameter server 171 | /// 172 | /// \param key Parameter name 173 | /// \param val Parameter value 174 | template 175 | // NOLINTNEXTLINE(readability-function-size) 176 | inline bool getParamIncludingLong(const std::string& key, T& val) { 177 | return ros::param::get(key, val); 178 | } 179 | 180 | /// \brief Get parameter from ROS parameter server (Overload for long, since long cannot be stored in the 181 | /// parameterserver directly, but a workaround with strings is needed) 182 | // NOLINTNEXTLINE(readability-function-size) 183 | inline bool getParamIncludingLong(const std::string& key, int64_t& val) { 184 | int valInt; 185 | if (ros::param::get(key, valInt)) { 186 | val = valInt; 187 | return true; 188 | } 189 | 190 | std::string valString; 191 | if (!ros::param::get(key, valString)) { 192 | ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it follow the long convention?"); 193 | return false; 194 | } 195 | 196 | try { 197 | size_t pos = valString.find('L'); 198 | if (pos != std::string::npos) { 199 | // If found then erase it 200 | valString.erase(pos, 1); 201 | } 202 | val = std::stol(valString); 203 | } catch (std::invalid_argument& e) { 204 | ROS_ERROR_STREAM("Could not retrieve parameter'" << key 205 | << "'. Does it have a different type? Error: " << e.what()); 206 | return false; 207 | } 208 | return true; 209 | } 210 | 211 | /// \brief Get parameter from ROS parameter server quietly 212 | /// 213 | /// \param key Parameter name 214 | /// \param val Parameter value 215 | template 216 | // NOLINTNEXTLINE(readability-function-size) 217 | inline bool getParamImpl(const std::string key, T& val) { 218 | if (!ros::param::has(key)) { 219 | return false; 220 | } 221 | if (!getParamIncludingLong(key, val)) { 222 | ROS_ERROR_STREAM("Could not retrieve parameter'" << key << "'. Does it have a different type?"); 223 | return false; 224 | } 225 | // Param was already retrieved with last if statement. 226 | return true; 227 | } 228 | 229 | /// \brief Get parameter from ROS parameter server or print error 230 | /// 231 | /// \param key Parameter name 232 | /// \param val Parameter value 233 | template 234 | // NOLINTNEXTLINE(readability-function-size) 235 | inline bool getParam(const std::string key, T& val) { 236 | if (!getParamImpl(key, val)) { 237 | ROS_ERROR_STREAM("Parameter '" << key << "' is not defined."); 238 | return false; 239 | } 240 | // Param was already retrieved with last if statement. 241 | return true; 242 | } 243 | 244 | /// \brief Get parameter from ROS parameter server or use default value 245 | /// 246 | /// If parameter does not exist on server yet, the default value is used and set on server. 247 | /// \param key Parameter name 248 | /// \param val Parameter value 249 | /// \param defaultValue Parameter default value 250 | template 251 | // NOLINTNEXTLINE(readability-function-size) 252 | inline bool getParam(const std::string& key, T& val, const T& defaultValue) { 253 | if (!getParamImpl(key, val)) { 254 | val = defaultValue; 255 | setParam(key, defaultValue); 256 | ROS_INFO_STREAM("Parameter '" << key << "' is not defined. Setting default value."); 257 | return true; 258 | } 259 | // Param was already retrieved with last if statement. 260 | return true; 261 | } 262 | 263 | /// \brief Tests that parameter is not set on the parameter server 264 | // NOLINTNEXTLINE(readability-function-size) 265 | inline bool testConstParam(const std::string& key) { 266 | if (ros::param::has(key)) { 267 | ROS_WARN_STREAM("Parameter " << key 268 | << "' was set on the parameter server eventhough it was defined to be constant."); 269 | return false; 270 | } 271 | return true; 272 | } 273 | 274 | /// \brief Limit parameter to lower bound if parameter is a scalar. 275 | /// 276 | /// \param key Parameter name 277 | /// \param val Parameter value 278 | /// \param min Lower Threshold 279 | template 280 | // NOLINTNEXTLINE(readability-function-size) 281 | inline void testMin(const std::string key, T& val, T min = std::numeric_limits::min()) { 282 | if (val < min) { 283 | ROS_WARN_STREAM("Value of " << val << " for " << key 284 | << " is smaller than minimal allowed value. Correcting value to min=" << min); 285 | val = min; 286 | } 287 | } 288 | 289 | /// \brief Limit parameter to lower bound if parameter is a vector. 290 | /// 291 | /// \param key Parameter name 292 | /// \param val Parameter value 293 | /// \param min Lower Threshold 294 | template 295 | inline void testMin(const std::string key, std::vector& val, T min = std::numeric_limits::min()) { 296 | for (auto& v : val) { 297 | testMin(key, v, min); 298 | } 299 | } 300 | 301 | /// \brief Limit parameter to lower bound if parameter is a map. 302 | /// 303 | /// \param key Parameter name 304 | /// \param val Parameter value 305 | /// \param min Lower Threshold 306 | template 307 | inline void testMin(const std::string key, std::map& val, T min = std::numeric_limits::min()) { 308 | for (auto& v : val) { 309 | testMin(key, v.second, min); 310 | } 311 | } 312 | 313 | /// \brief Limit parameter to upper bound if parameter is a scalar. 314 | /// 315 | /// \param key Parameter name 316 | /// \param val Parameter value 317 | /// \param min Lower Threshold 318 | template 319 | // NOLINTNEXTLINE(readability-function-size) 320 | inline void testMax(const std::string key, T& val, T max = std::numeric_limits::max()) { 321 | if (val > max) { 322 | ROS_WARN_STREAM("Value of " << val << " for " << key 323 | << " is greater than maximal allowed. Correcting value to max=" << max); 324 | val = max; 325 | } 326 | } 327 | 328 | /// \brief Limit parameter to upper bound if parameter is a vector. 329 | /// 330 | /// \param key Parameter name 331 | /// \param val Parameter value 332 | /// \param min Lower Threshold 333 | template 334 | inline void testMax(const std::string key, std::vector& val, T max = std::numeric_limits::max()) { 335 | for (auto& v : val) { 336 | testMax(key, v, max); 337 | } 338 | } 339 | 340 | /// \brief Limit parameter to upper bound if parameter is a map. 341 | /// 342 | /// \param key Parameter name 343 | /// \param val Parameter value 344 | /// \param min Lower Threshold 345 | template 346 | inline void testMax(const std::string key, std::map& val, T max = std::numeric_limits::max()) { 347 | for (auto& v : val) { 348 | testMax(key, v.second, max); 349 | } 350 | } 351 | 352 | /// \brief Convert at least one argument to a string 353 | /// \tparam Arg Type of required argument 354 | /// \tparam Args Type of additional arguments (optional) 355 | /// \param arg Required argument 356 | /// \param args Additional arguments (optional) 357 | /// \return 358 | template 359 | inline std::string asString(Arg&& arg, Args&&... Args_) { // NOLINT 360 | std::ostringstream oss; 361 | oss << std::forward(arg); 362 | (oss << ... << std::forward(Args_)); 363 | return oss.str(); 364 | } 365 | 366 | inline std::string asString(std::string&& arg) { 367 | return std::move(arg); 368 | } 369 | 370 | inline const std::string& asString(const std::string& arg) { 371 | return arg; 372 | } 373 | 374 | } // namespace rosinterface_handler 375 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rosinterface_handler 4 | 0.2.1 5 | An easy toolbox to generate interfaces from a node to the ROS world. 6 | BSD 7 | 8 | Kevin Rösch 9 | Claudio Bandera 10 | https://github.com/cbandera/rosinterface_handler.git 11 | 12 | catkin 13 | catkin 14 | rostest 15 | roscpp 16 | dynamic_reconfigure 17 | message_filters 18 | std_msgs 19 | geometry_msgs 20 | diagnostic_updater 21 | tf2_ros 22 | image_transport 23 | 24 | 25 | -------------------------------------------------------------------------------- /scripts/generate_yaml: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import os 4 | from rosinterface_handler.interface_generator_catkin import YamlGenerator 5 | 6 | 7 | def generate_yaml(in_file): 8 | 9 | if not os.path.exists(in_file): 10 | sys.exit('ERROR: Param file %s was not found!' % in_file) 11 | # Read argument 12 | in_file = os.path.abspath(in_file) 13 | basename = os.path.basename(in_file) 14 | basename, _ = os.path.splitext(basename) 15 | out_file = os.path.join(os.getcwd(), basename + "Parameters.yaml") 16 | print("Yaml file is written to: {}".format(out_file)) 17 | 18 | # Mock call arguments for call to InterfaceGenerator 19 | sys.argv[1:4] = ["", "", "", ""] 20 | # Redirect import statement 21 | sys.modules['rosinterface_handler.interface_generator_catkin'] = __import__('sys') 22 | 23 | # Execute param file in mocked environment 24 | global_vars = {"__file__": in_file, 'InterfaceGenerator': YamlGenerator} 25 | return execfile(in_file, global_vars) 26 | 27 | 28 | if __name__ == "__main__": 29 | if len(sys.argv) < 2: 30 | sys.exit('Usage: %s <.params file>' % sys.argv[0]) 31 | 32 | generate_yaml(sys.argv[1]) 33 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['rosinterface_handler'], 8 | package_dir={'': 'src'}, 9 | requires=[] 10 | ) 11 | 12 | setup(**d) 13 | -------------------------------------------------------------------------------- /src/rosinterface_handler/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KIT-MRT/rosinterface_handler/521e280ff6990256db63b11d523a846aa4802443/src/rosinterface_handler/__init__.py -------------------------------------------------------------------------------- /src/rosinterface_handler/interface_generator_catkin.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016, Claudio Bandera 2 | # All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # * Redistributions of source code must retain the above copyright 7 | # notice, this list of conditions and the following disclaimer. 8 | # * Redistributions in binary form must reproduce the above copyright 9 | # notice, this list of conditions and the following disclaimer in the 10 | # documentation and/or other materials provided with the distribution. 11 | # * Neither the name of the organization nor the 12 | # names of its contributors may be used to endorse or promote products 13 | # derived from this software without specific prior written permission. 14 | # 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | # 26 | # Author: Claudio Bandera 27 | # 28 | 29 | from __future__ import print_function 30 | from string import Template 31 | import sys 32 | import os 33 | import re 34 | import subprocess 35 | 36 | 37 | def eprint(*args, **kwargs): 38 | print("************************************************", file=sys.stderr, **kwargs) 39 | print("Error when setting up parameter '{}':".format(args[0]), file=sys.stderr, **kwargs) 40 | print(*args[1:], file=sys.stderr, **kwargs) 41 | print("************************************************", file=sys.stderr, **kwargs) 42 | sys.exit(1) 43 | 44 | 45 | class InterfaceGenerator(object): 46 | """Automatic config file and header generator""" 47 | 48 | def __init__(self, parent=None, group=""): 49 | """Constructor for InterfaceGenerator""" 50 | self.enums = [] 51 | self.parameters = [] 52 | self.subscribers = [] 53 | self.publishers = [] 54 | self.childs = [] 55 | self.tf = None 56 | self.verbosity = None 57 | self.parent = parent 58 | self.diagnostics_enabled = False 59 | self.simplified_diagnostics = False 60 | if group: 61 | self.group = group 62 | else: 63 | self.group = "gen" 64 | self.group_variable = "".join(filter(str.isalnum, self.group)) 65 | 66 | if len(sys.argv) != 5: 67 | eprint("InterfaceGenerator: Unexpected arguments, did you call this directly? You shouldn't do this!") 68 | 69 | self.dynconfpath = sys.argv[1] 70 | self.share_dir = sys.argv[2] 71 | self.cpp_gen_dir = sys.argv[3] 72 | self.py_gen_dir = sys.argv[4] 73 | 74 | self.pkgname = None 75 | self.nodename = None 76 | self.classname = None 77 | 78 | def add_verbosity_param(self, name='verbosity', default='info', configurable=False): 79 | """ 80 | Adds a parameter that will define the logging verbosity of the node. The verbosity can either be 'debug', 81 | 'info', 'warning', 'error' or 'fatal'. 82 | The verbosity will be updated automatically by fromParamServer() or fromConfig(). 83 | :param name: (optional) Name of the parameter that will define the verbosity 84 | :param default: (optional) Default verbosity 85 | :param configurable: (optional) Should the topic name and message queue size be dynamically configurable? 86 | :return: 87 | """ 88 | if self.verbosity: 89 | eprint("add_verbosity_param has already been called. You can only add one verbosity parameter per file!") 90 | if self.parent: 91 | eprint("You can not call add_verbosity_param on a group! Call it on the main parameter generator instead!") 92 | self.verbosity = name 93 | if configurable: 94 | self.add_enum(name, 'Sets the verbosity for this node', 95 | entry_strings=['debug', 'info', 'warning', 'error', 'fatal'], 96 | default=default, 97 | paramtype='std::string') 98 | else: 99 | self.add(name, description='Sets the verbosity for this node', paramtype='std::string', default=default) 100 | 101 | def add_diagnostic_updater(self, simplified_status=False): 102 | """ 103 | Adds a diagnostic updater to the interface struct. Make sure your project depends on the diagnostic_updater 104 | package. Must be called before adding any diagnostic-enabled publishers/subscribers. 105 | Unless simplified_status is true, the node must ensure to regularly call interface.updater.update(). 106 | :param simplified_status: Enables simplified status updates, 107 | e.g. interface.nodeStatus.set(NodeStatus::Error, "something is wrong"); 108 | :return: 109 | """ 110 | if self.parent: 111 | eprint("You can't call add_diagnostic_updater on a group! Call it on the main parameter generator instead!") 112 | self.diagnostics_enabled = True 113 | self.simplified_diagnostics = simplified_status 114 | 115 | def add_tf(self, buffer_name="tf_buffer", listener_name="tf_listener", broadcaster_name=None): 116 | """ 117 | Adds tf transformer/broadcaster as members to the interface object. Don't forget to depend on tf2_ros. 118 | :param buffer_name: Name of the tf2_ros::Buffer member in the interface object 119 | :param listener_name: Name of the tf2_ros::TransformListener member in the interface object. 120 | Will not be created if none. 121 | :param broadcaster_name: Name of the tf2_ros::TransformBroadcaster member in the interface object. 122 | Will not be created if none. 123 | """ 124 | if self.parent: 125 | eprint("You can not call add_tf on a group! Call it on the main parameter generator instead!") 126 | if listener_name and not buffer_name: 127 | eprint("If you specify a tf listener, the tf buffer can not be empty!") 128 | self.tf = {"buffer_name": buffer_name, "listener_name": listener_name, "broadcaster_name": broadcaster_name} 129 | 130 | def add_group(self, name): 131 | """ 132 | Add a new group in the dynamic reconfigure selection 133 | :param name: name of the new group 134 | :return: a new group object that you can add parameters to 135 | """ 136 | if not name: 137 | eprint("You have added a group with an empty group name. This is not supported!") 138 | child = InterfaceGenerator(self, name) 139 | self.childs.append(child) 140 | return child 141 | 142 | def add_enum(self, name, description, entry_strings, default=None, paramtype='int'): 143 | """ 144 | Add an enum to dynamic reconfigure 145 | :param name: Name of enum parameter 146 | :param description: Informative documentation string 147 | :param entry_strings: Enum entries, must be strings! (will be numbered with increasing value) 148 | :param default: Default value 149 | :param paramtype: 'int' or 'std::string'; defines if selecting an entry will set the parameter to the name of 150 | an entry or to an int (enumerated from zero in the order of the list) 151 | :return: 152 | """ 153 | if paramtype != 'int' and paramtype != 'std::string': 154 | eprint("Only 'int' or 'std::string' is supported as paramtype argument for add_enum!") 155 | 156 | entry_strings = [str(e) for e in entry_strings] # Make sure we only get strings 157 | if default is None: 158 | pass 159 | elif paramtype == 'int': 160 | default = entry_strings.index(default) 161 | elif default not in entry_strings: 162 | eprint("add_enum: Default value not found in entry_strings!") 163 | 164 | self.add(name=name, paramtype=paramtype, description=description, edit_method=name, default=default, 165 | configurable=True) 166 | for e in entry_strings: 167 | enum_val = entry_strings.index(e) if paramtype == 'int' else e 168 | self.add(name=name + "_" + e, paramtype=paramtype, description="Constant for enum {}".format(name), 169 | default=enum_val, constant=True) 170 | self.enums.append({'name': name, 'description': description, 'values': entry_strings, 'paramtype': paramtype}) 171 | 172 | def add_subscriber(self, name, message_type, description, default_topic=None, default_queue_size=5, no_delay=False, 173 | topic_param=None, queue_size_param=None, header=None, module=None, configurable=False, 174 | scope='private', constant=False, diagnosed=False, min_frequency=0., min_frequency_param=None, 175 | max_delay=float('inf'), max_delay_param=None, watch=[]): 176 | """ 177 | Adds a subscriber to your parameter struct and a parameter for its topic and queue size. Don't forget to add a 178 | dependency to message_filter and the package for the message used to your package.xml! 179 | :param name: Base name of the subscriber. Will be name of the subscriber in the parameter struct. The topic 180 | parameter is then _topic and the queue size _queue_size (unless overriden). 181 | :param message_type: Type of message including its namespace (e.g. std_msgs::Header) 182 | :param description: Chose an informative documentation string for this subscriber. 183 | :param default_topic: (optional) Default topic to subscribe to. 184 | :param default_queue_size: (optional) Default queue size of the subscriber. 185 | :param no_delay: (optional) Set the tcp_no_delay parameter for subscribing. Recommended for larger topics. 186 | :param topic_param: (optional) Name of the param configuring the topic. Will be "_topic" if None. 187 | :param queue_size_param: (optional) Name of param configuring the queue size. Defaults to "_queue_size". 188 | :param header: (optional) Header name (or a list of names) to include. Will be deduced for message type if None. 189 | :param module: (optional) Module name (or a list of names) to import from (e.g. std_msgs.msg). 190 | Will be automatically deduced if None. 191 | :param configurable: (optional) Should the topic name and message queue size be dynamically configurable? 192 | :param scope: (optional) Either "global", "public" or "private". A "global" subscriber will subscribe to /topic, 193 | a "public" one to /namespace/topic and a "private" one to /namespace/node_name/topic. 194 | :param constant: (optional) If this is true, the parameters will not be fetched from param server, 195 | but the default value is kept. 196 | :param diagnosed: (optional) Enables diagnostics for the subscriber. Can be configured with the params below. 197 | The message must have a header. Not yet supported for python. 198 | :param min_frequency: (optional) Sets the default minimum frequency for the subscriber 199 | :param min_frequency_param: (optional) Sets the parameter for the minimum frequency. 200 | Defaults to _min_frequency 201 | :param max_delay: (optional) Sets the default maximal header delay for the topics in seconds. 202 | :param max_delay_param: (optional) Parameter for the maximal delay. Defaults to _max_delay. 203 | :param watch: (optional) a list of connected publishers added with add_publisher. If it is nonempty, 204 | the subscriber will be a "smart_subscriber" meaning he will not execute callbacks if no one subscribed to the 205 | publishers. 206 | :return: None 207 | """ 208 | # add subscriber topic and queue size as param 209 | if not topic_param: 210 | topic_param = name + '_topic' 211 | if not queue_size_param: 212 | queue_size_param = name + '_queue_size' 213 | self.add(name=topic_param, paramtype='std::string', description='Topic for ' + description, 214 | default=default_topic, configurable=configurable, global_scope=False, constant=constant) 215 | self.add(name=queue_size_param, paramtype='int', description='Queue size for ' + description, min=0, 216 | default=default_queue_size, configurable=configurable, global_scope=False, constant=constant) 217 | for publisher in watch: 218 | if "name" not in publisher: 219 | eprint("Invalid input passed as 'watch' to add_subscriber. Expected a list of publisher objects!") 220 | 221 | if diagnosed: 222 | if not self._get_root().diagnostics_enabled: 223 | eprint("Please enable diagnostics before adding a diagnosed subscriber") 224 | if not min_frequency_param: 225 | min_frequency_param = name + '_min_frequency' 226 | if not max_delay_param: 227 | max_delay_param = name + '_max_delay' 228 | self.add( 229 | name=min_frequency_param, 230 | paramtype='double', 231 | description='Minimal message frequency for ' + 232 | description, 233 | default=min_frequency, 234 | configurable=configurable, 235 | global_scope=False, 236 | constant=constant) 237 | self.add(name=max_delay_param, paramtype='double', description='Maximal delay for ' + description, 238 | default=max_delay, configurable=configurable, global_scope=False, constant=constant) 239 | 240 | # normalize the topic type (we want it to contain ::) 241 | normalized_message_type = message_type.replace("/", "::").replace(".", "::") 242 | 243 | if not module: 244 | module = ".".join(normalized_message_type.split('::')[0:-1]) + '.msg' 245 | if not header: 246 | header = normalized_message_type.replace("::", "/") + ".h" 247 | 248 | watch = [publisher["name"] for publisher in watch] 249 | 250 | # add a subscriber object 251 | newparam = { 252 | 'name': name, 253 | 'type': normalized_message_type, 254 | 'header': header if isinstance(header, list) else [header], 255 | 'import': module if isinstance(module, list) else [module], 256 | 'topic_param': topic_param, 257 | 'queue_size_param': queue_size_param, 258 | 'no_delay': no_delay, 259 | 'configurable': configurable, 260 | 'description': description, 261 | 'scope': scope.lower(), 262 | 'diagnosed': diagnosed, 263 | 'watch': watch, 264 | 'min_frequency_param': min_frequency_param, 265 | 'max_delay_param': max_delay_param 266 | } 267 | self.subscribers.append(newparam) 268 | 269 | def add_publisher( 270 | self, 271 | name, 272 | message_type, 273 | description, 274 | default_topic=None, 275 | default_queue_size=5, 276 | topic_param=None, 277 | queue_size_param=None, 278 | header=None, 279 | module=None, 280 | configurable=False, 281 | scope="private", 282 | constant=False, 283 | diagnosed=False, 284 | min_frequency=0., 285 | min_frequency_param=None, 286 | max_delay=float('inf'), 287 | max_delay_param=None): 288 | """ 289 | Adds a publisher to your parameter struct and a parameter for its topic and queue size. Don't forget to add a 290 | dependency to message_filter and the package for the message used to your package.xml! 291 | :param name: Base name of the publisher. Will be name of the publisher in the parameter struct. The topic 292 | parameter is then _topic and the queue size _queue_size (unless overriden). 293 | :param message_type: Type of message including its namespace (e.g. std_msgs::Header) 294 | :param description: Chose an informative documentation string for this publisher. 295 | :param default_topic: (optional) Default topic to publish to. 296 | :param default_queue_size: (optional) Default queue size of the publisher. 297 | :param topic_param: (optional) Name of the param configuring the topic. Will be "_topic" if None. 298 | :param queue_size_param: (optional) Name of param configuring the queue size. Defaults to "_queue_size". 299 | :param header: (optional) Header name (or list of names) to include. Will be deduced for message type if None. 300 | :param module: (optional) Module name (or list of names) to import from (e.g. std_msgs.msg). 301 | Will be automatically deduced if None. 302 | :param configurable: (optional) Should the topic name and message queue size be dynamically configurable? 303 | :param scope: (optional) Either "global", "public" or "private". A "global" subscriber will subscribe to /topic, 304 | a "public" one to /namespace/topic and a "private" one to /namespace/node_name/topic. 305 | :param constant: (optional) If this is true, the parameters will not be fetched from param server, 306 | but the default value is kept. 307 | :param diagnosed: (optional) Enables diagnostics for the publisher. Can be configured with the params below. 308 | The message must have a header. Not yet supported for python. 309 | :param min_frequency: (optional) Sets the default minimum frequency for the publisher 310 | :param min_frequency_param: (optional) Sets the parameter for the minimum frequency. 311 | Defaults to _min_frequency 312 | :param max_delay: (optional) Sets the default maximal header delay for the topics in seconds. 313 | :param max_delay_param: (optional) Parameter for the maximal delay. Defaults to _max_delay. 314 | :return: a configuration dict for the created publisher 315 | """ 316 | # add publisher topic and queue size as param 317 | if not topic_param: 318 | topic_param = name + '_topic' 319 | if not queue_size_param: 320 | queue_size_param = name + '_queue_size' 321 | self.add(name=topic_param, paramtype='std::string', description='Topic for ' + description, 322 | default=default_topic, configurable=configurable, global_scope=False, constant=constant) 323 | self.add(name=queue_size_param, paramtype='int', description='Queue size for ' + description, min=0, 324 | default=default_queue_size, configurable=configurable, global_scope=False, constant=constant) 325 | 326 | if diagnosed: 327 | if not self._get_root().diagnostics_enabled: 328 | eprint("Please enable diagnostics before adding a diagnosed publisher") 329 | if not min_frequency_param: 330 | min_frequency_param = name + '_min_frequency' 331 | if not max_delay_param: 332 | max_delay_param = name + '_max_delay' 333 | self.add( 334 | name=min_frequency_param, 335 | paramtype='double', 336 | description='Minimal message frequency for ' + 337 | description, 338 | default=min_frequency, 339 | configurable=configurable, 340 | global_scope=False, 341 | constant=constant) 342 | self.add(name=max_delay_param, paramtype='double', description='Maximal delay for ' + description, 343 | default=max_delay, configurable=configurable, global_scope=False, constant=constant) 344 | 345 | # normalize the topic type (we want it to contain ::) 346 | normalized_message_type = message_type.replace("/", "::").replace(".", "::") 347 | 348 | if not module: 349 | module = ".".join(normalized_message_type.split('::')[0:-1]) + '.msg' 350 | if not header: 351 | header = normalized_message_type.replace("::", "/") + ".h" 352 | 353 | # add a publisher object 354 | newparam = { 355 | 'name': name, 356 | 'type': normalized_message_type, 357 | 'header': header if isinstance(header, list) else [header], 358 | 'import': module if isinstance(module, list) else [module], 359 | 'topic_param': topic_param, 360 | 'queue_size_param': queue_size_param, 361 | 'configurable': configurable, 362 | 'description': description, 363 | 'scope': scope.lower(), 364 | 'diagnosed': diagnosed, 365 | 'min_frequency_param': min_frequency_param, 366 | 'max_delay_param': max_delay_param 367 | } 368 | self.publishers.append(newparam) 369 | return newparam 370 | 371 | def add(self, name, paramtype, description, level=0, edit_method='""', default=None, min=None, max=None, 372 | configurable=False, global_scope=False, constant=False): 373 | """ 374 | Add parameters to your parameter struct. Call this method from your .params file! 375 | 376 | - If no default value is given, you need to specify one in your launch file 377 | - Global parameters, vectors, maps and constant params can not be configurable 378 | - Global parameters, vectors and maps can not have a default, min or max value 379 | 380 | :param self: 381 | :param name: The Name of you new parameter 382 | :param paramtype: The C++ type of this parameter. Can be any of ['std::string', 'int', 'bool', 'float', 383 | 'double'] or std::vector<...> or std::map 384 | :param description: Choose an informative documentation string for this parameter. 385 | :param level: (optional) Passed to dynamic_reconfigure 386 | :param edit_method: (optional) Passed to dynamic_reconfigure 387 | :param default: (optional) default value 388 | :param min: (optional) 389 | :param max: (optional) 390 | :param configurable: (optional) Should this parameter be dynamic configurable 391 | :param global_scope: (optional) If true, parameter is searched in global ('/') namespace instead of private ( 392 | '~') ns 393 | :param constant: (optional) If this is true, the parameter will not be fetched from param server, 394 | but the default value is kept. 395 | :return: None 396 | """ 397 | configurable = self._make_bool(configurable) 398 | global_scope = self._make_bool(global_scope) 399 | constant = self._make_bool(constant) 400 | newparam = { 401 | 'name': name, 402 | 'type': paramtype, 403 | 'default': default, 404 | 'level': level, 405 | 'edit_method': edit_method, 406 | 'description': description, 407 | 'min': min, 408 | 'max': max, 409 | 'is_vector': False, 410 | 'is_map': False, 411 | 'configurable': configurable, 412 | 'constant': constant, 413 | 'global_scope': global_scope, 414 | } 415 | self._perform_checks(newparam) 416 | # make sure verbosity is the first param 417 | if name == self.verbosity: 418 | self.parameters.insert(0, newparam) 419 | else: 420 | self.parameters.append(newparam) 421 | 422 | def _perform_checks(self, param): 423 | """ 424 | Will test some logical constraints as well as correct types. 425 | Throws Exception in case of error. 426 | :param self: 427 | :param param: Dictionary of one param 428 | :return: 429 | """ 430 | 431 | in_type = param['type'].strip() 432 | if param['max'] is not None or param['min'] is not None: 433 | if in_type in ["std::string", "bool", "int64_t"]: 434 | eprint(param['name'], "Max and min can not be specified for variable of type %s" % in_type) 435 | 436 | if in_type.startswith('std::vector'): 437 | param['is_vector'] = True 438 | if in_type.startswith('std::map'): 439 | param['is_map'] = True 440 | 441 | if (param['is_vector']): 442 | if (param['max'] is not None or param['min'] is not None): 443 | ptype = in_type[12:-1].strip() 444 | if ptype == "std::string": 445 | eprint(param['name'], "Max and min can not be specified for variable of type %s" % in_type) 446 | 447 | if (param['is_map']): 448 | if (param['max'] is not None or param['min'] is not None): 449 | ptype = in_type[9:-1].split(',') 450 | if len(ptype) != 2: 451 | eprint(param['name'], 452 | "Wrong syntax used for setting up std::map<... , ...>: You provided '%s' with " 453 | "parameter %s" % in_type) 454 | ptype = ptype[1].strip() 455 | if ptype == "std::string": 456 | eprint(param['name'], "Max and min can not be specified for variable of type %s" % in_type) 457 | 458 | pattern = r'^[a-zA-Z][a-zA-Z0-9_]*$' 459 | if not re.match(pattern, param['name']): 460 | eprint(param['name'], "The name of field does not follow the ROS naming conventions, " 461 | "see http://wiki.ros.org/ROS/Patterns/Conventions") 462 | if param['configurable'] and ( 463 | param['global_scope'] or param['is_vector'] or param['is_map'] or ( 464 | in_type == "int64_t") or param['constant']): 465 | eprint(param['name'], 466 | "Global parameters, vectors, maps, long and constant params can not be declared configurable! ") 467 | if param['global_scope'] and param['default'] is not None: 468 | eprint(param['name'], "Default values for global parameters should not be specified in node! ") 469 | if param['constant'] and param['default'] is None: 470 | eprint(param['name'], "Constant parameters need a default value!") 471 | if param['name'] in [p['name'] for p in self.parameters]: 472 | eprint(param['name'], "Parameter with the same name exists already") 473 | if param['edit_method'] == '': 474 | param['edit_method'] = '""' 475 | elif param['edit_method'] != '""': 476 | param['configurable'] = True 477 | 478 | # Check type 479 | if param['is_vector']: 480 | ptype = in_type[12:-1].strip() 481 | self._test_primitive_type(param['name'], ptype) 482 | param['type'] = 'std::vector<{}>'.format(ptype) 483 | elif param['is_map']: 484 | ptype = in_type[9:-1].split(',') 485 | if len(ptype) != 2: 486 | eprint(param['name'], "Wrong syntax used for setting up std::map<... , ...>: You provided '%s' with " 487 | "parameter %s" % in_type) 488 | ptype[0] = ptype[0].strip() 489 | ptype[1] = ptype[1].strip() 490 | if ptype[0] != "std::string": 491 | eprint(param['name'], "Can not setup map with %s as key type. Only std::map are allowed" % ptype[0]) 493 | self._test_primitive_type(param['name'], ptype[0]) 494 | self._test_primitive_type(param['name'], ptype[1]) 495 | param['type'] = 'std::map<{},{}>'.format(ptype[0], ptype[1]) 496 | else: 497 | # Pytype and defaults can only be applied to primitives 498 | self._test_primitive_type(param['name'], in_type) 499 | param['pytype'] = self._pytype(in_type) 500 | 501 | @staticmethod 502 | def _pytype(drtype): 503 | """Convert C++ type to python type""" 504 | return {'std::string': "str", 'int': "int", 'double': "double", 'bool': "bool", 'int64_t': "int"}[drtype] 505 | 506 | @staticmethod 507 | def _test_primitive_type(name, drtype): 508 | """ 509 | Test whether parameter has one of the accepted C++ types 510 | :param name: Interfacename 511 | :param drtype: Typestring 512 | :return: 513 | """ 514 | primitive_types = ['std::string', 'int', 'bool', 'float', 'double', 'int64_t'] 515 | if drtype not in primitive_types: 516 | raise TypeError("'%s' has type %s, but allowed are: %s" % (name, drtype, primitive_types)) 517 | 518 | @staticmethod 519 | def _get_cvalue(param, field): 520 | """ 521 | Helper function to convert strings and booleans to correct C++ syntax 522 | :param param: 523 | :return: C++ compatible representation 524 | """ 525 | value = param[field] 526 | if param['type'] == 'double' and param[field] == float('inf'): 527 | value = 'std::numeric_limits::infinity()' 528 | if param['type'] == 'double' and param[field] == float('-inf'): 529 | value = '-std::numeric_limits::infinity()' 530 | if param['type'] == 'std::string': 531 | value = '"{}"'.format(param[field]) 532 | elif param['type'] == 'bool': 533 | value = str(param[field]).lower() 534 | return str(value) 535 | 536 | @staticmethod 537 | def _get_pyvalue(param, field): 538 | """ 539 | Helper function to convert strings and booleans to correct C++ syntax 540 | :param param: 541 | :return: C++ compatible representation 542 | """ 543 | value = param[field] 544 | if param['type'] == 'double' and param[field] == float('inf'): 545 | value = '1e+308' # dynamic reconfigure does not supprt if, but this should be sufficient. 546 | if param['type'] == 'double' and param[field] == float('-inf'): 547 | value = '-1e+308' 548 | if param['type'] == 'std::string': 549 | value = '"{}"'.format(param[field]) 550 | elif param['type'] == 'bool': 551 | value = str(param[field]).capitalize() 552 | return str(value) 553 | 554 | @staticmethod 555 | def _get_cvaluelist(param, field): 556 | """ 557 | Helper function to convert python list of strings and booleans to correct C++ syntax 558 | :param param: 559 | :return: C++ compatible representation 560 | """ 561 | values = param[field] 562 | assert (isinstance(values, list)) 563 | form = "" 564 | for value in values: 565 | if param['type'] == 'std::vector': 566 | value = '"{}"'.format(value) 567 | elif param['type'] == 'std::vector': 568 | value = str(value).lower() 569 | else: 570 | value = str(value) 571 | form += value + ',' 572 | # remove last ',' 573 | return form[:-1] 574 | 575 | @staticmethod 576 | def _get_cvaluedict(param, field): 577 | """ 578 | Helper function to convert python dict of strings and booleans to correct C++ syntax 579 | :param param: 580 | :return: C++ compatible representation 581 | """ 582 | values = param[field] 583 | assert (isinstance(values, dict)) 584 | form = "" 585 | for key, value in values.items(): 586 | if param['type'] == 'std::map': 587 | pair = '{{"{}","{}"}}'.format(key, value) 588 | elif param['type'] == 'std::map': 589 | pair = '{{"{}",{}}}'.format(key, str(value).lower()) 590 | else: 591 | pair = '{{"{}",{}}}'.format(key, str(value)) 592 | form += pair + ',' 593 | # remove last ',' 594 | return form[:-1] 595 | 596 | def generate(self, pkgname, nodename, classname): 597 | """ 598 | Main working Function, call this at the end of your .params file! 599 | :param self: 600 | :param pkgname: Name of the catkin package 601 | :param nodename: Name of the Node that will hold these params 602 | :param classname: This should match your file name, so that cmake will detect changes in config file. 603 | :return: Exit Code 604 | """ 605 | self.pkgname = pkgname 606 | self.nodename = nodename 607 | self.classname = classname 608 | 609 | print("Generating interface file for node {} (class {}) in package {}".format(nodename, classname, pkgname)) 610 | 611 | if self.parent: 612 | eprint("You should not call generate on a group! Call it on the main parameter generator instead!") 613 | 614 | return self._generateImpl() 615 | 616 | def _generateImpl(self): 617 | """ 618 | Implementation level function. Can be overwritten by derived classes. 619 | :return: 620 | """ 621 | self._generatecfg() 622 | self._generatehpp() 623 | self._generatepy() 624 | 625 | return 0 626 | 627 | def _generatecfg(self): 628 | """ 629 | Generate .cfg file for dynamic reconfigure 630 | :param self: 631 | :return: 632 | """ 633 | templatefile = os.path.join(self.dynconfpath, "templates", "ConfigType.h.template") 634 | with open(templatefile, 'r') as f: 635 | template = f.read() 636 | 637 | param_entries = self._generate_param_entries() 638 | 639 | param_entries = "\n".join(param_entries) 640 | template = Template(template).substitute(pkgname=self.pkgname, nodename=self.nodename, 641 | classname=self.classname, params=param_entries) 642 | 643 | cfg_file = os.path.join(self.share_dir, "cfg", self.classname + ".cfg") 644 | try: 645 | if not os.path.exists(os.path.dirname(cfg_file)): 646 | os.makedirs(os.path.dirname(cfg_file)) 647 | except OSError: 648 | # Stupid error, sometimes the directory exists anyway 649 | pass 650 | with open(cfg_file, 'w') as f: 651 | f.write(template) 652 | os.chmod(cfg_file, 509) # entspricht 775 (octal) 653 | # calling sync mitigate issues in docker (see https://github.com/moby/moby/issues/9547) 654 | subprocess.check_call(["sync", "-f", cfg_file]) 655 | 656 | def _generatehpp(self): 657 | """ 658 | Generate C++ Header file, holding the parameter struct. 659 | :param self: 660 | :return: 661 | """ 662 | 663 | # Read in template file 664 | templatefile = os.path.join(self.dynconfpath, "templates", "Interface.h.template") 665 | with open(templatefile, 'r') as f: 666 | template = f.read() 667 | 668 | substitutions = {"pkgname": self.pkgname, "ClassName": self.classname, "nodename": self.nodename} 669 | param_entries = [] 670 | string_representation = [] 671 | from_server = [] 672 | to_server = [] 673 | non_default_params = [] 674 | from_config = [] 675 | test_limits = [] 676 | includes = [] 677 | sub_adv_from_server = [] 678 | sub_adv_from_config = [] 679 | subscriber_entries = [] 680 | subscribers_init = [] 681 | publisher_entries = [] 682 | print_subscribed = [] 683 | print_advertised = [] 684 | 685 | subscribers = self._get_subscribers() 686 | publishers = self._get_publishers() 687 | if subscribers or publishers: 688 | substitutions["includeMessageFiltersError"] = "#error message_filters was not found during compilation. " \ 689 | "Please recompile with message_filters." 690 | else: 691 | substitutions["includeMessageFiltersError"] = "" 692 | 693 | if any(subscriber["watch"] for subscriber in subscribers): 694 | includes.append('#include ') 695 | 696 | substitutions["includeDiagnosticUpdaterError"] = "" 697 | if self.diagnostics_enabled: 698 | param_entries.append(' diagnostic_updater::Updater updater; /*!< Manages diagnostics of this node */') 699 | subscribers_init.append(',\n updater{ros::NodeHandle(), private_node_handle, nodeNameWithNamespace()}') 700 | includes.append('#include ') 701 | from_server.append(' updater.setHardwareID("none");') 702 | if self.simplified_diagnostics: 703 | param_entries.append( 704 | ' rosinterface_handler::SimpleNodeStatus nodeStatus; /*!< Reports the status of this node */') 705 | subscribers_init.append(',\n nodeStatus{"status", private_node_handle, updater}') 706 | includes.append('#include ') 707 | substitutions["includeDiagnosticUpdaterError"] = "#error diagnostic_updater is missing as dependency." 708 | 709 | if self.tf: 710 | listener = self.tf["listener_name"] 711 | buffer = self.tf["buffer_name"] 712 | broadcaster = self.tf["broadcaster_name"] 713 | if buffer: 714 | includes.append('#include ') 715 | param_entries.append(' tf2_ros::Buffer {};'.format(buffer)) 716 | if listener: 717 | includes.append('#include ') 718 | param_entries.append(' tf2_ros::TransformListener {};'.format(listener)) 719 | subscribers_init.append(',\n {}{{{}}}'.format(listener, buffer)) 720 | if broadcaster: 721 | includes.append('#include ') 722 | param_entries.append(' tf2_ros::TransformBroadcaster {};'.format(broadcaster)) 723 | 724 | first = True 725 | for subscriber in subscribers: 726 | name = subscriber['name'] 727 | type = subscriber['type'] 728 | headers = subscriber['header'] 729 | description = subscriber['description'] 730 | diagnosed = subscriber['diagnosed'] 731 | watch = subscriber['watch'] 732 | 733 | # add include entry 734 | for header in headers: 735 | include = "#include <{}>".format(header) 736 | if include not in includes: 737 | includes.append(include) 738 | 739 | # add subscriber entry 740 | if diagnosed and watch: 741 | subscriber_t = 'DiagSubscriber$ptr<$type, rosinterface_handler::SmartSubscriber<$type>>' 742 | init = "updater, " + ", ".join(watch) 743 | elif watch: 744 | subscriber_t = 'rosinterface_handler::SmartSubscriber$ptr<$type>' 745 | init = ", ".join(watch) 746 | elif diagnosed: 747 | subscriber_t = 'DiagSubscriber$ptr<$type>' 748 | init = "updater" 749 | else: 750 | subscriber_t = "Subscriber$ptr<$type>" 751 | init = "" 752 | subscriber_type = Template(subscriber_t).substitute(type=type, ptr="") 753 | subscriber_ptr = Template(subscriber_t).substitute(type=type, ptr="Ptr") 754 | 755 | subscriber_entries.append(Template(' $subscriber ${name}; /*!< $description ' 756 | '*/').substitute(name=name, description=description, 757 | subscriber=subscriber_ptr)) 758 | 759 | # add initialisation 760 | subscribers_init.append(Template(',\n $name{std::make_shared<$subscriber>(${init})}') 761 | .substitute(name=name, subscriber=subscriber_type, init=init)) 762 | 763 | # add printing 764 | space = "" if first else '", " +' 765 | first = False 766 | print_subscribed.append(Template(' message += $space $name->getTopic();').substitute(name=name, 767 | space=space)) 768 | 769 | # add subscribe for parameter server 770 | topic_param = subscriber['topic_param'] 771 | queue_size_param = subscriber['queue_size_param'] 772 | min_freq_param = subscriber['min_frequency_param'] 773 | max_delay_param = subscriber['max_delay_param'] 774 | scope = subscriber['scope'] 775 | if scope == 'private': 776 | name_space = "privateNamespace_" 777 | elif scope == 'public': 778 | name_space = "publicNamespace_" 779 | elif scope == 'global': 780 | name_space = "globalNamespace_" 781 | else: 782 | eprint("Unknown scope specified for subscriber {}: {}".format(name, scope)) 783 | if subscriber['no_delay']: 784 | no_delay = ", ros::TransportHints().tcpNoDelay()" 785 | else: 786 | no_delay = "" 787 | if diagnosed: 788 | sub_adv_from_server.append(Template(' $name->minFrequency($minFParam).maxTimeDelay($maxTParam);') 789 | .substitute(name=name, minFParam=min_freq_param, maxTParam=max_delay_param)) 790 | sub_adv_from_server.append( 791 | Template( 792 | ' $name->subscribe(privateNodeHandle_, ' 793 | 'rosinterface_handler::getTopic($namespace, $topic), uint32_t($queue)$noDelay);') .substitute( 794 | name=name, 795 | topic=topic_param, 796 | queue=queue_size_param, 797 | noDelay=no_delay, 798 | namespace=name_space)) 799 | if subscriber['configurable']: 800 | if diagnosed: 801 | sub_adv_from_config.append( 802 | Template( 803 | ' $name->minFrequency(config.$minFParam)' 804 | '.maxTimeDelay(config.$maxTParam);') .substitute( 805 | name=name, 806 | minFParam=min_freq_param, 807 | maxTParam=max_delay_param)) 808 | sub_adv_from_config.append(Template(' if($topic != config.$topic || $queue != config.$queue) {\n' 809 | ' $name->subscribe(privateNodeHandle_, ' 810 | 'rosinterface_handler::getTopic($namespace, config.$topic), ' 811 | 'uint32_t(config.$queue)$noDelay);\n' 812 | ' }').substitute(name=name, topic=topic_param, 813 | queue=queue_size_param, noDelay=no_delay, 814 | namespace=name_space)) 815 | if watch: 816 | from_config.append(Template(' $name->updateTopics();').substitute(name=name)) 817 | test_limits.append(from_config[-1]) 818 | 819 | first = True 820 | for publisher in publishers: 821 | name = publisher['name'] 822 | type = publisher['type'] 823 | headers = publisher['header'] 824 | description = publisher['description'] 825 | diagnosed = publisher['diagnosed'] 826 | 827 | # add include entry 828 | for header in headers: 829 | include = "#include <{}>".format(header) 830 | if include not in includes: 831 | includes.append(include) 832 | 833 | # add publisher entry 834 | if diagnosed: 835 | publish = Template("DiagPublisher<$type>").substitute(type=type) 836 | init = "{updater}" 837 | else: 838 | publish = "ros::Publisher" 839 | init = "" 840 | 841 | publisher_entries.append(Template(' $publisher ${name}$init; /*!< $description */').substitute( 842 | publisher=publish, name=name, description=description, init=init)) 843 | 844 | # add printing 845 | space = "" if first else '", " +' 846 | first = False 847 | print_advertised.append(Template(' message += $space $name.getTopic();').substitute(name=name, 848 | space=space)) 849 | 850 | # add advertise for parameter server 851 | topic_param = publisher['topic_param'] 852 | queue_size_param = publisher['queue_size_param'] 853 | min_freq_param = publisher['min_frequency_param'] 854 | max_delay_param = publisher['max_delay_param'] 855 | scope = publisher['scope'].lower() 856 | if scope == 'private': 857 | name_space = "privateNamespace_" 858 | elif scope == 'public': 859 | name_space = "publicNamespace_" 860 | elif scope == 'global': 861 | name_space = "globalNamespace_" 862 | else: 863 | eprint("Unknown scope specified for publisher {}: {}".format(name, scope)) 864 | if diagnosed: 865 | sub_adv_from_server.append(Template(' $name.minFrequency($minFParam).maxTimeDelay($maxTParam);') 866 | .substitute(name=name, minFParam=min_freq_param, maxTParam=max_delay_param)) 867 | sub_adv_from_server.append(Template(' $name = privateNodeHandle_.advertise<$type>(' 868 | 'rosinterface_handler::getTopic($namespace, $topic), $queue);') 869 | .substitute(name=name, type=type, topic=topic_param, queue=queue_size_param, 870 | namespace=name_space)) 871 | if publisher['configurable']: 872 | if diagnosed: 873 | sub_adv_from_config.append( 874 | Template( 875 | ' $name.minFrequency(config.$minFParam)' 876 | '.maxTimeDelay(config.$maxTParam);') .substitute( 877 | name=name, 878 | minFParam=min_freq_param, 879 | maxTParam=max_delay_param)) 880 | sub_adv_from_config.append(Template(' if($topic != config.$topic || $queue != config.$queue) {\n' 881 | ' $name = privateNodeHandle_.advertise<$type>(' 882 | 'rosinterface_handler::getTopic($namespace, config.$topic), ' 883 | 'config.$queue);\n' 884 | ' }').substitute(name=name, type=type, topic=topic_param, 885 | queue=queue_size_param, 886 | namespace=name_space)) 887 | substitutions["includes"] = "\n".join(includes) 888 | substitutions["subscribers"] = "\n".join(subscriber_entries) 889 | substitutions["publishers"] = "\n".join(publisher_entries) 890 | substitutions["subscribeAdvertiseFromParamServer"] = "\n".join(sub_adv_from_server) 891 | substitutions["subscribeAdvertiseFromConfig"] = "\n".join(sub_adv_from_config) 892 | substitutions["print_advertised"] = "\n".join(print_advertised) 893 | substitutions["print_subscribed"] = "\n".join(print_subscribed) 894 | substitutions["initSubscribers"] = "".join(subscribers_init) 895 | 896 | params = self._get_parameters() 897 | 898 | # Create dynamic parts of the header file for every parameter 899 | for param in params: 900 | name = param['name'] 901 | 902 | # Adjust key for parameter server 903 | if param["global_scope"]: 904 | namespace = 'globalNamespace_' 905 | else: 906 | namespace = 'privateNamespace_' 907 | full_name = '{} + "{}"'.format(namespace, param["name"]) 908 | 909 | # Test for default value 910 | if param["default"] is None: 911 | default = "" 912 | non_default_params.append(Template(' << "\t" << $namespace << "$name" << " ($type) ' 913 | '\\n"\n').substitute( 914 | namespace=namespace, name=name, type=param["type"])) 915 | else: 916 | if param['is_vector']: 917 | default = ', {}'.format(str(param['type']) + "{" + self._get_cvaluelist(param, "default") + "}") 918 | elif param['is_map']: 919 | default = ', {}'.format(str(param['type']) + "{" + self._get_cvaluedict(param, "default") + "}") 920 | else: 921 | default = ', {}'.format(str(param['type']) + "{" + self._get_cvalue(param, "default") + "}") 922 | 923 | # Test for constant value 924 | if param['constant']: 925 | param_entries.append(Template(' static constexpr auto ${name} = $default; /*!< ${description} ' 926 | '*/').substitute(type=param['type'], name=name, 927 | description=param['description'], 928 | default=self._get_cvalue(param, "default"))) 929 | from_server.append( 930 | Template(' rosinterface_handler::testConstParam($paramname);').substitute( 931 | paramname=full_name)) 932 | else: 933 | param_entries.append(Template(' ${type} ${name}; /*!< ${description} */').substitute( 934 | type=param['type'], name=name, description=param['description'])) 935 | from_server.append(Template(' success &= rosinterface_handler::getParam($paramname, $name$default);') 936 | .substitute(paramname=full_name, name=name, 937 | default=default, description=param['description'])) 938 | to_server.append( 939 | Template(' rosinterface_handler::setParam(${paramname},${name});').substitute( 940 | paramname=full_name, name=name)) 941 | 942 | # Test for configurable params 943 | if param['configurable']: 944 | from_config.append(Template(' $name = config.$name;').substitute(name=name)) 945 | 946 | # Test limits 947 | if param['is_vector']: 948 | ttype = param['type'][12:-1].strip() 949 | elif param['is_map']: 950 | ttype = param['type'][9:-1].strip() 951 | else: 952 | ttype = param['type'] 953 | if param['min'] is not None: 954 | test_limits.append( 955 | Template(' rosinterface_handler::testMin<$type>($paramname, $name, $min);').substitute( 956 | paramname=full_name, name=name, min=param['min'], type=ttype)) 957 | if param['max'] is not None: 958 | test_limits.append( 959 | Template(' rosinterface_handler::testMax<$type>($paramname, $name, $max);').substitute( 960 | paramname=full_name, name=name, max=param['max'], type=ttype)) 961 | 962 | # Add debug output 963 | string_representation.append(Template(' << "\t" << p.$namespace << "$name:" << p.$name << ' 964 | '"\\n"\n').substitute(namespace=namespace, name=name)) 965 | 966 | # handle verbosity param 967 | if self.verbosity == name: 968 | from_server.append(Template(' rosinterface_handler::setLoggerLevel(privateNodeHandle_, "$verbosity",' 969 | ' nodeNameWithNamespace());').substitute( 970 | verbosity=self.verbosity)) 971 | if param['configurable']: 972 | verb_check = Template( 973 | ' if(config.$verbosity != this->$verbosity) {\n' 974 | ' rosinterface_handler::setParam(privateNamespace_ + "$verbosity", config.$verbosity);\n' 975 | ' rosinterface_handler::setLoggerLevel(privateNodeHandle_, "$verbosity", nodeNameWithNamespace());\n' 976 | ' }').substitute( 977 | verbosity=self.verbosity) 978 | from_config.insert(0, verb_check) 979 | 980 | substitutions["parameters"] = "\n".join(param_entries) 981 | substitutions["string_representation"] = "".join(string_representation) 982 | substitutions["non_default_params"] = "".join(non_default_params) 983 | substitutions["fromParamServer"] = "\n".join(from_server) 984 | substitutions["toParamServer"] = "\n".join(to_server) 985 | substitutions["fromConfig"] = "\n".join(from_config) 986 | substitutions["test_limits"] = "\n".join(test_limits) 987 | content = Template(template).substitute(**substitutions) 988 | 989 | header_file = os.path.join(self.cpp_gen_dir, self.classname + "Interface.h") 990 | try: 991 | if not os.path.exists(os.path.dirname(header_file)): 992 | os.makedirs(os.path.dirname(header_file)) 993 | except OSError: 994 | # Stupid error, sometimes the directory exists anyway 995 | pass 996 | with open(header_file, 'w') as f: 997 | f.write(content) 998 | 999 | def _generatepy(self): 1000 | """ 1001 | Generate Python parameter file 1002 | :param self: 1003 | :return: 1004 | """ 1005 | params = self._get_parameters() 1006 | 1007 | paramDescription = str(params) 1008 | subscriberDescription = str(self._get_subscribers()) 1009 | publisherDescription = str(self._get_publishers()) 1010 | if self.verbosity: 1011 | verbosityParam = '"' + self.verbosity + '"' 1012 | else: 1013 | verbosityParam = None 1014 | 1015 | # generate import statements 1016 | imports = set() 1017 | for subscriber in self._get_subscribers(): 1018 | imports.update("import {}".format(sub) for sub in subscriber['import']) 1019 | for publisher in self._get_publishers(): 1020 | imports.update("import {}".format(pub) for pub in publisher['import']) 1021 | if self.simplified_diagnostics: 1022 | imports.add("import rospy") 1023 | imports.add("import diagnostic_msgs") 1024 | imports = "\n".join(imports) 1025 | 1026 | # Read in template file 1027 | templatefile = os.path.join(self.dynconfpath, "templates", "Interface.py.template") 1028 | with open(templatefile, 'r') as f: 1029 | template = f.read() 1030 | 1031 | content = Template(template).substitute(pkgname=self.pkgname, ClassName=self.classname, imports=imports, 1032 | paramDescription=paramDescription, 1033 | subscriberDescription=subscriberDescription, 1034 | publisherDescription=publisherDescription, 1035 | verbosityParam=verbosityParam, 1036 | tfConfig=self.tf, 1037 | diagnosticsEnabled=self.diagnostics_enabled, 1038 | simplifiedDiagnostics=self.simplified_diagnostics) 1039 | 1040 | py_file = os.path.join(self.py_gen_dir, "interface", self.classname + "Interface.py") 1041 | try: 1042 | if not os.path.exists(os.path.dirname(py_file)): 1043 | os.makedirs(os.path.dirname(py_file)) 1044 | except OSError: 1045 | # Stupid error, sometimes the directory exists anyway 1046 | pass 1047 | with open(py_file, 'w') as f: 1048 | f.write(content) 1049 | init_file = os.path.join(self.py_gen_dir, "interface", "__init__.py") 1050 | with open(init_file, 'w') as f: 1051 | f.write("") 1052 | 1053 | def _generateyml(self): 1054 | """ 1055 | Generate .yaml file for roslaunch 1056 | :param self: 1057 | :return: 1058 | """ 1059 | params = self._get_parameters() 1060 | 1061 | content = "### This file was generated using the rosinterface_handler generate_yaml script.\n" 1062 | 1063 | for entry in params: 1064 | if not entry["constant"]: 1065 | content += "\n" 1066 | content += "# Name:\t" + str(entry["name"]) + "\n" 1067 | content += "# Desc:\t" + str(entry["description"]) + "\n" 1068 | content += "# Type:\t" + str(entry["type"]) + "\n" 1069 | if entry['min'] or entry['max']: 1070 | content += "# [min,max]:\t[" + str(entry["min"]) + "/" + str(entry["max"]) + "]" + "\n" 1071 | if entry["global_scope"]: 1072 | content += "# Lives in global namespace!\n" 1073 | if entry["default"] is not None: 1074 | content += str(entry["name"]) + ": " + str(entry["default"]) + "\n" 1075 | else: 1076 | content += str(entry["name"]) + ": \n" 1077 | 1078 | yaml_file = os.path.join(os.getcwd(), self.classname + "Interface.yaml") 1079 | 1080 | with open(yaml_file, 'w') as f: 1081 | f.write(content) 1082 | 1083 | def _get_parameters(self): 1084 | """ 1085 | Returns parameter of this and all childs 1086 | :return: list of all parameters 1087 | """ 1088 | params = [] 1089 | params += self.parameters 1090 | for child in self.childs: 1091 | params.extend(child._get_parameters()) 1092 | return params 1093 | 1094 | def _get_subscribers(self): 1095 | """ 1096 | Subscriber of this and all childs 1097 | :return: list of all subscribers 1098 | """ 1099 | subscribers = [] 1100 | subscribers.extend(self.subscribers) 1101 | for child in self.childs: 1102 | subscribers.extend(child._get_subscribers()) 1103 | return subscribers 1104 | 1105 | def _get_publishers(self): 1106 | """ 1107 | Subscriber of this and all childs 1108 | :return: list of all publishers 1109 | """ 1110 | publishers = [] 1111 | publishers.extend(self.publishers) 1112 | for child in self.childs: 1113 | publishers.extend(child._get_publishers()) 1114 | return publishers 1115 | 1116 | def _get_root(self): 1117 | """ 1118 | Returns the root object (for groups) 1119 | :return: 1120 | """ 1121 | if not self.parent: 1122 | return self 1123 | return self.parent._get_root() 1124 | 1125 | def _generate_param_entries(self): 1126 | """ 1127 | Generates the entries for the cfg-file 1128 | :return: list of param entries as string 1129 | """ 1130 | param_entries = [] 1131 | dynamic_params = [p for p in self.parameters if p["configurable"]] 1132 | 1133 | if self.parent: 1134 | param_entries.append(Template("$group_variable = $parent.add_group('$group')").substitute( 1135 | group_variable=self.group_variable, 1136 | group=self.group, 1137 | parent=self.parent.group_variable)) 1138 | 1139 | for enum in self.enums: 1140 | param_entries.append(Template("$name = gen.enum([").substitute( 1141 | name=enum['name'], 1142 | parent=self.group)) 1143 | paramtype = self._pytype(enum['paramtype']) 1144 | for value in enum['values']: 1145 | enum_val = enum['values'].index(value) if paramtype == 'int' else "'" + value + "'" 1146 | param_entries.append( 1147 | Template(" gen.const(name='$name', type='$type', value=$value, descr='$descr'),") 1148 | .substitute(name=value, type=paramtype, value=enum_val, descr="")) 1149 | param_entries.append(Template(" ], '$description')").substitute(description=enum["description"])) 1150 | 1151 | for param in dynamic_params: 1152 | content_line = Template("$group_variable.add(name = '$name', paramtype = '$paramtype', level = $level, " 1153 | "description = '$description', edit_method=$edit_method").substitute( 1154 | group_variable=self.group_variable, 1155 | name=param["name"], 1156 | paramtype=param['pytype'], 1157 | level=param['level'], 1158 | edit_method=param['edit_method'], 1159 | description=param['description']) 1160 | if param['default'] is not None: 1161 | content_line += Template(", default=$default").substitute(default=self._get_pyvalue(param, "default")) 1162 | if param['min'] is not None: 1163 | content_line += Template(", min=$min").substitute(min=param['min']) 1164 | if param['max'] is not None: 1165 | content_line += Template(", max=$max").substitute(max=param['max']) 1166 | content_line += ")" 1167 | param_entries.append(content_line) 1168 | 1169 | for child in self.childs: 1170 | param_entries.extend(child._generate_param_entries()) 1171 | return param_entries 1172 | 1173 | @staticmethod 1174 | def _make_bool(param): 1175 | if isinstance(param, bool): 1176 | return param 1177 | else: 1178 | # Pray and hope that it is a string 1179 | return bool(param) 1180 | 1181 | 1182 | # Create derived class for yaml generation 1183 | class YamlGenerator(InterfaceGenerator): 1184 | def _generateImpl(self): 1185 | self._generateyml() 1186 | return 0 1187 | -------------------------------------------------------------------------------- /templates/ConfigType.h.template: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # ********************************************************* 3 | # 4 | # File autogenerated for the $pkgname package 5 | # by the rosinterface_handler package. 6 | # Please do not edit. 7 | # 8 | # ********************************************************/ 9 | 10 | from dynamic_reconfigure.parameter_generator_catkin import * 11 | 12 | gen = ParameterGenerator() 13 | 14 | $params 15 | 16 | exit(gen.generate('$pkgname', '$nodename', '$classname')) 17 | -------------------------------------------------------------------------------- /templates/Interface.h.template: -------------------------------------------------------------------------------- 1 | // ********************************************************* 2 | // 3 | // File autogenerated for the ${pkgname} package 4 | // by the rosinterface_handler package. 5 | // Please do not edit. 6 | // 7 | // ********************************************************/ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #ifdef MESSAGE_FILTERS_FOUND 20 | #include 21 | $includes 22 | #else 23 | $includeMessageFiltersError 24 | #endif 25 | #ifndef DIAGNOSTIC_UPDATER_FOUND 26 | $includeDiagnosticUpdaterError 27 | #endif 28 | #ifdef DYNAMIC_RECONFIGURE_FOUND 29 | #pragma GCC diagnostic push 30 | #pragma GCC diagnostic ignored "-Wparentheses" 31 | #include <${pkgname}/${ClassName}Config.h> 32 | #pragma GCC diagnostic pop 33 | #else 34 | struct ${ClassName}Config{}; 35 | #endif 36 | 37 | namespace ${pkgname} { 38 | 39 | /// \brief Parameter struct generated by rosinterface_handler 40 | struct ${ClassName}Interface { 41 | 42 | using Config = ${ClassName}Config; 43 | #ifdef MESSAGE_FILTERS_FOUND 44 | template 45 | using Subscriber = message_filters::Subscriber; 46 | template 47 | using SubscriberPtr = std::shared_ptr>; 48 | #endif 49 | #ifdef IF_HANDLER_DIAGNOSTICS_INCLUDED 50 | template> 51 | using DiagSubscriber = rosinterface_handler::DiagnosedSubscriber; 52 | template> 53 | using DiagSubscriberPtr = std::shared_ptr>; 54 | template 55 | using DiagPublisher = rosinterface_handler::DiagnosedPublisher; 56 | #endif 57 | 58 | ${ClassName}Interface(const ros::NodeHandle& private_node_handle) 59 | : globalNamespace_{"/"}, 60 | publicNamespace_{rosinterface_handler::getParentNamespace(private_node_handle) + "/"}, 61 | privateNamespace_{private_node_handle.getNamespace() + "/"}, 62 | nodeName_{rosinterface_handler::getNodeName(private_node_handle)}, 63 | privateNodeHandle_{private_node_handle}$initSubscribers {} 64 | 65 | /// \brief Get values from parameter server 66 | /// 67 | /// Will fail if a value can not be found and no default value is given. 68 | void fromParamServer(){ 69 | bool success = true; 70 | $fromParamServer 71 | 72 | $subscribeAdvertiseFromParamServer 73 | 74 | $test_limits 75 | if(!success){ 76 | missingParamsWarning(); 77 | rosinterface_handler::exit("RosinterfaceHandler: GetParam could net retrieve parameter."); 78 | } 79 | ROS_DEBUG_STREAM(*this); 80 | } 81 | 82 | /// \brief Set parameters on ROS parameter server. 83 | void toParamServer(){ 84 | $toParamServer 85 | } 86 | 87 | /// \brief Update configurable parameters. 88 | /// 89 | /// \param config dynamic reconfigure struct 90 | /// \level ? 91 | void fromConfig(const Config& config, const uint32_t level = 0){ 92 | #ifdef DYNAMIC_RECONFIGURE_FOUND 93 | $subscribeAdvertiseFromConfig 94 | $fromConfig 95 | #else 96 | ROS_FATAL_STREAM("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); 97 | rosinterface_handler::exit("dynamic_reconfigure was not found during compilation. So fromConfig() is not available. Please recompile with dynamic_reconfigure."); 98 | #endif 99 | } 100 | 101 | /// \brief Stream operator for printing parameter struct 102 | friend std::ostream& operator<<(std::ostream& os, const ${ClassName}Interface& p) 103 | { 104 | os << "[" << p.nodeNameWithNamespace() << "]\nNode " << p.nodeNameWithNamespace() << " has the following parameters:\n" 105 | $string_representation; 106 | return os; 107 | } 108 | 109 | /// \brief get the node handle that the interface struct uses internally 110 | ros::NodeHandle getPrivateNodeHandle() { 111 | return privateNodeHandle_; 112 | } 113 | 114 | /// \brief returns the name of the node (works in nodelets, too) 115 | const std::string& nodeName() const { 116 | return nodeName_; 117 | } 118 | 119 | /// \brief returns the name of the node with its namespace (works in nodelets, too) 120 | const std::string& nodeNameWithNamespace() const { 121 | return privateNodeHandle_.getNamespace(); 122 | } 123 | 124 | /// \brief logs to the debug output. Works also within nodelets. 125 | // NOLINTNEXTLINE(readability-function-size) 126 | template 127 | inline void logDebug(const Msg& msg, const Msgs&... Msgs_) const { 128 | ROS_DEBUG_STREAM_NAMED(nodeNameWithNamespace(), rosinterface_handler::asString(msg, Msgs_...)); 129 | } 130 | 131 | /// \brief logs to the debug output. Works also within nodelets. Output is throttled. 132 | // NOLINTNEXTLINE(readability-function-size) 133 | template 134 | inline void logInfo(const Msg& msg, const Msgs&... Msgs_) const { 135 | ROS_INFO_STREAM_THROTTLE_NAMED(5, nodeNameWithNamespace(), rosinterface_handler::asString(msg, Msgs_...)); 136 | } 137 | 138 | /// \brief logs to the debug output. Works also within nodelets. Output is throttled. 139 | // NOLINTNEXTLINE(readability-function-size) 140 | template 141 | inline void logWarn(const Msg& msg, const Msgs&... Msgs_) const { 142 | ROS_WARN_STREAM_THROTTLE_NAMED(5, nodeNameWithNamespace(), rosinterface_handler::asString(msg, Msgs_...)); 143 | } 144 | 145 | /// \brief logs to the error output. Works also within nodelets. Output is throttled. 146 | // NOLINTNEXTLINE(readability-function-size) 147 | template 148 | inline void logError(const Msg& msg, const Msgs&... Msgs_) const { 149 | ROS_ERROR_STREAM_THROTTLE_NAMED(5, nodeNameWithNamespace(), rosinterface_handler::asString(msg, Msgs_...)); 150 | } 151 | 152 | /// \brief logs to the error output. Works also within nodelets. Not throttled! Dont call this in loops! 153 | // NOLINTNEXTLINE(readability-function-size) 154 | template 155 | inline void logErrorDirect(const Msg& msg, const Msgs&... Msgs_) const { 156 | ROS_ERROR_STREAM_NAMED(nodeNameWithNamespace(), rosinterface_handler::asString(msg, Msgs_...)); 157 | } 158 | 159 | /// \brief logs subscribed and advertised topics to the command line. Works also within nodelets. 160 | // NOLINTNEXTLINE(readability-function-size) 161 | void showNodeInfo() const { 162 | std::string message = "Node '" + nodeName() + "' from package '$pkgname', type '$ClassName'" 163 | " in namespace '" + publicNamespace_ + "'.\nSubscribed to: ["; 164 | $print_subscribed 165 | message += "]\nAdvertising: ["; 166 | $print_advertised 167 | message += ']'; 168 | logInfo(message); 169 | } 170 | 171 | private: 172 | const std::string globalNamespace_; 173 | const std::string publicNamespace_; 174 | const std::string privateNamespace_; 175 | const std::string nodeName_; 176 | ros::NodeHandle privateNodeHandle_; 177 | 178 | public: 179 | $parameters 180 | $publishers 181 | $subscribers 182 | 183 | private: 184 | /// \brief Issue a warning about missing default parameters. 185 | void missingParamsWarning(){ 186 | ROS_WARN_STREAM("[" << nodeName_ << "]\nThe following parameters do not have default values and need to be specified:\n" 187 | $non_default_params ); 188 | } 189 | }; 190 | } // namespace ${pkgname} 191 | -------------------------------------------------------------------------------- /templates/Interface.py.template: -------------------------------------------------------------------------------- 1 | # ********************************************************* 2 | # 3 | # File autogenerated for the ${pkgname} package 4 | # by the rosinterface_handler package. 5 | # Please do not edit. 6 | # 7 | # ********************************************************* 8 | import rospy 9 | $imports 10 | 11 | inf = float('inf') 12 | param_description = $paramDescription 13 | subscriber_description = $subscriberDescription 14 | publisher_description = $publisherDescription 15 | verbosity_param = $verbosityParam 16 | tf_config = $tfConfig 17 | with_diagnostics = $diagnosticsEnabled 18 | simplified_diagnostics = $simplifiedDiagnostics 19 | 20 | if subscriber_description: 21 | # only import if necessary 22 | import message_filters 23 | if verbosity_param: 24 | import logging 25 | if tf_config: 26 | import tf2_ros 27 | 28 | 29 | defaults = {} 30 | 31 | for param in param_description: 32 | defaults[param['name']] = param['default'] 33 | for subscriber in subscriber_description: 34 | defaults[subscriber['name']] = None 35 | for publisher in publisher_description: 36 | defaults[publisher['name']] = None 37 | 38 | class _SimplifiedNodeStatus: 39 | def __init__(self, updater): 40 | self._updater = updater 41 | self._updater.add("status", self.get_status) 42 | self._s = diagnostic_msgs.msg.DiagnosticStatus.STALE 43 | self._msg = "Initializing" 44 | self._timer = rospy.Timer(rospy.Duration(1.01), lambda event: self._updater.update(), oneshot=False) 45 | 46 | def set(self, diagnostic_status, message): 47 | """ 48 | Update the status of this node. To be called via the interface class as 49 | Interface.node_status.set(DiagnosticStatus.OK, "Everything ok") 50 | """ 51 | new_status = diagnostic_status != self._s and self._msg != message 52 | self._s = diagnostic_status 53 | self._msg = message 54 | if new_status and self._s == diagnostic_msgs.msg.DiagnosticStatus.ERROR: 55 | self._updater.force_update() 56 | 57 | def get_status(self, status_wrapper): 58 | status_wrapper.summary(self._s, self._msg) 59 | 60 | 61 | class ${ClassName}Interface(dict): 62 | def __init__(self): 63 | super(self.__class__, self).__init__(defaults) 64 | self.from_param_server() 65 | if tf_config: 66 | self._init_tf() 67 | if with_diagnostics: 68 | from diagnostic_updater import Updater 69 | self["updater"] = Updater() 70 | self["updater"].setHardwareID("none") 71 | if simplified_diagnostics: 72 | self["node_status"] = _SimplifiedNodeStatus(self["updater"]) 73 | 74 | __getattr__ = dict.__getitem__ 75 | __setattr__ = dict.__setitem__ 76 | 77 | def from_param_server(self): 78 | """ 79 | Reads and initializes parameters, subscribers and publishers with values from parameter server. 80 | Called automatically at initialization. 81 | """ 82 | for k, v in self.items(): 83 | config = next((item for item in param_description if item["name"] == k), None) 84 | if not config: 85 | continue 86 | if config['constant']: 87 | self.test_const_param(k) 88 | continue 89 | self[k] = self.get_param(k, config) 90 | 91 | for subscriber in subscriber_description: 92 | name = subscriber['name'] 93 | topic = self._get_topic(subscriber['scope'], self[subscriber['topic_param']]) 94 | 95 | # rescue the callbacks if existing 96 | if name in self and self[name]: 97 | callbacks = self[name].callbacks 98 | self[name].callbacks = {} 99 | else: 100 | callbacks = {} 101 | self[name] = message_filters.Subscriber(topic, self.get_type(subscriber), 102 | queue_size=self[subscriber['queue_size_param']], 103 | tcp_nodelay=subscriber['no_delay']) 104 | self[name].callbacks = callbacks 105 | for publisher in publisher_description: 106 | name = publisher['name'] 107 | topic = self._get_topic(publisher['scope'], self[publisher['topic_param']]) 108 | self[name] = rospy.Publisher(topic, self.get_type(publisher), 109 | queue_size=self[publisher['queue_size_param']]) 110 | if verbosity_param: 111 | logging.getLogger('rosout').setLevel(self._map_verbosity(self[verbosity_param])) 112 | 113 | def to_param_server(self): 114 | """ 115 | Sets parameters with current values on the parameter server. 116 | """ 117 | for param_name, param_value in self.items(): 118 | config = next((item for item in param_description if item["name"] == param_name), None) 119 | if config and not config['constant']: 120 | full_name = "/" + param_name if config['global_scope'] else "~" + param_name 121 | if config['type'] == 'int64_t': 122 | rospy.set_param(full_name, str(param_value)+"L") 123 | else: 124 | rospy.set_param(full_name, param_value) 125 | 126 | def from_config(self, config): 127 | """ 128 | Reads parameter from a dynamic_reconfigure config file. 129 | Should be called in the callback of dynamic_reconfigure. 130 | :param config: config object from dynamic reconfigure 131 | """ 132 | if verbosity_param and verbosity_param in config and config[verbosity_param] != self[verbosity_param]: 133 | logging.getLogger('rosout').setLevel(self._map_verbosity(config[verbosity_param])) 134 | rospy.logdebug("Verbosity set to " + config[verbosity_param]) 135 | for subscriber in subscriber_description: 136 | topic_param = subscriber['topic_param'] 137 | if not topic_param in config: 138 | continue 139 | name = subscriber['name'] 140 | queue_param = subscriber['queue_size_param'] 141 | if config[topic_param] != self[topic_param] or config[queue_param] != self[queue_param]: 142 | callbacks = self[name].callbacks 143 | self[name].callbacks = {} 144 | topic = self._get_topic(subscriber['scope'], config[topic_param]) 145 | self[name] = message_filters.Subscriber(topic, self.get_type(subscriber), 146 | queue_size=config[queue_param], 147 | tcp_nodelay=subscriber['no_delay']) 148 | self[name].callbacks = callbacks 149 | for publisher in publisher_description: 150 | topic_param = publisher['topic_param'] 151 | if not topic_param in config: 152 | continue 153 | name = publisher['name'] 154 | queue_param = publisher['queue_size_param'] 155 | topic = self._get_topic(publisher['scope'], config[topic_param]) 156 | if config[topic_param] != self[topic_param] or config[queue_param] != self[queue_param]: 157 | self[name] = rospy.Publisher(topic, self.get_type(publisher), queue_size=config[queue_param]) 158 | for k, v in config.items(): 159 | # handle reserved name groups 160 | if k == "groups": 161 | continue 162 | if not k in self: 163 | raise TypeError("Element {} of config is not part of parameters.".format(k)) 164 | self[k] = v 165 | 166 | def _init_tf(self): 167 | listener = tf_config["listener_name"] 168 | buffer = tf_config["buffer_name"] 169 | broadcaster = tf_config["broadcaster_name"] 170 | if buffer: 171 | self[buffer] = tf2_ros.Buffer() 172 | if listener: 173 | self[listener] = tf2_ros.TransformListener(self[buffer]) 174 | if broadcaster: 175 | self[broadcaster] = tf2_ros.TransformBroadcaster 176 | 177 | @staticmethod 178 | def test_const_param(param_name): 179 | if rospy.has_param("~" + param_name): 180 | rospy.logwarn( 181 | "Parameter {} was set on the parameter server even though it was defined to be constant.".format( 182 | param_name)) 183 | 184 | @staticmethod 185 | def get_param(param_name, config): 186 | def get_type(type_string): 187 | if type_string == 'std::string': 188 | return str 189 | elif type_string == 'int': 190 | return int 191 | elif type_string == 'bool': 192 | return bool 193 | elif type_string == 'float' or type_string == 'double': 194 | return float 195 | elif type_string == 'int64_t': 196 | return int 197 | else: 198 | raise ValueError() 199 | 200 | full_name = "/" + param_name if config['global_scope'] else "~" + param_name 201 | try: 202 | val = rospy.get_param(full_name) 203 | except KeyError: 204 | if config['default'] is None: 205 | raise KeyError("Parameter {} is neither set on the parameter server nor " 206 | "has it a default value".format(param_description)) 207 | rospy.loginfo("Parameter {} is not yet set. Setting default value".format(param_name)) 208 | rospy.set_param(full_name, config['default']) 209 | val = config['default'] 210 | 211 | # test whether type is correct 212 | try: 213 | if config['is_vector']: 214 | val = list(val) 215 | config_type = config['type'] 216 | val_type = get_type(config_type[config_type.find("<")+1:config_type.find(">")]) 217 | val = [ val_type(v) for v in val ] 218 | elif config['is_map']: 219 | val = dict(val) 220 | config_type = config['type'] 221 | key_type = get_type(config_type[config_type.find("<")+1:config_type.find(",")]) 222 | val_type = get_type(config_type[config_type.find(",")+1:config_type.find(">")]) 223 | val = { key_type(key): val_type(v) for key, v in val.items() } 224 | elif config['type'] == 'int64_t' and type(val) is str: 225 | val = val.replace("L","",1) 226 | val = int(val) 227 | else: 228 | val = get_type(config['type'])(val) 229 | except ValueError: 230 | rospy.logerr( 231 | "Parameter {} is set, but has a different type ({} instead of {}). Using default value instead.".format( 232 | param_name, type(val), config['type'])) 233 | val = config['default'] 234 | # test bounds 235 | if config['min'] is not None: 236 | if config['is_vector']: 237 | if min(val) < config['min']: 238 | rospy.logwarn( 239 | "Some values in {} for {} are smaller than minimal allowed value. " 240 | "Correcting them to min={}".format(val, param_name, config['min'])) 241 | val = [ v if v > config['min'] else config['min'] for v in val ] 242 | elif config['is_map']: 243 | if min(val.values()) < config['min']: 244 | rospy.logwarn( 245 | "Some values in {} for {} are smaller than minimal allowed value. " 246 | "Correcting them to min={}".format(val, param_name, config['min'])) 247 | val = { k: (v if v > config['min'] else config['min']) for k, v in val.items() } 248 | elif val < config['min']: 249 | rospy.logwarn( 250 | "Value of {} for {} is smaller than minimal allowed value. " 251 | "Correcting value to min={}".format(val, param_name, config['min'])) 252 | val = config['min'] 253 | 254 | if config['max'] is not None: 255 | if config['is_vector']: 256 | if max(val) > config['max']: 257 | rospy.logwarn( 258 | "Some values in {} for {} are greater than maximal allowed value. " 259 | "Correcting them to max={}".format(val, param_name, config['max'])) 260 | val = [ v if v < config['max'] else config['max'] for v in val ] 261 | elif config['is_map']: 262 | if max(val.values()) > config['max']: 263 | rospy.logwarn( 264 | "Some values in {} for {} are greater than maximal allowed value. " 265 | "Correcting them to max={}".format(val, param_name, config['max'])) 266 | val = { k: (v if v < config['max'] else config['max']) for k, v in val.items() } 267 | elif val > config['max']: 268 | rospy.logwarn( 269 | "Value of {} for {} is greater than maximal allowed value. " 270 | "Correcting value to max={}".format(val, param_name, config['max'])) 271 | val = config['max'] 272 | return val 273 | 274 | @staticmethod 275 | def get_type(config): 276 | for import_cmd in config['import']: 277 | try: 278 | return eval(import_cmd + '.' + config['type'].split('::')[-1]) 279 | except: 280 | pass 281 | 282 | 283 | @staticmethod 284 | def _get_topic(scope, topic): 285 | if not topic.startswith('/') and scope != 'public': 286 | return "/" + topic if scope == 'global' else "~" + topic 287 | return topic 288 | 289 | @staticmethod 290 | def _map_verbosity(verbosity): 291 | return { 292 | 'DEBUG': logging.DEBUG, 293 | 'INFO': logging.INFO, 294 | 'WARN': logging.WARN, 295 | 'WARNING': logging.WARN, 296 | 'ERROR': logging.ERROR, 297 | 'FATAL': logging.FATAL, 298 | }[verbosity.upper()] 299 | -------------------------------------------------------------------------------- /test/cfg/Defaults.rosif: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ####### workaround so that the module is found ####### 3 | import sys 4 | import os 5 | sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) 6 | ###################################################### 7 | 8 | from rosinterface_handler.interface_generator_catkin import * 9 | gen = InterfaceGenerator() 10 | 11 | gen.add_verbosity_param("verbosity_param_w_default", configurable=False, default='info') 12 | gen.add_diagnostic_updater(simplified_status=True) 13 | gen.add_tf(buffer_name="tf_buffer", listener_name="tf_listener", broadcaster_name="tf_broadcaster") 14 | 15 | # Parameters with different types 16 | gen.add("int_param_w_default", paramtype="int", description="An Integer parameter", default=1, configurable=True) 17 | gen.add("double_param_w_default", paramtype="double",description="A double parameter", default=1.1) 18 | gen.add("str_param_w_default", paramtype="std::string", description="A string parameter", default="Hello World") 19 | gen.add("bool_param_w_default", paramtype="bool", description="A Boolean parameter", default=True) 20 | gen.add("long_param_w_default_int", paramtype="int64_t", description="A long parameter", default=1) 21 | gen.add("long_param_w_default_int_str", paramtype="int64_t", description="A long parameter", default="-1") 22 | # gen.add("long_param_w_default_long", paramtype="int64_t", description="A long parameter", default=9223372036854775807) # (2**63 - 1) would fail due to xmlrpc lib 23 | gen.add("long_param_w_default_long_string", paramtype="int64_t", description="A long parameter", default="9223372036854775807L") 24 | 25 | gen.add("vector_int_param_w_default", paramtype="std::vector", description="A vector of int parameter", default=[1,2,3]) 26 | gen.add("vector_double_param_w_default", paramtype="std::vector", description="A vector of double parameter", default=[1.1, 1.2, 1.3]) 27 | gen.add("vector_bool_param_w_default", paramtype="std::vector", description="A vector of bool parameter", default=[False, True]) 28 | gen.add("vector_string_param_w_default", paramtype="std::vector", description="A vector of string parameter", default=["Hello", "World"]) 29 | gen.add("map_param_w_default", paramtype="std::map", description="A map parameter", default={"Hello": "World"}) 30 | 31 | gen.add_enum("enum_int_param_w_default", description="enum", entry_strings=["Small", "Medium", "Large", "ExtraLarge"], default="Medium", paramtype='int') 32 | gen.add_enum("enum_str_param_w_default", description="string enum", entry_strings=["Zero", "One", "Two", "Three"], default="One", paramtype='std::string') 33 | 34 | p1 = gen.add_publisher("publisher_w_default", description="publisher", default_topic="out_topic", message_type="std_msgs::Header", configurable=True) 35 | p2 = gen.add_publisher("publisher_diag_w_default", description="publisher", default_topic="out_point_topic", message_type="geometry_msgs::PointStamped", configurable=True, diagnosed=True) 36 | p3 = gen.add_publisher("publisher_public_w_default", description="public publisher", default_topic="out_topic", message_type="std_msgs::Header", scope="public", configurable=True) 37 | gen.add_publisher("publisher_global_w_default", description="global publisher", default_topic="out_topic", message_type="std_msgs::Header", scope="global", configurable=True) 38 | gen.add_subscriber("subscriber_w_default", description="subscriber", default_topic="in_topic", message_type="std_msgs::Header", configurable=True) 39 | gen.add_subscriber("subscriber_diag_w_default", description="subscriber", default_topic="in_point_topic", message_type="geometry_msgs::PointStamped", configurable=True, diagnosed=True) 40 | gen.add_subscriber("subscriber_public_w_default", description="public subscriber", default_topic="in_topic", message_type="std_msgs::Header", scope="public", configurable=True) 41 | gen.add_subscriber("subscriber_global_w_default", description="global subscriber", default_topic="in_topic", message_type="std_msgs::Header", scope="global", configurable=True) 42 | gen.add_subscriber("subscriber_smart", description="smart subscriber", default_topic="in_topic2", message_type="geometry_msgs::PointStamped", watch=[p1, p2, p3], configurable=True) 43 | gen.add_subscriber("subscriber_smart_diagnosed", description="smart diagnosed subscriber", default_topic="in_topic2", message_type="geometry_msgs::PointStamped", watch=[p1, p2, p3], diagnosed=True) 44 | 45 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 46 | exit(gen.generate("rosinterface_handler", "rosinterface_handler_test", "Defaults")) 47 | -------------------------------------------------------------------------------- /test/cfg/DefaultsAtLaunch.rosif: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ####### workaround so that the module is found ####### 3 | import sys 4 | import os 5 | sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) 6 | ###################################################### 7 | from rosinterface_handler.interface_generator_catkin import * 8 | gen = InterfaceGenerator() 9 | 10 | gen.add_verbosity_param("verbosity_param_wo_default", default=None, configurable=True) 11 | 12 | # Parameters with different types 13 | gen.add("int_param_wo_default", paramtype="int", description="An Integer parameter") 14 | gen.add("double_param_wo_default", paramtype="double",description="A double parameter") 15 | gen.add("str_param_wo_default", paramtype="std::string", description="A string parameter") 16 | gen.add("bool_param_wo_default", paramtype="bool", description="A Boolean parameter") 17 | gen.add("long_param_wo_default_int", paramtype="int64_t", description="A long parameter") 18 | gen.add("long_param_wo_default_int_str", paramtype="int64_t", description="A long parameter") 19 | gen.add("long_param_wo_default_long_str", paramtype="int64_t", description="A long parameter") 20 | 21 | gen.add("vector_int_param_wo_default", paramtype="std::vector", description="A vector of int parameter") 22 | gen.add("vector_double_param_wo_default", paramtype="std::vector", description="A vector of double parameter") 23 | gen.add("vector_bool_param_wo_default", paramtype="std::vector", description="A vector of bool parameter") 24 | gen.add("vector_string_param_wo_default", paramtype="std::vector", description="A vector of string parameter") 25 | 26 | gen.add("map_param_wo_default", paramtype="std::map", description="A map parameter") 27 | 28 | gen.add_enum("enum_int_param_wo_default", description="int enum", entry_strings=["Small", "Medium", "Large", "ExtraLarge"]) 29 | gen.add_enum("enum_str_param_wo_default", description="string enum", entry_strings=["Zero", "One", "Two", "Three"], paramtype='std::string') 30 | 31 | gen.add_publisher("publisher_wo_default", description="publisher", message_type="std_msgs::Header") 32 | gen.add_publisher("publisher_public_wo_default", description="public publisher", message_type="std_msgs::Header", scope='public') 33 | gen.add_publisher("publisher_global_wo_default", description="global publisher", message_type="std_msgs::Header", scope='global') 34 | gen.add_subscriber("subscriber_wo_default", description="subscriber", message_type="std_msgs::Header") 35 | gen.add_subscriber("subscriber_public_wo_default", description="public subscriber", message_type="std_msgs::Header", scope="public") 36 | gen.add_subscriber("subscriber_global_wo_default", description="global subscriber", message_type="std_msgs::Header", scope='global') 37 | 38 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 39 | exit(gen.generate("rosinterface_handler", "rosinterface_handler_test", "DefaultsAtLaunch")) 40 | -------------------------------------------------------------------------------- /test/cfg/DefaultsMissing.rosif: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ####### workaround so that the module is found for test files ####### 3 | import sys 4 | import os 5 | sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) 6 | ###################################################### 7 | 8 | from rosinterface_handler.interface_generator_catkin import * 9 | gen = InterfaceGenerator() 10 | 11 | gen.add("int_param", paramtype="int", description="Handler should keep node from starting, when no default is given here or at launch.") 12 | 13 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 14 | exit(gen.generate("rosinterface_handler", "rosinterface_handler_test", "DefaultsMissing")) 15 | -------------------------------------------------------------------------------- /test/cfg/MinMax.rosif: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | ####### workaround so that the module is found ####### 3 | import sys 4 | import os 5 | sys.path.append(os.path.join(os.path.dirname(__file__),"../../src")) 6 | ###################################################### 7 | 8 | from rosinterface_handler.interface_generator_catkin import * 9 | gen = InterfaceGenerator() 10 | 11 | # Parameters with different types 12 | gen.add("int_param_w_minmax", paramtype="int", description="An Integer parameter", default=3, min=0, max=2) 13 | gen.add("double_param_w_minmax", paramtype="double",description="A double parameter", default=3.1, min=0., max=2.) 14 | 15 | gen.add("vector_int_param_w_minmax", paramtype="std::vector", description="A vector of int parameter", default=[-1,2,3], min=0, max=2) 16 | 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.) 17 | 18 | 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.) 19 | 20 | #Syntax : Package, Node, Config Name(The final name will be MyDummyConfig) 21 | exit(gen.generate("rosinterface_handler", "rosinterface_handler_test", "MinMax")) 22 | -------------------------------------------------------------------------------- /test/launch/params/test_launch_parameters.yaml: -------------------------------------------------------------------------------- 1 | # Use this file to specify parameters, that do not have default values. 2 | # YAML Files can also be helpfull if you want to save parameters for different use-cases. 3 | # In any way: Default values should go into the .params file! 4 | # 5 | # Use 'key: value' pairs, e.g. 6 | # string: 'foo' 7 | # integer: 1234 8 | # float: 1234.5 9 | # boolean: true 10 | # vector: [1.0, 2.0, 3.4] 11 | # map: {"a": "b", "c": "d"} 12 | 13 | verbosity_param_wo_default: 'info' 14 | int_param_wo_default: 1 15 | double_param_wo_default: 1.1 16 | str_param_wo_default: "Hello World" 17 | bool_param_wo_default: true 18 | long_param_wo_default_int: 1 19 | long_param_wo_default_int_str: "1" 20 | # long_param_wo_default_long: 9223372036854775807 # (2**63 - 1) would fail due to xmlrpc lib 21 | long_param_wo_default_long_str: "9223372036854775807L" 22 | vector_int_param_wo_default: [1,2,3] 23 | vector_double_param_wo_default: [1.1, 1.2, 1.3] 24 | vector_bool_param_wo_default: [false, true] 25 | vector_string_param_wo_default: ["Hello","World"] 26 | map_param_wo_default: {"Hello": "World"} 27 | enum_int_param_wo_default: 1 28 | enum_str_param_wo_default: "Two" 29 | subscriber_wo_default_topic: "in_topic" 30 | subscriber_public_wo_default_topic: "in_topic" 31 | subscriber_global_wo_default_topic: "in_topic" 32 | publisher_wo_default_topic: "out_topic" 33 | publisher_public_wo_default_topic: "out_topic" 34 | publisher_global_wo_default_topic: "out_topic" 35 | -------------------------------------------------------------------------------- /test/launch/rosinterface_handler.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /test/launch/rosinterface_handler_python.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /test/python/rosinterface_handler_python_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import tests 4 | import rospy 5 | import unittest 6 | PKG = 'rosinterface_handler' 7 | 8 | if __name__ == '__main__': 9 | import rostest 10 | 11 | rospy.init_node(PKG) 12 | rostest.rosrun(PKG, "RosinterfaceTestSuite", "tests.RosinterfaceTestSuite") 13 | -------------------------------------------------------------------------------- /test/python/tests/__init__.py: -------------------------------------------------------------------------------- 1 | from os.path import dirname, basename, isfile 2 | import glob 3 | import unittest 4 | # automatically import all files in this module 5 | modules = glob.glob(dirname(__file__) + "/*.py") 6 | __all__ = [basename(f)[:-3] for f in modules if isfile(f) and not f.endswith('__init__.py')] 7 | 8 | 9 | class RosinterfaceTestSuite(unittest.TestSuite): 10 | 11 | def __init__(self): 12 | super(RosinterfaceTestSuite, self).__init__() 13 | 14 | # Collect test cases 15 | testcases = {} 16 | for test_module in __all__: 17 | module = __import__("tests." + test_module, fromlist=['*']) 18 | for name in dir(module): 19 | obj = getattr(module, name) 20 | if isinstance(obj, type) and issubclass(obj, unittest.TestCase): 21 | testcases[name] = unittest.TestLoader().loadTestsFromTestCase(obj) 22 | 23 | # Add testcases 24 | for test_name, test in testcases.items(): 25 | self.addTest(test) 26 | -------------------------------------------------------------------------------- /test/python/tests/test_defaults.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from rosinterface_handler.interface.DefaultsInterface import DefaultsInterface 3 | import rospy 4 | import diagnostic_updater 5 | 6 | 7 | class TestDefaults(unittest.TestCase): 8 | def test_defaults(self): 9 | """ 10 | tests defaults on server 11 | :return: 12 | """ 13 | params = DefaultsInterface() 14 | # make sure from_param_server can be called repeatedly 15 | params.from_param_server() 16 | 17 | self.assertEqual(params.verbosity_param_w_default, 'info') 18 | 19 | self.assertEqual(params.int_param_w_default, 1) 20 | self.assertAlmostEqual(params.double_param_w_default, 1.1) 21 | self.assertEqual(params.str_param_w_default, "Hello World") 22 | self.assertEqual(params.bool_param_w_default, True) 23 | self.assertEqual(params.long_param_w_default_int, 1) 24 | self.assertEqual(params.long_param_w_default_int_str, -1) 25 | self.assertEqual(params.long_param_w_default_long_string, 9223372036854775807) 26 | 27 | self.assertEqual(params.vector_int_param_w_default, [1, 2, 3]) 28 | self.assertEqual(params.vector_double_param_w_default, [1.1, 1.2, 1.3]) 29 | self.assertEqual(params.vector_string_param_w_default, ["Hello", "World"]) 30 | 31 | self.assertEqual(params.map_param_w_default, {"Hello": "World"}) 32 | self.assertEqual(params.enum_int_param_w_default, 1) 33 | self.assertEqual(params.enum_str_param_w_default, "One") 34 | 35 | def test_node_status(self): 36 | params = DefaultsInterface() 37 | params.node_status.set(diagnostic_updater.DiagnosticStatus.ERROR, "test error") 38 | params.node_status.set(diagnostic_updater.DiagnosticStatus.OK, "ok") 39 | 40 | def test_defaults_subscriber(self): 41 | params = DefaultsInterface() 42 | self.assertEqual(params.subscriber_w_default.sub.name, "/test/rosinterface_handler_python_test/in_topic") 43 | self.assertEqual(params.subscriber_public_w_default.sub.name, "/test/in_topic") 44 | self.assertEqual(params.subscriber_global_w_default.sub.name, "/in_topic") 45 | 46 | def test_defaults_publisher(self): 47 | params = DefaultsInterface() 48 | self.assertEqual(params.publisher_w_default.name, "/test/rosinterface_handler_python_test/out_topic") 49 | self.assertEqual(params.publisher_public_w_default.name, "/test/out_topic") 50 | self.assertEqual(params.publisher_global_w_default.name, "/out_topic") 51 | 52 | def test_defaults_on_server(self): 53 | params = DefaultsInterface() 54 | # now all parameters should be set on param server 55 | self.assertEqual(params.verbosity_param_w_default, rospy.get_param("~verbosity_param_w_default")) 56 | 57 | self.assertEqual(params.int_param_w_default, rospy.get_param("~int_param_w_default")) 58 | self.assertAlmostEqual(params.double_param_w_default, rospy.get_param("~double_param_w_default")) 59 | self.assertEqual(params.str_param_w_default, rospy.get_param("~str_param_w_default")) 60 | self.assertEqual(params.bool_param_w_default, rospy.get_param("~bool_param_w_default")) 61 | 62 | self.assertEqual(params.vector_int_param_w_default, rospy.get_param("~vector_int_param_w_default")) 63 | self.assertEqual(params.vector_double_param_w_default, rospy.get_param("~vector_double_param_w_default")) 64 | self.assertEqual(params.vector_string_param_w_default, rospy.get_param("~vector_string_param_w_default")) 65 | 66 | self.assertEqual(params.map_param_w_default, rospy.get_param("~map_param_w_default")) 67 | self.assertEqual(params.enum_int_param_w_default, rospy.get_param("~enum_int_param_w_default")) 68 | self.assertEqual(params.enum_str_param_w_default, rospy.get_param("~enum_str_param_w_default")) 69 | 70 | def test_set_parameters_on_server(self): 71 | params = DefaultsInterface() 72 | 73 | params.verbosity_param_w_default = 'warning' 74 | params.int_param_w_default = 2 75 | params.double_param_w_default = 2.2 76 | params.str_param_w_default = "World Hello" 77 | params.bool_param_w_default = False 78 | params.long_param_w_default_int = 1 79 | params.long_param_w_default_int_str = -1 80 | params.long_param_w_default_long_string = 9223372036854775807 81 | params.vector_int_param_w_default = [3, 2, 1] 82 | params.vector_double_param_w_default = [1.3, 1.2, 1.2] 83 | params.vector_bool_param_w_default = [True, False] 84 | params.vector_string_param_w_default = ["World", "Hello"] 85 | params.map_param_w_default = {"World": "Hello"} 86 | params.enum_int_param_w_default = 2 87 | params.enum_str_param_w_default = "Two" 88 | 89 | # Upload parameters 90 | params.to_param_server() 91 | 92 | # now all parameters should be set on param server 93 | self.assertEqual(params.verbosity_param_w_default, rospy.get_param("~verbosity_param_w_default")) 94 | 95 | self.assertEqual(params.int_param_w_default, rospy.get_param("~int_param_w_default")) 96 | self.assertAlmostEqual(params.double_param_w_default, rospy.get_param("~double_param_w_default")) 97 | self.assertEqual(params.str_param_w_default, rospy.get_param("~str_param_w_default")) 98 | self.assertEqual(params.bool_param_w_default, rospy.get_param("~bool_param_w_default")) 99 | # on parameter server, long is stored as string with appended "L" 100 | self.assertEqual("1L", rospy.get_param("~long_param_w_default_int")) 101 | self.assertEqual("-1L", rospy.get_param("~long_param_w_default_int_str")) 102 | self.assertEqual("9223372036854775807L", rospy.get_param("~long_param_w_default_long_string")) 103 | 104 | self.assertEqual(params.vector_int_param_w_default, rospy.get_param("~vector_int_param_w_default")) 105 | self.assertEqual(params.vector_double_param_w_default, rospy.get_param("~vector_double_param_w_default")) 106 | self.assertEqual(params.vector_string_param_w_default, rospy.get_param("~vector_string_param_w_default")) 107 | 108 | self.assertEqual(params.map_param_w_default, rospy.get_param("~map_param_w_default")) 109 | self.assertEqual(params.enum_int_param_w_default, rospy.get_param("~enum_int_param_w_default")) 110 | self.assertEqual(params.enum_str_param_w_default, rospy.get_param("~enum_str_param_w_default")) 111 | 112 | def test_set_reconfigure_config(self): 113 | interface = DefaultsInterface() 114 | interface.from_param_server() 115 | 116 | config = dict() 117 | config['int_param_w_default'] = 2 118 | config['subscriber_w_default_topic'] = "/in_topic" 119 | config['subscriber_public_w_default_topic'] = "/in_topic" 120 | config['subscriber_global_w_default_topic'] = "/in_topic" 121 | config['subscriber_w_default_queue_size'] = 5 122 | config['subscriber_public_w_default_queue_size'] = 5 123 | config['subscriber_global_w_default_queue_size'] = 5 124 | config['publisher_w_default_topic'] = "/out_topic" 125 | config['publisher_public_w_default_topic'] = "/out_topic" 126 | config['publisher_global_w_default_topic'] = "/out_topic" 127 | config['publisher_w_default_queue_size'] = 5 128 | config['publisher_public_w_default_queue_size'] = 5 129 | config['publisher_global_w_default_queue_size'] = 5 130 | 131 | interface.from_config(config) 132 | 133 | # test params 134 | self.assertEqual(config['int_param_w_default'], interface.int_param_w_default) 135 | 136 | # test subscriber 137 | self.assertEqual(interface.subscriber_w_default.sub.name, "/in_topic") 138 | self.assertEqual(interface.subscriber_public_w_default.sub.name, "/in_topic") 139 | self.assertEqual(interface.subscriber_global_w_default.sub.name, "/in_topic") 140 | 141 | # test publisher 142 | self.assertEqual(interface.publisher_w_default.name, "/out_topic") 143 | self.assertEqual(interface.publisher_public_w_default.name, "/out_topic") 144 | self.assertEqual(interface.publisher_global_w_default.name, "/out_topic") 145 | -------------------------------------------------------------------------------- /test/python/tests/test_defaultsAtLaunch.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from rosinterface_handler.interface.DefaultsAtLaunchInterface import DefaultsAtLaunchInterface 3 | 4 | 5 | class TestDefaultsAtLaunch(unittest.TestCase): 6 | def test_defaults_at_launch(self): 7 | params = DefaultsAtLaunchInterface() 8 | 9 | self.assertEqual(params.verbosity_param_wo_default, 'info') 10 | 11 | self.assertEqual(params.int_param_wo_default, 1) 12 | self.assertAlmostEqual(params.double_param_wo_default, 1.1) 13 | self.assertEqual(params.str_param_wo_default, "Hello World") 14 | self.assertEqual(params.bool_param_wo_default, True) 15 | self.assertEqual(params.long_param_wo_default_int, 1) 16 | self.assertEqual(params.long_param_wo_default_int_str, 1) 17 | self.assertEqual(params.long_param_wo_default_long_str, 9223372036854775807) 18 | 19 | self.assertEqual(params.vector_int_param_wo_default, [1, 2, 3]) 20 | self.assertEqual(params.vector_double_param_wo_default, [1.1, 1.2, 1.3]) 21 | self.assertEqual(params.vector_string_param_wo_default, ["Hello", "World"]) 22 | 23 | self.assertEqual(params.map_param_wo_default, {"Hello": "World"}) 24 | self.assertEqual(params.enum_int_param_wo_default, 1) 25 | self.assertEqual(params.enum_str_param_wo_default, "Two") 26 | 27 | def test_defaults_at_launch_subscriber(self): 28 | params = DefaultsAtLaunchInterface() 29 | self.assertEqual(params.subscriber_wo_default.sub.name, "/test/rosinterface_handler_python_test/in_topic") 30 | self.assertEqual(params.subscriber_public_wo_default.sub.name, "/test/in_topic") 31 | self.assertEqual(params.subscriber_global_wo_default.sub.name, "/in_topic") 32 | 33 | def test_defaults_at_launch_publisher(self): 34 | params = DefaultsAtLaunchInterface() 35 | self.assertEqual(params.publisher_wo_default.name, "/test/rosinterface_handler_python_test/out_topic") 36 | self.assertEqual(params.publisher_public_wo_default.name, "/test/out_topic") 37 | self.assertEqual(params.publisher_global_wo_default.name, "/out_topic") 38 | -------------------------------------------------------------------------------- /test/python/tests/test_defaultsMissing.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from rosinterface_handler.interface.DefaultsMissingInterface import DefaultsMissingInterface 3 | 4 | 5 | class TestDefaultsMissingInterface(unittest.TestCase): 6 | def test_defaults_missing(self): 7 | with self.assertRaises(KeyError): 8 | params = DefaultsMissingInterface() 9 | -------------------------------------------------------------------------------- /test/python/tests/test_minMax.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from rosinterface_handler.interface.MinMaxInterface import MinMaxInterface 3 | 4 | 5 | class TestMinMaxInterface(unittest.TestCase): 6 | def test_min_max_parameters(self): 7 | params = MinMaxInterface() 8 | self.assertEqual(params.int_param_w_minmax, 2) 9 | self.assertAlmostEqual(params.double_param_w_minmax, 2.) 10 | 11 | self.assertEqual(params.vector_int_param_w_minmax, [0, 2, 2]) 12 | self.assertEqual(params.vector_double_param_w_minmax, [0, 1.2, 2.]) 13 | 14 | self.assertEqual(params.map_param_w_minmax, {"value1": 0., "value2": 1.2, "value3": 2.}) 15 | -------------------------------------------------------------------------------- /test/src/test_defaults.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using IfType = rosinterface_handler::DefaultsInterface; 6 | using ConfigType = rosinterface_handler::DefaultsConfig; 7 | 8 | TEST(RosinterfaceHandler, DefaultParams) { // NOLINT(readability-function-size) 9 | IfType testInterface(ros::NodeHandle("~")); 10 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 11 | 12 | ASSERT_EQ("info", testInterface.verbosity_param_w_default); 13 | 14 | ASSERT_EQ(1, testInterface.int_param_w_default); 15 | ASSERT_DOUBLE_EQ(1.1, testInterface.double_param_w_default); 16 | ASSERT_EQ("Hello World", testInterface.str_param_w_default); 17 | ASSERT_EQ(true, testInterface.bool_param_w_default); 18 | ASSERT_EQ(1L, testInterface.long_param_w_default_int); 19 | ASSERT_EQ(-1L, testInterface.long_param_w_default_int_str); 20 | ASSERT_EQ(9223372036854775807L, testInterface.long_param_w_default_long_string); 21 | 22 | ASSERT_EQ(std::vector({1, 2, 3}), testInterface.vector_int_param_w_default); 23 | ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testInterface.vector_double_param_w_default); 24 | ASSERT_EQ(std::vector({false, true}), testInterface.vector_bool_param_w_default); 25 | ASSERT_EQ(std::vector({"Hello", "World"}), testInterface.vector_string_param_w_default); 26 | 27 | std::map tmp{{"Hello", "World"}}; 28 | ASSERT_EQ(tmp, testInterface.map_param_w_default); 29 | 30 | ASSERT_EQ(1, testInterface.enum_int_param_w_default); 31 | ASSERT_EQ("One", testInterface.enum_str_param_w_default); 32 | testInterface.showNodeInfo(); 33 | } 34 | 35 | TEST(RosinterfaceHandler, NodeStatus) { 36 | IfType testInterface(ros::NodeHandle("~")); 37 | testInterface.nodeStatus.set(rosinterface_handler::NodeStatus::ERROR, "Test error"); 38 | testInterface.nodeStatus.set(rosinterface_handler::NodeStatus::OK, "Initialized"); 39 | } 40 | 41 | TEST(RosinterfaceHandler, DefaultSubscriber) { 42 | IfType testInterface(ros::NodeHandle("~")); 43 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 44 | 45 | ASSERT_TRUE(!!testInterface.subscriber_w_default); 46 | ASSERT_EQ(testInterface.subscriber_w_default->getTopic(), "/test/rosinterface_handler_test/in_topic"); 47 | 48 | ASSERT_TRUE(!!testInterface.subscriber_public_w_default); 49 | ASSERT_EQ(testInterface.subscriber_public_w_default->getTopic(), "/test/in_topic"); 50 | 51 | ASSERT_TRUE(!!testInterface.subscriber_global_w_default); 52 | ASSERT_EQ(testInterface.subscriber_global_w_default->getTopic(), "/in_topic"); 53 | } 54 | 55 | TEST(RosinterfaceHandler, DefaultPublisher) { 56 | IfType testInterface(ros::NodeHandle("~")); 57 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 58 | 59 | ASSERT_EQ(testInterface.publisher_w_default.getTopic(), "/test/rosinterface_handler_test/out_topic"); 60 | ASSERT_EQ(testInterface.publisher_public_w_default.getTopic(), "/test/out_topic"); 61 | ASSERT_EQ(testInterface.publisher_global_w_default.getTopic(), "/out_topic"); 62 | } 63 | 64 | TEST(RosinterfaceHandler, DefaultsOnParamServer) { // NOLINT(readability-function-size) 65 | ros::NodeHandle nh("~"); 66 | IfType testInterface(nh); 67 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 68 | 69 | // values should now be set on parameter server 70 | { 71 | std::string verbosity; 72 | ASSERT_TRUE(nh.getParam("verbosity_param_w_default", verbosity)); 73 | EXPECT_EQ(verbosity, testInterface.verbosity_param_w_default); 74 | } 75 | { 76 | int intInterface = 0; 77 | ASSERT_TRUE(nh.getParam("int_param_w_default", intInterface)); 78 | ASSERT_EQ(intInterface, testInterface.int_param_w_default); 79 | } 80 | { 81 | double doubleInterface = 0; 82 | ASSERT_TRUE(nh.getParam("double_param_w_default", doubleInterface)); 83 | EXPECT_EQ(doubleInterface, testInterface.double_param_w_default); 84 | } 85 | { 86 | bool boolInterface = false; 87 | ASSERT_TRUE(nh.getParam("bool_param_w_default", boolInterface)); 88 | EXPECT_EQ(boolInterface, testInterface.bool_param_w_default); 89 | } 90 | // this does not work for long, since 91 | // on parameter server, long is stored as string with appended "L" 92 | // workaround: 93 | { 94 | std::string stringInterface; 95 | ASSERT_TRUE(nh.getParam("long_param_w_default_int", stringInterface)); 96 | EXPECT_EQ(stringInterface, "1L"); 97 | } 98 | { 99 | std::string stringInterface; 100 | ASSERT_TRUE(nh.getParam("long_param_w_default_int_str", stringInterface)); 101 | EXPECT_EQ(stringInterface, "-1L"); 102 | } 103 | { 104 | std::string stringInterface; 105 | ASSERT_TRUE(nh.getParam("long_param_w_default_long_string", stringInterface)); 106 | EXPECT_EQ(stringInterface, "9223372036854775807L"); 107 | } 108 | { 109 | std::string stringInterface; 110 | ASSERT_TRUE(nh.getParam("str_param_w_default", stringInterface)); 111 | EXPECT_EQ(stringInterface, testInterface.str_param_w_default); 112 | } 113 | { 114 | std::vector vectorIntInterface; 115 | ASSERT_TRUE(nh.getParam("vector_int_param_w_default", vectorIntInterface)); 116 | EXPECT_EQ(vectorIntInterface, testInterface.vector_int_param_w_default); 117 | } 118 | { 119 | std::vector vectorDoubleInterface; 120 | ASSERT_TRUE(nh.getParam("vector_double_param_w_default", vectorDoubleInterface)); 121 | EXPECT_EQ(vectorDoubleInterface, testInterface.vector_double_param_w_default); 122 | } 123 | { 124 | std::vector vectorBoolInterface; 125 | ASSERT_TRUE(nh.getParam("vector_bool_param_w_default", vectorBoolInterface)); 126 | EXPECT_EQ(vectorBoolInterface, testInterface.vector_bool_param_w_default); 127 | } 128 | { 129 | std::vector vectorStringInterface; 130 | ASSERT_TRUE(nh.getParam("vector_string_param_w_default", vectorStringInterface)); 131 | EXPECT_EQ(vectorStringInterface, testInterface.vector_string_param_w_default); 132 | } 133 | { 134 | std::map mapParamWDefault; 135 | ASSERT_TRUE(nh.getParam("map_param_w_default", mapParamWDefault)); 136 | EXPECT_EQ(mapParamWDefault, testInterface.map_param_w_default); 137 | } 138 | { 139 | int enumIntInterface = 0; 140 | ASSERT_TRUE(nh.getParam("enum_int_param_w_default", enumIntInterface)); 141 | EXPECT_EQ(enumIntInterface, testInterface.enum_int_param_w_default); 142 | } 143 | { 144 | std::string enumStrInterface; 145 | ASSERT_TRUE(nh.getParam("enum_str_param_w_default", enumStrInterface)); 146 | EXPECT_EQ(enumStrInterface, testInterface.enum_str_param_w_default); 147 | } 148 | } 149 | 150 | TEST(RosinterfaceHandler, SetParamOnServer) { // NOLINT(readability-function-size) 151 | ros::NodeHandle nh("~"); 152 | IfType testInterface(nh); 153 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 154 | 155 | testInterface.verbosity_param_w_default = "warning"; 156 | testInterface.int_param_w_default = 2; 157 | testInterface.double_param_w_default = 2.2; 158 | testInterface.str_param_w_default = "World Hello"; 159 | testInterface.bool_param_w_default = false; 160 | testInterface.long_param_w_default_int = 1L; 161 | testInterface.long_param_w_default_int_str = -1L; 162 | testInterface.long_param_w_default_long_string = 9223372036854775807L; 163 | testInterface.vector_int_param_w_default = std::vector{3, 2, 1}; 164 | testInterface.vector_double_param_w_default = std::vector{1.3, 1.2, 1.2}; 165 | testInterface.vector_bool_param_w_default = std::vector{true, false}; 166 | testInterface.vector_string_param_w_default = std::vector{"World", "Hello"}; 167 | testInterface.map_param_w_default = std::map{{"World", "Hello"}}; 168 | testInterface.enum_int_param_w_default = 2; 169 | testInterface.enum_str_param_w_default = "Two"; 170 | 171 | testInterface.toParamServer(); 172 | 173 | // values should now be set on parameter server 174 | { 175 | std::string verbosity; 176 | ASSERT_TRUE(nh.getParam("verbosity_param_w_default", verbosity)); 177 | EXPECT_EQ(verbosity, testInterface.verbosity_param_w_default); 178 | } 179 | { 180 | int intInterface = 0; 181 | ASSERT_TRUE(nh.getParam("int_param_w_default", intInterface)); 182 | ASSERT_EQ(intInterface, testInterface.int_param_w_default); 183 | } 184 | { 185 | double doubleInterface = 0; 186 | ASSERT_TRUE(nh.getParam("double_param_w_default", doubleInterface)); 187 | EXPECT_EQ(doubleInterface, testInterface.double_param_w_default); 188 | } 189 | { 190 | bool boolInterface = false; 191 | ASSERT_TRUE(nh.getParam("bool_param_w_default", boolInterface)); 192 | EXPECT_EQ(boolInterface, testInterface.bool_param_w_default); 193 | } 194 | // this does not work for long, since 195 | // on parameter server, long is stored as string with appended "L" 196 | // workaround: 197 | { 198 | std::string stringInterface; 199 | ASSERT_TRUE(nh.getParam("long_param_w_default_int", stringInterface)); 200 | EXPECT_EQ(stringInterface, "1L"); 201 | } 202 | { 203 | std::string stringInterface; 204 | ASSERT_TRUE(nh.getParam("long_param_w_default_int_str", stringInterface)); 205 | EXPECT_EQ(stringInterface, "-1L"); 206 | } 207 | { 208 | std::string stringInterface; 209 | ASSERT_TRUE(nh.getParam("long_param_w_default_long_string", stringInterface)); 210 | EXPECT_EQ(stringInterface, "9223372036854775807L"); 211 | } 212 | { 213 | std::string stringInterface; 214 | ASSERT_TRUE(nh.getParam("str_param_w_default", stringInterface)); 215 | EXPECT_EQ(stringInterface, testInterface.str_param_w_default); 216 | } 217 | { 218 | std::vector vectorIntInterface; 219 | ASSERT_TRUE(nh.getParam("vector_int_param_w_default", vectorIntInterface)); 220 | EXPECT_EQ(vectorIntInterface, testInterface.vector_int_param_w_default); 221 | } 222 | { 223 | std::vector vectorDoubleInterface; 224 | ASSERT_TRUE(nh.getParam("vector_double_param_w_default", vectorDoubleInterface)); 225 | EXPECT_EQ(vectorDoubleInterface, testInterface.vector_double_param_w_default); 226 | } 227 | { 228 | std::vector vectorBoolInterface; 229 | ASSERT_TRUE(nh.getParam("vector_bool_param_w_default", vectorBoolInterface)); 230 | EXPECT_EQ(vectorBoolInterface, testInterface.vector_bool_param_w_default); 231 | } 232 | { 233 | std::vector vectorStringInterface; 234 | ASSERT_TRUE(nh.getParam("vector_string_param_w_default", vectorStringInterface)); 235 | EXPECT_EQ(vectorStringInterface, testInterface.vector_string_param_w_default); 236 | } 237 | { 238 | std::map mapParamWDefault; 239 | ASSERT_TRUE(nh.getParam("map_param_w_default", mapParamWDefault)); 240 | EXPECT_EQ(mapParamWDefault, testInterface.map_param_w_default); 241 | } 242 | { 243 | int enumIntInterface = 0; 244 | ASSERT_TRUE(nh.getParam("enum_int_param_w_default", enumIntInterface)); 245 | EXPECT_EQ(enumIntInterface, testInterface.enum_int_param_w_default); 246 | } 247 | { 248 | std::string enumStrInterface; 249 | ASSERT_TRUE(nh.getParam("enum_str_param_w_default", enumStrInterface)); 250 | EXPECT_EQ(enumStrInterface, testInterface.enum_str_param_w_default); 251 | } 252 | } 253 | 254 | TEST(RosinterfaceHandler, FromDynamicReconfigure) { // NOLINT(readability-function-size) 255 | ros::NodeHandle nh("~"); 256 | IfType testInterface(nh); 257 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 258 | testInterface.updater.force_update(); 259 | 260 | ConfigType config; 261 | config.int_param_w_default = 2; 262 | config.subscriber_w_default_topic = "/in_topic"; 263 | config.subscriber_diag_w_default_topic = "/in_point_topic"; 264 | config.subscriber_public_w_default_topic = "/in_topic"; 265 | config.subscriber_global_w_default_topic = "/in_topic"; 266 | config.publisher_w_default_topic = "/out_topic"; 267 | config.publisher_diag_w_default_topic = "/out_point_topic"; 268 | config.publisher_public_w_default_topic = "/out_topic"; 269 | config.publisher_global_w_default_topic = "/out_topic"; 270 | testInterface.fromConfig(config); 271 | 272 | testInterface.updater.force_update(); 273 | 274 | // params 275 | EXPECT_EQ(testInterface.int_param_w_default, 2); 276 | 277 | // subscriber 278 | EXPECT_EQ(testInterface.subscriber_w_default->getTopic(), "/in_topic"); 279 | EXPECT_EQ(testInterface.subscriber_diag_w_default->getTopic(), "/in_point_topic"); 280 | EXPECT_EQ(testInterface.subscriber_public_w_default->getTopic(), "/in_topic"); 281 | EXPECT_EQ(testInterface.subscriber_global_w_default->getTopic(), "/in_topic"); 282 | 283 | // publisher 284 | EXPECT_EQ(testInterface.publisher_w_default.getTopic(), "/out_topic"); 285 | EXPECT_EQ(testInterface.publisher_diag_w_default.getTopic(), "/out_point_topic"); 286 | EXPECT_EQ(testInterface.publisher_public_w_default.getTopic(), "/out_topic"); 287 | EXPECT_EQ(testInterface.publisher_global_w_default.getTopic(), "/out_topic"); 288 | } 289 | -------------------------------------------------------------------------------- /test/src/test_defaultsAtLaunch.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using IfType = rosinterface_handler::DefaultsAtLaunchInterface; 5 | using ConfigType = rosinterface_handler::DefaultsAtLaunchConfig; 6 | 7 | TEST(RosinterfaceHandler, DefaultsAtLaunch) { // NOLINT(readability-function-size) 8 | IfType testInterface(ros::NodeHandle("~")); 9 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 10 | 11 | ASSERT_EQ("info", testInterface.verbosity_param_wo_default); 12 | 13 | ASSERT_EQ(1, testInterface.int_param_wo_default); 14 | ASSERT_DOUBLE_EQ(1.1, testInterface.double_param_wo_default); 15 | ASSERT_EQ("Hello World", testInterface.str_param_wo_default); 16 | ASSERT_EQ(true, testInterface.bool_param_wo_default); 17 | ASSERT_EQ(1L, testInterface.long_param_wo_default_int); 18 | ASSERT_EQ(1L, testInterface.long_param_wo_default_int_str); 19 | ASSERT_EQ(9223372036854775807L, testInterface.long_param_wo_default_long_str); 20 | 21 | ASSERT_EQ(std::vector({1, 2, 3}), testInterface.vector_int_param_wo_default); 22 | ASSERT_EQ(std::vector({1.1, 1.2, 1.3}), testInterface.vector_double_param_wo_default); 23 | ASSERT_EQ(std::vector({false, true}), testInterface.vector_bool_param_wo_default); 24 | ASSERT_EQ(std::vector({"Hello", "World"}), testInterface.vector_string_param_wo_default); 25 | 26 | std::map tmp{{"Hello", "World"}}; 27 | ASSERT_EQ(tmp, testInterface.map_param_wo_default); 28 | 29 | ASSERT_EQ(1, testInterface.enum_int_param_wo_default); 30 | ASSERT_EQ("Two", testInterface.enum_str_param_wo_default); 31 | } 32 | 33 | TEST(RosinterfaceHandler, AtLaunchSubscriber) { 34 | IfType testInterface(ros::NodeHandle("~")); 35 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 36 | 37 | ASSERT_TRUE(!!testInterface.subscriber_wo_default); 38 | ASSERT_EQ(testInterface.subscriber_wo_default->getTopic(), "/test/rosinterface_handler_test/in_topic"); 39 | 40 | ASSERT_TRUE(!!testInterface.subscriber_public_wo_default); 41 | ASSERT_EQ(testInterface.subscriber_public_wo_default->getTopic(), "/test/in_topic"); 42 | 43 | ASSERT_TRUE(!!testInterface.subscriber_global_wo_default); 44 | ASSERT_EQ(testInterface.subscriber_global_wo_default->getTopic(), "/in_topic"); 45 | } 46 | 47 | TEST(RosinterfaceHandler, AtLaunchPublisher) { 48 | IfType testInterface(ros::NodeHandle("~")); 49 | ASSERT_NO_THROW(testInterface.fromParamServer()); // NOLINT(cppcoreguidelines-avoid-goto) 50 | 51 | ASSERT_EQ(testInterface.publisher_wo_default.getTopic(), "/test/rosinterface_handler_test/out_topic"); 52 | ASSERT_EQ(testInterface.publisher_public_wo_default.getTopic(), "/test/out_topic"); 53 | ASSERT_EQ(testInterface.publisher_global_wo_default.getTopic(), "/out_topic"); 54 | } 55 | -------------------------------------------------------------------------------- /test/src/test_defaultsMissing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using IfType = rosinterface_handler::DefaultsMissingInterface; 5 | using ConfigType = rosinterface_handler::DefaultsMissingConfig; 6 | 7 | TEST(RosinterfaceHandler, DefaultsMissing) { 8 | IfType testInterface(ros::NodeHandle("~")); 9 | // NOLINTNEXTLINE(cppcoreguidelines-avoid-goto) 10 | ASSERT_THROW(testInterface.fromParamServer(), std::runtime_error); 11 | } 12 | -------------------------------------------------------------------------------- /test/src/test_diagnosed_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using MsgT = geometry_msgs::PointStamped; 7 | 8 | class DummyUpdater : public diagnostic_updater::Updater { 9 | public: 10 | void forceUpdate() { 11 | statusVec.clear(); 12 | for (const auto& task : getTasks()) { 13 | diagnostic_updater::DiagnosticStatusWrapper status; 14 | 15 | status.name = task.getName(); 16 | status.level = 2; 17 | status.message = "No message was set"; 18 | status.hardware_id = "none"; 19 | 20 | task.run(status); 21 | statusVec.push_back(status); 22 | } 23 | } 24 | std::vector statusVec; 25 | }; 26 | 27 | using DiagPub = rosinterface_handler::DiagnosedPublisher; 28 | using DiagSub = rosinterface_handler::DiagnosedSubscriber; 29 | class TestDiagnosedPubSub : public testing::Test { 30 | protected: 31 | void SetUp() override { 32 | updater.statusVec.clear(); 33 | pub = nh.advertise("test_topic", 5); 34 | sub.subscribe(nh, "test_topic", 5); 35 | pub.minFrequency(10).maxTimeDelay(1); 36 | sub.minFrequency(10).maxTimeDelay(1); 37 | messageCounter = 0; 38 | } 39 | 40 | public: 41 | ros::NodeHandle nh; 42 | DummyUpdater updater; 43 | DiagPub pub{updater}; 44 | DiagSub sub{updater}; 45 | int messageCounter{0}; 46 | }; 47 | 48 | TEST_F(TestDiagnosedPubSub, publishAndReceiveOK) { 49 | auto onTimer = [this](const ros::TimerEvent& e) { 50 | this->messageCounter++; 51 | auto msg = boost::make_shared(); 52 | msg->header.stamp = e.current_real; 53 | this->pub.publish(msg); 54 | }; 55 | auto timer = nh.createTimer(ros::Duration(0.05), onTimer); 56 | while (messageCounter < 10) { 57 | ros::spinOnce(); 58 | } 59 | updater.forceUpdate(); 60 | ASSERT_GE(4, updater.statusVec.size()); 61 | for (auto& status : updater.statusVec) { 62 | EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level); 63 | } 64 | } 65 | 66 | TEST_F(TestDiagnosedPubSub, publishAndReceiveFail) { 67 | auto onTimer = [this](const ros::TimerEvent& e) { 68 | this->messageCounter++; 69 | auto msg = boost::make_shared(); 70 | msg->header.stamp = e.current_real - ros::Duration(1.5); 71 | this->pub.publish(msg); 72 | }; 73 | auto timer = nh.createTimer(ros::Duration(0.15), onTimer); 74 | while (messageCounter < 10) { 75 | ros::spinOnce(); 76 | } 77 | updater.forceUpdate(); 78 | ASSERT_LE(2, updater.statusVec.size()); 79 | for (auto& status : updater.statusVec) { 80 | EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::ERROR, status.level); 81 | } 82 | } 83 | 84 | TEST_F(TestDiagnosedPubSub, overrideTopic) { 85 | this->sub.subscribe(nh, "new_topic", 5); 86 | this->pub = nh.advertise("new_topic", 5); 87 | auto onTimer = [this](const ros::TimerEvent& e) { 88 | this->messageCounter++; 89 | auto msg = boost::make_shared(); 90 | msg->header.stamp = e.current_real; 91 | this->pub.publish(msg); 92 | }; 93 | auto timer = nh.createTimer(ros::Duration(0.05), onTimer); 94 | while (messageCounter < 10) { 95 | ros::spinOnce(); 96 | } 97 | updater.forceUpdate(); 98 | ASSERT_LE(2, updater.statusVec.size()); 99 | for (auto& status : updater.statusVec) { 100 | EXPECT_EQ(diagnostic_msgs::DiagnosticStatus::OK, status.level); 101 | } 102 | } 103 | 104 | TEST_F(TestDiagnosedPubSub, assign) { 105 | this->pub = DiagPub(updater); 106 | auto onTimer = [this](const ros::TimerEvent& e) { 107 | this->messageCounter++; 108 | auto msg = boost::make_shared(); 109 | msg->header.stamp = e.current_real; 110 | this->pub.publish(msg); 111 | }; 112 | auto timer = nh.createTimer(ros::Duration(0.05), onTimer); 113 | while (messageCounter < 10) { 114 | ros::spinOnce(); 115 | } 116 | updater.forceUpdate(); 117 | ASSERT_LE(1, updater.statusVec.size()); 118 | } 119 | 120 | TEST(TestDiagnosedAndSmartCombined, constructAndSubscribe) { 121 | ros::NodeHandle nh; 122 | ros::Publisher thisPublisher = nh.advertise("sometopic", 5); 123 | using Subscriber = rosinterface_handler::DiagnosedSubscriber>; 124 | DummyUpdater updater; 125 | Subscriber s(updater, thisPublisher); 126 | EXPECT_FALSE(s.isSubscribed()); 127 | s.subscribe(nh, "atopic", 5); 128 | EXPECT_FALSE(s.isSubscribed()); 129 | ros::Subscriber otherSubscriber = nh.subscribe( 130 | "sometopic", 5, +[](const MsgT::ConstPtr& msg) {}); 131 | ros::spinOnce(); 132 | ASSERT_GT(thisPublisher.getNumSubscribers(), 0); 133 | s.subscribeCallback(); 134 | EXPECT_TRUE(s.isSubscribed()); 135 | } 136 | -------------------------------------------------------------------------------- /test/src/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv) { 5 | testing::InitGoogleTest(&argc, argv); 6 | ros::init(argc, argv, "rosinterface_handler_test"); 7 | testing::FLAGS_gtest_catch_exceptions = false; 8 | // The async spinner lets you publish and receive messages during the tests, 9 | // no need to call spinOnce() 10 | ros::AsyncSpinner spinner(2); 11 | spinner.start(); 12 | int ret = RUN_ALL_TESTS(); 13 | ros::shutdown(); 14 | return ret; 15 | } 16 | -------------------------------------------------------------------------------- /test/src/test_minMax.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using IfType = rosinterface_handler::MinMaxInterface; 5 | using ConfigType = rosinterface_handler::MinMaxConfig; 6 | 7 | TEST(RosinterfaceHandler, MinMax) { 8 | IfType testInterface(ros::NodeHandle("~")); 9 | // NOLINTNEXTLINE(cppcoreguidelines-avoid-goto) 10 | ASSERT_NO_THROW(testInterface.fromParamServer()); 11 | 12 | ASSERT_EQ(2, testInterface.int_param_w_minmax); 13 | ASSERT_DOUBLE_EQ(2., testInterface.double_param_w_minmax); 14 | 15 | ASSERT_EQ(std::vector({0, 2, 2}), testInterface.vector_int_param_w_minmax); 16 | ASSERT_EQ(std::vector({0., 1.2, 2.}), testInterface.vector_double_param_w_minmax); 17 | 18 | std::map tmp{{"value1", 0.}, {"value2", 1.2}, {"value3", 2.}}; 19 | ASSERT_EQ(tmp, testInterface.map_param_w_minmax); 20 | } 21 | -------------------------------------------------------------------------------- /test/src/test_smart_subscriber.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "rosinterface_handler/smart_subscriber.hpp" 7 | 8 | using Msg = std_msgs::Int32; 9 | class TestNode { 10 | public: 11 | TestNode() { 12 | ros::NodeHandle nh; 13 | pub = nh.advertise("/output", 5); 14 | sub = nh.subscribe("/input", 5, &TestNode::msgCallback, this); 15 | smartSub.subscribe(nh, "/input", 5); 16 | smartSub.addPublisher(pub); 17 | smartSub.registerCallback([this](auto&& msg) { smartMsgCallback(msg); }); 18 | } 19 | int msgCount{0}; 20 | int smartMsgCount{0}; 21 | ros::Publisher pub; 22 | ros::Subscriber sub; 23 | rosinterface_handler::SmartSubscriber smartSub; 24 | 25 | private: 26 | void msgCallback(const Msg::ConstPtr& msg) { 27 | msgCount++; 28 | pub.publish(msg); 29 | } 30 | void smartMsgCallback(const Msg::ConstPtr& /*msg*/) { 31 | smartMsgCount++; 32 | } 33 | }; 34 | void intCb(const std_msgs::Int32& /*msg*/) { 35 | } 36 | 37 | class ImageNode { 38 | using SmartSub = rosinterface_handler::SmartSubscriber; 39 | 40 | public: 41 | ImageNode() : transport(ros::NodeHandle()) { 42 | image_transport::SubscriberStatusCallback cb = [this](const auto& /*s*/) { smartSub.subscribeCallback(); }; 43 | pub = transport.advertise("/img_out", 5, cb, cb); 44 | ros::NodeHandle nh; 45 | smartSub.subscribe(nh, "/img_in", 5); 46 | smartSub.addPublisher(pub); 47 | smartSub.registerCallback([this](auto&& msg) { imgCallback(msg); }); 48 | } 49 | int msgCount{0}; 50 | int smartMsgCount{0}; 51 | image_transport::ImageTransport transport; 52 | image_transport::Publisher pub; 53 | SmartSub smartSub; 54 | 55 | private: 56 | void imgCallback(const sensor_msgs::Image::ConstPtr& msg) { 57 | msgCount++; 58 | pub.publish(msg); 59 | } 60 | }; 61 | void cb(const sensor_msgs::Image& /*msg*/) { 62 | } 63 | 64 | template 65 | bool tryRepeatedly(Func&& f) { 66 | int i = 0; 67 | while (!f() && i++ < 20) { 68 | std::this_thread::sleep_for(std::chrono::milliseconds(20)); 69 | } 70 | return i <= 20; 71 | } 72 | 73 | TEST(SmartSubscriber, subscribeTests) { 74 | // subscribe to a topic twice (smart and non smart) 75 | // make sure the smart subscriber publishes no messages after rostopic echo stops listening 76 | TestNode node; 77 | ros::Subscriber listener = ros::NodeHandle().subscribe("/output", 5, intCb); 78 | // check we are subscribed 79 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 80 | node.msgCount = 0; 81 | node.smartMsgCount = 0; 82 | EXPECT_LT(0, node.pub.getNumSubscribers()); 83 | EXPECT_TRUE(node.smartSub.isSubscribed()); 84 | 85 | // test that we similar numbers of msgs than with a normal subscriber 86 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 87 | EXPECT_NEAR(node.msgCount, node.smartMsgCount, 5); 88 | EXPECT_TRUE(node.smartSub.isSubscribed()); 89 | EXPECT_GT(node.smartMsgCount, 0); 90 | listener.shutdown(); 91 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 92 | 93 | // by now surely no one will listen 94 | const int oldSmartMsgCount = node.smartMsgCount; 95 | 96 | std::this_thread::sleep_for(std::chrono::milliseconds(500)); 97 | EXPECT_FALSE(node.smartSub.isSubscribed()); 98 | EXPECT_NEAR(oldSmartMsgCount, node.smartMsgCount, 2); 99 | } 100 | 101 | TEST(SmartSubscriber, basicTests) { 102 | ros::NodeHandle nh; 103 | ros::Publisher myPub = nh.advertise("/output", 5); 104 | ros::Publisher myPub2 = nh.advertise("/output2", 5); 105 | rosinterface_handler::SmartSubscriber sub(myPub); 106 | sub.subscribe(nh, "/input", 5); 107 | EXPECT_TRUE(sub.removePublisher(myPub.getTopic())); 108 | EXPECT_FALSE(sub.removePublisher(myPub2.getTopic())); 109 | sub.addPublisher(myPub2); 110 | EXPECT_TRUE(sub.removePublisher(myPub2.getTopic())); 111 | } 112 | 113 | TEST(SmartSubscriber, nondefaultPublisher) { 114 | ImageNode node; 115 | EXPECT_FALSE(node.smartSub.isSubscribed()); 116 | ros::NodeHandle nh; 117 | // check we subscribe 118 | auto testIsSubscribed = [&]() { 119 | ros::spinOnce(); 120 | return node.smartSub.isSubscribed(); 121 | }; 122 | { 123 | auto sub = nh.subscribe("/img_out", 5, cb); 124 | // usually this works instantly but on high-load systems it may take longer 125 | EXPECT_TRUE(tryRepeatedly(testIsSubscribed)); 126 | } 127 | // check we are no longer subscribed 128 | auto testNotSubscribed = [&]() { return !testIsSubscribed(); }; 129 | EXPECT_TRUE(tryRepeatedly(testNotSubscribed)); 130 | 131 | // force subscription 132 | node.smartSub.setSmart(false); 133 | EXPECT_FALSE(node.smartSub.smart()); 134 | EXPECT_TRUE(node.smartSub.isSubscribed()); 135 | } 136 | --------------------------------------------------------------------------------