From eb46122431791ab41eb78c3287cd4483d4db7795 Mon Sep 17 00:00:00 2001 From: doronhi <40540281+doronhi@users.noreply.github.com> Date: Thu, 13 Dec 2018 14:47:48 +0200 Subject: [PATCH] use ddynamic_reconfigure and support D435i (#535) Add dynamic dynamic reconfigure. That means there are no longer differences in the code between D415, D430, SR300. Add dynamic options for filters Add support for camera D435i. Add clipping_disance option. enabled with parameter: clip_distance. units: meters. Default: no clipping. Add linear accel covariance - Default: 0.01 Add option: unite_imu - send linear acceleration and radial velocity in the same Imu message. Default: True Add parameter: hold_back_imu_for_frames. If set to true, hold imu messages that arrived while manipulating frames, until frames are actually sent. Comply with librealsense v2.17.0 Add opensource_tracking.launch - demo that runs realsense2_camera, imu_filter_madgwick, rtabmap and robot_localization to demonstrate Slam with realsense D435i Set accel_fps to 250 as this is the new maximal rate in librealsense v2.17.0 * Add NOTICE file, to emphasize the contribution of the ddynamic_reconfigure project. Known Issue: Option for toggling sensor on and off while running is missing. --- .gitignore | 1 + .travis.yml | 5 +- NOTICE | 4 + ddynamic_reconfigure/.gitignore | 2 + ddynamic_reconfigure/CHANGELOG.rst | 65 ++ ddynamic_reconfigure/CMakeLists.txt | 77 +++ ddynamic_reconfigure/README.md | 624 ++++++++++++++++++ .../include/ddynamic_reconfigure/dd_param.h | 122 ++++ .../include/ddynamic_reconfigure/dd_value.h | 161 +++++ .../ddynamic_reconfigure.h | 239 +++++++ .../param/dd_all_params.h | 12 + .../param/dd_bool_param.h | 70 ++ .../param/dd_double_param.h | 80 +++ .../param/dd_enum_param.h | 119 ++++ .../ddynamic_reconfigure/param/dd_int_param.h | 78 +++ .../param/dd_string_param.h | 70 ++ ddynamic_reconfigure/package.xml | 20 + ddynamic_reconfigure/src/dd_param.cpp | 12 + .../src/ddynamic_reconfigure.cpp | 214 ++++++ .../src/param/dd_bool_param.cpp | 58 ++ .../src/param/dd_double_param.cpp | 58 ++ .../src/param/dd_enum_param.cpp | 148 +++++ .../src/param/dd_int_param.cpp | 58 ++ .../src/param/dd_string_param.cpp | 58 ++ ddynamic_reconfigure/test/TutorialParams.srv | 6 + .../test/dd_full_scale/dd_client.cpp | 75 +++ .../test/dd_full_scale/dd_full_scale.test | 4 + .../test/dd_full_scale/dd_server.cpp | 62 ++ .../test/dd_param/dd_bool.test | 3 + .../test/dd_param/dd_double.test | 3 + .../test/dd_param/dd_enum.test | 3 + .../test/dd_param/dd_int.test | 3 + .../test/dd_param/dd_string.test | 3 + .../test/dd_param/test_dd_bool.cpp | 61 ++ .../test/dd_param/test_dd_double.cpp | 61 ++ .../test/dd_param/test_dd_enum.cpp | 106 +++ .../test/dd_param/test_dd_int.cpp | 61 ++ .../test/dd_param/test_dd_string.cpp | 61 ++ ddynamic_reconfigure/test/dd_value.test | 3 + .../test/ddynamic_reconfigure.test | 3 + ddynamic_reconfigure/test/test-all.sh | 18 + ddynamic_reconfigure/test/test_dd_value.cpp | 188 ++++++ .../test/test_ddynamic_reconfigure.cpp | 539 +++++++++++++++ ddynamic_reconfigure/uml/class_struct.puml | 104 +++ ddynamic_reconfigure/uml/file_struct.puml | 38 ++ ddynamic_reconfigure/uml/ros_struct.puml | 22 + realsense2_camera/CMakeLists.txt | 22 +- realsense2_camera/cfg/base_d400_params.cfg | 13 - realsense2_camera/cfg/base_d400_params.py | 23 - realsense2_camera/cfg/rs415_params.cfg | 43 -- realsense2_camera/cfg/rs435_params.cfg | 42 -- realsense2_camera/cfg/sr300_params.cfg | 70 -- .../include/base_realsense_node.h | 90 ++- realsense2_camera/include/constants.h | 7 +- .../include/realsense_node_factory.h | 2 +- realsense2_camera/include/rs415_node.h | 50 -- realsense2_camera/include/rs435_node.h | 48 -- realsense2_camera/include/sr300_node.h | 51 -- .../launch/includes/nodelet.launch.xml | 8 +- .../launch/opensource_tracking.launch | 51 ++ realsense2_camera/launch/rs_camera.launch | 8 +- realsense2_camera/package.xml | 6 +- realsense2_camera/src/base_realsense_node.cpp | 610 +++++++++++------ .../src/realsense_node_factory.cpp | 31 +- realsense2_camera/src/rs415_node.cpp | 142 ---- realsense2_camera/src/rs435_node.cpp | 130 ---- realsense2_camera/src/sr300_node.cpp | 153 ----- 67 files changed, 4332 insertions(+), 1050 deletions(-) create mode 100644 NOTICE create mode 100644 ddynamic_reconfigure/.gitignore create mode 100644 ddynamic_reconfigure/CHANGELOG.rst create mode 100644 ddynamic_reconfigure/CMakeLists.txt create mode 100644 ddynamic_reconfigure/README.md create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h create mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h create mode 100644 ddynamic_reconfigure/package.xml create mode 100644 ddynamic_reconfigure/src/dd_param.cpp create mode 100644 ddynamic_reconfigure/src/ddynamic_reconfigure.cpp create mode 100644 ddynamic_reconfigure/src/param/dd_bool_param.cpp create mode 100644 ddynamic_reconfigure/src/param/dd_double_param.cpp create mode 100644 ddynamic_reconfigure/src/param/dd_enum_param.cpp create mode 100644 ddynamic_reconfigure/src/param/dd_int_param.cpp create mode 100644 ddynamic_reconfigure/src/param/dd_string_param.cpp create mode 100644 ddynamic_reconfigure/test/TutorialParams.srv create mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp create mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test create mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp create mode 100644 ddynamic_reconfigure/test/dd_param/dd_bool.test create mode 100644 ddynamic_reconfigure/test/dd_param/dd_double.test create mode 100644 ddynamic_reconfigure/test/dd_param/dd_enum.test create mode 100644 ddynamic_reconfigure/test/dd_param/dd_int.test create mode 100644 ddynamic_reconfigure/test/dd_param/dd_string.test create mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp create mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_double.cpp create mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp create mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_int.cpp create mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_string.cpp create mode 100644 ddynamic_reconfigure/test/dd_value.test create mode 100644 ddynamic_reconfigure/test/ddynamic_reconfigure.test create mode 100644 ddynamic_reconfigure/test/test-all.sh create mode 100644 ddynamic_reconfigure/test/test_dd_value.cpp create mode 100644 ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp create mode 100644 ddynamic_reconfigure/uml/class_struct.puml create mode 100644 ddynamic_reconfigure/uml/file_struct.puml create mode 100644 ddynamic_reconfigure/uml/ros_struct.puml delete mode 100755 realsense2_camera/cfg/base_d400_params.cfg delete mode 100755 realsense2_camera/cfg/base_d400_params.py delete mode 100755 realsense2_camera/cfg/rs415_params.cfg delete mode 100755 realsense2_camera/cfg/rs435_params.cfg delete mode 100755 realsense2_camera/cfg/sr300_params.cfg delete mode 100644 realsense2_camera/include/rs415_node.h delete mode 100644 realsense2_camera/include/rs435_node.h delete mode 100644 realsense2_camera/include/sr300_node.h create mode 100644 realsense2_camera/launch/opensource_tracking.launch delete mode 100644 realsense2_camera/src/rs415_node.cpp delete mode 100644 realsense2_camera/src/rs435_node.cpp delete mode 100644 realsense2_camera/src/sr300_node.cpp diff --git a/.gitignore b/.gitignore index b53a61763e..55a9718f84 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ *.ini .idea/ .vs/ +*.json diff --git a/.travis.yml b/.travis.yml index 7e57a822cc..bf9b74b268 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,7 +25,10 @@ install: - sudo apt-get install ros-kinetic-diagnostic-updater -y - source ~/.bashrc - mkdir -p ~/catkin_ws/src/realsense - + + # download ddynamic_reconfigure: + - git clone -v --progress https://github.com/awesomebytes/ddynamic_reconfigure.git + # install realsense2-camera - mv * ~/catkin_ws/src/realsense/ # This leaves behind .git, .gitignore and .travis.yml but no matter. - cd ~/catkin_ws/src/ diff --git a/NOTICE b/NOTICE new file mode 100644 index 0000000000..f986287012 --- /dev/null +++ b/NOTICE @@ -0,0 +1,4 @@ +This project uses code from the following third-party projects, listed here +with the full text of their respective licenses. + +ddynamic_reconfigure (BSD) - https://github.com/awesomebytes/ddynamic_reconfigure diff --git a/ddynamic_reconfigure/.gitignore b/ddynamic_reconfigure/.gitignore new file mode 100644 index 0000000000..48c896e410 --- /dev/null +++ b/ddynamic_reconfigure/.gitignore @@ -0,0 +1,2 @@ +*~ +*user diff --git a/ddynamic_reconfigure/CHANGELOG.rst b/ddynamic_reconfigure/CHANGELOG.rst new file mode 100644 index 0000000000..25a7690f09 --- /dev/null +++ b/ddynamic_reconfigure/CHANGELOG.rst @@ -0,0 +1,65 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ddynamic_reconfigure +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.6 (2018-07-02) +------------------ +* Recreated classes to enable OOD (adding more param types will be easy) +* Added string and enum support +* generalised the callback + You can now look into the new values with the current callback format. +* Level support added. +* Added unit-tests for all param classes. +* Added unit-tests for value class. +* Upgraded fake-server test & removed bool-server test (obsolete). +* Added description support. +* Added stream (<<) operator to ddynamic and its params. +* Contributors: Noam Dori + +0.0.5 (2016-04-14) +------------------ +* Merge branch 'user-callback' into 'dubnium-devel' + User callback + Remember that we have to re release everyone who depends on this since it breaks API. + See merge request !1 +* Add test for double param +* Add hack to have namespaced DdynamicReconfigure, for easier migration +* Add user callback and unit tests +* Migrate package to format 2 +* Contributors: Hilario Tome, Victor Lopez + +0.0.4 (2016-03-07) +------------------ +* Added destructor, fixed bug +* Added to dynamic reconfigure to parse from param server the initial value if it is availlable +* Contributors: Hilario Tome + +0.0.3 (2015-06-10) +------------------ +* Added license and documentation +* Contributors: Hilario Tome + +0.0.2 (2015-05-25) +------------------ +* Added min and max value specification when registering a variable +* Contributors: Hilario Tome + +0.0.1 (2015-01-26) +------------------ +* fix author, mantainer +* move ddynamic reconfigure to standalone repo +* Prepare ddynamic_reconfigure for standalone package +* Added safe header +* Added test folder +* Fixed a bug when generating the config description, the int vector was being used in the bool part +* Added typedef for ddreconfigure +* Bug fix, now the parameters can be seen in dynamic reconfigure even if they have changed from c++ +* Updated DDynamic reconfigure to published updated values persistently +* Added working momentum task +* Fixed bug, wrong return statement +* Fixed export +* Fixed bug in ddynamic reconfigure and its CmakeFile +* Minor changes to add the abstract reference to the goto dynamic tasks +* Dynamics wbc is working again (Really slowly with uquadprog) visualization of torques and partially of forces (also partial force integration) +* Added DDyanmic_reconfigure package, a way to have dynamic reconfigure functionality without a cfg +* Contributors: Hilario Tome, Luca Marchionni diff --git a/ddynamic_reconfigure/CMakeLists.txt b/ddynamic_reconfigure/CMakeLists.txt new file mode 100644 index 0000000000..539329f47d --- /dev/null +++ b/ddynamic_reconfigure/CMakeLists.txt @@ -0,0 +1,77 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ddynamic_reconfigure) + +find_package(catkin REQUIRED COMPONENTS roscpp message_generation) + +set(CMAKE_CXX_STANDARD 98) + +############## +## Services ## +############## + +add_service_files(DIRECTORY test FILES TutorialParams.srv) +generate_messages(DEPENDENCIES std_msgs) + +############ +## Catkin ## +############ + +catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS) + +############# +## Library ## +############# + +include_directories(include ${catkin_INCLUDE_DIRS}) +add_library(${PROJECT_NAME} + src/ddynamic_reconfigure.cpp + include/ddynamic_reconfigure/dd_param.h + src/param/dd_int_param.cpp + include/ddynamic_reconfigure/dd_value.h + src/param/dd_double_param.cpp + src/param/dd_bool_param.cpp + src/param/dd_string_param.cpp + src/param/dd_enum_param.cpp + include/ddynamic_reconfigure/param/dd_all_params.h src/dd_param.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +############# +## Testing ## +############# + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + ## DDynamic tester + add_rostest_gtest(ddynamic_reconfigure-test test/ddynamic_reconfigure.test test/test_ddynamic_reconfigure.cpp) + target_link_libraries(ddynamic_reconfigure-test ${PROJECT_NAME}) + + ## DDParam tester + foreach (param_type int double bool string enum) + add_rostest_gtest(dd_${param_type}-test test/dd_param/dd_${param_type}.test test/dd_param/test_dd_${param_type}.cpp) + target_link_libraries(dd_${param_type}-test ${PROJECT_NAME}) + endforeach () + + ## DDValue tester + add_rostest_gtest(dd_value-test test/dd_value.test test/test_dd_value.cpp) + target_link_libraries(dd_value-test ${PROJECT_NAME}) + + ## Full scale tester + add_executable(dd_server test/dd_full_scale/dd_server.cpp) + target_link_libraries(dd_server ${PROJECT_NAME} ${catkin_LIBRARIES}) + add_dependencies(dd_server ${PROJECT_NAME}_gencpp) + + add_rostest_gtest(dd_full_scale-test test/dd_full_scale/dd_full_scale.test test/dd_full_scale/dd_client.cpp) + target_link_libraries(dd_full_scale-test ${PROJECT_NAME}) +endif(CATKIN_ENABLE_TESTING) \ No newline at end of file diff --git a/ddynamic_reconfigure/README.md b/ddynamic_reconfigure/README.md new file mode 100644 index 0000000000..a4af37fc25 --- /dev/null +++ b/ddynamic_reconfigure/README.md @@ -0,0 +1,624 @@ +DDynamic-Reconfigure +================================================== +[![Build Status](http://venus:8080/view/Integration%20Jobs/job/I40-ddynamic_reconfigure-dubnium-devel_dubnium/badge/icon)](http://venus:8080/view/Integration%20Jobs/job/I40-ddynamic_reconfigure-dubnium-devel_dubnium/) + +The DDynamic-Reconfigure package (or 2D-reconfig) is a **C++** based extension to Dynamic-Reconfigure (or 1D-reconfig) which allows C++ based nodes to self-initiate. + +## Dependencies +2D-reconfig depends only on the default packages, not even 1D-reconfig. + +## Configuration +Other than the installation of the package to your workspace, no other configuration is needed. +The package used is called ``ddynamic_reconfigure``, +and this both the namespace and the include directory used to implement the program. + +## Implementation +let us look into the following code, which implements 2D-Reconfig: +````cpp +#include + +#include +#include + +using namespace ddynamic_reconfigure; + +void callback(const DDMap& map, int) { + ROS_INFO("Reconfigure Request: %d %f %s %s %ld", + get(map, "int_param").toInt(), get(map, "double_param").toDouble(), + get(map, "str_param").toString().c_str(), + get(map, "bool_param").toBool() ? "True" : "False", + map.size()); +} + +int main(int argc, char **argv) { + // ROS init stage + ros::init(argc, argv, "ddynamic_tutorials"); + ros::NodeHandle nh; + + // DDynamic setup stage + DDynamicReconfigure dd(nh); + dd.add(new DDInt("int_param", 0, "An Integer parameter", 50, 0, 100)); + dd.add(new DDDouble("double_param", 0, "A double parameter", .5, 0, 1)); + dd.add(new DDString("str_param", 0, "A string parameter", "Hello World")); + dd.add(new DDBool("bool_param", 0, "A Boolean parameter", true)); + std::map dict; // An enum to set size + dict["Small"] = 0; // A small constant + dict["Medium"] = 1; // A medium constant + dict["Large"] = 2; // A large constant + dict["ExtraLarge"] = 3; // An extra large constant + dd.add(new DDEnum("enum_param", 0, "A size parameter which is edited via an enum", 1, dict)); + dd.start(callback); + + // Actual Server Node code + ROS_INFO("Spinning node"); + ros::spin(); + return 0; +} +```` +This segment of code is used for declaring the configuration file and for setting up the server in place of the node which uses the parameters. + +### Breakdown + +Let's break down the code line by line: +```cpp +#include + +#include +#include + +using namespace ddynamic_reconfigure; +``` +In here, we import all needed files: +* ```` provides basic ROS management. +* ```` provides the 2D-reconfigure API +* ```` allows you to use all default parameter types. + +The non include line allows us to use classes and functions provided in the ``ddynamic_reconfigure`` namespace +without mentioning what package they are from. + +In contrast to 1D-reconfigure, these do not change. + +```cpp +void callback(const DDMap& map, int) { + ROS_INFO("Reconfigure Request: %d %f %s %s %ld", + get(map, "int_param").toInt(), get(map, "double_param").toDouble(), + get(map, "str_param").toString().c_str(), + get(map, "bool_param").toBool() ? "True" : "False", + map.size()); +} +``` + +This is the callback used when 2D-reconfig receives a parameter change request. +It takes two parameters: the first is a map of the new configuration mapping from the name of the parameter to the actual parameter object, +and the second is the level, which is the highest level of severity caused by the parameter change. +This is calculated by applying the OR operator on all levels of the parameters that changed. + +In this callback the level is not used, but we do print out the new configuration. + +```cpp +int main(int argc, char **argv) { + // ROS init stage + ros::init(argc, argv, "ddynamic_tutorials"); + ros::NodeHandle nh; +``` + +All this section do is initialise our ROS node and its handler. +This is default stuff you do anyways. + +```cpp + // DDynamic setup stage + DDynamicReconfigure dd(nh); + dd.add(new DDInt("int_param", 0, "An Integer parameter", 50, 0, 100)); + dd.add(new DDDouble("double_param", 0, "A double parameter", .5, 0, 1)); + dd.add(new DDString("str_param", 0, "A string parameter", "Hello World")); + dd.add(new DDBool("bool_param", 0, "A Boolean parameter", true)); +``` + +This is we start using 2D-reconfig. First, we initialise our 2D-reconfig object. +Then, we start adding parameters to it. In 2D-reconfig, adding parameters is not just a simple function, +but you have to add a parameter object (an instance of the abstract ``DDParam`` class). +Let's look into the param objects above to see some common factors: +* The type of the parameter is declared first by specifying ``new DDType()``. + For example, adding a new int parameter is done by doing ``dd.add(new DDInt(...))`` + +* Within the param constructor, the first argument is the name of the parameter. + For example, in our int parameter, the name is set to ``"int_param"``. + +* The second argument is the level of the parameter, that is, + what needs to be reset or redone in the software/hardware in order to reapply this parameter? + Usually, the higher the level, the more drastic measures you need to take to re-implement the parameter. + +* The third parameter is the description of the parameter. This is great for documentation and for commandline tools. + +* The fourth parameter is the default value. Depending on the type of parameter, each may treat this argument differently. + +* ``DDInt`` and ``DDDouble`` have a fifth and sixth optional parameters: minimum and maximum allowed values. + While the server side does not care about these values, the client may want to know these. + +* It is important to note that the first 4 arguments are standardised for all param types, + but from there onwards each param type may choose what to place there. + +```cpp + std::map dict; // An enum to set size + dict["Small"] = 0; // A small constant + dict["Medium"] = 1; // A medium constant + dict["Large"] = 2; // A large constant + dict["ExtraLarge"] = 3; // An extra large constant + dd.add(new DDEnum("enum_param", 0, "A size parameter which is edited via an enum", 1, dict)); +``` + +Here we add an int-enum parameter to our 2D-reconfig. ``DDEnum`` is an int like parameter that also contains a dictionary +to remap predefined strings to usable integers. This param type has a required 5th argument (in contract to ``DDInt`` having 5th and 6th optional) +which is a ``std::map`` object mapping string values to integers. + +In the code above we can see how to create a dictionary of our liking: + +* we first initiate a map and name it with ``std::map dict``. +* we then populate it with the format ``dict[] = `` where ```` is the string alias for the value, + and ```` is the value you want to give an alias to. + +This dictionary is then added into the enum as the 5th argument. + +```cpp + dd.start(callback); + + // Actual Server Node code + ROS_INFO("Spinning node"); + ros::spin(); + return 0; +} +``` + +This section of code actually allows 2D-reconfigure to start working. Let's look into the two sections: +* ``dd.start(callback)`` sets the callback of 2D-reconfigure to be the method ``callback`` and jump starts 2D-reconfigure. +* ``ros::spin()`` allows 2D-reconfigure to listen to parameter-change requests. + Although the node now requires a spin, this does not mean you cannot add your own service-servers and subscribers to this node. + ``ros::spin()`` can take care of multiple subscribers/service-servers in the same spinners (although in the same thread). + If you want 2D-reconfig and your actual node to work on separate threads, consider using ``ros::MultiThreadedSpinner``. + 2D-reconfigure only uses 1 service-server and no subscribers, so 1 thread for it is more than enough. + +### How does this compare with Dynamic-Reconfigure? +Let's go over the main differences between 2D-reconfig's implementation with 1D-reconfig's implementation: + +#### Parameter Generation + +**1D-Reconfigure:** +```python +gen = ParameterGenerator() + +gen.add("int_param", int_t, 0, "An Integer parameter", 50, 0, 100) +gen.add("double_param", double_t, 0, "A double parameter", .5, 0, 1) +gen.add("str_param", str_t, 0, "A string parameter", "Hello World") +gen.add("bool_param", bool_t, 0, "A Boolean parameter", True) + +size_enum = gen.enum([ gen.const("Small", int_t, 0, "A small constant"), + gen.const("Medium", int_t, 1, "A medium constant"), + gen.const("Large", int_t, 2, "A large constant"), + gen.const("ExtraLarge", int_t, 3, "An extra large constant")], + "An enum to set size") + +gen.add("size", int_t, 0, "A size parameter which is edited via an enum", 1, 0, 3, edit_method=size_enum) + +exit(gen.generate(PACKAGE, "dynamic_tutorials", "Tutorials")) +``` +**2D-Reconfigure:** +```cpp +DDynamicReconfigure dd(nh); + +dd.add(new DDInt("int_param", 0, "An Integer parameter", 50, 0, 100)); +dd.add(new DDDouble("double_param", 0, "A double parameter", .5, 0, 1)); +dd.add(new DDString("str_param", 0, "A string parameter", "Hello World")); +dd.add(new DDBool("bool_param", 0, "A Boolean parameter", true)); + +std::map dict; // An enum to set size + dict["Small"] = 0; // A small constant + dict["Medium"] = 1; // A medium constant + dict["Large"] = 2; // A large constant + dict["ExtraLarge"] = 3; // An extra large constant + +dd.add(new DDEnum("enum_param", 0, "A size parameter which is edited via an enum", 1, dict)); + +dd.start(callback); +``` +While these two code snippets accomplish the exact same things, they do so in different manners: +* 1D-reconfig specifies the type of the parameter using a string (for example ``int_t = "int"``), while 2D-reconfig uses classes to accomplish that (``new DDInt`` in place of ``int_t``). + + Why classes instead of strings? In contrast to strings, classes can be extended and modified so they get a special treatment. + Take enums for example. In order to work with enums, 1D-reconfig had to add a whole new parameter input to handle the dictionary of the enums, + while 2D-reconfig simply extended the ``DDInt`` class (to ``DDEnum``) to handle dictionaries. + + This will be discussed more thoroughly on "Architecture". + +* Enums are dramatically different. + * 2D-reconfig uses well defined standard C++ objects for its dictionaries, + while 1D-reconfig defines its own constants and enums. This allows you to use well known and reliable API instead of a loosely defined one. + + * while ``DDEnum`` is an extension of ``DDInt``, you do not need to mention that. The API takes care of that for you! + An added bonus of this is that the enums automatically inference their boundaries, you don't need to mention ``int_t, max, min``. + + * 2D-reconfig's supported physical enums have been stripped of descriptions and the constants were as well. + This is because the descriptions were not used anywhere. You can still make enums with const and enum descriptions, but they will not be used anywhere. + Adding line comments to the parameters is a good alternative. + +* 2D-reconfig requires a node handler. This is due to how 1D-reconfigure handles parameters in its ROS architecture for C++. + +* all parameters provided in 1D-reconfig's last line are not needed in 2D-reconfig, which requires nothing. + +#### Callback & Server + +**1D-Reconfigure:** +```cpp +void callback(dynamic_tutorials::TutorialsConfig &config, uint32_t level) { + ROS_INFO("Reconfigure Request: %d %f %s %s %d", + config.int_param, config.double_param, + config.str_param.c_str(), + config.bool_param?"True":"False", + config.size); +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "dynamic_tutorials"); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + + f = boost::bind(&callback, _1, _2); + server.setCallback(f); + + ROS_INFO("Spinning node"); + ros::spin(); + return 0; +} +``` +**2D-Reconfigure:** +```cpp +void callback(const DDMap& map, int) { + ROS_INFO("Reconfigure Request: %d %f %s %s %ld", + get(map, "int_param").toInt(), get(map, "double_param").toDouble(), + get(map, "str_param").toString().c_str(), + get(map, "bool_param").toBool() ? "True" : "False", + map.size()); +} + +int main(int argc, char **argv) { + ros::init(argc, argv, "ddynamic_tutorials"); + ros::NodeHandle nh; + + DDynamicReconfigure dd(nh); + + dd.start(callback); + + ROS_INFO("Spinning node"); + ros::spin(); + return 0; +} +``` + +* The callback of the 1D-reconfigure requires a custom data-type per configuration. This is problematic, especially if you want dynamic parameters like vectors. + 2D-reconfigure uses ``DDMap`` as its parameter container, which is generic and can work over multiple config types (and therefore can handle vectors) + + This also changes the way to access these parameters. + ``DDMap`` is actually a map from string to a pointer to the generic parameter used with the parameter generator. + This allows you to use all functions ``std::map`` provides, and regardless, 2D-reconfigure has additional API that could be used on ``DDMap`` objects, + such as ``ddynamic_reconfigure::get`` and ``ddynamic_reconfigure::at``. + + The generic interface no longer gives you a specific primitive value, but rather an instance of ``ddynamic_reconfigure::Value``, + which must be converted into a primitive type. While a bit cumbersome, this does allow rather implicit conversion between types. + +* The 2D-reconfig server does not internally initialise a node handler. This means you can implement 2D-reconfigure within the node that actually uses the parameters. + +* 2D-reconfig actually needs you to start it. While a disadvantage, it is done anyways on 1D-reconfig, + and 2D-reconfig also has ``DDynamicReconfigure::setCallback`` to change callbacks, so nothing much is lost. + +* again, 2D-reconfig does not require you to have a custom made config class, making your init code a lot shorter. + +* ``start`` is a bit more fluid and allows you to place member function pointers or regular function pointers instead of ``boost::function``. + This helps clean up the code. + +### Simplified API + +#### Value + +The Value class is used to wrap all basic data-types (bool,int,double,string) in something generic. +The value object always stores an explicit basic data-type. +This has three main uses: + +1. Values can represent all basic data-types. This means that arguments that need something relatively similar from all basic data-types can now just use the value in its argument. + This also goes for when you need to return something that is of different data-types from different classes (one can only return integer, other can only return strings). + +2. Values can be explicitly converted to all basic data-types they wrap. + This means that converting an int to a string is far easier. + +3. Values store the type they were instantiated with. This can be tested against to get the original piece of data the value stored. + +##### Constructors + +``Value(int val)``,``Value(double val)``,``Value(bool val)``,``Value(string val)``,``Value(const char* val)`` +are all constructors that assign the value type to the type they are given (with the exception for ``const char*`` which returns string and is there for convenience), +then store the value itself in its basic form. + +##### Getter + +There is only one true getter: ``getType()``, which returns the string name of the type it stores. + +##### Converters + +Each basic data-type has its own converter: ``toInt()``,``toDouble()``,``toBool()``,``toString()``. +When one is called, the value will attempt to return a converted form of what it stores into the required data-type. +The value does not just use an implicit cast. It tries to convert the data-type according to common needs that are not answered with other one-liners. +For example, converting a string to an int, a Value will first attempt to scan the string and see it fits a numeric format. +If it succeeds, it will convert and return that number. Otherwise, it will return the next best thing: a hash value of the string. + +#### DDParam + +The DDParam class is *the* abstraction of all parameter types, and is the template for creating them. +At this point, not much is known about the parameter, but the following: + +* the parameter has a name +* the parameter has a severity level +* the parameter has a description +* the parameter contains some value, though its type and contents are unknown. + +Other than storing data, the parameter also has specialised methods to interact with DDynamicReconfigure in order to apply changes and send them. +These methods should not be touched by the user. + +Since this class is abstract, the class has multiple implementations whicch are not directly exposed but are used, +so its worth checking out their descriptions. + +While this class is abstract, it does have one implemented thing, and that is its stream operator (`<<`) which can be freely used. + +##### Generic Constructor + +While DDParam is abstract, all of its concrete implementations should follow this guideline: +```cpp +DD(const string &name, unsigned int level, const string &description, def, ) +``` +Where: +* ```` is the type name you are implementing +* ``name`` is the reference name +* ``level`` is the severity level +* ``description`` is the object's description +* ``def`` is the default value and the first value stored right after construction. + +You may then include extra arguments as you wish, required or optional. + +##### Getters + +parameters have three well known getters: +* ``getName()`` gets the name of the parameter. +* ``getLevel()`` gets the severity level of the parameter. +* ``getValue()`` gets the value the parameter stores. + +Other getters, such as "getDesc()", may be added in the future. + +the parameters also have a stream (``<<``) operator which can be used to convert said parameters into neat strings. + +##### Setter + +2D-params are only required to be dynamic with their values, +so the only setter they are required to have is ``setValue(Value val)``, +which changes the value the parameter stores. + +##### Testers + +DDParams are also required to have some out-of-the-box testing features: +* ``sameType(Value val)`` checks whether or not + the raw value stored in the value is compatible with the given parameter. + Compatible is a very broad word in this scenario. + It means that the value can be placed in the parameter regardless of other limitations. + +* ``sameValue(Value val)`` checks whether or not the value stored in the value object, + when converted to the type of the internal value, are equal. This acts regardless of type. + +#### DDynamicReconfigure + +The DDynamicReconfigure class is the main class responsible for keeping track of parameters basic properties, +values, descriptions, etc. + +It is also responsible of handling callbacks, config change requests, description setup and config setup, and the ROS publishers and services. + +To operate a DDynamic instance, you must go through the following procedure: + +1. Construct a DDynamicReconfigure instance with proper handling. +2. Add parameters to the instance as needed with any of the ``add`` methods. +3. Start the ROS services with any of the ``start`` methods. +4. If you need to change the callback after startup you may do so using ``setCallback``. +5. When you need to get any of the stored parameters, call either ``get`` or ``at`` on this instance, + rather than through the callback. + +##### Constructor + +DDynamicReconfigure has one sole constructor: ``DDynamicReconfigure(NodeHandle &nh)`` which constructs the instance and +sets the handler to the one you are using. + +##### Parameter Handling + +All parameter handling is done through registration using an ``add`` function: + +* ``add(DDPtr param)`` is the main function which uses boost's shared pointers to represent the data in a virtual manner (and allows polymorphism) +* ``add(DDParam *param)`` is a convenience function which converts ``param`` into a shared pointer and uses the other add function. + +Both of these functions will add a generic ``DDParam`` object into the given instance and will index it for later searches. +Perhaps in the future a "remove(string name)" function will be added. + +##### Callback Handling & Startup + +Below are the two default functions that are used by the rest: + +* ``start()`` initializes all publishers and services and releases the needed messages for the commandline and other clients. +* ``setCallback(DDFunc callback)`` sets the triggered callback to the one specified, and triggers nothing else. + +There is also ``clearCallback()`` which resets the callback to do nothing when triggered. + +Following are convenience function which utilize ``start()`` and ``setCallback()``: + +* ``start(DDFunc callback)`` calls start(), then setCallback(callback) +* ``start(void(*callback)(const DDMap&, int))`` remaps the void pointer to a boost function (of type ``DDFunc``) then calls start(callback) +* ``template void start(void(T::*callback)(const DDMap&, int), T *obj)`` + binds the **member** function into a boost function (of type ``DDFunc``) then calls start(callback) + +##### Parameter Fetching + +There are multiple proper ways to get the values stored within the DDynamicReconfigure instance: + +* through ``at(string name)``: this will get you the pointer to the parameter with the name you specified. + If no such parameter exists it will return you a null-pointer (be careful not to de-reference those!) + +* through ``get(string name)``: this will get you the value stored in the parameter with the name you specified. + If no such parameter exists it will return you a value storing a NULL character. + +* through the stream (``<<``) operator: this will convert the 2D-reconfig instance into a string and stream it into the + given streamer. + +both ``at`` and ``get`` have alternate static versions which apply directly on ``DDMap`` objects. + +## Architecture + +### Code Design + +#### Include Structure: + +![](http://www.plantuml.com/plantuml/png/3OnDIuP054Rt_efQjCm9JB8WqcXJ44Nu4hIH-RZgu7p8dNiT_FSDFAk7SqwVI2AnTzMr3Tgn0KPtjHBjwKa8bBbUBAsiE07g60W2rJfw8JEaw46T14aOSmRfhPuG2ZFRXH54XjpTtneuHAcBsJgO4Y5hglTol53S83mFxpzt-FNuyA7KvLDVpAiST3isgg6vu-_VRakj-ZlMCGytpLjPrKCmHVy7) + +To operate 2D-reconfigure, you will need to include 2 file types: + +* The ``ddynamic_reconfigure`` file, which gives you access to the ``DDynamicReconfigure`` class, + the ``DDParam`` class, the ``DDValue`` class, and the toolbox methods. + This will allow you to operate on the top level API without caring about what type of parameters you will get. + +* the file ``dd_all_params`` or any of the ``DDParam`` implementations. You will need the implementations to insert physical + (and not abstract) parameters into your ``DDynamicReconfigure`` server. + As a shortcut, ``dd_all_params`` gives you all basic parameter types (int,double,bool,string,enum) in one include. + +As a bonus, you also get two static class-less methods: ``get`` and ``at``. + +#### Class Structure: + +![](http://www.plantuml.com/plantuml/png/3OnBIyD054Rt_HMwS6d6HmDH43EIZRfgmOBTX7dS96Fc4Uwzqw7_te5lzN7EwOaLSWv-T-kYyTb2Hd-pC6_qAWIgqioEbwmp0PeK6I8t9WMX2b0AeAyC9AozHXMS6H4gCxav8uW2fTlVMxY8MXV6AwAH6BFXPglFEwSLuflyF3wWXDFJYlmrdDpXyNl_yN9e_xhsz-UevKgjFbzWAlBkUQZRzH1jrVy1) + +Like the API section shows, there are only 3 major classes: ``DDValue``,``DDParam``,``DDynamicReconfigure``. + +The DDValue class is a concrete class which should not be inherited, since it wraps physical values. +Each instance stores 5 values: one for each type is can handle, and one to store the type. +When a value is instantiated, the value is stored in its raw form according to the chosen type, +and the rest stay with default values. When the value is accessed only then is the value converted (but not saved!) + +The DDParam interface class is an abstract class which should be implemented. +Its basic implementations (int,double,bool,string) have already been implemented in the standard package. +These basic forms can also be further extended. For example, DDEnum **extends** DDInt because it has all of the features DDInt has. +This can be done to other DDParam implementations, and you can also further extend the extended classes (for example, DDInvertibleEnum). +An example is given at the Extension section if you want to look more into this. +When anny DDParam implementation is extended, the user has access to everything within the object so that he can do what he needs to. + +The DDynamicReconfigure class is the concrete class that does the work against ROS and interfaces with the user. +Unlike DDValue, this class can be extended, and it has an internal API that can aid users who wish to extend this class. +In the Extension section below this is elaborated. Keep in mind that extending DDynamicReconfigure is not required. +While DDynamicReconfigure allows extension, it does not provide full access to everything, +since the base function of DDynamic should not be modified. + +### ROS Design + +![](http://www.plantuml.com/plantuml/png/3OnDIyKm44Nt_HMwS6aZg222s8BWgo8QGOHkGZwcRMoJb9b9m_lt1kxcmZcd8zR8EMpDfOzsomuoRXSByqwFGg0kxUnvoIOJe4sH8N9hKn2w0AK0vin0mhbprC5RXL2PoSyPGHGe3tVN3WvHwm8JAMBCbjkz_cTEAyIdVlY-mTFRwxiCgAIHu_JpgORVrG38hzF7ljAz6Oy_MVghsvUwfeFegluF) + +Like 1D-reconfigure, 2D-reconfigure is built on two subscribers and one service: + +* ``desc_pub_`` publishes to topic "/parameter_descriptions", and is responsible for updating the descriptions of the parameter for commandline. +* ``update_pub_`` publishes to "/parameter_descriptions", and is responsible for updating the configuration values for commandline and client. +* ``set_service`` publishes and listens to requests on "/set_parameters", and is used to trigger parameter updates. + It also contains the new parameters sent from client or commandline. + +Since the DDynamicReconfigure object is held on the server side, so are these ROS entities. + +## Extension + +***In all of these extensions, make sure to add the proper includes!*** + +### Adding a new Parameter type + +To add a new parameter type, you must either: +* Extend one of the existing classes +* Implement the base class, ``DDParam``. + +In some cases, you might want your class to extend multiple classes, for example ``DDIntVector`` both implements ``DDVector`` and extends ``DDInt``. +(``DDVector`` does not exist in the standard param library). + +Let us look into an example implementation of the param type "DDIntEnforcer", which will update other parameters to its value when it updates. + +```cpp +#ifndef DDYNAMIC_RECONFIGURE_DD_INT_ENFORCER_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_INT_ENFORCER_PARAM_H + +#include +#include + +namespace my_dd_reconfig { + // class definition + class DDIntEnforcer : public DDInt { + public: + + void setValue(Value val); + + // adds a parameter to be enforced by this param. + DDIntEnforcer &addEnforced(DDPtr param); + + // removes a parameter from being enforced by this param. + void removeEnforced(DDPtr param); + + /** + * creates a new int enforcer param + * @param name the name of the parameter + * @param level the change level + * @param def the default value + * @param description details about the parameter + * @param max the maximum allowed value. Defaults to INT32_MAX + * @param min the minimum allowed value. Defaults to INT32_MIN + */ + inline DDIntEnforcer(const string &name, unsigned int level, const string &description, + int def, int max = INT32_MAX, int min = INT32_MIN) : + DDInt(name,level,description,def) {}; + + protected: + list enforced_params_; + }; + + DDIntEnforcer::setValue(Value val) { + val_ = val.toInt(); + for(list::iterator it = enforced_params_.begin(); it != enforced_params_.end(); ++it) { + if(!enforced_params_[it].sameValue(val)) { + enforced_params_[it].setValue(val); + } + } + }; + + DDIntEnforcer &DDIntEnforcer::addEnforced(DDPtr param) { + enforced_params_.push_back(param); + return *this; + }; + + void DDIntEnforcer::removeEnforced(DDPtr param) { + enforced_params_.remove(param); + }; +} + +#endif //DDYNAMIC_RECONFIGURE_DD_INT_ENFORCER_PARAM_H +``` + +Notice how nothing within this class is private. This allows further extension of this class. +Moreover, notice that in here we are also using variables inherited from ``DDInt``, specifically ``val_``. + +### Extending DDynamic's functions + +Extending DDynamicReconfigure means that you need additional functionality from the parameter server which 2D-reconfigure does not provide. +If that is the case, extending a class from DDynamic gives you access to make new methods as need for the extra functionality, +and access to the following to make work with DDynamic a bit easier: +* ``nh_``: this is the node handler used to create all publishers and subscribers in the parent class. +* ``params_`` this is the current parameter map 2D-reconfig uses to update parameters and add new ones. +* ``desc_pub_``: As explained before, this is the publisher responsible of updating the descriptions for the parameters and other metadata for the client and commandline. +* ``update_pub_``: This is the publisher responsible for updating the configuration values for the client and commandline. +* ``makeDescription()``: This is a helper method that generates a new Description message to be published by ``desc_pub_``. + The message can be modified. +* ``makeConfiguration()``: This is a helper method that generates a new Description message to be published by ``update_pub_``. + The message can be modified. +* ``internalCallback()``: This is a helper method that allows you to call the base param change callback built into 2D-reconfigure. + +From there, it's your choice what to do with these. \ No newline at end of file diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h new file mode 100644 index 0000000000..18b3e10912 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h @@ -0,0 +1,122 @@ +// +// Created by Noam Dori on 18/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_PARAM_H + +#include +#include +#include +#include +#include "dd_value.h" + +using namespace dynamic_reconfigure; +using namespace std; +namespace ddynamic_reconfigure { + class DDParam;// declaration for the sake of order. + // this is the pointer to any type of Dynamic Dynamic parameter. + typedef boost::shared_ptr DDPtr; + /** + * @brief The DDParam class is the abstraction of all parameter types, and is the template for creating them. + * At this point, not much is known about the parameter, but the following: + * + * - the parameter has a name + * - the parameter has a severity level + * - the parameter has a description + * - the parameter contains some value, though its type and contents are unknown. + * + * Other than storing data, the parameter also has specialised methods to interact with DDynamicReconfigure in order to apply changes and send them. + * These methods should not be touched by the user. + * + * Since this class is abstract, the class has multiple implementations whicch are not directly exposed but are used, + * so its worth checking out their descriptions. + * + * While this class is abstract, it does have one implemented thing, and that is its stream operator (`<<`) which can be freely used. + * + * While DDParam is abstract, all of its concrete implementations should follow this guideline: + * DD(const string &name, unsigned int level, const string &description, def, ) + * Where: + * - is the type name you are implementing + * - name is the reference name + * - level is the severity level + * - description is the object's description + * - def is the default value and the first value stored right after construction. + * + * You may then include extra arguments as you wish, required or optional. + */ + class DDParam { + public: + + /** + * @brief gets the name of the parameter, that is, the ID used in the program when requesting it. + * @return the unique string name of the parameter. + */ + virtual string getName() const = 0; + + /** + * @brief fetches the level of the parameter + * @return the level of the param. + */ + virtual int getLevel() const = 0; + + /** + * @brief gets the value of this parameter. + * @return the value stored in this param. + */ + virtual Value getValue() const = 0; + + /** + * @brief checks whether or not the raw value stored in the value is compatible with the given parameter. + * Compatible is a very broad word in this scenario. + * It means that the value can be placed in the parameter regardless of other limitations. + * @param val the value to test + * @return true is this parameter can handle the original value, false otherwise. + */ + virtual bool sameType(Value val) = 0; + + /** + * @brief checks whether or not the value stored in the value object, + * when converted to the type of the internal value, are equal. This acts regardless of type. + * @param val the value to test + * @return true is this parameter can is the same as the original value, false otherwise. + */ + virtual bool sameValue(Value val) = 0; + + /** + * @brief sets the value of this parameter as this one. + * @param val the value to use + */ + virtual void setValue(Value val) = 0; + + /** + * @brief updates a group message according to this param's info. + * @param group the group to update. + * @note this is an internal method. It is recommended not to use it. + */ + virtual void prepGroup(Group &group) = 0; + + /** + * @brief updates a config message according to this param's info. + * @param conf the group to update. + * @note this is an internal method. It is recommended not to use it. + */ + virtual void prepConfig(Config &conf) = 0; + + /** + * @brief updates a config description message according to this param's info. + * @param conf_desc the config description to update. + * @note this is an internal method. It is recommended not to use it. + */ + virtual void prepConfigDescription(ConfigDescription &conf_desc) = 0; + + /** + * @brief the operator taking care of streaming the param values + * @param os the stream to place the param into + * @param param the param you want to place into the stream + * @return os, but with param added. + */ + friend ostream& operator<<(ostream& os, const DDParam ¶m); + }; +} +#endif //DDYNAMIC_RECONFIGURE_DD_PARAM_H diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h new file mode 100644 index 0000000000..c5726ffcd5 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h @@ -0,0 +1,161 @@ +// +// Created by Noam Dori on 19/06/18. +// +#include +#include +#include +#include + +#ifndef DDYNAMIC_RECONFIGURE_DD_VALUE_H +#define DDYNAMIC_RECONFIGURE_DD_VALUE_H +using namespace std; +using namespace boost; +namespace ddynamic_reconfigure { + /** + * @brief The Value class is used to wrap all basic data-types (bool,int,double,string) in something generic. + * The value object always stores an explicit basic data-type. + * This has three main uses: + * + * 1. Values can represent all basic data-types. + * This means that arguments that need something relatively similar from all basic data-types can now just use the value in its argument. + * This also goes for when you need to return something that is of different data-types from different classes + * (one can only return integer, other can only return strings). + * + * 2. Values can be explicitly converted to all basic data-types they wrap. + * This means that converting an int to a string is far easier. + * + * 3. Values store the type they were instantiated with. This can be tested against to get the original piece of data the value stored. + */ + class Value { + private: + int int_val_; + string str_val_, type_; + double double_val_; + bool bool_val_; + public: + /** + * @brief creates an integer value wrapper + * @param val the int to wrap + */ + inline explicit Value(int val) : double_val_(0.0), bool_val_(false) { + type_ = "int"; + int_val_ = val; + } + + /** + * @brief creates an double value wrapper + * @param val the double to wrap + */ + inline explicit Value(double val) : int_val_(0), bool_val_(false) { + type_ = "double"; + double_val_ = val; + } + + /** + * @brief creates a string value wrapper + * @param val the string to wrap + */ + inline explicit Value(const string &val) : int_val_(0), double_val_(0.0), bool_val_(false) { + type_ = "string"; + str_val_ = val; + } + + /** + * @brief creates a c-string value wrapper, though it is considered a regular string. + * @param val the c-string to wrap + */ + inline explicit Value(const char* val) : int_val_(0), double_val_(0.0), bool_val_(false) { + *this = Value(string(val)); + } + + /** + * @brief creates an integer value wrapper + * @param val the boolean to wrap + */ + inline explicit Value(bool val) : int_val_(0), double_val_(0.0) { + type_ = "bool"; + bool_val_ = val; + } + + /** + * @brief gets the type this value wrapper stores + * @return a string containing the type: one of {"int","double","bool","string"} + */ + inline string getType() const { + return type_; + } + + /** + * @brief converts the stored value into an integer. + * @return for native integers: returns itself. + * for native doubles: returns a casted integer. + * for native booleans: returns 1 if true, 0 if false. + * for native strings: if the string contains an integer value (for example "1" contains the int '1' in it), + * it will return the integer interpretation of that string. + * Otherwise, returns the hash value of the string. + */ + inline int toInt() const { + if (type_ == "string") { + int i; + if (sscanf(str_val_.c_str(), "%d", &i) == 0) { + return (int) boost::hash()(str_val_); + } + return i; + } else if (type_ == "bool") { return bool_val_ ? 1 : 0; } + else if (type_ == "double") { return (int) double_val_; } + else { return int_val_; } + } + + /** + * @brief converts the stored value into a string. + * @return for native integers: returns the number in string form ('1' -> "1") + * for native doubles: returns the number in shorthand string form ('1.0' -> "1") + * for native booleans: returns "true" if true, "false" if false. + * for native strings: returns itself. + */ + inline string toString() const { + stringstream ss; + if(type_ == "string") { return str_val_;} + else if(type_ == "bool") {return bool_val_ ? "true" : "false";} + else if(type_ == "double") {ss << double_val_; return ss.str();} + else {ss << int_val_; return ss.str();} + } + + /** + * @brief converts the stored value into a double. + * @return for native integers: returns itself + * for native doubles: returns a itself + * for native booleans: returns 1.0 if true, 0.0 if false. + * for native strings: if the string contains a floating value (for example "1.1" contains the double '1.1' in it), + * it will return the double interpretation of that string. + * Otherwise, returns the hash value of the string. + */ + inline double toDouble() const { + if(type_ == "string") { + double f; + if(sscanf(str_val_.c_str(), "%lf", &f) == 0) { + return boost::hash()(str_val_); + } + return f; + } else if(type_ == "bool") {return bool_val_ ? 1.0 : 0.0;} + else if(type_ == "double") {return double_val_;} + else {return int_val_;} + } + + /** + * @brief converts the stored value into a boolean. + * @return for native integers: returns true if the value is bigger than 0, false otherwise. + * for native doubles: returns true if the value is bigger than 0, false otherwise. + * for native booleans: returns itself + * for native strings: returns true if the string's value is 'true', false otherwise. + */ + inline bool toBool() const { + if(type_ == "string") { return str_val_ == "true";} + else if(type_ == "bool") {return bool_val_;} + else if(type_ == "double") {return double_val_ > 0.0;} + else {return int_val_ > 0;} + } + }; +} + +#endif //DDYNAMIC_RECONFIGURE_DD_VALUE_H diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h new file mode 100644 index 0000000000..bff63cb921 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h @@ -0,0 +1,239 @@ +// +// Created by Noam Dori on 18/06/18. +// +//include space, written in C++03 +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "dd_param.h" +using namespace std; +using namespace boost; +using namespace dynamic_reconfigure; + +#ifndef DDYNAMIC_RECONFIGURE_DDYNAMIC_RECONFIGURE_H +#define DDYNAMIC_RECONFIGURE_DDYNAMIC_RECONFIGURE_H +namespace ddynamic_reconfigure { + // this is a map from the DDParam name to the object. Acts like a set with a search function. + typedef map DDMap; + // the function you will use a lot + typedef boost::function DDFunc; + + /** + * @brief The DDynamicReconfigure class is the main class responsible for keeping track of parameters basic properties, + * values, descriptions, etc. + * + * It is also responsible of handling callbacks, config change requests, description setup and config setup, + * and the ROS publishers and services. + * + * To operate a DDynamic instance, you must go through the following procedure: + * + * 1. Construct a DDynamicReconfigure instance with proper handling. + * 2. Add parameters to the instance as needed with any of the "add" methods. + * 3. Start the ROS services with any of the "start" methods. + * 4. If you need to change the callback after startup you may do so using "setCallback". + * 5. When you need to get any of the stored parameters, call either "get" or "at" on this instance, + * rather than through the callback. + */ + class DDynamicReconfigure { + public: + /** + * @brief creates the most basic instance of a 2d-conf object. + * @param nh the node handler of the node this is placed at. + */ + explicit DDynamicReconfigure(ros::NodeHandle &nh); + + /** + * @brief adds a parameter to the list, allowing it to be generated. + * @param param the pointer to the 2d-param to add to the list. + */ + virtual void add(DDPtr param); + + /** + * @brief a convenience method for adding a parameter to the list, allowing it to be generated. + * @warning When adding in this manner, you must be careful. After using this method to add the parameter, + * running any of the "remove" methods on this object WILL cause the entire param object to be deleted! + * To make sure that you can add the object back after removing it, please use the other "add" method. + * @param param the pointer to the 2d-param to add to the list. + */ + virtual void add(DDParam *param); + + /** + * removes the specified parameter from the list. + * @param param the parameter to remove. + */ + virtual void remove(DDPtr param); + + /** + * removes the specified parameter from the list. + * @param param the parameter to remove. + */ + virtual void remove(DDParam *param); + + /** + * removes the specified parameter from the list. + * @param param_name the name of the parameter to remove. + */ + virtual void remove(string param_name); + + /** + * @brief sets the callback to this. + * @param callback a boost function with the method to use when values are updated. + */ + void setCallback(DDFunc callback); + + /** + * @brief sets the callback to be empty. + */ + void clearCallback(); + + /** + * @brief starts the server and config, without having any callback. + */ + virtual void start(); + + /** + * @brief starts the server, using the given callback in function form. + * @param callback a boost function with the method to use when values are updated. + */ + void start(DDFunc callback); + + /** + * @brief starts the server, using the given callback in method form. + * @param callback a void pointer accepting a callback type with the method to use when values are updated. + */ + void start(void(*callback)(const DDMap&, int)); + + /** + * @brief starts the server, using the given callback in class-wise static method form. + * @param callback a class void pointer accepting a callback type with the method to use when values are updated. + * @param obj the object used for reference in the class void + * @tparam T the class of the object. + */ + template + void start(void(T::*callback)(const DDMap&, int), T *obj) { + DDFunc f = bind(callback,obj,_1,_2); + start(); + } + + /** + * @brief a tool people who use this API can use to find the value given within the param map. + * @param name the string to look for + * @return the value of param with the given name if it exists, a string value containing "\000" otherwise + */ + Value get(const char* name); + + /** + * @brief a tool people who use this API can use to find the param given within the param map. + * @param name the string to look for + * @return the param with the given name if it exists, nullptr otherwise + */ + DDPtr at(const char* name); + + /** + * @brief the operator taking care of streaming the param values + * @param os the stream to place the param into + * @param dd the dd-reconfigure you want to place into the stream + * @return os, but with dd-reconfigure added. + */ + friend ostream& operator<<(ostream& os, const DDynamicReconfigure &dd); + protected: + + /** + * @brief makes the config descriptions for publishing + * @return a ROS message of type ConfigDescription + */ + ConfigDescription makeDescription(); + + /** + * @brief makes the config update for publishing + * @return a ROS message of type Config + */ + Config makeConfig(); + + /** + * @brief calls the internal callback for the low-level service, not exposed to us. + * @param obj the object we are using for its callback. + * @param req ----(ROS) + * @param rsp ----(ROS) + * @return -------(ROS) + * @note this is here so that deriving methods can call the internal callback. + */ + static bool internalCallback(DDynamicReconfigure *obj, Reconfigure::Request &req, Reconfigure::Response &rsp); + + /** + * @brief the ROS node handler to use to make everything ROS related. + */ + ros::NodeHandle nh_; + /** + * @brief a map of all contained parameters. + */ + DDMap params_; + /** + * @brief the publisher responsible for updating the descriptions of the parameter for commandline (desc_pub_), + * and the publisher responsible for updating the configuration values for commandline and client (update_pub_). + * desc_pub_ publishes to "parameter_descriptions", and update_pub_ publishes to "/parameter_updates". + */ + ros::Publisher desc_pub_, update_pub_; + + private: + + /** + * @brief reassigns a value to the internal map assuming it is registered. + * @param map the map that is being edited + * @param name the name of the parameter to test + * @param value the value of the new parameter + * @tparam T the type of value + * @return -1 if the value could not be reassigned, + * 0 if the value was not changed, + * otherwise the level of the parameter changed. + */ + template + static int reassign(DDMap& map, const string &name, T value); + + /** + * @brief gets the updates and assigns them to DDMap + * @param req the ROS request holding info about the new map + * @param config the map to update + * @return the level of change (integer) + */ + int getUpdates(const Reconfigure::Request &req, DDMap &config); + + /** + * @brief the use defined callback to call when parameters are updated. + */ + boost::shared_ptr callback_; + #pragma clang diagnostic push // CLion suppressor + #pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" + /** + * @brief the service server used to trigger parameter updates. + * It also contains the new parameters sent from client or commandline. + */ + ros::ServiceServer set_service_; + #pragma clang diagnostic pop + }; + + /** + * @brief a tool people who use this API can use to find the param given within the param map. + * @param name the string to look for + * @param map the map to search + * @return the param with the given name if it exists, nullptr otherwise + */ + DDPtr at(const DDMap& map, const char* name); // I could do this with an operator, but its bad design. + + /** + * @brief a tool people who use this API can use to find the value given within the param map. + * @param name the string to look for + * @param map the map to search + * @return the value of param with the given name if it exists, a string value containing "\000" otherwise + */ + Value get(const DDMap& map, const char* name); // I could do this with an operator, but its bad design. +} +#endif //DDYNAMIC_RECONFIGURE_DDYNAMIC_RECONFIGURE_H \ No newline at end of file diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h new file mode 100644 index 0000000000..27884cc729 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h @@ -0,0 +1,12 @@ +// +// Created by Noam Dori on 20/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_ALL_PARAMS_H +#define DDYNAMIC_RECONFIGURE_DD_ALL_PARAMS_H +#include +#include +#include +#include +#include +#endif //DDYNAMIC_RECONFIGURE_DD_ALL_PARAMS_H diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h new file mode 100644 index 0000000000..80dd6d7a43 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h @@ -0,0 +1,70 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_BOOL_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_BOOL_PARAM_H + +#include "ddynamic_reconfigure/dd_param.h" + +namespace ddynamic_reconfigure { + /** + * @brief a boolean implementation of the parameter. + * These are used to handle true/false values, or bit quantities if needed. + * In ROS, booleans are handled as u-bytes (u-int8), so be careful with these! + */ + class DDBool : virtual public DDParam { + public: + string getName() const; + + void prepGroup(Group &group); + + void prepConfig(Config &conf); + + void prepConfigDescription(ConfigDescription &conf_desc); + + int getLevel() const; + + bool sameType(Value val); + + bool sameValue(Value val); + + void setValue(Value val); + + Value getValue() const; + + /** + * @brief creates a new bool param + * @param name the name of the parameter + * @param level the change level + * @param description details about the parameter + * @param def the default value + */ + DDBool(const string &name, unsigned int level, const string &description, bool def) { + name_ = name; + level_ = level; + desc_ = description; + def_ = def; + val_ = def; + } + protected: + /** + * @brief the level of the parameter: + * the degree in which things need to be shut down if this param changes + */ + unsigned int level_; + /** + * @brief the default value (def_), + * and the current value (val_) + */ + bool def_,val_; + /** + * @brief the name of the parameter (name_), + * and its description (desc_) + */ + string name_, desc_; + }; +} + + +#endif //DDYNAMIC_RECONFIGURE_DD_BOOL_PARAM_H diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h new file mode 100644 index 0000000000..72c74124f4 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h @@ -0,0 +1,80 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_DOUBLE_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_DOUBLE_PARAM_H + +#include "ddynamic_reconfigure/dd_param.h" + + +namespace ddynamic_reconfigure { + typedef numeric_limits d_limit; + /** + * @brief a double implementation of the parameter. + * This is used to handle double-precision floating point numbers, + * though it can handle single precision as well. + */ + class DDDouble : virtual public DDParam { + public: + string getName() const; + + void prepGroup(Group &group); + + void prepConfig(Config &conf); + + void prepConfigDescription(ConfigDescription &conf_desc); + + int getLevel() const; + + bool sameType(Value val); + + bool sameValue(Value val); + + void setValue(Value val); + + Value getValue() const; + + /** + * creates a new double param + * @param name the name of the parameter + * @param level the change level + * @param def the default value + * @param description details about the parameter + * @param max the maximum allowed value. Defaults to DBL_MAX + * @param min the minimum allowed value. Defaults to -DBL_MAX + */ + DDDouble(const string &name, unsigned int level, const string &description, double def, + double min = -d_limit::infinity(), double max = d_limit::infinity()) + : max_(), min_() { + name_ = name; + level_ = level; + desc_ = description; + def_ = def; + val_ = def; + max_ = max; + min_ = min; + } + protected: + /** + * @brief the level of the parameter: + * the degree in which things need to be shut down if this param changes + */ + unsigned int level_; + /** + * @brief the default value (def_), + * the current value (val_), + * the minimum allowed value (min_), + * and the maximum allowed value (max_) + */ + double def_,max_,min_,val_; + /** + * @brief the name of the parameter (name_), + * and its description (desc_) + */ + string name_, desc_; + }; +} + + +#endif //DDYNAMIC_RECONFIGURE_DD_DOUBLE_PARAM_H diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h new file mode 100644 index 0000000000..12669a6bd9 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h @@ -0,0 +1,119 @@ +// +// Created by Noam Dori on 20/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_ENUM_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_ENUM_PARAM_H + +#include "dd_int_param.h" +#include + +namespace ddynamic_reconfigure { + typedef map > EnumMap; + /** + * @brief an integer enum implementation of the parameter. + * This is an extension to the int parameter, + * which allows creating string aliases for certain (if not all) numbers available. + * + */ + class DDEnum : virtual public DDInt { + public: + + void prepGroup(Group &group); + + bool sameType(Value val); + + bool sameValue(Value val); + + void setValue(Value val); + + /** + * @brief creates a new int-enum param + * @param name the name of the parameter + * @param level the change level + * @param def the default value in integer form + * @param description details about the parameter + * @param dictionary the alias dictionary this enum will use. + */ + DDEnum(const string &name, unsigned int level, const string &description, + int def, const map &dictionary); + + /** + * @brief creates a new int-enum param + * @param name the name of the parameter + * @param level the change level + * @param def an alias of the default value + * @param description details about the parameter + * @param dictionary the alias dictionary this enum will use. + */ + DDEnum(const string &name, unsigned int level, const string &description, + const string& def, const map &dictionary); + + #pragma clang diagnostic push + #pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" + /** + * @brief creates a new int-enum param + * @param name the name of the parameter + * @param level the change level + * @param def the default value in integer form + * @param description details about the parameter + * @param dictionary the alias dictionary this enum will use. + * @note since ROS cannot send the enum and const descriptions, this method is useless. + * Please use the constructor which takes a map instead. + * @deprecated see note. This is not tested, so it may fail. + */ + DDEnum(const string &name, unsigned int level, const string &description, + int def, const pair >,string> &dictionary); + + /** + * @brief creates a new int-enum param + * @param name the name of the parameter + * @param level the change level + * @param def an alias of the default value + * @param description details about the parameter + * @param dictionary the alias dictionary this enum will use. + * @note since ROS cannot send the enum and const descriptions, this method is useless. + * Please use the constructor which takes a map instead. + * @deprecated see note. This is not tested, so it may fail. + */ + DDEnum(const string &name, unsigned int level, const string &description, + const string& def, const pair >,string> &dictionary); + #pragma clang diagnostic pop + + protected: + /** + * @brief A dictionary from the string aliases to their integer counterparts. + * This method of storage allows integers to have multiple aliases. + */ + const EnumMap dict_; + /** + * @brief this holds the physical enum's description. why is this here? because 1D-reconfigure. + */ + string enum_description_; + private: + /** + * converts the value given to an integer according to the embedded dictionary. + * @param val the value to look up within the dictionary + * @return if the value is a string which exists in the dictionary, returns the int definition of the term given. + * otherwise, returns the Value object defined conversion of the type to an integer. + */ + int lookup(Value val); + + /** + * generates the 'edit_method' sting for prepGroup(). + * @return a string that should not be touched. + */ + string makeEditMethod(); + + /** + * generates a 'const' sting for prepGroup(). + * @param name the name of the constant + * @param value the value of the constant + * @param desc the description given to the constant. + * @return a string that should not be touched. + */ + string makeConst(string name, int value, string desc); + }; +} + +#endif //DDYNAMIC_RECONFIGURE_DD_ENUM_PARAM_H \ No newline at end of file diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h new file mode 100644 index 0000000000..a207ec4845 --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h @@ -0,0 +1,78 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_INT_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_INT_PARAM_H + +#include "ddynamic_reconfigure/dd_param.h" + +namespace ddynamic_reconfigure { + /** + * @brief an integer implementation of the parameter. + * This is used to 32 bit signed integral numbers. + * This can also handle shorts, bytes, and other integrals provided they are not too big + * (by then looping will occur) + */ + class DDInt : virtual public DDParam { + public: + string getName() const; + + void prepGroup(Group &group); + + void prepConfig(Config &conf); + + void prepConfigDescription(ConfigDescription &conf_desc); + + int getLevel() const; + + bool sameType(Value val); + + bool sameValue(Value val); + + void setValue(Value val); + + Value getValue() const; + + /** + * creates a new int param + * @param name the name of the parameter + * @param level the change level + * @param def the default value + * @param description details about the parameter + * @param max the maximum allowed value. Defaults to INT32_MAX + * @param min the minimum allowed value. Defaults to INT32_MIN + */ + inline DDInt(const string &name, unsigned int level, const string &description, + int def, int min = INT32_MIN, int max = INT32_MAX) { + name_ = name; + level_ = level; + desc_ = description; + def_ = def; + val_ = def; + max_ = max; + min_ = min; + } + + protected: + /** + * @brief the level of the parameter: + * the degree in which things need to be shut down if this param changes + */ + unsigned int level_; + /** + * @brief the default value (def_), + * the current value (val_), + * the minimum allowed value (min_), + * and the maximum allowed value (max_) + */ + int def_,max_,min_,val_; + /** + * @brief the name of the parameter (name_), + * and its description (desc_) + */ + string name_, desc_; + }; +} + +#endif //DDYNAMIC_RECONFIGURE_DD_INT_PARAM_H diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h new file mode 100644 index 0000000000..6a40ccbfcc --- /dev/null +++ b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h @@ -0,0 +1,70 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#ifndef DDYNAMIC_RECONFIGURE_DD_STRING_PARAM_H +#define DDYNAMIC_RECONFIGURE_DD_STRING_PARAM_H + +#include "ddynamic_reconfigure/dd_param.h" + +namespace ddynamic_reconfigure { + /** + * @brief a string implementation of the parameter. + * This is used to handle strings of characters of variable length. + * Like string, each param value can hold up to 2^32-1 characters. + */ + class DDString : virtual public DDParam { + public: + string getName() const; + + void prepGroup(Group &group); + + void prepConfig(Config &conf); + + void prepConfigDescription(ConfigDescription &conf_desc); + + int getLevel() const; + + bool sameType(Value val); + + bool sameValue(Value val); + + void setValue(Value val); + + Value getValue() const; + + /** + * creates a new string param + * @param name the name of the parameter + * @param level the change level + * @param description details about the parameter + * @param def the default value + */ + DDString(const string &name, unsigned int level, const string &description, const string &def) { + name_ = name; + level_ = level; + desc_ = description; + def_ = def; + val_ = def; + } + protected: + /** + * @brief the level of the parameter: + * the degree in which things need to be shut down if this param changes + */ + unsigned int level_; + /** + * @brief the default value (def_), + * and the current value (val_) + */ + string def_,val_; + /** + * @brief the name of the parameter (name_), + * and its description (desc_) + */ + string name_, desc_; + }; +} + + +#endif //DDYNAMIC_RECONFIGURE_DD_STRING_PARAM_H diff --git a/ddynamic_reconfigure/package.xml b/ddynamic_reconfigure/package.xml new file mode 100644 index 0000000000..a5aed627a6 --- /dev/null +++ b/ddynamic_reconfigure/package.xml @@ -0,0 +1,20 @@ + + + ddynamic_reconfigure + 0.0.6 + The ddynamic_reconfigure package + + Noam Dori + + BSD + + + + Noam Dori + + catkin + roscpp + + rostest + gmock + diff --git a/ddynamic_reconfigure/src/dd_param.cpp b/ddynamic_reconfigure/src/dd_param.cpp new file mode 100644 index 0000000000..f3d698e129 --- /dev/null +++ b/ddynamic_reconfigure/src/dd_param.cpp @@ -0,0 +1,12 @@ +// +// Created by Noam Dori on 02/07/18. +// + +#include + +namespace ddynamic_reconfigure { + ostream &operator<<(ostream &os, const DDParam ¶m) { + os << param.getName() << ":" << param.getValue().toString(); + return os; + } +} \ No newline at end of file diff --git a/ddynamic_reconfigure/src/ddynamic_reconfigure.cpp b/ddynamic_reconfigure/src/ddynamic_reconfigure.cpp new file mode 100644 index 0000000000..f5880c42dc --- /dev/null +++ b/ddynamic_reconfigure/src/ddynamic_reconfigure.cpp @@ -0,0 +1,214 @@ +#pragma clang diagnostic push +#pragma ide diagnostic ignored "OCDFAInspection" +#pragma clang diagnostic ignored "-Wunknown-pragmas" +#pragma ide diagnostic ignored "modernize-loop-convert" +#pragma ide diagnostic ignored "modernize-use-auto" +// +// Created by Noam Dori on 18/06/18. +// +#include +#include + +using namespace boost; +namespace ddynamic_reconfigure { + + DDynamicReconfigure::DDynamicReconfigure(ros::NodeHandle &nh) { + nh_ = nh; + }; + + void DDynamicReconfigure::add(DDPtr param) { + params_[param->getName()] = param; + }; + + void DDynamicReconfigure::add(DDParam *param) { + add(DDPtr(param)); + }; + + void DDynamicReconfigure::remove(DDPtr param) { + remove(param->getName()); + }; + + void DDynamicReconfigure::remove(DDParam *param) { + remove(param->getName()); + }; + + void DDynamicReconfigure::remove(string param_name) { + params_.erase(param_name); + }; + + void DDynamicReconfigure::start() { + ConfigDescription conf_desc = makeDescription(); // registers defaults and max/min descriptions. + Config conf = makeConfig(); // the actual config file in C++ form. + + function callback = bind(&internalCallback,this,_1,_2); + // publishes Config and ConfigDescription. + set_service_ = nh_.advertiseService("set_parameters", callback); // this allows changes to the parameters + + // this makes the parameter descriptions + desc_pub_ = nh_.advertise("parameter_descriptions", 1, true); + desc_pub_.publish(conf_desc); + + // this creates the type/level of everything + update_pub_ = nh_.advertise("parameter_updates", 1, true); + update_pub_.publish(conf); + } + + Config DDynamicReconfigure::makeConfig() { + Config conf; + GroupState group_state; // I dunno, but its important? + + // action 3 - prepping the GroupState msg for all params + group_state.name = "Default"; + group_state.state = (unsigned char)true; + + // action 4 - prepping the Config msg for all params + conf.groups.push_back(group_state); + for(DDMap::const_iterator it = params_.begin(); it != params_.end(); it++) {it->second->prepConfig(conf);} + return conf; + } + + ConfigDescription DDynamicReconfigure::makeDescription() { + ConfigDescription conf_desc; + Group group; // registers level, type, description. + + // action 1 - prepping the Group msg for all params + group.name = "default"; + for(DDMap::const_iterator it = params_.begin(); it != params_.end(); it++) {it->second->prepGroup(group);} + + // action 2 - prepping the ConfigDescription msg for all params + for(DDMap::const_iterator it = params_.begin(); it != params_.end(); it++) {it->second->prepConfigDescription(conf_desc);} + conf_desc.groups.push_back(group); + return conf_desc; + }; + + void DDynamicReconfigure::start(DDFunc callback) { + start(); + setCallback(callback); + }; + + void DDynamicReconfigure::start(void(*callback)(const DDMap&,int)) { + DDFunc f(callback); + start(f); + }; + + // this is also available in the header file (linking template functions is problematic. + // template void DDynamicReconfigure::start(void(T::*callback)(const DDMap&,int), T *obj) { + // DDFunc f = bind(callback,obj,_1); + // start(); + // } + + void DDynamicReconfigure::setCallback(DDFunc callback) { + callback_ = make_shared >(callback); + }; + + void defaultCallback(const DDMap&,int) {}; + + void DDynamicReconfigure::clearCallback() { + callback_ = make_shared(&defaultCallback); + }; + + // Private function: internal callback used by the service to call our lovely callback. + bool DDynamicReconfigure::internalCallback(DDynamicReconfigure *obj, Reconfigure::Request& req, Reconfigure::Response& rsp) { + ROS_DEBUG_STREAM("Called config callback of ddynamic_reconfigure"); + + int level = obj->getUpdates(req, obj->params_); + + if (obj->callback_) { + try { + (*obj->callback_)(obj->params_,level); + } catch (std::exception &e) { + ROS_WARN("Reconfigure callback failed with exception %s: ", e.what()); + } catch (...) { + ROS_WARN("Reconfigure callback failed with unprintable exception."); + } + } + + obj->update_pub_.publish(obj->makeConfig()); // updates the config + + return true; + } + + int DDynamicReconfigure::getUpdates(const Reconfigure::Request &req, DDMap &config) { + int level = 0; + // the ugly part of the code, since ROS does not provide a nice generic message. Oh well... + BOOST_FOREACH(const IntParameter i,req.config.ints) { + int new_level = reassign(config, i.name, i.value); + if(new_level == -1) { + ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered"); + } else { + level |= new_level; + } + } + BOOST_FOREACH(const DoubleParameter i,req.config.doubles) { + int new_level = reassign(config, i.name, i.value); + if(new_level == -1) { + ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered"); + } else { + level |= new_level; + } + } + BOOST_FOREACH(const BoolParameter i,req.config.bools) { + int new_level = reassign(config, i.name, (bool)i.value); + if(new_level == -1) { + ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered"); + } else { + level |= new_level; + } + } + BOOST_FOREACH(const StrParameter i,req.config.strs) { + int new_level = reassign(config, i.name, i.value); + if(new_level == -1) { + ROS_ERROR_STREAM("Variable [" << i.name << "] is not registered"); + } else { + level |= new_level; + } + } + return level; + } + + template + int DDynamicReconfigure::reassign(DDMap& map, const string &name, T value) { + Value val(value); // abusing C++ against generic types here. + if(map.find(name) != map.end() && map[name]->sameType(val)) { + DDPtr old = map[name]; // this is the old map which might be updated. + if(old->sameValue(val)) { return 0;} else { + old->setValue(val); + return old->getLevel(); + } + } else { + return -1; + } + } + + Value DDynamicReconfigure::get(const char *name) { + return ddynamic_reconfigure::get(params_,name); + } + + DDPtr DDynamicReconfigure::at(const char *name) { + return ddynamic_reconfigure::at(params_,name); + } + + ostream &operator<<(ostream &os, const DDynamicReconfigure &dd) { + os << "{" << *dd.params_.begin()->second; + for(DDMap::const_iterator it = ++dd.params_.begin(); it != dd.params_.end(); it++) { + os << "," << *it->second; + } + os << "}"; + return os; + } + + DDPtr at(const DDMap& map, const char *name) { + DDMap::const_iterator it = map.find(name); + if(it == map.end()) { + return DDPtr(); // null pointer + } else { return it->second;} + } + + Value get(const DDMap& map, const char *name) { + DDMap::const_iterator it = map.find(name); + if(it == map.end()) { + return Value("\000"); + } else { return it->second->getValue();} + } +} +#pragma clang diagnostic pop \ No newline at end of file diff --git a/ddynamic_reconfigure/src/param/dd_bool_param.cpp b/ddynamic_reconfigure/src/param/dd_bool_param.cpp new file mode 100644 index 0000000000..036b332f17 --- /dev/null +++ b/ddynamic_reconfigure/src/param/dd_bool_param.cpp @@ -0,0 +1,58 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#include + +namespace ddynamic_reconfigure { + string DDBool::getName() const { + return name_; + } + + void DDBool::prepGroup(Group &group) { + ParamDescription desc; + desc.name = name_; + desc.level = level_; + desc.description = desc_; + desc.type = "bool"; + group.parameters.push_back(desc); + } + + void DDBool::prepConfig(Config &conf) { + BoolParameter param; + param.name = name_; + param.value = (unsigned char)val_; + conf.bools.push_back(param); + } + + void DDBool::prepConfigDescription(ConfigDescription &conf_desc) { + BoolParameter param; + param.name = name_; + param.value = (unsigned char)def_; + conf_desc.dflt.bools.push_back(param); + param.value = (unsigned char)true; + conf_desc.max.bools.push_back(param); + param.value = (unsigned char)false; + conf_desc.min.bools.push_back(param); + } + + int DDBool::getLevel() const { + return level_; + } + + bool DDBool::sameType(Value val) { + return val.getType() == "bool"; + } + + bool DDBool::sameValue(Value val) { + return val.toBool() == val_; + } + + void DDBool::setValue(Value val) { + val_ = val.toBool(); + } + + Value DDBool::getValue() const { + return Value(val_); + } +} diff --git a/ddynamic_reconfigure/src/param/dd_double_param.cpp b/ddynamic_reconfigure/src/param/dd_double_param.cpp new file mode 100644 index 0000000000..3c988de240 --- /dev/null +++ b/ddynamic_reconfigure/src/param/dd_double_param.cpp @@ -0,0 +1,58 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#include + +namespace ddynamic_reconfigure { + string DDDouble::getName() const { + return name_; + } + + void DDDouble::prepGroup(Group &group) { + ParamDescription desc; + desc.name = name_; + desc.level = level_; + desc.description = desc_; + desc.type = "double"; + group.parameters.push_back(desc); + } + + void DDDouble::prepConfig(Config &conf) { + DoubleParameter param; + param.name = name_; + param.value = val_; + conf.doubles.push_back(param); + } + + void DDDouble::prepConfigDescription(ConfigDescription &conf_desc) { + DoubleParameter param; + param.name = name_; + param.value = def_; + conf_desc.dflt.doubles.push_back(param); + param.value = max_; + conf_desc.max.doubles.push_back(param); + param.value = min_; + conf_desc.min.doubles.push_back(param); + } + + int DDDouble::getLevel() const { + return level_; + } + + bool DDDouble::sameType(Value val) { + return val.getType() == "double"; + } + + bool DDDouble::sameValue(Value val) { + return val.toDouble() == val_; + } + + void DDDouble::setValue(Value val) { + val_ = val.toDouble(); + } + + Value DDDouble::getValue() const { + return Value(val_); + } +} diff --git a/ddynamic_reconfigure/src/param/dd_enum_param.cpp b/ddynamic_reconfigure/src/param/dd_enum_param.cpp new file mode 100644 index 0000000000..c840ba915c --- /dev/null +++ b/ddynamic_reconfigure/src/param/dd_enum_param.cpp @@ -0,0 +1,148 @@ +#pragma clang diagnostic push +#pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" +#pragma clang diagnostic ignored "-Wunknown-pragmas" +#pragma ide diagnostic ignored "modernize-loop-convert" +#pragma ide diagnostic ignored "modernize-use-auto" +// +// Created by Noam Dori on 20/06/18. +// + +#include + +#include "ddynamic_reconfigure/param/dd_enum_param.h" + +map > fillGaps(map old_map) { + map > ret; + for(map::const_iterator it = old_map.begin(); it != old_map.end(); it++) { + ret[it->first] = pair(it->second,""); + }; + return ret; +}; + +namespace ddynamic_reconfigure { + + void DDEnum::prepGroup(Group &group) { + ParamDescription desc; + desc.name = name_; + desc.level = level_; + desc.description = desc_; + desc.type = "int"; + desc.edit_method = makeEditMethod(); + group.parameters.push_back(desc); + } + + bool DDEnum::sameType(Value val) { + return val.getType() == "int" || val.getType() == "string"; + } + + bool DDEnum::sameValue(Value val) { + if(val.getType() == "string" && dict_.find(val.toString())->second.first == val_) { + return true; + } else { + return val.toInt() == val_; + } + } + + void DDEnum::setValue(Value val) { + if(val.getType() == "string" && dict_.find(val.toString()) != dict_.end()) { + val_ = lookup(val); + } else { + val_ = val.toInt(); + } + } + + int DDEnum::lookup(Value val) { + if(val.getType() == "string" && dict_.find(val.toString()) != dict_.end()) { + return dict_.find(val.toString())->second.first; + } else { + return val.toInt(); + } + } + + DDEnum::DDEnum(const string &name, unsigned int level, const string &description, + int def, const map &dictionary) : + DDInt(name,level,description,def), + dict_(fillGaps(dictionary)) { + max_ = def; + min_ = def; + for(map::const_iterator it = dictionary.begin(); it != dictionary.end(); it++) { + if(it->second > max_) {max_ = it->second;} + if(it->second < min_) {min_ = it->second;} + }; + } + + DDEnum::DDEnum(const string &name, unsigned int level, const string &description, + const string &def, const map &dictionary) : + DDInt(name,level,description,dictionary.find(def)->second), + dict_(fillGaps(dictionary)) { + max_ = def_; + min_ = def_; + for(map::const_iterator it = dictionary.begin(); it != dictionary.end(); it++) { + if(it->second > max_) {max_ = it->second;} + if(it->second < min_) {min_ = it->second;} + }; + } + + DDEnum::DDEnum(const string &name, unsigned int level, const string &description, int def, + const pair &dictionary) : + DDInt(name,level,description,def), + dict_(dictionary.first) { + max_ = def; + min_ = def; + for(EnumMap::const_iterator it = dict_.begin(); it != dict_.end(); it++) { + if(it->second.first > max_) {max_ = it->second.first;} + if(it->second.first < min_) {min_ = it->second.first;} + }; + enum_description_ = dictionary.second; + } + + DDEnum::DDEnum(const string &name, unsigned int level, const string &description, const string &def, + const pair &dictionary) : + DDInt(name,level,description,dictionary.first.find(def)->second.first), + dict_(dictionary.first) { + max_ = def_; + min_ = def_; + for(EnumMap::const_iterator it = dict_.begin(); it != dict_.end(); it++) { + if(it->second.first > max_) {max_ = it->second.first;} + if(it->second.first < min_) {min_ = it->second.first;} + }; + enum_description_ = dictionary.second; + } + + string DDEnum::makeEditMethod() { + stringstream ret; + ret << "{"; + { + ret << "'enum_description': '" << enum_description_ << "', "; + ret << "'enum': ["; + { + EnumMap::const_iterator it = dict_.begin(); + ret << makeConst(it->first, it->second.first, it->second.second); + for(it++; it != dict_.end(); it++) { + ret << ", " << makeConst(it->first, it->second.first, it->second.second); + }; + } + ret << "]"; + } + ret << "}"; + return ret.str(); + } + + string DDEnum::makeConst(string name, int value, string desc) { + stringstream ret; + ret << "{"; + { + ret << "'srcline': 0, "; // the sole reason this is here is because dynamic placed it in its enum JSON. + ret << "'description': '" << desc << "', "; + ret << "'srcfile': '/does/this/really/matter.cfg', "; // the answer is no. This is useless. + ret << "'cconsttype': 'const int', "; + ret << "'value': " << value << ", "; + ret << "'ctype': 'int', "; + ret << "'type': 'int', "; + ret << "'name': '" << name << "'"; + } + ret << "}"; + return ret.str(); + } +} +#pragma clang diagnostic pop \ No newline at end of file diff --git a/ddynamic_reconfigure/src/param/dd_int_param.cpp b/ddynamic_reconfigure/src/param/dd_int_param.cpp new file mode 100644 index 0000000000..f8b52d174b --- /dev/null +++ b/ddynamic_reconfigure/src/param/dd_int_param.cpp @@ -0,0 +1,58 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#include + +namespace ddynamic_reconfigure { + string DDInt::getName() const { + return name_; + } + + void DDInt::prepGroup(Group &group) { + ParamDescription desc; + desc.name = name_; + desc.level = level_; + desc.description = desc_; + desc.type = "int"; + group.parameters.push_back(desc); + } + + void DDInt::prepConfig(Config &conf) { + IntParameter param; + param.name = name_; + param.value = val_; + conf.ints.push_back(param); + } + + void DDInt::prepConfigDescription(ConfigDescription &conf_desc) { + IntParameter param; + param.name = name_; + param.value = def_; + conf_desc.dflt.ints.push_back(param); + param.value = max_; + conf_desc.max.ints.push_back(param); + param.value = min_; + conf_desc.min.ints.push_back(param); + } + + int DDInt::getLevel() const { + return level_; + } + + bool DDInt::sameType(Value val) { + return val.getType() == "int"; + } + + bool DDInt::sameValue(Value val) { + return val.toInt() == val_; + } + + void DDInt::setValue(Value val) { + val_ = val.toInt(); + } + + Value DDInt::getValue() const { + return Value(val_); + } +} diff --git a/ddynamic_reconfigure/src/param/dd_string_param.cpp b/ddynamic_reconfigure/src/param/dd_string_param.cpp new file mode 100644 index 0000000000..ee8ca850b0 --- /dev/null +++ b/ddynamic_reconfigure/src/param/dd_string_param.cpp @@ -0,0 +1,58 @@ +// +// Created by Noam Dori on 19/06/18. +// + +#include + +namespace ddynamic_reconfigure { + string DDString::getName() const { + return name_; + } + + void DDString::prepGroup(Group &group) { + ParamDescription desc; + desc.name = name_; + desc.level = level_; + desc.description = desc_; + desc.type = "string"; + group.parameters.push_back(desc); + } + + void DDString::prepConfig(Config &conf) { + StrParameter param; + param.name = name_; + param.value = val_; + conf.strs.push_back(param); + } + + void DDString::prepConfigDescription(ConfigDescription &conf_desc) { + StrParameter param; + param.name = name_; + param.value = def_; + conf_desc.dflt.strs.push_back(param); + param.value = ""; + conf_desc.max.strs.push_back(param); + param.value = ""; + conf_desc.min.strs.push_back(param); + } + + int DDString::getLevel() const { + return level_; + } + + bool DDString::sameType(Value val) { + return val.getType() == "string"; + } + + bool DDString::sameValue(Value val) { + return val.toString() == val_; + } + + void DDString::setValue(Value val) { + val_ = val.toString(); + } + + Value DDString::getValue() const { + return Value(val_); + } +} diff --git a/ddynamic_reconfigure/test/TutorialParams.srv b/ddynamic_reconfigure/test/TutorialParams.srv new file mode 100644 index 0000000000..1a8ba2f86e --- /dev/null +++ b/ddynamic_reconfigure/test/TutorialParams.srv @@ -0,0 +1,6 @@ +--- +int32 int_param +float64 double_param +string str_param +bool bool_param +int32 enum_param \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp b/ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp new file mode 100644 index 0000000000..43ff73b2b1 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp @@ -0,0 +1,75 @@ +// +// Created by Noam Dori on 01/07/18. +// +#include +#include +#include +#include +#include + +using namespace ros; +namespace ddynamic_reconfigure { + + /** + * @brief A ROS client making sure the server sends the new information. + */ + TEST(DDFullScaleTest, doTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + // ROS init stage + NodeHandle nh; + + ServiceClient dynamics = nh.serviceClient("set_parameters"); + int new_int = 2, new_enum = 2; + double new_double = 0.2; + string new_string = "2"; + bool new_bool = false; + + dynamic_reconfigure::Reconfigure srv; + + dynamic_reconfigure::IntParameter int_param; + int_param.name = "int_param"; + int_param.value = new_int; + srv.request.config.ints.push_back(int_param); + + dynamic_reconfigure::DoubleParameter double_param; + double_param.name = "double_param"; + double_param.value = new_double; + srv.request.config.doubles.push_back(double_param); + + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "bool_param"; + bool_param.value = (unsigned char)new_bool; + srv.request.config.bools.push_back(bool_param); + + dynamic_reconfigure::StrParameter str_param; + str_param.name = "str_param"; + str_param.value = new_string; + srv.request.config.strs.push_back(str_param); + + dynamic_reconfigure::IntParameter enum_param; + enum_param.name = "enum_param"; + enum_param.value = new_enum; + srv.request.config.ints.push_back(enum_param); + + ASSERT_TRUE(dynamics.call(srv)); + + ServiceClient client = nh.serviceClient("get_params"); + ddynamic_reconfigure::TutorialParams params; + ASSERT_TRUE(client.call(params)); + + ASSERT_EQ(new_int,params.response.int_param); + ASSERT_EQ(new_double,params.response.double_param); + ASSERT_EQ(new_string,params.response.str_param); + ASSERT_EQ(new_bool,params.response.bool_param); + ASSERT_EQ(new_enum,params.response.enum_param); + } +} + + +int main(int argc, char** argv) { + init(argc, argv, "dd_client"); + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test b/ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test new file mode 100644 index 0000000000..f1690d38d1 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp b/ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp new file mode 100644 index 0000000000..1d9a591ea7 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp @@ -0,0 +1,62 @@ +#include +#include +#include +#include + +/** + Topics: + * /dd_server/parameter_descriptions [dynamic_reconfigure/ConfigDescription] + * /dd_server/parameter_updates [dynamic_reconfigure/Config] + + Services: + * /dd_server/set_parameters: dynamic_reconfigure/Reconfigure +*/ +using namespace ddynamic_reconfigure; +using namespace ros; +using namespace ddynamic_reconfigure; + +bool paramService(TutorialParams::Request& req, TutorialParams::Response& res, DDynamicReconfigure& dd) { + res.int_param = dd.get("int_param").toInt(); + res.double_param = dd.get("double_param").toDouble(); + res.str_param = dd.get("str_param").toString(); + res.enum_param = dd.get("enum_param").toInt(); + return true; +} + +void callback(const DDMap& map, int) { + ROS_INFO("Reconfigure Request: %d %f %s %s %ld", + get(map, "int_param").toInt(), + get(map, "double_param").toDouble(), + get(map, "str_param").toString().c_str(), + get(map, "bool_param").toBool() ? "True" : "False", + map.size()); +} + +int main(int argc, char **argv) { + // ROS init stage + init(argc, argv, "dd_server"); + NodeHandle nh; + + // DDynamic setup stage + DDynamicReconfigure dd(nh); + dd.add(new DDInt("int_param", 0, "An Integer parameter", 0, 50, 100)); + dd.add(new DDDouble("double_param", 0, "A double parameter", .5, 0, 1)); + dd.add(new DDString("str_param", 0, "A string parameter", "Hello World")); + dd.add(new DDBool("bool_param", 0, "A Boolean parameter", true)); + map dict; + dict["Small"] = 0; + dict["Medium"] = 1; + dict["Large"] = 2; + dict["ExtraLarge"] = 3; + dd.add(new DDEnum("enum_param", 0, "A size parameter which is edited via an enum", 1, dict)); + dd.start(callback); + + // Actual Server Node code + ROS_INFO("Spinning node"); + function f = bind(paramService, _1, _2, dd); + ServiceServer checkParam = nh.advertiseService("get_params", f); + MultiThreadedSpinner spinner(2); + spinner.spin(); + return 0; +} + diff --git a/ddynamic_reconfigure/test/dd_param/dd_bool.test b/ddynamic_reconfigure/test/dd_param/dd_bool.test new file mode 100644 index 0000000000..e2616df940 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/dd_bool.test @@ -0,0 +1,3 @@ + + + diff --git a/ddynamic_reconfigure/test/dd_param/dd_double.test b/ddynamic_reconfigure/test/dd_param/dd_double.test new file mode 100644 index 0000000000..8cac0e7d55 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/dd_double.test @@ -0,0 +1,3 @@ + + + diff --git a/ddynamic_reconfigure/test/dd_param/dd_enum.test b/ddynamic_reconfigure/test/dd_param/dd_enum.test new file mode 100644 index 0000000000..2d7a7d3e85 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/dd_enum.test @@ -0,0 +1,3 @@ + + + diff --git a/ddynamic_reconfigure/test/dd_param/dd_int.test b/ddynamic_reconfigure/test/dd_param/dd_int.test new file mode 100644 index 0000000000..c85d98b52f --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/dd_int.test @@ -0,0 +1,3 @@ + + + diff --git a/ddynamic_reconfigure/test/dd_param/dd_string.test b/ddynamic_reconfigure/test/dd_param/dd_string.test new file mode 100644 index 0000000000..7e9b4bec68 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/dd_string.test @@ -0,0 +1,3 @@ + + + diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp new file mode 100644 index 0000000000..6cbe7cf9fc --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp @@ -0,0 +1,61 @@ +#include +#include +#include + +namespace ddynamic_reconfigure { + + /** + * @brief preliminary test which makes sure we can use the object. + */ + TEST(DDBoolTest, constructorTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDBool param1("param1",0,"param1",true); + DDBool param2("",0,"",false); + DDBool param3("\000",0,"\000",false); // NOLINT(bugprone-string-literal-with-embedded-nul) + } + + /** + * @brief a test making sure we can handle all API for handling the values of the param + */ + TEST(DDBoolTest, valueTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDBool param("dd_param",0,"dd_param",true); + // we won't do any tests on getLevel or getName, as those are implicit. + Value v(true); + ASSERT_TRUE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(1); + ASSERT_FALSE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(false); + ASSERT_TRUE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + v = Value(0); + ASSERT_FALSE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + param.setValue(v); + v = Value(true); + ASSERT_FALSE(param.sameValue(v)); // makes sure anti-aliasing happens + + ASSERT_TRUE(param.getValue().getType() == "bool"); + ASSERT_TRUE(param.sameValue(Value(false))); + } + + TEST(DDBoolTest, streamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDBool param1("param1",0,"param1",true); + stringstream stream; + stream << param1; + ASSERT_EQ(param1.getName() + ":" + param1.getValue().toString(),stream.str()); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_double.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_double.cpp new file mode 100644 index 0000000000..87081461e5 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/test_dd_double.cpp @@ -0,0 +1,61 @@ +#include +#include +#include + +namespace ddynamic_reconfigure { + + /** + * @brief preliminary test which makes sure we can use the object. + */ + TEST(DDDoubleTest, constructorTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDDouble param1("param1",0,"param1",0.5); + DDDouble param2("",0,"",1,10); + DDDouble param3("\000",0,"\000", -0, -3.4e100, 43.5e20); // NOLINT(bugprone-string-literal-with-embedded-nul) + } + + /** + * @brief a test making sure we can handle all API for handling the values of the param + */ + TEST(DDDoubleTest, valueTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDDouble param("dd_param",0,"dd_param",1); + // we won't do any tests on getLevel or getName, as those are implicit. + Value v(1.0); + ASSERT_TRUE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(1); + ASSERT_FALSE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(2.0); + ASSERT_TRUE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + v = Value(2); + ASSERT_FALSE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + param.setValue(v); + v = Value(3); + ASSERT_FALSE(param.sameValue(v)); // makes sure anti-aliasing happens + + ASSERT_TRUE(param.getValue().getType() == "double"); + ASSERT_TRUE(param.sameValue(Value(2))); + } + + TEST(DDDoubleTest, streamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDDouble param1("param1",0,"param1",1.0); + stringstream stream; + stream << param1; + ASSERT_EQ(param1.getName() + ":" + param1.getValue().toString(),stream.str()); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp new file mode 100644 index 0000000000..8fc8f45c1b --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp @@ -0,0 +1,106 @@ +#include +#include +#include + +namespace ddynamic_reconfigure { + + /** + * @brief preliminary test which makes sure we can use the object. + */ + TEST(DDEnumTest, constructorTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + map dict; + dict["ONE"] = 1; + dict["NEG-ONE"] = -1; + dict["TEN"] = 10; + DDEnum param1("param1",0,"param1",0,dict); + DDEnum param2("",0,"","ONE",dict); + DDEnum param3("\000",0,"\000", 0, dict); // NOLINT(bugprone-string-literal-with-embedded-nul) + } + + /** + * @brief a test making sure we can handle all API for handling the values of the param + */ + TEST(DDEnumTest, valueTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + map dict; + dict["ONE"] = 1; + dict["NEG-ONE"] = -1; + dict["TEN"] = 10; + DDEnum param("dd_param",0,"dd_param",1,dict); + // we won't do any tests on getLevel or getName, as those are implicit. + Value v(1); + ASSERT_TRUE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value("ONE"); + ASSERT_TRUE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(1.0); + ASSERT_FALSE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(10); + ASSERT_TRUE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + v = Value("TEN"); + ASSERT_TRUE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + v = Value(10.0); + ASSERT_FALSE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + param.setValue(v); + v = Value(-1); + ASSERT_FALSE(param.sameValue(v)); // makes sure anti-aliasing happens regarding int setValue + + ASSERT_TRUE(param.getValue().getType() == "int"); + ASSERT_TRUE(param.sameValue(Value(10))); + + param.setValue(v); + param.setValue(Value("TEN")); + ASSERT_FALSE(param.sameValue(v)); // makes sure anti-aliasing happens regarding string setValue + + ASSERT_TRUE(param.getValue().getType() == "int"); + ASSERT_TRUE(param.sameValue(Value(10))); + + // make sure setValue and sameValue can handle int-string values + v = Value("10"); + ASSERT_TRUE(param.sameValue(v)); + param.setValue(v); + + ASSERT_TRUE(param.getValue().getType() == "int"); + ASSERT_TRUE(param.sameValue(Value(10))); + + // make sure setValue and sameValue can handle non-number non-dictionary strings + v = Value("TWO"); + // 'two' is not in our dictionary, so we will attempt to place it in there using a hash conversion + ASSERT_FALSE(param.sameValue(v)); + param.setValue(v); + + ASSERT_TRUE(param.getValue().getType() == "int"); + int hash = (int)boost::hash()("TWO"); + ASSERT_TRUE(param.sameValue(Value(hash))); + } + + TEST(DDEnumTest, streamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + map dict; + dict["ONE"] = 1; + dict["NEG-ONE"] = -1; + dict["TEN"] = 10; + DDEnum param1("param1",0,"param1",1,dict); + stringstream stream; + stream << param1; + ASSERT_EQ(param1.getName() + ":" + param1.getValue().toString(),stream.str()); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_int.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_int.cpp new file mode 100644 index 0000000000..f289e6fd31 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/test_dd_int.cpp @@ -0,0 +1,61 @@ +#include +#include +#include + +namespace ddynamic_reconfigure { + + /** + * @brief preliminary test which makes sure we can use the object. + */ + TEST(DDIntTest, constructorTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDInt param1("param1",0,"param1",1); + DDInt param2("",0,"",1,100); + DDInt param3("\000",(unsigned int)-1,"param1", 1, -100, -10); // NOLINT(bugprone-string-literal-with-embedded-nul) + } + + /** + * @brief a test making sure we can handle all API for handling the values of the param + */ + TEST(DDIntTest, valueTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDInt param("dd_param",0,"param1",1); + // we won't do any tests on getLevel or getName, as those are implicit. + Value v(1); + ASSERT_TRUE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(1.0); + ASSERT_FALSE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(2); + ASSERT_TRUE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + v = Value(2.0); + ASSERT_FALSE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + param.setValue(v); + v = Value(3); + ASSERT_FALSE(param.sameValue(v)); // makes sure anti-aliasing happens + + ASSERT_TRUE(param.getValue().getType() == "int"); + ASSERT_TRUE(param.sameValue(Value(2))); + } + + TEST(DDIntTest, streamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDInt param1("param1",0,"param1",1); + stringstream stream; + stream << param1; + ASSERT_EQ(param1.getName() + ":" + param1.getValue().toString(),stream.str()); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_string.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_string.cpp new file mode 100644 index 0000000000..fb34b680e6 --- /dev/null +++ b/ddynamic_reconfigure/test/dd_param/test_dd_string.cpp @@ -0,0 +1,61 @@ +#include +#include +#include + +namespace ddynamic_reconfigure { + + /** + * @brief preliminary test which makes sure we can use the object. + */ + TEST(DDStringTest, constructorTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDString param1("param1",0,"param1","Hello World"); + DDString param2("",0,"",""); + DDString param3("\000",0,"\000","\000"); // NOLINT(bugprone-string-literal-with-embedded-nul) + } + + /** + * @brief a test making sure we can handle all API for handling the values of the param + */ + TEST(DDStringTest, valueTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDString param("dd_param",0,"param1","1"); + // we won't do any tests on getLevel or getName, as those are implicit. + Value v("1"); + ASSERT_TRUE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value(1); + ASSERT_FALSE(param.sameType(v)); + ASSERT_TRUE(param.sameValue(v)); + + v = Value("2"); + ASSERT_TRUE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + v = Value(2); + ASSERT_FALSE(param.sameType(v)); + ASSERT_FALSE(param.sameValue(v)); + + param.setValue(v); + v = Value("3"); + ASSERT_FALSE(param.sameValue(v)); // makes sure anti-aliasing happens + + ASSERT_TRUE(param.getValue().getType() == "string"); + ASSERT_TRUE(param.sameValue(Value("2"))); + } + + TEST(DDStringTest, streamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + DDString param1("param1",0,"param1","Hello World"); + stringstream stream; + stream << param1; + ASSERT_EQ(param1.getName() + ":" + param1.getValue().toString(),stream.str()); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/dd_value.test b/ddynamic_reconfigure/test/dd_value.test new file mode 100644 index 0000000000..9e663c9e0b --- /dev/null +++ b/ddynamic_reconfigure/test/dd_value.test @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/ddynamic_reconfigure/test/ddynamic_reconfigure.test b/ddynamic_reconfigure/test/ddynamic_reconfigure.test new file mode 100644 index 0000000000..80263a5028 --- /dev/null +++ b/ddynamic_reconfigure/test/ddynamic_reconfigure.test @@ -0,0 +1,3 @@ + + + diff --git a/ddynamic_reconfigure/test/test-all.sh b/ddynamic_reconfigure/test/test-all.sh new file mode 100644 index 0000000000..b8496a3721 --- /dev/null +++ b/ddynamic_reconfigure/test/test-all.sh @@ -0,0 +1,18 @@ +#!/usr/bin/env bash +DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # gets the current dir +PKG=$(cd ${DIR}/..; printf %s "$PWD") # gets this ROS pkg path + +echo -e "package \e[1;4m"${PKG##*/}"\e[0m": +for test in $(find ${DIR} -name *.test -printf '%f\n'); do # finds the test file names + FAILS=$(rostest ${PKG##*/} ${test} | grep "[a-zA-Z_][a-zA-Z_0-9]*\]\[FAILURE") # executes the tests and stores in FAILS + SUITE="suite \e[1;4m"${test%.*}"\e[0m": + if [ -z "${FAILS}" ] ; then + echo -e " "${SUITE}"\e[32m[SUCCESS]\e[0m" # suite was successful + else + echo -e " "${SUITE}"\e[31m[FAILURE]\e[0m" # suite was unsuccessful + for msg in ${FAILS}; do + CASE=${msg%]*}; CASE=${CASE%]*}; CASE=${CASE##*/} # fetch case from GREP line + echo " "${CASE} + done + fi +done \ No newline at end of file diff --git a/ddynamic_reconfigure/test/test_dd_value.cpp b/ddynamic_reconfigure/test/test_dd_value.cpp new file mode 100644 index 0000000000..e118ea90b8 --- /dev/null +++ b/ddynamic_reconfigure/test/test_dd_value.cpp @@ -0,0 +1,188 @@ +#include +#include +#include + +namespace ddynamic_reconfigure { + + /** + * @brief a test making sure integer interpretations of value work all around. + */ + TEST(DDValueTest, intTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + // test init properly works + Value v(0); + ASSERT_EQ(0,v.toInt()); + ASSERT_EQ(v.getType(),"int"); + v = Value(-1); + ASSERT_EQ(-1,v.toInt()); + ASSERT_EQ(v.getType(),"int"); + v = Value(INT32_MAX); + ASSERT_EQ(INT32_MAX,v.toInt()); + ASSERT_EQ(v.getType(),"int"); + v = Value(INT32_MIN); + ASSERT_EQ(INT32_MIN,v.toInt()); + ASSERT_EQ(v.getType(),"int"); + // test that conversions from any value to this type properly works + int i = 1; + v = Value(i + 0.1); + ASSERT_EQ(i,v.toInt()); + ASSERT_EQ(v.getType(),"double"); + v = Value((bool)i); + ASSERT_EQ(i,v.toInt()); + ASSERT_EQ(v.getType(),"bool"); + stringstream str; str << i; + v = Value(str.str()); + ASSERT_EQ(i,v.toInt()); + ASSERT_EQ(v.getType(),"string"); + } + + /** + * @brief a test making sure double interpretations of value work all around. + */ + TEST(DDValueTest, doubleTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + // test init properly works + Value v(0.0); + ASSERT_EQ(0.0,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + v = Value(-1.0); + ASSERT_EQ(-1.0,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + v = Value((double)INT32_MAX); + ASSERT_EQ((double)INT32_MAX,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + v = Value((double)INT32_MIN); + ASSERT_EQ((double)INT32_MIN,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + v = Value(DBL_MAX); + ASSERT_EQ(DBL_MAX,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + v = Value(DBL_MIN); + ASSERT_EQ(DBL_MIN,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + v = Value(-DBL_MAX); + ASSERT_EQ(-DBL_MAX,v.toDouble()); + ASSERT_EQ(v.getType(),"double"); + // test that conversions from any value to this type properly works + double f = 1.0; + v = Value((int)f); + ASSERT_EQ(f,v.toDouble()); + ASSERT_EQ(v.getType(),"int"); + v = Value((int)(f + 0.1)); + ASSERT_NE(f + 0.1,v.toDouble()); + ASSERT_EQ(v.getType(),"int"); + v = Value((bool)f); + ASSERT_EQ(f,v.toDouble()); + ASSERT_EQ(v.getType(),"bool"); + stringstream str; str << f; + v = Value(str.str()); + ASSERT_EQ(f,v.toDouble()); + ASSERT_EQ(v.getType(),"string"); + str << ".000"; + v = Value(str.str()); + ASSERT_EQ(f,v.toDouble()); + ASSERT_EQ(v.getType(),"string"); + } + + /** + * @brief a test making sure boolean interpretations of value work all around. + */ + TEST(DDValueTest, boolTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) +// test init properly works + Value v(false); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"bool"); + v = Value(true); + ASSERT_EQ(true,v.toBool()); + ASSERT_EQ(v.getType(),"bool"); + // test that conversions from any value to this type properly works + v = Value(1); + ASSERT_EQ(true,v.toBool()); + ASSERT_EQ(v.getType(),"int"); + v = Value(0); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"int"); + v = Value(2); + ASSERT_EQ(true,v.toBool()); + ASSERT_EQ(v.getType(),"int"); + v = Value(-1); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"int"); + v = Value(1.0); + ASSERT_EQ(true,v.toBool()); + ASSERT_EQ(v.getType(),"double"); + v = Value(0.0); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"double"); + v = Value(2.0); + ASSERT_EQ(true,v.toBool()); + ASSERT_EQ(v.getType(),"double"); + v = Value(-1.0); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"double"); + v = Value("true"); + ASSERT_EQ(true,v.toBool()); + ASSERT_EQ(v.getType(),"string"); + v = Value("false"); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"string"); + v = Value("not a bool"); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"string"); + v = Value(""); + ASSERT_EQ(false,v.toBool()); + ASSERT_EQ(v.getType(),"string"); + } + + /** + * @brief a test making sure string interpretations of value work all around. + */ + TEST(DDValueTest, stringTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + // test init properly works + Value v("normal"); + ASSERT_EQ("normal",v.toString()); + ASSERT_EQ(v.getType(),"string"); + v = Value("\000"); + ASSERT_EQ("\000",v.toString()); + ASSERT_EQ(v.getType(),"string"); + v = Value(""); + ASSERT_EQ("",v.toString()); + ASSERT_EQ(v.getType(),"string"); + string s("How long can I make this string go on for? Well, You would be surprised! I can make it really long, but I won't."); + v = Value(s); + ASSERT_EQ(s,v.toString()); + ASSERT_EQ(v.getType(),"string"); + // test that conversions from any value to this type properly works + v = Value(1); + ASSERT_EQ("1",v.toString()); + ASSERT_EQ(v.getType(),"int"); + v = Value(-1); + ASSERT_EQ("-1",v.toString()); + ASSERT_EQ(v.getType(),"int"); + v = Value(1.); + ASSERT_EQ("1",v.toString()); + ASSERT_EQ(v.getType(),"double"); + v = Value(-1.); + ASSERT_EQ("-1",v.toString()); + ASSERT_EQ(v.getType(),"double"); + v = Value(1.1); + ASSERT_EQ("1.1",v.toString()); + ASSERT_EQ(v.getType(),"double"); + v = Value(-1.1); + ASSERT_EQ("-1.1",v.toString()); + ASSERT_EQ(v.getType(),"double"); + v = Value(true); + ASSERT_EQ("true",v.toString()); + ASSERT_EQ(v.getType(),"bool"); + v = Value(false); + ASSERT_EQ("false",v.toString()); + ASSERT_EQ(v.getType(),"bool"); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp b/ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp new file mode 100644 index 0000000000..14359c3876 --- /dev/null +++ b/ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp @@ -0,0 +1,539 @@ +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunknown-pragmas" +#pragma ide diagnostic ignored "OCDFAInspection" +#include +#include +#include +#include +#include +using namespace std; +namespace ddynamic_reconfigure { + + TEST(DDynamicReconfigureTest, mapTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + + DDParam* ptr = new DDBool("exists",0,"",true); + DDPtr dd_ptr = DDPtr(ptr); + + dd.add(dd_ptr); + ASSERT_NE(DDPtr(),dd.at("exists")); + + dd.remove(ptr); + ASSERT_EQ(DDPtr(),dd.at("exists")); + + dd.remove(dd_ptr); + ASSERT_EQ(DDPtr(),dd.at("exists")); + } + + void basicCallback(const DDMap& map, int, bool *flag) { + *flag = true; + } + + /** + * @brief preliminary test which makes sure we can use callbacks + */ + TEST(DDynamicReconfigureTest, basicCallbackTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + + bool flag = false; + DDFunc callback = bind(&basicCallback,_1,_2,&flag); + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_TRUE(flag); + } + + void intCallback(const DDMap& map, int, int *flag) { + ASSERT_EQ("int",at(map,"int_param")->getValue().getType()); + *flag = at(map,"int_param")->getValue().toInt(); + } + + /** + * @brief tests that int parameters are registered properly + */ + TEST(DDynamicReconfigureTest, intTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + int flag = 0; + DDFunc callback = bind(&intCallback,_1,_2,&flag); + + DDynamicReconfigure dd(nh); + dd.add(new DDInt("int_param", 0,"int_param", 0)); + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + dynamic_reconfigure::IntParameter int_param; + int_param.name = "int_param"; + + int_param.value = (int)random(); + srv.request.config.ints.push_back(int_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(int_param.value, flag); + + int_param.value = INT32_MAX; + srv.request.config.ints.push_back(int_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(int_param.value, flag); + + int_param.value = INT32_MIN; + srv.request.config.ints.push_back(int_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(int_param.value, flag); + } + + void doubleCallback(const DDMap& map, int, double *flag) { + ASSERT_EQ("double",at(map,"double_param")->getValue().getType()); + *flag = at(map,"double_param")->getValue().toDouble(); + } + + /** + * @brief tests that double parameters are registered properly + */ + TEST(DDynamicReconfigureTest, doubleTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + double flag = 0; + DDFunc callback = bind(&doubleCallback,_1,_2,&flag); + + DDynamicReconfigure dd(nh); + dd.add(new DDDouble("double_param", 0,"double_param", 0)); + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + dynamic_reconfigure::DoubleParameter double_param; + double_param.name = "double_param"; + + double_param.value = (double)random(); + srv.request.config.doubles.push_back(double_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(double_param.value, flag); + + double_param.value = DBL_MAX; + srv.request.config.doubles.push_back(double_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(double_param.value, flag); + + double_param.value = DBL_MIN; + srv.request.config.doubles.push_back(double_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(double_param.value, flag); + + double_param.value = -DBL_MAX; + srv.request.config.doubles.push_back(double_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(double_param.value, flag); + + double_param.value = -DBL_MIN; + srv.request.config.doubles.push_back(double_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(double_param.value, flag); + } + + void boolCallback(const DDMap& map, int, bool *flag) { + ASSERT_EQ("bool",at(map,"bool_param")->getValue().getType()); + *flag = at(map,"bool_param")->getValue().toBool(); + } + + /** + * @brief tests that boolean parameters are registered properly + */ + TEST(DDynamicReconfigureTest, boolTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + bool flag = true; + DDFunc callback = bind(&boolCallback,_1,_2,&flag); + + DDynamicReconfigure dd(nh); + dd.add(new DDBool("bool_param", 0,"bool_param", false)); + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "bool_param"; + + bool_param.value = (unsigned char)false; + srv.request.config.bools.push_back(bool_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ((bool)bool_param.value, flag); + + flag = false; + + bool_param.value = (unsigned char)true; + srv.request.config.bools.push_back(bool_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ((bool)bool_param.value, flag); + } + + void strCallback(const DDMap& map, int, string *flag) { + ASSERT_EQ("string",at(map,"string_param")->getValue().getType()); + *flag = at(map,"string_param")->getValue().toString(); + } + + /** + * @brief tests that string parameters are registered properly + */ + TEST(DDynamicReconfigureTest, stringTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + string flag = "YOU SHOULDN'T RECEIVE THIS"; + DDFunc callback = bind(&strCallback,_1,_2,&flag); + + DDynamicReconfigure dd(nh); + dd.add(new DDString("string_param", 0,"string_param", "")); + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + dynamic_reconfigure::StrParameter string_param; + string_param.name = "string_param"; + + string_param.value = string("\000"); // NOLINT(bugprone-string-literal-with-embedded-nul) + srv.request.config.strs.push_back(string_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(string_param.value, flag); + + string_param.value = ""; + srv.request.config.strs.push_back(string_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(string_param.value, flag); + + string_param.value = "Hello World"; + srv.request.config.strs.push_back(string_param); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(string_param.value, flag); + } + + void enumCallback(const DDMap& map, int, int *flag) { + ASSERT_EQ("int",at(map,"enum_param")->getValue().getType()); + *flag = at(map,"enum_param")->getValue().toInt(); + } + + /** + * @brief tests that int-enum parameters are registered properly + */ + TEST(DDynamicReconfigureTest, enumTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + int flag = 0; + DDFunc callback = bind(&enumCallback,_1,_2,&flag); + + map dict; + dict["ONE"] = 1; + dict["NEG-ONE"] = -1; + dict["TEN"] = 10; + + DDynamicReconfigure dd(nh); + dd.add(new DDEnum("enum_param", 0,"enum_param", "ONE", dict)); + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + + dynamic_reconfigure::IntParameter int_enum; + int_enum.name = "enum_param"; + + int_enum.value = 1; + srv.request.config.ints.push_back(int_enum); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(int_enum.value, flag); + + int_enum.value = 10; + srv.request.config.ints.push_back(int_enum); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(int_enum.value, flag); + + int_enum.value = -1; + srv.request.config.ints.push_back(int_enum); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(int_enum.value, flag); + + srv.request.config.ints.clear(); + dynamic_reconfigure::StrParameter str_enum; + str_enum.name = "enum_param"; + + str_enum.value = "ONE"; + srv.request.config.strs.push_back(str_enum); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(dict[str_enum.value], flag); + + str_enum.value = "TEN"; + srv.request.config.strs.push_back(str_enum); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(dict[str_enum.value], flag); + + str_enum.value = "NEG-ONE"; + srv.request.config.strs.push_back(str_enum); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(dict[str_enum.value], flag); + } + + void complexCallback(const DDMap& map, int level) { + ASSERT_EQ(0, level); + ASSERT_EQ(1, at(map,"int_param")->getValue().toInt()); + ASSERT_EQ(0.6, at(map,"double_param")->getValue().toDouble()); + ASSERT_EQ("Goodbye Home", at(map,"str_param")->getValue().toString()); + ASSERT_EQ(false, at(map,"bool_param")->getValue().toBool()); + ASSERT_EQ(3, at(map,"enum_param")->getValue().toInt()); + } + + /** + * @brief tests that ddynamic can handle complex callbacks and param lists. + */ + TEST(DDynamicReconfigureTest, callbackTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + dd.add(new DDInt("int_param", 0, "An Integer parameter", 0, 50, 100)); + dd.add(new DDDouble("double_param", 0, "A double parameter", .5, 0, 1)); + dd.add(new DDString("str_param", 0, "A string parameter", "Hello World")); + dd.add(new DDBool("bool_param", 0, "A Boolean parameter", true)); + map dict; { + dict["Small"] = 0; + dict["Medium"] = 1; + dict["Large"] = 2; + dict["ExtraLarge"] = 3; + } + dd.add(new DDEnum("enum_param", 0, "A size parameter which is edited via an enum", 0, dict)); + dd.start(complexCallback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + + dynamic_reconfigure::IntParameter int_param; + int_param.name = "int_param"; + int_param.value = 1; + srv.request.config.ints.push_back(int_param); + + dynamic_reconfigure::DoubleParameter double_param; + double_param.name = "double_param"; + double_param.value = 0.6; + srv.request.config.doubles.push_back(double_param); + + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "bool_param"; + bool_param.value = (unsigned char) false; + srv.request.config.bools.push_back(bool_param); + + dynamic_reconfigure::StrParameter string_param; + string_param.name = "str_param"; + string_param.value = "Goodbye Home"; + srv.request.config.strs.push_back(string_param); + + dynamic_reconfigure::StrParameter enum_param; + enum_param.name = "enum_param"; + enum_param.value = "ExtraLarge"; + srv.request.config.strs.push_back(enum_param); + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + + bool flag = false; + DDFunc callback = bind(&basicCallback,_1,_2,&flag); + dd.setCallback(callback); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_TRUE(flag); + + flag = false; + dd.clearCallback(); + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_FALSE(flag); + } + + class InternalClass { + public: + inline void internalCallback(const DDMap& map, int level) {} + }; + + /** + * @brief tests that ddynamic can take member methods as callbacks + */ + TEST(DDynamicReconfigureTest, memberCallbackTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + + dd.start(&InternalClass::internalCallback,new InternalClass); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + } + + void levelCallback(const DDMap&, int level, int *flag) { + *flag = level; + } + + /** + * @brief tests that ddynamic properly handles param change levels + */ + TEST(DDynamicReconfigureTest, levelTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + int flag = 0; + DDFunc callback = bind(&levelCallback, _1, _2, &flag); + + DDynamicReconfigure dd(nh); + int top = (int) random() % 5 + 5; + unsigned int or_sum = 0, next; + for (int i = 1; i < top; i++) { + next = (unsigned int) random(); + or_sum |= next; + dd.add(new DDInt((format("param_%d") % i).str(), next,"level_param", 0)); + } + dd.start(callback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + dynamic_reconfigure::IntParameter int_param; + for (int i = 1; i < top; i++) { + int_param.name = (format("param_%d") % i).str(); + int_param.value = 1; + srv.request.config.ints.push_back(int_param); + } + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(or_sum, flag); + + dd.add(new DDInt("unchanged_param", 1,"unchanged_param", 0)); //u-int max means everything is 1, so the result must also be that. + dynamic_reconfigure::IntParameter unchanged_param; + unchanged_param.name = "unchanged_param"; + unchanged_param.value = 1; + srv.request.config.ints.push_back(unchanged_param); + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(1, flag); + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + ASSERT_EQ(0, flag); + } + + void badCallback(const DDMap&, int) { + std::exception e; + throw e; // NOLINT(cert-err09-cpp,cert-err61-cpp,misc-throw-by-value-catch-by-reference) + } + + /** + * @brief tests that ddynamic can properly handle exceptions + */ + TEST(DDynamicReconfigureTest, badCallbackTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + dd.start(badCallback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + // this is the best way to see exceptions doesn't make the whole thing tumble + } + + void missingCallback(const DDMap& map, int) { + ASSERT_EQ(map.end(),map.find("int_param")); + ASSERT_EQ(map.end(),map.find("double_param")); + ASSERT_EQ(map.end(),map.find("bool_param")); + ASSERT_EQ(map.end(),map.find("str_param")); + ASSERT_EQ(map.end(),map.find("enum_param")); + } + + /** + * @brief tests that ddynamic can properly handle missing/unregistered parameters + */ + TEST(DDynamicReconfigureTest, unknownParamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + dd.start(missingCallback); + + ros::AsyncSpinner spinner(1); + spinner.start(); + + dynamic_reconfigure::Reconfigure srv; + + dynamic_reconfigure::IntParameter int_param; + int_param.name = "int_param"; + int_param.value = 1; + srv.request.config.ints.push_back(int_param); + + dynamic_reconfigure::DoubleParameter double_param; + double_param.name = "double_param"; + double_param.value = 0.6; + srv.request.config.doubles.push_back(double_param); + + dynamic_reconfigure::BoolParameter bool_param; + bool_param.name = "bool_param"; + bool_param.value = (unsigned char) false; + srv.request.config.bools.push_back(bool_param); + + dynamic_reconfigure::StrParameter string_param; + string_param.name = "str_param"; + string_param.value = "Goodbye Home"; + srv.request.config.strs.push_back(string_param); + + dynamic_reconfigure::StrParameter enum_param; + enum_param.name = "enum_param"; + enum_param.value = "ExtraLarge"; + srv.request.config.strs.push_back(enum_param); + + ASSERT_TRUE(ros::service::call(nh.getNamespace() + "/set_parameters", srv)); + } + + /** + * @brief tests that ddynamic's stream operator properly works + */ + TEST(DDynamicReconfigureTest, streamTest) { // NOLINT(cert-err58-cpp,modernize-use-equals-delete) + ros::NodeHandle nh("~"); + DDynamicReconfigure dd(nh); // gets our main class running + DDInt dd_int("int_param", 0, "An Integer parameter", 0, 50, 100); + DDDouble dd_double("double_param", 0, "A double parameter", .5, 0, 1); + DDString dd_string("str_param", 0, "A string parameter", "Hello World"); + DDBool dd_bool("bool_param", 0, "A Boolean parameter", true); + dd.add(new DDInt(dd_int)); + dd.add(new DDDouble(dd_double)); + dd.add(new DDString(dd_string)); + dd.add(new DDBool(dd_bool)); + map dict; { + dict["Small"] = 0; + dict["Medium"] = 1; + dict["Large"] = 2; + dict["ExtraLarge"] = 3; + } + DDEnum dd_enum("enum_param", 0, "A size parameter which is edited via an enum", 0, dict); + dd.add(new DDEnum(dd_enum)); + + stringstream stream, explicit_stream; + stream << dd; + + explicit_stream << "{" << dd_bool << "," << dd_double << "," << dd_enum << "," << dd_int << "," << dd_string << "}"; + ASSERT_EQ(explicit_stream.str(),stream.str()); + } +} + + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "ddynamic_reconfigure_test"); + + srand((unsigned int)random()); + + return RUN_ALL_TESTS(); +} +#pragma clang diagnostic pop \ No newline at end of file diff --git a/ddynamic_reconfigure/uml/class_struct.puml b/ddynamic_reconfigure/uml/class_struct.puml new file mode 100644 index 0000000000..6b2f276e72 --- /dev/null +++ b/ddynamic_reconfigure/uml/class_struct.puml @@ -0,0 +1,104 @@ +@startuml +class DDynamicReconfigure { + #nh_ : NodeHandle + #params_ : DDMap + #desc_pub_ : Publisher + #update_pub_ : Publisher + -callback_ : shared_ptr + -set_service_ : ServiceServer + __ + +add() : void + +remove(): void + +setCallback() : void + +clearCallback() : void + +start() : void + ..getters.. + +get() : Value + +at() : DDPtr + +operator<<() : ostream& + ..internal.. + #makeDescription() : void + #makeConfig() : void + #{static} internalCallback() : bool + -reassign() <> : int + -getUpdates() : int +} +note right: DDPtr := shared_ptr\nDDMap := map\nDDFunc := function +class DDValue { + -int_val_ : int + -double_val_ : double + -bool_val_ : bool + -str_val_ : string + -type_ : string + +getType() : string + +toInt() : int + +toDouble() : double + +toBool() : bool + +toString() : string +} +package dd_param <> { + interface DDParam { + __ + +{abstract} getName() : string + +{abstract} getLevel() : u_int + +{abstract} getValue() : DDValue + +operator<<() : ostream& + ..setters.. + +{abstract} setValue() : void + ..testers.. + +{abstract} sameType() : bool + +{abstract} sameValue() : bool + ..internal.. + +{abstract} prepGroup() : void + +{abstract} prepConfig() : void + +{abstract} prepConfigDescription() : void + } + class DDInt { + #level_ : u_int + #name_ : string + #desc_ : string + #def_ : int + #val_ : int + #max_ : int + #min_ : int + } + class DDDouble { + #level_ : u_int + #name_ : string + #desc_ : string + #def_ : double + #val_ : double + #max_ : double + #min_ : double + } + class DDBool { + #level_ : u_int + #name_ : string + #desc_ : string + #def_ : bool + #val_ : bool + } + class DDString { + #level_ : u_int + #name_ : string + #desc_ : string + #def_ : string + #val_ : string + } + class DDEnum { + #dict_ : const map > + #enum_description_ : string + -lookup() : int + -makeEditMethod() : string + -makeConst() : string + } +} + +DDParam .> DDValue +DDInt .u.|> DDParam +DDDouble .u.|> DDParam +DDBool .u.|> DDParam +DDString .u.|> DDParam +DDEnum -u-|> DDInt +DDynamicReconfigure "0..*" --o DDParam +@enduml \ No newline at end of file diff --git a/ddynamic_reconfigure/uml/file_struct.puml b/ddynamic_reconfigure/uml/file_struct.puml new file mode 100644 index 0000000000..610805eade --- /dev/null +++ b/ddynamic_reconfigure/uml/file_struct.puml @@ -0,0 +1,38 @@ +@startuml +package ddynamic_reconfigure as ddynamic_reconfigure_pkg { + file ddynamic_reconfigure { + component at #Yellow + component get #Yellow + component DDynamicReconfigure + + DDynamicReconfigure .u.> at + DDynamicReconfigure .u.> get + } + interface DDParam + folder param { + component DDInt + component DDDouble + component DDBool + component DDString + component DDEnum + file dd_all_params + + DDInt -u-> DDParam + DDDouble -u-> DDParam + DDBool -u-> DDParam + DDString -u-> DDParam + DDEnum -u-> DDInt + dd_all_params -u-> DDInt + dd_all_params -u--> DDDouble + dd_all_params -u--> DDBool + dd_all_params -u--> DDString + dd_all_params -u-> DDEnum + } + component DDValue + DDynamicReconfigure --> DDParam + DDParam -> DDValue +} +component "ddynamic\nserver" as server +server -> dd_all_params +server -> ddynamic_reconfigure +@enduml \ No newline at end of file diff --git a/ddynamic_reconfigure/uml/ros_struct.puml b/ddynamic_reconfigure/uml/ros_struct.puml new file mode 100644 index 0000000000..01916dd04a --- /dev/null +++ b/ddynamic_reconfigure/uml/ros_struct.puml @@ -0,0 +1,22 @@ +@startuml +component DDynamicReconfigure as ddr { + rectangle update_pub_ <> as uppub + rectangle desc_pub_ <> as descpub + rectangle set_service_ <> as set + set -[hidden]->descpub + descpub -[hidden]->uppub +} +component client { + cloud "/set_parameters" as pset +} +component dynamic_reconfigure\ncommandline { + cloud "/parameter_descriptions" as pdesc + cloud "/parameter_updates" as pup + pdesc -[hidden]->pup +} + +uppub -> pup +descpub -> pdesc +set -> pset +pset -l-> set +@enduml \ No newline at end of file diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d53ea4154d..140fcf62c4 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport tf - dynamic_reconfigure + ddynamic_reconfigure diagnostic_updater ) @@ -65,12 +65,12 @@ include_directories( ) # Generate dynamic reconfigure options from .cfg files -generate_dynamic_reconfigure_options( - cfg/base_d400_params.cfg - cfg/sr300_params.cfg - cfg/rs415_params.cfg - cfg/rs435_params.cfg -) +#generate_dynamic_reconfigure_options( +# cfg/base_d400_params.cfg +# cfg/sr300_params.cfg +# cfg/rs415_params.cfg +# cfg/rs435_params.cfg +#) # RealSense ROS Node catkin_package( @@ -79,21 +79,15 @@ catkin_package( nodelet cv_bridge image_transport - dynamic_reconfigure + ddynamic_reconfigure ) add_library(${PROJECT_NAME} include/constants.h include/realsense_node_factory.h include/base_realsense_node.h - include/rs415_node.h - include/rs435_node.h - include/sr300_node.h src/realsense_node_factory.cpp src/base_realsense_node.cpp - src/rs415_node.cpp - src/rs435_node.cpp - src/sr300_node.cpp ) add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) diff --git a/realsense2_camera/cfg/base_d400_params.cfg b/realsense2_camera/cfg/base_d400_params.cfg deleted file mode 100755 index cd566169f1..0000000000 --- a/realsense2_camera/cfg/base_d400_params.cfg +++ /dev/null @@ -1,13 +0,0 @@ -#!/usr/bin/env python - -PACKAGE="realsense2_camera" - -from dynamic_reconfigure.parameter_generator_catkin import * - -import base_d400_params - -gen = ParameterGenerator() - -base_d400_params.add_base_params(gen, "base_") - -exit(gen.generate(PACKAGE, "realsense2_camera", "base_d400_params")) \ No newline at end of file diff --git a/realsense2_camera/cfg/base_d400_params.py b/realsense2_camera/cfg/base_d400_params.py deleted file mode 100755 index c617b79523..0000000000 --- a/realsense2_camera/cfg/base_d400_params.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - - -from dynamic_reconfigure.parameter_generator_catkin import * - -def add_base_params(gen, prefix): - # Level should match the order of appearance in base_depth_param of base_realsense_node.h - # Name Type Level Description Default Min Max - gen.add(str(prefix) + "depth_gain", int_t, 1, "Gain", 16, 16, 248) - gen.add(str(prefix) + "depth_enable_auto_exposure", bool_t, 2, "Enable Auto Exposure", True) - preset_enum = gen.enum([gen.const("Custom", int_t, 0, "Custom"), - gen.const("Default", int_t, 1, "Default Preset"), - gen.const("Hand", int_t, 2, "Hand Gesture"), - gen.const("HighAccuracy", int_t, 3, "High Accuracy"), - gen.const("HighDensity", int_t, 4, "High Density"), - gen.const("MediumDensity", int_t, 5, "Medium Density")], "D400 Visual Presets") - gen.add(str(prefix) + "depth_visual_preset", int_t, 3, "D400 Visual Presets", 0, 0, 5, edit_method=preset_enum) - gen.add(str(prefix) + "depth_frames_queue_size", int_t, 4, "Frames Queue Size", 16, 0, 32) - gen.add(str(prefix) + "depth_error_polling_enabled", bool_t, 5, "Error Polling Enabled", False) - gen.add(str(prefix) + "depth_output_trigger_enabled", bool_t, 6, "Output Trigger Enabled", False) - gen.add(str(prefix) + "depth_units", double_t, 7, "Depth Units", 0.001, 0.001, 0.001) - gen.add(str(prefix) + "sensors_enabled", bool_t, 8, "Sensors Enabled", True) - gen.add(str(prefix) + "JSON_file_path", str_t, 9, "JSON_file_path", "") \ No newline at end of file diff --git a/realsense2_camera/cfg/rs415_params.cfg b/realsense2_camera/cfg/rs415_params.cfg deleted file mode 100755 index 9caf103fe9..0000000000 --- a/realsense2_camera/cfg/rs415_params.cfg +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -PACKAGE="realsense2_camera" - -from dynamic_reconfigure.parameter_generator_catkin import * - -import base_d400_params - -gen = ParameterGenerator() - -base_d400_params.add_base_params(gen, "rs415_") - -# Name Type Level Description Default Min Max -gen.add("rs415_depth_enable_auto_white_balance", bool_t, 9, "Enable Auto White Balance", False) -gen.add("rs415_depth_exposure", int_t, 10, "Exposure", 1650, 1, 8300) # The exposure step is 20 by definition but the param step is 1 by default so we divided all values by 20 and multiply it back in SW -gen.add("rs415_depth_laser_power", double_t, 11, "Laser Power", 12.5, 0, 12) # Multiple value by 30 -emitter_enabled_enum = gen.enum([gen.const("Off", int_t, 0, "Off"), - gen.const("On", int_t, 1, "On"), - gen.const("Auto", int_t, 2, "Auto")], "Depth Emitter") -gen.add("rs415_depth_emitter_enabled", int_t, 12, "Depth Emitter Enabled", 1, 0, 2, edit_method=emitter_enabled_enum) - -gen.add("rs415_color_backlight_compensation", bool_t, 13, "Backlight Compensation", False) -gen.add("rs415_color_brightness", int_t, 14, "Brightness", 0, -64, 64) -gen.add("rs415_color_contrast", int_t, 15, "Contrast", 50, 0, 100) -gen.add("rs415_color_exposure", int_t, 16, "Exposure", 166, 41, 10000) -gen.add("rs415_color_gain", int_t, 17, "Gain", 64, 0, 128) -gen.add("rs415_color_gamma", int_t, 18, "Gamma", 300, 100, 500) -gen.add("rs415_color_hue", int_t, 19, "Hue", 0, -180, 180) -gen.add("rs415_color_saturation", int_t, 20, "Saturation", 64, 0, 100) -gen.add("rs415_color_sharpness", int_t, 21, "Sharpness", 50, 0, 100) -gen.add("rs415_color_white_balance", int_t, 22, "White Balance", 460, 280, 650) # Multiple value by 10 -gen.add("rs415_color_enable_auto_exposure", bool_t, 23, "Enable Auto Exposure", True) -gen.add("rs415_color_enable_auto_white_balance", bool_t, 24, "Enable Auto White Balance", True) -gen.add("rs415_color_frames_queue_size", int_t, 25, "Frames Queue Size", 16, 0, 32) -power_line_frequency_enum = gen.enum([gen.const("Disable", int_t, 0, "Disable"), - gen.const("50Hz", int_t, 1, "50Hz"), - gen.const("60Hz", int_t, 2, "60Hz"), - gen.const("AutoFrequency", int_t, 3, "Auto")], "Power Line Frequency") -gen.add("rs415_color_power_line_frequency", int_t, 26, "Power Line Frequency", 3, 0, 3, edit_method=power_line_frequency_enum) -gen.add("rs415_color_auto_exposure_priority", bool_t, 27, "Auto Exposure Priority", False) - -exit(gen.generate(PACKAGE, "realsense2_camera", "rs415_params")) - diff --git a/realsense2_camera/cfg/rs435_params.cfg b/realsense2_camera/cfg/rs435_params.cfg deleted file mode 100755 index c2b8534319..0000000000 --- a/realsense2_camera/cfg/rs435_params.cfg +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -PACKAGE="realsense2_camera" - -from dynamic_reconfigure.parameter_generator_catkin import * - -import base_d400_params - -gen = ParameterGenerator() - -# Prefix parameters that are common to all camera types -base_d400_params.add_base_params(gen, "rs435_") - -# Name Type Level Description Default Min Max -gen.add("rs435_depth_exposure", int_t, 9, "Exposure", 425, 1, 8300) # The exposure step is 20 by definition but the param step is 1 by default so we divided all values by 20 and multiply it back in SW -gen.add("rs435_depth_laser_power", double_t, 10, "Laser Power", 12.5, 0, 12) # Multiple value by 30 -emitter_enabled_enum = gen.enum([gen.const("Off", int_t, 0, "Off"), - gen.const("On", int_t, 1, "On"), - gen.const("Auto", int_t, 2, "Auto")], "Depth Emitter") -gen.add("rs435_depth_emitter_enabled", int_t, 11, "Depth Emitter Enabled", 1, 0, 2, edit_method=emitter_enabled_enum) - -gen.add("rs435_color_backlight_compensation", bool_t, 12, "Backlight Compensation", False) -gen.add("rs435_color_brightness", int_t, 13, "Brightness", 0, -64, 64) -gen.add("rs435_color_contrast", int_t, 14, "Contrast", 50, 0, 100) -gen.add("rs435_color_exposure", int_t, 15, "Exposure", 166, 41, 10000) -gen.add("rs435_color_gain", int_t, 16, "Gain", 64, 0, 128) -gen.add("rs435_color_gamma", int_t, 17, "Gamma", 300, 100, 500) -gen.add("rs435_color_hue", int_t, 18, "Hue", 0, -180, 180) -gen.add("rs435_color_saturation", int_t, 19, "Saturation", 64, 0, 100) -gen.add("rs435_color_sharpness", int_t, 20, "Sharpness", 50, 0, 100) -gen.add("rs435_color_white_balance", int_t, 21, "White Balance", 460, 280, 650) # Multiple value by 10 -gen.add("rs435_color_enable_auto_exposure", bool_t, 22, "Enable Auto Exposure", True) -gen.add("rs435_color_frames_queue_size", int_t, 23, "Frames Queue Size", 16, 0, 32) -power_line_frequency_enum = gen.enum([gen.const("Disable", int_t, 0, "Disable"), - gen.const("50Hz", int_t, 1, "50Hz"), - gen.const("60Hz", int_t, 2, "60Hz"), - gen.const("AutoFrequency", int_t, 3, "Auto Frequency")], "Power Line Frequency") -gen.add("rs435_color_power_line_frequency", int_t, 24, "Power Line Frequency", 3, 0, 3, edit_method=power_line_frequency_enum) -gen.add("rs435_color_auto_exposure_priority", bool_t, 25, "Auto Exposure Priority", False) - - -exit(gen.generate(PACKAGE, "realsense2_camera", "rs435_params")) \ No newline at end of file diff --git a/realsense2_camera/cfg/sr300_params.cfg b/realsense2_camera/cfg/sr300_params.cfg deleted file mode 100755 index 57132b6c6f..0000000000 --- a/realsense2_camera/cfg/sr300_params.cfg +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python - -PACKAGE="realsense2_camera" - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -# Name Type Level Description Default Min Max -gen.add("sr300_color_backlight_compensation", int_t, 1, "Backlight Compensation", 0, 0, 4) -gen.add("sr300_color_brightness", int_t, 2, "Brightness", 0, -64, 64) -gen.add("sr300_color_contrast", int_t, 3, "Contrast", 50, 0, 100) -gen.add("sr300_color_gain", int_t, 4, "Gain", 64, 0, 128) -gen.add("sr300_color_gamma", int_t, 5, "Gamma", 300, 100, 500) -gen.add("sr300_color_hue", int_t, 6, "Hue", 0, -180, 180) -gen.add("sr300_color_saturation", int_t, 7, "Saturation", 64, 0, 100) -gen.add("sr300_color_sharpness", int_t, 8, "Sharpness", 50, 0, 100) - -# Must be set only if color_enable_auto_white_balance is disabled -gen.add("sr300_color_white_balance", int_t, 9, "White Balance", 4600, 2800, 6500) - -gen.add("sr300_color_enable_auto_white_balance", bool_t, 10, "Enable Auto White Balance", True) - -# Must be set only if color_enable_auto_exposure is disabled -gen.add("sr300_color_exposure", int_t, 11, "Exposure", 156, 39, 10000) - -gen.add("sr300_color_enable_auto_exposure", bool_t, 12, "Enable Auto Exposure", True) - -preset_enum = gen.enum([gen.const("ShortRange", int_t, 0, "Short Range"), - gen.const("LongRange", int_t, 1, "Long Range"), - gen.const("BackgroundSegmentation", int_t, 2, "Background Segmentation"), - gen.const("GestureRecognition", int_t, 3, "Gesture Recognition"), - gen.const("ObjectScanning", int_t, 4, "Object Scanning"), - gen.const("FaceAnalytics", int_t, 5, "Face Analytics"), - gen.const("FaceLogin", int_t, 6, "Face Login"), - gen.const("GrCursor", int_t, 7, "GR Cursor"), - gen.const("Default", int_t, 8, "Default"), - gen.const("MidRange", int_t, 9, "Mid Range"), - gen.const("IrOnly", int_t, 10, "IR Only")], "SR300 Visual Presets") - -gen.add("sr300_depth_visual_preset", int_t, 13, "SR300 Visual Presets", 8, 0, 10, edit_method=preset_enum) -gen.add("sr300_depth_laser_power", int_t, 14, "Laser Power", 16, 0, 16) -gen.add("sr300_depth_accuracy", int_t, 15, "Accuracy", 1, 1, 3) -gen.add("sr300_depth_motion_range", int_t, 16, "Motion Range", 9, 0, 220) -gen.add("sr300_depth_filter_option", int_t, 17, "Filter Option", 5, 0, 7) -gen.add("sr300_depth_confidence_threshold", int_t, 18, "Confidence Threshold", 3, 0, 15) -gen.add("sr300_depth_frames_queue_size", int_t, 19, "Frames Queue Size", 16, 0, 32) -gen.add("sr300_depth_units", double_t, 20, "Depth Units", 0.000124987, 0.000124987, 0.000124987) - - -# TODO -# Following are the Auto range options -#gen.add("sr300_auto_range_enable_motion_versus_range", int_t, 13, "Enable Motion Versus Range", 1, 0, 2) -#gen.add("sr300_auto_range_enable_laser", int_t, 14, "Enable Laser", 1, 0, 1) - -# Must be set only if sr300_auto_range_enable_motion_versus_range is set to 1 -#gen.add("sr300_auto_range_min_motion_versus_range", int_t, 15, "Min Motion Versus Range", 180, -32767, 32767) -#gen.add("sr300_auto_range_max_motion_versus_range", int_t, 16, "Max Motion Versus Range", 605, -32767, 32767) -#gen.add("sr300_auto_range_start_motion_versus_range", int_t, 17, "Start Motion Versus Range", 303, -32767, 32767) - -# Must be set only if sr300_auto_range_enable_laser is enabled -#gen.add("sr300_auto_range_min_laser", int_t, 18, "Min Laser", 2, -32767, 32767) -#gen.add("sr300_auto_range_max_laser", int_t, 19, "Max Laser", 16, -32767, 32767) -#gen.add("sr300_auto_range_start_laser", int_t, 20, "Start Laser", -1, -32767, 32767) - -#gen.add("sr300_auto_range_upper_threshold", int_t, 21, "Upper Threshold", 1250, 0, 65535) -#gen.add("sr300_auto_range_lower_threshold", int_t, 22, "Lower Threshold", 650, 0, 65535) - -exit(gen.generate(PACKAGE, "realsense2_camera", "sr300_params")) - diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 53741041cf..ccd326c01d 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -4,30 +4,17 @@ #pragma once #include "../include/realsense_node_factory.h" -#include -#include -#include -#include +#include +#include #include #include +#include +#include namespace realsense2_camera { - enum base_depth_param{ - base_depth_gain = 1, - base_depth_visual_preset, - base_depth_frames_queue_size, - base_depth_error_polling_enabled, - base_depth_output_trigger_enabled, - base_depth_units, - base_sensors_enabled, - base_JSON_file_path, - base_depth_enable_auto_exposure = 100, - base_depth_count - }; - struct FrequencyDiagnostics { FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id) : @@ -54,19 +41,12 @@ namespace realsense2_camera class NamedFilter { - private: -#if RS2_API_VERSION >= 21700 - typedef rs2::filter RS2_FilterType; -#else // RS2_API_VERSION - typedef rs2::processing_block RS2_FilterType; -#endif // RS2_API_VERSION - public: std::string _name; - std::shared_ptr _filter; + std::shared_ptr _filter; public: - NamedFilter(std::string name, std::shared_ptr filter): + NamedFilter(std::string name, std::shared_ptr filter): _name(name), _filter(filter) {} }; @@ -80,6 +60,30 @@ namespace realsense2_camera } }; + class SyncedImuPublisher + { + public: + SyncedImuPublisher() {_is_enabled=false;}; + SyncedImuPublisher(ros::Publisher imu_publisher, std::size_t waiting_list_size=1000); + ~SyncedImuPublisher(); + void Pause(); // Pause sending messages. All messages from now on are saved in queue. + void Resume(); // Send all pending messages and allow sending future messages. + void Publish(sensor_msgs::Imu msg); //either send or hold message. + uint32_t getNumSubscribers() { return _publisher.getNumSubscribers();}; + void Enable(bool is_enabled) {_is_enabled=is_enabled;}; + + private: + void PublishPendingMessages(); + + private: + std::mutex _mutex; + ros::Publisher _publisher; + bool _pause_mode; + std::queue _pendeing_messages; + std::size_t _waiting_list_size; + bool _is_enabled; + }; + class BaseRealSenseNode : public InterfaceRealSenseNode { public: @@ -90,7 +94,7 @@ namespace realsense2_camera void toggleSensors(bool enabled); virtual void publishTopics() override; - virtual void registerDynamicReconfigCb() override; + virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) override; virtual ~BaseRealSenseNode() {} protected: @@ -99,6 +103,10 @@ namespace realsense2_camera rs2::device _dev; ros::NodeHandle& _node_handle, _pnh; std::map _sensors; + std::vector> _ddynrec; + + static void callback(const ddynamic_reconfigure::DDMap& map, int, rs2::options sensor); + void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name); private: struct float3 @@ -118,6 +126,7 @@ namespace realsense2_camera void enable_devices(); void setupFilters(); void setupStreams(); + void clip_depth(rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); tf::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const; void publish_static_tf(const ros::Time& t, @@ -153,6 +162,9 @@ namespace realsense2_camera std::string _json_file_path; std::string _serial_no; float _depth_scale_meters; + float _clipping_distance; + float _linear_accel_cov; + bool _hold_back_imu_for_frames; std::map _stream_intrinsics; std::map _width; @@ -164,6 +176,7 @@ namespace realsense2_camera std::map _image_publishers; std::map _imu_publishers; + std::shared_ptr _synced_imu_publisher; std::map _image_format; std::map _format; std::map _info_publisher; @@ -186,10 +199,12 @@ namespace realsense2_camera bool _align_depth; bool _sync_frames; bool _pointcloud; + bool _unite_imu; std::string _filters_str; stream_index_pair _pointcloud_texture; PipelineSyncer _syncer; std::vector _filters; + std::vector _dev_sensors; // Declare pointcloud object, for calculating pointclouds and texture mappings // rs2::pointcloud _pc_filter; @@ -207,24 +222,5 @@ namespace realsense2_camera const std::string _namespace; };//end class - class BaseD400Node : public BaseRealSenseNode - { - public: - BaseD400Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - virtual void registerDynamicReconfigCb() override; - - protected: - void setParam(rs415_paramsConfig &config, base_depth_param param); - void setParam(rs435_paramsConfig &config, base_depth_param param); - - private: - void callback(base_d400_paramsConfig &config, uint32_t level); - void setOption(stream_index_pair sip, rs2_option opt, float val); - void setParam(base_d400_paramsConfig &config, base_depth_param param); - - std::shared_ptr> _server; - dynamic_reconfigure::Server::CallbackType _f; - }; } + diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index c638f67f9e..9652490c1b 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -30,6 +30,7 @@ namespace realsense2_camera const uint16_t RS430_MM_RGB_PID = 0x0b01; // AWGCT const uint16_t RS460_PID = 0x0b03; // DS5U const uint16_t RS435_RGB_PID = 0x0b07; // AWGC + const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM const uint16_t RS405_PID = 0x0b0c; // DS5U const bool ALIGN_DEPTH = false; @@ -57,8 +58,8 @@ namespace realsense2_camera const int INFRA2_FPS = 30; const int COLOR_FPS = 30; const int FISHEYE_FPS = 30; - const int GYRO_FPS = 1000; - const int ACCEL_FPS = 1000; + const int GYRO_FPS = 400; + const int ACCEL_FPS = 250; const bool ENABLE_DEPTH = true; @@ -67,6 +68,8 @@ namespace realsense2_camera const bool ENABLE_COLOR = true; const bool ENABLE_FISHEYE = true; const bool ENABLE_IMU = true; + const bool UNITE_IMU = true; + const bool HOLD_BACK_IMU_FOR_FRAMES = false; const std::string DEFAULT_BASE_FRAME_ID = "camera_link"; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index e0720feffb..0824dc3462 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -54,7 +54,7 @@ namespace realsense2_camera { public: virtual void publishTopics() = 0; - virtual void registerDynamicReconfigCb() = 0; + virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) = 0; virtual ~InterfaceRealSenseNode() = default; }; diff --git a/realsense2_camera/include/rs415_node.h b/realsense2_camera/include/rs415_node.h deleted file mode 100644 index 178719e535..0000000000 --- a/realsense2_camera/include/rs415_node.h +++ /dev/null @@ -1,50 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include "../include/base_realsense_node.h" -#include - -namespace realsense2_camera -{ - enum rs415_param{ - rs415_depth_enable_auto_white_balance = 9, - rs415_depth_exposure, - rs415_depth_laser_power, - rs415_depth_emitter_enabled, - rs415_color_backlight_compensation, - rs415_color_brightness, - rs415_color_contrast, - rs415_color_exposure, - rs415_color_gain, - rs415_color_gamma, - rs415_color_hue, - rs415_color_saturation, - rs415_color_sharpness, - rs415_color_white_balance, - rs415_color_enable_auto_exposure, - rs415_color_enable_auto_white_balance, - rs415_color_frames_queue_size, - rs415_color_power_line_frequency, - rs415_color_auto_exposure_priority, - rs415_param_count - }; - - class RS415Node : public BaseD400Node - { - public: - RS415Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - - virtual void registerDynamicReconfigCb() override; - - private: - void callback(rs415_paramsConfig &config, uint32_t level); - void setParam(rs415_paramsConfig &config, rs415_param param); - - dynamic_reconfigure::Server _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} diff --git a/realsense2_camera/include/rs435_node.h b/realsense2_camera/include/rs435_node.h deleted file mode 100644 index b1afaf4222..0000000000 --- a/realsense2_camera/include/rs435_node.h +++ /dev/null @@ -1,48 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include "../include/base_realsense_node.h" -#include - -namespace realsense2_camera -{ - enum rs435_param{ - rs435_depth_exposure = 9, - rs435_depth_laser_power, - rs435_depth_emitter_enabled, - rs435_color_backlight_compensation, - rs435_color_brightness, - rs435_color_contrast, - rs435_color_exposure, - rs435_color_gain, - rs435_color_gamma, - rs435_color_hue, - rs435_color_saturation, - rs435_color_sharpness, - rs435_color_white_balance, - rs435_color_enable_auto_exposure, - rs435_color_frames_queue_size, - rs435_color_power_line_frequency, - rs435_color_auto_exposure_priority, - rs435_param_count - }; - - class RS435Node : public BaseD400Node - { - public: - RS435Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - - virtual void registerDynamicReconfigCb() override; - - private: - void callback(rs435_paramsConfig &config, uint32_t level); - void setParam(rs435_paramsConfig &config, rs435_param param); - - dynamic_reconfigure::Server _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} diff --git a/realsense2_camera/include/sr300_node.h b/realsense2_camera/include/sr300_node.h deleted file mode 100644 index 81d685e4fd..0000000000 --- a/realsense2_camera/include/sr300_node.h +++ /dev/null @@ -1,51 +0,0 @@ -// License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2018 Intel Corporation. All Rights Reserved - -#pragma once - -#include "../include/base_realsense_node.h" -#include - -namespace realsense2_camera -{ - enum sr300_param{ - sr300_param_color_backlight_compensation = 1, - sr300_param_color_brightness, - sr300_param_color_contrast, - sr300_param_color_gain, - sr300_param_color_gamma, - sr300_param_color_hue, - sr300_param_color_saturation, - sr300_param_color_sharpness, - sr300_param_color_white_balance, - sr300_param_color_enable_auto_white_balance, - sr300_param_color_exposure, - sr300_param_color_enable_auto_exposure, - sr300_param_depth_visual_preset, - sr300_param_depth_laser_power, - sr300_param_depth_accuracy, - sr300_param_depth_motion_range, - sr300_param_depth_filter_option, - sr300_param_depth_confidence_threshold, - sr300_param_depth_frames_queue_size, - sr300_param_depth_units, - sr300_param_count - }; - - class SR300Node : public BaseRealSenseNode - { - public: - SR300Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no); - - virtual void registerDynamicReconfigCb() override; - - private: - void callback(sr300_paramsConfig &config, uint32_t level); - void setParam(sr300_paramsConfig &config, sr300_param param); - - dynamic_reconfigure::Server _server; - dynamic_reconfigure::Server::CallbackType _f; - }; -} diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 4717f3e48f..9911a09232 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -67,7 +67,9 @@ - + + + @@ -130,7 +132,9 @@ - + + + diff --git a/realsense2_camera/launch/opensource_tracking.launch b/realsense2_camera/launch/opensource_tracking.launch new file mode 100644 index 0000000000..ef12ff4ce4 --- /dev/null +++ b/realsense2_camera/launch/opensource_tracking.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [true,true,true, + true,true,true, + true,true,true, + true,true,true, + true,true,true] + + + + + + + [false, false, false, + true, true, true, + true, true, true, + true, true, true, + true, true, true] + + + + + + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 2d3c96b203..ba6821f71f 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -29,8 +29,8 @@ - - + + @@ -41,6 +41,8 @@ + + @@ -84,6 +86,8 @@ + + diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 5a709a662e..82699c3056 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -22,7 +22,8 @@ cv_bridge image_transport tf - dynamic_reconfigure + ddynamic_reconfigure + dddynamic_reconfigure diagnostic_updater image_transport cv_bridge @@ -33,7 +34,8 @@ std_msgs message_runtime tf - dynamic_reconfigure + ddynamic_reconfigure + dddynamic_reconfigure diagnostic_updater diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 1cb2e158f8..76608c8e7d 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1,10 +1,67 @@ #include "../include/base_realsense_node.h" -#include "../include/sr300_node.h" #include "assert.h" #include #include +#include +#include using namespace realsense2_camera; +using namespace ddynamic_reconfigure; + +SyncedImuPublisher::SyncedImuPublisher(ros::Publisher imu_publisher, std::size_t waiting_list_size): + _publisher(imu_publisher), + _waiting_list_size(waiting_list_size) + {} + +SyncedImuPublisher::~SyncedImuPublisher() +{ + PublishPendingMessages(); +} + +void SyncedImuPublisher::Publish(sensor_msgs::Imu imu_msg) +{ + std::lock_guard lock_guard(_mutex); + if (_pause_mode) + { + if (_pendeing_messages.size() >= _waiting_list_size) + { + throw std::runtime_error("SyncedImuPublisher inner list reached maximum size of " + std::to_string(_pendeing_messages.size())); + } + _pendeing_messages.push(imu_msg); + } + else + { + _publisher.publish(imu_msg); + // ROS_INFO_STREAM("iid1:" << imu_msg.header.seq << ", time: " << std::setprecision (20) << imu_msg.header.stamp.toSec()); + } + return; +} + +void SyncedImuPublisher::Pause() +{ + if (!_is_enabled) return; + std::lock_guard lock_guard(_mutex); + _pause_mode = true; +} + +void SyncedImuPublisher::Resume() +{ + std::lock_guard lock_guard(_mutex); + PublishPendingMessages(); + _pause_mode = false; +} + +void SyncedImuPublisher::PublishPendingMessages() +{ + // ROS_INFO_STREAM("publish imu: " << _pendeing_messages.size()); + while (!_pendeing_messages.empty()) + { + const sensor_msgs::Imu &imu_msg = _pendeing_messages.front(); + _publisher.publish(imu_msg); + // ROS_INFO_STREAM("iid2:" << imu_msg.header.seq << ", time: " << std::setprecision (20) << imu_msg.header.stamp.toSec()); + _pendeing_messages.pop(); + } +} std::string BaseRealSenseNode::getNamespaceStr() { @@ -114,9 +171,112 @@ void BaseRealSenseNode::publishTopics() ROS_INFO_STREAM("RealSense Node Is Up!"); } -void BaseRealSenseNode::registerDynamicReconfigCb() +bool is_checkbox(rs2::options sensor, rs2_option option) +{ + rs2::option_range op_range = sensor.get_option_range(option); + return op_range.max == 1.0f && + op_range.min == 0.0f && + op_range.step == 1.0f; +} + +bool is_enum_option(rs2::options sensor, rs2_option option) { - ROS_INFO("Dynamic reconfig parameters is not implemented in the base node."); + rs2::option_range op_range = sensor.get_option_range(option); + if (op_range.step < 0.001f) return false; + for (auto i = op_range.min; i <= op_range.max; i += op_range.step) + { + if (sensor.get_option_value_description(option, i) == nullptr) + return false; + } + return true; +} + +bool is_int_option(rs2::options sensor, rs2_option option) +{ + rs2::option_range op_range = sensor.get_option_range(option); + return (op_range.step == 1.0); +} + +std::map get_enum_method(rs2::options sensor, rs2_option option) +{ + std::map dict; // An enum to set size + if (is_enum_option(sensor, option)) + { + rs2::option_range op_range = sensor.get_option_range(option); + for (auto val = op_range.min; val <= op_range.max; val += op_range.step) + { + dict[sensor.get_option_value_description(option, val)] = int(val); + } + } + return dict; +} + +void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name) +{ + ros::NodeHandle nh1(module_name); + std::shared_ptr ddynrec = std::make_shared(nh1); + for (auto i = 0; i < RS2_OPTION_COUNT; i++) + { + rs2_option option = static_cast(i); + if (!sensor.supports(option) || sensor.is_option_read_only(option)) + { + continue; + } + if (is_checkbox(sensor, option)) + { + ddynrec->add(new DDBool(rs2_option_to_string(option), i, sensor.get_option_description(option), bool(sensor.get_option(option)))); + continue; + } + std::map enum_dict = get_enum_method(sensor, option); + if (enum_dict.empty()) + { + rs2::option_range op_range = sensor.get_option_range(option); + if (is_int_option(sensor, option)) + { + ddynrec->add(new DDInt(rs2_option_to_string(option), i, sensor.get_option_description(option), sensor.get_option(option), op_range.min, op_range.max)); + } + else + { + ddynrec->add(new DDDouble(rs2_option_to_string(option), i, sensor.get_option_description(option), sensor.get_option(option), op_range.min, op_range.max)); + } + } + else + { + ROS_DEBUG_STREAM("Add enum: " << rs2_option_to_string(option) << ". value=" << int(sensor.get_option(option))); + ddynrec->add(new DDEnum(rs2_option_to_string(option), i, sensor.get_option_description(option), int(sensor.get_option(option)), enum_dict)); + } + } + ddynrec->start(boost::bind(callback, _1, _2, sensor)); + _ddynrec.push_back(ddynrec); +} + +void BaseRealSenseNode::registerDynamicReconfigCb(ros::NodeHandle& nh) +{ + ROS_INFO("Setting Dynamic reconfig parameters."); + + for(rs2::sensor sensor : _dev_sensors) + { + std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME); + std::replace( module_name.begin(), module_name.end(), ' ', '_'); // replace all ' ' to '_' + ROS_DEBUG_STREAM("module_name:" << module_name); + registerDynamicOption(nh, sensor, module_name); + } + + for (NamedFilter nfilter : _filters) + { + std::string module_name = nfilter._name; + auto sensor = *(nfilter._filter); + ROS_DEBUG_STREAM("module_name:" << module_name); + registerDynamicOption(nh, sensor, module_name); + } + ROS_INFO("Done Setting Dynamic reconfig parameters."); +} + +void BaseRealSenseNode::callback(const ddynamic_reconfigure::DDMap& map, int level, rs2::options sensor) { + rs2_option option = static_cast(level); + double value = get(map, rs2_option_to_string(option)).toDouble(); + ROS_DEBUG_STREAM("option: " << rs2_option_to_string(option) << ". value: " << value); + sensor.set_option(option, value); } rs2_stream BaseRealSenseNode::rs2_string_to_stream(std::string str) @@ -182,6 +342,7 @@ void BaseRealSenseNode::getParameters() _pnh.param("accel_fps", _fps[ACCEL], ACCEL_FPS); _pnh.param("enable_imu", _enable[GYRO], ENABLE_IMU); _pnh.param("enable_imu", _enable[ACCEL], ENABLE_IMU); + _pnh.param("unite_imu", _unite_imu, UNITE_IMU); _pnh.param("base_frame_id", _base_frame_id, DEFAULT_BASE_FRAME_ID); _pnh.param("depth_frame_id", _frame_id[DEPTH], DEFAULT_DEPTH_FRAME_ID); @@ -197,13 +358,23 @@ void BaseRealSenseNode::getParameters() _pnh.param("infra2_optical_frame_id", _optical_frame_id[INFRA2], DEFAULT_INFRA2_OPTICAL_FRAME_ID); _pnh.param("color_optical_frame_id", _optical_frame_id[COLOR], DEFAULT_COLOR_OPTICAL_FRAME_ID); _pnh.param("fisheye_optical_frame_id", _optical_frame_id[FISHEYE], DEFAULT_FISHEYE_OPTICAL_FRAME_ID); - _pnh.param("gyro_optical_frame_id", _optical_frame_id[GYRO], DEFAULT_GYRO_OPTICAL_FRAME_ID); + if (_unite_imu) + { + _pnh.param("imu_optical_frame_id", _optical_frame_id[GYRO], DEFAULT_IMU_OPTICAL_FRAME_ID); + } + else + { + _pnh.param("gyro_optical_frame_id", _optical_frame_id[GYRO], DEFAULT_GYRO_OPTICAL_FRAME_ID); + } _pnh.param("accel_optical_frame_id", _optical_frame_id[ACCEL], DEFAULT_ACCEL_OPTICAL_FRAME_ID); _pnh.param("aligned_depth_to_color_frame_id", _depth_aligned_frame_id[COLOR], DEFAULT_ALIGNED_DEPTH_TO_COLOR_FRAME_ID); _pnh.param("aligned_depth_to_infra1_frame_id", _depth_aligned_frame_id[INFRA1], DEFAULT_ALIGNED_DEPTH_TO_INFRA1_FRAME_ID); _pnh.param("aligned_depth_to_infra2_frame_id", _depth_aligned_frame_id[INFRA2], DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID); _pnh.param("aligned_depth_to_fisheye_frame_id", _depth_aligned_frame_id[FISHEYE], DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID); + _pnh.param("clip_distance", _clipping_distance, static_cast(-1.0)); + _pnh.param("linear_accel_cov", _linear_accel_cov, static_cast(0.01)); + _pnh.param("hold_back_imu_for_frames", _hold_back_imu_for_frames, HOLD_BACK_IMU_FOR_FRAMES); } void BaseRealSenseNode::setupDevice() @@ -251,10 +422,10 @@ void BaseRealSenseNode::setupDevice() ROS_INFO_STREAM("Align Depth: " << ((_align_depth)?"On":"Off")); ROS_INFO_STREAM("Sync Mode: " << ((_sync_frames)?"On":"Off")); - auto dev_sensors = _dev.query_sensors(); + _dev_sensors = _dev.query_sensors(); ROS_INFO_STREAM("Device Sensors: "); - for(auto&& elem : dev_sensors) + for(auto&& elem : _dev_sensors) { std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME); if ("Stereo Module" == module_name) @@ -390,16 +561,28 @@ void BaseRealSenseNode::setupPublishers() _depth_to_other_extrinsics_publishers[INFRA2] = _node_handle.advertise("extrinsics/depth_to_infra2", 1, true); } - if (_enable[GYRO]) + _synced_imu_publisher = std::make_shared(); + if (_unite_imu && _enable[GYRO] && _enable[ACCEL]) { - _imu_publishers[GYRO] = _node_handle.advertise("gyro/sample", 100); - _info_publisher[GYRO] = _node_handle.advertise("gyro/imu_info", 1, true); - } + ROS_INFO("Start publisher IMU"); + _synced_imu_publisher = std::make_shared(_node_handle.advertise("imu", 1)); + _synced_imu_publisher->Enable(_hold_back_imu_for_frames); - if (_enable[ACCEL]) + _info_publisher[GYRO] = _node_handle.advertise("imu_info", 1, true); + } + else { - _imu_publishers[ACCEL] = _node_handle.advertise("accel/sample", 100); - _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); + if (_enable[GYRO]) + { + _imu_publishers[GYRO] = _node_handle.advertise("gyro/sample", 100); + _info_publisher[GYRO] = _node_handle.advertise("gyro/imu_info", 1, true); + } + + if (_enable[ACCEL]) + { + _imu_publishers[ACCEL] = _node_handle.advertise("accel/sample", 100); + _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); + } } } @@ -579,6 +762,31 @@ void BaseRealSenseNode::setupFilters() ROS_INFO("num_filters: %d", static_cast(_filters.size())); } +void BaseRealSenseNode::clip_depth(rs2::depth_frame& depth_frame, float depth_scale, float clipping_dist) +{ + uint16_t* p_depth_frame = reinterpret_cast(const_cast(depth_frame.get_data())); + + int width = depth_frame.get_width(); + int height = depth_frame.get_height(); + + #pragma omp parallel for schedule(dynamic) //Using OpenMP to try to parallelise the loop + for (int y = 0; y < height; y++) + { + auto depth_pixel_index = y * width; + for (int x = 0; x < width; x++, ++depth_pixel_index) + { + // Get the depth value of the current pixel + auto pixels_distance = depth_scale * p_depth_frame[depth_pixel_index]; + + // Check if the depth value is greater than the threashold + if (pixels_distance > clipping_dist) + { + p_depth_frame[depth_pixel_index] = -1; //Set to invalid (<=0) value. + } + } + } +} + void BaseRealSenseNode::setupStreams() { ROS_INFO("setupStreams..."); @@ -596,7 +804,10 @@ void BaseRealSenseNode::setupStreams() auto frame_callback = [this](rs2::frame frame) { + _synced_imu_publisher->Pause(); try{ + double frame_time = frame.get_timestamp(); + // We compute a ROS timestamp which is based on an initial ROS time at point of first frame, // and the incremental timestamp from the camera. // In sync mode the timestamp is based on ROS time @@ -607,14 +818,18 @@ void BaseRealSenseNode::setupStreams() _intialize_time_base = true; _ros_time_base = ros::Time::now(); - _camera_time_base = frame.get_timestamp(); + _camera_time_base = frame_time; } ros::Time t; if (_sync_frames) + { t = ros::Time::now(); + } else - t = ros::Time(_ros_time_base.toSec()+ (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000); + { + t = ros::Time(_ros_time_base.toSec()+ (/*ms*/ frame_time - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000); + } std::map is_frame_arrived(_is_frame_arrived); if (frame.is()) @@ -634,8 +849,17 @@ void BaseRealSenseNode::setupStreams() updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", - rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame_time, t.toNSec()); + } + // Clip depth_frame for max range: + if (_clipping_distance > 0) + { + rs2::depth_frame depth_frame = frameset.get_depth_frame(); + if (depth_frame) + this->clip_depth(depth_frame,_depth_scale_meters, _clipping_distance); } + + ROS_DEBUG("num_filters: %d", static_cast(_filters.size())); for (std::vector::const_iterator filter_it = _filters.begin(); filter_it != _filters.end(); filter_it++) { @@ -653,7 +877,7 @@ void BaseRealSenseNode::setupStreams() auto stream_unique_id = f.get_profile().unique_id(); ROS_DEBUG("Frameset contain (%s, %d, %s %d) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", - rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), stream_unique_id, frame.get_frame_number(), frame_time, t.toNSec()); } ROS_DEBUG("END OF LIST"); ROS_DEBUG_STREAM("Remove streams with same type and index:"); @@ -663,9 +887,6 @@ void BaseRealSenseNode::setupStreams() // use the same publisher. // As a workaround we remove the earlier one, the original one, assuming that if colorizer filter is // set it means that that's what the client wants. - // However, that procedure also eliminates the pointcloud , although it uses - // another publisher. - // That's why currently it can't send both pointcloud and colorized depth image. // bool points_in_set(false); std::vector frames_to_publish; @@ -706,7 +927,7 @@ void BaseRealSenseNode::setupStreams() auto stream_format = f.get_profile().format(); ROS_DEBUG("Frameset contain (%s, %d, %s) frame. frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", - rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + rs2_stream_to_string(stream_type), stream_index, rs2_format_to_string(stream_format), frame.get_frame_number(), frame_time, t.toNSec()); if (f.is()) { @@ -743,7 +964,7 @@ void BaseRealSenseNode::setupStreams() auto stream_index = frame.get_profile().stream_index(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); ROS_DEBUG("Single video frame arrived (%s, %d). frame_number: %llu ; frame_TS: %f ; ros_TS(NSec): %lu", - rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame.get_timestamp(), t.toNSec()); + rs2_stream_to_string(stream_type), stream_index, frame.get_frame_number(), frame_time, t.toNSec()); stream_index_pair sip{stream_type,stream_index}; publishFrame(frame, t, @@ -759,6 +980,7 @@ void BaseRealSenseNode::setupStreams() { ROS_ERROR_STREAM("An error has occurred during frame callback: " << ex.what()); } + _synced_imu_publisher->Resume(); }; // frame_callback // Streaming IMAGES @@ -813,13 +1035,24 @@ void BaseRealSenseNode::setupStreams() auto profiles = sens.get_stream_profiles(); for (rs2::stream_profile& profile : profiles) { - if (profile.fps() == _fps[elem] && + if (profile.stream_type() == elem.first && + profile.fps() == _fps[elem] && profile.format() == _format[elem]) { _enabled_profiles[elem].push_back(profile); break; } } + if (_enabled_profiles[elem].size() == 0) + { + ROS_WARN_STREAM("No mathcing profile found for " << _stream_name[elem] << " with fps=" << _fps[elem] << " and format=" << _format[elem]); + ROS_WARN_STREAM("profiles found for " << _stream_name[elem] << ":"); + for (rs2::stream_profile& profile : profiles) + { + if (profile.stream_type() != elem.first) continue; + ROS_WARN_STREAM("fps: " << profile.fps() << ". format: " << profile.format()); + } + } } } } @@ -830,13 +1063,14 @@ void BaseRealSenseNode::setupStreams() if (gyro_profile != _enabled_profiles.end() && accel_profile != _enabled_profiles.end()) { + ROS_INFO("starting imu..."); std::vector profiles; profiles.insert(profiles.begin(), gyro_profile->second.begin(), gyro_profile->second.end()); profiles.insert(profiles.begin(), accel_profile->second.begin(), accel_profile->second.end()); auto& sens = _sensors[GYRO]; sens.open(profiles); - sens.start([this](rs2::frame frame){ + auto imu_callback = [this](rs2::frame frame){ auto stream = frame.get_profile().stream_type(); if (false == _intialize_time_base) return; @@ -850,7 +1084,8 @@ void BaseRealSenseNode::setupStreams() if (0 != _info_publisher[stream_index].getNumSubscribers() || 0 != _imu_publishers[stream_index].getNumSubscribers()) { - double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000; + // double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000; + double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base); ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); auto imu_msg = sensor_msgs::Imu(); @@ -861,18 +1096,18 @@ void BaseRealSenseNode::setupStreams() imu_msg.orientation.w = 0.0; imu_msg.orientation_covariance = { -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - auto axes = *(reinterpret_cast(frame.get_data())); + auto axis = *(reinterpret_cast(frame.get_data())); if (GYRO == stream_index) { - imu_msg.angular_velocity.x = axes.x; - imu_msg.angular_velocity.y = axes.y; - imu_msg.angular_velocity.z = axes.z; + imu_msg.angular_velocity.x = axis.x; + imu_msg.angular_velocity.y = axis.y; + imu_msg.angular_velocity.z = axis.z; } else if (ACCEL == stream_index) { - imu_msg.linear_acceleration.x = axes.x; - imu_msg.linear_acceleration.y = axes.y; - imu_msg.linear_acceleration.z = axes.z; + imu_msg.linear_acceleration.x = axis.x; + imu_msg.linear_acceleration.y = axis.y; + imu_msg.linear_acceleration.z = axis.z; } _seq[stream_index] += 1; imu_msg.header.seq = _seq[stream_index]; @@ -880,20 +1115,118 @@ void BaseRealSenseNode::setupStreams() _imu_publishers[stream_index].publish(imu_msg); ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); } - }); + }; + + auto imu_callback_sync = [this](rs2::frame frame){ + static std::mutex m_mutex; + static const stream_index_pair stream_imu = GYRO; + static sensor_msgs::Imu imu_msg = sensor_msgs::Imu(); + static int seq = 0; + static bool init_gyro(false), init_accel(false); + static double accel_factor(0); + imu_msg.header.frame_id = _frame_id[stream_imu]; + imu_msg.orientation.x = 0.0; + imu_msg.orientation.y = 0.0; + imu_msg.orientation.z = 0.0; + imu_msg.orientation.w = 0.0; + + imu_msg.orientation_covariance = { -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + imu_msg.linear_acceleration_covariance = { _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov, 0.0, 0.0, 0.0, _linear_accel_cov}; + imu_msg.angular_velocity_covariance = { 0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01}; + + m_mutex.lock(); + + while (true) + { + auto stream = frame.get_profile().stream_type(); + auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; + double frame_time = frame.get_timestamp(); + + if (false == _intialize_time_base) + break; + + double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; + ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); + seq += 1; - if (_enable[GYRO]) + if (0 != _synced_imu_publisher->getNumSubscribers()) + { + auto axis = *(reinterpret_cast(frame.get_data())); + if (true) + { + // Convert from optical frame to frame: + float3 temp; + temp.x = axis.z; + temp.y = -axis.x; + temp.z = -axis.y; + + axis.x = temp.x; + axis.y = temp.y; + axis.z = temp.z; + imu_msg.header.frame_id = _frame_id[stream_index]; + } + if (GYRO == stream_index) + { + init_gyro = true; + imu_msg.angular_velocity.x = axis.x; + imu_msg.angular_velocity.y = axis.y; + imu_msg.angular_velocity.z = axis.z; + } + else if (ACCEL == stream_index) + { + if (!init_accel) + { + // Init accel_factor: + Eigen::Vector3d v(axis.x, axis.y, axis.z); + accel_factor = 9.81 / v.norm(); + ROS_INFO_STREAM("accel_factor set to: " << accel_factor); + } + init_accel = true; + if (true) + { + Eigen::Vector3d v(axis.x, axis.y, axis.z); + v*=accel_factor; + axis.x = v.x(); + axis.y = v.y(); + axis.z = v.z(); + } + imu_msg.linear_acceleration.x = axis.x; + imu_msg.linear_acceleration.y = axis.y; + imu_msg.linear_acceleration.z = axis.z; + } + imu_msg.header.seq = seq; + imu_msg.header.stamp = t; + if (!(init_gyro && init_accel)) + break; + _synced_imu_publisher->Publish(imu_msg); + ROS_DEBUG("Publish united %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + } + break; + } + m_mutex.unlock(); + }; + if (_unite_imu) { - ROS_INFO_STREAM(_stream_name[GYRO] << " stream is enabled - " << "fps: " << _fps[GYRO]); - auto gyroInfo = getImuInfo(GYRO); - _info_publisher[GYRO].publish(gyroInfo); + ROS_INFO_STREAM("Gyro and accelometer are enabled and combined to IMU message at " << "fps: " << (_fps[GYRO] + _fps[ACCEL])); + sens.start(imu_callback_sync); } - - if (_enable[ACCEL]) + else { - ROS_INFO_STREAM(_stream_name[ACCEL] << " stream is enabled - " << "fps: " << _fps[ACCEL]); - auto accelInfo = getImuInfo(ACCEL); - _info_publisher[ACCEL].publish(accelInfo); + sens.start(imu_callback); + + if (_enable[GYRO]) + { + ROS_INFO_STREAM(_stream_name[GYRO] << " stream is enabled - " << "fps: " << _fps[GYRO]); + auto gyroInfo = getImuInfo(GYRO); + _info_publisher[GYRO].publish(gyroInfo); + } + + if (_enable[ACCEL]) + { + ROS_INFO_STREAM(_stream_name[ACCEL] << " stream is enabled - " << "fps: " << _fps[ACCEL]); + auto accelInfo = getImuInfo(ACCEL); + _info_publisher[ACCEL].publish(accelInfo); + } } } @@ -1054,7 +1387,6 @@ void BaseRealSenseNode::publish_static_tf(const ros::Time& t, void BaseRealSenseNode::publishStaticTransforms() { - ROS_INFO("publishStaticTransforms..."); // Publish static transforms tf::Quaternion quaternion_optical; quaternion_optical.setRPY(-M_PI / 2, 0.0, -M_PI / 2); @@ -1080,7 +1412,6 @@ void BaseRealSenseNode::publishStaticTransforms() exit(1); } - if (_enable[COLOR]) { // Transform base to color @@ -1168,6 +1499,43 @@ void BaseRealSenseNode::publishStaticTransforms() publish_static_tf(transform_ts_, zero_trans, q2, _depth_aligned_frame_id[FISHEYE], _optical_frame_id[FISHEYE]); } } + if (_enable[GYRO]) + { + // Transform base to Gyro + // const auto& ex = getRsExtrinsics(Gyro, DEPTH); + // Currently, No extrinsics available: + const rs2_extrinsics ex = {{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}; + + auto Q = rotationMatrixToQuaternion(ex.rotation); + Q = quaternion_optical * Q * quaternion_optical.inverse(); + + float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; + quaternion q1{Q.getX(), Q.getY(), Q.getZ(), Q.getW()}; + publish_static_tf(transform_ts_, trans, q1, _base_frame_id, _frame_id[GYRO]); + + // Transform gyro frame to gyro optical frame + quaternion q2{quaternion_optical.getX(), quaternion_optical.getY(), quaternion_optical.getZ(), quaternion_optical.getW()}; + publish_static_tf(transform_ts_, zero_trans, q2, _frame_id[GYRO], _optical_frame_id[GYRO]); + } + if (_enable[ACCEL] and !_unite_imu) + { + // Transform base to Accel + // const auto& ex = getRsExtrinsics(Accel, DEPTH); + // Currently, No extrinsics available: + const rs2_extrinsics ex = {{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}; + + auto Q = rotationMatrixToQuaternion(ex.rotation); + Q = quaternion_optical * Q * quaternion_optical.inverse(); + + float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]}; + quaternion q1{Q.getX(), Q.getY(), Q.getZ(), Q.getW()}; + publish_static_tf(transform_ts_, trans, q1, _base_frame_id, _frame_id[ACCEL]); + + // Transform gyro frame to gyro optical frame + quaternion q2{quaternion_optical.getX(), quaternion_optical.getY(), quaternion_optical.getZ(), quaternion_optical.getW()}; + publish_static_tf(transform_ts_, zero_trans, q2, _frame_id[ACCEL], _optical_frame_id[ACCEL]); + } + } rs2::frame BaseRealSenseNode::get_frame(const rs2::frameset& frameset, const rs2_stream stream, const int index) @@ -1306,7 +1674,16 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) { IMUInfo info{}; auto sp = _enabled_profiles[stream_index].front().as(); - auto imuIntrinsics = sp.get_motion_intrinsics(); + rs2_motion_device_intrinsic imuIntrinsics; + try + { + imuIntrinsics = sp.get_motion_intrinsics(); + } + catch(const runtime_error &ex) + { + ROS_DEBUG_STREAM("No Motion Intrinsics available."); + imuIntrinsics = {{{1,0,0,0},{0,1,0,0},{0,0,1,0}}, {0,0,0}, {0,0,0}}; + } if (GYRO == stream_index) { info.header.frame_id = "imu_gyro"; @@ -1386,6 +1763,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, image_publisher.first.publish(img); image_publisher.second->update(); + // ROS_INFO_STREAM("fid: " << cam_info.header.seq << ", time: " << std::setprecision (20) << t.toSec()); ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); } } @@ -1404,145 +1782,3 @@ bool BaseRealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, return true; } - -BaseD400Node::BaseD400Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no) - : BaseRealSenseNode(nodeHandle, - privateNodeHandle, - dev, serial_no) -{} - -void BaseD400Node::callback(base_d400_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("D400 - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < base_depth_count ; ++i) - { - ROS_DEBUG_STREAM("base_depth_param = " << i); - try - { - setParam(config ,(base_depth_param)i); - } - catch(...) - { - ROS_ERROR_STREAM("Failed. Skip initialization of parameter " << (base_depth_param)i); - } - } - } - else - { - setParam(config, (base_depth_param)level); - } -} - -void BaseD400Node::setOption(stream_index_pair sip, rs2_option opt, float val) -{ - _sensors[sip].set_option(opt, val); -} - -void BaseD400Node::setParam(rs435_paramsConfig &config, base_depth_param param) -{ - base_d400_paramsConfig base_config; - base_config.base_depth_gain = config.rs435_depth_gain; - base_config.base_depth_enable_auto_exposure = config.rs435_depth_enable_auto_exposure; - base_config.base_depth_visual_preset = config.rs435_depth_visual_preset; - base_config.base_depth_frames_queue_size = config.rs435_depth_frames_queue_size; - base_config.base_depth_error_polling_enabled = config.rs435_depth_error_polling_enabled; - base_config.base_depth_output_trigger_enabled = config.rs435_depth_output_trigger_enabled; - base_config.base_depth_units = config.rs435_depth_units; - base_config.base_JSON_file_path = config.rs435_JSON_file_path; - base_config.base_sensors_enabled = config.rs435_sensors_enabled; - setParam(base_config, param); -} - -void BaseD400Node::setParam(rs415_paramsConfig &config, base_depth_param param) -{ - base_d400_paramsConfig base_config; - base_config.base_depth_gain = config.rs415_depth_gain; - base_config.base_depth_enable_auto_exposure = config.rs415_depth_enable_auto_exposure; - base_config.base_depth_visual_preset = config.rs415_depth_visual_preset; - base_config.base_depth_frames_queue_size = config.rs415_depth_frames_queue_size; - base_config.base_depth_error_polling_enabled = config.rs415_depth_error_polling_enabled; - base_config.base_depth_output_trigger_enabled = config.rs415_depth_output_trigger_enabled; - base_config.base_depth_units = config.rs415_depth_units; - base_config.base_JSON_file_path = config.rs415_JSON_file_path; - base_config.base_sensors_enabled = config.rs415_sensors_enabled; - setParam(base_config, param); -} - -void BaseD400Node::setParam(base_d400_paramsConfig &config, base_depth_param param) -{ - // W/O for zero param - if (0 == param) - return; - - // Switch based on the level, defined in .py or .cfg file - switch (param) { - case base_depth_gain: - ROS_DEBUG_STREAM("base_depth_gain: " << config.base_depth_gain); - setOption(DEPTH, RS2_OPTION_GAIN, config.base_depth_gain); - break; - case base_depth_enable_auto_exposure: - ROS_DEBUG_STREAM("base_depth_enable_auto_exposure: " << config.base_depth_enable_auto_exposure); - setOption(DEPTH, RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.base_depth_enable_auto_exposure); - break; - case base_depth_visual_preset: - ROS_DEBUG_STREAM("base_depth_visual_preset: " << config.base_depth_visual_preset); - setOption(DEPTH, RS2_OPTION_VISUAL_PRESET, config.base_depth_visual_preset); - break; - case base_depth_frames_queue_size: - ROS_DEBUG_STREAM("base_depth_frames_queue_size: " << config.base_depth_frames_queue_size); - setOption(DEPTH, RS2_OPTION_FRAMES_QUEUE_SIZE, config.base_depth_frames_queue_size); - break; - case base_depth_error_polling_enabled: - ROS_DEBUG_STREAM("base_depth_error_polling_enabled: " << config.base_depth_error_polling_enabled); - setOption(DEPTH, RS2_OPTION_ERROR_POLLING_ENABLED, config.base_depth_error_polling_enabled); - break; - case base_depth_output_trigger_enabled: - ROS_DEBUG_STREAM("base_depth_output_trigger_enabled: " << config.base_depth_output_trigger_enabled); - setOption(DEPTH, RS2_OPTION_OUTPUT_TRIGGER_ENABLED, config.base_depth_output_trigger_enabled); - break; - case base_depth_units: - break; - case base_sensors_enabled: - { - ROS_DEBUG_STREAM("base_sensors_enabled: " << config.base_sensors_enabled); - toggleSensors(config.base_sensors_enabled); - break; - } - case base_JSON_file_path: - { - ROS_DEBUG_STREAM("base_JSON_file_path: " << config.base_JSON_file_path); - auto adv_dev = _dev.as(); - if (!adv_dev) - { - ROS_WARN_STREAM("Device doesn't support Advanced Mode!"); - return; - } - if (!config.base_JSON_file_path.empty()) - { - std::ifstream in(config.base_JSON_file_path); - if (!in.is_open()) - { - ROS_WARN_STREAM("JSON file provided doesn't exist!"); - return; - } - - adv_dev.load_json(config.base_JSON_file_path); - } - break; - } - case base_depth_count: - break; - } -} - -void BaseD400Node::registerDynamicReconfigCb() -{ - _server = std::make_shared>(); - _f = boost::bind(&BaseD400Node::callback, this, _1, _2); - _server->setCallback(_f); -} diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 15a4f80132..1590330da0 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -2,9 +2,7 @@ // Copyright(c) 2017 Intel Corporation. All Rights Reserved #include "../include/realsense_node_factory.h" -#include "../include/sr300_node.h" -#include "../include/rs415_node.h" -#include "../include/rs435_node.h" +#include "../include/base_realsense_node.h" #include #include @@ -111,41 +109,20 @@ void RealSenseNodeFactory::onInit() switch(pid) { case SR300_PID: - _realSenseNode = std::unique_ptr(new SR300Node(nh, privateNh, _device, serial_no)); - break; case RS400_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS405_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS410_PID: case RS460_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS415_PID: - _realSenseNode = std::unique_ptr(new RS415Node(nh, privateNh, _device, serial_no)); - break; case RS420_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS420_MM_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS430_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS430_MM_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS430_MM_RGB_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); - break; case RS435_RGB_PID: - _realSenseNode = std::unique_ptr(new RS435Node(nh, privateNh, _device, serial_no)); - break; + case RS435i_RGB_PID: case RS_USB2_PID: - _realSenseNode = std::unique_ptr(new BaseD400Node(nh, privateNh, _device, serial_no)); + _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, serial_no)); break; default: ROS_FATAL_STREAM("Unsupported device!" << " Product ID: 0x" << pid_str); @@ -155,7 +132,7 @@ void RealSenseNodeFactory::onInit() } assert(_realSenseNode); _realSenseNode->publishTopics(); - _realSenseNode->registerDynamicReconfigCb(); + _realSenseNode->registerDynamicReconfigCb(nh); } catch(const std::exception& ex) { diff --git a/realsense2_camera/src/rs415_node.cpp b/realsense2_camera/src/rs415_node.cpp deleted file mode 100644 index 1b5116e73d..0000000000 --- a/realsense2_camera/src/rs415_node.cpp +++ /dev/null @@ -1,142 +0,0 @@ -#include "../include/rs415_node.h" - -using namespace realsense2_camera; - - -RS415Node::RS415Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no) - : BaseD400Node(nodeHandle, privateNodeHandle, dev, serial_no) -{} - -void RS415Node::registerDynamicReconfigCb() -{ - _f = boost::bind(&RS415Node::callback, this, _1, _2); - _server.setCallback(_f); -} - - -void RS415Node::setParam(rs415_paramsConfig &config, rs415_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case rs415_color_backlight_compensation: - ROS_DEBUG_STREAM("base_JSON_file_path: " << config.rs415_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs415_color_backlight_compensation); - break; - case rs415_color_brightness: - ROS_DEBUG_STREAM("rs415_color_brightness: " << config.rs415_color_brightness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs415_color_brightness); - break; - case rs415_color_contrast: - ROS_DEBUG_STREAM("rs415_color_contrast: " << config.rs415_color_contrast); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs415_color_contrast); - break; - case rs415_color_gain: - ROS_DEBUG_STREAM("rs415_color_gain: " << config.rs415_color_gain); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs415_color_gain); - break; - case rs415_color_gamma: - ROS_DEBUG_STREAM("rs415_color_gamma: " << config.rs415_color_gamma); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs415_color_gamma); - break; - case rs415_color_hue: - ROS_DEBUG_STREAM("rs415_color_hue: " << config.rs415_color_hue); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs415_color_hue); - break; - case rs415_color_saturation: - ROS_DEBUG_STREAM("rs415_color_saturation: " << config.rs415_color_saturation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs415_color_saturation); - break; - case rs415_color_sharpness: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs415_color_sharpness); - break; - case rs415_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_color_enable_auto_white_balance); - break; - case rs415_color_enable_auto_exposure: - ROS_DEBUG_STREAM("rs415_color_sharpness: " << config.rs415_color_enable_auto_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs415_color_enable_auto_exposure); - break; - case rs415_color_exposure: - ROS_DEBUG_STREAM("rs415_color_exposure: " << config.rs415_color_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_color_exposure); - break; - case rs415_color_white_balance: - { - static const auto rs415_color_white_balance_factor = 10; - ROS_DEBUG_STREAM("rs415_color_white_balance: " << config.rs415_color_white_balance * rs415_color_white_balance_factor); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs415_color_white_balance * rs415_color_white_balance_factor); - } - break; - case rs415_color_frames_queue_size: - ROS_DEBUG_STREAM("rs415_color_frames_queue_size: " << config.rs415_color_frames_queue_size); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs415_color_frames_queue_size); - break; - case rs415_color_power_line_frequency: - ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_power_line_frequency); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs415_color_power_line_frequency); - break; - case rs415_color_auto_exposure_priority: - ROS_DEBUG_STREAM("rs415_color_power_line_frequency: " << config.rs415_color_auto_exposure_priority); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs415_color_auto_exposure_priority); - break; - case rs415_depth_exposure: - { - static const auto rs415_depth_exposure_factor = 20; - ROS_DEBUG_STREAM("rs415_depth_exposure: " << config.rs415_depth_exposure * rs415_depth_exposure_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs415_depth_exposure * rs415_depth_exposure_factor); - } - break; - case rs415_depth_laser_power: - { - static const auto rs415_depth_laser_power_factor = 30; - ROS_DEBUG_STREAM("rs415_depth_laser_power: " << config.rs415_depth_laser_power * rs415_depth_laser_power_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs415_depth_laser_power * rs415_depth_laser_power_factor); - } - break; - case rs415_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs415_depth_emitter_enabled: " << config.rs415_depth_emitter_enabled); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs415_depth_emitter_enabled); - break; - case rs415_depth_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs415_depth_enable_auto_white_balance: " << config.rs415_depth_enable_auto_white_balance); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs415_depth_enable_auto_white_balance); - break; - default: - BaseD400Node::setParam(config, (base_depth_param)param); - break; - } -} - -void RS415Node::callback(rs415_paramsConfig &config, uint32_t level) -{ - ROS_INFO_STREAM("Start updating dynamic parameters..."); - ROS_DEBUG_STREAM("RS415Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < base_depth_count ; ++i) - { - ROS_DEBUG_STREAM("rs415_param = " << i); - try - { - setParam(config ,(rs415_param)i); - } - catch(...) - { - ROS_ERROR_STREAM("Failed. Skip initialization of parameter " << (rs415_param)i); - } - } - } - else - { - setParam(config, (rs415_param)level); - } - ROS_INFO_STREAM("Done updating dynamic parameters..."); -} diff --git a/realsense2_camera/src/rs435_node.cpp b/realsense2_camera/src/rs435_node.cpp deleted file mode 100644 index c8f70adf96..0000000000 --- a/realsense2_camera/src/rs435_node.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "../include/rs435_node.h" - -using namespace realsense2_camera; - - -RS435Node::RS435Node(ros::NodeHandle& nodeHandle, - ros::NodeHandle& privateNodeHandle, - rs2::device dev, const std::string& serial_no) - : BaseD400Node(nodeHandle, privateNodeHandle, dev, serial_no) -{} - -void RS435Node::registerDynamicReconfigCb() -{ - _f = boost::bind(&RS435Node::callback, this, _1, _2); - _server.setCallback(_f); -} - -void RS435Node::setParam(rs435_paramsConfig &config, rs435_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case rs435_color_backlight_compensation: - ROS_DEBUG_STREAM("rs435_color_backlight_compensation: " << config.rs435_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.rs435_color_backlight_compensation); - break; - case rs435_color_brightness: - ROS_DEBUG_STREAM("rs435_color_brightness: " << config.rs435_color_brightness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.rs435_color_brightness); - break; - case rs435_color_contrast: - ROS_DEBUG_STREAM("rs435_color_contrast: " << config.rs435_color_contrast); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.rs435_color_contrast); - break; - case rs435_color_gain: - ROS_DEBUG_STREAM("rs435_color_gain: " << config.rs435_color_gain); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.rs435_color_gain); - break; - case rs435_color_gamma: - ROS_DEBUG_STREAM("rs435_color_gamma: " << config.rs435_color_gamma); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.rs435_color_gamma); - break; - case rs435_color_hue: - ROS_DEBUG_STREAM("rs435_color_hue: " << config.rs435_color_hue); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.rs435_color_hue); - break; - case rs435_color_saturation: - ROS_DEBUG_STREAM("rs435_color_saturation: " << config.rs435_color_saturation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.rs435_color_saturation); - break; - case rs435_color_sharpness: - ROS_DEBUG_STREAM("rs435_color_sharpness: " << config.rs435_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.rs435_color_sharpness); - break; - case rs435_color_enable_auto_exposure: - ROS_DEBUG_STREAM("rs435_color_enable_auto_exposure: " << config.rs435_color_enable_auto_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.rs435_color_enable_auto_exposure); - break; - case rs435_color_exposure: - ROS_DEBUG_STREAM("rs435_color_exposure: " << config.rs435_color_exposure); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_color_exposure); - break; - case rs435_color_white_balance: - ROS_DEBUG_STREAM("rs435_color_white_balance: " << config.rs435_color_white_balance * 10); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.rs435_color_white_balance * 10); - break; - case rs435_color_frames_queue_size: - ROS_DEBUG_STREAM("rs435_color_frames_queue_size: " << config.rs435_color_frames_queue_size); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.rs435_color_frames_queue_size); - break; - case rs435_color_power_line_frequency: - ROS_DEBUG_STREAM("rs435_color_power_line_frequency: " << config.rs435_color_power_line_frequency); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_POWER_LINE_FREQUENCY, config.rs435_color_power_line_frequency); - break; - case rs435_color_auto_exposure_priority: - ROS_DEBUG_STREAM("rs435_color_auto_exposure_priority: " << config.rs435_color_auto_exposure_priority); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_AUTO_EXPOSURE_PRIORITY, config.rs435_color_auto_exposure_priority); - break; - case rs435_depth_exposure: - { - static const auto rs435_depth_exposure_factor = 20; - ROS_DEBUG_STREAM("rs435_depth_exposure: " << config.rs435_depth_exposure * rs435_depth_exposure_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.rs435_depth_exposure * rs435_depth_exposure_factor); - } - break; - case rs435_depth_laser_power: - { - static const auto rs435_depth_laser_power_factor = 30; - ROS_DEBUG_STREAM("rs435_depth_laser_power: " << config.rs435_depth_laser_power * rs435_depth_laser_power_factor); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.rs435_depth_laser_power * rs435_depth_laser_power_factor); - } - break; - case rs435_depth_emitter_enabled: - ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.rs435_depth_emitter_enabled); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, config.rs435_depth_emitter_enabled); - break; - default: - BaseD400Node::setParam(config, (base_depth_param)param); - break; - } -} - -void RS435Node::callback(rs435_paramsConfig &config, uint32_t level) -{ - ROS_INFO_STREAM("Start updating dynamic parameters..."); - ROS_DEBUG_STREAM("RS435Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < base_depth_count ; ++i) - { - ROS_DEBUG_STREAM("rs435_param = " << i); - try - { - setParam(config ,(rs435_param)i); - } - catch(...) - { - ROS_ERROR_STREAM("Failed. Skip initialization of parameter " << (rs435_param)i); - } - } - } - else - { - setParam(config, (rs435_param)level); - } - ROS_INFO_STREAM("Done updating dynamic parameters..."); -} \ No newline at end of file diff --git a/realsense2_camera/src/sr300_node.cpp b/realsense2_camera/src/sr300_node.cpp deleted file mode 100644 index 063ddc8e62..0000000000 --- a/realsense2_camera/src/sr300_node.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include "../include/sr300_node.h" - -using namespace realsense2_camera; - -SR300Node::SR300Node(ros::NodeHandle& nodeHandle, ros::NodeHandle& privateNodeHandle, rs2::device dev, const std::string& serial_no) - : BaseRealSenseNode(nodeHandle, privateNodeHandle, dev, serial_no) -{} - -void SR300Node::registerDynamicReconfigCb() -{ - _f = boost::bind(&SR300Node::callback, this, _1, _2); - _server.setCallback(_f); -} - -void SR300Node::setParam(sr300_paramsConfig &config, sr300_param param) -{ - // W/O for zero param - if (0 == param) - return; - - switch (param) { - case sr300_param_color_backlight_compensation: - ROS_DEBUG_STREAM("sr300_param_color_backlight_compensation: " << config.sr300_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BACKLIGHT_COMPENSATION, config.sr300_color_backlight_compensation); - break; - case sr300_param_color_brightness: - ROS_DEBUG_STREAM("sr300_color_backlight_compensation: " << config.sr300_color_backlight_compensation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_BRIGHTNESS, config.sr300_color_brightness); - break; - case sr300_param_color_contrast: - ROS_DEBUG_STREAM("sr300_param_color_contrast: " << config.sr300_color_contrast); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_CONTRAST, config.sr300_color_contrast); - break; - case sr300_param_color_gain: - ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gain); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAIN, config.sr300_color_gain); - break; - case sr300_param_color_gamma: - ROS_DEBUG_STREAM("sr300_param_color_gain: " << config.sr300_color_gamma); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_GAMMA, config.sr300_color_gamma); - break; - case sr300_param_color_hue: - ROS_DEBUG_STREAM("sr300_param_color_hue: " << config.sr300_color_hue); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_HUE, config.sr300_color_hue); - break; - case sr300_param_color_saturation: - ROS_DEBUG_STREAM("sr300_param_color_saturation: " << config.sr300_color_saturation); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SATURATION, config.sr300_color_saturation); - break; - case sr300_param_color_sharpness: - ROS_DEBUG_STREAM("sr300_param_color_sharpness: " << config.sr300_color_sharpness); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_SHARPNESS, config.sr300_color_sharpness); - break; - case sr300_param_color_white_balance: - ROS_DEBUG_STREAM("sr300_param_color_white_balance: " << config.sr300_color_white_balance); - if (_sensors[COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE)) - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, 0); - - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_WHITE_BALANCE, config.sr300_color_white_balance); - break; - case sr300_param_color_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs435_depth_emitter_enabled: " << config.sr300_color_enable_auto_white_balance); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.sr300_color_enable_auto_white_balance); - break; - case sr300_param_color_exposure: - ROS_DEBUG_STREAM("sr300_param_color_exposure: " << config.sr300_color_exposure); - if (_sensors[COLOR].get_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE)) - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); - - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_EXPOSURE, config.sr300_color_exposure); - break; - case sr300_param_color_enable_auto_exposure: - ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_color_enable_auto_white_balance); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, config.sr300_color_enable_auto_exposure); - break; - case sr300_param_depth_visual_preset: - ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_visual_preset); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_VISUAL_PRESET, config.sr300_depth_visual_preset); - break; - case sr300_param_depth_laser_power: - ROS_DEBUG_STREAM("sr300_param_color_enable_auto_exposure: " << config.sr300_depth_laser_power); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_LASER_POWER, config.sr300_depth_laser_power); - break; - case sr300_param_depth_accuracy: - ROS_DEBUG_STREAM("sr300_param_depth_accuracy: " << config.sr300_depth_accuracy); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_ACCURACY, config.sr300_depth_accuracy); - break; - case sr300_param_depth_motion_range: - ROS_DEBUG_STREAM("sr300_param_depth_motion_range: " << config.sr300_depth_motion_range); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_MOTION_RANGE, config.sr300_depth_motion_range); - break; - case sr300_param_depth_filter_option: - ROS_DEBUG_STREAM("sr300_param_depth_filter_option: " << config.sr300_depth_filter_option); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_FILTER_OPTION, config.sr300_depth_filter_option); - break; - case sr300_param_depth_confidence_threshold: - ROS_DEBUG_STREAM("sr300_param_depth_confidence_threshold: " << config.sr300_depth_confidence_threshold); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_CONFIDENCE_THRESHOLD, config.sr300_depth_confidence_threshold); - break; - case sr300_param_depth_frames_queue_size: - ROS_DEBUG_STREAM("sr300_param_depth_frames_queue_size: " << config.sr300_depth_frames_queue_size); - _sensors[DEPTH].set_option(rs2_option::RS2_OPTION_FRAMES_QUEUE_SIZE, config.sr300_depth_frames_queue_size); - break; - case sr300_param_depth_units: - break; - default: - ROS_WARN_STREAM("Unrecognized sr300 param (" << param << ")"); - break; - } - - // TODO -// // Auto range options -// "sr300_auto_range_enable_motion_versus_range" -// "sr300_auto_range_enable_laser", - -// // Set only if sr300_auto_range_enable_motion_versus_range is set to 1 -// "sr300_auto_range_min_motion_versus_range", -// "sr300_auto_range_max_motion_versus_range", -// "sr300_auto_range_start_motion_versus_range", - -// // Set only if sr300_auto_range_enable_laser is enabled -// "sr300_auto_range_min_laser" -// "sr300_auto_range_max_laser" -// "sr300_auto_range_start_laser" - -// "sr300_auto_range_upper_threshold" -// "sr300_auto_range_lower_threshold" -} - -void SR300Node::callback(sr300_paramsConfig &config, uint32_t level) -{ - ROS_DEBUG_STREAM("SR300Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < base_depth_count ; ++i) - { - ROS_DEBUG_STREAM("sr300_param = " << i); - try - { - setParam(config ,(sr300_param)i); - } - catch(...) - { - ROS_ERROR_STREAM("Failed. Skip initialization of parameter " << (sr300_param)i); - } - } - } - else - { - setParam(config, (sr300_param)level); - } -}