diff --git a/.gitignore b/.gitignore index ba456f1416..55a9718f84 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,6 @@ *.bag *.pyc *.ini +.idea/ +.vs/ +*.json diff --git a/.travis.yml b/.travis.yml index 7e57a822cc..8ac2284d3e 100644 --- a/.travis.yml +++ b/.travis.yml @@ -25,7 +25,7 @@ install: - sudo apt-get install ros-kinetic-diagnostic-updater -y - source ~/.bashrc - mkdir -p ~/catkin_ws/src/realsense - + # 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/README.md b/README.md index 2abe56ea59..3acd8f40e9 100644 --- a/README.md +++ b/README.md @@ -5,16 +5,15 @@ These are packages for using Intel RealSense cameras (D400 series and the SR300) The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kinetic, on **Ubuntu 16.04**. +#### The simplest way to install on a clean machine is to follow the instructions on the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 3 steps: + ### Step 1: Install the latest Intel® RealSense™ SDK 2.0 -- #### Install from [Debian Package](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) +- #### Install from [Debian Package](https://github.com/IntelRealSense/librealsense/blob/master/doc/distribution_linux.md#installing-the-packages) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev package. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/latest) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) +- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.17.0) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) ### Step 2: Install the ROS distribution -- #### Install [ROS Indigo](http://wiki.ros.org/indigo/Installation/Ubuntu), on Ubuntu 14.04 - -#### OR - #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 ### Step 3: Install Intel® RealSense™ ROS from Sources @@ -81,6 +80,35 @@ roslaunch realsense2_camera rs_multiple_devices.launch serial_no_camera1:= +roslaunch realsense2_camera rs_camera.launch camera:=cam_2 serial_no:= +... + +``` +### Enabling post processing filters. +realsense2_camera includes some built in post processing filters:
+colorizer - creates an RGB image instead of depth image. Used to visualize the depth image.
+spatial - filter the depth image spatially.
+temporal - filter the depth image temporally.
+pointcloud - it is now possible to enable point cloud with the same command as any other post processing filter.
+ +The colorizer filter replaces the image in the topic: //depth/image_rect_raw. +The spatial and temporal filters affect the depth image and all that is derived from it, i.e. pointcloud and colorizer. + +to activate the filters, use the argument "filters" and deperate them with a comma: +```bash +roslaunch realsense2_camera rs_camera.launch filters:=temporal,spatial,pointcloud +``` + ## Packages using RealSense ROS Camera | Title | Links | 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..a191f9e967 --- /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 + google-mock + 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..fd616fba78 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(realsense2_camera) +add_compile_options(-std=c++11) option(BUILD_WITH_OPENMP "Use OpenMP" OFF) option(SET_USER_BREAK_AT_STARTUP "Set user wait point in startup (for debug)" OFF) @@ -13,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport tf - dynamic_reconfigure + ddynamic_reconfigure diagnostic_updater ) @@ -32,7 +33,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2) +find_package(realsense2 2.17.1 REQUIRED) if(NOT realsense2_FOUND) message(FATAL_ERROR "\n\n Intel RealSense SDK 2.0 is missing, please install it from https://github.com/IntelRealSense/librealsense/releases\n\n") endif() @@ -64,14 +65,6 @@ include_directories( ${realsense_INCLUDE_DIR} ) -# 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 -) - # RealSense ROS Node catkin_package( LIBRARIES ${PROJECT_NAME} @@ -79,21 +72,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 1a7394a00e..0000000000 --- a/realsense2_camera/cfg/base_d400_params.cfg +++ /dev/null @@ -1,14 +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")) - diff --git a/realsense2_camera/cfg/base_d400_params.py b/realsense2_camera/cfg/base_d400_params.py deleted file mode 100755 index af2f9c0b5a..0000000000 --- a/realsense2_camera/cfg/base_d400_params.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python - - -from dynamic_reconfigure.parameter_generator_catkin import * - -def add_base_params(gen, prefix): - # 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", False) - 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) + "JSON_file_path", str_t, 8, "JSON_file_path", "") - gen.add(str(prefix) + "enable_depth_to_disparity_filter", bool_t, 9, "Enable Depth to Disparity Filter", False) - gen.add(str(prefix) + "enable_spatial_filter", bool_t, 10, "Enable Spatial Filter", False) - gen.add(str(prefix) + "enable_temporal_filter", bool_t, 11, "Enable Temporal Filter", False) - gen.add(str(prefix) + "enable_disparity_to_depth_filter", bool_t, 12, "Enable Disparity to Depth Filter", False) - gen.add(str(prefix) + "spatial_filter_magnitude", double_t, 13, "Spatial Filter Magnitude", 2.0, 1.0, 5.0) - gen.add(str(prefix) + "spatial_filter_smooth_alpha", double_t, 14, "Spatial Filter Smooth Alpha", 0.5, 0.25, 1.0) - gen.add(str(prefix) + "spatial_filter_smooth_delta", double_t, 15, "Spatial Filter Smooth Delta", 20.0, 1.0, 50.0) - spatial_filter_holes_fill_enum = gen.enum([gen.const("SpatialFillHoleDisabled", int_t, 0, "Spatial - Fill Hole Disabled"), - gen.const("2PxielRadius", int_t, 1, "2-Pixel Radius"), - gen.const("4PxielRadius", int_t, 2, "4-Pixel Radius"), - gen.const("8PxielRadius", int_t, 3, "8-Pixel Radius"), - gen.const("16PxielRadius", int_t, 4, "16-Pixel Radius"), - gen.const("Unlimited", int_t, 5, "Unlimited")], "Spatial Filter Holes Fill") - gen.add(str(prefix) + "spatial_filter_holes_fill", int_t, 16, "Spatial Filter Holes Filter", 0, 0, 5, edit_method=spatial_filter_holes_fill_enum) - gen.add(str(prefix) + "temporal_filter_smooth_alpha", double_t, 17, "Temporal Smooth Alpha", 0.4, 0.0, 1.0) - gen.add(str(prefix) + "temporal_filter_smooth_delta", double_t, 18, "Temporal Smooth Delta", 20.0, 1.0, 100.0) - temporal_filter_holes_fill_enum = gen.enum([gen.const("TemporalFillHoleDisabled", int_t, 0, "Temporal - Fill Hole Disabled"), - gen.const("ValidIn8Of8", int_t, 1, "Valid In 8 Of 8"), - gen.const("ValidIn2Oflast3", int_t, 2, "Valid In 2 Of last3"), - gen.const("ValidIn2Oflast4", int_t, 3, "Valid In 2 Of last4"), - gen.const("ValidIn2Of8", int_t, 4, "Valid In 2 Of 8"), - gen.const("ValidIn1Oflast2", int_t, 5, "Valid In 1 Of last2"), - gen.const("ValidIn1Oflast5", int_t, 6, "Valid in 1 Of last5")], "Temporal Filter Holes Fill") - gen.add(str(prefix) + "temporal_filter_holes_fill", int_t, 19, "Temporal Filter Holes Fill", 3, 0, 6, edit_method=temporal_filter_holes_fill_enum) \ 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 83f081f784..0000000000 --- a/realsense2_camera/cfg/rs415_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() - -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, 20, "Enable Auto White Balance", False) -gen.add("rs415_depth_exposure", int_t, 21, "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, 22, "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, 23, "Depth Emitter Enabled", 1, 0, 2, edit_method=emitter_enabled_enum) - -gen.add("rs415_color_backlight_compensation", bool_t, 24, "Backlight Compensation", False) -gen.add("rs415_color_brightness", int_t, 25, "Brightness", 0, -64, 64) -gen.add("rs415_color_contrast", int_t, 26, "Contrast", 50, 0, 100) -gen.add("rs415_color_exposure", int_t, 27, "Exposure", 166, 41, 10000) -gen.add("rs415_color_gain", int_t, 28, "Gain", 64, 0, 128) -gen.add("rs415_color_gamma", int_t, 29, "Gamma", 300, 100, 500) -gen.add("rs415_color_hue", int_t, 30, "Hue", 0, -180, 180) -gen.add("rs415_color_saturation", int_t, 31, "Saturation", 64, 0, 100) -gen.add("rs415_color_sharpness", int_t, 32, "Sharpness", 50, 0, 100) -gen.add("rs415_color_white_balance", int_t, 33, "White Balance", 460, 280, 650) # Multiple value by 10 -gen.add("rs415_color_enable_auto_exposure", bool_t, 34, "Enable Auto Exposure", True) -gen.add("rs415_color_enable_auto_white_balance", bool_t, 35, "Enable Auto White Balance", True) -gen.add("rs415_color_frames_queue_size", int_t, 36, "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, 37, "Power Line Frequency", 3, 0, 3, edit_method=power_line_frequency_enum) -gen.add("rs415_color_auto_exposure_priority", bool_t, 38, "Auto Exposure Priority", False) - -exit(gen.generate(PACKAGE, "realsense2_camera", "rs415_params")) \ No newline at end of file diff --git a/realsense2_camera/cfg/rs435_params.cfg b/realsense2_camera/cfg/rs435_params.cfg deleted file mode 100755 index c920b117ad..0000000000 --- a/realsense2_camera/cfg/rs435_params.cfg +++ /dev/null @@ -1,41 +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, "rs435_") - -# Name Type Level Description Default Min Max -gen.add("rs435_depth_exposure", int_t, 20, "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, 21, "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, 22, "Depth Emitter Enabled", 1, 0, 2, edit_method=emitter_enabled_enum) - -gen.add("rs435_color_backlight_compensation", bool_t, 23, "Backlight Compensation", False) -gen.add("rs435_color_brightness", int_t, 24, "Brightness", 0, -64, 64) -gen.add("rs435_color_contrast", int_t, 25, "Contrast", 50, 0, 100) -gen.add("rs435_color_exposure", int_t, 26, "Exposure", 166, 41, 10000) -gen.add("rs435_color_gain", int_t, 27, "Gain", 64, 0, 128) -gen.add("rs435_color_gamma", int_t, 28, "Gamma", 300, 100, 500) -gen.add("rs435_color_hue", int_t, 29, "Hue", 0, -180, 180) -gen.add("rs435_color_saturation", int_t, 30, "Saturation", 64, 0, 100) -gen.add("rs435_color_sharpness", int_t, 31, "Sharpness", 50, 0, 100) -gen.add("rs435_color_white_balance", int_t, 32, "White Balance", 460, 280, 650) # Multiple value by 10 -gen.add("rs435_color_enable_auto_exposure", bool_t, 33, "Enable Auto Exposure", True) -gen.add("rs435_color_enable_auto_white_balance", bool_t, 34, "Enable Auto White Balance", True) -gen.add("rs435_color_frames_queue_size", int_t, 35, "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, 36, "Power Line Frequency", 3, 0, 3, edit_method=power_line_frequency_enum) -gen.add("rs435_color_auto_exposure_priority", bool_t, 37, "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 e1347e45a5..e71034abb3 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -4,47 +4,17 @@ #pragma once #include "../include/realsense_node_factory.h" -#include -#include -#include -#include +#include +#include #include #include -#include + +#include +#include namespace realsense2_camera { - enum base_depth_param{ - base_depth_gain = 1, - base_depth_enable_auto_exposure, - base_depth_visual_preset, - base_depth_frames_queue_size, - base_depth_error_polling_enabled, - base_depth_output_trigger_enabled, - base_depth_units, - base_JSON_file_path, - base_enable_depth_to_disparity_filter, - base_enable_spatial_filter, - base_enable_temporal_filter, - base_enable_disparity_to_depth_filter, - base_spatial_filter_magnitude, - base_spatial_filter_smooth_alpha, - base_spatial_filter_smooth_delta, - base_spatial_filter_holes_fill, - base_temporal_filter_smooth_alpha, - base_temporal_filter_smooth_delta, - base_temporal_filter_holes_fill, - base_depth_count - }; - - enum filters{ - DEPTH_TO_DISPARITY, - SPATIAL, - TEMPORAL, - DISPARITY_TO_DEPTH - }; - struct FrequencyDiagnostics { FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id) : @@ -69,17 +39,49 @@ namespace realsense2_camera }; typedef std::pair> ImagePublisherWithFrequencyDiagnostics; - /** - Class to encapsulate a filter alongside its options - */ - class filter_options + class NamedFilter { - public: - filter_options(const std::string name, rs2::process_interface& filter); - filter_options(filter_options&& other); - std::string filter_name; // Friendly name of the filter - rs2::process_interface& filter; // The filter in use - std::atomic_bool is_enabled; // A boolean controlled by the user that determines whether to apply the filter or not + public: + std::string _name; + std::shared_ptr _filter; + + public: + NamedFilter(std::string name, std::shared_ptr filter): + _name(name), _filter(filter) + {} + }; + + class PipelineSyncer : public rs2::asynchronous_syncer + { + public: + void operator()(rs2::frame f) const + { + invoke(std::move(f)); + } + }; + + 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 @@ -90,26 +92,43 @@ namespace realsense2_camera rs2::device dev, const std::string& serial_no); + void toggleSensors(bool enabled); virtual void publishTopics() override; - virtual void registerDynamicReconfigCb() override; + virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) override; virtual ~BaseRealSenseNode() {} + public: + enum imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION}; + protected: const uint32_t set_default_dynamic_reconfig_values = 0xffffffff; rs2::device _dev; ros::NodeHandle& _node_handle, _pnh; std::map _sensors; - rs2::spatial_filter spat_filter; // Spatial - edge-preserving spatial smoothing - rs2::temporal_filter temp_filter; // Temporal - reduces temporal noise - rs2::disparity_transform depth_to_disparity{true}; - rs2::disparity_transform disparity_to_depth{false}; - std::vector filters; + std::vector> _ddynrec; private: - struct float3 + class float3 { - float x, y, z; + public: + float x, y, z; + + public: + float3& operator*=(const float& factor) + { + x*=factor; + y*=factor; + z*=factor; + return (*this); + } + float3& operator+=(const float3& other) + { + x+=other.x; + y+=other.y; + z+=other.z; + return (*this); + } }; struct quaternion @@ -117,12 +136,49 @@ namespace realsense2_camera double x, y, z, w; }; + class CIMUHistory + { + public: + enum sensor_name {mGYRO, mACCEL}; + class imuData + { + public: + imuData(const imuData& other): + imuData(other.m_reading, other.m_time) + {}; + imuData(const float3 reading, double time): + m_reading(reading), + m_time(time) + {}; + imuData operator*(const double factor); + imuData operator+(const imuData& other); + public: + BaseRealSenseNode::float3 m_reading; + double m_time; + }; + + private: + size_t m_max_size; + std::map > m_map; + + public: + CIMUHistory(size_t size); + void add_data(sensor_name module, imuData data); + bool is_all_data(sensor_name); + bool is_data(sensor_name); + const std::list& get_data(sensor_name module); + imuData last_data(sensor_name module); + }; + static std::string getNamespaceStr(); void getParameters(); void setupDevice(); + void setupErrorCallback(); void setupPublishers(); 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, @@ -131,8 +187,7 @@ namespace realsense2_camera const std::string& from, const std::string& to); void publishStaticTransforms(); - void publishRgbToDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived); - void publishDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived); + void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset); Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const; rs2_extrinsics getRsExtrinsics(const stream_index_pair& from_stream, const stream_index_pair& to_stream); @@ -153,20 +208,24 @@ namespace realsense2_camera void updateIsFrameArrived(std::map& is_frame_arrived, rs2_stream stream_type, int stream_index); - void publishAlignedDepthToOthers(rs2::frame depth_frame, const std::vector& frames, const ros::Time& t); - - void alignFrame(const rs2_intrinsics& from_intrin, - const rs2_intrinsics& other_intrin, - rs2::frame from_image, - uint32_t output_image_bytes_per_pixel, - const rs2_extrinsics& from_to_other, - std::vector& out_vec); + void publishAlignedDepthToOthers(rs2::frameset frames, const ros::Time& t); + static void callback(const ddynamic_reconfigure::DDMap& map, int, rs2::options sensor); + double FillImuData_Copy(const stream_index_pair stream_index, const CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg); + double FillImuData_LinearInterpolation(const stream_index_pair stream_index, const CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg); + static void ConvertFromOpticalFrameToFrame(float3& data); + void imu_callback(rs2::frame frame); + void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY); + void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name); + rs2_stream rs2_string_to_stream(std::string str); void TemperatureUpdate(diagnostic_updater::DiagnosticStatusWrapper& stat); 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; @@ -178,6 +237,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; @@ -195,13 +255,17 @@ namespace realsense2_camera double _camera_time_base; std::map> _enabled_profiles; - ros::Publisher _pointcloud_xyz_publisher; - ros::Publisher _pointcloud_xyzrgb_publisher; + ros::Publisher _pointcloud_publisher; ros::Time _ros_time_base; bool _align_depth; bool _sync_frames; bool _pointcloud; - rs2::asynchronous_syncer _syncer; + imu_sync_method _imu_sync_method; + std::string _filters_str; + stream_index_pair _pointcloud_texture; + PipelineSyncer _syncer; + std::vector _filters; + std::vector _dev_sensors; std::map _depth_aligned_image; std::map _depth_aligned_encoding; @@ -221,24 +285,7 @@ namespace realsense2_camera int temperature_; };//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 3b98ac4d68..0e893cc13e 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -6,8 +6,8 @@ #include #define REALSENSE_ROS_MAJOR_VERSION 2 -#define REALSENSE_ROS_MINOR_VERSION 0 -#define REALSENSE_ROS_PATCH_VERSION 3 +#define REALSENSE_ROS_MINOR_VERSION 1 +#define REALSENSE_ROS_PATCH_VERSION 4 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) @@ -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,7 @@ namespace realsense2_camera const bool ENABLE_COLOR = true; const bool ENABLE_FISHEYE = true; const bool ENABLE_IMU = true; + const bool HOLD_BACK_IMU_FOR_FRAMES = false; const std::string DEFAULT_BASE_FRAME_ID = "camera_link"; @@ -91,5 +93,8 @@ namespace realsense2_camera const std::string DEFAULT_ALIGNED_DEPTH_TO_INFRA2_FRAME_ID = "camera_aligned_depth_to_infra2_frame"; const std::string DEFAULT_ALIGNED_DEPTH_TO_FISHEYE_FRAME_ID = "camera_aligned_depth_to_fisheye_frame"; + const std::string DEFAULT_UNITE_IMU_METHOD = ""; + const std::string DEFAULT_FILTERS = ""; + using stream_index_pair = std::pair; } // namespace realsense2_camera diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 53544d463f..63397c1f96 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -26,7 +26,6 @@ #include #include - namespace realsense2_camera { const stream_index_pair COLOR{RS2_STREAM_COLOR, 0}; @@ -43,18 +42,11 @@ namespace realsense2_camera const std::vector> HID_STREAMS = {{GYRO, ACCEL}}; - inline void signalHandler(int signum) - { - ROS_INFO_STREAM(strsignal(signum) << " Signal is received! Terminating RealSense Node..."); - ros::shutdown(); - exit(signum); - } - class InterfaceRealSenseNode { public: virtual void publishTopics() = 0; - virtual void registerDynamicReconfigCb() = 0; + virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) = 0; virtual ~InterfaceRealSenseNode() = default; }; @@ -65,12 +57,12 @@ namespace realsense2_camera virtual ~RealSenseNodeFactory() {} private: + static void signalHandler(int signum); rs2::device getDevice(std::string& serial_no); virtual void onInit() override; void tryGetLogSeverity(rs2_log_severity& severity) const; std::unique_ptr _realSenseNode; - rs2::device _device; rs2::context _ctx; }; }//end namespace diff --git a/realsense2_camera/include/rs415_node.h b/realsense2_camera/include/rs415_node.h deleted file mode 100644 index ad1df12252..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 = 20, - 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 95cf03a55a..0000000000 --- a/realsense2_camera/include/rs435_node.h +++ /dev/null @@ -1,49 +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 = 20, - 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_enable_auto_white_balance, - 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/demo_pointcloud.launch b/realsense2_camera/launch/demo_pointcloud.launch index c77b400d68..8779d7c8ff 100644 --- a/realsense2_camera/launch/demo_pointcloud.launch +++ b/realsense2_camera/launch/demo_pointcloud.launch @@ -21,6 +21,7 @@ + diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 0121aca3a6..d63a32318a 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -1,6 +1,7 @@ + @@ -36,14 +37,43 @@ - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -51,6 +81,8 @@ + + @@ -83,18 +115,30 @@ - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/realsense2_camera/launch/opensource_tracking.launch b/realsense2_camera/launch/opensource_tracking.launch new file mode 100644 index 0000000000..44ef4be5db --- /dev/null +++ b/realsense2_camera/launch/opensource_tracking.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [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_aligned_depth.launch b/realsense2_camera/launch/rs_aligned_depth.launch index 111941108a..2b931c0504 100644 --- a/realsense2_camera/launch/rs_aligned_depth.launch +++ b/realsense2_camera/launch/rs_aligned_depth.launch @@ -2,6 +2,7 @@ + @@ -28,17 +29,19 @@ - - + + + + @@ -74,6 +77,8 @@ + + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index fc77ae5a85..ff735b2ca9 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -2,6 +2,7 @@ + @@ -28,20 +29,32 @@ - - + + - - - + + + + + + + + + + + + + + + @@ -73,6 +86,12 @@ + + + + + + diff --git a/realsense2_camera/launch/rs_from_file.launch b/realsense2_camera/launch/rs_from_file.launch index 67ef609fc8..d7c7b83ea7 100644 --- a/realsense2_camera/launch/rs_from_file.launch +++ b/realsense2_camera/launch/rs_from_file.launch @@ -3,7 +3,7 @@ - + @@ -29,14 +29,16 @@ - - + + + + @@ -76,6 +78,7 @@ + diff --git a/realsense2_camera/launch/rs_multiple_devices.launch b/realsense2_camera/launch/rs_multiple_devices.launch index 805e9c4b4f..12174ad2a2 100644 --- a/realsense2_camera/launch/rs_multiple_devices.launch +++ b/realsense2_camera/launch/rs_multiple_devices.launch @@ -1,16 +1,25 @@ - - + + + + + + + - + - + + + - + - + + + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index a5f2b0aef4..394e0dece7 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -39,6 +39,7 @@ Processing enabled by this node: + @@ -71,8 +72,8 @@ Processing enabled by this node: - - + + @@ -112,6 +113,7 @@ Processing enabled by this node: + diff --git a/realsense2_camera/launch/view_d415_model.launch b/realsense2_camera/launch/view_d415_model.launch new file mode 100644 index 0000000000..12fd3aebb7 --- /dev/null +++ b/realsense2_camera/launch/view_d415_model.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/realsense2_camera/meshes/d415.stl b/realsense2_camera/meshes/d415.stl new file mode 100644 index 0000000000..6c6c82b058 Binary files /dev/null and b/realsense2_camera/meshes/d415.stl differ diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index abcdcf83d0..7bd6a0d58c 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,17 +1,17 @@ realsense2_camera - 2.0.5 + 2.1.1 RealSense Camera package allowing access to Intel 3D D400 cameras Sergey Dorodnicov - Itay Carpis + Doron Hirshberg Apache 2.0 http://www.ros.org/wiki/RealSense https://github.com/intel-ros/realsense/issues Sergey Dorodnicov - Itay Carpis + Doron Hirshberg catkin message_generation roscpp @@ -22,7 +22,7 @@ cv_bridge image_transport tf - dynamic_reconfigure + ddynamic_reconfigure diagnostic_updater image_transport cv_bridge @@ -33,7 +33,7 @@ std_msgs message_runtime tf - dynamic_reconfigure + ddynamic_reconfigure diagnostic_updater diff --git a/realsense2_camera/rviz/urdf.rviz b/realsense2_camera/rviz/urdf.rviz index 63fe44b7f3..cb0607b926 100644 --- a/realsense2_camera/rviz/urdf.rviz +++ b/realsense2_camera/rviz/urdf.rviz @@ -7,9 +7,8 @@ Panels: - /Global Options1 - /Status1 - /RobotModel1 - - /RobotModel1/Link/Joint Tree1 - - /RobotModel1/Link/Joint Tree1/base_link1 - - /RobotModel1/Link/Joint Tree1/base_link1/Details1 + - /TF1 + - /TF1/Frames1 Splitter Ratio: 0.636029422 Tree Height: 595 - Class: rviz/Selection @@ -56,59 +55,57 @@ Visualization Manager: Class: rviz/RobotModel Collision Enabled: false Enabled: true - Link/Joint Tree: + Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false - Link Tree Style: Tree of links and joints + Link Tree Style: Links in Alphabetic Order base_link: - Details: - Alpha: 1 - Show Axes: true - Show Trail: false - camera_joint: - Details: - Show Axes: false - Value: true - camera_link: - Details: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_rgb_joint: - Details: - Show Axes: false - camera_rgb_frame: - Details: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_joint: - Details: - Show Axes: false - camera_depth_frame: - Details: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_depth_optical_joint: - Details: - Show Axes: false - camera_depth_optical_frame: - Details: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_rgb_optical_joint: - Details: - Show Axes: false - camera_rgb_optical_frame: - Details: - Alpha: 1 - Show Axes: false - Show Trail: false + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_right_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false Name: RobotModel Robot Description: robot_description TF Prefix: "" @@ -116,22 +113,59 @@ Visualization Manager: Value: true Visual Enabled: true - Class: rviz/TF - Enabled: false + Enabled: true Frame Timeout: 15 Frames: - All Enabled: true - Marker Scale: 1 + All Enabled: false + base_link: + Value: true + camera_bottom_screw_frame: + Value: false + camera_color_frame: + Value: false + camera_color_optical_frame: + Value: false + camera_depth_frame: + Value: false + camera_depth_optical_frame: + Value: false + camera_left_ir_frame: + Value: false + camera_left_ir_optical_frame: + Value: false + camera_link: + Value: true + camera_right_ir_frame: + Value: true + camera_right_ir_optical_frame: + Value: false + Marker Scale: 0.100000001 Name: TF Show Arrows: true Show Axes: true Show Names: true Tree: - {} + base_link: + camera_bottom_screw_frame: + camera_link: + camera_depth_frame: + camera_color_frame: + camera_color_optical_frame: + {} + camera_depth_optical_frame: + {} + camera_left_ir_frame: + camera_left_ir_optical_frame: + {} + camera_right_ir_frame: + camera_right_ir_optical_frame: + {} Update Interval: 0 - Value: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 + Default Light: true Fixed Frame: base_link Frame Rate: 30 Name: root @@ -165,12 +199,13 @@ Visualization Manager: Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.0500000007 + Invert Z Axis: false Name: Current View Near Clip Distance: 0.00999999978 - Pitch: 0 + Pitch: -0.00499998499 Target Frame: Value: ThirdPersonFollower (rviz) - Yaw: 0 + Yaw: 0.685000181 Saved: ~ Window Geometry: Displays: @@ -178,7 +213,7 @@ Window Geometry: Height: 876 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000232000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055f0000003efc0100000002fb0000000800540069006d006501000000000000055f0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000212000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000232000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000055f0000003efc0100000002fb0000000800540069006d006501000000000000055f0000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000212000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index 30ec29679b..4c05632e23 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -2,11 +2,30 @@ import time import rospy from sensor_msgs.msg import Image as msg_Image +from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +import sensor_msgs.point_cloud2 as pc2 import numpy as np from cv_bridge import CvBridge, CvBridgeError import inspect +import ctypes +import struct +def pc2_to_xyzrgb(point): + # Thanks to Panos for his code used in this function. + x, y, z = point[:3] + rgb = point[3] + + # cast float32 to int so that bitwise operations are possible + s = struct.pack('>f', rgb) + i = struct.unpack('>l', s)[0] + # you can get back the float value by the inverse operations + pack = ctypes.c_uint32(i).value + r = (pack & 0x00FF0000) >> 16 + g = (pack & 0x0000FF00) >> 8 + b = (pack & 0x000000FF) + return x, y, z, r, g, b + class CWaitForMessage: def __init__(self, params={}): @@ -19,31 +38,75 @@ def __init__(self, params={}): self.node_name = params.get('node_name', 'rs2_listener') self.bridge = CvBridge() - self.themes = {'depthStream': {'topic': '', 'callback': self.imageDepthCallback, 'msg_type': msg_Image}, - 'colorStream': {'topic': '/camera/color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}} + self.themes = {'depthStream': {'topic': '/camera/depth/image_rect_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, + 'colorStream': {'topic': '/camera/color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, + 'pointscloud': {'topic': '/camera/depth/color/points', 'callback': self.pointscloudCallback, 'msg_type': msg_PointCloud2}, + 'alignedDepthInfra1': {'topic': '/camera/aligned_depth_to_infra1/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, + 'alignedDepthColor': {'topic': '/camera/aligned_depth_to_color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, + } self.func_data = dict() - def imageColorCallback(self, data): - self.prev_time = time.time() - func_name = inspect.stack()[0][3] - theme_name = [key for key, value in self.themes.items() if func_name in value['callback'].__name__][0] - self.func_data[theme_name].setdefault('avg', []) - self.func_data[theme_name].setdefault('num_channels', []) - - try: - cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") - except CvBridgeError as e: - print(e) - return - (rows, cols, channels) = cv_image.shape - pyimg = np.asarray(cv_image) - self.func_data[theme_name]['avg'].append(pyimg.mean()) - self.func_data[theme_name]['num_channels'].append(channels) + def imageColorCallback(self, theme_name): + def _imageColorCallback(data): + self.prev_time = time.time() + self.func_data[theme_name].setdefault('avg', []) + self.func_data[theme_name].setdefault('ok_percent', []) + self.func_data[theme_name].setdefault('num_channels', []) + self.func_data[theme_name].setdefault('shape', []) + self.func_data[theme_name].setdefault('reported_size', []) + + try: + cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) + except CvBridgeError as e: + print(e) + return + channels = cv_image.shape[2] if len(cv_image.shape) > 2 else 1 + pyimg = np.asarray(cv_image) + + ok_number = (pyimg != 0).sum() + + self.func_data[theme_name]['avg'].append(pyimg.sum() / ok_number) + self.func_data[theme_name]['ok_percent'].append(float(ok_number) / (pyimg.shape[0] * pyimg.shape[1]) / channels) + self.func_data[theme_name]['num_channels'].append(channels) + self.func_data[theme_name]['shape'].append(cv_image.shape) + self.func_data[theme_name]['reported_size'].append((data.width, data.height, data.step)) + return _imageColorCallback def imageDepthCallback(self, data): pass + def pointscloudCallback(self, theme_name): + def _pointscloudCallback(data): + self.prev_time = time.time() + print 'Got pointcloud: %d, %d' % (data.width, data.height) + + self.func_data[theme_name].setdefault('frame_counter', 0) + self.func_data[theme_name].setdefault('avg', []) + self.func_data[theme_name].setdefault('size', []) + self.func_data[theme_name].setdefault('width', []) + self.func_data[theme_name].setdefault('height', []) + # until parsing pointcloud is done in real time, I'll use only the first frame. + self.func_data[theme_name]['frame_counter'] += 1 + + if self.func_data[theme_name]['frame_counter'] == 1: + # Known issue - 1st pointcloud published has invalid texture. Skip 1st frame. + return + + if len(self.func_data[theme_name]['width']) > 0: + return + + try: + points = np.array([pc2_to_xyzrgb(pp) for pp in pc2.read_points(data, skip_nans=True, field_names=("x", "y", "z", "rgb")) if pp[0] > 0]) + except Exception as e: + print(e) + return + self.func_data[theme_name]['avg'].append(points.mean(0)) + self.func_data[theme_name]['size'].append(len(points)) + self.func_data[theme_name]['width'].append(data.width) + self.func_data[theme_name]['height'].append(data.height) + return _pointscloudCallback + def wait_for_message(self, params): topic = params['topic'] print 'connect to ROS with name: %s' % self.node_name @@ -77,7 +140,7 @@ def wait_for_messages(self, themes): for theme_name in themes: theme = self.themes[theme_name] rospy.loginfo('Subscribing %s on topic: %s' % (theme_name, theme['topic'])) - self.func_data[theme_name]['sub'] = rospy.Subscriber(theme['topic'], theme['msg_type'], theme['callback']) + self.func_data[theme_name]['sub'] = rospy.Subscriber(theme['topic'], theme['msg_type'], theme['callback'](theme_name)) self.prev_time = time.time() break_timeout = False @@ -104,8 +167,9 @@ def main(): if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv: print 'USAGE:' print '------' - print 'rs2_listener.py [Options]' + print 'rs2_listener.py [Options]' print 'example: rs2_listener.py /camera/color/image_raw --time 1532423022.044515610 --timeout 3' + print 'example: rs2_listener.py pointscloud' print '' print 'Application subscribes on , wait for the first message matching [Options].' print 'When found, prints the timestamp.' @@ -120,8 +184,7 @@ def main(): # wanted_seq = 58250 wanted_topic = sys.argv[1] - msg_params = {'topic': wanted_topic} - + msg_params = {} for idx in range(2, len(sys.argv)): if sys.argv[idx] == '-s': msg_params['seq'] = int(sys.argv[idx + 1]) @@ -131,9 +194,14 @@ def main(): msg_params['timeout_secs'] = int(sys.argv[idx + 1]) msg_retriever = CWaitForMessage(msg_params) - res = msg_retriever.wait_for_message() - - rospy.loginfo('Got message: %s' % res.header) + if '/' in wanted_topic: + msg_params = {'topic': wanted_topic} + res = msg_retriever.wait_for_message(msg_params) + rospy.loginfo('Got message: %s' % res.header) + else: + themes = [wanted_topic] + res = msg_retriever.wait_for_messages(themes) + print res if __name__ == '__main__': diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index 41a6007b1c..1e81c79dad 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -9,30 +9,60 @@ import subprocess -def ImageColorGetData(rec_filename): +def ImageGetData(rec_filename, topic): bag = rosbag.Bag(rec_filename) bridge = CvBridge() all_avg = [] + ok_percent = [] res = dict() - for topic, msg, t in bag.read_messages(topics='/device_0/sensor_1/Color_0/image/data'): + for topic, msg, t in bag.read_messages(topics=topic): try: - cv_image = bridge.imgmsg_to_cv2(msg, "bgr8") + cv_image = bridge.imgmsg_to_cv2(msg, msg.encoding) except CvBridgeError as e: print(e) continue pyimg = np.asarray(cv_image) - all_avg.append(pyimg.mean()) + ok_number = (pyimg != 0).sum() + ok_percent.append(float(ok_number) / (pyimg.shape[0] * pyimg.shape[1])) + all_avg.append(pyimg.sum() / ok_number) all_avg = np.array(all_avg) - (rows, cols, channels) = cv_image.shape + channels = cv_image.shape[2] if len(cv_image.shape) > 2 else 1 res['num_channels'] = channels + res['shape'] = cv_image.shape res['avg'] = all_avg.mean() - res['epsilon'] = max(all_avg.max() - res['avg'], res['avg'] - all_avg.min()) + res['ok_percent'] = {'value': (np.array(ok_percent).mean()) / channels, 'epsilon': 0.01} + res['epsilon'] = max(all_avg.max() - res['avg'], res['avg'] - all_avg.min()) + res['reported_size'] = [msg.width, msg.height, msg.step] return res +def ImageColorGetData(rec_filename): + return ImageGetData(rec_filename, '/device_0/sensor_1/Color_0/image/data') + + +def ImageDepthGetData(rec_filename): + return ImageGetData(rec_filename, '/device_0/sensor_0/Depth_0/image/data') + + +def ImageDepthInColorShapeGetData(rec_filename): + gt_data = ImageDepthGetData(rec_filename) + color_data = ImageColorGetData(rec_filename) + gt_data['shape'] = color_data['shape'][:2] + gt_data['reported_size'] = color_data['reported_size'] + gt_data['reported_size'][2] = gt_data['reported_size'][0]*2 + gt_data['ok_percent']['epsilon'] *= 3 + return gt_data + +def ImageDepthGetData_decimation(rec_filename): + gt_data = ImageDepthGetData(rec_filename) + gt_data['shape'] = [x/2 for x in gt_data['shape']] + gt_data['reported_size'] = [x/2 for x in gt_data['reported_size']] + gt_data['epsilon'] *= 3 + return gt_data + def ImageColorTest(data, gt_data): # check that all data['num_channels'] are the same as gt_data['num_channels'] and that avg value of all # images are within epsilon of gt_data['avg'] @@ -41,51 +71,107 @@ def ImageColorTest(data, gt_data): print 'Expect %d channels. Got %d channels.' % (channels[0], gt_data['num_channels']) if len(channels) > 1 or channels[0] != gt_data['num_channels']: return False - print 'Expect avarage of %.3f. Got avarage of %.3f.' % (np.array(data['avg']).mean(), gt_data['avg'].mean()) - if abs(np.array(data['avg']) - gt_data['avg']).max() > gt_data['epsilon']: + print 'Expected all received images to be the same shape. Got %s' % str(set(data['shape'])) + if len(set(data['shape'])) > 1: + return False + print 'Expected shape to be %s. Got %s' % (gt_data['shape'], list(set(data['shape']))[0]) + if (np.array(list(set(data['shape']))[0]) != np.array(gt_data['shape'])).any(): return False + print 'Expected header [width, height, step] to be %s. Got %s' % (gt_data['reported_size'], list(set(data['reported_size']))[0]) + if (np.array(list(set(data['reported_size']))[0]) != np.array(gt_data['reported_size'])).any(): + return False + print 'Expect average of %.3f (+-%.3f). Got average of %.3f.' % (gt_data['avg'].mean(), gt_data['epsilon'], np.array(data['avg']).mean()) + if abs(np.array(data['avg']).mean() - gt_data['avg'].mean()) > gt_data['epsilon']: + return False + + print 'Expect no holes percent > %.3f. Got %.3f.' % (gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon'], np.array(data['ok_percent']).mean()) + if np.array(data['ok_percent']).mean() < gt_data['ok_percent']['value']-gt_data['ok_percent']['epsilon']: + return False + except Exception as e: print 'Test Failed: %s' % e return False return True +def ImageColorTest_3epsilon(data, gt_data): + gt_data['epsilon'] *= 3 + return ImageColorTest(data, gt_data) + + +def PointCloudTest(data, gt_data): + print 'Expect image size %d(+-%d), %d. Got %d, %d.' % (gt_data['width'][0], gt_data['width'][1], gt_data['height'][0], data['width'][0], data['height'][0]) + if abs(data['width'][0] - gt_data['width'][0]) > gt_data['width'][1] or data['height'][0] != gt_data['height'][0]: + return False + print 'Expect average position of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][:3], gt_data['epsilon'][0], data['avg'][0][:3]) + if abs(data['avg'][0][:3] - gt_data['avg'][0][:3]).max() > gt_data['epsilon'][0]: + return False + print 'Expect average color of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][3:], gt_data['epsilon'][1], data['avg'][0][3:]) + if abs(data['avg'][0][3:] - gt_data['avg'][0][3:]).max() > gt_data['epsilon'][1]: + return False + + return True + + test_types = {'vis_avg': {'listener_theme': 'colorStream', 'data_func': ImageColorGetData, 'test_func': ImageColorTest}, + 'depth_avg': {'listener_theme': 'depthStream', + 'data_func': ImageDepthGetData, + 'test_func': ImageColorTest}, 'no_file': {'listener_theme': 'colorStream', 'data_func': lambda x: None, 'test_func': lambda x, y: not ImageColorTest(x, y)}, + 'pointscloud_avg': {'listener_theme': 'pointscloud', + 'data_func': lambda x: {'width': [776534, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 65, 88, 95])], 'epsilon': [0.02, 2]}, + 'test_func': PointCloudTest}, + 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1', + 'data_func': ImageDepthGetData, + 'test_func': ImageColorTest}, + 'align_depth_color': {'listener_theme': 'alignedDepthColor', + 'data_func': ImageDepthInColorShapeGetData, + 'test_func': ImageColorTest_3epsilon}, + 'depth_avg_decimation': {'listener_theme': 'depthStream', + 'data_func': ImageDepthGetData_decimation, + 'test_func': ImageColorTest}, + 'align_depth_ir1_decimation': {'listener_theme': 'alignedDepthInfra1', + 'data_func': ImageDepthGetData, + 'test_func': ImageColorTest}, } def run_test(test, listener_res): - # gather ground truth with test_types[test['type']]['data_func'] and recording from test['rec_filename'] + # gather ground truth with test_types[test['type']]['data_func'] and recording from test['rosbag_filename'] # return results from test_types[test['type']]['test_func'] test_type = test_types[test['type']] - gt_data = test_type['data_func'](test['rec_filename']) + gt_data = test_type['data_func'](test['params']['rosbag_filename']) return test_type['test_func'](listener_res[test_type['listener_theme']], gt_data) def print_results(results): - print '{:^20s}'.format('TEST RESULTS') - print '-'*20 - print '{:<10s}{:>10s}'.format('test name', 'score') - print '-'*9 + ' '*2 + '-'*9 - print '\n'.join(['{:<10s}{:>10s}'.format(test[0], 'OK' if test[1] else 'FAILED') for test in results]) + title = 'TEST RESULTS' + headers = ['test name', 'score'] + col_0_width = max([len(headers[0])] + [len(test[0]) for test in results]) + 1 + col_1_width = max([len(headers[1]), len('OK'), len('FAILED')]) + 1 + total_width = col_0_width + col_1_width + print ('{:^%ds}'%total_width).format(title) + print '-'*total_width + print ('{:<%ds}{:>%ds}' % (col_0_width, col_1_width)).format('test name', 'score') + print '-'*(col_0_width-1) + ' '*2 + '-'*(col_1_width-1) + print '\n'.join([('{:<%ds}{:>%ds}' % (col_0_width, col_1_width)).format(test[0], 'OK' if test[1] else 'FAILED') for test in results]) print def run_tests(tests): msg_params = {'timeout_secs': 5} results = [] - rec_filenames = set([os.path.abspath(test['rec_filename']) for test in tests]) - for rec in rec_filenames: - rec_tests = [test for test in tests if os.path.abspath(test['rec_filename']) == rec] + params_strs = set([test['params_str'] for test in tests]) + for params_str in params_strs: + rec_tests = [test for test in tests if test['params_str'] == params_str] themes = [test_types[test['type']]['listener_theme'] for test in rec_tests] msg_retriever = CWaitForMessage(msg_params) print 'Starting ROS' - p_wrapper = subprocess.Popen(['roslaunch', 'realsense2_camera', 'rs_from_file.launch', 'rosbag_filename:=%s' % rec], stdout=None, stderr=None) + p_wrapper = subprocess.Popen(['roslaunch', 'realsense2_camera', 'rs_from_file.launch'] + params_str.split(' '), stdout=None, stderr=None) listener_res = msg_retriever.wait_for_messages(themes) print 'Killing ROS' p_wrapper.terminate() @@ -103,9 +189,24 @@ def run_tests(tests): return results def main(): - all_tests = [{'name': 'vis_avg_1', 'type': 'no_file', 'rec_filename': '/home/non_existent_file.txt'}, - # {'name': 'vis_avg_2', 'type': 'vis_avg', 'rec_filename': '/home/doronhi/Downloads/checkerboard_30cm.bag'}, - {'name': 'vis_avg_2', 'type': 'vis_avg', 'rec_filename': './records/outdoors.bag'}] + all_tests = [{'name': 'vis_avg_1', 'type': 'no_file', 'params': {'rosbag_filename': '/home/non_existent_file.txt'}}, + # {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': '/home/doronhi/Downloads/checkerboard_30cm.bag'}}, + {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': './records/outdoors.bag'}}, + {'name': 'depth_avg_1', 'type': 'depth_avg', 'params': {'rosbag_filename': './records/outdoors.bag'}}, + {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': './records/outdoors.bag', 'enable_pointcloud': 'true'}}, + {'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': './records/outdoors.bag', 'enable_pointcloud': 'true'}}, + {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': './records/outdoors.bag', 'align_depth': 'true'}}, + {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': './records/outdoors.bag', 'align_depth': 'true'}}, + {'name': 'depth_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': './records/outdoors.bag', 'filters': 'decimation', 'align_depth': 'true'}}, + {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': './records/outdoors.bag', 'filters': 'decimation'}}, + {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': './records/outdoors.bag', 'filters': 'decimation', 'align_depth': 'true'}} + + ] + + # Normalize parameters: + for test in all_tests: + test['params']['rosbag_filename'] = os.path.abspath(test['params']['rosbag_filename']) + test['params_str'] = ' '.join([key + ':=' + test['params'][key] for key in sorted(test['params'].keys())]) if len(sys.argv) < 2 or '--help' in sys.argv or '/?' in sys.argv: print 'USAGE:' diff --git a/realsense2_camera/scripts/set_cams_transforms.py b/realsense2_camera/scripts/set_cams_transforms.py new file mode 100644 index 0000000000..c649484df2 --- /dev/null +++ b/realsense2_camera/scripts/set_cams_transforms.py @@ -0,0 +1,136 @@ +import rospy +import sys +import tf +import tf2_ros +import geometry_msgs.msg + +import termios +import tty +import os +import time +import math +import json + + +def getch(): + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + try: + tty.setraw(sys.stdin.fileno()) + ch = sys.stdin.read(1) + + finally: + termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) + return ch + + +def main(): + return + + +def print_status(status): + sys.stdout.write('%-8s%-8s%-8s%-40s\r' % (status['mode'], status[status['mode']]['value'], status[status['mode']]['step'], status['message'])) + + +def publish_status(broadcaster, status): + static_transformStamped = geometry_msgs.msg.TransformStamped() + static_transformStamped.header.stamp = rospy.Time.now() + static_transformStamped.header.frame_id = from_cam + + static_transformStamped.child_frame_id = to_cam + static_transformStamped.transform.translation.x = status['x']['value'] + static_transformStamped.transform.translation.y = status['y']['value'] + static_transformStamped.transform.translation.z = status['z']['value'] + + quat = tf.transformations.quaternion_from_euler(math.radians(status['roll']['value']), + math.radians(status['pitch']['value']), + math.radians(status['azimuth']['value'])) + static_transformStamped.transform.rotation.x = quat[0] + static_transformStamped.transform.rotation.y = quat[1] + static_transformStamped.transform.rotation.z = quat[2] + static_transformStamped.transform.rotation.w = quat[3] + broadcaster.sendTransform(static_transformStamped) + + +if __name__ == '__main__': + if len(sys.argv) < 3: + print 'USAGE:' + print 'set_cams_transforms.py from_cam to_cam x y z azimuth pitch roll' + print 'x, y, z: in meters' + print 'azimuth, pitch, roll: in degrees' + print + print 'If parameters are not given read last used parameters.' + print + print '[OPTIONS]' + print '--file : if given, default values are loaded from file' + sys.exit(-1) + + from_cam, to_cam = sys.argv[1:3] + try: + filename = sys.argv[sys.argv.index('--file')+1] + print 'Using file %s' % os.path.abspath(filename) + except: + filename = os.path.join(os.path.dirname(__file__), '_set_cams_info_file.txt') + print 'Using default file %s' % os.path.abspath(filename) + + if len(sys.argv) >= 9: + x, y, z, yaw, pitch, roll = [float(arg) for arg in sys.argv[3:10]] + status = {'mode': 'pitch', + 'x': {'value': x, 'step': 0.1}, + 'y': {'value': y, 'step': 0.1}, + 'z': {'value': z, 'step': 0.1}, + 'azimuth': {'value': yaw, 'step': 1}, + 'pitch': {'value': pitch, 'step': 1}, + 'roll': {'value': roll, 'step': 1}, + 'message': ''} + print 'Use given initial values.' + else: + try: + status = json.load(open(filename, 'r')) + print 'Read initial values from file.' + except IOError as e: + print 'Failed reading initial parameters from file %s' % filename + print 'Initial parameters must be given for initial run or if an un-initialized file has been given.' + sys.exit(-1) + + rospy.init_node('my_static_tf2_broadcaster') + broadcaster = tf2_ros.StaticTransformBroadcaster() + + print + print 'Press the following keys to change mode: x, y, z, (a)zimuth, (p)itch, (r)oll' + print 'For each mode, press 6 to increase by step and 4 to decrease' + print 'Press + to multiply step by 2 or - to divide' + print + print 'Press Q to quit' + print + + status_keys = [key[0] for key in status.keys()] + print '%-8s%-8s%-8s%s' % ('Mode', 'value', 'step', 'message') + print_status(status) + publish_status(broadcaster, status) + while True: + kk = getch() + status['message'] = '' + try: + key_idx = status_keys.index(kk) + status['mode'] = status.keys()[key_idx] + except ValueError as e: + if kk.upper() == 'Q': + sys.stdout.write('\n') + exit(0) + elif kk == '4': + status[status['mode']]['value'] -= status[status['mode']]['step'] + elif kk == '6': + status[status['mode']]['value'] += status[status['mode']]['step'] + elif kk == '-': + status[status['mode']]['step'] /= 2.0 + elif kk == '+': + status[status['mode']]['step'] *= 2.0 + else: + status['message'] = 'Invalid key:' + kk + + print_status(status) + publish_status(broadcaster, status) + json.dump(status, open(filename, 'w'), indent=4) + + #rospy.spin() diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 904f9ecba1..9d1c3b03b3 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1,7 +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() { @@ -80,32 +140,182 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _unit_step_size[ACCEL] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size _stream_name[ACCEL] = "accel"; - // TODO: Improve the pipeline to accept decimation filter - // TODO: Provide disparity map if requested - filters.emplace_back("Depth_to_Disparity", depth_to_disparity); - filters.emplace_back("Spatial", spat_filter); - filters.emplace_back("Temporal", temp_filter); - filters.emplace_back("Disparity_to_Depth", disparity_to_depth); - - filters[0].is_enabled = false; - filters[1].is_enabled = false; - filters[2].is_enabled = false; - filters[3].is_enabled = false; + +} + +void BaseRealSenseNode::toggleSensors(bool enabled) +{ + for (auto it=_sensors.begin(); it != _sensors.end(); it++) + { + auto& sens = _sensors[it->first]; + try + { + if (enabled) + sens.start(_syncer); + else + sens.stop(); + } + catch(const rs2::wrong_api_call_sequence_error& ex) + { + ROS_DEBUG_STREAM("toggleSensors: " << ex.what()); + } + } +} + +void BaseRealSenseNode::setupErrorCallback() +{ + for (auto&& s : _dev.query_sensors()) + { + s.set_notifications_callback([&](const rs2::notification& n) + { + std::vector error_strings({"RT IC2 Config error", + "Motion Module force pause"}); + if (n.get_severity() >= RS2_LOG_SEVERITY_ERROR) + { + ROS_WARN_STREAM("Hardware Notification:" << n.get_description() << "," << n.get_timestamp() << "," << n.get_severity() << "," << n.get_category()); + } + if (error_strings.end() != find_if(error_strings.begin(), error_strings.end(), [&n] (std::string err) + {return (n.get_description().find(err) != std::string::npos); })) + { + ROS_ERROR_STREAM("Hardware Reset is needed. use option: initial_reset:=true"); + // _dev.hardware_reset(); + } + }); + } } void BaseRealSenseNode::publishTopics() { getParameters(); setupDevice(); + setupErrorCallback(); setupPublishers(); setupStreams(); + setupFilters(); publishStaticTransforms(); 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) +{ + 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) { - ROS_INFO("Dynamic reconfig parameters is not implemented in the base node."); + 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(), '-', '_'); + 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) +{ + if (str == "RS2_STREAM_ANY") + return RS2_STREAM_ANY; + if (str == "RS2_STREAM_COLOR") + return RS2_STREAM_COLOR; + if (str == "RS2_STREAM_INFRARED") + return RS2_STREAM_INFRARED; + if (str == "RS2_STREAM_FISHEYE") + return RS2_STREAM_FISHEYE; + throw std::runtime_error("Unknown stream string " + str); } void BaseRealSenseNode::getParameters() @@ -114,8 +324,17 @@ void BaseRealSenseNode::getParameters() _pnh.param("align_depth", _align_depth, ALIGN_DEPTH); _pnh.param("enable_pointcloud", _pointcloud, POINTCLOUD); + std::string pc_texture_stream(""); + int pc_texture_idx; + _pnh.param("pointcloud_texture_stream", pc_texture_stream, std::string("RS2_STREAM_COLOR")); + _pnh.param("pointcloud_texture_index", pc_texture_idx, 0); + _pointcloud_texture = stream_index_pair{rs2_string_to_stream(pc_texture_stream), pc_texture_idx}; + + _pnh.param("filters", _filters_str, DEFAULT_FILTERS); + _pointcloud |= (_filters_str.find("pointcloud") != std::string::npos); + _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES); - if (_pointcloud || _align_depth) + if (_pointcloud || _align_depth || _filters_str.size() > 0) _sync_frames = true; _pnh.param("json_file_path", _json_file_path, std::string("")); @@ -124,36 +343,39 @@ void BaseRealSenseNode::getParameters() _pnh.param("depth_height", _height[DEPTH], DEPTH_HEIGHT); _pnh.param("depth_fps", _fps[DEPTH], DEPTH_FPS); _pnh.param("enable_depth", _enable[DEPTH], ENABLE_DEPTH); - _aligned_depth_images[DEPTH].resize(_width[DEPTH] * _height[DEPTH] * _unit_step_size[DEPTH]); _pnh.param("infra1_width", _width[INFRA1], INFRA1_WIDTH); _pnh.param("infra1_height", _height[INFRA1], INFRA1_HEIGHT); _pnh.param("infra1_fps", _fps[INFRA1], INFRA1_FPS); _pnh.param("enable_infra1", _enable[INFRA1], ENABLE_INFRA1); - _aligned_depth_images[INFRA1].resize(_width[DEPTH] * _height[DEPTH] * _unit_step_size[DEPTH]); _pnh.param("infra2_width", _width[INFRA2], INFRA2_WIDTH); _pnh.param("infra2_height", _height[INFRA2], INFRA2_HEIGHT); _pnh.param("infra2_fps", _fps[INFRA2], INFRA2_FPS); _pnh.param("enable_infra2", _enable[INFRA2], ENABLE_INFRA2); - _aligned_depth_images[INFRA2].resize(_width[DEPTH] * _height[DEPTH] * _unit_step_size[DEPTH]); _pnh.param("color_width", _width[COLOR], COLOR_WIDTH); _pnh.param("color_height", _height[COLOR], COLOR_HEIGHT); _pnh.param("color_fps", _fps[COLOR], COLOR_FPS); _pnh.param("enable_color", _enable[COLOR], ENABLE_COLOR); - _aligned_depth_images[COLOR].resize(_width[DEPTH] * _height[DEPTH] * _unit_step_size[DEPTH]); _pnh.param("fisheye_width", _width[FISHEYE], FISHEYE_WIDTH); _pnh.param("fisheye_height", _height[FISHEYE], FISHEYE_HEIGHT); _pnh.param("fisheye_fps", _fps[FISHEYE], FISHEYE_FPS); _pnh.param("enable_fisheye", _enable[FISHEYE], ENABLE_FISHEYE); - _aligned_depth_images[FISHEYE].resize(_width[DEPTH] * _height[DEPTH] * _unit_step_size[DEPTH]); _pnh.param("gyro_fps", _fps[GYRO], GYRO_FPS); _pnh.param("accel_fps", _fps[ACCEL], ACCEL_FPS); _pnh.param("enable_imu", _enable[GYRO], ENABLE_IMU); _pnh.param("enable_imu", _enable[ACCEL], ENABLE_IMU); + std::string unite_imu_method_str(""); + _pnh.param("unite_imu_method", unite_imu_method_str, DEFAULT_UNITE_IMU_METHOD); + if (unite_imu_method_str == "linear_interpolation") + _imu_sync_method = imu_sync_method::LINEAR_INTERPOLATION; + else if (unite_imu_method_str == "copy") + _imu_sync_method = imu_sync_method::COPY; + else + _imu_sync_method = imu_sync_method::NONE; _pnh.param("base_frame_id", _base_frame_id, DEFAULT_BASE_FRAME_ID); _pnh.param("depth_frame_id", _frame_id[DEPTH], DEFAULT_DEPTH_FRAME_ID); @@ -169,13 +391,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 (_imu_sync_method > imu_sync_method::NONE) + { + _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() @@ -223,10 +455,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) @@ -362,8 +594,7 @@ void BaseRealSenseNode::setupPublishers() if (stream == DEPTH && _pointcloud) { - _pointcloud_xyz_publisher = _node_handle.advertise("depth/points", 1); - _pointcloud_xyzrgb_publisher = _node_handle.advertise("depth/color/points", 1); + _pointcloud_publisher = _node_handle.advertise("depth/color/points", 1); } } } @@ -392,75 +623,27 @@ 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 (_imu_sync_method > imu_sync_method::NONE && _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]) - { - _imu_publishers[ACCEL] = _node_handle.advertise("accel/sample", 100); - _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); + _info_publisher[GYRO] = _node_handle.advertise("imu_info", 1, true); } -} - -void BaseRealSenseNode::alignFrame(const rs2_intrinsics& from_intrin, - const rs2_intrinsics& other_intrin, - rs2::frame from_image, - uint32_t output_image_bytes_per_pixel, - const rs2_extrinsics& from_to_other, - std::vector& out_vec) -{ - static const auto meter_to_mm = 0.001f; - uint8_t* p_out_frame = out_vec.data(); - - static const auto blank_color = 0x00; - memset(p_out_frame, blank_color, other_intrin.height * other_intrin.width * output_image_bytes_per_pixel); - - auto p_from_frame = reinterpret_cast(from_image.get_data()); - auto from_stream_type = from_image.get_profile().stream_type(); - float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)?_depth_scale_meters:1.f); -#pragma omp parallel for schedule(dynamic) - for (int from_y = 0; from_y < from_intrin.height; ++from_y) + else { - int from_pixel_index = from_y * from_intrin.width; - for (int from_x = 0; from_x < from_intrin.width; ++from_x, ++from_pixel_index) + if (_enable[GYRO]) { - // Skip over depth pixels with the value of zero - float depth = (from_stream_type == RS2_STREAM_DEPTH)?(depth_units * ((const uint16_t*)p_from_frame)[from_pixel_index]): 1.f; - if (depth) - { - // Map the top-left corner of the depth pixel onto the other image - float from_pixel[2] = { from_x - 0.5f, from_y - 0.5f }, from_point[3], other_point[3], other_pixel[2]; - rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth); - rs2_transform_point_to_point(other_point, &from_to_other, from_point); - rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point); - const int other_x0 = static_cast(other_pixel[0] + 0.5f); - const int other_y0 = static_cast(other_pixel[1] + 0.5f); - - // Map the bottom-right corner of the depth pixel onto the other image - from_pixel[0] = from_x + 0.5f; from_pixel[1] = from_y + 0.5f; - rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth); - rs2_transform_point_to_point(other_point, &from_to_other, from_point); - rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point); - const int other_x1 = static_cast(other_pixel[0] + 0.5f); - const int other_y1 = static_cast(other_pixel[1] + 0.5f); - - if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) - continue; - - for (int y = other_y0; y <= other_y1; ++y) - { - for (int x = other_x0; x <= other_x1; ++x) - { - int out_pixel_index = y * other_intrin.width + x; - uint16_t* p_from_depth_frame = (uint16_t*)p_from_frame; - uint16_t* p_out_depth_frame = (uint16_t*)p_out_frame; - p_out_depth_frame[out_pixel_index] = p_from_depth_frame[from_pixel_index] * (depth_units / meter_to_mm); - } - } - } + _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); } } } @@ -478,49 +661,41 @@ void BaseRealSenseNode::updateIsFrameArrived(std::map& } } -void BaseRealSenseNode::publishAlignedDepthToOthers(rs2::frame depth_frame, const std::vector& frames, const ros::Time& t) +void BaseRealSenseNode::publishAlignedDepthToOthers(rs2::frameset frames, const ros::Time& t) { - for (auto&& other_frame : frames) + for (auto it = frames.begin(); it != frames.end(); ++it) { - auto stream_type = other_frame.get_profile().stream_type(); + auto frame = (*it); + auto stream_type = frame.get_profile().stream_type(); + if (RS2_STREAM_DEPTH == stream_type) continue; - auto stream_index = other_frame.get_profile().stream_index(); + auto stream_index = frame.get_profile().stream_index(); stream_index_pair sip{stream_type, stream_index}; auto& info_publisher = _depth_aligned_info_publisher.at(sip); auto& image_publisher = _depth_aligned_image_publishers.at(sip); + if(0 != info_publisher.getNumSubscribers() || 0 != image_publisher.first.getNumSubscribers()) { - auto from_image_frame = depth_frame.as(); - auto& out_vec = _aligned_depth_images[sip]; - alignFrame(_stream_intrinsics[DEPTH], _stream_intrinsics[sip], - depth_frame, from_image_frame.get_bytes_per_pixel(), - _depth_to_other_extrinsics[sip], out_vec); - - auto& from_image = _depth_aligned_image[sip]; - from_image.data = out_vec.data(); + rs2::align align(stream_type); + rs2::frameset processed = frames.apply_filter(align); + rs2::depth_frame aligned_depth_frame = processed.get_depth_frame(); - publishFrame(depth_frame, t, sip, + publishFrame(aligned_depth_frame, t, sip, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, _depth_aligned_seq, _depth_aligned_camera_info, _optical_frame_id, - _depth_aligned_encoding, false); + _depth_aligned_encoding); } } } void BaseRealSenseNode::filterFrame(rs2::frame& frame) { - for (auto&& filter : filters) - { - if (filter.is_enabled) - { - frame = filter.filter.process(frame); - } - } + } void BaseRealSenseNode::enable_devices() @@ -554,7 +729,7 @@ void BaseRealSenseNode::enable_devices() _enabled_profiles[elem].push_back(profile); - _image[elem] = cv::Mat(_width[elem], _height[elem], _image_format[elem], cv::Scalar(0, 0, 0)); + _image[elem] = cv::Mat(_height[elem], _width[elem], _image_format[elem], cv::Scalar(0, 0, 0)); ROS_INFO_STREAM(_stream_name[elem] << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem]); break; @@ -578,11 +753,353 @@ void BaseRealSenseNode::enable_devices() { for (auto& profiles : _enabled_profiles) { - _depth_aligned_image[profiles.first] = cv::Mat(_width[DEPTH], _height[DEPTH], _image_format[DEPTH], cv::Scalar(0, 0, 0)); + _depth_aligned_image[profiles.first] = cv::Mat(_height[DEPTH], _width[DEPTH], _image_format[DEPTH], cv::Scalar(0, 0, 0)); } } } +void BaseRealSenseNode::setupFilters() +{ + std::vector filters_str; + boost::split(filters_str, _filters_str, [](char c){return c == ',';}); + bool use_disparity_filter(false); + bool use_colorizer_filter(false); + for (std::vector::const_iterator s_iter=filters_str.begin(); s_iter!=filters_str.end(); s_iter++) + { + if ((*s_iter) == "colorizer") + { + use_colorizer_filter = true; + } + else if ((*s_iter) == "disparity") + { + use_disparity_filter = true; + } + else if ((*s_iter) == "spatial") + { + ROS_INFO("Add Filter: spatial"); + _filters.push_back(NamedFilter("spatial", std::make_shared())); + } + else if ((*s_iter) == "temporal") + { + ROS_INFO("Add Filter: temporal"); + _filters.push_back(NamedFilter("temporal", std::make_shared())); + } + else if ((*s_iter) == "decimation") + { + ROS_INFO("Add Filter: decimation"); + _filters.push_back(NamedFilter("decimation", std::make_shared())); + } + else if ((*s_iter) == "pointcloud") + { + assert(_pointcloud); // For now, it is set in getParameters().. + } + else if ((*s_iter).size() > 0) + { + ROS_ERROR_STREAM("Unknown Filter: " << (*s_iter)); + throw; + } + } + if (use_disparity_filter) + { + ROS_INFO("Add Filter: disparity"); + _filters.insert(_filters.begin(), NamedFilter("disparity_start", std::make_shared())); + _filters.push_back(NamedFilter("disparity_end", std::make_shared(false))); + ROS_INFO("Done Add Filter: disparity"); + } + if (use_colorizer_filter) + { + ROS_INFO("Add Filter: colorizer"); + _filters.push_back(NamedFilter("colorizer", std::make_shared())); + + // Types for depth stream + _format[DEPTH] = _format[COLOR]; // libRS type + _image_format[DEPTH] = _image_format[COLOR]; // CVBridge type + _encoding[DEPTH] = _encoding[COLOR]; // ROS message type + _unit_step_size[DEPTH] = _unit_step_size[COLOR]; // sensor_msgs::ImagePtr row step size + + _width[DEPTH] = _width[COLOR]; + _height[DEPTH] = _height[COLOR]; + _image[DEPTH] = cv::Mat(_height[DEPTH], _width[DEPTH], _image_format[DEPTH], cv::Scalar(0, 0, 0)); + } + if (_pointcloud) + { + ROS_INFO("Add Filter: pointcloud"); + _filters.push_back(NamedFilter("pointcloud", std::make_shared(_pointcloud_texture.first, _pointcloud_texture.second))); + } + 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. + } + } + } +} + +BaseRealSenseNode::CIMUHistory::CIMUHistory(size_t size) +{ + m_max_size = size; +} +void BaseRealSenseNode::CIMUHistory::add_data(sensor_name module, BaseRealSenseNode::CIMUHistory::imuData data) +{ + m_map[module].push_front(data); + if (m_map[module].size() > m_max_size) + m_map[module].pop_back(); +} +bool BaseRealSenseNode::CIMUHistory::is_all_data(sensor_name module) +{ + return m_map[module].size() == m_max_size; +} +bool BaseRealSenseNode::CIMUHistory::is_data(sensor_name module) +{ + return m_map[module].size() > 0; +} +const std::list& BaseRealSenseNode::CIMUHistory::get_data(sensor_name module) +{ + return m_map[module]; +} +BaseRealSenseNode::CIMUHistory::imuData BaseRealSenseNode::CIMUHistory::last_data(sensor_name module) +{ + return m_map[module].front(); +} +BaseRealSenseNode::CIMUHistory::imuData BaseRealSenseNode::CIMUHistory::imuData::operator*(const double factor) +{ + BaseRealSenseNode::CIMUHistory::imuData new_data(*this); + new_data.m_reading *= factor; + new_data.m_time *= factor; + return new_data; +} + +BaseRealSenseNode::CIMUHistory::imuData BaseRealSenseNode::CIMUHistory::imuData::operator+(const BaseRealSenseNode::CIMUHistory::imuData& other) +{ + BaseRealSenseNode::CIMUHistory::imuData new_data(*this); + new_data.m_reading += other.m_reading; + new_data.m_time += other.m_time; + return new_data; +} + +double BaseRealSenseNode::FillImuData_LinearInterpolation(const stream_index_pair stream_index, const BaseRealSenseNode::CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg) +{ + static CIMUHistory _imu_history(2); + CIMUHistory::sensor_name this_sensor(static_cast(ACCEL == stream_index)); + CIMUHistory::sensor_name that_sensor(static_cast(!this_sensor)); + _imu_history.add_data(this_sensor, imu_data); + + if (!_imu_history.is_all_data(this_sensor) || !_imu_history.is_data(that_sensor) ) + return -1; + const std::list this_data = _imu_history.get_data(this_sensor); + CIMUHistory::imuData that_last_data = _imu_history.last_data(that_sensor); + std::list::const_iterator this_data_iter = this_data.begin(); + CIMUHistory::imuData this_last_data(*this_data_iter); + this_data_iter++; + CIMUHistory::imuData this_prev_data(*this_data_iter); + if (this_prev_data.m_time > that_last_data.m_time) + return -1; // "that" data was already sent. + double factor( (that_last_data.m_time - this_prev_data.m_time) / (this_last_data.m_time - this_prev_data.m_time) ); + CIMUHistory::imuData interp_data = this_prev_data*(1-factor) + this_last_data*factor; + + CIMUHistory::imuData accel_data = that_last_data; + CIMUHistory::imuData gyro_data = interp_data; + if (this_sensor == CIMUHistory::sensor_name::mACCEL) + { + std::swap(accel_data, gyro_data); + } + imu_msg.angular_velocity.x = gyro_data.m_reading.x; + imu_msg.angular_velocity.y = gyro_data.m_reading.y; + imu_msg.angular_velocity.z = gyro_data.m_reading.z; + + imu_msg.linear_acceleration.x = accel_data.m_reading.x; + imu_msg.linear_acceleration.y = accel_data.m_reading.y; + imu_msg.linear_acceleration.z = accel_data.m_reading.z; + return that_last_data.m_time; +} + + +double BaseRealSenseNode::FillImuData_Copy(const stream_index_pair stream_index, const BaseRealSenseNode::CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg) +{ + if (GYRO == stream_index) + { + imu_msg.angular_velocity.x = imu_data.m_reading.x; + imu_msg.angular_velocity.y = imu_data.m_reading.y; + imu_msg.angular_velocity.z = imu_data.m_reading.z; + } + else if (ACCEL == stream_index) + { + imu_msg.linear_acceleration.x = imu_data.m_reading.x; + imu_msg.linear_acceleration.y = imu_data.m_reading.y; + imu_msg.linear_acceleration.z = imu_data.m_reading.z; + } + return imu_data.m_time; +} + +void BaseRealSenseNode::ConvertFromOpticalFrameToFrame(float3& data) +{ + float3 temp; + temp.x = data.z; + temp.y = -data.x; + temp.z = -data.y; + + data.x = temp.x; + data.y = temp.y; + data.z = temp.z; +} + +void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync_method) +{ + 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; + + seq += 1; + double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; + + if (0 != _synced_imu_publisher->getNumSubscribers()) + { + auto crnt_reading = *(reinterpret_cast(frame.get_data())); + if (true) + { + // Convert from optical frame to frame: + ConvertFromOpticalFrameToFrame(crnt_reading); + imu_msg.header.frame_id = _frame_id[stream_index]; + } + if (GYRO == stream_index) + { + init_gyro = true; + } + if (ACCEL == stream_index) + { + if (!init_accel) + { + // Init accel_factor: + Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); + accel_factor = 9.81 / v.norm(); + ROS_INFO_STREAM("accel_factor set to: " << accel_factor); + } + init_accel = true; + if (true) + { + Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); + v*=accel_factor; + crnt_reading.x = v.x(); + crnt_reading.y = v.y(); + crnt_reading.z = v.z(); + } + } + CIMUHistory::imuData imu_data(crnt_reading, elapsed_camera_ms); + switch (sync_method) + { + case NONE: //Cannot really be NONE. Just to avoid compilation warning. + case COPY: + elapsed_camera_ms = FillImuData_Copy(stream_index, imu_data, imu_msg); + break; + case LINEAR_INTERPOLATION: + elapsed_camera_ms = FillImuData_LinearInterpolation(stream_index, imu_data, imu_msg); + break; + } + if (elapsed_camera_ms < 0) + break; + ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); + 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(); +}; + +void BaseRealSenseNode::imu_callback(rs2::frame frame) +{ + auto stream = frame.get_profile().stream_type(); + if (false == _intialize_time_base) + return; + + ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", + rs2_stream_to_string(frame.get_profile().stream_type()), + frame.get_profile().stream_index(), + rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); + + auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; + 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) / 1000.0; + ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); + + auto imu_msg = sensor_msgs::Imu(); + imu_msg.header.frame_id = _optical_frame_id[stream_index]; + 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}; + + auto crnt_reading = *(reinterpret_cast(frame.get_data())); + ConvertFromOpticalFrameToFrame(crnt_reading); + if (GYRO == stream_index) + { + imu_msg.angular_velocity.x = crnt_reading.x; + imu_msg.angular_velocity.y = crnt_reading.y; + imu_msg.angular_velocity.z = crnt_reading.z; + } + else if (ACCEL == stream_index) + { + imu_msg.linear_acceleration.x = crnt_reading.x; + imu_msg.linear_acceleration.y = crnt_reading.y; + imu_msg.linear_acceleration.z = crnt_reading.z; + } + _seq[stream_index] += 1; + imu_msg.header.seq = _seq[stream_index]; + imu_msg.header.stamp = t; + _imu_publishers[stream_index].publish(imu_msg); + ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + } +} + void BaseRealSenseNode::setupStreams() { ROS_INFO("setupStreams..."); @@ -600,7 +1117,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 @@ -611,38 +1131,130 @@ 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); - std::vector frames; if (frame.is()) { ROS_DEBUG("Frameset arrived."); bool is_depth_arrived = false; rs2::frame depth_frame; auto frameset = frame.as(); + ROS_DEBUG("List of frameset before applying filters: size: %d", static_cast(frameset.size())); for (auto it = frameset.begin(); it != frameset.end(); ++it) { auto f = (*it); auto stream_type = f.get_profile().stream_type(); auto stream_index = f.get_profile().stream_index(); + auto stream_format = f.get_profile().format(); + auto stream_unique_id = f.get_profile().unique_id(); updateIsFrameArrived(is_frame_arrived, stream_type, stream_index); - ROS_DEBUG("Frameset contain (%s, %d) frame. 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()); + 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_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); + } + - if (stream_type == RS2_STREAM_DEPTH) + ROS_DEBUG("num_filters: %d", static_cast(_filters.size())); + for (std::vector::const_iterator filter_it = _filters.begin(); filter_it != _filters.end(); filter_it++) + { + ROS_DEBUG("Applying filter: %s", filter_it->_name.c_str()); + frameset = filter_it->_filter->process(frameset); + } + + ROS_DEBUG("List of frameset after applying filters: size: %d", static_cast(frameset.size())); + for (auto it = frameset.begin(); it != frameset.end(); ++it) + { + auto f = (*it); + auto stream_type = f.get_profile().stream_type(); + auto stream_index = f.get_profile().stream_index(); + auto stream_format = f.get_profile().format(); + 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_time, t.toNSec()); + } + ROS_DEBUG("END OF LIST"); + ROS_DEBUG_STREAM("Remove streams with same type and index:"); + // TODO - Fix the following issue: + // Currently publishers are set using a map of stream type and index only. + // It means that colorized depth image and colorized depth image + // 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. + // + bool points_in_set(false); + std::vector frames_to_publish; + std::vector is_in_set; + for (auto it = frameset.begin(); it != frameset.end(); ++it) + { + auto f = (*it); + auto stream_type = f.get_profile().stream_type(); + auto stream_index = f.get_profile().stream_index(); + auto stream_format = f.get_profile().format(); + if (f.is()) { - filterFrame(f); + if (!points_in_set) + { + points_in_set = true; + frames_to_publish.push_back(f); + } + continue; } + stream_index_pair sip{stream_type,stream_index}; + if (std::find(is_in_set.begin(), is_in_set.end(), sip) == is_in_set.end()) + { + is_in_set.push_back(sip); + frames_to_publish.push_back(f); + } + if (_align_depth && stream_type == RS2_STREAM_DEPTH && stream_format == RS2_FORMAT_Z16) + { + depth_frame = f; + is_depth_arrived = true; + } + } + for (auto it = frames_to_publish.begin(); it != frames_to_publish.end(); ++it) + { + auto f = (*it); + auto stream_type = f.get_profile().stream_type(); + auto stream_index = f.get_profile().stream_index(); + 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_time, t.toNSec()); + + if (f.is()) + { + if (0 != _pointcloud_publisher.getNumSubscribers()) + { + ROS_DEBUG("Publish pointscloud"); + publishPointCloud(f.as(), t, frameset); + } + continue; + } + else + { + ROS_DEBUG("Not points"); + } stream_index_pair sip{stream_type,stream_index}; publishFrame(f, t, sip, @@ -651,21 +1263,12 @@ void BaseRealSenseNode::setupStreams() _image_publishers, _seq, _camera_info, _optical_frame_id, _encoding); - if (_align_depth && stream_type != RS2_STREAM_DEPTH) - { - frames.push_back(f); - } - else - { - depth_frame = f; - is_depth_arrived = true; - } } if (_align_depth && is_depth_arrived) { ROS_DEBUG("publishAlignedDepthToOthers(...)"); - publishAlignedDepthToOthers(depth_frame, frames, t); + publishAlignedDepthToOthers(frameset, t); } } else @@ -674,7 +1277,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()); if (stream_type == RS2_STREAM_DEPTH) { @@ -690,22 +1293,12 @@ void BaseRealSenseNode::setupStreams() _camera_info, _optical_frame_id, _encoding); } - - if(_pointcloud && (0 != _pointcloud_xyzrgb_publisher.getNumSubscribers())) - { - ROS_DEBUG("publishRgbToDepthPCTopic(...)"); - publishRgbToDepthPCTopic(t, is_frame_arrived); - } - if(_pointcloud && (0 != _pointcloud_xyz_publisher.getNumSubscribers())) - { - ROS_DEBUG("publishDepthPCTopic(...)"); - publishDepthPCTopic(t, is_frame_arrived); - } } catch(const std::exception& ex) { ROS_ERROR_STREAM("An error has occurred during frame callback: " << ex.what()); } + _synced_imu_publisher->Resume(); }; // frame_callback // Streaming IMAGES @@ -733,7 +1326,6 @@ void BaseRealSenseNode::setupStreams() auto depth_sensor = sens.as(); _depth_scale_meters = depth_sensor.get_depth_scale(); } - if (_sync_frames) { sens.start(_syncer); @@ -761,13 +1353,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.find(elem) == _enabled_profiles.end()) + { + 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()); + } + } } } } @@ -778,70 +1381,52 @@ 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 stream = frame.get_profile().stream_type(); - if (false == _intialize_time_base) - return; + auto imu_callback_inner = [this](rs2::frame frame){ + imu_callback(frame); + }; - ROS_DEBUG("Frame arrived: stream: %s ; index: %d ; Timestamp Domain: %s", - rs2_stream_to_string(frame.get_profile().stream_type()), - frame.get_profile().stream_index(), - rs2_timestamp_domain_to_string(frame.get_frame_timestamp_domain())); + auto imu_callback_sync_inner = [this](rs2::frame frame){ + imu_callback_sync(frame, _imu_sync_method); + }; - auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; - if (0 != _info_publisher[stream_index].getNumSubscribers() || - 0 != _imu_publishers[stream_index].getNumSubscribers()) + if (_imu_sync_method > imu_sync_method::NONE) + { + std::string unite_method_str; + int expected_fps(_fps[GYRO] + _fps[ACCEL]); + unite_method_str = "COPY"; + if (_imu_sync_method == imu_sync_method::LINEAR_INTERPOLATION) { - double elapsed_camera_ms = (/*ms*/ frame.get_timestamp() - /*ms*/ _camera_time_base) / /*ms to seconds*/ 1000; - ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); - - auto imu_msg = sensor_msgs::Imu(); - imu_msg.header.frame_id = _optical_frame_id[stream_index]; - 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}; - - auto axes = *(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; - } - 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; - } - _seq[stream_index] += 1; - imu_msg.header.seq = _seq[stream_index]; - imu_msg.header.stamp = t; - _imu_publishers[stream_index].publish(imu_msg); - ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + unite_method_str = "LINEAR_INTERPOLATION"; + expected_fps = 2 * std::min(_fps[GYRO], _fps[ACCEL]); } - }); - - if (_enable[GYRO]) - { - 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 " + << expected_fps << " fps by method:" << unite_method_str); + sens.start(imu_callback_sync_inner); } - - 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_inner); + + 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); + } } } @@ -1002,7 +1587,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); @@ -1028,7 +1612,6 @@ void BaseRealSenseNode::publishStaticTransforms() exit(1); } - if (_enable[COLOR]) { // Transform base to color @@ -1116,176 +1699,190 @@ void BaseRealSenseNode::publishStaticTransforms() publish_static_tf(transform_ts_, zero_trans, q2, _depth_aligned_frame_id[FISHEYE], _optical_frame_id[FISHEYE]); } } -} - -void BaseRealSenseNode::publishDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived) -{ - try + if (_enable[GYRO]) { - if (!is_frame_arrived.at(DEPTH)) - { - ROS_DEBUG("Skipping publish PC topic! Depth frame didn't arrive."); - return; - } + // 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]); } - catch (std::out_of_range) + if (_enable[ACCEL] and _imu_sync_method == imu_sync_method::NONE) { - ROS_DEBUG("Skipping publish PC topic! Depth frame didn't configure."); - return; - } - + // 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(); - auto image_depth16 = reinterpret_cast(_image[DEPTH].data); - auto depth_intrinsics = _stream_intrinsics[DEPTH]; - sensor_msgs::PointCloud2 msg_pointcloud; - msg_pointcloud.header.stamp = t; - msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; - msg_pointcloud.width = depth_intrinsics.width; - msg_pointcloud.height = depth_intrinsics.height; - msg_pointcloud.is_dense = true; + 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]); - sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); + // 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]); + } - modifier.setPointCloud2Fields(3, - "x", 1, sensor_msgs::PointField::FLOAT32, - "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32); - modifier.setPointCloud2FieldsByString(1, "xyz"); +} - sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); +void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) +{ + size_t i; - float depth_point[3], scaled_depth; + for (i=0; i < n; ++i) + dst[n-1-i] = src[i]; +} - // Fill the PointCloud2 fields - for (int y = 0; y < depth_intrinsics.height; ++y) +void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, const rs2::frameset& frameset) +{ + std::vector::iterator pc_filter = find_if(_filters.begin(), _filters.end(), [] (NamedFilter s) { return s._name == "pointcloud"; } ); + rs2_stream texture_source_id = static_cast(pc_filter->_filter->get_option(rs2_option::RS2_OPTION_STREAM_FILTER)); + bool use_texture = texture_source_id != RS2_STREAM_ANY; + rs2::frameset::iterator texture_frame_itr = frameset.end(); + if (use_texture) { - for (int x = 0; x < depth_intrinsics.width; ++x) + std::set available_formats{ rs2_format::RS2_FORMAT_RGB8, rs2_format::RS2_FORMAT_Y8 }; + + texture_frame_itr = find_if(frameset.begin(), frameset.end(), [&texture_source_id, &available_formats] (rs2::frame f) + {return (rs2_stream(f.get_profile().stream_type()) == texture_source_id) && + (available_formats.find(f.get_profile().format()) != available_formats.end()); }); + if (texture_frame_itr == frameset.end()) { - scaled_depth = static_cast(*image_depth16) * _depth_scale_meters; - float depth_pixel[2] = {static_cast(x), static_cast(y)}; - rs2_deproject_pixel_to_point(depth_point, &depth_intrinsics, depth_pixel, scaled_depth); - - if (depth_point[2] <= 0.f) - { - depth_point[0] = 0.f; - depth_point[1] = 0.f; - depth_point[2] = 0.f; - } - - *iter_x = depth_point[0]; - *iter_y = depth_point[1]; - *iter_z = depth_point[2]; - - ++image_depth16; - ++iter_x; ++iter_y; ++iter_z; + std::string texture_source_name = pc_filter->_filter->get_option_value_description(rs2_option::RS2_OPTION_STREAM_FILTER, static_cast(texture_source_id)); + ROS_WARN_STREAM("No stream match for pointcloud chosen texture " << texture_source_name); + return; } } - _pointcloud_xyz_publisher.publish(msg_pointcloud); -} + int texture_width(0), texture_height(0); + int num_colors(0); -void BaseRealSenseNode::publishRgbToDepthPCTopic(const ros::Time& t, const std::map& is_frame_arrived) -{ - try + const rs2::vertex* vertex = pc.get_vertices(); + const rs2::texture_coordinate* color_point = pc.get_texture_coordinates(); + int num_valid_points(0); + if (use_texture) { - if (!is_frame_arrived.at(COLOR) || !is_frame_arrived.at(DEPTH)) + for (size_t point_idx=0; point_idx < pc.size(); point_idx++, color_point++) { - ROS_DEBUG("Skipping publish PC topic! Color or Depth frame didn't arrive."); - return; + float i = static_cast(color_point->u); + float j = static_cast(color_point->v); + + if (i >= 0.f && i <= 1.f && j >= 0.f && j <= 1.f) + { + num_valid_points++; + } } } - catch (std::out_of_range) + else { - ROS_DEBUG("Skipping publish PC topic! Color or Depth frame didn't configure."); - return; + for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++) + { + if (static_cast(vertex->z) > 0) + { + num_valid_points++; + } + } } - auto& depth2color_extrinsics = _depth_to_other_extrinsics[COLOR]; - auto color_intrinsics = _stream_intrinsics[COLOR]; - auto image_depth16 = reinterpret_cast(_image[DEPTH].data); - auto depth_intrinsics = _stream_intrinsics[DEPTH]; sensor_msgs::PointCloud2 msg_pointcloud; msg_pointcloud.header.stamp = t; msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; - msg_pointcloud.width = depth_intrinsics.width; - msg_pointcloud.height = depth_intrinsics.height; + msg_pointcloud.width = num_valid_points; + msg_pointcloud.height = 1; msg_pointcloud.is_dense = true; sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); + modifier.setPointCloud2FieldsByString(1, "xyz"); - modifier.setPointCloud2Fields(4, - "x", 1, sensor_msgs::PointField::FLOAT32, - "y", 1, sensor_msgs::PointField::FLOAT32, - "z", 1, sensor_msgs::PointField::FLOAT32, - "rgb", 1, sensor_msgs::PointField::FLOAT32); - modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - - sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); - - sensor_msgs::PointCloud2Iteratoriter_r(msg_pointcloud, "r"); - sensor_msgs::PointCloud2Iteratoriter_g(msg_pointcloud, "g"); - sensor_msgs::PointCloud2Iteratoriter_b(msg_pointcloud, "b"); - - float depth_point[3], color_point[3], color_pixel[2], scaled_depth; - unsigned char* color_data = _image[COLOR].data; - - // Fill the PointCloud2 fields - for (int y = 0; y < depth_intrinsics.height; ++y) + vertex = pc.get_vertices(); + if (use_texture) { - for (int x = 0; x < depth_intrinsics.width; ++x) + rs2::video_frame texture_frame = (*texture_frame_itr).as(); + texture_width = texture_frame.get_width(); + texture_height = texture_frame.get_height(); + num_colors = texture_frame.get_bytes_per_pixel(); + uint8_t* color_data = (uint8_t*)texture_frame.get_data(); + std::string format_str; + switch(texture_frame.get_profile().format()) { - scaled_depth = static_cast(*image_depth16) * _depth_scale_meters; - float depth_pixel[2] = {static_cast(x), static_cast(y)}; - rs2_deproject_pixel_to_point(depth_point, &depth_intrinsics, depth_pixel, scaled_depth); - - if (depth_point[2] <= 0.f || depth_point[2] > 5.f) + case RS2_FORMAT_RGB8: + format_str = "rgb"; + break; + case RS2_FORMAT_Y8: + format_str = "intensity"; + break; + default: + throw std::runtime_error("Unhandled texture format passed in pointcloud " + std::to_string(texture_frame.get_profile().format())); + } + msg_pointcloud.point_step = addPointField(msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::UINT32, msg_pointcloud.point_step); + msg_pointcloud.row_step = msg_pointcloud.width * msg_pointcloud.point_step; + msg_pointcloud.data.resize(msg_pointcloud.height * msg_pointcloud.row_step); + + sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_color(msg_pointcloud, format_str); + color_point = pc.get_texture_coordinates(); + + float color_pixel[2]; + for (size_t point_idx=0; point_idx < pc.size(); vertex++, point_idx++, color_point++) + { + float i = static_cast(color_point->u); + float j = static_cast(color_point->v); + if (i >= 0.f && i <= 1.f && j >= 0.f && j <= 1.f) { - depth_point[0] = 0.f; - depth_point[1] = 0.f; - depth_point[2] = 0.f; - } + *iter_x = vertex->x; + *iter_y = vertex->y; + *iter_z = vertex->z; - *iter_x = depth_point[0]; - *iter_y = depth_point[1]; - *iter_z = depth_point[2]; + color_pixel[0] = i * texture_width; + color_pixel[1] = j * texture_height; - rs2_transform_point_to_point(color_point, &depth2color_extrinsics, depth_point); - rs2_project_point_to_pixel(color_pixel, &color_intrinsics, color_point); + int pixx = static_cast(color_pixel[0]); + int pixy = static_cast(color_pixel[1]); + int offset = (pixy * texture_width + pixx) * num_colors; + reverse_memcpy(&(*iter_color), color_data+offset, num_colors); // PointCloud2 order of rgb is bgr. - if (color_pixel[1] < 0.f || color_pixel[1] >= color_intrinsics.height - || color_pixel[0] < 0.f || color_pixel[0] >= color_intrinsics.width) - { - // For out of bounds color data, default to a shade of blue in order to visually distinguish holes. - // This color value is same as the librealsense out of bounds color value. - *iter_r = static_cast(96); - *iter_g = static_cast(157); - *iter_b = static_cast(198); + ++iter_x; ++iter_y; ++iter_z; + ++iter_color; } - else + } + } + else + { + sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); + for (size_t point_idx=0; point_idx < pc.size(); vertex++, point_idx++) + { + if (static_cast(vertex->z) > 0) { - auto i = static_cast(color_pixel[0]); - auto j = static_cast(color_pixel[1]); + *iter_x = vertex->x; + *iter_y = vertex->y; + *iter_z = vertex->z; - auto offset = i * 3 + j * color_intrinsics.width * 3; - *iter_r = static_cast(color_data[offset]); - *iter_g = static_cast(color_data[offset + 1]); - *iter_b = static_cast(color_data[offset + 2]); + ++iter_x; ++iter_y; ++iter_z; } - - ++image_depth16; - ++iter_x; ++iter_y; ++iter_z; - ++iter_r; ++iter_g; ++iter_b; } } - - _pointcloud_xyzrgb_publisher.publish(msg_pointcloud); + _pointcloud_publisher.publish(msg_pointcloud); } + Extrinsics BaseRealSenseNode::rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const { Extrinsics extrinsicsMsg; @@ -1311,7 +1908,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"; @@ -1347,9 +1953,23 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, bool copy_data_from_frame) { ROS_DEBUG("publishFrame(...)"); + unsigned int width = 0; + unsigned int height = 0; + auto bpp = 1; + if (f.is()) + { + auto image = f.as(); + width = image.get_width(); + height = image.get_height(); + bpp = image.get_bytes_per_pixel(); + } auto& image = images[stream]; if (copy_data_from_frame) { + if (images[stream].size() != cv::Size(width, height)) + { + image.create(height, width, image.type()); + } if (stream != DEPTH) { image.data = (uint8_t*)f.get_data(); } else { @@ -1365,17 +1985,6 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, if(0 != info_publisher.getNumSubscribers() || 0 != image_publisher.first.getNumSubscribers()) { - auto width = 0; - auto height = 0; - auto bpp = 1; - if (f.is()) - { - auto image = f.as(); - width = image.get_width(); - height = image.get_height(); - bpp = image.get_bytes_per_pixel(); - } - sensor_msgs::ImagePtr img; img = cv_bridge::CvImage(std_msgs::Header(), encoding.at(stream), image).toImageMsg(); img->width = width; @@ -1387,12 +1996,17 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const ros::Time& t, img->header.seq = seq[stream]; auto& cam_info = camera_info.at(stream); + if (cam_info.width != width) + { + updateStreamCalibData(f.get_profile().as()); + } cam_info.header.stamp = t; cam_info.header.seq = seq[stream]; info_publisher.publish(cam_info); 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())); } } @@ -1411,209 +2025,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); - setParam(config ,(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_enable_depth_to_disparity_filter = config.rs435_enable_depth_to_disparity_filter; - base_config.base_enable_spatial_filter = config.rs435_enable_spatial_filter; - base_config.base_enable_temporal_filter = config.rs435_enable_temporal_filter; - base_config.base_enable_disparity_to_depth_filter = config.rs435_enable_disparity_to_depth_filter; - base_config.base_spatial_filter_magnitude = config.rs435_spatial_filter_magnitude; - base_config.base_spatial_filter_smooth_alpha = config.rs435_spatial_filter_smooth_alpha; - base_config.base_spatial_filter_smooth_delta = config.rs435_spatial_filter_smooth_delta; - base_config.base_spatial_filter_holes_fill = config.rs435_spatial_filter_holes_fill; - base_config.base_temporal_filter_smooth_alpha = config.rs435_temporal_filter_smooth_alpha; - base_config.base_temporal_filter_smooth_delta = config.rs435_temporal_filter_smooth_delta; - base_config.base_temporal_filter_holes_fill = config.rs435_temporal_filter_holes_fill; - 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_enable_depth_to_disparity_filter = config.rs415_enable_depth_to_disparity_filter; - base_config.base_enable_spatial_filter = config.rs415_enable_spatial_filter; - base_config.base_enable_temporal_filter = config.rs415_enable_temporal_filter; - base_config.base_enable_disparity_to_depth_filter = config.rs415_enable_disparity_to_depth_filter; - base_config.base_spatial_filter_magnitude = config.rs415_spatial_filter_magnitude; - base_config.base_spatial_filter_smooth_alpha = config.rs415_spatial_filter_smooth_alpha; - base_config.base_spatial_filter_smooth_delta = config.rs415_spatial_filter_smooth_delta; - base_config.base_spatial_filter_holes_fill = config.rs415_spatial_filter_holes_fill; - base_config.base_temporal_filter_smooth_alpha = config.rs415_temporal_filter_smooth_alpha; - base_config.base_temporal_filter_smooth_delta = config.rs415_temporal_filter_smooth_delta; - base_config.base_temporal_filter_holes_fill = config.rs415_temporal_filter_holes_fill; - 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 (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_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_enable_depth_to_disparity_filter: - ROS_DEBUG_STREAM("base_enable_depth_to_disparity_filter: " << config.base_enable_depth_to_disparity_filter); - filters[DEPTH_TO_DISPARITY].is_enabled = config.base_enable_depth_to_disparity_filter; - break; - case base_enable_spatial_filter: - ROS_DEBUG_STREAM("base_enable_spatial_filter: " << config.base_enable_spatial_filter); - filters[SPATIAL].is_enabled = config.base_enable_spatial_filter; - break; - case base_enable_temporal_filter: - ROS_DEBUG_STREAM("base_enable_temporal_filter: " << config.base_enable_temporal_filter); - filters[TEMPORAL].is_enabled = config.base_enable_temporal_filter; - break; - case base_enable_disparity_to_depth_filter: - ROS_DEBUG_STREAM("base_enable_disparity_to_depth_filter: " << config.base_enable_disparity_to_depth_filter); - filters[DISPARITY_TO_DEPTH].is_enabled = config.base_enable_disparity_to_depth_filter; - break; - case base_spatial_filter_magnitude: - ROS_DEBUG_STREAM("base_spatial_filter_magnitude: " << config.base_spatial_filter_magnitude); - filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, config.base_spatial_filter_magnitude); - break; - case base_spatial_filter_smooth_alpha: - ROS_DEBUG_STREAM("base_spatial_filter_smooth_alpha: " << config.base_spatial_filter_smooth_alpha); - filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_spatial_filter_smooth_alpha); - break; - case base_spatial_filter_smooth_delta: - ROS_DEBUG_STREAM("base_spatial_filter_smooth_delta: " << config.base_spatial_filter_smooth_delta); - filters[SPATIAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_spatial_filter_smooth_delta); - break; - case base_spatial_filter_holes_fill: - ROS_DEBUG_STREAM("base_spatial_filter_holes_fill: " << config.base_spatial_filter_holes_fill); - filters[SPATIAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_spatial_filter_holes_fill); - break; - case base_temporal_filter_smooth_alpha: - ROS_DEBUG_STREAM("base_temporal_filter_smooth_alpha: " << config.base_temporal_filter_smooth_alpha); - filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_ALPHA, config.base_temporal_filter_smooth_alpha); - break; - case base_temporal_filter_smooth_delta: - ROS_DEBUG_STREAM("base_temporal_filter_smooth_delta: " << config.base_temporal_filter_smooth_delta); - filters[TEMPORAL].filter.set_option(RS2_OPTION_FILTER_SMOOTH_DELTA, config.base_temporal_filter_smooth_delta); - break; - case base_temporal_filter_holes_fill: - ROS_DEBUG_STREAM("base_temporal_filter_holes_fill: " << config.base_temporal_filter_holes_fill); - filters[TEMPORAL].filter.set_option(RS2_OPTION_HOLES_FILL, config.base_temporal_filter_holes_fill); - break; - default: - ROS_WARN_STREAM("Unrecognized D400 param (" << param << ")"); - break; - } -} - -void BaseD400Node::registerDynamicReconfigCb() -{ - _server = std::make_shared>(); - _f = boost::bind(&BaseD400Node::callback, this, _1, _2); - _server->setCallback(_f); -} - -/** -Constructor for filter_options, takes a name and a filter. -*/ -filter_options::filter_options(const std::string name, rs2::process_interface& filter) : - filter_name(name), - filter(filter), - is_enabled(true) {} - -filter_options::filter_options(filter_options&& other) : - filter_name(std::move(other.filter_name)), - filter(other.filter), - is_enabled(other.is_enabled.load()) {} \ No newline at end of file diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 4cf38fbfd5..7d4256bc4e 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -2,11 +2,12 @@ // 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 +#include +#include +#include using namespace realsense2_camera; @@ -15,134 +16,139 @@ constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR PLUGINLIB_EXPORT_CLASS(realsense2_camera::RealSenseNodeFactory, nodelet::Nodelet) +rs2::device _device; + RealSenseNodeFactory::RealSenseNodeFactory() { - ROS_INFO("RealSense ROS v%s", REALSENSE_ROS_VERSION_STR); - ROS_INFO("Running with LibRealSense v%s", RS2_API_VERSION_STR); + ROS_INFO("RealSense ROS v%s", REALSENSE_ROS_VERSION_STR); + ROS_INFO("Running with LibRealSense v%s", RS2_API_VERSION_STR); + + signal(SIGINT, signalHandler); + auto severity = rs2_log_severity::RS2_LOG_SEVERITY_WARN; + tryGetLogSeverity(severity); + if (rs2_log_severity::RS2_LOG_SEVERITY_DEBUG == severity) + ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); - signal(SIGINT, signalHandler); - auto severity = rs2_log_severity::RS2_LOG_SEVERITY_WARN; - tryGetLogSeverity(severity); - if (rs2_log_severity::RS2_LOG_SEVERITY_DEBUG == severity) - ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug); + rs2::log_to_console(severity); +} - rs2::log_to_console(severity); +void RealSenseNodeFactory::signalHandler(int signum) +{ + ROS_INFO_STREAM(strsignal(signum) << " Signal is received! Terminating RealSense Node..."); + for(rs2::sensor sensor : _device.query_sensors()) + { + sensor.stop(); + sensor.close(); + } + ros::shutdown(); + exit(signum); } rs2::device RealSenseNodeFactory::getDevice(std::string& serial_no) { - auto list = _ctx.query_devices(); - if (0 == list.size()) - { - ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); - ros::shutdown(); - exit(1); - } - - bool found = false; - rs2::device retDev; - - for (auto&& dev : list) - { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); - if (serial_no.empty()) - { - retDev = dev; - serial_no = sn; - found = true; - break; - } - else if (sn == serial_no) - { - retDev = dev; - found = true; - break; - } - } - - if (!found) - { - ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); - ros::shutdown(); - exit(1); - } - - return retDev; + auto list = _ctx.query_devices(); + if (0 == list.size()) + { + ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); + ros::shutdown(); + exit(1); + } + + bool found = false; + rs2::device retDev; + + for (auto&& dev : list) + { + auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); + if (serial_no.empty()) + { + retDev = dev; + serial_no = sn; + found = true; + break; + } + else if (sn == serial_no) + { + retDev = dev; + found = true; + break; + } + } + + if (!found) + { + ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); + ros::shutdown(); + exit(1); + } + + return retDev; } void RealSenseNodeFactory::onInit() { - try{ + try{ #ifdef BPDEBUG std::cout << "Attach to Process: " << getpid() << std::endl; std::cout << "Press key to continue." << std::endl; std::cin.get(); #endif + auto nh = getNodeHandle(); auto privateNh = getPrivateNodeHandle(); std::string serial_no(""); privateNh.param("serial_no", serial_no, std::string("")); - + std::string rosbag_filename(""); - privateNh.param("rosbag_filename", rosbag_filename, std::string("")); - if (!rosbag_filename.empty()) - { + privateNh.param("rosbag_filename", rosbag_filename, std::string("")); + if (!rosbag_filename.empty()) + { ROS_INFO_STREAM("publish topics from rosbag file: " << rosbag_filename.c_str()); auto pipe = std::make_shared(); rs2::config cfg; cfg.enable_device_from_file(rosbag_filename.c_str(), false); cfg.enable_all_streams(); pipe->start(cfg); //File will be opened in read mode at this point - auto _device = pipe->get_active_profile().get_device(); + _device = pipe->get_active_profile().get_device(); _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, serial_no)); } - else - { - auto list = _ctx.query_devices(); - if (0 == list.size()) - { - ROS_ERROR("No RealSense devices were found! Terminating RealSense Node..."); - ros::shutdown(); - exit(1); - } - - bool found = false; - for (auto&& dev : list) + else + { + bool initial_reset; + privateNh.param("initial_reset", initial_reset, false); + if (initial_reset) { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); - if (serial_no.empty()) - { - _device = dev; - serial_no = sn; - found = true; - break; - } - else if (sn == serial_no) + ROS_INFO("Resetting device..."); + std::mutex mtx; + std::condition_variable cv; + rs2::device dev; + _ctx.set_devices_changed_callback([&dev, &cv](rs2::event_information& info) + { + if (info.was_removed(dev)) + { + cv.notify_one(); + } + }); + + dev = getDevice(serial_no); + dev.hardware_reset(); { - _device = dev; - found = true; - break; + std::unique_lock lk(mtx); + cv.wait(lk); } } - - if (!found) - { - ROS_FATAL_STREAM("The requested device with serial number " << serial_no << " is NOT found!"); - ros::shutdown(); - exit(1); - } + _device = getDevice(serial_no); _ctx.set_devices_changed_callback([this](rs2::event_information& info) - { - if (info.was_removed(_device)) - { + { + if (info.was_removed(_device)) + { ROS_FATAL("The device has been disconnected! Terminating RealSense Node..."); ros::shutdown(); exit(1); - } - }); + } + }); // TODO auto pid_str = _device.get_info(RS2_CAMERA_INFO_PRODUCT_ID); @@ -153,41 +159,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); @@ -196,40 +181,40 @@ void RealSenseNodeFactory::onInit() } } assert(_realSenseNode); - _realSenseNode->publishTopics(); - _realSenseNode->registerDynamicReconfigCb(); + _realSenseNode->publishTopics(); + _realSenseNode->registerDynamicReconfigCb(nh); + } + catch(const std::exception& ex) + { + ROS_ERROR_STREAM("An exception has been thrown: " << ex.what()); + throw; + } + catch(...) + { + ROS_ERROR_STREAM("Unknown exception has occured!"); + throw; } - catch(const std::exception& ex) - { - ROS_ERROR_STREAM("An exception has been thrown: " << ex.what()); - throw; - } - catch(...) - { - ROS_ERROR_STREAM("Unknown exception has occured!"); - throw; - } } void RealSenseNodeFactory::tryGetLogSeverity(rs2_log_severity& severity) const { - static const char* severity_var_name = "LRS_LOG_LEVEL"; - auto content = getenv(severity_var_name); - - if (content) - { - std::string content_str(content); - std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::toupper); - - for (uint32_t i = 0; i < RS2_LOG_SEVERITY_COUNT; i++) - { - auto current = std::string(rs2_log_severity_to_string((rs2_log_severity)i)); - std::transform(current.begin(), current.end(), current.begin(), ::toupper); - if (content_str == current) - { - severity = (rs2_log_severity)i; - break; - } - } - } + static const char* severity_var_name = "LRS_LOG_LEVEL"; + auto content = getenv(severity_var_name); + + if (content) + { + std::string content_str(content); + std::transform(content_str.begin(), content_str.end(), content_str.begin(), ::toupper); + + for (uint32_t i = 0; i < RS2_LOG_SEVERITY_COUNT; i++) + { + auto current = std::string(rs2_log_severity_to_string((rs2_log_severity)i)); + std::transform(current.begin(), current.end(), current.begin(), ::toupper); + if (content_str == current) + { + severity = (rs2_log_severity)i; + break; + } + } + } } diff --git a/realsense2_camera/src/rs415_node.cpp b/realsense2_camera/src/rs415_node.cpp deleted file mode 100644 index f0050ebfd5..0000000000 --- a/realsense2_camera/src/rs415_node.cpp +++ /dev/null @@ -1,133 +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_DEBUG_STREAM("RS415Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < rs415_param_count ; ++i) - { - ROS_DEBUG_STREAM("rs415_param = " << i); - setParam(config ,(rs415_param)i); - } - } - else - { - setParam(config, (rs415_param)level); - } -} diff --git a/realsense2_camera/src/rs435_node.cpp b/realsense2_camera/src/rs435_node.cpp deleted file mode 100644 index 4b0b0b26a6..0000000000 --- a/realsense2_camera/src/rs435_node.cpp +++ /dev/null @@ -1,125 +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_enable_auto_white_balance: - ROS_DEBUG_STREAM("rs435_color_enable_auto_white_balance: " << config.rs435_color_enable_auto_white_balance); - _sensors[COLOR].set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_WHITE_BALANCE, config.rs435_color_enable_auto_white_balance); - 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_DEBUG_STREAM("RS435Node - Level: " << level); - - if (set_default_dynamic_reconfig_values == level) - { - for (int i = 1 ; i < rs435_param_count ; ++i) - { - ROS_DEBUG_STREAM("rs435_param = " << i); - setParam(config ,(rs435_param)i); - } - } - else - { - setParam(config, (rs435_param)level); - } -} diff --git a/realsense2_camera/src/sr300_node.cpp b/realsense2_camera/src/sr300_node.cpp deleted file mode 100644 index a6590229b6..0000000000 --- a/realsense2_camera/src/sr300_node.cpp +++ /dev/null @@ -1,146 +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 < sr300_param_count ; ++i) - { - ROS_DEBUG_STREAM("sr300_param = " << i); - setParam(config ,(sr300_param)i); - } - } - else - { - setParam(config, (sr300_param)level); - } -} diff --git a/realsense2_camera/urdf/_d415.urdf.xacro b/realsense2_camera/urdf/_d415.urdf.xacro new file mode 100644 index 0000000000..759309c73c --- /dev/null +++ b/realsense2_camera/urdf/_d415.urdf.xacro @@ -0,0 +1,137 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_camera/urdf/_d435.urdf.xacro b/realsense2_camera/urdf/_d435.urdf.xacro index bb7dee8484..34b79ef210 100644 --- a/realsense2_camera/urdf/_d435.urdf.xacro +++ b/realsense2_camera/urdf/_d435.urdf.xacro @@ -41,12 +41,19 @@ aluminum peripherial evaluation case. + + + + + + + - + @@ -56,7 +63,7 @@ aluminum peripherial evaluation case. - + @@ -71,7 +78,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_camera/urdf/test_d415_camera.urdf.xacro b/realsense2_camera/urdf/test_d415_camera.urdf.xacro new file mode 100644 index 0000000000..835b73312b --- /dev/null +++ b/realsense2_camera/urdf/test_d415_camera.urdf.xacro @@ -0,0 +1,9 @@ + + + + + + + + +