From 52543ac3d188577b4544ea001c1857ced63f8417 Mon Sep 17 00:00:00 2001 From: Allison Thackston Date: Wed, 20 Mar 2019 10:49:49 -0700 Subject: [PATCH 001/131] moved description files into realsense2_description package --- realsense2_description/CMakeLists.txt | 30 ++++++++++++++++++ .../launch/view_d415_model.launch | 0 .../launch/view_d435_model.launch | 0 .../launch/view_r410_model.launch | 0 .../launch/view_r430_model.launch | 0 .../meshes/d415.stl | Bin .../meshes/d435.dae | 0 realsense2_description/package.xml | 17 ++++++++++ .../rviz/urdf.rviz | 0 .../urdf/_d415.urdf.xacro | 0 .../urdf/_d435.urdf.xacro | 0 .../urdf/_r410.urdf.xacro | 0 .../urdf/_r430.urdf.xacro | 0 .../urdf/test_d415_camera.urdf.xacro | 0 .../urdf/test_d435_camera.urdf.xacro | 0 .../urdf/test_r410_camera.urdf.xacro | 0 .../urdf/test_r430_camera.urdf.xacro | 0 17 files changed, 47 insertions(+) create mode 100644 realsense2_description/CMakeLists.txt rename {realsense2_camera => realsense2_description}/launch/view_d415_model.launch (100%) rename {realsense2_camera => realsense2_description}/launch/view_d435_model.launch (100%) rename {realsense2_camera => realsense2_description}/launch/view_r410_model.launch (100%) rename {realsense2_camera => realsense2_description}/launch/view_r430_model.launch (100%) rename {realsense2_camera => realsense2_description}/meshes/d415.stl (100%) rename {realsense2_camera => realsense2_description}/meshes/d435.dae (100%) create mode 100644 realsense2_description/package.xml rename {realsense2_camera => realsense2_description}/rviz/urdf.rviz (100%) rename {realsense2_camera => realsense2_description}/urdf/_d415.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/_d435.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/_r410.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/_r430.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/test_d415_camera.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/test_d435_camera.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/test_r410_camera.urdf.xacro (100%) rename {realsense2_camera => realsense2_description}/urdf/test_r430_camera.urdf.xacro (100%) diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt new file mode 100644 index 0000000000..83c6f8c55b --- /dev/null +++ b/realsense2_description/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 2.8.3) +project(realsense2_description) + +find_package(catkin REQUIRED COMPONENTS + ) + +# RealSense ROS Node +catkin_package( + ) + + +# Install launch files +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + ) + +# Install rviz files +install(DIRECTORY rviz/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz + ) + +# Install urdf files +install(DIRECTORY urdf/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf + ) + +# Install mesh files +install(DIRECTORY meshes/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes + ) diff --git a/realsense2_camera/launch/view_d415_model.launch b/realsense2_description/launch/view_d415_model.launch similarity index 100% rename from realsense2_camera/launch/view_d415_model.launch rename to realsense2_description/launch/view_d415_model.launch diff --git a/realsense2_camera/launch/view_d435_model.launch b/realsense2_description/launch/view_d435_model.launch similarity index 100% rename from realsense2_camera/launch/view_d435_model.launch rename to realsense2_description/launch/view_d435_model.launch diff --git a/realsense2_camera/launch/view_r410_model.launch b/realsense2_description/launch/view_r410_model.launch similarity index 100% rename from realsense2_camera/launch/view_r410_model.launch rename to realsense2_description/launch/view_r410_model.launch diff --git a/realsense2_camera/launch/view_r430_model.launch b/realsense2_description/launch/view_r430_model.launch similarity index 100% rename from realsense2_camera/launch/view_r430_model.launch rename to realsense2_description/launch/view_r430_model.launch diff --git a/realsense2_camera/meshes/d415.stl b/realsense2_description/meshes/d415.stl similarity index 100% rename from realsense2_camera/meshes/d415.stl rename to realsense2_description/meshes/d415.stl diff --git a/realsense2_camera/meshes/d435.dae b/realsense2_description/meshes/d435.dae similarity index 100% rename from realsense2_camera/meshes/d435.dae rename to realsense2_description/meshes/d435.dae diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml new file mode 100644 index 0000000000..8398059579 --- /dev/null +++ b/realsense2_description/package.xml @@ -0,0 +1,17 @@ + + + realsense2_description + 2.2.0 + RealSense Camera description package for Intel 3D D400 cameras + Sergey Dorodnicov + Doron Hirshberg + Apache 2.0 + + http://www.ros.org/wiki/RealSense + https://github.com/intel-ros/realsense/issues + + Sergey Dorodnicov + Doron Hirshberg + catkin + + diff --git a/realsense2_camera/rviz/urdf.rviz b/realsense2_description/rviz/urdf.rviz similarity index 100% rename from realsense2_camera/rviz/urdf.rviz rename to realsense2_description/rviz/urdf.rviz diff --git a/realsense2_camera/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/_d415.urdf.xacro rename to realsense2_description/urdf/_d415.urdf.xacro diff --git a/realsense2_camera/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/_d435.urdf.xacro rename to realsense2_description/urdf/_d435.urdf.xacro diff --git a/realsense2_camera/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/_r410.urdf.xacro rename to realsense2_description/urdf/_r410.urdf.xacro diff --git a/realsense2_camera/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/_r430.urdf.xacro rename to realsense2_description/urdf/_r430.urdf.xacro diff --git a/realsense2_camera/urdf/test_d415_camera.urdf.xacro b/realsense2_description/urdf/test_d415_camera.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/test_d415_camera.urdf.xacro rename to realsense2_description/urdf/test_d415_camera.urdf.xacro diff --git a/realsense2_camera/urdf/test_d435_camera.urdf.xacro b/realsense2_description/urdf/test_d435_camera.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/test_d435_camera.urdf.xacro rename to realsense2_description/urdf/test_d435_camera.urdf.xacro diff --git a/realsense2_camera/urdf/test_r410_camera.urdf.xacro b/realsense2_description/urdf/test_r410_camera.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/test_r410_camera.urdf.xacro rename to realsense2_description/urdf/test_r410_camera.urdf.xacro diff --git a/realsense2_camera/urdf/test_r430_camera.urdf.xacro b/realsense2_description/urdf/test_r430_camera.urdf.xacro similarity index 100% rename from realsense2_camera/urdf/test_r430_camera.urdf.xacro rename to realsense2_description/urdf/test_r430_camera.urdf.xacro From 691f6b6abaad3a22994613df9e1d8b233ac0248e Mon Sep 17 00:00:00 2001 From: Allison Thackston Date: Wed, 20 Mar 2019 10:59:08 -0700 Subject: [PATCH 002/131] updated references to realsense2_description --- realsense2_camera/CMakeLists.txt | 9 --------- realsense2_description/CMakeLists.txt | 3 +-- realsense2_description/launch/view_d415_model.launch | 6 +++--- realsense2_description/launch/view_d435_model.launch | 6 +++--- realsense2_description/launch/view_r410_model.launch | 6 +++--- realsense2_description/launch/view_r430_model.launch | 6 +++--- realsense2_description/urdf/_d415.urdf.xacro | 2 +- realsense2_description/urdf/_d435.urdf.xacro | 4 ++-- realsense2_description/urdf/_r410.urdf.xacro | 4 ++-- realsense2_description/urdf/_r430.urdf.xacro | 4 ++-- realsense2_description/urdf/test_d415_camera.urdf.xacro | 2 +- realsense2_description/urdf/test_d435_camera.urdf.xacro | 2 +- realsense2_description/urdf/test_r410_camera.urdf.xacro | 2 +- realsense2_description/urdf/test_r430_camera.urdf.xacro | 2 +- 14 files changed, 24 insertions(+), 34 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3c4bf55bcb..499f52b7c0 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -127,12 +127,3 @@ install(FILES nodelet_plugins.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -# Install urdf files -install(DIRECTORY urdf/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf - ) - -# Install mesh files -install(DIRECTORY meshes/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes - ) diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt index 83c6f8c55b..6fcb9be5ff 100644 --- a/realsense2_description/CMakeLists.txt +++ b/realsense2_description/CMakeLists.txt @@ -4,11 +4,10 @@ project(realsense2_description) find_package(catkin REQUIRED COMPONENTS ) -# RealSense ROS Node +# RealSense description catkin_package( ) - # Install launch files install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch diff --git a/realsense2_description/launch/view_d415_model.launch b/realsense2_description/launch/view_d415_model.launch index 12fd3aebb7..5ff30f2265 100644 --- a/realsense2_description/launch/view_d415_model.launch +++ b/realsense2_description/launch/view_d415_model.launch @@ -1,8 +1,8 @@ - + - - + + diff --git a/realsense2_description/launch/view_d435_model.launch b/realsense2_description/launch/view_d435_model.launch index 4fe9cb0948..cfc07b8392 100644 --- a/realsense2_description/launch/view_d435_model.launch +++ b/realsense2_description/launch/view_d435_model.launch @@ -1,8 +1,8 @@ - + - - + + diff --git a/realsense2_description/launch/view_r410_model.launch b/realsense2_description/launch/view_r410_model.launch index 88e0b51388..d627e22e58 100644 --- a/realsense2_description/launch/view_r410_model.launch +++ b/realsense2_description/launch/view_r410_model.launch @@ -1,8 +1,8 @@ - + - - + + diff --git a/realsense2_description/launch/view_r430_model.launch b/realsense2_description/launch/view_r430_model.launch index e21217bbe0..57c1c18300 100644 --- a/realsense2_description/launch/view_r430_model.launch +++ b/realsense2_description/launch/view_r430_model.launch @@ -1,8 +1,8 @@ - + - - + + diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index 759309c73c..cbc3627b7c 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -56,7 +56,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 34b79ef210..8aeb32c025 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -56,8 +56,8 @@ aluminum peripherial evaluation case. - - + + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index 9670fa47f5..07b401f442 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -47,8 +47,8 @@ aluminum peripherial evalution case. - - + + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 5733b4f0c0..28540bda29 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -48,8 +48,8 @@ aluminum peripherial evaluation case. - - + + diff --git a/realsense2_description/urdf/test_d415_camera.urdf.xacro b/realsense2_description/urdf/test_d415_camera.urdf.xacro index 835b73312b..524821f859 100644 --- a/realsense2_description/urdf/test_d415_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d415_camera.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/urdf/test_d435_camera.urdf.xacro b/realsense2_description/urdf/test_d435_camera.urdf.xacro index c5b2e1e6d8..dd5a6ee7eb 100644 --- a/realsense2_description/urdf/test_d435_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d435_camera.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/urdf/test_r410_camera.urdf.xacro b/realsense2_description/urdf/test_r410_camera.urdf.xacro index cb273aede3..e1876cf570 100644 --- a/realsense2_description/urdf/test_r410_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r410_camera.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/urdf/test_r430_camera.urdf.xacro b/realsense2_description/urdf/test_r430_camera.urdf.xacro index d88c443e76..4f78ad8a4b 100644 --- a/realsense2_description/urdf/test_r430_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r430_camera.urdf.xacro @@ -1,6 +1,6 @@ - + From 23ac156b2beb5b0d22b2649b9f7205371ec869dc Mon Sep 17 00:00:00 2001 From: Allison Thackston Date: Wed, 20 Mar 2019 12:07:47 -0700 Subject: [PATCH 003/131] changed install to single block --- realsense2_description/CMakeLists.txt | 22 +++------------------- 1 file changed, 3 insertions(+), 19 deletions(-) diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt index 6fcb9be5ff..6edf86d385 100644 --- a/realsense2_description/CMakeLists.txt +++ b/realsense2_description/CMakeLists.txt @@ -8,22 +8,6 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( ) -# Install launch files -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - ) - -# Install rviz files -install(DIRECTORY rviz/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz - ) - -# Install urdf files -install(DIRECTORY urdf/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf - ) - -# Install mesh files -install(DIRECTORY meshes/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes - ) +# Install files +install(DIRECTORY launch meshes rviz urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From 8864a8d0b1f59e29a5f6ebc184ef21dbe9dfec5f Mon Sep 17 00:00:00 2001 From: Stephan Sundermann Date: Tue, 26 Mar 2019 14:25:59 +0100 Subject: [PATCH 004/131] Add option to use external nodelet manager Right now a nodelet manager is always created when running the realsense node. This is not always a wanted behaviour as a nodelet manager might already be running somewhere else in the ROS system. This patch adds a parameter to disable launching a new nodelet manager. In this case the nodelet manager must be provided using the manager arg. The new parameter is set to false by default, so existing behaviour is not changed. --- realsense2_camera/launch/includes/nodelet.launch.xml | 3 ++- realsense2_camera/launch/rs_rgbd.launch | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 72066f3358..a3256839a6 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -1,4 +1,5 @@ + @@ -79,7 +80,7 @@ - + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index f9b3eaceea..8869198bdf 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -40,6 +40,7 @@ Processing enabled by this node: + @@ -110,6 +111,7 @@ Processing enabled by this node: + From 4071a3a8bc6beb61d48a1cb9a9dcda417085b602 Mon Sep 17 00:00:00 2001 From: Stephan Sundermann Date: Fri, 29 Mar 2019 11:54:02 +0100 Subject: [PATCH 005/131] Register dynamic options in parent node handle If running in an external nodelet manager this makes sure dynamic options are in the right namespace. This does not change existing behaviour. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index a33e8c0cdb..46ed2da704 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -261,7 +261,7 @@ std::string create_graph_resource_name(const std::string &original_name) void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name) { - ros::NodeHandle nh1(module_name); + ros::NodeHandle nh1(nh, module_name); std::shared_ptr ddynrec = std::make_shared(nh1); for (auto i = 0; i < RS2_OPTION_COUNT; i++) { From f04caf4e5c2972f932f2906f962cae3a9bb46dc8 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 3 Apr 2019 06:40:55 +0300 Subject: [PATCH 006/131] change frame_id for imu messages to camera_link's coordinates system, same as imu's sync messages. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 54fc5284f0..4929a5c7fc 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1213,7 +1213,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) 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.header.frame_id = _frame_id[stream_index]; imu_msg.orientation.x = 0.0; imu_msg.orientation.y = 0.0; imu_msg.orientation.z = 0.0; From 565555a84a4b59747cca15f3ba5ace1f2cc3167e Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 20 Mar 2019 16:51:38 +0100 Subject: [PATCH 007/131] Remove copied version of ddynamic_reconfigure --- ddynamic_reconfigure/.gitignore | 2 - ddynamic_reconfigure/CHANGELOG.rst | 65 -- ddynamic_reconfigure/CMakeLists.txt | 77 --- ddynamic_reconfigure/README.md | 624 ------------------ .../include/ddynamic_reconfigure/dd_param.h | 122 ---- .../include/ddynamic_reconfigure/dd_value.h | 161 ----- .../ddynamic_reconfigure.h | 243 ------- .../param/dd_all_params.h | 12 - .../param/dd_bool_param.h | 70 -- .../param/dd_double_param.h | 80 --- .../param/dd_enum_param.h | 123 ---- .../ddynamic_reconfigure/param/dd_int_param.h | 78 --- .../param/dd_string_param.h | 70 -- ddynamic_reconfigure/package.xml | 24 - ddynamic_reconfigure/src/dd_param.cpp | 12 - .../src/ddynamic_reconfigure.cpp | 218 ------ .../src/param/dd_bool_param.cpp | 58 -- .../src/param/dd_double_param.cpp | 58 -- .../src/param/dd_enum_param.cpp | 152 ----- .../src/param/dd_int_param.cpp | 58 -- .../src/param/dd_string_param.cpp | 58 -- ddynamic_reconfigure/test/TutorialParams.srv | 6 - .../test/dd_full_scale/dd_client.cpp | 75 --- .../test/dd_full_scale/dd_full_scale.test | 4 - .../test/dd_full_scale/dd_server.cpp | 62 -- .../test/dd_param/dd_bool.test | 3 - .../test/dd_param/dd_double.test | 3 - .../test/dd_param/dd_enum.test | 3 - .../test/dd_param/dd_int.test | 3 - .../test/dd_param/dd_string.test | 3 - .../test/dd_param/test_dd_bool.cpp | 61 -- .../test/dd_param/test_dd_double.cpp | 61 -- .../test/dd_param/test_dd_enum.cpp | 106 --- .../test/dd_param/test_dd_int.cpp | 61 -- .../test/dd_param/test_dd_string.cpp | 61 -- ddynamic_reconfigure/test/dd_value.test | 3 - .../test/ddynamic_reconfigure.test | 3 - ddynamic_reconfigure/test/test-all.sh | 18 - ddynamic_reconfigure/test/test_dd_value.cpp | 188 ------ .../test/test_ddynamic_reconfigure.cpp | 539 --------------- ddynamic_reconfigure/uml/class_struct.puml | 104 --- ddynamic_reconfigure/uml/file_struct.puml | 38 -- ddynamic_reconfigure/uml/ros_struct.puml | 22 - 43 files changed, 3792 deletions(-) delete mode 100644 ddynamic_reconfigure/.gitignore delete mode 100644 ddynamic_reconfigure/CHANGELOG.rst delete mode 100644 ddynamic_reconfigure/CMakeLists.txt delete mode 100644 ddynamic_reconfigure/README.md delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h delete mode 100644 ddynamic_reconfigure/package.xml delete mode 100644 ddynamic_reconfigure/src/dd_param.cpp delete mode 100644 ddynamic_reconfigure/src/ddynamic_reconfigure.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_bool_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_double_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_enum_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_int_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_string_param.cpp delete mode 100644 ddynamic_reconfigure/test/TutorialParams.srv delete mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp delete mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test delete mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_bool.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_double.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_enum.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_int.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_string.test delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_double.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_int.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_string.cpp delete mode 100644 ddynamic_reconfigure/test/dd_value.test delete mode 100644 ddynamic_reconfigure/test/ddynamic_reconfigure.test delete mode 100644 ddynamic_reconfigure/test/test-all.sh delete mode 100644 ddynamic_reconfigure/test/test_dd_value.cpp delete mode 100644 ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp delete mode 100644 ddynamic_reconfigure/uml/class_struct.puml delete mode 100644 ddynamic_reconfigure/uml/file_struct.puml delete mode 100644 ddynamic_reconfigure/uml/ros_struct.puml diff --git a/ddynamic_reconfigure/.gitignore b/ddynamic_reconfigure/.gitignore deleted file mode 100644 index 48c896e410..0000000000 --- a/ddynamic_reconfigure/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*~ -*user diff --git a/ddynamic_reconfigure/CHANGELOG.rst b/ddynamic_reconfigure/CHANGELOG.rst deleted file mode 100644 index 25a7690f09..0000000000 --- a/ddynamic_reconfigure/CHANGELOG.rst +++ /dev/null @@ -1,65 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -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 deleted file mode 100644 index ed26f24624..0000000000 --- a/ddynamic_reconfigure/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(ddynamic_reconfigure) - -find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure roscpp message_generation std_msgs) - -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 dynamic_reconfigure message_runtime std_msgs) - -############# -## 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) diff --git a/ddynamic_reconfigure/README.md b/ddynamic_reconfigure/README.md deleted file mode 100644 index a4af37fc25..0000000000 --- a/ddynamic_reconfigure/README.md +++ /dev/null @@ -1,624 +0,0 @@ -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 deleted file mode 100644 index 18b3e10912..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h +++ /dev/null @@ -1,122 +0,0 @@ -// -// 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 deleted file mode 100644 index c5726ffcd5..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h +++ /dev/null @@ -1,161 +0,0 @@ -// -// 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 deleted file mode 100644 index d354595b43..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h +++ /dev/null @@ -1,243 +0,0 @@ -// -// 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_; - #ifdef __clang__ - #pragma clang diagnostic push // CLion suppressor - #pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" - #endif - /** - * @brief the service server used to trigger parameter updates. - * It also contains the new parameters sent from client or commandline. - */ - ros::ServiceServer set_service_; - #ifdef __clang__ - #pragma clang diagnostic pop - #endif - }; - - /** - * @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 diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h deleted file mode 100644 index 27884cc729..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h +++ /dev/null @@ -1,12 +0,0 @@ -// -// 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 deleted file mode 100644 index 80dd6d7a43..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h +++ /dev/null @@ -1,70 +0,0 @@ -// -// 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 deleted file mode 100644 index 72c74124f4..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h +++ /dev/null @@ -1,80 +0,0 @@ -// -// 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 deleted file mode 100644 index 47e22f47d8..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h +++ /dev/null @@ -1,123 +0,0 @@ -// -// 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); - - #ifdef __clang__ - #pragma clang diagnostic push - #pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" - #endif - /** - * @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); - #ifdef __clang__ - #pragma clang diagnostic pop - #endif - - 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 diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h deleted file mode 100644 index a207ec4845..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h +++ /dev/null @@ -1,78 +0,0 @@ -// -// 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 deleted file mode 100644 index 6a40ccbfcc..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h +++ /dev/null @@ -1,70 +0,0 @@ -// -// 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 deleted file mode 100644 index 546b6c76dd..0000000000 --- a/ddynamic_reconfigure/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - ddynamic_reconfigure - 0.0.6 - The ddynamic_reconfigure package - - Noam Dori - - BSD - - - - Noam Dori - - catkin - message_generation - std_msgs - roscpp - dynamic_reconfigure - - message_runtime - rostest - google-mock - diff --git a/ddynamic_reconfigure/src/dd_param.cpp b/ddynamic_reconfigure/src/dd_param.cpp deleted file mode 100644 index f3d698e129..0000000000 --- a/ddynamic_reconfigure/src/dd_param.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// -// 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 deleted file mode 100644 index 02982f01a9..0000000000 --- a/ddynamic_reconfigure/src/ddynamic_reconfigure.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#ifdef __clang__ -#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" -#endif -// -// 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();} - } -} -#ifdef __clang__ -#pragma clang diagnostic pop -#endif diff --git a/ddynamic_reconfigure/src/param/dd_bool_param.cpp b/ddynamic_reconfigure/src/param/dd_bool_param.cpp deleted file mode 100644 index 036b332f17..0000000000 --- a/ddynamic_reconfigure/src/param/dd_bool_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index 3c988de240..0000000000 --- a/ddynamic_reconfigure/src/param/dd_double_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index 1c704dfe07..0000000000 --- a/ddynamic_reconfigure/src/param/dd_enum_param.cpp +++ /dev/null @@ -1,152 +0,0 @@ -#ifdef __clang__ -#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" -#endif -// -// 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(); - } -} -#ifdef __clang__ -#pragma clang diagnostic pop -#endif diff --git a/ddynamic_reconfigure/src/param/dd_int_param.cpp b/ddynamic_reconfigure/src/param/dd_int_param.cpp deleted file mode 100644 index f8b52d174b..0000000000 --- a/ddynamic_reconfigure/src/param/dd_int_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index ee8ca850b0..0000000000 --- a/ddynamic_reconfigure/src/param/dd_string_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index 1a8ba2f86e..0000000000 --- a/ddynamic_reconfigure/test/TutorialParams.srv +++ /dev/null @@ -1,6 +0,0 @@ ---- -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 deleted file mode 100644 index 43ff73b2b1..0000000000 --- a/ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// 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 deleted file mode 100644 index f1690d38d1..0000000000 --- a/ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ 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 deleted file mode 100644 index 1d9a591ea7..0000000000 --- a/ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#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 deleted file mode 100644 index e2616df940..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_bool.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_double.test b/ddynamic_reconfigure/test/dd_param/dd_double.test deleted file mode 100644 index 8cac0e7d55..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_double.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_enum.test b/ddynamic_reconfigure/test/dd_param/dd_enum.test deleted file mode 100644 index 2d7a7d3e85..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_enum.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_int.test b/ddynamic_reconfigure/test/dd_param/dd_int.test deleted file mode 100644 index c85d98b52f..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_int.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_string.test b/ddynamic_reconfigure/test/dd_param/dd_string.test deleted file mode 100644 index 7e9b4bec68..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_string.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp deleted file mode 100644 index 6cbe7cf9fc..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index 87081461e5..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_double.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index 8fc8f45c1b..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#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 deleted file mode 100644 index f289e6fd31..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_int.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index fb34b680e6..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_string.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index 9e663c9e0b..0000000000 --- a/ddynamic_reconfigure/test/dd_value.test +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/ddynamic_reconfigure/test/ddynamic_reconfigure.test b/ddynamic_reconfigure/test/ddynamic_reconfigure.test deleted file mode 100644 index 80263a5028..0000000000 --- a/ddynamic_reconfigure/test/ddynamic_reconfigure.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/test-all.sh b/ddynamic_reconfigure/test/test-all.sh deleted file mode 100644 index b8496a3721..0000000000 --- a/ddynamic_reconfigure/test/test-all.sh +++ /dev/null @@ -1,18 +0,0 @@ -#!/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 deleted file mode 100644 index e118ea90b8..0000000000 --- a/ddynamic_reconfigure/test/test_dd_value.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#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 deleted file mode 100644 index 14359c3876..0000000000 --- a/ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp +++ /dev/null @@ -1,539 +0,0 @@ -#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 deleted file mode 100644 index 6b2f276e72..0000000000 --- a/ddynamic_reconfigure/uml/class_struct.puml +++ /dev/null @@ -1,104 +0,0 @@ -@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 deleted file mode 100644 index 610805eade..0000000000 --- a/ddynamic_reconfigure/uml/file_struct.puml +++ /dev/null @@ -1,38 +0,0 @@ -@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 deleted file mode 100644 index 01916dd04a..0000000000 --- a/ddynamic_reconfigure/uml/ros_struct.puml +++ /dev/null @@ -1,22 +0,0 @@ -@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 From 3f1892850312d967e40fa9243cf04c8393485e37 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 20 Mar 2019 16:51:53 +0100 Subject: [PATCH 008/131] Migrate to https://github.com/pal-robotics/ddynamic_reconfigure --- .../include/base_realsense_node.h | 2 - realsense2_camera/src/base_realsense_node.cpp | 40 ++++++++++--------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d57de8321e..e628a1f9de 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -5,7 +5,6 @@ #include "../include/realsense_node_factory.h" #include -#include #include #include @@ -212,7 +211,6 @@ namespace realsense2_camera bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile); 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); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9e7a8ccced..3500c6da3b 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -278,7 +278,10 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options { sensor.set_option(option, option_value); } - ddynrec->add(new DDBool(option_name, i, sensor.get_option_description(option), option_value)); + ddynrec->registerVariable( + option_name, option_value, + [option, sensor](bool new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option)); continue; } const auto enum_dict = get_enum_method(sensor, option); @@ -303,7 +306,10 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options } if (is_int_option(sensor, option)) { - ddynrec->add(new DDInt(option_name, i, sensor.get_option_description(option), option_value, op_range.min, op_range.max)); + ddynrec->registerVariable( + option_name, int(option_value), + [option, sensor](int new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option), int(op_range.min), int(op_range.max)); } else { @@ -320,7 +326,10 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options } else { - ddynrec->add(new DDDouble(option_name, i, sensor.get_option_description(option), option_value, op_range.min, op_range.max)); + ddynrec->registerVariable( + option_name, option_value, + [option, sensor](double new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option), double(op_range.min), double(op_range.max)); } } } @@ -345,10 +354,13 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor.set_option(option, option_value); } } - ddynrec->add(new DDEnum(option_name, i, sensor.get_option_description(option), option_value, enum_dict)); + ddynrec->registerEnumVariable( + option_name, option_value, + [option, sensor](int new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option), enum_dict); } } - ddynrec->start(boost::bind(callback, _1, _2, sensor)); + ddynrec->publishServicesTopics(); _ddynrec.push_back(ddynrec); } @@ -373,14 +385,6 @@ void BaseRealSenseNode::registerDynamicReconfigCb(ros::NodeHandle& nh) 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); - std::string option_name = create_graph_resource_name(rs2_option_to_string(option)); - double value = get(map, option_name.c_str()).toDouble(); - ROS_DEBUG_STREAM("option: " << option_name << ". value: " << value); - sensor.set_option(option, value); -} - rs2_stream BaseRealSenseNode::rs2_string_to_stream(std::string str) { if (str == "RS2_STREAM_ANY") @@ -417,7 +421,7 @@ void BaseRealSenseNode::getParameters() for (auto& stream : IMAGE_STREAMS) { - string param_name(_stream_name[stream.first] + "_width"); + std::string param_name(_stream_name[stream.first] + "_width"); ROS_DEBUG_STREAM("reading parameter:" << param_name); _pnh.param(param_name, _width[stream], IMAGE_WIDTH); param_name = _stream_name[stream.first] + "_height"; @@ -433,7 +437,7 @@ void BaseRealSenseNode::getParameters() for (auto& stream : HID_STREAMS) { - string param_name(_stream_name[stream.first] + "_fps"); + std::string param_name(_stream_name[stream.first] + "_fps"); ROS_DEBUG_STREAM("reading parameter:" << param_name); _pnh.param(param_name, _fps[stream], IMU_FPS); param_name = "enable_" + STREAM_NAME(stream); @@ -447,7 +451,7 @@ void BaseRealSenseNode::getParameters() streams.insert(streams.end(), HID_STREAMS.begin(), HID_STREAMS.end()); for (auto& stream : streams) { - string param_name(static_cast(std::ostringstream() << STREAM_NAME(stream) << "_frame_id").str()); + std::string param_name(static_cast(std::ostringstream() << STREAM_NAME(stream) << "_frame_id").str()); _pnh.param(param_name, _frame_id[stream], FRAME_ID(stream)); ROS_DEBUG_STREAM("frame_id: reading parameter:" << param_name << " : " << _frame_id[stream]); param_name = static_cast(std::ostringstream() << STREAM_NAME(stream) << "_optical_frame_id").str(); @@ -473,7 +477,7 @@ void BaseRealSenseNode::getParameters() { if (stream == DEPTH) continue; if (stream.second > 1) continue; - string param_name(static_cast(std::ostringstream() << "aligned_depth_to_" << STREAM_NAME(stream) << "_frame_id").str()); + std::string param_name(static_cast(std::ostringstream() << "aligned_depth_to_" << STREAM_NAME(stream) << "_frame_id").str()); _pnh.param(param_name, _depth_aligned_frame_id[stream], ALIGNED_DEPTH_TO_FRAME_ID(stream)); } @@ -2005,7 +2009,7 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) { imuIntrinsics = sp.get_motion_intrinsics(); } - catch(const runtime_error &ex) + catch(const std::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}}; From 719f9a8b98f36e1b326d6e86b18a4d53e9f55396 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Mon, 8 Apr 2019 08:52:33 +0200 Subject: [PATCH 009/131] Add rule to install ddynamic_reconfigure --- .travis.yml | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/.travis.yml b/.travis.yml index 8ac2284d3e..9cba36b804 100644 --- a/.travis.yml +++ b/.travis.yml @@ -7,9 +7,9 @@ before_install: # - sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keys.gnupg.net:80 --recv-key C8B3A55A6F3EFCDE - sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" - sudo apt-get update -qq - - sudo apt-get install librealsense2-dkms --allow-unauthenticated -y + - sudo apt-get install librealsense2-dkms --allow-unauthenticated -y - sudo apt-get install librealsense2-dev --allow-unauthenticated -y - + install: # install ROS: - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' @@ -23,29 +23,30 @@ install: - sudo apt-get install ros-kinetic-image-transport - sudo apt-get install ros-kinetic-tf -y - sudo apt-get install ros-kinetic-diagnostic-updater -y + - sudo apt-get install ros-kinetic-ddynamic-reconfigure -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/ - - catkin_init_workspace + - catkin_init_workspace - cd .. - catkin_make clean - catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release - catkin_make install - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc - source ~/.bashrc - + # download data: - bag_filename="http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors.bag"; - wget $bag_filename -P "records/" - + # Run test: script: - python src/realsense/realsense2_camera/scripts/rs2_test.py --all - - + + before_cache: - rm -f $HOME/.gradle/caches/modules-2/modules-2.lock - rm -fr $HOME/.gradle/caches/*/plugin-resolution/ @@ -53,4 +54,4 @@ cache: directories: - $HOME/.gradle/caches/ - $HOME/.gradle/wrapper/ - - $HOME/.android/build-cache \ No newline at end of file + - $HOME/.android/build-cache From ce8b6ee500a9192e3591c8dbe8ce350818e0d11e Mon Sep 17 00:00:00 2001 From: Abhijit Majumdar Date: Thu, 18 Apr 2019 10:42:23 -0500 Subject: [PATCH 010/131] add decimation filter at the front of the filter list, before the start of disparity filter --- realsense2_camera/src/base_realsense_node.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9e7a8ccced..c0c3d4d466 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -872,6 +872,7 @@ void BaseRealSenseNode::setupFilters() boost::split(filters_str, _filters_str, [](char c){return c == ',';}); bool use_disparity_filter(false); bool use_colorizer_filter(false); + bool use_decimation_filter(false); for (std::vector::const_iterator s_iter=filters_str.begin(); s_iter!=filters_str.end(); s_iter++) { if ((*s_iter) == "colorizer") @@ -899,8 +900,7 @@ void BaseRealSenseNode::setupFilters() } else if ((*s_iter) == "decimation") { - ROS_INFO("Add Filter: decimation"); - _filters.push_back(NamedFilter("decimation", std::make_shared())); + use_decimation_filter = true; } else if ((*s_iter) == "pointcloud") { @@ -919,6 +919,11 @@ void BaseRealSenseNode::setupFilters() _filters.push_back(NamedFilter("disparity_end", std::make_shared(false))); ROS_INFO("Done Add Filter: disparity"); } + if (use_decimation_filter) + { + ROS_INFO("Add Filter: decimation"); + _filters.insert(_filters.begin(),NamedFilter("decimation", std::make_shared())); + } if (use_colorizer_filter) { ROS_INFO("Add Filter: colorizer"); From 7955ecebd78695421a13b58c694566a992a6cb9a Mon Sep 17 00:00:00 2001 From: Phillip Schmidt Date: Tue, 30 Apr 2019 17:16:37 -0700 Subject: [PATCH 011/131] sync get devices --- realsense2_camera/src/realsense_node_factory.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 7b0ce0bd6b..19efec610c 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -20,6 +20,7 @@ constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR PLUGINLIB_EXPORT_CLASS(realsense2_camera::RealSenseNodeFactory, nodelet::Nodelet) rs2::device _device; +std::mutex _device_mutex; RealSenseNodeFactory::RealSenseNodeFactory() { @@ -107,6 +108,7 @@ rs2::device RealSenseNodeFactory::getDevice() void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) { + std::lock_guard guard(_device_mutex); if (info.was_removed(_device)) { ROS_ERROR("The device has been disconnected!"); @@ -141,6 +143,8 @@ void RealSenseNodeFactory::onInit() auto privateNh = getPrivateNodeHandle(); privateNh.param("serial_no", _serial_no, std::string("")); + std::lock_guard guard(_device_mutex); + std::string rosbag_filename(""); privateNh.param("rosbag_filename", rosbag_filename, std::string("")); if (!rosbag_filename.empty()) From e9621238342e6f4f5d1d3701c0071d6a74f197ec Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 12 May 2019 08:42:39 +0300 Subject: [PATCH 012/131] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 275ef2cef1..234a9a9570 100644 --- a/README.md +++ b/README.md @@ -94,7 +94,7 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc. - **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)** - **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. Under the new topic, all the fields in the Imu message are filled out. - - linear_interpolation: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. (items A and B are accel and gyro but without specific) + - linear_interpolation: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. - copy: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. From a34ee97e42e8fb37b91dcc9a83e7eee0ec7e4e1e Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 12 May 2019 14:34:10 +0300 Subject: [PATCH 013/131] fix bug in pointcloud. Used to send points with Z=0. add feature: _allow_no_texture_points - if set to true, will send points with depth, both with and without texture. --- .../include/base_realsense_node.h | 2 + realsense2_camera/include/constants.h | 1 + realsense2_camera/scripts/rs2_test.py | 2 +- realsense2_camera/src/base_realsense_node.cpp | 77 +++++++++---------- 4 files changed, 39 insertions(+), 43 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index ad29141ae4..2ce7d98d28 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -233,6 +233,8 @@ namespace realsense2_camera std::string _serial_no; float _depth_scale_meters; float _clipping_distance; + bool _allow_no_texture_points; + double _linear_accel_cov; double _angular_velocity_cov; bool _hold_back_imu_for_frames; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 6642408f1e..a9480f9c29 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -37,6 +37,7 @@ namespace realsense2_camera const bool ALIGN_DEPTH = false; const bool POINTCLOUD = false; + const bool ALLOW_NO_TEXTURE_POINTS = false; const bool SYNC_FRAMES = false; const int IMAGE_WIDTH = 640; diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index e9a2aef4c2..75c032db2a 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -153,7 +153,7 @@ def staticTFTest(data, gt_data): 'test_func': NotImageColorTest}, '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]}, - 'data_func': lambda x: {'width': [776534, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 125, 116, 102])], 'epsilon': [0.02, 2]}, + 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 125, 116, 102])], 'epsilon': [0.02, 2]}, 'test_func': PointCloudTest}, 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1', 'data_func': ImageDepthGetData, diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4929a5c7fc..4e40f0492d 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -477,6 +477,7 @@ void BaseRealSenseNode::getParameters() _pnh.param(param_name, _depth_aligned_frame_id[stream], ALIGNED_DEPTH_TO_FRAME_ID(stream)); } + _pnh.param("allow_no_texture_points", _allow_no_texture_points, ALLOW_NO_TEXTURE_POINTS); _pnh.param("clip_distance", _clipping_distance, static_cast(-1.0)); _pnh.param("linear_accel_cov", _linear_accel_cov, static_cast(0.01)); _pnh.param("angular_velocity_cov", _angular_velocity_cov, static_cast(0.01)); @@ -1854,27 +1855,16 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co 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) + std::list valid_indices; + for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, color_point++) { - for (size_t point_idx=0; point_idx < pc.size(); point_idx++, color_point++) + if (static_cast(vertex->z) > 0) { 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++; - } - } - } - else - { - for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++) - { - if (static_cast(vertex->z) > 0) + if (_allow_no_texture_points || (i >= 0.f && i <= 1.f && j >= 0.f && j <= 1.f)) { - num_valid_points++; + valid_indices.push_back(point_idx); } } } @@ -1882,7 +1872,7 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co sensor_msgs::PointCloud2 msg_pointcloud; msg_pointcloud.header.stamp = t; msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; - msg_pointcloud.width = num_valid_points; + msg_pointcloud.width = valid_indices.size(); msg_pointcloud.height = 1; msg_pointcloud.is_dense = true; @@ -1920,27 +1910,28 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co 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++) + unsigned int prev_idx(0); + for (auto idx=valid_indices.begin(); idx != valid_indices.end(); idx++) { - 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) - { - *iter_x = vertex->x; - *iter_y = vertex->y; - *iter_z = vertex->z; + unsigned int idx_jump(*idx-prev_idx); + prev_idx = *idx; + vertex+=idx_jump; + color_point+=idx_jump; - color_pixel[0] = i * texture_width; - color_pixel[1] = j * texture_height; + *iter_x = vertex->x; + *iter_y = vertex->y; + *iter_z = vertex->z; - 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. + color_pixel[0] = static_cast(color_point->u) * texture_width; + color_pixel[1] = static_cast(color_point->v) * texture_height; - ++iter_x; ++iter_y; ++iter_z; - ++iter_color; - } + 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. + + ++iter_x; ++iter_y; ++iter_z; + ++iter_color; } } else @@ -1948,16 +1939,18 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co 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++) + unsigned int prev_idx(0); + for (auto idx=valid_indices.begin(); idx != valid_indices.end(); idx++) { - if (static_cast(vertex->z) > 0) - { - *iter_x = vertex->x; - *iter_y = vertex->y; - *iter_z = vertex->z; + unsigned int idx_jump(*idx-prev_idx); + prev_idx = *idx; + vertex+=idx_jump; - ++iter_x; ++iter_y; ++iter_z; - } + *iter_x = vertex->x; + *iter_y = vertex->y; + *iter_z = vertex->z; + + ++iter_x; ++iter_y; ++iter_z; } } _pointcloud_publisher.publish(msg_pointcloud); From 5eb3f7ae502b39e2c686ee9f33a2d04a64322fe1 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 20 May 2019 13:19:12 +0300 Subject: [PATCH 014/131] removed lock_guard. set_devices_changed_callback called AFTER getDevice() Keep checking for devices until device is found - for cases where T265 was momentarily taken by another node at the time of query. Add a 3rd, optional camera, to rs_multiple_devices.launch file. --- .../include/realsense_node_factory.h | 5 +- .../launch/rs_multiple_devices.launch | 11 +++ .../src/realsense_node_factory.cpp | 88 +++++++++++-------- 3 files changed, 66 insertions(+), 38 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 077bfeb711..db21705a08 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace realsense2_camera { @@ -62,7 +63,7 @@ namespace realsense2_camera static void closeDevice(); void StartDevice(); void change_device_callback(rs2::event_information& info); - rs2::device getDevice(); + void getDevice(rs2::device_list list); virtual void onInit() override; void tryGetLogSeverity(rs2_log_severity& severity) const; @@ -70,5 +71,7 @@ namespace realsense2_camera rs2::context _ctx; std::string _serial_no; bool _initial_reset; + std::thread _query_thread; + }; }//end namespace diff --git a/realsense2_camera/launch/rs_multiple_devices.launch b/realsense2_camera/launch/rs_multiple_devices.launch index 12174ad2a2..20ef3cf243 100644 --- a/realsense2_camera/launch/rs_multiple_devices.launch +++ b/realsense2_camera/launch/rs_multiple_devices.launch @@ -1,10 +1,13 @@ + + + @@ -22,4 +25,12 @@ + + + + + + + + diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 19efec610c..7a3e9576db 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -53,49 +53,51 @@ void RealSenseNodeFactory::signalHandler(int signum) exit(signum); } -rs2::device RealSenseNodeFactory::getDevice() +void RealSenseNodeFactory::getDevice(rs2::device_list list) { - rs2::device retDev; - auto list = _ctx.query_devices(); - if (0 == list.size()) - { - ROS_WARN("No RealSense devices were found!"); - } - else + if (!_device) { - bool found = false; - for (auto&& dev : list) + if (0 == list.size()) { - auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - ROS_DEBUG_STREAM("Device with serial number " << sn << " was found."); - if (_serial_no.empty() || sn == _serial_no) - { - retDev = dev; - _serial_no = sn; - found = true; - break; - } + ROS_WARN("No RealSense devices were found!"); } - if (!found) + else { - ROS_ERROR_STREAM("The requested device with serial number " << _serial_no << " is NOT found!"); + bool found = false; + 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() || sn == _serial_no) + { + _device = dev; + _serial_no = sn; + found = true; + break; + } + } + if (!found) + { + // T265 could be caught by another node. + ROS_ERROR_STREAM("The requested device with serial number " << _serial_no << " is NOT found. Will Try again."); + } } } - bool remove_tm2_handle(retDev && RS_T265_PID != std::stoi(retDev.get_info(RS2_CAMERA_INFO_PRODUCT_ID), 0, 16)); + bool remove_tm2_handle(_device && RS_T265_PID != std::stoi(_device.get_info(RS2_CAMERA_INFO_PRODUCT_ID), 0, 16)); if (remove_tm2_handle) { _ctx.unload_tracking_module(); } - if (retDev && _initial_reset) + if (_device && _initial_reset) { _initial_reset = false; try { ROS_INFO("Resetting device..."); - retDev.hardware_reset(); - retDev = rs2::device(); + _device.hardware_reset(); + _device = rs2::device(); } catch(const std::exception& ex) @@ -103,12 +105,10 @@ rs2::device RealSenseNodeFactory::getDevice() ROS_WARN_STREAM("An exception has been thrown: " << ex.what()); } } - return retDev; } void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) { - std::lock_guard guard(_device_mutex); if (info.was_removed(_device)) { ROS_ERROR("The device has been disconnected!"); @@ -121,7 +121,7 @@ void RealSenseNodeFactory::change_device_callback(rs2::event_information& info) if (new_devices.size() > 0) { ROS_INFO("Checking new devices..."); - _device = getDevice(); + getDevice(new_devices); if (_device) { StartDevice(); @@ -143,8 +143,6 @@ void RealSenseNodeFactory::onInit() auto privateNh = getPrivateNodeHandle(); privateNh.param("serial_no", _serial_no, std::string("")); - std::lock_guard guard(_device_mutex); - std::string rosbag_filename(""); privateNh.param("rosbag_filename", rosbag_filename, std::string("")); if (!rosbag_filename.empty()) @@ -159,18 +157,34 @@ void RealSenseNodeFactory::onInit() _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); _realSenseNode->publishTopics(); _realSenseNode->registerDynamicReconfigCb(nh); + if (_device) + { + StartDevice(); + } } else { - std::function change_device_callback_function = [this](rs2::event_information& info){change_device_callback(info);}; - _ctx.set_devices_changed_callback(change_device_callback_function); privateNh.param("initial_reset", _initial_reset, false); - _device = getDevice(); - } - if (_device) - { - StartDevice(); + _query_thread = std::thread([=]() + { + std::chrono::milliseconds timespan(6000); + while (!_device) + { + // _ctx.init_tracking_module(); // Unavailable function. + getDevice(_ctx.query_devices()); + if (_device) + { + std::function change_device_callback_function = [this](rs2::event_information& info){change_device_callback(info);}; + _ctx.set_devices_changed_callback(change_device_callback_function); + StartDevice(); + } + else + { + std::this_thread::sleep_for(timespan); + } + } + }); } } catch(const std::exception& ex) From b286e4c3c4b55f4f1b8bb5aeaf1e65636677d4a0 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 20 May 2019 14:33:47 +0300 Subject: [PATCH 015/131] fix inserted bug reading from file --- .../src/realsense_node_factory.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 7a3e9576db..cab97c7edc 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -147,16 +147,18 @@ void RealSenseNodeFactory::onInit() 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 - _device = pipe->get_active_profile().get_device(); - _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); - _realSenseNode->publishTopics(); - _realSenseNode->registerDynamicReconfigCb(nh); + { + 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 + _device = pipe->get_active_profile().get_device(); + _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); + _realSenseNode->publishTopics(); + _realSenseNode->registerDynamicReconfigCb(nh); + } if (_device) { StartDevice(); From 2738820270b2418a9a904bed7ffbd80f1a78bbc7 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 20 May 2019 15:32:44 +0300 Subject: [PATCH 016/131] camera_link for t265 is POSE instead of GYRO. fix is needed due to the availability of t265 extrinsics. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- realsense2_camera/src/t265_realsense_node.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9e7a8ccced..bba2358f82 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1762,7 +1762,7 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& s void BaseRealSenseNode::publishStaticTransforms() { // Publish static transforms - const std::vector base_stream_priority = {DEPTH, GYRO}; + const std::vector base_stream_priority = {DEPTH, POSE}; std::vector::const_iterator base_stream(base_stream_priority.begin()); while( (_sensors.find(*base_stream) == _sensors.end()) && (base_stream != base_stream_priority.end())) diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp index d41046bc70..854478a88b 100644 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ b/realsense2_camera/src/t265_realsense_node.cpp @@ -72,7 +72,7 @@ void T265RealsenseNode::calcAndPublishStaticTransform(const stream_index_pair& s { // Transform base to stream tf::Quaternion quaternion_optical; - quaternion_optical.setRPY(M_PI / 2, 0.0, -M_PI / 2); + quaternion_optical.setRPY(M_PI / 2, 0.0, -M_PI / 2); //Pose To ROS float3 zero_trans{0, 0, 0}; ros::Time transform_ts_ = ros::Time::now(); @@ -101,8 +101,6 @@ void T265RealsenseNode::calcAndPublishStaticTransform(const stream_index_pair& s if (stream == POSE) { Q = Q.inverse(); - quaternion_optical = quaternion_optical.inverse(); - publish_static_tf(transform_ts_, trans, Q, _frame_id[stream], _base_frame_id); } else From 2d0ad23e78b0e94a12b106b948bad46a578e8968 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 20 May 2019 16:22:35 +0300 Subject: [PATCH 017/131] Add description of frame ids to README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 234a9a9570..3305b6d630 100644 --- a/README.md +++ b/README.md @@ -175,6 +175,12 @@ To visualize the pose output and frames in RViz, start: roslaunch realsense2_camera demo_t265.launch ``` +### About Frame ID +The wrapper publishes static transformations(TFs). The Frame Ids are divided into 3 groups: +- ROS convention frames: follow the format of _<_stream>"_frame" for example: camera_depth_frame, camera_infra1_frame, etc. +- Original frame coordinate system: with the suffix of <_optical_frame>. For example: camera_infra1_optical_frame. Check the device documentation for specific coordinate system for each stream. +- base_link: For example: camera_link. A reference frame for the device. In D400 series and SR300 it is the depth frame. In T265, the pose frame. + ## Packages using RealSense ROS Camera | Title | Links | | ----- | ----- | From 95a9613c38ec46314bfbde0c3d6df3db11007464 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 21 May 2019 14:01:45 +0300 Subject: [PATCH 018/131] fixed bug: imu and synced imu are now sent in original device coordinates frames - i.e. gyro_optical_frame, accel_optical_frame, imu_optical_frame. Fix issue for both t265 and d435i with different coordinate systems. fixed bug: sending united imu without images enabled. add imu_optical_frame_id to nodelet.launch.xml. --- .../include/base_realsense_node.h | 1 - .../launch/includes/nodelet.launch.xml | 2 ++ realsense2_camera/src/base_realsense_node.cpp | 23 ++----------------- 3 files changed, 4 insertions(+), 22 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index d57de8321e..f5e39f226b 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -215,7 +215,6 @@ namespace realsense2_camera 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 pose_callback(rs2::frame frame); diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 9b06a852bd..f507096ed8 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -61,6 +61,7 @@ + @@ -142,6 +143,7 @@ + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index bba2358f82..3eee0bb3ac 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -16,7 +16,7 @@ using namespace ddynamic_reconfigure; #define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast(std::ostringstream() << "camera_aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str() SyncedImuPublisher::SyncedImuPublisher(ros::Publisher imu_publisher, std::size_t waiting_list_size): - _publisher(imu_publisher), + _publisher(imu_publisher), _pause_mode(false), _waiting_list_size(waiting_list_size) {} @@ -1104,18 +1104,6 @@ double BaseRealSenseNode::FillImuData_Copy(const stream_index_pair stream_index, 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; @@ -1124,7 +1112,7 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync 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.header.frame_id = _optical_frame_id[stream_imu]; imu_msg.orientation.x = 0.0; imu_msg.orientation.y = 0.0; imu_msg.orientation.z = 0.0; @@ -1154,12 +1142,6 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync 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; @@ -1242,7 +1224,6 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) imu_msg.angular_velocity_covariance = { _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; auto crnt_reading = *(reinterpret_cast(frame.get_data())); - ConvertFromOpticalFrameToFrame(crnt_reading); if (GYRO == stream_index) { imu_msg.angular_velocity.x = crnt_reading.x; From 7833c16886d2de7c69375cacc3658b042486e734 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 22 May 2019 07:57:50 +0300 Subject: [PATCH 019/131] removed global variable _device, based on @akirayou at https://github.com/IntelRealSense/realsense-ros/issues/774#issuecomment-494236047 --- realsense2_camera/include/realsense_node_factory.h | 6 +++--- realsense2_camera/src/realsense_node_factory.cpp | 9 +-------- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index db21705a08..b692748d96 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -56,17 +56,17 @@ namespace realsense2_camera { public: RealSenseNodeFactory(); - virtual ~RealSenseNodeFactory() {} + virtual ~RealSenseNodeFactory(); private: - static void signalHandler(int signum); - static void closeDevice(); + void closeDevice(); void StartDevice(); void change_device_callback(rs2::event_information& info); void getDevice(rs2::device_list list); virtual void onInit() override; void tryGetLogSeverity(rs2_log_severity& severity) const; + rs2::device _device; std::unique_ptr _realSenseNode; rs2::context _ctx; std::string _serial_no; diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index cab97c7edc..d4b9c100c6 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -19,15 +19,11 @@ constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR PLUGINLIB_EXPORT_CLASS(realsense2_camera::RealSenseNodeFactory, nodelet::Nodelet) -rs2::device _device; -std::mutex _device_mutex; - RealSenseNodeFactory::RealSenseNodeFactory() { 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) @@ -45,12 +41,9 @@ void RealSenseNodeFactory::closeDevice() } } -void RealSenseNodeFactory::signalHandler(int signum) +RealSenseNodeFactory::~RealSenseNodeFactory() { - ROS_INFO_STREAM(strsignal(signum) << " Signal is received! Terminating RealSense Node..."); closeDevice(); - ros::shutdown(); - exit(signum); } void RealSenseNodeFactory::getDevice(rs2::device_list list) From b6e8f732b944f6cbe3d88dcb21d3802790d8ed87 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 22 May 2019 09:01:04 +0300 Subject: [PATCH 020/131] add note to rs_rgbd.launch, reminding users to initially install ros package rgbd_launch. --- realsense2_camera/launch/rs_rgbd.launch | 2 ++ 1 file changed, 2 insertions(+) diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index f9b3eaceea..644656405e 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -18,6 +18,8 @@ limitations under the License. A launch file, derived from rgbd_launch and customized for Realsense ROS driver, to publish XYZRGB point cloud like an OpenNI camera. +NOTICE: To use this launch file you must first install ros package rgbd_launch. + To launch Realsense with software registeration (ROS Image Pipeline and rgbd_launch): $ roslaunch realsense2_camera rs_rgbd.launch Processing enabled by ROS driver: From b4060b618c0dccead44dad09ecfd6f19f766c2b1 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 22 May 2019 10:51:49 +0300 Subject: [PATCH 021/131] add option: allow_no_texture_points --- README.md | 4 +++- .../launch/includes/nodelet.launch.xml | 2 ++ realsense2_camera/launch/rs_camera.launch | 24 ++++++++++--------- realsense2_camera/src/base_realsense_node.cpp | 18 ++++++++------ 4 files changed, 29 insertions(+), 19 deletions(-) diff --git a/README.md b/README.md index 3305b6d630..8232c5cd8f 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,9 @@ The following parameters are available by the wrapper: The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc. - **filters**: any of the following options, separated by commas:
- ```colorizer```: will color the depth image. On the depth topic an RGB image will be published, instead of the 16bit depth values . - - ```pointcloud```: will add a pointcloud topic `/camera/depth/color/points`. The texture of the pointcloud can be modified in rqt_reconfigure (see below) or using the parameters: `pointcloud_texture_stream` and `pointcloud_texture_index`. Run rqt_reconfigure to see available values for these parameters. + - ```pointcloud```: will add a pointcloud topic `/camera/depth/color/points`. The texture of the pointcloud can be modified in rqt_reconfigure (see below) or using the parameters: `pointcloud_texture_stream` and `pointcloud_texture_index`. Run rqt_reconfigure to see available values for these parameters.
+ The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting `allow_no_texture_points` to true. + - The following filters have detailed descriptions in : https://github.com/IntelRealSense/librealsense/blob/master/doc/post-processing-filters.md - ```disparity``` - convert depth to disparity before applying other filters and back. - ```spatial``` - filter the depth image spatially. diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index f507096ed8..ab9827ce64 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -80,6 +80,7 @@ + @@ -162,6 +163,7 @@ + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 97984c7df9..b2db68008e 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -34,17 +34,18 @@ - - - - - - - - - - - + + + + + + + + + + + + @@ -93,6 +94,7 @@ + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 21efabc5a4..2801269842 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1917,13 +1917,17 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co *iter_y = vertex->y; *iter_z = vertex->z; - color_pixel[0] = static_cast(color_point->u) * texture_width; - color_pixel[1] = static_cast(color_point->v) * texture_height; - - 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. + float i(color_point->u); + float j(color_point->v); + if (i >= 0.f && i <= 1.f && j >= 0.f && j <= 1.f) + { + color_pixel[0] = i * texture_width; + color_pixel[1] = j * texture_height; + 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. + } ++iter_x; ++iter_y; ++iter_z; ++iter_color; From 4a5a508a34c1b7f29e2557daee83c4d81ecfd971 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 22 May 2019 10:53:22 +0300 Subject: [PATCH 022/131] update version: 2.2.4 --- realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 3c4bf55bcb..3535370aa9 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -34,7 +34,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2 2.19.0) +find_package(realsense2 2.22.0) 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() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index a9480f9c29..0aca12feca 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 3 +#define REALSENSE_ROS_PATCH_VERSION 4 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 0b6f58455a..06a9724c5b 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,8 +1,8 @@ realsense2_camera - 2.2.3 - RealSense Camera package allowing access to Intel 3D D400 cameras + 2.2.4 + RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg Apache 2.0 From ea86fc229343f3073ace3441ac5666b29b372176 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 22 May 2019 13:10:06 +0300 Subject: [PATCH 023/131] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 8232c5cd8f..be4c2899ab 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine - #### 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 and librealsense-dkms packages. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.19.2) 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.22.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 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 From b529f5a2c2f64cae60531268c38d0a27240108ca Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 26 May 2019 15:07:10 +0300 Subject: [PATCH 024/131] fixed bug: wrong frame_id for imu frames. (#784) --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2801269842..e2a56df1e8 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1220,7 +1220,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); auto imu_msg = sensor_msgs::Imu(); - imu_msg.header.frame_id = _frame_id[stream_index]; + 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; From cfd5d0c9c40f24fbdb24f7c69ad604031b58605c Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 20 Mar 2019 16:51:38 +0100 Subject: [PATCH 025/131] Remove copied version of ddynamic_reconfigure --- ddynamic_reconfigure/.gitignore | 2 - ddynamic_reconfigure/CHANGELOG.rst | 65 -- ddynamic_reconfigure/CMakeLists.txt | 77 --- ddynamic_reconfigure/README.md | 624 ------------------ .../include/ddynamic_reconfigure/dd_param.h | 122 ---- .../include/ddynamic_reconfigure/dd_value.h | 161 ----- .../ddynamic_reconfigure.h | 243 ------- .../param/dd_all_params.h | 12 - .../param/dd_bool_param.h | 70 -- .../param/dd_double_param.h | 80 --- .../param/dd_enum_param.h | 123 ---- .../ddynamic_reconfigure/param/dd_int_param.h | 78 --- .../param/dd_string_param.h | 70 -- ddynamic_reconfigure/package.xml | 24 - ddynamic_reconfigure/src/dd_param.cpp | 12 - .../src/ddynamic_reconfigure.cpp | 218 ------ .../src/param/dd_bool_param.cpp | 58 -- .../src/param/dd_double_param.cpp | 58 -- .../src/param/dd_enum_param.cpp | 152 ----- .../src/param/dd_int_param.cpp | 58 -- .../src/param/dd_string_param.cpp | 58 -- ddynamic_reconfigure/test/TutorialParams.srv | 6 - .../test/dd_full_scale/dd_client.cpp | 75 --- .../test/dd_full_scale/dd_full_scale.test | 4 - .../test/dd_full_scale/dd_server.cpp | 62 -- .../test/dd_param/dd_bool.test | 3 - .../test/dd_param/dd_double.test | 3 - .../test/dd_param/dd_enum.test | 3 - .../test/dd_param/dd_int.test | 3 - .../test/dd_param/dd_string.test | 3 - .../test/dd_param/test_dd_bool.cpp | 61 -- .../test/dd_param/test_dd_double.cpp | 61 -- .../test/dd_param/test_dd_enum.cpp | 106 --- .../test/dd_param/test_dd_int.cpp | 61 -- .../test/dd_param/test_dd_string.cpp | 61 -- ddynamic_reconfigure/test/dd_value.test | 3 - .../test/ddynamic_reconfigure.test | 3 - ddynamic_reconfigure/test/test-all.sh | 18 - ddynamic_reconfigure/test/test_dd_value.cpp | 188 ------ .../test/test_ddynamic_reconfigure.cpp | 539 --------------- ddynamic_reconfigure/uml/class_struct.puml | 104 --- ddynamic_reconfigure/uml/file_struct.puml | 38 -- ddynamic_reconfigure/uml/ros_struct.puml | 22 - 43 files changed, 3792 deletions(-) delete mode 100644 ddynamic_reconfigure/.gitignore delete mode 100644 ddynamic_reconfigure/CHANGELOG.rst delete mode 100644 ddynamic_reconfigure/CMakeLists.txt delete mode 100644 ddynamic_reconfigure/README.md delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h delete mode 100644 ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h delete mode 100644 ddynamic_reconfigure/package.xml delete mode 100644 ddynamic_reconfigure/src/dd_param.cpp delete mode 100644 ddynamic_reconfigure/src/ddynamic_reconfigure.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_bool_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_double_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_enum_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_int_param.cpp delete mode 100644 ddynamic_reconfigure/src/param/dd_string_param.cpp delete mode 100644 ddynamic_reconfigure/test/TutorialParams.srv delete mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp delete mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test delete mode 100644 ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_bool.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_double.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_enum.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_int.test delete mode 100644 ddynamic_reconfigure/test/dd_param/dd_string.test delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_double.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_int.cpp delete mode 100644 ddynamic_reconfigure/test/dd_param/test_dd_string.cpp delete mode 100644 ddynamic_reconfigure/test/dd_value.test delete mode 100644 ddynamic_reconfigure/test/ddynamic_reconfigure.test delete mode 100644 ddynamic_reconfigure/test/test-all.sh delete mode 100644 ddynamic_reconfigure/test/test_dd_value.cpp delete mode 100644 ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp delete mode 100644 ddynamic_reconfigure/uml/class_struct.puml delete mode 100644 ddynamic_reconfigure/uml/file_struct.puml delete mode 100644 ddynamic_reconfigure/uml/ros_struct.puml diff --git a/ddynamic_reconfigure/.gitignore b/ddynamic_reconfigure/.gitignore deleted file mode 100644 index 48c896e410..0000000000 --- a/ddynamic_reconfigure/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*~ -*user diff --git a/ddynamic_reconfigure/CHANGELOG.rst b/ddynamic_reconfigure/CHANGELOG.rst deleted file mode 100644 index 25a7690f09..0000000000 --- a/ddynamic_reconfigure/CHANGELOG.rst +++ /dev/null @@ -1,65 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -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 deleted file mode 100644 index ed26f24624..0000000000 --- a/ddynamic_reconfigure/CMakeLists.txt +++ /dev/null @@ -1,77 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(ddynamic_reconfigure) - -find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure roscpp message_generation std_msgs) - -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 dynamic_reconfigure message_runtime std_msgs) - -############# -## 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) diff --git a/ddynamic_reconfigure/README.md b/ddynamic_reconfigure/README.md deleted file mode 100644 index a4af37fc25..0000000000 --- a/ddynamic_reconfigure/README.md +++ /dev/null @@ -1,624 +0,0 @@ -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 deleted file mode 100644 index 18b3e10912..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_param.h +++ /dev/null @@ -1,122 +0,0 @@ -// -// 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 deleted file mode 100644 index c5726ffcd5..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/dd_value.h +++ /dev/null @@ -1,161 +0,0 @@ -// -// 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 deleted file mode 100644 index d354595b43..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/ddynamic_reconfigure.h +++ /dev/null @@ -1,243 +0,0 @@ -// -// 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_; - #ifdef __clang__ - #pragma clang diagnostic push // CLion suppressor - #pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" - #endif - /** - * @brief the service server used to trigger parameter updates. - * It also contains the new parameters sent from client or commandline. - */ - ros::ServiceServer set_service_; - #ifdef __clang__ - #pragma clang diagnostic pop - #endif - }; - - /** - * @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 diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h deleted file mode 100644 index 27884cc729..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_all_params.h +++ /dev/null @@ -1,12 +0,0 @@ -// -// 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 deleted file mode 100644 index 80dd6d7a43..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_bool_param.h +++ /dev/null @@ -1,70 +0,0 @@ -// -// 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 deleted file mode 100644 index 72c74124f4..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_double_param.h +++ /dev/null @@ -1,80 +0,0 @@ -// -// 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 deleted file mode 100644 index 47e22f47d8..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_enum_param.h +++ /dev/null @@ -1,123 +0,0 @@ -// -// 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); - - #ifdef __clang__ - #pragma clang diagnostic push - #pragma ide diagnostic ignored "OCUnusedGlobalDeclarationInspection" - #endif - /** - * @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); - #ifdef __clang__ - #pragma clang diagnostic pop - #endif - - 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 diff --git a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h b/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h deleted file mode 100644 index a207ec4845..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_int_param.h +++ /dev/null @@ -1,78 +0,0 @@ -// -// 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 deleted file mode 100644 index 6a40ccbfcc..0000000000 --- a/ddynamic_reconfigure/include/ddynamic_reconfigure/param/dd_string_param.h +++ /dev/null @@ -1,70 +0,0 @@ -// -// 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 deleted file mode 100644 index 546b6c76dd..0000000000 --- a/ddynamic_reconfigure/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - ddynamic_reconfigure - 0.0.6 - The ddynamic_reconfigure package - - Noam Dori - - BSD - - - - Noam Dori - - catkin - message_generation - std_msgs - roscpp - dynamic_reconfigure - - message_runtime - rostest - google-mock - diff --git a/ddynamic_reconfigure/src/dd_param.cpp b/ddynamic_reconfigure/src/dd_param.cpp deleted file mode 100644 index f3d698e129..0000000000 --- a/ddynamic_reconfigure/src/dd_param.cpp +++ /dev/null @@ -1,12 +0,0 @@ -// -// 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 deleted file mode 100644 index 02982f01a9..0000000000 --- a/ddynamic_reconfigure/src/ddynamic_reconfigure.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#ifdef __clang__ -#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" -#endif -// -// 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();} - } -} -#ifdef __clang__ -#pragma clang diagnostic pop -#endif diff --git a/ddynamic_reconfigure/src/param/dd_bool_param.cpp b/ddynamic_reconfigure/src/param/dd_bool_param.cpp deleted file mode 100644 index 036b332f17..0000000000 --- a/ddynamic_reconfigure/src/param/dd_bool_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index 3c988de240..0000000000 --- a/ddynamic_reconfigure/src/param/dd_double_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index 1c704dfe07..0000000000 --- a/ddynamic_reconfigure/src/param/dd_enum_param.cpp +++ /dev/null @@ -1,152 +0,0 @@ -#ifdef __clang__ -#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" -#endif -// -// 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(); - } -} -#ifdef __clang__ -#pragma clang diagnostic pop -#endif diff --git a/ddynamic_reconfigure/src/param/dd_int_param.cpp b/ddynamic_reconfigure/src/param/dd_int_param.cpp deleted file mode 100644 index f8b52d174b..0000000000 --- a/ddynamic_reconfigure/src/param/dd_int_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index ee8ca850b0..0000000000 --- a/ddynamic_reconfigure/src/param/dd_string_param.cpp +++ /dev/null @@ -1,58 +0,0 @@ -// -// 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 deleted file mode 100644 index 1a8ba2f86e..0000000000 --- a/ddynamic_reconfigure/test/TutorialParams.srv +++ /dev/null @@ -1,6 +0,0 @@ ---- -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 deleted file mode 100644 index 43ff73b2b1..0000000000 --- a/ddynamic_reconfigure/test/dd_full_scale/dd_client.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// -// 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 deleted file mode 100644 index f1690d38d1..0000000000 --- a/ddynamic_reconfigure/test/dd_full_scale/dd_full_scale.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - \ 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 deleted file mode 100644 index 1d9a591ea7..0000000000 --- a/ddynamic_reconfigure/test/dd_full_scale/dd_server.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#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 deleted file mode 100644 index e2616df940..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_bool.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_double.test b/ddynamic_reconfigure/test/dd_param/dd_double.test deleted file mode 100644 index 8cac0e7d55..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_double.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_enum.test b/ddynamic_reconfigure/test/dd_param/dd_enum.test deleted file mode 100644 index 2d7a7d3e85..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_enum.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_int.test b/ddynamic_reconfigure/test/dd_param/dd_int.test deleted file mode 100644 index c85d98b52f..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_int.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/dd_string.test b/ddynamic_reconfigure/test/dd_param/dd_string.test deleted file mode 100644 index 7e9b4bec68..0000000000 --- a/ddynamic_reconfigure/test/dd_param/dd_string.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp b/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp deleted file mode 100644 index 6cbe7cf9fc..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_bool.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index 87081461e5..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_double.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index 8fc8f45c1b..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_enum.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#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 deleted file mode 100644 index f289e6fd31..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_int.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index fb34b680e6..0000000000 --- a/ddynamic_reconfigure/test/dd_param/test_dd_string.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#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 deleted file mode 100644 index 9e663c9e0b..0000000000 --- a/ddynamic_reconfigure/test/dd_value.test +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/ddynamic_reconfigure/test/ddynamic_reconfigure.test b/ddynamic_reconfigure/test/ddynamic_reconfigure.test deleted file mode 100644 index 80263a5028..0000000000 --- a/ddynamic_reconfigure/test/ddynamic_reconfigure.test +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/ddynamic_reconfigure/test/test-all.sh b/ddynamic_reconfigure/test/test-all.sh deleted file mode 100644 index b8496a3721..0000000000 --- a/ddynamic_reconfigure/test/test-all.sh +++ /dev/null @@ -1,18 +0,0 @@ -#!/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 deleted file mode 100644 index e118ea90b8..0000000000 --- a/ddynamic_reconfigure/test/test_dd_value.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#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 deleted file mode 100644 index 14359c3876..0000000000 --- a/ddynamic_reconfigure/test/test_ddynamic_reconfigure.cpp +++ /dev/null @@ -1,539 +0,0 @@ -#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 deleted file mode 100644 index 6b2f276e72..0000000000 --- a/ddynamic_reconfigure/uml/class_struct.puml +++ /dev/null @@ -1,104 +0,0 @@ -@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 deleted file mode 100644 index 610805eade..0000000000 --- a/ddynamic_reconfigure/uml/file_struct.puml +++ /dev/null @@ -1,38 +0,0 @@ -@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 deleted file mode 100644 index 01916dd04a..0000000000 --- a/ddynamic_reconfigure/uml/ros_struct.puml +++ /dev/null @@ -1,22 +0,0 @@ -@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 From bf9b16fe45eb60d2d752b76e717615b9fd965472 Mon Sep 17 00:00:00 2001 From: Victor Lopez Date: Wed, 20 Mar 2019 16:51:53 +0100 Subject: [PATCH 026/131] Migrate to https://github.com/pal-robotics/ddynamic_reconfigure --- .../include/base_realsense_node.h | 2 - realsense2_camera/src/base_realsense_node.cpp | 40 ++++++++++--------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 2578be8f76..1d164edfdd 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -5,7 +5,6 @@ #include "../include/realsense_node_factory.h" #include -#include #include #include @@ -212,7 +211,6 @@ namespace realsense2_camera bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile); 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); void imu_callback(rs2::frame frame); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index e2a56df1e8..39a2b32ac1 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -278,7 +278,10 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options { sensor.set_option(option, option_value); } - ddynrec->add(new DDBool(option_name, i, sensor.get_option_description(option), option_value)); + ddynrec->registerVariable( + option_name, option_value, + [option, sensor](bool new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option)); continue; } const auto enum_dict = get_enum_method(sensor, option); @@ -303,7 +306,10 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options } if (is_int_option(sensor, option)) { - ddynrec->add(new DDInt(option_name, i, sensor.get_option_description(option), option_value, op_range.min, op_range.max)); + ddynrec->registerVariable( + option_name, int(option_value), + [option, sensor](int new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option), int(op_range.min), int(op_range.max)); } else { @@ -320,7 +326,10 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options } else { - ddynrec->add(new DDDouble(option_name, i, sensor.get_option_description(option), option_value, op_range.min, op_range.max)); + ddynrec->registerVariable( + option_name, option_value, + [option, sensor](double new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option), double(op_range.min), double(op_range.max)); } } } @@ -345,10 +354,13 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor.set_option(option, option_value); } } - ddynrec->add(new DDEnum(option_name, i, sensor.get_option_description(option), option_value, enum_dict)); + ddynrec->registerEnumVariable( + option_name, option_value, + [option, sensor](int new_value) { sensor.set_option(option, new_value); }, + sensor.get_option_description(option), enum_dict); } } - ddynrec->start(boost::bind(callback, _1, _2, sensor)); + ddynrec->publishServicesTopics(); _ddynrec.push_back(ddynrec); } @@ -373,14 +385,6 @@ void BaseRealSenseNode::registerDynamicReconfigCb(ros::NodeHandle& nh) 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); - std::string option_name = create_graph_resource_name(rs2_option_to_string(option)); - double value = get(map, option_name.c_str()).toDouble(); - ROS_DEBUG_STREAM("option: " << option_name << ". value: " << value); - sensor.set_option(option, value); -} - rs2_stream BaseRealSenseNode::rs2_string_to_stream(std::string str) { if (str == "RS2_STREAM_ANY") @@ -417,7 +421,7 @@ void BaseRealSenseNode::getParameters() for (auto& stream : IMAGE_STREAMS) { - string param_name(_stream_name[stream.first] + "_width"); + std::string param_name(_stream_name[stream.first] + "_width"); ROS_DEBUG_STREAM("reading parameter:" << param_name); _pnh.param(param_name, _width[stream], IMAGE_WIDTH); param_name = _stream_name[stream.first] + "_height"; @@ -433,7 +437,7 @@ void BaseRealSenseNode::getParameters() for (auto& stream : HID_STREAMS) { - string param_name(_stream_name[stream.first] + "_fps"); + std::string param_name(_stream_name[stream.first] + "_fps"); ROS_DEBUG_STREAM("reading parameter:" << param_name); _pnh.param(param_name, _fps[stream], IMU_FPS); param_name = "enable_" + STREAM_NAME(stream); @@ -447,7 +451,7 @@ void BaseRealSenseNode::getParameters() streams.insert(streams.end(), HID_STREAMS.begin(), HID_STREAMS.end()); for (auto& stream : streams) { - string param_name(static_cast(std::ostringstream() << STREAM_NAME(stream) << "_frame_id").str()); + std::string param_name(static_cast(std::ostringstream() << STREAM_NAME(stream) << "_frame_id").str()); _pnh.param(param_name, _frame_id[stream], FRAME_ID(stream)); ROS_DEBUG_STREAM("frame_id: reading parameter:" << param_name << " : " << _frame_id[stream]); param_name = static_cast(std::ostringstream() << STREAM_NAME(stream) << "_optical_frame_id").str(); @@ -473,7 +477,7 @@ void BaseRealSenseNode::getParameters() { if (stream == DEPTH) continue; if (stream.second > 1) continue; - string param_name(static_cast(std::ostringstream() << "aligned_depth_to_" << STREAM_NAME(stream) << "_frame_id").str()); + std::string param_name(static_cast(std::ostringstream() << "aligned_depth_to_" << STREAM_NAME(stream) << "_frame_id").str()); _pnh.param(param_name, _depth_aligned_frame_id[stream], ALIGNED_DEPTH_TO_FRAME_ID(stream)); } @@ -1988,7 +1992,7 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) { imuIntrinsics = sp.get_motion_intrinsics(); } - catch(const runtime_error &ex) + catch(const std::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}}; From 2bdf2632b2a08014e920e061767b916b09cc623e Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 28 May 2019 13:26:53 +0300 Subject: [PATCH 027/131] update README.md to include realsense2_description --- README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index be4c2899ab..8a30f606b5 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ # ROS Wrapper for Intel® RealSense™ Devices -These are packages for using Intel RealSense cameras (D400 series and the SR300) with ROS. +These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. ## Installation Instructions @@ -183,6 +183,13 @@ The wrapper publishes static transformations(TFs). The Frame Ids are divided int - Original frame coordinate system: with the suffix of <_optical_frame>. For example: camera_infra1_optical_frame. Check the device documentation for specific coordinate system for each stream. - base_link: For example: camera_link. A reference frame for the device. In D400 series and SR300 it is the depth frame. In T265, the pose frame. + +### realsense2_description package: +For viewing included models, a separate package is included. For example: +```bash +roslaunch realsense2_description view_d415_model.launch +``` + ## Packages using RealSense ROS Camera | Title | Links | | ----- | ----- | From 2a2e73f59e630afeda49b933b11090c338989493 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 28 May 2019 15:12:45 +0300 Subject: [PATCH 028/131] exit node if failed to initialize. --- realsense2_camera/src/realsense_node_factory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index d4b9c100c6..4c0af3ac85 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -185,12 +185,12 @@ void RealSenseNodeFactory::onInit() catch(const std::exception& ex) { ROS_ERROR_STREAM("An exception has been thrown: " << ex.what()); - throw; + exit(1); } catch(...) { ROS_ERROR_STREAM("Unknown exception has occured!"); - throw; + exit(1); } } From 51300b5fe8f82fc99a1ce42117cbbe0ac6012297 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 09:20:18 +0300 Subject: [PATCH 029/131] update version to 2.2.5 --- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 0aca12feca..b663868604 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 4 +#define REALSENSE_ROS_PATCH_VERSION 5 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 06a9724c5b..81e48a89c0 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.4 + 2.2.5 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg From a328ca58520f34fc954d95c877eb908cb6a17298 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 10:22:22 +0300 Subject: [PATCH 030/131] Add accel test * add rs_rtabmap.launch * Add test for accel in d435i. Needs recorded file: 20190527_D435i.bag * add d435i accel unit-test --- .travis.yml | 4 +- README.md | 16 +++- realsense2_camera/launch/rs_from_file.launch | 4 +- realsense2_camera/launch/rs_rtabmap.launch | 37 +++++++++ realsense2_camera/scripts/rs2_listener.py | 29 +++++++ realsense2_camera/scripts/rs2_test.py | 79 +++++++++++++++++--- 6 files changed, 154 insertions(+), 15 deletions(-) create mode 100644 realsense2_camera/launch/rs_rtabmap.launch diff --git a/.travis.yml b/.travis.yml index 9cba36b804..4989e6c411 100644 --- a/.travis.yml +++ b/.travis.yml @@ -41,7 +41,9 @@ install: # download data: - bag_filename="http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors.bag"; - wget $bag_filename -P "records/" - + - bag_filename="http://realsense-hw-public.s3-eu-west-1.amazonaws.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag"; + - wget $bag_filename -P "records/" + # Run test: script: - python src/realsense/realsense2_camera/scripts/rs2_test.py --all diff --git a/README.md b/README.md index 8a30f606b5..a1301c1981 100644 --- a/README.md +++ b/README.md @@ -179,8 +179,8 @@ roslaunch realsense2_camera demo_t265.launch ### About Frame ID The wrapper publishes static transformations(TFs). The Frame Ids are divided into 3 groups: -- ROS convention frames: follow the format of _<_stream>"_frame" for example: camera_depth_frame, camera_infra1_frame, etc. -- Original frame coordinate system: with the suffix of <_optical_frame>. For example: camera_infra1_optical_frame. Check the device documentation for specific coordinate system for each stream. +- ROS convention frames: follow the format of \_<\_stream>"\_frame" for example: camera_depth_frame, camera_infra1_frame, etc. +- Original frame coordinate system: with the suffix of <\_optical_frame>. For example: camera_infra1_optical_frame. Check the device documentation for specific coordinate system for each stream. - base_link: For example: camera_link. A reference frame for the device. In D400 series and SR300 it is the depth frame. In T265, the pose frame. @@ -190,6 +190,18 @@ For viewing included models, a separate package is included. For example: roslaunch realsense2_description view_d415_model.launch ``` +### Unit tests: +Unit-tests are based on bag files saved on S3 server. These can be downloaded using the following commands: +```bash +cd catkin_ws +wget "http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors.bag" -P "records/" +wget "http://realsense-hw-public.s3-eu-west-1.amazonaws.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag" -P "records/" +``` +Then, unit-tests can be run using the following command: +```bash +python src/realsense/realsense2_camera/scripts/rs2_test.py --all +``` + ## Packages using RealSense ROS Camera | Title | Links | | ----- | ----- | diff --git a/realsense2_camera/launch/rs_from_file.launch b/realsense2_camera/launch/rs_from_file.launch index e043bb7cff..c428e5292e 100644 --- a/realsense2_camera/launch/rs_from_file.launch +++ b/realsense2_camera/launch/rs_from_file.launch @@ -26,8 +26,8 @@ - - + + diff --git a/realsense2_camera/launch/rs_rtabmap.launch b/realsense2_camera/launch/rs_rtabmap.launch new file mode 100644 index 0000000000..5436f743ca --- /dev/null +++ b/realsense2_camera/launch/rs_rtabmap.launch @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index 0169b77968..91c5889c78 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -4,11 +4,13 @@ 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 +from sensor_msgs.msg import Imu as msg_Imu import numpy as np from cv_bridge import CvBridge, CvBridgeError import inspect import ctypes import struct +import tf def pc2_to_xyzrgb(point): @@ -37,6 +39,7 @@ def __init__(self, params={}): self.time = params.get('time', None) self.node_name = params.get('node_name', 'rs2_listener') self.bridge = CvBridge() + self.listener = None 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}, @@ -44,10 +47,36 @@ def __init__(self, params={}): '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}, 'static_tf': {'topic': '/camera/aligned_depth_to_color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, + 'accelStream': {'topic': '/camera/accel/sample', 'callback': self.imuCallback, 'msg_type': msg_Imu}, } self.func_data = dict() + def imuCallback(self, theme_name): + def _imuCallback(data): + if self.listener is None: + self.listener = tf.TransformListener() + self.prev_time = time.time() + self.func_data[theme_name].setdefault('value', []) + self.func_data[theme_name].setdefault('ros_value', []) + try: + frame_id = data.header.frame_id + value = data.linear_acceleration + + (trans,rot) = self.listener.lookupTransform('/camera_link', frame_id, rospy.Time(0)) + quat = tf.transformations.quaternion_matrix(rot) + point = np.matrix([value.x, value.y, value.z, 1], dtype='float32') + point.resize((4, 1)) + rotated = quat*point + rotated.resize(1,4) + rotated = np.array(rotated)[0][:3] + except Exception as e: + print(e) + return + self.func_data[theme_name]['value'].append(value) + self.func_data[theme_name]['ros_value'].append(rotated) + return _imuCallback + def imageColorCallback(self, theme_name): def _imageColorCallback(data): self.prev_time = time.time() diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index 75c032db2a..0a83ffe439 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -15,6 +15,57 @@ global tf_timeout tf_timeout = 5 +def ImuGetData(rec_filename, topic): + bag = rosbag.Bag(rec_filename) + res = dict() + res['value'] = None + res['max_diff'] = [0,0,0] + for topic, msg, t in bag.read_messages(topics=topic): + value = np.array([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]) + if res['value'] is None: + res['value'] = value + else: + diff = abs(value - res['value']) + res['max_diff'] = [max(diff[x], res['max_diff'][x]) for x in range(len(diff))] + res['max_diff'] = np.array(res['max_diff']) + return res + +def AccelGetData(rec_filename): + return ImuGetData(rec_filename, '/device_0/sensor_2/Accel_0/imu/data') + +def AccelGetDataDeviceStandStraight(rec_filename): + gt_data = AccelGetData(rec_filename) + gt_data['ros_value'] = np.array([0.63839424, 0.05380408, 9.85343552]) + gt_data['ros_max_diff'] = np.array([1.97013582e-02, 4.65862500e-09, 2.06165277e-02]) + return gt_data + +def ImuTest(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'] + try: + v_data = np.array([data['value'][0].x, data['value'][0].y, data['value'][0].z]) + v_gt_data = gt_data['value'] + diff = v_data - v_gt_data + max_diff = abs(diff).max() + msg = 'original accel: Expect max diff of %.3f. Got %.3f.' % (gt_data['max_diff'].max(), max_diff) + print msg + if max_diff > gt_data['max_diff'].max(): + return False, msg + + v_data = data['ros_value'][0] + v_gt_data = gt_data['ros_value'] + diff = v_data - v_gt_data + max_diff = abs(diff).max() + msg = 'rotated to ROS: Expect max diff of %.3f. Got %.3f.' % (gt_data['ros_max_diff'].max(), max_diff) + print msg + if max_diff > gt_data['ros_max_diff'].max(): + return False, msg + except Exception as e: + msg = '%s' % e + print 'Test Failed: %s' % msg + return False, msg + return True, '' + def ImageGetData(rec_filename, topic): bag = rosbag.Bag(rec_filename) bridge = CvBridge() @@ -74,7 +125,7 @@ def ImageColorTest(data, gt_data): # images are within epsilon of gt_data['avg'] try: channels = list(set(data['num_channels'])) - msg = 'Expect %d channels. Got %d channels.' % (channels[0], gt_data['num_channels']) + msg = 'Expect %d channels. Got %d channels.' % (gt_data['num_channels'], channels[0]) print msg if len(channels) > 1 or channels[0] != gt_data['num_channels']: return False, msg @@ -176,6 +227,9 @@ def staticTFTest(data, gt_data): ('camera_infra1_frame', 'camera_color_frame'): ([-0.00010158783697988838, 0.014841210097074509, -0.00022671300393994898], [-0.0008337442995980382, 0.0010442184284329414, -0.0009920650627464056, 0.9999986290931702])} , 'test_func': staticTFTest}, + 'accel_up': {'listener_theme': 'accelStream', + 'data_func': AccelGetDataDeviceStandStraight, + 'test_func': ImuTest}, } @@ -189,16 +243,18 @@ def run_test(test, listener_res): def print_results(results): title = 'TEST RESULTS' - headers = ['test name', 'score', 'message'] - 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 - col_2_width = max([len(headers[2])] + [len(test[1][1]) for test in results]) + 1 - total_width = col_0_width + col_1_width + headers = ['index', 'test name', 'score', 'message'] + col_0_width = len(headers[0]) + 1 + col_1_width = max([len(headers[1])] + [len(test[0]) for test in results]) + 1 + col_2_width = max([len(headers[2]), len('OK'), len('FAILED')]) + 1 + col_3_width = max([len(headers[3])] + [len(test[1][1]) for test in results]) + 1 + total_width = col_0_width + col_1_width + col_2_width + col_3_width + print print ('{:^%ds}'%total_width).format(title) print '-'*total_width - print ('{:<%ds}{:>%ds} : {:<%ds}' % (col_0_width, col_1_width, col_2_width)).format('test name', 'score', 'message') - print '-'*(col_0_width-1) + ' '*2 + '-'*(col_1_width-1) + ' '*3 + '-'*(col_2_width-1) - print '\n'.join([('{:<%ds}{:>%ds} : {:%ds} : {:<%ds}' % (col_0_width, col_1_width, col_2_width, col_3_width)).format(*headers) + print '-'*(col_0_width-1) + ' '*1 + '-'*(col_1_width-1) + ' '*2 + '-'*(col_2_width-1) + ' '*3 + '-'*(col_3_width-1) + print '\n'.join([('{:<%dd}{:<%ds}{:>%ds} : {: 0 @@ -286,6 +344,7 @@ def main(): {'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'}}, {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': './records/outdoors.bag'}}, + {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}}, ] # Normalize parameters: From b572924755e704ba64f2369df4e18fef71fa8c25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mario=20L=C3=BCder?= Date: Sun, 2 Jun 2019 09:24:21 +0200 Subject: [PATCH 031/131] Added ddynamic_reconfigure installation note (#795) --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index a1301c1981..ee29200606 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,7 @@ mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src/ ``` - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' +- Make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) ```bash catkin_init_workspace From 891c7ff68fd1efed5fed3008b164232a72e4bbc5 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 10:25:20 +0300 Subject: [PATCH 032/131] fix remarks to imu test --- realsense2_camera/scripts/rs2_test.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index 0a83ffe439..6949cc69f7 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -16,6 +16,9 @@ tf_timeout = 5 def ImuGetData(rec_filename, topic): + # res['value'] = first value of topic. + # res['max_diff'] = max difference between returned value and all other values of topic in recording. + bag = rosbag.Bag(rec_filename) res = dict() res['value'] = None @@ -40,8 +43,8 @@ def AccelGetDataDeviceStandStraight(rec_filename): return gt_data def ImuTest(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'] + # check that the imu data received is the same as in the recording. + # check that in the rotated imu received the g-accelartation is pointing up according to ROS standards. try: v_data = np.array([data['value'][0].x, data['value'][0].y, data['value'][0].z]) v_gt_data = gt_data['value'] From 205d6b62ef4f3c6352dbbc75333c2df4b85e40d9 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 10:38:27 +0300 Subject: [PATCH 033/131] fixed README.md file. --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index ee29200606..ba1e7e4a08 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,8 @@ mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src/ ``` - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' -- Make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) +- Make sure all dependent packages are installed. You can check .travis.yml file for reference. +- Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) ```bash catkin_init_workspace From 032b3d38f6b31bd78e66db2c5326a62eb83135b3 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 14:02:47 +0300 Subject: [PATCH 034/131] add librealsense2.rdmanifest file --- realsense2_camera/librealsense2.rdmanifest | 37 ++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 realsense2_camera/librealsense2.rdmanifest diff --git a/realsense2_camera/librealsense2.rdmanifest b/realsense2_camera/librealsense2.rdmanifest new file mode 100644 index 0000000000..68f129899c --- /dev/null +++ b/realsense2_camera/librealsense2.rdmanifest @@ -0,0 +1,37 @@ +uri: 'https://raw.githubusercontent.com/magazino/pylon_camera/indigo-devel/rosdep/empty.tar' +check-presence-script: | + #!/bin/bash + + if [ $(dpkg-query -W -f='${Status}' librealsense2-dkms 2>/dev/null | grep -c "ok installed") -eq 0 ]; + then + exit 1 + else + exit 0 + fi + + +install-script: | + #!/bin/bash + + # Install + if [[ "$EUID" -ne 0 ]]; then + # Install add-apt-repository + sudo apt-get install -y software-properties-common + # Register the server's public key + sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u + # Install the libraries + sudo apt-get install -y librealsense2-dkms librealsense2-dev + else + # Install add-apt-repository + apt-get install -y software-properties-common + # Register the server's public key + apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo xenial main" -u + # Install the libraries + apt-get install -y librealsense2-dkms librealsense2-dev + fi + + exit $? \ No newline at end of file From c54c21aee993199d3580551af486f5984559fba1 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 14:26:07 +0300 Subject: [PATCH 035/131] update version to 2.2.6 --- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index b663868604..eb1c6a10b1 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 5 +#define REALSENSE_ROS_PATCH_VERSION 6 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 81e48a89c0..06c6f3e268 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.5 + 2.2.6 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 8398059579..c5d6c68b8e 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.0 + 2.2.6 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From c15066fd2aa73c0c786782a84607ba9e62c1bd51 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 2 Jun 2019 14:33:44 +0300 Subject: [PATCH 036/131] add librealsense2 dependency --- realsense2_camera/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 06c6f3e268..83a8ab39bb 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -25,6 +25,7 @@ tf ddynamic_reconfigure diagnostic_updater + librealsense2 From 9ba6c2dfbdf0dfa085e5771be5e64dd54d174471 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 3 Jun 2019 15:40:06 +0300 Subject: [PATCH 037/131] disable static_tf_1 test - not working on Travis. Modified points_cloud_1 test - make more robust --- realsense2_camera/scripts/rs2_listener.py | 5 +---- realsense2_camera/scripts/rs2_test.py | 22 ++++++++++++++-------- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index 91c5889c78..2ca24b6351 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -46,7 +46,7 @@ def __init__(self, params={}): '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}, - 'static_tf': {'topic': '/camera/aligned_depth_to_color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, + 'static_tf': {'topic': '/camera/color/image_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, 'accelStream': {'topic': '/camera/accel/sample', 'callback': self.imuCallback, 'msg_type': msg_Imu}, } @@ -123,9 +123,6 @@ def _pointscloudCallback(data): # 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: diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index 6949cc69f7..ad37839339 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -170,16 +170,21 @@ def NotImageColorTest(data, gt_data): return (not res[0], res[1]) def PointCloudTest(data, gt_data): - msg = '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]) + width = np.array(data['width']).mean() + height = np.array(data['height']).mean() + msg = 'Expect image size %d(+-%d), %d. Got %d, %d.' % (gt_data['width'][0], gt_data['width'][1], gt_data['height'][0], width, height) print msg - if abs(data['width'][0] - gt_data['width'][0]) > gt_data['width'][1] or data['height'][0] != gt_data['height'][0]: + if abs(width - gt_data['width'][0]) > gt_data['width'][1] or height != gt_data['height'][0]: return False, msg - msg = 'Expect average position of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][:3], gt_data['epsilon'][0], data['avg'][0][:3]) + mean_pos = np.array([xx[:3] for xx in data['avg']]).mean(0) + msg = 'Expect average position of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][:3], gt_data['epsilon'][0], mean_pos) print msg - if abs(data['avg'][0][:3] - gt_data['avg'][0][:3]).max() > gt_data['epsilon'][0]: + if abs(mean_pos - gt_data['avg'][0][:3]).max() > gt_data['epsilon'][0]: return False, msg - msg = '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]: + mean_col = np.array([xx[3:] for xx in data['avg']]).mean(0) + msg = 'Expect average color of %s (+-%.3f). Got average of %s.' % (gt_data['avg'][0][3:], gt_data['epsilon'][1], mean_col) + print msg + if abs(mean_col - gt_data['avg'][0][3:]).max() > gt_data['epsilon'][1]: return False, msg return True, '' @@ -207,7 +212,8 @@ def staticTFTest(data, gt_data): 'test_func': NotImageColorTest}, '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]}, - 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 125, 116, 102])], 'epsilon': [0.02, 2]}, + # 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 125, 116, 102])], 'epsilon': [0.02, 2]}, + 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 123, 124, 113])], 'epsilon': [0.04, 5]}, 'test_func': PointCloudTest}, 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1', 'data_func': ImageDepthGetData, @@ -346,7 +352,7 @@ def main(): {'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'}}, - {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': './records/outdoors.bag'}}, + # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': './records/outdoors.bag'}}, # Not working in Travis... {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}}, ] From 1eb0d68781964857841ddd54ae6f249f00a65690 Mon Sep 17 00:00:00 2001 From: Antoine Hoarau Date: Tue, 4 Jun 2019 21:43:53 +0200 Subject: [PATCH 038/131] add eigen dependency --- realsense2_camera/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 83a8ab39bb..0a97c3fcdf 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -13,6 +13,7 @@ Sergey Dorodnicov Doron Hirshberg catkin + eigen image_transport cv_bridge nav_msgs From 2b3365f5c96fcafa53412881e8e4484a6648bc23 Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 6 Jun 2019 07:12:22 +0300 Subject: [PATCH 039/131] rename librealsense2.rdmanifest to librealsense2_xenial.rdmanifest add file: librealsense2_bionic.rdmanifest --- .../librealsense2_bionic.rdmanifest | 37 +++++++++++++++++++ ...nifest => librealsense2_xenial.rdmanifest} | 0 2 files changed, 37 insertions(+) create mode 100644 realsense2_camera/librealsense2_bionic.rdmanifest rename realsense2_camera/{librealsense2.rdmanifest => librealsense2_xenial.rdmanifest} (100%) diff --git a/realsense2_camera/librealsense2_bionic.rdmanifest b/realsense2_camera/librealsense2_bionic.rdmanifest new file mode 100644 index 0000000000..092c454a88 --- /dev/null +++ b/realsense2_camera/librealsense2_bionic.rdmanifest @@ -0,0 +1,37 @@ +uri: 'https://raw.githubusercontent.com/magazino/pylon_camera/indigo-devel/rosdep/empty.tar' +check-presence-script: | + #!/bin/bash + + if [ $(dpkg-query -W -f='${Status}' librealsense2-dkms 2>/dev/null | grep -c "ok installed") -eq 0 ]; + then + exit 1 + else + exit 0 + fi + + +install-script: | + #!/bin/bash + + # Install + if [[ "$EUID" -ne 0 ]]; then + # Install add-apt-repository + sudo apt-get install -y software-properties-common + # Register the server's public key + sudo apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + sudo add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u + # Install the libraries + sudo apt-get install -y librealsense2-dkms librealsense2-dev + else + # Install add-apt-repository + apt-get install -y software-properties-common + # Register the server's public key + apt-key adv --keyserver keys.gnupg.net --recv-key C8B3A55A6F3EFCDE || apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE + # Add the server to the list of repositories + add-apt-repository "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" -u + # Install the libraries + apt-get install -y librealsense2-dkms librealsense2-dev + fi + + exit $? \ No newline at end of file diff --git a/realsense2_camera/librealsense2.rdmanifest b/realsense2_camera/librealsense2_xenial.rdmanifest similarity index 100% rename from realsense2_camera/librealsense2.rdmanifest rename to realsense2_camera/librealsense2_xenial.rdmanifest From 157db37da61e6d98076766ce2ffe9457a5b64e0a Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 10 Jun 2019 16:06:32 +0300 Subject: [PATCH 040/131] publish imu_info --- realsense2_camera/src/base_realsense_node.cpp | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 39a2b32ac1..62e0883973 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -684,8 +684,6 @@ void BaseRealSenseNode::setupPublishers() 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); - - _info_publisher[GYRO] = _node_handle.advertise("imu_info", 1, true); } else { @@ -1217,8 +1215,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) 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()) + if (0 != _imu_publishers[stream_index].getNumSubscribers()) { double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); @@ -1252,6 +1249,13 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) _imu_publishers[stream_index].publish(imu_msg); ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); } + if (0 != _info_publisher[stream_index].getNumSubscribers()) + { + IMUInfo info_msg = getImuInfo(stream_index); + info_msg.header.frame_id = _optical_frame_id[stream_index]; + _info_publisher[stream_index].publish(info_msg); + ROS_DEBUG("Publish info for %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + } } void BaseRealSenseNode::pose_callback(rs2::frame frame) @@ -1997,14 +2001,6 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) 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"; - } - else if (ACCEL == stream_index) - { - info.header.frame_id = "imu_accel"; - } auto index = 0; for (int i = 0; i < 3; ++i) From 71faa5ce3041ba374c24aa6a71659f333c72cca7 Mon Sep 17 00:00:00 2001 From: David W Date: Tue, 11 Jun 2019 10:51:35 +0100 Subject: [PATCH 041/131] Fix empty cmake variable and dependency on librealsense (#747) --- realsense2_camera/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 476090d3b1..46818460aa 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -60,10 +60,11 @@ generate_messages( std_msgs ) +set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories( include + ${realsense2_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} - ${realsense_INCLUDE_DIR} ) # RealSense ROS Node @@ -90,9 +91,8 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - target_include_directories(${PROJECT_NAME} - PRIVATE ${realsense_INCLUDE_DIR}) + PRIVATE ${realsense2_INCLUDE_DIR}) target_link_libraries(${PROJECT_NAME} ${realsense2_LIBRARY} From a3649af6b63cac2ff07bf006a253691f69146497 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 11 Jun 2019 13:33:36 +0300 Subject: [PATCH 042/131] update apt-key for ROS repos. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 4989e6c411..b30798a25a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,7 +13,7 @@ before_install: install: # install ROS: - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - - sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 + - sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt-get update -qq - sudo apt-get install ros-kinetic-ros-base -y - sudo rosdep init From d274d587e2dcb7cb64335e0fdb02bd70093a2368 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 11 Jun 2019 13:39:27 +0300 Subject: [PATCH 043/131] update apt-key for ROS repos. --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 4989e6c411..b30798a25a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -13,7 +13,7 @@ before_install: install: # install ROS: - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' - - sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 + - sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 - sudo apt-get update -qq - sudo apt-get install ros-kinetic-ros-base -y - sudo rosdep init From 80630285156744bbfd7e43902dbf7968ff62493e Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 11 Jun 2019 15:01:54 +0300 Subject: [PATCH 044/131] add external_manager option to rs_camera.launch --- realsense2_camera/launch/rs_camera.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index b2db68008e..230b98d94d 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -3,6 +3,8 @@ + + @@ -51,6 +53,8 @@ + + From 311456d36de8397d22b4438312c16c8bc88d96b2 Mon Sep 17 00:00:00 2001 From: baumanta Date: Wed, 5 Jun 2019 13:25:33 +0200 Subject: [PATCH 045/131] enable reaquire tag whan launching node --- realsense2_camera/launch/includes/nodelet.launch.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 9d796b34f8..3937402d19 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -5,6 +5,7 @@ + @@ -84,8 +85,8 @@ - - + + From de1649a04ff7f96b549773fc709b3f8b2a81d397 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 12 Jun 2019 11:40:08 +0300 Subject: [PATCH 046/131] clarify installation process. --- README.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ba1e7e4a08..8d5533ecec 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,12 @@ mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src/ ``` - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' +```bashrc +git clone https://github.com/IntelRealSense/realsense-ros.git +cd realsense-ros/ +git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1` +cd .. +``` - Make sure all dependent packages are installed. You can check .travis.yml file for reference. - Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) @@ -98,8 +104,8 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc. - **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)** - **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. Under the new topic, all the fields in the Imu message are filled out. - - linear_interpolation: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. - - copy: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. + - **linear_interpolation**: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. + - **copy**: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. From 9340af09d961c9b0f9bbd104bc2cd3902aeecf15 Mon Sep 17 00:00:00 2001 From: Stephan Sundermann Date: Tue, 26 Mar 2019 14:25:59 +0100 Subject: [PATCH 047/131] Add option to use external nodelet manager Right now a nodelet manager is always created when running the realsense node. This is not always a wanted behaviour as a nodelet manager might already be running somewhere else in the ROS system. This patch adds a parameter to disable launching a new nodelet manager. In this case the nodelet manager must be provided using the manager arg. The new parameter is set to false by default, so existing behaviour is not changed. --- realsense2_camera/launch/includes/nodelet.launch.xml | 3 ++- realsense2_camera/launch/rs_rgbd.launch | 2 ++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index ab9827ce64..9d796b34f8 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -1,4 +1,5 @@ + @@ -83,7 +84,7 @@ - + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index 644656405e..835143c881 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -42,6 +42,7 @@ Processing enabled by this node: + @@ -112,6 +113,7 @@ Processing enabled by this node: + From fddfe3b4a784d321287e5529e326a6a9566245e8 Mon Sep 17 00:00:00 2001 From: Stephan Sundermann Date: Fri, 29 Mar 2019 11:54:02 +0100 Subject: [PATCH 048/131] Register dynamic options in parent node handle If running in an external nodelet manager this makes sure dynamic options are in the right namespace. This does not change existing behaviour. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 62e0883973..8a761ef631 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -261,7 +261,7 @@ std::string create_graph_resource_name(const std::string &original_name) void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name) { - ros::NodeHandle nh1(module_name); + ros::NodeHandle nh1(nh, module_name); std::shared_ptr ddynrec = std::make_shared(nh1); for (auto i = 0; i < RS2_OPTION_COUNT; i++) { From fc1fd10d9fb6fa468b974300e639b3dbec9e0bfb Mon Sep 17 00:00:00 2001 From: David W Date: Tue, 11 Jun 2019 10:51:35 +0100 Subject: [PATCH 049/131] Fix empty cmake variable and dependency on librealsense (#747) --- realsense2_camera/CMakeLists.txt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 476090d3b1..46818460aa 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -60,10 +60,11 @@ generate_messages( std_msgs ) +set(CMAKE_NO_SYSTEM_FROM_IMPORTED true) include_directories( include + ${realsense2_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} - ${realsense_INCLUDE_DIR} ) # RealSense ROS Node @@ -90,9 +91,8 @@ add_library(${PROJECT_NAME} add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) - target_include_directories(${PROJECT_NAME} - PRIVATE ${realsense_INCLUDE_DIR}) + PRIVATE ${realsense2_INCLUDE_DIR}) target_link_libraries(${PROJECT_NAME} ${realsense2_LIBRARY} From e2b7dd4ed2e75bde1e848d54f6908e019d915d21 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 11 Jun 2019 15:01:54 +0300 Subject: [PATCH 050/131] add external_manager option to rs_camera.launch --- realsense2_camera/launch/rs_camera.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index b2db68008e..230b98d94d 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -3,6 +3,8 @@ + + @@ -51,6 +53,8 @@ + + From 025a19df901c92f9059a524dc574663863b0db0e Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 12 Jun 2019 11:40:08 +0300 Subject: [PATCH 051/131] clarify installation process. --- README.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ba1e7e4a08..8d5533ecec 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,12 @@ mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src/ ``` - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' +```bashrc +git clone https://github.com/IntelRealSense/realsense-ros.git +cd realsense-ros/ +git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1` +cd .. +``` - Make sure all dependent packages are installed. You can check .travis.yml file for reference. - Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) @@ -98,8 +104,8 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc. - **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)** - **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. Under the new topic, all the fields in the Imu message are filled out. - - linear_interpolation: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. - - copy: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. + - **linear_interpolation**: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. + - **copy**: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. From 95a08320e1b516b5a75f13d7cf9205b1b2bfdbc9 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 12 Jun 2019 12:09:07 +0300 Subject: [PATCH 052/131] publish gyro/imu_info and accel/imu_info only once. --- .../include/base_realsense_node.h | 1 + realsense2_camera/msg/IMUInfo.msg | 4 +-- realsense2_camera/src/base_realsense_node.cpp | 28 +++++++++++++------ 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 1d164edfdd..4a10115b2e 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -194,6 +194,7 @@ namespace realsense2_camera void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); void publishStaticTransforms(); + void publishIntrinsics(); 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; diff --git a/realsense2_camera/msg/IMUInfo.msg b/realsense2_camera/msg/IMUInfo.msg index 68f47b7757..85e103d2ab 100644 --- a/realsense2_camera/msg/IMUInfo.msg +++ b/realsense2_camera/msg/IMUInfo.msg @@ -1,6 +1,4 @@ -# header.frame_id is either set to "imu_accel" or "imu_gyro" -# to distinguish between "accel" and "gyro" info. -std_msgs/Header header +string frame_id float64[12] data float64[3] noise_variances float64[3] bias_variances diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 8a761ef631..b4d807baee 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -174,6 +174,7 @@ void BaseRealSenseNode::publishTopics() setupStreams(); setupFilters(); publishStaticTransforms(); + publishIntrinsics(); ROS_INFO_STREAM("RealSense Node Is Up!"); } @@ -690,13 +691,11 @@ void BaseRealSenseNode::setupPublishers() if (_enable[GYRO]) { _imu_publishers[GYRO] = _node_handle.advertise("gyro/sample", 100); - _info_publisher[GYRO] = _node_handle.advertise("gyro/imu_info", 1, true); } if (_enable[ACCEL]) { _imu_publishers[ACCEL] = _node_handle.advertise("accel/sample", 100); - _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); } } if (_enable[POSE]) @@ -1249,13 +1248,6 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) _imu_publishers[stream_index].publish(imu_msg); ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); } - if (0 != _info_publisher[stream_index].getNumSubscribers()) - { - IMUInfo info_msg = getImuInfo(stream_index); - info_msg.header.frame_id = _optical_frame_id[stream_index]; - _info_publisher[stream_index].publish(info_msg); - ROS_DEBUG("Publish info for %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); - } } void BaseRealSenseNode::pose_callback(rs2::frame frame) @@ -1819,6 +1811,23 @@ void BaseRealSenseNode::publishStaticTransforms() } +void BaseRealSenseNode::publishIntrinsics() +{ + if (_enable[GYRO]) + { + _info_publisher[GYRO] = _node_handle.advertise("gyro/imu_info", 1, true); + IMUInfo info_msg = getImuInfo(GYRO); + _info_publisher[GYRO].publish(info_msg); + } + + if (_enable[ACCEL]) + { + _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); + IMUInfo info_msg = getImuInfo(ACCEL); + _info_publisher[ACCEL].publish(info_msg); + } +} + void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) { size_t i; @@ -2003,6 +2012,7 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) } auto index = 0; + info.frame_id = _optical_frame_id[stream_index]; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 4; ++j) From d3867401225ab819fdae1efdb57909f294e99e3e Mon Sep 17 00:00:00 2001 From: Nick Giancola Date: Tue, 25 Jun 2019 20:35:26 -0700 Subject: [PATCH 053/131] Make T265 less noisy in the info log (#823) --- realsense2_camera/src/t265_realsense_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp index 854478a88b..77fe76b8d7 100644 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ b/realsense2_camera/src/t265_realsense_node.cpp @@ -59,12 +59,12 @@ void T265RealsenseNode::setupSubscribers() void T265RealsenseNode::odom_in_callback(const nav_msgs::Odometry::ConstPtr& msg) { - ROS_INFO("Got in_odom message"); + ROS_DEBUG("Got in_odom message"); rs2_vector velocity {-(float)(msg->twist.twist.linear.y), (float)(msg->twist.twist.linear.z), -(float)(msg->twist.twist.linear.x)}; - ROS_INFO_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z); + ROS_DEBUG_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z); _wo_snr.send_wheel_odometry(0, 0, velocity); } From 7fc0f9b2131553709ed43d77561d2c7cf83733d9 Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 4 Jul 2019 14:07:50 +0300 Subject: [PATCH 054/131] cleaning --- realsense2_camera/include/base_realsense_node.h | 2 ++ realsense2_camera/include/realsense_node_factory.h | 2 -- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4a10115b2e..4564107fa2 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include #include diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index b692748d96..b7bc536512 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -15,8 +15,6 @@ #include #include #include -#include -#include #include #include #include From c1010ab2992b80ee2aea2ef92757a579b8c02686 Mon Sep 17 00:00:00 2001 From: Andrew Saba Date: Thu, 4 Jul 2019 07:12:31 -0400 Subject: [PATCH 055/131] Updated README to include notice of running multiple T265 cameras not being currently supported (#834) --- README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/README.md b/README.md index 8d5533ecec..d849ca8784 100644 --- a/README.md +++ b/README.md @@ -137,6 +137,8 @@ rosrun rqt_reconfigure rqt_reconfigure

### Work with multiple cameras +**Important Notice:** Launching multiple T265 cameras is currently not supported. This will be addressed in a later version. + Here is an example of how to start the camera node and streaming with two cameras using the [rs_multiple_devices.launch](./realsense2_camera/launch/rs_multiple_devices.launch). ```bash roslaunch realsense2_camera rs_multiple_devices.launch serial_no_camera1:= serial_no_camera2:= @@ -219,6 +221,7 @@ python src/realsense/realsense2_camera/scripts/rs2_test.py --all * This ROS node does not currently support [ROS Lunar Loggerhead](http://wiki.ros.org/lunar). * This ROS node does not currently work with [ROS 2](https://github.com/ros2/ros2/wiki). * This ROS node currently does not provide the unit-tests which ensure the proper operation of the camera. Future versions of the node will provide ROS compatible unit-tests. +* This ROS node currently does not support running multiple T265 cameras at once. This will be addressed in a future update. ## License Copyright 2018 Intel Corporation From 387f12b7429eac216cb012600640a150dab52b12 Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 4 Jul 2019 14:14:29 +0300 Subject: [PATCH 056/131] fixed bug: advertise unsupported streams. (#811) * fixed bug: advertise unsupported streams. * constraints on accel_up test - loosen up a bit to compensate for different in frames arriving from file. --- realsense2_camera/scripts/rs2_test.py | 2 +- realsense2_camera/src/base_realsense_node.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index ad37839339..ff62fdce5b 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -39,7 +39,7 @@ def AccelGetData(rec_filename): def AccelGetDataDeviceStandStraight(rec_filename): gt_data = AccelGetData(rec_filename) gt_data['ros_value'] = np.array([0.63839424, 0.05380408, 9.85343552]) - gt_data['ros_max_diff'] = np.array([1.97013582e-02, 4.65862500e-09, 2.06165277e-02]) + gt_data['ros_max_diff'] = np.array([1.97013582e-02, 4.65862500e-09, 4.06165277e-02]) return gt_data def ImuTest(data, gt_data): diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 44706c0d43..68558afe34 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -170,6 +170,7 @@ void BaseRealSenseNode::publishTopics() getParameters(); setupDevice(); setupErrorCallback(); + enable_devices(); setupPublishers(); setupStreams(); setupFilters(); @@ -866,6 +867,7 @@ void BaseRealSenseNode::enable_devices() if (profile.stream_type() != elem.first) continue; ROS_WARN_STREAM("fps: " << profile.fps() << ". format: " << profile.format()); } + _enable[elem] = false; } } } @@ -1550,7 +1552,6 @@ void BaseRealSenseNode::setBaseTime(double frame_time, bool warn_no_metadata) void BaseRealSenseNode::setupStreams() { ROS_INFO("setupStreams..."); - enable_devices(); try{ // Publish image stream info for (auto& profiles : _enabled_profiles) From 2452cc263e0e167ed3f4e8f9229feb02a3bb839d Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 4 Jul 2019 14:25:08 +0300 Subject: [PATCH 057/131] publish imu_info (#809) * publish imu_info * update apt-key for ROS repos. * Add option to use external nodelet manager * Register dynamic options in parent node handle * Fix empty cmake variable and dependency on librealsense (#747) * add external_manager option to rs_camera.launch * clarify installation process. * publish gyro/imu_info and accel/imu_info only once. * cleaning --- .../include/base_realsense_node.h | 3 ++ .../include/realsense_node_factory.h | 2 -- realsense2_camera/msg/IMUInfo.msg | 4 +-- realsense2_camera/src/base_realsense_node.cpp | 34 +++++++++++-------- 4 files changed, 24 insertions(+), 19 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 1d164edfdd..4564107fa2 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include #include @@ -194,6 +196,7 @@ namespace realsense2_camera void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); void publishStaticTransforms(); + void publishIntrinsics(); 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; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index b692748d96..b7bc536512 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -15,8 +15,6 @@ #include #include #include -#include -#include #include #include #include diff --git a/realsense2_camera/msg/IMUInfo.msg b/realsense2_camera/msg/IMUInfo.msg index 68f47b7757..85e103d2ab 100644 --- a/realsense2_camera/msg/IMUInfo.msg +++ b/realsense2_camera/msg/IMUInfo.msg @@ -1,6 +1,4 @@ -# header.frame_id is either set to "imu_accel" or "imu_gyro" -# to distinguish between "accel" and "gyro" info. -std_msgs/Header header +string frame_id float64[12] data float64[3] noise_variances float64[3] bias_variances diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 68558afe34..364db42516 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -175,6 +175,7 @@ void BaseRealSenseNode::publishTopics() setupStreams(); setupFilters(); publishStaticTransforms(); + publishIntrinsics(); ROS_INFO_STREAM("RealSense Node Is Up!"); } @@ -685,21 +686,17 @@ void BaseRealSenseNode::setupPublishers() 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); - - _info_publisher[GYRO] = _node_handle.advertise("imu_info", 1, true); } else { if (_enable[GYRO]) { _imu_publishers[GYRO] = _node_handle.advertise("gyro/sample", 100); - _info_publisher[GYRO] = _node_handle.advertise("gyro/imu_info", 1, true); } if (_enable[ACCEL]) { _imu_publishers[ACCEL] = _node_handle.advertise("accel/sample", 100); - _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); } } if (_enable[POSE]) @@ -1219,8 +1216,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) 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()) + if (0 != _imu_publishers[stream_index].getNumSubscribers()) { double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); @@ -1816,6 +1812,23 @@ void BaseRealSenseNode::publishStaticTransforms() } +void BaseRealSenseNode::publishIntrinsics() +{ + if (_enable[GYRO]) + { + _info_publisher[GYRO] = _node_handle.advertise("gyro/imu_info", 1, true); + IMUInfo info_msg = getImuInfo(GYRO); + _info_publisher[GYRO].publish(info_msg); + } + + if (_enable[ACCEL]) + { + _info_publisher[ACCEL] = _node_handle.advertise("accel/imu_info", 1, true); + IMUInfo info_msg = getImuInfo(ACCEL); + _info_publisher[ACCEL].publish(info_msg); + } +} + void reverse_memcpy(unsigned char* dst, const unsigned char* src, size_t n) { size_t i; @@ -1998,16 +2011,9 @@ IMUInfo BaseRealSenseNode::getImuInfo(const stream_index_pair& stream_index) 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"; - } - else if (ACCEL == stream_index) - { - info.header.frame_id = "imu_accel"; - } auto index = 0; + info.frame_id = _optical_frame_id[stream_index]; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 4; ++j) From 71cbe920c43529f5afba4769bdfaceddd78c9a29 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 10 Jul 2019 13:52:04 +0300 Subject: [PATCH 058/131] move ddynamic_reconfigure settings to before start streaming. (#843) --- realsense2_camera/src/base_realsense_node.cpp | 3 ++- realsense2_camera/src/realsense_node_factory.cpp | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 364db42516..eab80d8a3b 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -169,6 +169,7 @@ void BaseRealSenseNode::publishTopics() { getParameters(); setupDevice(); + registerDynamicReconfigCb(_node_handle); setupErrorCallback(); enable_devices(); setupPublishers(); @@ -296,7 +297,7 @@ void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options { if (option_value < op_range.min || op_range.max < option_value) { - ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option + ROS_WARN_STREAM("Param '" << nh1.resolveName(option_name) << "' has value " << option_value << " outside the range [" << op_range.min << ", " << op_range.max << "]. Using current sensor value " << sensor_option_value << " instead."); option_value = sensor_option_value; diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 4c0af3ac85..290fc2a40d 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -229,7 +229,6 @@ void RealSenseNodeFactory::StartDevice() } assert(_realSenseNode); _realSenseNode->publishTopics(); - _realSenseNode->registerDynamicReconfigCb(nh); } void RealSenseNodeFactory::tryGetLogSeverity(rs2_log_severity& severity) const From 2ae0a55d23eb63b65e00ab503fde6a9862b91f6b Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 10 Jul 2019 15:10:54 +0300 Subject: [PATCH 059/131] Add support for SR305 (#844) fix bug in scaling depth. --- realsense2_camera/include/constants.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 4 ++-- realsense2_camera/src/realsense_node_factory.cpp | 1 + 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index eb1c6a10b1..1dc1f36aff 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -17,6 +17,7 @@ namespace realsense2_camera { const uint16_t SR300_PID = 0x0aa5; // SR300 + const uint16_t SR300v2_PID = 0x0B48; // SR305 const uint16_t RS400_PID = 0x0ad1; // PSR const uint16_t RS410_PID = 0x0ad2; // ASR const uint16_t RS415_PID = 0x0ad3; // ASRC diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index eab80d8a3b..d0434cc735 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -955,8 +955,8 @@ void BaseRealSenseNode::setupFilters() cv::Mat& BaseRealSenseNode::fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image) { - static const auto meter_to_mm = 0.001f; - if (abs(_depth_scale_meters - meter_to_mm) < 1e-6) + static const float meter_to_mm = 0.001f; + if (fabs(_depth_scale_meters - meter_to_mm) < 1e-6) { to_image = from_image; return to_image; diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 290fc2a40d..8c8fc10542 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -204,6 +204,7 @@ void RealSenseNodeFactory::StartDevice() switch(pid) { case SR300_PID: + case SR300v2_PID: case RS400_PID: case RS405_PID: case RS410_PID: From 31fa75ec835650d319f2d5abd3f7d0ab66fd37f4 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 10 Jul 2019 16:48:53 +0300 Subject: [PATCH 060/131] update version to 2.2.7 --- README.md | 2 +- realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index d849ca8784..809dd2fbfc 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine - #### 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 and librealsense-dkms packages. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.22.0) 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.24.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 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 46818460aa..47ecd84fac 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -34,7 +34,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2 2.22.0) +find_package(realsense2 2.24.0) 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() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 1dc1f36aff..61d84bd479 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 6 +#define REALSENSE_ROS_PATCH_VERSION 7 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 83a8ab39bb..62beda0767 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.6 + 2.2.7 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index c5d6c68b8e..9e934ba6eb 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.6 + 2.2.7 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From 13375ec5a1fcca0faf138fe3069dd711afcaa96e Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Thu, 11 Jul 2019 20:55:41 +0200 Subject: [PATCH 061/131] Dependency on xacro missing. --- realsense2_description/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 9e934ba6eb..57c833e173 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -13,5 +13,6 @@ Sergey Dorodnicov Doron Hirshberg catkin + xacro From 5ef502ebcee5508adc073d228b4ed52d07acd5f7 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 23 Jul 2019 13:03:04 +0300 Subject: [PATCH 062/131] fix bug: filters do not show in rqt_reconfigure. (#858) --- realsense2_camera/src/base_realsense_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index d0434cc735..067ec022fb 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -169,12 +169,12 @@ void BaseRealSenseNode::publishTopics() { getParameters(); setupDevice(); + setupFilters(); registerDynamicReconfigCb(_node_handle); setupErrorCallback(); enable_devices(); setupPublishers(); setupStreams(); - setupFilters(); publishStaticTransforms(); publishIntrinsics(); ROS_INFO_STREAM("RealSense Node Is Up!"); @@ -1773,7 +1773,7 @@ void BaseRealSenseNode::publishStaticTransforms() } } - // Publish Extinsics Topics: + // Publish Extrinsics Topics: if (_enable[DEPTH] && _enable[FISHEYE]) { From c2d3fafa76cad06f474d010205eb55582f517d40 Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 8 Aug 2019 11:02:19 +0300 Subject: [PATCH 063/131] add asic temperature and projector temperature to diagnostics topic. (#878) * add asic temperature and projector temperature to diagnostics topic. * fix bug relating reading from file. --- README.md | 1 + .../include/base_realsense_node.h | 25 ++++++++ realsense2_camera/src/base_realsense_node.cpp | 57 ++++++++++++++++++- .../src/realsense_node_factory.cpp | 2 - realsense2_camera/src/t265_realsense_node.cpp | 1 + 5 files changed, 81 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 809dd2fbfc..fec90df1bb 100644 --- a/README.md +++ b/README.md @@ -73,6 +73,7 @@ After running the above command with D435i attached, the following list of topic - /camera/gyro/sample - /camera/accel/imu_info - /camera/accel/sample +- /diagnostics The "/camera" prefix is the default and can be changed. Check the rs_multiple_devices.launch file for an example. If using D435 or D415, the gyro and accel topics wont be available. Likewise, other topics will be available when using T265 (see below). diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 4564107fa2..86171332fd 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -46,6 +46,24 @@ namespace realsense2_camera }; typedef std::pair> ImagePublisherWithFrequencyDiagnostics; + class TemperatureDiagnostics + { + public: + TemperatureDiagnostics(std::string name, std::string serial_no); + void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status); + + void update(double crnt_temperaure) + { + _crnt_temp = crnt_temperaure; + _updater.update(); + } + + private: + double _crnt_temp; + diagnostic_updater::Updater _updater; + + }; + class NamedFilter { public: @@ -137,6 +155,7 @@ namespace realsense2_camera std::map _depth_aligned_frame_id; ros::NodeHandle& _node_handle, _pnh; bool _align_depth; + std::vector _monitor_options; virtual void calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile); rs2::stream_profile getAProfile(const stream_index_pair& stream); @@ -195,6 +214,7 @@ namespace realsense2_camera cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image); void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); + void SetBaseStream(); void publishStaticTransforms(); void publishIntrinsics(); void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset); @@ -223,6 +243,8 @@ namespace realsense2_camera void frame_callback(rs2::frame frame); void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name); rs2_stream rs2_string_to_stream(std::string str); + void startMonitoring(); + void publish_temperature(); rs2::device _dev; std::map _sensors; @@ -285,6 +307,9 @@ namespace realsense2_camera std::map _depth_to_other_extrinsics_publishers; std::map _depth_to_other_extrinsics; + typedef std::pair> OptionTemperatureDiag; + std::vector< OptionTemperatureDiag > _temperature_nodes; + stream_index_pair _base_stream; const std::string _namespace; };//end class diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 067ec022fb..b2da308e74 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -121,6 +121,8 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _stream_name[RS2_STREAM_ACCEL] = "accel"; _stream_name[RS2_STREAM_POSE] = "pose"; + + _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE}; } void BaseRealSenseNode::toggleSensors(bool enabled) @@ -175,8 +177,10 @@ void BaseRealSenseNode::publishTopics() enable_devices(); setupPublishers(); setupStreams(); + SetBaseStream(); publishStaticTransforms(); publishIntrinsics(); + startMonitoring(); ROS_INFO_STREAM("RealSense Node Is Up!"); } @@ -1748,9 +1752,8 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& s } } -void BaseRealSenseNode::publishStaticTransforms() +void BaseRealSenseNode::SetBaseStream() { - // Publish static transforms const std::vector base_stream_priority = {DEPTH, POSE}; std::vector::const_iterator base_stream(base_stream_priority.begin()); @@ -1764,7 +1767,13 @@ void BaseRealSenseNode::publishStaticTransforms() } ROS_INFO_STREAM("SELECTED BASE:" << base_stream->first << ", " << base_stream->second); - rs2::stream_profile base_profile = getAProfile(*base_stream); + _base_stream = *base_stream; +} + +void BaseRealSenseNode::publishStaticTransforms() +{ + // Publish static transforms + rs2::stream_profile base_profile = getAProfile(_base_stream); for (std::pair ienable : _enable) { if (ienable.second) @@ -2111,3 +2120,45 @@ bool BaseRealSenseNode::getEnabledProfile(const stream_index_pair& stream_index, return true; } +void BaseRealSenseNode::startMonitoring() +{ + for (rs2_option option : _monitor_options) + { + _temperature_nodes.push_back({option, std::make_shared(rs2_option_to_string(option), _serial_no )}); + } + + int time_interval(1000); + std::thread t([=]() { + while(true) { + std::this_thread::sleep_for(std::chrono::milliseconds(time_interval)); + publish_temperature(); + } + }); + t.detach(); +} + +void BaseRealSenseNode::publish_temperature() +{ + rs2::options sensor(_sensors[_base_stream]); + for (OptionTemperatureDiag option_diag : _temperature_nodes) + { + rs2_option option(option_diag.first); + if (sensor.supports(option)) + { + auto option_value = sensor.get_option(option); + option_diag.second->update(option_value); + } + } +} + +TemperatureDiagnostics::TemperatureDiagnostics(std::string name, std::string serial_no) + { + _updater.add(name, this, &TemperatureDiagnostics::diagnostics); + _updater.setHardwareID(serial_no); + } + +void TemperatureDiagnostics::diagnostics(diagnostic_updater::DiagnosticStatusWrapper& status) +{ + status.summary(0, "OK"); + status.add("Index", _crnt_temp); +} diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 8c8fc10542..fc05d65588 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -149,8 +149,6 @@ void RealSenseNodeFactory::onInit() pipe->start(cfg); //File will be opened in read mode at this point _device = pipe->get_active_profile().get_device(); _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); - _realSenseNode->publishTopics(); - _realSenseNode->registerDynamicReconfigCb(nh); } if (_device) { diff --git a/realsense2_camera/src/t265_realsense_node.cpp b/realsense2_camera/src/t265_realsense_node.cpp index 77fe76b8d7..0fc244be90 100644 --- a/realsense2_camera/src/t265_realsense_node.cpp +++ b/realsense2_camera/src/t265_realsense_node.cpp @@ -10,6 +10,7 @@ T265RealsenseNode::T265RealsenseNode(ros::NodeHandle& nodeHandle, _wo_snr(dev.first()), _use_odom_in(false) { + _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_MOTION_MODULE_TEMPERATURE}; initializeOdometryInput(); } From d77e6eb309e5fbdb45f9d2eacdeda1a70e96caab Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 8 Aug 2019 13:40:30 +0300 Subject: [PATCH 064/131] set auto_exposure ROI. (#883) * Add the ability to set auto_exposure ROI. Available using dynamic reconfigure parameters or by setting the following to the launch file: /camera/rgb_camera/auto_exposure_roi/right: 250 /camera/stereo_module/auto_exposure_roi/top: 50 --- .../include/base_realsense_node.h | 8 ++ realsense2_camera/launch/rs_camera.launch | 1 - realsense2_camera/src/base_realsense_node.cpp | 108 +++++++++++++++++- 3 files changed, 111 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 86171332fd..f5c5b943d9 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -217,6 +217,7 @@ namespace realsense2_camera void SetBaseStream(); void publishStaticTransforms(); void publishIntrinsics(); + void runFirstFrameInitialization(rs2_stream stream_type); 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; @@ -242,6 +243,10 @@ namespace realsense2_camera void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method); void frame_callback(rs2::frame frame); void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name); + void readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, int* option_value); + void registerAutoExposureROIOptions(ros::NodeHandle& nh); + void set_auto_exposure_roi(const std::string option_name, rs2::sensor sensor, int new_value); + void set_sensor_auto_exposure_roi(rs2::sensor sensor); rs2_stream rs2_string_to_stream(std::string str); void startMonitoring(); void publish_temperature(); @@ -306,6 +311,9 @@ namespace realsense2_camera std::map _depth_aligned_image_publishers; std::map _depth_to_other_extrinsics_publishers; std::map _depth_to_other_extrinsics; + std::map _auto_exposure_roi; + std::map _is_first_frame; + std::map > > _video_functions_stack; typedef std::pair> OptionTemperatureDiag; std::vector< OptionTemperatureDiag > _temperature_nodes; diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 230b98d94d..4cc1479ff3 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -49,7 +49,6 @@ - diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index b2da308e74..a99ade064e 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -5,6 +5,7 @@ #include #include #include +#include using namespace realsense2_camera; using namespace ddynamic_reconfigure; @@ -178,12 +179,34 @@ void BaseRealSenseNode::publishTopics() setupPublishers(); setupStreams(); SetBaseStream(); + registerAutoExposureROIOptions(_node_handle); publishStaticTransforms(); publishIntrinsics(); startMonitoring(); ROS_INFO_STREAM("RealSense Node Is Up!"); } +void BaseRealSenseNode::runFirstFrameInitialization(rs2_stream stream_type) +{ + if (_is_first_frame[stream_type]) + { + ROS_DEBUG_STREAM("runFirstFrameInitialization: " << _video_functions_stack.size() << ", " << rs2_stream_to_string(stream_type)); + _is_first_frame[stream_type] = false; + if (!_video_functions_stack[stream_type].empty()) + { + std::thread t = std::thread([=]() + { + while (!_video_functions_stack[stream_type].empty()) + { + _video_functions_stack[stream_type].back()(); + _video_functions_stack[stream_type].pop_back(); + } + }); + t.detach(); + } + } +} + bool is_checkbox(rs2::options sensor, rs2_option option) { rs2::option_range op_range = sensor.get_option_range(option); @@ -266,6 +289,84 @@ std::string create_graph_resource_name(const std::string &original_name) return fixed_name; } +void BaseRealSenseNode::set_auto_exposure_roi(const std::string option_name, rs2::sensor sensor, int new_value) +{ + rs2::region_of_interest& auto_exposure_roi(_auto_exposure_roi[sensor.get_info(RS2_CAMERA_INFO_NAME)]); + if (option_name == "left") + auto_exposure_roi.min_x = new_value; + else if (option_name == "right") + auto_exposure_roi.max_x = new_value; + else if (option_name == "top") + auto_exposure_roi.min_y = new_value; + else if (option_name == "bottom") + auto_exposure_roi.max_y = new_value; + else + { + ROS_WARN_STREAM("Invalid option_name: " << option_name << " while setting auto exposure ROI."); + return; + } + set_sensor_auto_exposure_roi(sensor); +} + +void BaseRealSenseNode::set_sensor_auto_exposure_roi(rs2::sensor sensor) +{ + const rs2::region_of_interest& auto_exposure_roi(_auto_exposure_roi[sensor.get_info(RS2_CAMERA_INFO_NAME)]); + try + { + sensor.as().set_region_of_interest(auto_exposure_roi); + } + catch(const std::runtime_error& e) + { + ROS_ERROR_STREAM(e.what()); + } +} + +void BaseRealSenseNode::readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, + const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, + int* option_value) +{ + nh1.param(option_name, *option_value, *option_value); //param (const std::string ¶m_name, T ¶m_val, const T &default_val) const + if (*option_value < min_val) *option_value = min_val; + if (*option_value > max_val) *option_value = max_val; + + ddynrec->registerVariable( + option_name, *option_value, [this, sensor, option_name](int new_value){set_auto_exposure_roi(option_name, sensor, new_value);}, + "auto-exposure " + option_name + " coordinate", min_val, max_val); +} + +void BaseRealSenseNode::registerAutoExposureROIOptions(ros::NodeHandle& nh) +{ + for (const std::pair>& profile : _enabled_profiles) + { + rs2::sensor sensor = _sensors[profile.first]; + std::string module_base_name(sensor.get_info(RS2_CAMERA_INFO_NAME)); + if (sensor.is() && _auto_exposure_roi.find(module_base_name) == _auto_exposure_roi.end()) + { + int max_x(_width[profile.first]-1); + int max_y(_height[profile.first]-1); + + std::string module_name = create_graph_resource_name(module_base_name) +"/auto_exposure_roi"; + ros::NodeHandle nh1(nh, module_name); + std::shared_ptr ddynrec = std::make_shared(nh1); + + _auto_exposure_roi[module_base_name] = {0, 0, max_x, max_y}; + rs2::region_of_interest& auto_exposure_roi(_auto_exposure_roi[module_base_name]); + readAndSetDynamicParam(nh1, ddynrec, "left", 0, max_x, sensor, &(auto_exposure_roi.min_x)); + readAndSetDynamicParam(nh1, ddynrec, "right", 0, max_x, sensor, &(auto_exposure_roi.max_x)); + readAndSetDynamicParam(nh1, ddynrec, "top", 0, max_y, sensor, &(auto_exposure_roi.min_y)); + readAndSetDynamicParam(nh1, ddynrec, "bottom", 0, max_y, sensor, &(auto_exposure_roi.max_y)); + + ddynrec->publishServicesTopics(); + _ddynrec.push_back(ddynrec); + + // Initiate the call to set_sensor_auto_exposure_roi, after the first frame arrive. + rs2_stream stream_type = profile.first.first; + _video_functions_stack[stream_type].push_back([this, sensor](){set_sensor_auto_exposure_roi(sensor);}); + _is_first_frame[stream_type] = true; + } + } +} + void BaseRealSenseNode::registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name) { ros::NodeHandle nh1(nh, module_name); @@ -1392,6 +1493,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) 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()); + runFirstFrameInitialization(stream_type); } // Clip depth_frame for max range: rs2::depth_frame depth_frame = frameset.get_depth_frame(); @@ -1499,6 +1601,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) auto stream_index = frame.get_profile().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_time, t.toNSec()); + runFirstFrameInitialization(stream_type); stream_index_pair sip{stream_type,stream_index}; if (frame.is()) @@ -1584,11 +1687,6 @@ void BaseRealSenseNode::setupStreams() for (const std::pair >& sensor_profile : profiles) { std::string module_name = sensor_profile.first; - // for (const rs2::stream_profile& profile : sensor_profile.second) - // { - // ROS_WARN_STREAM("type:" << rs2_stream_to_string(profile.stream_type()) << - // " fps: " << profile.fps() << ". format: " << profile.format()); - // } rs2::sensor sensor = active_sensors[module_name]; sensor.open(sensor_profile.second); sensor.start(_sensors_callback[module_name]); From 3d45f3ef808e1edaab8bfb919b0d1b3482588660 Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 8 Aug 2019 14:13:13 +0300 Subject: [PATCH 065/131] Check temperature freqency (#884) Add the ability to set auto_exposure ROI. Available using dynamic reconfigure parameters or by setting the following to the launch file: /camera/rgb_camera/auto_exposure_roi/right: 250 /camera/stereo_module/auto_exposure_roi/top: 50 * update temperature checks to once every 10 seconds. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index a99ade064e..6b99afa50e 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -2225,7 +2225,7 @@ void BaseRealSenseNode::startMonitoring() _temperature_nodes.push_back({option, std::make_shared(rs2_option_to_string(option), _serial_no )}); } - int time_interval(1000); + int time_interval(10000); std::thread t([=]() { while(true) { std::this_thread::sleep_for(std::chrono::milliseconds(time_interval)); From da4bb5dc3e29c25be9e48e4d4a66b2acdb7b1150 Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 8 Aug 2019 14:41:35 +0300 Subject: [PATCH 066/131] upgrade version: 2.2.8 --- README.md | 4 +++- realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 5 files changed, 7 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index fec90df1bb..30d15693d0 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. +LibRealSense supported version: v2.25.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) + ## Installation Instructions The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kinetic, on **Ubuntu 16.04**. @@ -11,7 +13,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine - #### 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 and librealsense-dkms packages. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.24.0) 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.25.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 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 47ecd84fac..b76154928f 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -34,7 +34,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2 2.24.0) +find_package(realsense2 2.25.0) 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() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 61d84bd479..d8cdc98a98 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 7 +#define REALSENSE_ROS_PATCH_VERSION 8 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 62beda0767..0ab316a86d 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.7 + 2.2.8 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 9e934ba6eb..d615370ce4 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.7 + 2.2.8 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From c04f95d3bd31795fac2b1fad2f23274c89d26118 Mon Sep 17 00:00:00 2001 From: Tyler Kurtz Date: Thu, 8 Aug 2019 15:06:00 -0700 Subject: [PATCH 067/131] See issue 885. Added name tag to 435 xacro. Added multiple camera test launch file. Renamed xacro frames to be consistent with drivers. Added an option to enable static frames in the urdf --- .../launch/rs_d435_camera_with_model.launch | 109 +++++++++++++ .../launch/view_d435_model.launch | 2 +- .../launch/view_multiple_d435_models.launch | 8 + realsense2_description/urdf/_d435.urdf.xacro | 152 +++++++++--------- .../test_d435_multiple_cameras.urdf.xacro | 13 ++ 5 files changed, 207 insertions(+), 77 deletions(-) create mode 100644 realsense2_camera/launch/rs_d435_camera_with_model.launch create mode 100644 realsense2_description/launch/view_multiple_d435_models.launch create mode 100644 realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro diff --git a/realsense2_camera/launch/rs_d435_camera_with_model.launch b/realsense2_camera/launch/rs_d435_camera_with_model.launch new file mode 100644 index 0000000000..0235c217d1 --- /dev/null +++ b/realsense2_camera/launch/rs_d435_camera_with_model.launch @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/launch/view_d435_model.launch b/realsense2_description/launch/view_d435_model.launch index cfc07b8392..2ef6b165f9 100644 --- a/realsense2_description/launch/view_d435_model.launch +++ b/realsense2_description/launch/view_d435_model.launch @@ -1,5 +1,5 @@ - + diff --git a/realsense2_description/launch/view_multiple_d435_models.launch b/realsense2_description/launch/view_multiple_d435_models.launch new file mode 100644 index 0000000000..5f5d18b24d --- /dev/null +++ b/realsense2_description/launch/view_multiple_d435_models.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 8aeb32c025..ce282df86c 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -9,63 +9,60 @@ aluminum peripherial evaluation case. --> - + - + - - + + - + - + - - - - - - + - + - + - + - - + + - + - + - + + + - + @@ -75,65 +72,68 @@ aluminum peripherial evaluation case. - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + - - - - - - + + + + + + - - - - - - - + + + + + + + - - - - - - + + + + + + - - - - - - - + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro new file mode 100644 index 0000000000..f89281061f --- /dev/null +++ b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + From ac384d7829d81d6182268a60a07e18debd206b0d Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 12 Aug 2019 09:05:02 +0300 Subject: [PATCH 068/131] fix issue: node crash when device disconnects. --- .../include/base_realsense_node.h | 7 +++- realsense2_camera/src/base_realsense_node.cpp | 40 ++++++++++++++----- 2 files changed, 37 insertions(+), 10 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index f5c5b943d9..128af745d1 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -15,6 +15,7 @@ #include #include #include +#include #include #include @@ -120,7 +121,7 @@ namespace realsense2_camera void toggleSensors(bool enabled); virtual void publishTopics() override; virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) override; - virtual ~BaseRealSenseNode() {} + virtual ~BaseRealSenseNode(); public: enum imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION}; @@ -148,6 +149,7 @@ namespace realsense2_camera } }; + bool _is_running; std::string _base_frame_id; std::string _odom_frame_id; std::map _frame_id; @@ -317,6 +319,9 @@ namespace realsense2_camera typedef std::pair> OptionTemperatureDiag; std::vector< OptionTemperatureDiag > _temperature_nodes; + std::shared_ptr _monitoring_t; + mutable std::condition_variable _cv; + stream_index_pair _base_stream; const std::string _namespace; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 6b99afa50e..343ad5d62b 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -82,7 +82,7 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, ros::NodeHandle& privateNodeHandle, rs2::device dev, const std::string& serial_no) : - _base_frame_id(""), _node_handle(nodeHandle), + _is_running(true), _base_frame_id(""), _node_handle(nodeHandle), _pnh(privateNodeHandle), _dev(dev), _json_file_path(""), _serial_no(serial_no), _is_initialized_time_base(false), @@ -126,6 +126,16 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _monitor_options = {RS2_OPTION_ASIC_TEMPERATURE, RS2_OPTION_PROJECTOR_TEMPERATURE}; } +BaseRealSenseNode::~BaseRealSenseNode() +{ + _is_running = false; + _cv.notify_one(); + if (_monitoring_t && _monitoring_t->joinable()) + { + _monitoring_t->join(); + } +} + void BaseRealSenseNode::toggleSensors(bool enabled) { for (auto it=_sensors.begin(); it != _sensors.end(); it++) @@ -2226,13 +2236,18 @@ void BaseRealSenseNode::startMonitoring() } int time_interval(10000); - std::thread t([=]() { - while(true) { - std::this_thread::sleep_for(std::chrono::milliseconds(time_interval)); - publish_temperature(); + std::function func = [this, time_interval](){ + std::mutex mu; + std::unique_lock lock(mu); + while(_is_running) { + _cv.wait_for(lock, std::chrono::milliseconds(time_interval), [&]{return !_is_running;}); + if (_is_running) + { + publish_temperature(); + } } - }); - t.detach(); + }; + _monitoring_t = std::make_shared(func); } void BaseRealSenseNode::publish_temperature() @@ -2243,8 +2258,15 @@ void BaseRealSenseNode::publish_temperature() rs2_option option(option_diag.first); if (sensor.supports(option)) { - auto option_value = sensor.get_option(option); - option_diag.second->update(option_value); + try + { + auto option_value = sensor.get_option(option); + option_diag.second->update(option_value); + } + catch(const std::exception& e) + { + ROS_DEBUG_STREAM("Failed checking for temperature." << std::endl << e.what()); + } } } } From 68d8dc5c90d7ba4b581c9253349892f63a9a62b3 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 13 Aug 2019 10:46:36 +0200 Subject: [PATCH 069/131] Add option to disable odom tf to be published This is useful if you want to link your camera to your existing robot tf tree. tf can not handle two parents for the camera_pose_frame --- realsense2_camera/launch/rs_t265.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch index c060249a05..8f3b135a03 100644 --- a/realsense2_camera/launch/rs_t265.launch +++ b/realsense2_camera/launch/rs_t265.launch @@ -27,6 +27,8 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + + @@ -50,6 +52,8 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + + From 8dbe9d0aa2562ffb6dcfab17c47ea98996527761 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Fri, 16 Aug 2019 09:57:36 +0200 Subject: [PATCH 070/131] Add option to disable odom tf --- realsense2_camera/launch/rs_t265.launch | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch index c060249a05..8f3b135a03 100644 --- a/realsense2_camera/launch/rs_t265.launch +++ b/realsense2_camera/launch/rs_t265.launch @@ -27,6 +27,8 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + + @@ -50,6 +52,8 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + + From e0076b2b608c2fe3df02aa4f2daa45c970275251 Mon Sep 17 00:00:00 2001 From: Pavlo Kolomiiets Date: Mon, 11 Mar 2019 16:09:22 +0200 Subject: [PATCH 071/131] Add dynamic transform support again. https://github.com/intel-ros/realsense/pull/169 https://github.com/intel-ros/realsense/issues/660 --- .../include/base_realsense_node.h | 7 +++ realsense2_camera/include/constants.h | 3 ++ .../launch/includes/nodelet.launch.xml | 6 +++ realsense2_camera/launch/rs_camera.launch | 6 +++ realsense2_camera/src/base_realsense_node.cpp | 52 ++++++++++++++++--- 5 files changed, 67 insertions(+), 7 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 128af745d1..d8b2d5296c 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -20,6 +20,7 @@ #include #include #include +#include namespace realsense2_camera { @@ -218,6 +219,7 @@ namespace realsense2_camera void updateStreamCalibData(const rs2::video_stream_profile& video_profile); void SetBaseStream(); void publishStaticTransforms(); + void publishDynamicTransforms(); void publishIntrinsics(); void runFirstFrameInitialization(rs2_stream stream_type); void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset); @@ -274,7 +276,12 @@ namespace realsense2_camera std::map _fps; std::map _enable; std::map _stream_name; + bool _publish_tf; + double _tf_publish_rate; tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster; + tf2_ros::TransformBroadcaster _dynamic_tf_broadcaster; + std::vector _static_tf_msgs; + std::shared_ptr _tf_t; std::map _image_publishers; std::map _imu_publishers; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index d8cdc98a98..d507dbeb54 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -41,6 +41,9 @@ namespace realsense2_camera const bool ALLOW_NO_TEXTURE_POINTS = false; const bool SYNC_FRAMES = false; + const bool PUBLISH_TF = true; + const double TF_PUBLISH_RATE = 0; // Static transform + const int IMAGE_WIDTH = 640; const int IMAGE_HEIGHT = 480; const int IMAGE_FPS = 30; diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 9d796b34f8..904463b8fa 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -72,6 +72,9 @@ + + + @@ -155,6 +158,9 @@ + + + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index 4cc1479ff3..cfa18e4864 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -39,6 +39,9 @@ + + + @@ -89,6 +92,9 @@ + + + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 343ad5d62b..0e41466046 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -4,8 +4,6 @@ #include #include #include -#include -#include using namespace realsense2_camera; using namespace ddynamic_reconfigure; @@ -128,6 +126,10 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, BaseRealSenseNode::~BaseRealSenseNode() { + // Kill dynamic transform thread + if (_publish_tf && _tf_publish_rate > 0) + _tf_t->join(); + _is_running = false; _cv.notify_one(); if (_monitoring_t && _monitoring_t->joinable()) @@ -191,6 +193,15 @@ void BaseRealSenseNode::publishTopics() SetBaseStream(); registerAutoExposureROIOptions(_node_handle); publishStaticTransforms(); + if (_publish_tf) + { + // Static transform for non-positive values + if (_tf_publish_rate > 0) + _tf_t = std::shared_ptr(new std::thread(boost::bind(&BaseRealSenseNode::publishDynamicTransforms, this))); + else + _static_tf_broadcaster.sendTransform(_static_tf_msgs); + } + publishIntrinsics(); startMonitoring(); ROS_INFO_STREAM("RealSense Node Is Up!"); @@ -531,6 +542,9 @@ void BaseRealSenseNode::getParameters() _pnh.param("filters", _filters_str, DEFAULT_FILTERS); _pointcloud |= (_filters_str.find("pointcloud") != std::string::npos); + _pnh.param("publish_tf", _publish_tf, PUBLISH_TF); + _pnh.param("tf_publish_rate", _tf_publish_rate, TF_PUBLISH_RATE); + _pnh.param("enable_sync", _sync_frames, SYNC_FRAMES); if (_pointcloud || _align_depth || _filters_str.size() > 0) _sync_frames = true; @@ -1814,7 +1828,7 @@ void BaseRealSenseNode::publish_static_tf(const ros::Time& t, msg.transform.rotation.y = q.getY(); msg.transform.rotation.z = q.getZ(); msg.transform.rotation.w = q.getW(); - _static_tf_broadcaster.sendTransform(msg); + _static_tf_msgs.push_back(msg); } void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile) @@ -1880,13 +1894,17 @@ void BaseRealSenseNode::SetBaseStream() void BaseRealSenseNode::publishStaticTransforms() { - // Publish static transforms rs2::stream_profile base_profile = getAProfile(_base_stream); - for (std::pair ienable : _enable) + + // Publish static transforms + if (_publish_tf) { - if (ienable.second) + for (std::pair ienable : _enable) { - calcAndPublishStaticTransform(ienable.first, base_profile); + if (ienable.second) + { + calcAndPublishStaticTransform(ienable.first, base_profile); + } } } @@ -1930,6 +1948,26 @@ void BaseRealSenseNode::publishStaticTransforms() } +void BaseRealSenseNode::publishDynamicTransforms() +{ + // Publish transforms for the cameras + ROS_WARN("Publishing dynamic camera transforms (/tf) at %.f Hz", _tf_publish_rate); + + ros::Rate loop_rate(_tf_publish_rate); + + while (ros::ok()) + { + // Update the time stamp for publication + ros::Time t = ros::Time::now(); + for(auto& msg : _static_tf_msgs) + msg.header.stamp = t; + + _dynamic_tf_broadcaster.sendTransform(_static_tf_msgs); + + loop_rate.sleep(); + } +} + void BaseRealSenseNode::publishIntrinsics() { if (_enable[GYRO]) From 0c3ce648a726dd0016cdb87d784ef73b76a9a517 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 16 Sep 2019 10:56:49 +0300 Subject: [PATCH 072/131] force infrared stream to choose stable Y8 format. --- realsense2_camera/include/base_realsense_node.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 5 ++++- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 128af745d1..019db81c02 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -272,6 +272,7 @@ namespace realsense2_camera std::map _width; std::map _height; std::map _fps; + std::map _format; std::map _enable; std::map _stream_name; tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 343ad5d62b..ea7d7ffebd 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -96,6 +96,8 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, _depth_aligned_encoding[RS2_STREAM_DEPTH] = sensor_msgs::image_encodings::TYPE_16UC1; // Infrared stream + _format[RS2_STREAM_INFRARED] = RS2_FORMAT_Y8; + _image_format[RS2_STREAM_INFRARED] = CV_8UC1; // CVBridge type _encoding[RS2_STREAM_INFRARED] = sensor_msgs::image_encodings::MONO8; // ROS message type _unit_step_size[RS2_STREAM_INFRARED] = sizeof(uint8_t); // sensor_msgs::ImagePtr row step size @@ -912,6 +914,7 @@ void BaseRealSenseNode::enable_devices() (_width[elem] == 0 || video_profile.width() == _width[elem]) && (_height[elem] == 0 || video_profile.height() == _height[elem]) && (_fps[elem] == 0 || video_profile.fps() == _fps[elem]) && + (_format.find(elem.first) == _format.end() || video_profile.format() == _format[elem.first] ) && video_profile.stream_index() == elem.second) { _width[elem] = video_profile.width(); @@ -922,7 +925,7 @@ void BaseRealSenseNode::enable_devices() _image[elem] = cv::Mat(_height[elem], _width[elem], _image_format[elem.first], cv::Scalar(0, 0, 0)); - ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem]); + ROS_INFO_STREAM(STREAM_NAME(elem) << " stream is enabled - width: " << _width[elem] << ", height: " << _height[elem] << ", fps: " << _fps[elem] << ", " << "Format: " << video_profile.format()); break; } } From 936b98c1d3dd186e167df0696eb7d334de0b8560 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 16 Sep 2019 12:08:30 +0300 Subject: [PATCH 073/131] add publish_tf and tf_publish_rate to README.md fixed print. --- README.md | 2 ++ realsense2_camera/src/base_realsense_node.cpp | 16 ++++++---------- 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/README.md b/README.md index 30d15693d0..e73a19dbfa 100644 --- a/README.md +++ b/README.md @@ -114,6 +114,8 @@ Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. - **topic_odom_in**: For T265, add wheel odometry information through this topic. The code refers only to the *twist.linear* field in the message. - **calib_odom_file**: For the T265 to include odometry input, it must be given a [configuration file](https://github.com/IntelRealSense/librealsense/blob/master/unit-tests/resources/calibration_odometry.json). Explanations can be found [here](https://github.com/IntelRealSense/librealsense/pull/3462). The calibration is done in ROS coordinates system. +- **publish_tf**: boolean, publish or not TF at all. Defaults to True. +- **tf_publish_rate**: double, positive values mean dynamic transform publication with specified rate, all other values mean static transform publication. Defaults to 0 - **publish_odom_tf**: If True (default) publish TF from odom_frame to pose_frame. diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0e41466046..0c683e3358 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -193,15 +193,6 @@ void BaseRealSenseNode::publishTopics() SetBaseStream(); registerAutoExposureROIOptions(_node_handle); publishStaticTransforms(); - if (_publish_tf) - { - // Static transform for non-positive values - if (_tf_publish_rate > 0) - _tf_t = std::shared_ptr(new std::thread(boost::bind(&BaseRealSenseNode::publishDynamicTransforms, this))); - else - _static_tf_broadcaster.sendTransform(_static_tf_msgs); - } - publishIntrinsics(); startMonitoring(); ROS_INFO_STREAM("RealSense Node Is Up!"); @@ -1906,6 +1897,11 @@ void BaseRealSenseNode::publishStaticTransforms() calcAndPublishStaticTransform(ienable.first, base_profile); } } + // Static transform for non-positive values + if (_tf_publish_rate > 0) + _tf_t = std::shared_ptr(new std::thread(boost::bind(&BaseRealSenseNode::publishDynamicTransforms, this))); + else + _static_tf_broadcaster.sendTransform(_static_tf_msgs); } // Publish Extrinsics Topics: @@ -1951,7 +1947,7 @@ void BaseRealSenseNode::publishStaticTransforms() void BaseRealSenseNode::publishDynamicTransforms() { // Publish transforms for the cameras - ROS_WARN("Publishing dynamic camera transforms (/tf) at %.f Hz", _tf_publish_rate); + ROS_WARN("Publishing dynamic camera transforms (/tf) at %g Hz", _tf_publish_rate); ros::Rate loop_rate(_tf_publish_rate); From f6f8efab0534e4b046ac9a64952c1945581eb28c Mon Sep 17 00:00:00 2001 From: Jorge Rodriguez Date: Mon, 14 Oct 2019 12:03:56 +0200 Subject: [PATCH 074/131] passing camera name as a parameter the material has been moved to an specific file so it can be imported from other files --- realsense2_description/urdf/_d415.urdf.xacro | 88 +++++++++---------- realsense2_description/urdf/_d435.urdf.xacro | 88 +++++++++---------- .../urdf/_materials.urdf.xacro | 15 ++++ realsense2_description/urdf/_r410.urdf.xacro | 63 +++++++------ realsense2_description/urdf/_r430.urdf.xacro | 79 ++++++++--------- 5 files changed, 171 insertions(+), 162 deletions(-) create mode 100644 realsense2_description/urdf/_materials.urdf.xacro diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index cbc3627b7c..411002cd3c 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -8,7 +8,10 @@ aluminum peripherial evaluation case. --> - + + + + - + - + - + - + - - + + - + @@ -75,63 +73,63 @@ aluminum peripherial evaluation case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 8aeb32c025..e7dca709cf 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -9,7 +9,10 @@ aluminum peripherial evaluation case. --> - + + + + - + - + - + - + - - + + - + @@ -77,63 +75,63 @@ aluminum peripherial evaluation case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/_materials.urdf.xacro b/realsense2_description/urdf/_materials.urdf.xacro new file mode 100644 index 0000000000..90e35b9323 --- /dev/null +++ b/realsense2_description/urdf/_materials.urdf.xacro @@ -0,0 +1,15 @@ + + + + + + + + + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index 07b401f442..090cb6db73 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -9,6 +9,9 @@ aluminum peripherial evalution case. --> + + + - + - + - + @@ -67,48 +66,48 @@ aluminum peripherial evalution case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 28540bda29..13840dd2d1 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -9,6 +9,9 @@ aluminum peripherial evaluation case. --> + + + - + - + - + @@ -68,63 +67,63 @@ aluminum peripherial evaluation case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + From 05b25bd0201fb1d6c2cbd93c85bf586caec9c752 Mon Sep 17 00:00:00 2001 From: Jorge Rodriguez Date: Tue, 15 Oct 2019 09:55:08 +0200 Subject: [PATCH 075/131] added unit testing to test the dual set --- realsense2_description/CMakeLists.txt | 5 +++++ realsense2_description/tests/dual_d415.xacro | 12 +++++++++++ realsense2_description/tests/dual_d435.xacro | 12 +++++++++++ realsense2_description/tests/dual_r410.xacro | 12 +++++++++++ realsense2_description/tests/dual_r430.xacro | 12 +++++++++++ .../tests/one_of_each.xacro | 21 +++++++++++++++++++ realsense2_description/tests/test_xacro.py | 21 +++++++++++++++++++ 7 files changed, 95 insertions(+) create mode 100644 realsense2_description/tests/dual_d415.xacro create mode 100644 realsense2_description/tests/dual_d435.xacro create mode 100644 realsense2_description/tests/dual_r410.xacro create mode 100644 realsense2_description/tests/dual_r430.xacro create mode 100644 realsense2_description/tests/one_of_each.xacro create mode 100644 realsense2_description/tests/test_xacro.py diff --git a/realsense2_description/CMakeLists.txt b/realsense2_description/CMakeLists.txt index 6edf86d385..3b285b434d 100644 --- a/realsense2_description/CMakeLists.txt +++ b/realsense2_description/CMakeLists.txt @@ -11,3 +11,8 @@ catkin_package( # Install files install(DIRECTORY launch meshes rviz urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +# Tests +if (CATKIN_ENABLE_TESTING) + catkin_add_nosetests(tests) +endif() \ No newline at end of file diff --git a/realsense2_description/tests/dual_d415.xacro b/realsense2_description/tests/dual_d415.xacro new file mode 100644 index 0000000000..692a9b6cd0 --- /dev/null +++ b/realsense2_description/tests/dual_d415.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/realsense2_description/tests/dual_d435.xacro b/realsense2_description/tests/dual_d435.xacro new file mode 100644 index 0000000000..46758cf182 --- /dev/null +++ b/realsense2_description/tests/dual_d435.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/realsense2_description/tests/dual_r410.xacro b/realsense2_description/tests/dual_r410.xacro new file mode 100644 index 0000000000..cca77d832d --- /dev/null +++ b/realsense2_description/tests/dual_r410.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/realsense2_description/tests/dual_r430.xacro b/realsense2_description/tests/dual_r430.xacro new file mode 100644 index 0000000000..6d4d528526 --- /dev/null +++ b/realsense2_description/tests/dual_r430.xacro @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/realsense2_description/tests/one_of_each.xacro b/realsense2_description/tests/one_of_each.xacro new file mode 100644 index 0000000000..f458b92546 --- /dev/null +++ b/realsense2_description/tests/one_of_each.xacro @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/tests/test_xacro.py b/realsense2_description/tests/test_xacro.py new file mode 100644 index 0000000000..939fced2c9 --- /dev/null +++ b/realsense2_description/tests/test_xacro.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python + +import rospkg +import subprocess +import os + +r = rospkg.RosPack() +path = r.get_path('realsense2_description') + + +def run_xacro_in_file(filename): + assert(filename != "") + assert(subprocess.check_output(["xacro", "--inorder", "tests/{}".format(filename)], + cwd=path)) + + +def test_files(): + for _, _, filenames in os.walk(os.path.join(path, "tests")): + for file in filenames: + if file.endswith(".xacro"): + yield run_xacro_in_file, file From 7ace746d14520c0c50daa21468c16420ba72f3a1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 21 Oct 2019 14:46:50 +0200 Subject: [PATCH 076/131] added support for multiple cameras --- realsense2_description/urdf/_d415.urdf.xacro | 86 +++++++++---------- realsense2_description/urdf/_d435.urdf.xacro | 86 +++++++++---------- realsense2_description/urdf/_r410.urdf.xacro | 67 +++++++-------- realsense2_description/urdf/_r430.urdf.xacro | 83 +++++++++--------- .../test_d435_multiple_cameras.urdf.xacro | 13 +++ 5 files changed, 173 insertions(+), 162 deletions(-) create mode 100644 realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index cbc3627b7c..8c833c7b65 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -8,7 +8,7 @@ aluminum peripherial evaluation case. --> - + - + - + - + - + - - + + - + - + - + @@ -75,63 +75,63 @@ aluminum peripherial evaluation case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 8aeb32c025..fbc3ea67b2 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -9,7 +9,7 @@ aluminum peripherial evaluation case. --> - + - + - + - + - + - - + + - + - + - + @@ -77,63 +77,63 @@ aluminum peripherial evaluation case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index 07b401f442..64b178618b 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -30,27 +30,26 @@ aluminum peripherial evalution case. - - - - - + + + + - + - + - + - + - + @@ -67,48 +66,48 @@ aluminum peripherial evalution case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 28540bda29..97a5f6f280 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -31,27 +31,26 @@ aluminum peripherial evaluation case. - - - - - + + + + - + - + - + - + - + @@ -68,63 +67,63 @@ aluminum peripherial evaluation case. - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + - + - - + + - + diff --git a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro new file mode 100644 index 0000000000..043ea35de3 --- /dev/null +++ b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + From d2d38a1e51e90f4c485eb6715927a59b2005e15a Mon Sep 17 00:00:00 2001 From: RenaudeauBrice Date: Wed, 23 Oct 2019 11:28:12 +0200 Subject: [PATCH 077/131] Allow to use usb port number to connect to devices --- .../include/realsense_node_factory.h | 1 + .../launch/includes/nodelet.launch.xml | 2 + realsense2_camera/launch/rs_rgbd.launch | 2 + realsense2_camera/launch/rs_t265.launch | 2 + realsense2_camera/src/base_realsense_node.cpp | 4 ++ .../src/realsense_node_factory.cpp | 42 +++++++++++++++++-- 6 files changed, 50 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index b7bc536512..b81977711f 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -68,6 +68,7 @@ namespace realsense2_camera std::unique_ptr _realSenseNode; rs2::context _ctx; std::string _serial_no; + std::string _port_no; bool _initial_reset; std::thread _query_thread; diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 8fab55e464..60c20fe2db 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -2,6 +2,7 @@ + @@ -91,6 +92,7 @@ + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index 835143c881..aaff237bfb 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -48,6 +48,7 @@ Processing enabled by this node: + @@ -117,6 +118,7 @@ Processing enabled by this node: + diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch index 8f3b135a03..9d7ce67074 100644 --- a/realsense2_camera/launch/rs_t265.launch +++ b/realsense2_camera/launch/rs_t265.launch @@ -6,6 +6,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib --> + @@ -34,6 +35,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2897de936d..2f29cdcb74 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -649,6 +649,10 @@ void BaseRealSenseNode::setupDevice() ROS_INFO_STREAM("Device Serial No: " << _serial_no); + auto camera_id = _dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT); + + ROS_INFO_STREAM("Device physical port: " << camera_id); + auto fw_ver = _dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION); ROS_INFO_STREAM("Device FW version: " << fw_ver); diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index fc05d65588..55254dc6b7 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -12,6 +12,8 @@ #include #include +#include + using namespace realsense2_camera; #define REALSENSE_ROS_EMBEDDED_VERSION_STR (VAR_ARG_STRING(VERSION: REALSENSE_ROS_MAJOR_VERSION.REALSENSE_ROS_MINOR_VERSION.REALSENSE_ROS_PATCH_VERSION)) @@ -57,22 +59,55 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) else { bool found = false; + ROS_INFO_STREAM(" "); 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() || sn == _serial_no) + auto pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT); + auto name = dev.get_info(RS2_CAMERA_INFO_NAME); + ROS_INFO_STREAM("Device with physical ID " << pn << " was found."); + std::string port_id; + std::vector results; + ROS_INFO_STREAM("Device with name " << name << " was found."); + if(strcmp(name,"Intel RealSense T265") == 0) + { + ROS_DEBUG_STREAM("T265 was found!"); + boost::split(results, pn, [](char c){return c == ' ';}); + std::string bus_line = results[2]; + std::string port_line = results[3]; + boost::split(results, bus_line, [](char c){return c == '_';}); + std::string bus_no = results[1]; + boost::split(results, port_line, [](char c){return c == '_';}); + std::string port_no = results[1]; + port_id = bus_no +"-"+ port_no; + } + else// if(strcmp(name, "Intel RealSense D435") == 0) + { + //ROS_DEBUG_STREAM("D345 was found!"); + boost::split(results, pn, [](char c){return c == ':';}); + std::string bus_line = results[3]; + boost::split(results, bus_line, [](char c){return c == '/';}); + std::string port_line = results[results.size()-1]; + boost::replace_all(port_line,".","-"); + port_id = port_line; + } + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); + + + ROS_INFO_STREAM("Device with serial number " << sn << " was found."< Date: Thu, 24 Oct 2019 14:00:21 +0300 Subject: [PATCH 078/131] use regex to parse usb description string. handle cases where failed to extract usb bus and port. --- realsense2_camera/launch/rs_camera.launch | 2 + .../src/realsense_node_factory.cpp | 103 +++++++++++------- 2 files changed, 67 insertions(+), 38 deletions(-) diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index cfa18e4864..aba6d62b05 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -1,5 +1,6 @@ + @@ -58,6 +59,7 @@ + diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 55254dc6b7..e994b664ec 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -11,8 +11,7 @@ #include #include #include - -#include +#include using namespace realsense2_camera; @@ -59,55 +58,83 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) else { bool found = false; - ROS_INFO_STREAM(" "); + ROS_INFO_STREAM(" "); for (auto&& dev : list) { auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - auto pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT); - auto name = dev.get_info(RS2_CAMERA_INFO_NAME); - ROS_INFO_STREAM("Device with physical ID " << pn << " was found."); - std::string port_id; - std::vector results; - ROS_INFO_STREAM("Device with name " << name << " was found."); - if(strcmp(name,"Intel RealSense T265") == 0) - { - ROS_DEBUG_STREAM("T265 was found!"); - boost::split(results, pn, [](char c){return c == ' ';}); - std::string bus_line = results[2]; - std::string port_line = results[3]; - boost::split(results, bus_line, [](char c){return c == '_';}); - std::string bus_no = results[1]; - boost::split(results, port_line, [](char c){return c == '_';}); - std::string port_no = results[1]; - port_id = bus_no +"-"+ port_no; - } - else// if(strcmp(name, "Intel RealSense D435") == 0) - { - //ROS_DEBUG_STREAM("D345 was found!"); - boost::split(results, pn, [](char c){return c == ':';}); - std::string bus_line = results[3]; - boost::split(results, bus_line, [](char c){return c == '/';}); - std::string port_line = results[results.size()-1]; - boost::replace_all(port_line,".","-"); - port_id = port_line; - } - ROS_INFO_STREAM("Device with port number " << port_id << " was found."); - + std::string pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT); + const char* name = dev.get_info(RS2_CAMERA_INFO_NAME); + ROS_INFO_STREAM("Device with physical ID " << pn << " was found."); + std::string port_id; + std::vector results; + ROS_INFO_STREAM("Device with name " << name << " was found."); + std::regex self_regex; + if(strcmp(name,"Intel RealSense T265") == 0) + { + self_regex = std::regex(".*?bus_([0-9]+) port_([0-9]+).*", std::regex_constants::ECMAScript); + } + else// if(strcmp(name, "Intel RealSense D435") == 0) + { + self_regex = std::regex("[^ ]*?usb[0-9]+/([0-9.-]+)/[^ ]*", std::regex_constants::ECMAScript); + } + std::smatch base_match; + bool found_usb_desc = std::regex_match(pn, base_match, self_regex); + found_usb_desc = false; + if (found_usb_desc) + { + std::ssub_match base_sub_match = base_match[1]; + port_id = base_sub_match.str(); + for (unsigned int mi = 2; mi < base_match.size(); mi++) + { + std::ssub_match base_sub_match = base_match[mi]; + port_id += "-" + base_sub_match.str(); + } + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); + } + else + { + std::stringstream msg; + msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; + if (_port_no.empty()) + { + ROS_WARN_STREAM(msg.str()); + } + else + { + ROS_ERROR_STREAM(msg.str()); + ROS_ERROR_STREAM("Please use serial number instead of usb port."); + } + } - ROS_INFO_STREAM("Device with serial number " << sn << " was found."< Date: Thu, 24 Oct 2019 15:18:10 +0300 Subject: [PATCH 079/131] Add device_type option to choose the device type using regular expression. --- .../include/realsense_node_factory.h | 1 + .../launch/includes/nodelet.launch.xml | 4 ++- realsense2_camera/launch/rs_camera.launch | 2 ++ .../launch/rs_d400_and_t265.launch | 12 ++++++--- realsense2_camera/launch/rs_rgbd.launch | 2 ++ realsense2_camera/launch/rs_rtabmap.launch | 8 ++++-- .../src/realsense_node_factory.cpp | 27 ++++++++++++++----- 7 files changed, 43 insertions(+), 13 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index b81977711f..423e02b535 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -69,6 +69,7 @@ namespace realsense2_camera rs2::context _ctx; std::string _serial_no; std::string _port_no; + std::string _device_type; bool _initial_reset; std::thread _query_thread; diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index 60c20fe2db..f8f3a16be3 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -2,7 +2,8 @@ - + + @@ -93,6 +94,7 @@ + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index aba6d62b05..e02cbfbaf6 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -1,6 +1,7 @@ + @@ -60,6 +61,7 @@ + diff --git a/realsense2_camera/launch/rs_d400_and_t265.launch b/realsense2_camera/launch/rs_d400_and_t265.launch index 3d47f73c53..1c373425d0 100644 --- a/realsense2_camera/launch/rs_d400_and_t265.launch +++ b/realsense2_camera/launch/rs_d400_and_t265.launch @@ -1,8 +1,10 @@ - - - - + + + + + + @@ -17,6 +19,7 @@ + @@ -29,6 +32,7 @@ + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index aaff237bfb..6e633beaec 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -49,6 +49,7 @@ Processing enabled by this node: + @@ -119,6 +120,7 @@ Processing enabled by this node: + diff --git a/realsense2_camera/launch/rs_rtabmap.launch b/realsense2_camera/launch/rs_rtabmap.launch index 5436f743ca..8fec042480 100644 --- a/realsense2_camera/launch/rs_rtabmap.launch +++ b/realsense2_camera/launch/rs_rtabmap.launch @@ -4,8 +4,10 @@ For installation type: apt-get install ros-kinetic-rtabmap-ros --> - - + + + + @@ -14,6 +16,8 @@ + + diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index e994b664ec..529479e2d2 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -62,14 +62,15 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) for (auto&& dev : list) { auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); + ROS_INFO_STREAM("Device with serial number " << sn << " was found."< results; ROS_INFO_STREAM("Device with name " << name << " was found."); std::regex self_regex; - if(strcmp(name,"Intel RealSense T265") == 0) + if(name == std::string("Intel RealSense T265")) { self_regex = std::regex(".*?bus_([0-9]+) port_([0-9]+).*", std::regex_constants::ECMAScript); } @@ -79,7 +80,6 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } std::smatch base_match; bool found_usb_desc = std::regex_match(pn, base_match, self_regex); - found_usb_desc = false; if (found_usb_desc) { std::ssub_match base_sub_match = base_match[1]; @@ -105,9 +105,14 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) ROS_ERROR_STREAM("Please use serial number instead of usb port."); } } + bool found_device_type(true); + if (!_device_type.empty()) + { + std::regex device_type_regex(_device_type.c_str(), std::regex::icase); + found_device_type = std::regex_search(name, base_match, device_type_regex); + } - ROS_INFO_STREAM("Device with serial number " << sn << " was found."< Date: Thu, 24 Oct 2019 15:27:43 +0300 Subject: [PATCH 080/131] update README.md regarding port_no and device_type options. --- README.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index e73a19dbfa..aacd361d24 100644 --- a/README.md +++ b/README.md @@ -82,7 +82,10 @@ If using D435 or D415, the gyro and accel topics wont be available. Likewise, ot ### Launch parameters The following parameters are available by the wrapper: -- **serial_no**: will attach to the device with the given serial number. Default, attach to available RealSense device in random. +- **serial_no**: will attach to the device with the given serial number (*serial_no*) number. Default, attach to available RealSense device in random. +- **port_no**: will attach to the device with the given USB port (*port_no*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. +- **device_type**: will attach to a device whose name includes the given *device_type* regular expression pattern. Default, ignore device type. For example, device_type:=d435 will match d435 and d435i. device_type=d435(?!i) will match d435 but not d435i. + - **rosbag_filename**: Will publish topics from rosbag file. - **initial_reset**: On occasions the device was not closed properly and due to firmware issues needs to reset. If set to true, the device will reset prior to usage. - **align_depth**: If set to true, will publish additional topics with the all the images aligned to the depth image.
From ca76160665094437e60e96456b07a27306cf3e4a Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 24 Oct 2019 16:09:02 +0300 Subject: [PATCH 081/131] renamed porn_no to usb_port_id --- realsense2_camera/include/realsense_node_factory.h | 2 +- realsense2_camera/launch/includes/nodelet.launch.xml | 4 ++-- realsense2_camera/launch/rs_camera.launch | 4 ++-- realsense2_camera/launch/rs_rgbd.launch | 4 ++-- realsense2_camera/launch/rs_t265.launch | 4 ++-- realsense2_camera/src/realsense_node_factory.cpp | 10 +++++----- 6 files changed, 14 insertions(+), 14 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 423e02b535..0a345c52a4 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -68,7 +68,7 @@ namespace realsense2_camera std::unique_ptr _realSenseNode; rs2::context _ctx; std::string _serial_no; - std::string _port_no; + std::string _usb_port_id; std::string _device_type; bool _initial_reset; std::thread _query_thread; diff --git a/realsense2_camera/launch/includes/nodelet.launch.xml b/realsense2_camera/launch/includes/nodelet.launch.xml index f8f3a16be3..d780c1ee23 100644 --- a/realsense2_camera/launch/includes/nodelet.launch.xml +++ b/realsense2_camera/launch/includes/nodelet.launch.xml @@ -2,7 +2,7 @@ - + @@ -93,7 +93,7 @@ - + diff --git a/realsense2_camera/launch/rs_camera.launch b/realsense2_camera/launch/rs_camera.launch index e02cbfbaf6..3039390dff 100644 --- a/realsense2_camera/launch/rs_camera.launch +++ b/realsense2_camera/launch/rs_camera.launch @@ -1,6 +1,6 @@ - + @@ -60,7 +60,7 @@ - + diff --git a/realsense2_camera/launch/rs_rgbd.launch b/realsense2_camera/launch/rs_rgbd.launch index 6e633beaec..cdd74ad62f 100644 --- a/realsense2_camera/launch/rs_rgbd.launch +++ b/realsense2_camera/launch/rs_rgbd.launch @@ -48,7 +48,7 @@ Processing enabled by this node: - + @@ -119,7 +119,7 @@ Processing enabled by this node: - + diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch index 9d7ce67074..fc687a49b2 100644 --- a/realsense2_camera/launch/rs_t265.launch +++ b/realsense2_camera/launch/rs_t265.launch @@ -6,7 +6,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib --> - + @@ -35,7 +35,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib - + diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 529479e2d2..82e56427e7 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -95,7 +95,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) { std::stringstream msg; msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; - if (_port_no.empty()) + if (_usb_port_id.empty()) { ROS_WARN_STREAM(msg.str()); } @@ -112,7 +112,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) found_device_type = std::regex_search(name, base_match, device_type_regex); } - if ((_serial_no.empty() || sn == _serial_no) && (_port_no.empty() || port_id == _port_no) && found_device_type) + if ((_serial_no.empty() || sn == _serial_no) && (_usb_port_id.empty() || port_id == _usb_port_id) && found_device_type) { _device = dev; _serial_no = sn; @@ -130,13 +130,13 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) msg += "serial number " + _serial_no; add_and = true; } - if (!_port_no.empty()) + if (!_usb_port_id.empty()) { if (add_and) { msg += " and "; } - msg += "port number " + _port_no; + msg += "usb port id " + _usb_port_id; add_and = true; } if (!_device_type.empty()) @@ -211,7 +211,7 @@ void RealSenseNodeFactory::onInit() ros::NodeHandle nh = getNodeHandle(); auto privateNh = getPrivateNodeHandle(); privateNh.param("serial_no", _serial_no, std::string("")); - privateNh.param("port_no", _port_no, std::string("")); + privateNh.param("usb_port_id", _usb_port_id, std::string("")); privateNh.param("device_type", _device_type, std::string("")); std::string rosbag_filename(""); From be3fec40808ba22b4993b996003f27eacb7e228c Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 24 Oct 2019 16:12:54 +0300 Subject: [PATCH 082/131] update README.md regarding usb_port_id --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index aacd361d24..380dc9a5a7 100644 --- a/README.md +++ b/README.md @@ -83,7 +83,7 @@ If using D435 or D415, the gyro and accel topics wont be available. Likewise, ot ### Launch parameters The following parameters are available by the wrapper: - **serial_no**: will attach to the device with the given serial number (*serial_no*) number. Default, attach to available RealSense device in random. -- **port_no**: will attach to the device with the given USB port (*port_no*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. +- **usb_port_id**: will attach to the device with the given USB port (*usb_port_id*). i.e 4-1, 4-2 etc. Default, ignore USB port when choosing a device. - **device_type**: will attach to a device whose name includes the given *device_type* regular expression pattern. Default, ignore device type. For example, device_type:=d435 will match d435 and d435i. device_type=d435(?!i) will match d435 but not d435i. - **rosbag_filename**: Will publish topics from rosbag file. From 0bc36ae29abd7d4e6849fa5a5c2658d6bc623a94 Mon Sep 17 00:00:00 2001 From: Aswin Kumar Date: Thu, 24 Oct 2019 13:13:47 +0000 Subject: [PATCH 083/131] Update opensource_tracking.launch Fixed a wrong parameter in imufiltermadgwick --- realsense2_camera/launch/opensource_tracking.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/launch/opensource_tracking.launch b/realsense2_camera/launch/opensource_tracking.launch index 44ef4be5db..5cc356511e 100644 --- a/realsense2_camera/launch/opensource_tracking.launch +++ b/realsense2_camera/launch/opensource_tracking.launch @@ -8,7 +8,7 @@ - + From 8e34324a749ac4d888bd7d91aaf9ef62bf66fb64 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 28 Oct 2019 16:02:41 +0200 Subject: [PATCH 084/131] set serial number in diagnostics when running from bag file --- realsense2_camera/launch/rs_t265.launch | 2 ++ realsense2_camera/src/realsense_node_factory.cpp | 1 + 2 files changed, 3 insertions(+) diff --git a/realsense2_camera/launch/rs_t265.launch b/realsense2_camera/launch/rs_t265.launch index fc687a49b2..ea2b86b7cf 100644 --- a/realsense2_camera/launch/rs_t265.launch +++ b/realsense2_camera/launch/rs_t265.launch @@ -7,6 +7,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + @@ -36,6 +37,7 @@ https://github.com/IntelRealSense/librealsense/blob/master/third-party/libtm/lib + diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 82e56427e7..84f3fcdf62 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -226,6 +226,7 @@ void RealSenseNodeFactory::onInit() cfg.enable_all_streams(); pipe->start(cfg); //File will be opened in read mode at this point _device = pipe->get_active_profile().get_device(); + _serial_no = _device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); } if (_device) From 1ed5a452a92f004d2c448c6074c2de2d9fb336a8 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 28 Oct 2019 16:39:39 +0200 Subject: [PATCH 085/131] upgrade version: 2.2.9 --- README.md | 4 ++-- realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 380dc9a5a7..926ee3de91 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. -LibRealSense supported version: v2.25.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) +LibRealSense supported version: v2.29.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) ## Installation Instructions @@ -13,7 +13,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine - #### 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 and librealsense-dkms packages. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.25.0) 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.29.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 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index b76154928f..6bc4adf28a 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -34,7 +34,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2 2.25.0) +find_package(realsense2 2.29.0) 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() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index d507dbeb54..62704da272 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 8 +#define REALSENSE_ROS_PATCH_VERSION 9 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 0ab316a86d..198fec6053 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.8 + 2.2.9 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 22e7bb33cc..df81cd4a98 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.8 + 2.2.9 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From 609c024ad476475a4e92e0aea4650016185b5f59 Mon Sep 17 00:00:00 2001 From: Mike Purvis Date: Tue, 29 Oct 2019 20:38:27 +0000 Subject: [PATCH 086/131] Fix USB port regex to work with hub-connected cams. --- realsense2_camera/src/realsense_node_factory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 84f3fcdf62..1c85591f1a 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -76,7 +76,7 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) } else// if(strcmp(name, "Intel RealSense D435") == 0) { - self_regex = std::regex("[^ ]*?usb[0-9]+/([0-9.-]+)/[^ ]*", std::regex_constants::ECMAScript); + self_regex = std::regex("[^ ]+/usb[0-9]+[0-9./-]*/([0-9.-]+):[^ ]*", std::regex_constants::ECMAScript); } std::smatch base_match; bool found_usb_desc = std::regex_match(pn, base_match, self_regex); From 45d1049c2077715cb143d72db05eef8da285b0e8 Mon Sep 17 00:00:00 2001 From: sahandy Date: Fri, 15 Nov 2019 14:21:09 +0100 Subject: [PATCH 087/131] Fix wrong PointField Type for Colored Point Clouds * Related issue: #680 * Use `FLOAT32` for `sensor_msgs::PointField` when preparing the point cloud message, in case where texture is available and chosen. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2f29cdcb74..00a26108ac 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -2071,7 +2071,7 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co 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.point_step = addPointField(msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::FLOAT32, 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); From fc63959f1213cbab1f4677240cb2569022a0202b Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 4 Dec 2019 14:38:02 +0200 Subject: [PATCH 088/131] fixed unite_imu_method copy to transmit at gyro's rate. fix rs2_listener.py to display a warning if messages times goes backwards. --- realsense2_camera/scripts/rs2_listener.py | 31 ++++++++++++++++--- realsense2_camera/src/base_realsense_node.cpp | 25 ++++++++------- 2 files changed, 41 insertions(+), 15 deletions(-) diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index 2ca24b6351..a1589d3359 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -3,6 +3,7 @@ import rospy from sensor_msgs.msg import Image as msg_Image from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 +from theora_image_transport.msg import Packet as msg_theora import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import Imu as msg_Imu import numpy as np @@ -40,6 +41,8 @@ def __init__(self, params={}): self.node_name = params.get('node_name', 'rs2_listener') self.bridge = CvBridge() self.listener = None + self.prev_msg_time = 0 + 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}, @@ -134,13 +137,13 @@ def _pointscloudCallback(data): self.func_data[theme_name]['height'].append(data.height) return _pointscloudCallback - def wait_for_message(self, params): + def wait_for_message(self, params, msg_type=msg_Image): topic = params['topic'] print 'connect to ROS with name: %s' % self.node_name rospy.init_node(self.node_name, anonymous=True) rospy.loginfo('Subscribing on topic: %s' % topic) - self.sub = rospy.Subscriber(topic, msg_Image, self.callback) + self.sub = rospy.Subscriber(topic, msg_type, self.callback) self.prev_time = time.time() break_timeout = False @@ -180,7 +183,18 @@ def wait_for_messages(self, themes): return self.func_data def callback(self, data): - rospy.loginfo('Got message. Seq %d, secs: %d, nsecs: %d' % (data.header.seq, data.header.stamp.secs, data.header.stamp.nsecs)) + msg_time = data.header.stamp.secs + 1e-9 * data.header.stamp.nsecs + #rospy.loginfo('Got message. Seq %d, secs: %d, nsecs: %d' % (data.header.seq, data.header.stamp.secs, data.header.stamp.nsecs)) + #rospy.loginfo('Got message. Seq %d, secs: %f' % (data.header.seq, msg_time)) + # print data + + if (self.prev_msg_time > msg_time): + rospy.loginfo('Out of order: %.9f > %.9f' % (self.prev_msg_time, msg_time)) + # if type(data) == msg_Imu: + # print self.prev_msg_data + # print data + self.prev_msg_time = msg_time + self.prev_msg_data = data self.prev_time = time.time() if any([self.seq > 0 and data.header.seq >= self.seq, @@ -212,6 +226,15 @@ def main(): wanted_topic = sys.argv[1] msg_params = {} + if 'points' in wanted_topic: + msg_type = msg_PointCloud2 + elif 'imu' in wanted_topic: + msg_type = msg_Imu + elif 'theora' in wanted_topic: + msg_type = msg_theora + else: + msg_type = msg_Image + for idx in range(2, len(sys.argv)): if sys.argv[idx] == '-s': msg_params['seq'] = int(sys.argv[idx + 1]) @@ -223,7 +246,7 @@ def main(): msg_retriever = CWaitForMessage(msg_params) if '/' in wanted_topic: msg_params = {'topic': wanted_topic} - res = msg_retriever.wait_for_message(msg_params) + res = msg_retriever.wait_for_message(msg_params, msg_type) rospy.loginfo('Got message: %s' % res.header) else: themes = [wanted_topic] diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 2f29cdcb74..25447887ca 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1223,21 +1223,24 @@ double BaseRealSenseNode::FillImuData_LinearInterpolation(const stream_index_pai 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) + static CIMUHistory::imuData _accel_data({0,0,0}, -1.0); + 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; + _accel_data = imu_data; + return -1; } + if (_accel_data.m_time < 0) + return -1; + + 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; + + 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 imu_data.m_time; } From 99999016d638726610c012d2e244b037f3c18aa4 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 4 Dec 2019 16:23:15 +0200 Subject: [PATCH 089/131] fix rs2_listener.py to avoid dependency of theora-image-transport --- realsense2_camera/scripts/rs2_listener.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index a1589d3359..d7f87fefec 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -3,7 +3,6 @@ import rospy from sensor_msgs.msg import Image as msg_Image from sensor_msgs.msg import PointCloud2 as msg_PointCloud2 -from theora_image_transport.msg import Packet as msg_theora import sensor_msgs.point_cloud2 as pc2 from sensor_msgs.msg import Imu as msg_Imu import numpy as np @@ -12,6 +11,10 @@ import ctypes import struct import tf +try: + from theora_image_transport.msg import Packet as msg_theora +except Exception + pass def pc2_to_xyzrgb(point): @@ -231,7 +234,11 @@ def main(): elif 'imu' in wanted_topic: msg_type = msg_Imu elif 'theora' in wanted_topic: - msg_type = msg_theora + try: + msg_type = msg_theora + except NameError as e: + print ('theora_image_transport is not installed. \nType "sudo apt-get install ros-kinetic-theora-image-transport" to enable registering on messages of type theora.') + raise else: msg_type = msg_Image From 96ef72a235ffaaa794b083cdd20a8ae57eb77014 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 4 Dec 2019 16:46:06 +0200 Subject: [PATCH 090/131] syntax error. --- realsense2_camera/scripts/rs2_listener.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index d7f87fefec..e126677252 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -13,7 +13,7 @@ import tf try: from theora_image_transport.msg import Packet as msg_theora -except Exception +except Exception: pass From 2befb29db291dc209c54c2fd52729c7f57cf7515 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 8 Dec 2019 07:19:16 +0200 Subject: [PATCH 091/131] fixed unite_imu_method LINEAR_INTERPOLATION to transmit at gyro's rate. --- .../include/base_realsense_node.h | 46 ++-- realsense2_camera/src/base_realsense_node.cpp | 203 ++++++++---------- 2 files changed, 109 insertions(+), 140 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 39cdabbd58..f020ce58ad 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -171,38 +171,19 @@ namespace realsense2_camera private: - class CIMUHistory + class CimuData { 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; - + CimuData() : m_time(-1) {}; + CimuData(const stream_index_pair type, Eigen::Vector3d data, double time): + m_type(type), + m_data(data), + m_time(time){}; + bool is_set() {return m_time > 0;}; 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); + stream_index_pair m_type; + Eigen::Vector3d m_data; + double m_time; }; static std::string getNamespaceStr(); @@ -239,8 +220,11 @@ namespace realsense2_camera bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile); void publishAlignedDepthToOthers(rs2::frameset frames, const ros::Time& t); - 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); + sensor_msgs::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data); + + void FillImuData_Copy(const CimuData imu_data, std::deque& imu_msgs); + void ImuMessage_AddDefaultValues(sensor_msgs::Imu& imu_msg); + void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque& imu_msgs); void imu_callback(rs2::frame frame); void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY); void pose_callback(rs2::frame frame); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 25447887ca..9dd07effea 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -811,7 +811,7 @@ void BaseRealSenseNode::setupPublishers() if (_imu_sync_method > imu_sync_method::NONE && _enable[GYRO] && _enable[ACCEL]) { ROS_INFO("Start publisher IMU"); - _synced_imu_publisher = std::make_shared(_node_handle.advertise("imu", 1)); + _synced_imu_publisher = std::make_shared(_node_handle.advertise("imu", 5)); _synced_imu_publisher->Enable(_hold_back_imu_for_frames); } else @@ -1145,114 +1145,91 @@ void BaseRealSenseNode::clip_depth(rs2::depth_frame depth_frame, float clipping_ } } -BaseRealSenseNode::CIMUHistory::CIMUHistory(size_t size) +sensor_msgs::Imu BaseRealSenseNode::CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data) { - 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; + sensor_msgs::Imu imu_msg; + ros::Time t(gyro_data.m_time); + imu_msg.header.seq = 0; + imu_msg.header.stamp = t; + + imu_msg.angular_velocity.x = gyro_data.m_data.x(); + imu_msg.angular_velocity.y = gyro_data.m_data.y(); + imu_msg.angular_velocity.z = gyro_data.m_data.z(); + + imu_msg.linear_acceleration.x = accel_data.m_data.x(); + imu_msg.linear_acceleration.y = accel_data.m_data.y(); + imu_msg.linear_acceleration.z = accel_data.m_data.z(); + return imu_msg; } -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; +template T lerp(const T &a, const T &b, const double t) { + return a * (1.0 - t) + b * t; } -double BaseRealSenseNode::FillImuData_LinearInterpolation(const stream_index_pair stream_index, const BaseRealSenseNode::CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg) +void BaseRealSenseNode::FillImuData_LinearInterpolation(const CimuData imu_data, std::deque& imu_msgs) { - 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; + static std::deque _imu_history; + _imu_history.push_back(imu_data); + stream_index_pair type(imu_data.m_type); + imu_msgs.clear(); + + if ((type != ACCEL) || _imu_history.size() < 3) + return; + + std::deque gyros_data; + CimuData accel0, accel1, crnt_imu; + + while (_imu_history.size()) + { + crnt_imu = _imu_history.front(); + _imu_history.pop_front(); + if (!accel0.is_set() && crnt_imu.m_type == ACCEL) + { + accel0 = crnt_imu; + } + else if (accel0.is_set() && crnt_imu.m_type == ACCEL) + { + accel1 = crnt_imu; + const double dt = accel1.m_time - accel0.m_time; + + while (gyros_data.size()) + { + CimuData crnt_gyro = gyros_data.front(); + gyros_data.pop_front(); + const double alpha = (crnt_gyro.m_time - accel0.m_time) / dt; + CimuData crnt_accel(ACCEL, lerp(accel0.m_data, accel1.m_data, alpha), crnt_gyro.m_time); + imu_msgs.push_back(CreateUnitedMessage(crnt_accel, crnt_gyro)); + } + accel0 = accel1; + } + else if (accel0.is_set() && crnt_imu.m_time >= accel0.m_time && crnt_imu.m_type == GYRO) + { + gyros_data.push_back(crnt_imu); + } + } + _imu_history.push_back(crnt_imu); + return; } -double BaseRealSenseNode::FillImuData_Copy(const stream_index_pair stream_index, const BaseRealSenseNode::CIMUHistory::imuData imu_data, sensor_msgs::Imu& imu_msg) +void BaseRealSenseNode::FillImuData_Copy(const CimuData imu_data, std::deque& imu_msgs) { - static CIMUHistory::imuData _accel_data({0,0,0}, -1.0); - if (ACCEL == stream_index) + stream_index_pair type(imu_data.m_type); + + static CimuData _accel_data(ACCEL, {0,0,0}, -1.0); + if (ACCEL == type) { _accel_data = imu_data; - return -1; + return; } if (_accel_data.m_time < 0) - return -1; - - 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; - - 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 imu_data.m_time; + return; + + imu_msgs.push_back(CreateUnitedMessage(_accel_data, imu_data)); } -void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync_method) +void BaseRealSenseNode::ImuMessage_AddDefaultValues(sensor_msgs::Imu& imu_msg) { - 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 = _optical_frame_id[stream_imu]; + imu_msg.header.frame_id = _optical_frame_id[GYRO]; imu_msg.orientation.x = 0.0; imu_msg.orientation.y = 0.0; imu_msg.orientation.z = 0.0; @@ -1261,6 +1238,14 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync 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 = { _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; +} + +void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync_method) +{ + static std::mutex m_mutex; + static int seq = 0; + static bool init_accel(false); + static double accel_factor(0); m_mutex.lock(); @@ -1282,10 +1267,6 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync if (0 != _synced_imu_publisher->getNumSubscribers()) { auto crnt_reading = *(reinterpret_cast(frame.get_data())); - if (GYRO == stream_index) - { - init_gyro = true; - } if (ACCEL == stream_index) { if (!init_accel) @@ -1305,26 +1286,30 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync crnt_reading.z = v.z(); } } - CIMUHistory::imuData imu_data(crnt_reading, elapsed_camera_ms); + Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); + CimuData imu_data(stream_index, v, elapsed_camera_ms); + std::deque imu_msgs; 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); + FillImuData_Copy(imu_data, imu_msgs); break; case LINEAR_INTERPOLATION: - elapsed_camera_ms = FillImuData_LinearInterpolation(stream_index, imu_data, imu_msg); + FillImuData_LinearInterpolation(imu_data, imu_msgs); 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())); + while (imu_msgs.size()) + { + sensor_msgs::Imu imu_msg = imu_msgs.front(); + ros::Time t(_ros_time_base.toSec() + imu_msg.header.stamp.toSec()); + imu_msg.header.seq = seq; + imu_msg.header.stamp = t; + ImuMessage_AddDefaultValues(imu_msg); + _synced_imu_publisher->Publish(imu_msg); + ROS_DEBUG("Publish united %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + imu_msgs.pop_front(); + } } break; } From acc68a0cf10d9161983af3f1bca2fb2ae314518e Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 8 Dec 2019 11:32:41 +0200 Subject: [PATCH 092/131] fix publishing rate issue for united imu. Add printing options to rs2_listener.py. --- realsense2_camera/scripts/rs2_listener.py | 35 +++++-- realsense2_camera/src/base_realsense_node.cpp | 93 +++++++------------ 2 files changed, 61 insertions(+), 67 deletions(-) diff --git a/realsense2_camera/scripts/rs2_listener.py b/realsense2_camera/scripts/rs2_listener.py index e126677252..c179588c21 100644 --- a/realsense2_camera/scripts/rs2_listener.py +++ b/realsense2_camera/scripts/rs2_listener.py @@ -45,6 +45,7 @@ def __init__(self, params={}): self.bridge = CvBridge() self.listener = None self.prev_msg_time = 0 + self.fout = None self.themes = {'depthStream': {'topic': '/camera/depth/image_rect_raw', 'callback': self.imageColorCallback, 'msg_type': msg_Image}, @@ -145,6 +146,17 @@ def wait_for_message(self, params, msg_type=msg_Image): print 'connect to ROS with name: %s' % self.node_name rospy.init_node(self.node_name, anonymous=True) + out_filename = params.get('filename', None) + if (out_filename): + self.fout = open(out_filename, 'w') + if msg_type is msg_Imu: + col_w = 20 + print 'Writing to file: %s' % out_filename + columns = ['frame_number', 'frame_time(sec)', 'accel.x', 'accel.y', 'accel.z', 'gyro.x', 'gyro.y', 'gyro.z'] + line = ('{:<%d}'*len(columns) % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(*columns) + '\n' + sys.stdout.write(line) + self.fout.write(line) + rospy.loginfo('Subscribing on topic: %s' % topic) self.sub = rospy.Subscriber(topic, msg_type, self.callback) @@ -187,15 +199,19 @@ def wait_for_messages(self, themes): def callback(self, data): msg_time = data.header.stamp.secs + 1e-9 * data.header.stamp.nsecs - #rospy.loginfo('Got message. Seq %d, secs: %d, nsecs: %d' % (data.header.seq, data.header.stamp.secs, data.header.stamp.nsecs)) - #rospy.loginfo('Got message. Seq %d, secs: %f' % (data.header.seq, msg_time)) - # print data if (self.prev_msg_time > msg_time): rospy.loginfo('Out of order: %.9f > %.9f' % (self.prev_msg_time, msg_time)) - # if type(data) == msg_Imu: - # print self.prev_msg_data - # print data + if type(data) == msg_Imu: + col_w = 20 + frame_number = data.header.seq + accel = data.linear_acceleration + gyro = data.angular_velocity + line = ('\n{:<%d}{:<%d.6f}{:<%d.4f}{:<%d.4f}{:<%d.4f}{:<%d.4f}{:<%d.4f}{:<%d.4f}' % (col_w, col_w, col_w, col_w, col_w, col_w, col_w, col_w)).format(frame_number, msg_time, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z) + sys.stdout.write(line) + if self.fout: + self.fout.write(line) + self.prev_msg_time = msg_time self.prev_msg_data = data @@ -222,6 +238,7 @@ def main(): print '-s ' print '--time ' print '--timeout ' + print '--filename : write output to file' exit(-1) # wanted_topic = '/device_0/sensor_0/Depth_0/image/data' @@ -231,7 +248,7 @@ def main(): msg_params = {} if 'points' in wanted_topic: msg_type = msg_PointCloud2 - elif 'imu' in wanted_topic: + elif ('imu' in wanted_topic) or ('gyro' in wanted_topic) or ('accel' in wanted_topic): msg_type = msg_Imu elif 'theora' in wanted_topic: try: @@ -249,10 +266,12 @@ def main(): msg_params['time'] = dict(zip(['secs', 'nsecs'], [int(part) for part in sys.argv[idx + 1].split('.')])) if sys.argv[idx] == '--timeout': msg_params['timeout_secs'] = int(sys.argv[idx + 1]) + if sys.argv[idx] == '--filename': + msg_params['filename'] = sys.argv[idx+1] msg_retriever = CWaitForMessage(msg_params) if '/' in wanted_topic: - msg_params = {'topic': wanted_topic} + msg_params.setdefault('topic', wanted_topic) res = msg_retriever.wait_for_message(msg_params, msg_type) rospy.loginfo('Got message: %s' % res.header) else: diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 9dd07effea..4a9f2c6f23 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1244,74 +1244,49 @@ void BaseRealSenseNode::imu_callback_sync(rs2::frame frame, imu_sync_method sync { static std::mutex m_mutex; static int seq = 0; - static bool init_accel(false); - static double accel_factor(0); 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(); + + bool placeholder_false(false); + if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) { - auto stream = frame.get_profile().stream_type(); - auto stream_index = (stream == GYRO.first)?GYRO:ACCEL; - double frame_time = frame.get_timestamp(); + setBaseTime(frame_time, RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME == frame.get_frame_timestamp_domain()); + } - bool placeholder_false(false); - if (_is_initialized_time_base.compare_exchange_strong(placeholder_false, true) ) + 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())); + Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); + CimuData imu_data(stream_index, v, elapsed_camera_ms); + std::deque imu_msgs; + switch (sync_method) { - setBaseTime(frame_time, RS2_TIMESTAMP_DOMAIN_SYSTEM_TIME == frame.get_frame_timestamp_domain()); + case NONE: //Cannot really be NONE. Just to avoid compilation warning. + case COPY: + FillImuData_Copy(imu_data, imu_msgs); + break; + case LINEAR_INTERPOLATION: + FillImuData_LinearInterpolation(imu_data, imu_msgs); + break; } - - seq += 1; - double elapsed_camera_ms = (/*ms*/ frame_time - /*ms*/ _camera_time_base) / 1000.0; - - if (0 != _synced_imu_publisher->getNumSubscribers()) + while (imu_msgs.size()) { - auto crnt_reading = *(reinterpret_cast(frame.get_data())); - 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(); - } - } - Eigen::Vector3d v(crnt_reading.x, crnt_reading.y, crnt_reading.z); - CimuData imu_data(stream_index, v, elapsed_camera_ms); - std::deque imu_msgs; - switch (sync_method) - { - case NONE: //Cannot really be NONE. Just to avoid compilation warning. - case COPY: - FillImuData_Copy(imu_data, imu_msgs); - break; - case LINEAR_INTERPOLATION: - FillImuData_LinearInterpolation(imu_data, imu_msgs); - break; - } - while (imu_msgs.size()) - { - sensor_msgs::Imu imu_msg = imu_msgs.front(); - ros::Time t(_ros_time_base.toSec() + imu_msg.header.stamp.toSec()); - imu_msg.header.seq = seq; - imu_msg.header.stamp = t; - ImuMessage_AddDefaultValues(imu_msg); - _synced_imu_publisher->Publish(imu_msg); - ROS_DEBUG("Publish united %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); - imu_msgs.pop_front(); - } + sensor_msgs::Imu imu_msg = imu_msgs.front(); + ros::Time t(_ros_time_base.toSec() + imu_msg.header.stamp.toSec()); + imu_msg.header.seq = seq; + imu_msg.header.stamp = t; + ImuMessage_AddDefaultValues(imu_msg); + _synced_imu_publisher->Publish(imu_msg); + ROS_DEBUG("Publish united %s stream", rs2_stream_to_string(frame.get_profile().stream_type())); + imu_msgs.pop_front(); } - break; } m_mutex.unlock(); }; From 770cdb0a9944a779b8a945485d6b5acc234b2345 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 8 Dec 2019 14:34:40 +0200 Subject: [PATCH 093/131] cleaning --- realsense2_camera/src/base_realsense_node.cpp | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 4a9f2c6f23..99130ee623 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1313,14 +1313,8 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) ros::Time t(_ros_time_base.toSec() + elapsed_camera_ms); auto imu_msg = sensor_msgs::Imu(); + ImuMessage_AddDefaultValues(imu_msg); 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}; - 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 = { _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov, 0.0, 0.0, 0.0, _angular_velocity_cov}; auto crnt_reading = *(reinterpret_cast(frame.get_data())); if (GYRO == stream_index) From 5988326c9165044f76d69ab54fbf234e67a95927 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 9 Dec 2019 15:06:33 +0200 Subject: [PATCH 094/131] fix point cloud test to work with outdorrs_1color.bag file. --- realsense2_camera/scripts/rs2_test.py | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index ff62fdce5b..e1a51596d7 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -211,9 +211,7 @@ def staticTFTest(data, gt_data): 'data_func': lambda x: None, 'test_func': NotImageColorTest}, '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]}, - # 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 125, 116, 102])], 'epsilon': [0.02, 2]}, - 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 123, 124, 113])], 'epsilon': [0.04, 5]}, + 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 80, 160, 240])], 'epsilon': [0.04, 5]}, 'test_func': PointCloudTest}, 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1', 'data_func': ImageDepthGetData, @@ -341,18 +339,19 @@ def run_tests(tests): return results + def main(): + outdoors_filename = './records/outdoors_1color.bag' all_tests = [{'name': 'non_existent_file', 'type': 'no_file', 'params': {'rosbag_filename': '/home/non_existent_file.txt'}}, - {'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'}}, - # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': './records/outdoors.bag'}}, # Not working in Travis... + {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': outdoors_filename}}, + {'name': 'depth_avg_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename}}, + {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}}, + {'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}}, + {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}}, + {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}}, + {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation'}}, + {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation', 'align_depth': 'true'}}, + # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename}}, # Not working in Travis... {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}}, ] From 42d5df7d3ff98f6ddc3dde30d937303d73685502 Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 9 Dec 2019 15:10:39 +0200 Subject: [PATCH 095/131] fix point cloud test to work with outdorrs_1color.bag file. --- .travis.yml | 2 +- realsense2_camera/scripts/rs2_test.py | 25 ++++++++++++------------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/.travis.yml b/.travis.yml index b30798a25a..5a60176a3f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -39,7 +39,7 @@ install: - source ~/.bashrc # download data: - - bag_filename="http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors.bag"; + - bag_filename="http://realsense-hw-public.s3.amazonaws.com/rs-tests/TestData/outdoors_1color.bag"; - wget $bag_filename -P "records/" - bag_filename="http://realsense-hw-public.s3-eu-west-1.amazonaws.com/rs-tests/D435i_Depth_and_IMU_Stands_still.bag"; - wget $bag_filename -P "records/" diff --git a/realsense2_camera/scripts/rs2_test.py b/realsense2_camera/scripts/rs2_test.py index ff62fdce5b..e1a51596d7 100644 --- a/realsense2_camera/scripts/rs2_test.py +++ b/realsense2_camera/scripts/rs2_test.py @@ -211,9 +211,7 @@ def staticTFTest(data, gt_data): 'data_func': lambda x: None, 'test_func': NotImageColorTest}, '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]}, - # 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 125, 116, 102])], 'epsilon': [0.02, 2]}, - 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 123, 124, 113])], 'epsilon': [0.04, 5]}, + 'data_func': lambda x: {'width': [660353, 2300], 'height': [1], 'avg': [np.array([ 1.28251814, -0.15839984, 4.82235184, 80, 160, 240])], 'epsilon': [0.04, 5]}, 'test_func': PointCloudTest}, 'align_depth_ir1': {'listener_theme': 'alignedDepthInfra1', 'data_func': ImageDepthGetData, @@ -341,18 +339,19 @@ def run_tests(tests): return results + def main(): + outdoors_filename = './records/outdoors_1color.bag' all_tests = [{'name': 'non_existent_file', 'type': 'no_file', 'params': {'rosbag_filename': '/home/non_existent_file.txt'}}, - {'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'}}, - # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': './records/outdoors.bag'}}, # Not working in Travis... + {'name': 'vis_avg_2', 'type': 'vis_avg', 'params': {'rosbag_filename': outdoors_filename}}, + {'name': 'depth_avg_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename}}, + {'name': 'depth_w_cloud_1', 'type': 'depth_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}}, + {'name': 'points_cloud_1', 'type': 'pointscloud_avg', 'params': {'rosbag_filename': outdoors_filename, 'enable_pointcloud': 'true'}}, + {'name': 'align_depth_color_1', 'type': 'align_depth_color', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}}, + {'name': 'align_depth_ir1_1', 'type': 'align_depth_ir1', 'params': {'rosbag_filename': outdoors_filename, 'align_depth': 'true'}}, + {'name': 'depth_avg_decimation_1', 'type': 'depth_avg_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation'}}, + {'name': 'align_depth_ir1_decimation_1', 'type': 'align_depth_ir1_decimation', 'params': {'rosbag_filename': outdoors_filename, 'filters': 'decimation', 'align_depth': 'true'}}, + # {'name': 'static_tf_1', 'type': 'static_tf', 'params': {'rosbag_filename': outdoors_filename}}, # Not working in Travis... {'name': 'accel_up_1', 'type': 'accel_up', 'params': {'rosbag_filename': './records/D435i_Depth_and_IMU_Stands_still.bag'}}, ] From 80492f40bcd8a41c532a241eece0979b68f029e2 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 10 Dec 2019 13:05:34 +0200 Subject: [PATCH 096/131] add pointcloud to rs_d435_camera_with_model.launch and fix urdf.rviz accordingly. fix view_d415_model.launch, view_r410_model.launch, view_r430_model.launch with flag: use_nominal_extrinsics _d415.urdf, _d435.urdf, _r410.urdf, _r430.urdf : rename left_ir and right_ir according to librealsense2 convention to infra1, infra2 --- .../launch/rs_d435_camera_with_model.launch | 2 +- .../launch/view_d415_model.launch | 2 +- .../launch/view_r410_model.launch | 2 +- .../launch/view_r430_model.launch | 2 +- realsense2_description/rviz/urdf.rviz | 100 +++++++------- realsense2_description/urdf/_d415.urdf.xacro | 129 ++++++++--------- realsense2_description/urdf/_d435.urdf.xacro | 5 +- realsense2_description/urdf/_r410.urdf.xacro | 126 ++++++++--------- realsense2_description/urdf/_r430.urdf.xacro | 130 +++++++++--------- 9 files changed, 251 insertions(+), 247 deletions(-) diff --git a/realsense2_camera/launch/rs_d435_camera_with_model.launch b/realsense2_camera/launch/rs_d435_camera_with_model.launch index 0235c217d1..2cc52f4b67 100644 --- a/realsense2_camera/launch/rs_d435_camera_with_model.launch +++ b/realsense2_camera/launch/rs_d435_camera_with_model.launch @@ -39,7 +39,7 @@ - + diff --git a/realsense2_description/launch/view_d415_model.launch b/realsense2_description/launch/view_d415_model.launch index 5ff30f2265..46827f3e52 100644 --- a/realsense2_description/launch/view_d415_model.launch +++ b/realsense2_description/launch/view_d415_model.launch @@ -1,5 +1,5 @@ - + diff --git a/realsense2_description/launch/view_r410_model.launch b/realsense2_description/launch/view_r410_model.launch index d627e22e58..68d9addb26 100644 --- a/realsense2_description/launch/view_r410_model.launch +++ b/realsense2_description/launch/view_r410_model.launch @@ -1,5 +1,5 @@ - + diff --git a/realsense2_description/launch/view_r430_model.launch b/realsense2_description/launch/view_r430_model.launch index 57c1c18300..a835dec4e2 100644 --- a/realsense2_description/launch/view_r430_model.launch +++ b/realsense2_description/launch/view_r430_model.launch @@ -1,5 +1,5 @@ - + diff --git a/realsense2_description/rviz/urdf.rviz b/realsense2_description/rviz/urdf.rviz index cb0607b926..18372e149d 100644 --- a/realsense2_description/rviz/urdf.rviz +++ b/realsense2_description/rviz/urdf.rviz @@ -29,7 +29,9 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -69,43 +71,11 @@ Visualization Manager: 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: "" @@ -129,16 +99,16 @@ Visualization Manager: Value: false camera_depth_optical_frame: Value: false - camera_left_ir_frame: - Value: false - camera_left_ir_optical_frame: - Value: false - camera_link: + camera_infra1_frame: Value: true - camera_right_ir_frame: + camera_infra1_optical_frame: + Value: true + camera_infra2_frame: + Value: true + camera_infra2_optical_frame: + Value: true + camera_link: Value: true - camera_right_ir_optical_frame: - Value: false Marker Scale: 0.100000001 Name: TF Show Arrows: true @@ -148,20 +118,50 @@ Visualization Manager: base_link: camera_bottom_screw_frame: camera_link: + camera_color_frame: + camera_color_optical_frame: + {} 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: - {} + camera_infra1_frame: + camera_infra1_optical_frame: + {} + camera_infra2_frame: + camera_infra2_optical_frame: + {} Update Interval: 0 Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.00999999978 + Style: Flat Squares + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index 70ee221760..7dc06ddd0f 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -11,13 +11,13 @@ aluminum peripherial evaluation case. - + - - + + - + @@ -72,64 +72,67 @@ aluminum peripherial evaluation case. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index 8736567b0f..b6e3259bf1 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -54,11 +54,8 @@ aluminum peripherial evaluation case. - - - + - diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index 04d0d4ac0d..8c27c3b38d 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -5,35 +5,34 @@ License: Apache 2.0. See LICENSE file in root directory. Copyright(c) 2017 Intel Corporation. All Rights Reserved This is the URDF model for the Intel RealSense 410 camera, in it's -aluminum peripherial evalution case. +aluminum peripherial evaluation case. --> + + + - + + + - - - + + + + + - - - - - - - - - - - - + + + + @@ -49,7 +48,7 @@ aluminum peripherial evalution case. - + @@ -65,49 +64,52 @@ aluminum peripherial evalution case. - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - + + + + + + - - - - - - + + + + + + + - - - - - - - + + + + + + - - - - - - + + + + + + + + + + + + + + + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 6f18a3218d..44cea7dad6 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -12,12 +12,13 @@ aluminum peripherial evaluation case. + - - + + @@ -50,7 +49,7 @@ aluminum peripherial evaluation case. - + @@ -65,65 +64,68 @@ aluminum peripherial evaluation case. - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From c942692abaf17ec7aac01d2fb1b5a69575226c71 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 10 Dec 2019 15:35:19 +0200 Subject: [PATCH 097/131] add use_nominal_extrinsics to urdf tests --- realsense2_description/tests/dual_d415.xacro | 1 + realsense2_description/tests/dual_d435.xacro | 1 + realsense2_description/tests/dual_r410.xacro | 3 ++- realsense2_description/tests/dual_r430.xacro | 1 + realsense2_description/tests/one_of_each.xacro | 1 + 5 files changed, 6 insertions(+), 1 deletion(-) diff --git a/realsense2_description/tests/dual_d415.xacro b/realsense2_description/tests/dual_d415.xacro index 692a9b6cd0..fa269f648b 100644 --- a/realsense2_description/tests/dual_d415.xacro +++ b/realsense2_description/tests/dual_d415.xacro @@ -2,6 +2,7 @@ + diff --git a/realsense2_description/tests/dual_d435.xacro b/realsense2_description/tests/dual_d435.xacro index 46758cf182..531aff2347 100644 --- a/realsense2_description/tests/dual_d435.xacro +++ b/realsense2_description/tests/dual_d435.xacro @@ -2,6 +2,7 @@ + diff --git a/realsense2_description/tests/dual_r410.xacro b/realsense2_description/tests/dual_r410.xacro index cca77d832d..79e5ad0258 100644 --- a/realsense2_description/tests/dual_r410.xacro +++ b/realsense2_description/tests/dual_r410.xacro @@ -2,8 +2,9 @@ + - + diff --git a/realsense2_description/tests/dual_r430.xacro b/realsense2_description/tests/dual_r430.xacro index 6d4d528526..c2be68e736 100644 --- a/realsense2_description/tests/dual_r430.xacro +++ b/realsense2_description/tests/dual_r430.xacro @@ -2,6 +2,7 @@ + diff --git a/realsense2_description/tests/one_of_each.xacro b/realsense2_description/tests/one_of_each.xacro index f458b92546..910fd56f8a 100644 --- a/realsense2_description/tests/one_of_each.xacro +++ b/realsense2_description/tests/one_of_each.xacro @@ -5,6 +5,7 @@ + From b02c598d63b190e99b0d7786b89425663f819e83 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 10 Dec 2019 16:45:42 +0200 Subject: [PATCH 098/131] upgrade version: 2.2.10 --- README.md | 4 ++-- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 926ee3de91..ac2a094e4c 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. -LibRealSense supported version: v2.29.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) +LibRealSense supported version: v2.30.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) ## Installation Instructions @@ -13,7 +13,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine - #### 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 and librealsense-dkms packages. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.29.0) 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.30.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 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 62704da272..a6ed03154b 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 9 +#define REALSENSE_ROS_PATCH_VERSION 10 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 198fec6053..5236b38cf8 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.9 + 2.2.10 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 62ffd3f797..b29e3a5508 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.9 + 2.2.10 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From 008e2e434a1a6523f7c9b733689273e9fa1579a7 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 11 Dec 2019 08:53:24 +0200 Subject: [PATCH 099/131] upgrade version: 2.2.11 --- README.md | 4 ++-- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index ac2a094e4c..cd1f2aac78 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. -LibRealSense supported version: v2.30.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) +LibRealSense supported version: v2.31.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) ## Installation Instructions @@ -13,7 +13,7 @@ The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kine - #### 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 and librealsense-dkms packages. #### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.30.0) 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.31.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 Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index a6ed03154b..3ff2df4a36 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 10 +#define REALSENSE_ROS_PATCH_VERSION 11 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 5236b38cf8..8894337ba6 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.10 + 2.2.11 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index b29e3a5508..7e1716a8a8 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.10 + 2.2.11 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From 63de2e77c652eca6ca9fcf4fca7b64db8f976f3c Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 17 Dec 2019 09:04:24 +0200 Subject: [PATCH 100/131] Support L515. switch sensor detection to new librealsense extensions API --- realsense2_camera/include/constants.h | 1 + realsense2_camera/src/base_realsense_node.cpp | 47 ++++++++----------- .../src/realsense_node_factory.cpp | 1 + 3 files changed, 22 insertions(+), 27 deletions(-) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 3ff2df4a36..effe0fb028 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -34,6 +34,7 @@ namespace realsense2_camera const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM const uint16_t RS405_PID = 0x0b0c; // DS5U const uint16_t RS_T265_PID = 0x0b37; // + const uint16_t RS_L515_PID = 0x0B3D; // const bool ALIGN_DEPTH = false; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 980b24500e..3d055c067f 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -692,46 +692,39 @@ void BaseRealSenseNode::setupDevice() std::function multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame, _imu_sync_method);}; ROS_INFO_STREAM("Device Sensors: "); - for(auto&& elem : _dev_sensors) + for(auto&& sensor : _dev_sensors) { - std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME); - - if ("Stereo Module" == module_name) - { - _sensors[DEPTH] = elem; - _sensors[INFRA1] = elem; - _sensors[INFRA2] = elem; - _sensors_callback[module_name] = frame_callback_function; - } - else if ("Coded-Light Depth Sensor" == module_name) + std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME); + if (sensor.is()) { - _sensors[DEPTH] = elem; - _sensors[INFRA1] = elem; + _sensors[DEPTH] = sensor; + _sensors[INFRA1] = sensor; + _sensors[INFRA2] = sensor; _sensors_callback[module_name] = frame_callback_function; } - else if ("RGB Camera" == module_name) + else if (sensor.is()) { - _sensors[COLOR] = elem; + _sensors[COLOR] = sensor; _sensors_callback[module_name] = frame_callback_function; } - else if ("Wide FOV Camera" == module_name) + else if (sensor.is()) { - _sensors[FISHEYE] = elem; + _sensors[FISHEYE] = sensor; _sensors_callback[module_name] = frame_callback_function; } - else if ("Motion Module" == module_name) + else if (sensor.is()) { - _sensors[GYRO] = elem; - _sensors[ACCEL] = elem; + _sensors[GYRO] = sensor; + _sensors[ACCEL] = sensor; _sensors_callback[module_name] = imu_callback_function; } - else if ("Tracking Module" == module_name) + else if (sensor.is()) { - _sensors[GYRO] = elem; - _sensors[ACCEL] = elem; - _sensors[POSE] = elem; - _sensors[FISHEYE1] = elem; - _sensors[FISHEYE2] = elem; + _sensors[GYRO] = sensor; + _sensors[ACCEL] = sensor; + _sensors[POSE] = sensor; + _sensors[FISHEYE1] = sensor; + _sensors[FISHEYE2] = sensor; _sensors_callback[module_name] = multiple_message_callback_function; } else @@ -740,7 +733,7 @@ void BaseRealSenseNode::setupDevice() ros::shutdown(); exit(1); } - ROS_INFO_STREAM(std::string(elem.get_info(RS2_CAMERA_INFO_NAME)) << " was found."); + ROS_INFO_STREAM(std::string(sensor.get_info(RS2_CAMERA_INFO_NAME)) << " was found."); } // Update "enable" map diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 1c85591f1a..0f16c40f75 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -295,6 +295,7 @@ void RealSenseNodeFactory::StartDevice() case RS435_RGB_PID: case RS435i_RGB_PID: case RS_USB2_PID: + case RS_L515_PID: _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); break; case RS_T265_PID: From 3b3aa8596bbf3264a4bfb60e1b54d01c6eff7b38 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 1 Jan 2020 10:35:20 +0200 Subject: [PATCH 101/131] fix a stability issue with reading rosbag files. --- realsense2_camera/src/base_realsense_node.cpp | 2 +- realsense2_camera/src/realsense_node_factory.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 3d055c067f..83933d8682 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -129,7 +129,7 @@ BaseRealSenseNode::BaseRealSenseNode(ros::NodeHandle& nodeHandle, BaseRealSenseNode::~BaseRealSenseNode() { // Kill dynamic transform thread - if (_publish_tf && _tf_publish_rate > 0) + if (_tf_t) _tf_t->join(); _is_running = false; diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 0f16c40f75..d1b77e8213 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -227,7 +227,6 @@ void RealSenseNodeFactory::onInit() pipe->start(cfg); //File will be opened in read mode at this point _device = pipe->get_active_profile().get_device(); _serial_no = _device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); - _realSenseNode = std::unique_ptr(new BaseRealSenseNode(nh, privateNh, _device, _serial_no)); } if (_device) { @@ -273,6 +272,7 @@ void RealSenseNodeFactory::onInit() void RealSenseNodeFactory::StartDevice() { + if (_realSenseNode) _realSenseNode.reset(); ros::NodeHandle nh = getNodeHandle(); ros::NodeHandle privateNh = getPrivateNodeHandle(); // TODO From 69d199d9e01a7c57b78831a6486c83ad8653aba9 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 12 Jan 2020 09:01:26 +0200 Subject: [PATCH 102/131] upgrade version: 2.2.12 --- realsense2_camera/CHANGELOG.rst | 7 +++++++ realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/CHANGELOG.rst | 6 ++++++ realsense2_description/package.xml | 2 +- 5 files changed, 16 insertions(+), 3 deletions(-) create mode 100644 realsense2_camera/CHANGELOG.rst create mode 100644 realsense2_description/CHANGELOG.rst diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst new file mode 100644 index 0000000000..6591f46948 --- /dev/null +++ b/realsense2_camera/CHANGELOG.rst @@ -0,0 +1,7 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package realsense2_camera +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- +* Add eigen dependency - missing for Melodic. Contributors: Antoine Hoarau diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 3ff2df4a36..672a1217fd 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 11 +#define REALSENSE_ROS_PATCH_VERSION 12 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 25423b2c68..40f0b97997 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.11 + 2.2.12 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst new file mode 100644 index 0000000000..8aafa705a3 --- /dev/null +++ b/realsense2_description/CHANGELOG.rst @@ -0,0 +1,6 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package realsense2_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Forthcoming +----------- diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 7e1716a8a8..3a853b2961 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.11 + 2.2.12 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From 1d3bfaa6c2ef49f8b4cd9cfccef57927a994e0c8 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 9 Feb 2020 14:03:07 +0200 Subject: [PATCH 103/131] fix bug: enable building release version instead of debug. --- realsense2_camera/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 6bc4adf28a..dc447b032e 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -39,7 +39,8 @@ 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() -if (CMAKE_BUILD_TYPE EQUAL "RELEASE") +string(TOUPPER "${CMAKE_BUILD_TYPE}" uppercase_CMAKE_BUILD_TYPE) +if (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE") message(STATUS "Create Release Build.") set(CMAKE_CXX_FLAGS "-O2 ${CMAKE_CXX_FLAGS}") else() From a35401ac8cff486312129d32b6edf0a10d9578cf Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 16 Feb 2020 12:24:29 +0200 Subject: [PATCH 104/131] add error message "Left IC2 Config error" to reset inducing errors list. --- realsense2_camera/src/base_realsense_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 980b24500e..593a6d9632 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -165,7 +165,8 @@ void BaseRealSenseNode::setupErrorCallback() { s.set_notifications_callback([&](const rs2::notification& n) { - std::vector error_strings({"RT IC2 Config error", + std::vector error_strings({"Left IC2 Config error", + "RT IC2 Config error", "Motion Module force pause", "stream start failure"}); if (n.get_severity() >= RS2_LOG_SEVERITY_ERROR) From 9c447a2f90e8c479111412eedf8e830090ad5b78 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 18 Feb 2020 11:19:05 +0200 Subject: [PATCH 105/131] fix usb_port_id parsing. Including librealsense fixed port_id for T265. --- .../include/realsense_node_factory.h | 1 + .../src/realsense_node_factory.cpp | 55 ++++++++++--------- 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 0a345c52a4..d585ee50d6 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -63,6 +63,7 @@ namespace realsense2_camera void getDevice(rs2::device_list list); virtual void onInit() override; void tryGetLogSeverity(rs2_log_severity& severity) const; + static std::string parse_usb_port(std::string line); rs2::device _device; std::unique_ptr _realSenseNode; diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 1c85591f1a..724ba2d055 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -47,6 +47,28 @@ RealSenseNodeFactory::~RealSenseNodeFactory() closeDevice(); } +std::string RealSenseNodeFactory::parse_usb_port(std::string line) +{ + std::string port_id; + std::regex self_regex("(?:[^ ]+/usb[0-9]+[0-9./-]*/){0,1}([0-9.-]+)(:){0,1}[^ ]*", std::regex_constants::ECMAScript); + std::smatch base_match; + bool found = std::regex_match(line, base_match, self_regex); + if (found) + { + port_id = base_match[1].str(); + if (base_match[2].str().size() == 0) //This is libuvc string. Remove counter is exists. + { + std::regex end_regex = std::regex(".+(-[0-9]+$)", std::regex_constants::ECMAScript); + bool found_end = std::regex_match(port_id, base_match, end_regex); + if (found_end) + { + port_id = port_id.substr(0, port_id.size() - base_match[1].str().size()); + } + } + } + return port_id; +} + void RealSenseNodeFactory::getDevice(rs2::device_list list) { if (!_device) @@ -66,32 +88,10 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) std::string pn = dev.get_info(RS2_CAMERA_INFO_PHYSICAL_PORT); std::string name = dev.get_info(RS2_CAMERA_INFO_NAME); ROS_INFO_STREAM("Device with physical ID " << pn << " was found."); - std::string port_id; std::vector results; ROS_INFO_STREAM("Device with name " << name << " was found."); - std::regex self_regex; - if(name == std::string("Intel RealSense T265")) - { - self_regex = std::regex(".*?bus_([0-9]+) port_([0-9]+).*", std::regex_constants::ECMAScript); - } - else// if(strcmp(name, "Intel RealSense D435") == 0) - { - self_regex = std::regex("[^ ]+/usb[0-9]+[0-9./-]*/([0-9.-]+):[^ ]*", std::regex_constants::ECMAScript); - } - std::smatch base_match; - bool found_usb_desc = std::regex_match(pn, base_match, self_regex); - if (found_usb_desc) - { - std::ssub_match base_sub_match = base_match[1]; - port_id = base_sub_match.str(); - for (unsigned int mi = 2; mi < base_match.size(); mi++) - { - std::ssub_match base_sub_match = base_match[mi]; - port_id += "-" + base_sub_match.str(); - } - ROS_INFO_STREAM("Device with port number " << port_id << " was found."); - } - else + std::string port_id = parse_usb_port(pn); + if (port_id.empty()) { std::stringstream msg; msg << "Error extracting usb port from device with physical ID: " << pn << std::endl << "Please report on github issue at https://github.com/IntelRealSense/realsense-ros"; @@ -105,11 +105,16 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) ROS_ERROR_STREAM("Please use serial number instead of usb port."); } } + else + { + ROS_INFO_STREAM("Device with port number " << port_id << " was found."); + } bool found_device_type(true); if (!_device_type.empty()) { + std::smatch match_results; std::regex device_type_regex(_device_type.c_str(), std::regex::icase); - found_device_type = std::regex_search(name, base_match, device_type_regex); + found_device_type = std::regex_search(name, match_results, device_type_regex); } if ((_serial_no.empty() || sn == _serial_no) && (_usb_port_id.empty() || port_id == _usb_port_id) && found_device_type) From a4d9335f73915ad44d15f888708d995e439d0fee Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 18 Feb 2020 11:59:25 +0200 Subject: [PATCH 106/131] fix: add default value: release. --- realsense2_camera/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index dc447b032e..d8f24d0fae 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -39,6 +39,10 @@ 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() +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + string(TOUPPER "${CMAKE_BUILD_TYPE}" uppercase_CMAKE_BUILD_TYPE) if (${uppercase_CMAKE_BUILD_TYPE} STREQUAL "RELEASE") message(STATUS "Create Release Build.") From 142bafcbe1ca21f6f40b7ae6c1f47f50d42e42f0 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 1 Mar 2020 13:17:27 +0200 Subject: [PATCH 107/131] Fix thread not joined error which caused an exception before calling BaseRealSenseNode's destructor. Add "Left IC2 Config error" and "stream start failure" to reset causing errors list and remove "Motion Module force pause" --- realsense2_camera/src/base_realsense_node.cpp | 4 ++-- realsense2_camera/src/realsense_node_factory.cpp | 13 +++---------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 980b24500e..fb54c884f5 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -165,8 +165,8 @@ void BaseRealSenseNode::setupErrorCallback() { s.set_notifications_callback([&](const rs2::notification& n) { - std::vector error_strings({"RT IC2 Config error", - "Motion Module force pause", + std::vector error_strings({"RT IC2 Config error", + "Left IC2 Config error", "stream start failure"}); if (n.get_severity() >= RS2_LOG_SEVERITY_ERROR) { diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 1c85591f1a..2845ffb952 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -33,20 +33,14 @@ RealSenseNodeFactory::RealSenseNodeFactory() rs2::log_to_console(severity); } -void RealSenseNodeFactory::closeDevice() +RealSenseNodeFactory::~RealSenseNodeFactory() { - for(rs2::sensor sensor : _device.query_sensors()) + if (_query_thread.joinable()) { - sensor.stop(); - sensor.close(); + _query_thread.join(); } } -RealSenseNodeFactory::~RealSenseNodeFactory() -{ - closeDevice(); -} - void RealSenseNodeFactory::getDevice(rs2::device_list list) { if (!_device) @@ -243,7 +237,6 @@ void RealSenseNodeFactory::onInit() std::chrono::milliseconds timespan(6000); while (!_device) { - // _ctx.init_tracking_module(); // Unavailable function. getDevice(_ctx.query_devices()); if (_device) { From 379653aa515fb82ce181f2ef4ed85c5e2a9e0902 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 1 Mar 2020 13:17:27 +0200 Subject: [PATCH 108/131] Fix thread not joined error which caused an exception before calling BaseRealSenseNode's destructor. Add "Left IC2 Config error" and "stream start failure" to reset causing errors list and remove "Motion Module force pause" --- realsense2_camera/src/base_realsense_node.cpp | 1 - realsense2_camera/src/realsense_node_factory.cpp | 13 +++---------- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 593a6d9632..d878c06480 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -167,7 +167,6 @@ void BaseRealSenseNode::setupErrorCallback() { std::vector error_strings({"Left IC2 Config error", "RT IC2 Config error", - "Motion Module force pause", "stream start failure"}); if (n.get_severity() >= RS2_LOG_SEVERITY_ERROR) { diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 724ba2d055..f1afb327d1 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -33,20 +33,14 @@ RealSenseNodeFactory::RealSenseNodeFactory() rs2::log_to_console(severity); } -void RealSenseNodeFactory::closeDevice() +RealSenseNodeFactory::~RealSenseNodeFactory() { - for(rs2::sensor sensor : _device.query_sensors()) + if (_query_thread.joinable()) { - sensor.stop(); - sensor.close(); + _query_thread.join(); } } -RealSenseNodeFactory::~RealSenseNodeFactory() -{ - closeDevice(); -} - std::string RealSenseNodeFactory::parse_usb_port(std::string line) { std::string port_id; @@ -248,7 +242,6 @@ void RealSenseNodeFactory::onInit() std::chrono::milliseconds timespan(6000); while (!_device) { - // _ctx.init_tracking_module(); // Unavailable function. getDevice(_ctx.query_devices()); if (_device) { From d71bde7f1f7e9b973008ab8c9cf4da58f885db0d Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 1 Mar 2020 13:45:16 +0200 Subject: [PATCH 109/131] remove "stream start failure" from reset causing list. --- realsense2_camera/src/base_realsense_node.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index d878c06480..4bffac7da2 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -166,8 +166,8 @@ void BaseRealSenseNode::setupErrorCallback() s.set_notifications_callback([&](const rs2::notification& n) { std::vector error_strings({"Left IC2 Config error", - "RT IC2 Config error", - "stream start failure"}); + "RT IC2 Config error" + }); 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()); @@ -175,7 +175,7 @@ void BaseRealSenseNode::setupErrorCallback() 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"); + ROS_ERROR_STREAM("Performing Hardware Reset."); _dev.hardware_reset(); } }); From 26e6768055cdfa6a51797ce06e36ec7b9887cfe9 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Mon, 2 Mar 2020 11:47:19 +0100 Subject: [PATCH 110/131] Reduce memory allocations in BaseRealSenseNode::publishPointCloud() There were two missed opportunities for optimization: - sensor_msgs::PointCloud2 should not be created at any frame, because, that would force memory allocation, that is costly. - using std::list should be forbidden ;) --- .../include/base_realsense_node.h | 3 ++ realsense2_camera/src/base_realsense_node.cpp | 46 +++++++++---------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index f020ce58ad..d38b0a24e6 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -317,6 +317,9 @@ namespace realsense2_camera stream_index_pair _base_stream; const std::string _namespace; + sensor_msgs::PointCloud2 _msg_pointcloud; + std::vector< unsigned int > _valid_pc_indices; + };//end class } diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 593a6d9632..5622916c99 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1985,7 +1985,8 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co const rs2::vertex* vertex = pc.get_vertices(); const rs2::texture_coordinate* color_point = pc.get_texture_coordinates(); - std::list valid_indices; + + _valid_pc_indices.clear(); for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, color_point++) { if (static_cast(vertex->z) > 0) @@ -1994,19 +1995,18 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co float j = static_cast(color_point->v); if (_allow_no_texture_points || (i >= 0.f && i <= 1.f && j >= 0.f && j <= 1.f)) { - valid_indices.push_back(point_idx); + _valid_pc_indices.push_back(point_idx); } } } - sensor_msgs::PointCloud2 msg_pointcloud; - msg_pointcloud.header.stamp = t; - msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; - msg_pointcloud.width = valid_indices.size(); - msg_pointcloud.height = 1; - msg_pointcloud.is_dense = true; + _msg_pointcloud.header.stamp = t; + _msg_pointcloud.header.frame_id = _optical_frame_id[DEPTH]; + _msg_pointcloud.width = _valid_pc_indices.size(); + _msg_pointcloud.height = 1; + _msg_pointcloud.is_dense = true; - sensor_msgs::PointCloud2Modifier modifier(msg_pointcloud); + sensor_msgs::PointCloud2Modifier modifier(_msg_pointcloud); modifier.setPointCloud2FieldsByString(1, "xyz"); vertex = pc.get_vertices(); @@ -2029,19 +2029,19 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co 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::FLOAT32, 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); + _msg_pointcloud.point_step = addPointField(_msg_pointcloud, format_str.c_str(), 1, sensor_msgs::PointField::FLOAT32, _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]; unsigned int prev_idx(0); - for (auto idx=valid_indices.begin(); idx != valid_indices.end(); idx++) + for (auto idx=_valid_pc_indices.begin(); idx != _valid_pc_indices.end(); idx++) { unsigned int idx_jump(*idx-prev_idx); prev_idx = *idx; @@ -2070,11 +2070,11 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co } else { - sensor_msgs::PointCloud2Iteratoriter_x(msg_pointcloud, "x"); - sensor_msgs::PointCloud2Iteratoriter_y(msg_pointcloud, "y"); - sensor_msgs::PointCloud2Iteratoriter_z(msg_pointcloud, "z"); + sensor_msgs::PointCloud2Iteratoriter_x(_msg_pointcloud, "x"); + sensor_msgs::PointCloud2Iteratoriter_y(_msg_pointcloud, "y"); + sensor_msgs::PointCloud2Iteratoriter_z(_msg_pointcloud, "z"); unsigned int prev_idx(0); - for (auto idx=valid_indices.begin(); idx != valid_indices.end(); idx++) + for (auto idx=_valid_pc_indices.begin(); idx != _valid_pc_indices.end(); idx++) { unsigned int idx_jump(*idx-prev_idx); prev_idx = *idx; @@ -2087,7 +2087,7 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co ++iter_x; ++iter_y; ++iter_z; } } - _pointcloud_publisher.publish(msg_pointcloud); + _pointcloud_publisher.publish(_msg_pointcloud); } From c4bb30ecdae9292535cd2a2e89e9fb298dbead83 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 8 Mar 2020 14:08:06 +0200 Subject: [PATCH 111/131] catch failure on device creation. kill query_thread if closed before device is found. --- .../include/realsense_node_factory.h | 1 + .../src/realsense_node_factory.cpp | 18 +++++++++++++++--- 2 files changed, 16 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index d585ee50d6..6b4110f923 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -73,6 +73,7 @@ namespace realsense2_camera std::string _device_type; bool _initial_reset; std::thread _query_thread; + bool _is_alive; }; }//end namespace diff --git a/realsense2_camera/src/realsense_node_factory.cpp b/realsense2_camera/src/realsense_node_factory.cpp index 1d3c7d1039..3f99a693ca 100644 --- a/realsense2_camera/src/realsense_node_factory.cpp +++ b/realsense2_camera/src/realsense_node_factory.cpp @@ -20,7 +20,8 @@ constexpr auto realsense_ros_camera_version = REALSENSE_ROS_EMBEDDED_VERSION_STR PLUGINLIB_EXPORT_CLASS(realsense2_camera::RealSenseNodeFactory, nodelet::Nodelet) -RealSenseNodeFactory::RealSenseNodeFactory() +RealSenseNodeFactory::RealSenseNodeFactory(): + _is_alive(true) { ROS_INFO("RealSense ROS v%s", REALSENSE_ROS_VERSION_STR); ROS_INFO("Running with LibRealSense v%s", RS2_API_VERSION_STR); @@ -35,6 +36,7 @@ RealSenseNodeFactory::RealSenseNodeFactory() RealSenseNodeFactory::~RealSenseNodeFactory() { + _is_alive = false; if (_query_thread.joinable()) { _query_thread.join(); @@ -75,8 +77,18 @@ void RealSenseNodeFactory::getDevice(rs2::device_list list) { bool found = false; ROS_INFO_STREAM(" "); - for (auto&& dev : list) + for (size_t count = 0; count < list.size(); count++) { + rs2::device dev; + try + { + dev = list[count]; + } + catch(const std::exception& ex) + { + ROS_WARN_STREAM("Device " << count+1 << "/" << list.size() << " failed with exception: " << ex.what()); + continue; + } auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER); ROS_INFO_STREAM("Device with serial number " << sn << " was found."< Date: Sun, 8 Mar 2020 14:21:59 +0200 Subject: [PATCH 112/131] upgrade version to 2.2.13 update README.md file. --- README.md | 84 ++++++++++++++++----------- realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_camera/package.xml | 2 +- realsense2_description/package.xml | 2 +- 5 files changed, 54 insertions(+), 38 deletions(-) diff --git a/README.md b/README.md index cd1f2aac78..4d5ec52f1d 100644 --- a/README.md +++ b/README.md @@ -1,48 +1,65 @@ # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. -LibRealSense supported version: v2.31.0 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) +LibRealSense supported version: v2.33.1 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) ## Installation Instructions -The following instructions support ROS Indigo, on **Ubuntu 14.04**, and ROS Kinetic, on **Ubuntu 16.04**. +The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but apply to ROS Melodic on **Ubuntu 18.04** as well, by replacing kinetic with melodic wherever is needed. -#### 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 ROS distribution + - #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 or [ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) on Ubuntu 18.04. -### 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) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense-dkms packages. -#### OR -- #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.31.0) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) +### There are 2 sources to install realsense2_camera from: -### Step 2: Install the ROS distribution -- #### Install [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu), on Ubuntu 16.04 +* ### Method 1: The ROS distribution: + realsense2_camera is available as a debian package of ROS distribution. It can be installed by typing: + + `sudo apt-get install ros-kinetic-realsense-camera` -### Step 3: Install Intel® RealSense™ ROS from Sources -- Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace -```bash -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws/src/ -``` -- Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' -```bashrc -git clone https://github.com/IntelRealSense/realsense-ros.git -cd realsense-ros/ -git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1` -cd .. -``` -- Make sure all dependent packages are installed. You can check .travis.yml file for reference. -- Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.0) + This will install both realsense2_camera and its dependents, including librealsense2 library. -```bash -catkin_init_workspace -cd .. -catkin_make clean -catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release -catkin_make install -echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc -source ~/.bashrc -``` + Notice: + * The version of librealsense2 is almost always behind the one availeable in RealSense™ official repository. + * librealsense2 is not built to use native v4l2 driver but the less stable RS-USB protocol. That is because the last is more general and operational on a larger variety of platforms. + +* ### Method 2: The RealSense™ distribution: + + #### This option is demonstrated in the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 2 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) - In that case treat yourself as a developer. Make sure you follow the instructions to also install librealsense2-dev and librealsense-dkms packages. + + #### OR + - #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.33.1) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) + + + ### Step 2: Install Intel® RealSense™ ROS from Sources + - Create a [catkin](http://wiki.ros.org/catkin#Installing_catkin) workspace + ```bash + mkdir -p ~/catkin_ws/src + cd ~/catkin_ws/src/ + ``` + - Clone the latest Intel® RealSense™ ROS from [here](https://github.com/intel-ros/realsense/releases) into 'catkin_ws/src/' + ```bashrc + git clone https://github.com/IntelRealSense/realsense-ros.git + cd realsense-ros/ + git checkout `git tag | sort -V | grep -P "^\d+\.\d+\.\d+" | tail -1` + cd .. + ``` + - Make sure all dependent packages are installed. You can check .travis.yml file for reference. + - Specifically, make sure that the ros package *ddynamic_reconfigure* is installed. If *ddynamic_reconfigure* cannot be installed using APT, you may clone it into your workspace 'catkin_ws/src/' from [here](https://github.com/pal-robotics/ddynamic_reconfigure/tree/kinetic-devel) (Version 0.2.2) + + ```bash + catkin_init_workspace + cd .. + catkin_make clean + catkin_make -DCATKIN_ENABLE_TESTING=False -DCMAKE_BUILD_TYPE=Release + catkin_make install + echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc + source ~/.bashrc + ``` ## Usage Instructions @@ -228,7 +245,6 @@ python src/realsense/realsense2_camera/scripts/rs2_test.py --all ## Known Issues * This ROS node does not currently support [ROS Lunar Loggerhead](http://wiki.ros.org/lunar). * This ROS node does not currently work with [ROS 2](https://github.com/ros2/ros2/wiki). -* This ROS node currently does not provide the unit-tests which ensure the proper operation of the camera. Future versions of the node will provide ROS compatible unit-tests. * This ROS node currently does not support running multiple T265 cameras at once. This will be addressed in a future update. ## License diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index d8f24d0fae..5327d5f1fd 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -34,7 +34,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2 2.29.0) +find_package(realsense2 2.32.1) 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() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index 17b9995671..d6a561e638 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 12 +#define REALSENSE_ROS_PATCH_VERSION 13 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index 40f0b97997..bac96d5709 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.12 + 2.2.13 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 3a853b2961..8aa10d4e16 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.12 + 2.2.13 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg From 98a8c71d707b349249fb01ac1f18a03abda3a9b2 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 8 Mar 2020 14:40:14 +0200 Subject: [PATCH 113/131] add CHANGELOG.rst --- realsense2_camera/CHANGELOG.rst | 6 ++++++ realsense2_description/CHANGELOG.rst | 1 + 2 files changed, 7 insertions(+) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index 6591f46948..d445c8a175 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -4,4 +4,10 @@ Changelog for package realsense2_camera Forthcoming ----------- +* upgrade version to 2.2.13 +* fix ctrl-C closing issues. +* handle device creation exceptions. +* support LiDAR camera L515. +* optimize pointcloud. Contributors: Davide Faconti +* fix usb port id parsing issues. * Add eigen dependency - missing for Melodic. Contributors: Antoine Hoarau diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 8aafa705a3..85fd1505e4 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -4,3 +4,4 @@ Changelog for package realsense2_description Forthcoming ----------- +* upgrade version to 2.2.13 From b4363754069b00907cf30e098b881242cd925e2e Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Sat, 14 Mar 2020 17:39:13 +0000 Subject: [PATCH 114/131] correct offset between camera_link and base_link --- realsense2_description/urdf/_d435.urdf.xacro | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index b6e3259bf1..7af8629fd0 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -28,6 +28,12 @@ aluminum peripherial evaluation case. + + + + + + @@ -44,14 +50,15 @@ aluminum peripherial evaluation case. - + - + + From 5ffc651aa6bdb786e0a188cce5de594e54c8e403 Mon Sep 17 00:00:00 2001 From: Marco Camurri Date: Sun, 15 Mar 2020 00:12:20 +0000 Subject: [PATCH 115/131] fix use_nominal_extrinsics arg/property --- realsense2_description/urdf/_d415.urdf.xacro | 2 +- realsense2_description/urdf/_d435.urdf.xacro | 2 +- realsense2_description/urdf/_r410.urdf.xacro | 2 +- realsense2_description/urdf/_r430.urdf.xacro | 2 +- realsense2_description/urdf/test_d415_camera.urdf.xacro | 3 ++- realsense2_description/urdf/test_d435_camera.urdf.xacro | 3 ++- .../urdf/test_d435_multiple_cameras.urdf.xacro | 5 +++-- realsense2_description/urdf/test_r410_camera.urdf.xacro | 3 ++- realsense2_description/urdf/test_r430_camera.urdf.xacro | 3 ++- 9 files changed, 15 insertions(+), 10 deletions(-) diff --git a/realsense2_description/urdf/_d415.urdf.xacro b/realsense2_description/urdf/_d415.urdf.xacro index 7dc06ddd0f..f99b7b9216 100644 --- a/realsense2_description/urdf/_d415.urdf.xacro +++ b/realsense2_description/urdf/_d415.urdf.xacro @@ -73,7 +73,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_d435.urdf.xacro b/realsense2_description/urdf/_d435.urdf.xacro index b6e3259bf1..2688e59309 100644 --- a/realsense2_description/urdf/_d435.urdf.xacro +++ b/realsense2_description/urdf/_d435.urdf.xacro @@ -72,7 +72,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r410.urdf.xacro b/realsense2_description/urdf/_r410.urdf.xacro index 8c27c3b38d..664f8ea9c7 100644 --- a/realsense2_description/urdf/_r410.urdf.xacro +++ b/realsense2_description/urdf/_r410.urdf.xacro @@ -65,7 +65,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/_r430.urdf.xacro b/realsense2_description/urdf/_r430.urdf.xacro index 44cea7dad6..acabbf3537 100644 --- a/realsense2_description/urdf/_r430.urdf.xacro +++ b/realsense2_description/urdf/_r430.urdf.xacro @@ -66,7 +66,7 @@ aluminum peripherial evaluation case. - + diff --git a/realsense2_description/urdf/test_d415_camera.urdf.xacro b/realsense2_description/urdf/test_d415_camera.urdf.xacro index 524821f859..dcd017aca3 100644 --- a/realsense2_description/urdf/test_d415_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d415_camera.urdf.xacro @@ -1,9 +1,10 @@ + - + diff --git a/realsense2_description/urdf/test_d435_camera.urdf.xacro b/realsense2_description/urdf/test_d435_camera.urdf.xacro index dd5a6ee7eb..3d41f35309 100644 --- a/realsense2_description/urdf/test_d435_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d435_camera.urdf.xacro @@ -1,9 +1,10 @@ + - + diff --git a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro index 043ea35de3..3768ac8ad9 100644 --- a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro +++ b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro @@ -1,13 +1,14 @@ + - + - + diff --git a/realsense2_description/urdf/test_r410_camera.urdf.xacro b/realsense2_description/urdf/test_r410_camera.urdf.xacro index e1876cf570..f843daf3e2 100644 --- a/realsense2_description/urdf/test_r410_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r410_camera.urdf.xacro @@ -1,9 +1,10 @@ + - + diff --git a/realsense2_description/urdf/test_r430_camera.urdf.xacro b/realsense2_description/urdf/test_r430_camera.urdf.xacro index 4f78ad8a4b..d11478ca9b 100644 --- a/realsense2_description/urdf/test_r430_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r430_camera.urdf.xacro @@ -1,9 +1,10 @@ + - + From 3ab5077763e980ae72c0c29aec4a697067d43744 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 22 Mar 2020 07:48:56 +0200 Subject: [PATCH 116/131] Update README.md fix typo in README.md: ros-kinetic-realsense-camera -> ros-kinetic-realsense2-camera --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 4d5ec52f1d..780bc7c781 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,7 @@ The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but * ### Method 1: The ROS distribution: realsense2_camera is available as a debian package of ROS distribution. It can be installed by typing: - `sudo apt-get install ros-kinetic-realsense-camera` + `sudo apt-get install ros-kinetic-realsense2-camera` This will install both realsense2_camera and its dependents, including librealsense2 library. From da9dfd6fd6548d10fec17f5ee92f6a789141f00f Mon Sep 17 00:00:00 2001 From: potatehoes Date: Thu, 7 May 2020 18:06:58 -0600 Subject: [PATCH 117/131] Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 780bc7c781..4ac060c3ad 100644 --- a/README.md +++ b/README.md @@ -24,6 +24,12 @@ The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but * The version of librealsense2 is almost always behind the one availeable in RealSense™ official repository. * librealsense2 is not built to use native v4l2 driver but the less stable RS-USB protocol. That is because the last is more general and operational on a larger variety of platforms. + realsense2_description is available as a debian package of ROS distribution. It is necessary for running some of the RealSense™ d400 series. It can be installed by typing: + + `sudo apt-get install ros-kinetic-realsense2-description` + + This will install both realsense2_description and its dependents + * ### Method 2: The RealSense™ distribution: #### This option is demonstrated in the [.travis.yml](https://github.com/intel-ros/realsense/blob/development/.travis.yml) file. It basically summerize the elaborate instructions in the following 2 steps: From 5e8f30b28cbc24f068d8147607a400d388d9cf4c Mon Sep 17 00:00:00 2001 From: doronhi Date: Mon, 18 May 2020 05:45:29 +0300 Subject: [PATCH 118/131] fix bug: sometimes sensors are not closed when program terminates --- realsense2_camera/src/base_realsense_node.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 48634f8769..6220c9e470 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -138,6 +138,18 @@ BaseRealSenseNode::~BaseRealSenseNode() { _monitoring_t->join(); } + + std::set module_names; + for (const std::pair>& profile : _enabled_profiles) + { + std::string module_name = _sensors[profile.first].get_info(RS2_CAMERA_INFO_NAME); + std::pair< std::set::iterator, bool> res = module_names.insert(module_name); + if (res.second) + { + _sensors[profile.first].stop(); + _sensors[profile.first].close(); + } + } } void BaseRealSenseNode::toggleSensors(bool enabled) From d72f5cbb6bbc4048c67470d69dcecb450e4a2e28 Mon Sep 17 00:00:00 2001 From: doronhi Date: Sun, 24 May 2020 12:41:50 +0300 Subject: [PATCH 119/131] README.md: fix unite_imu_method description. --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 780bc7c781..4274d1049a 100644 --- a/README.md +++ b/README.md @@ -126,9 +126,9 @@ The topics are of the form: ```/camera/aligned_depth_to_color/image_raw``` etc. - **odom_frame_id**: defines the origin coordinate system in ROS convention (X-Forward, Y-Left, Z-Up). pose topic defines the pose relative to that system. - **All the rest of the frame_ids can be found in the template launch file: [nodelet.launch.xml](./realsense2_camera/launch/includes/nodelet.launch.xml)** - **unite_imu_method**: The D435i and T265 cameras have built in IMU components which produce 2 unrelated streams: *gyro* - which shows angular velocity and *accel* which shows linear acceleration. Each with it's own frequency. By default, 2 corresponding topics are available, each with only the relevant fields of the message sensor_msgs::Imu are filled out. -Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. Under the new topic, all the fields in the Imu message are filled out. - - **linear_interpolation**: Each message contains the last original value of item A interpolated with the previous value of item A, combined with the last original value of item B on last item B's timestamp. Items A and B are accel and gyro interchangeably, according to which type recently arrived from the sensor. The idea is to give the most recent information, united and without repetitions. - - **copy**: For each new message, accel or gyro, the relevant fields and timestamp are filled out while the others maintain the previous data. +Setting *unite_imu_method* creates a new topic, *imu*, that replaces the default *gyro* and *accel* topics. The *imu* topic is published at the rate of the gyro. All the fields of the Imu message under the *imu* topic are filled out. + - **linear_interpolation**: Every gyro message is attached by the an accel message interpolated to the gyro's timestamp. + - **copy**: Every gyro message is attached by the last accel message. - **clip_distance**: remove from the depth image all values above a given value (meters). Disable by giving negative value (default) - **linear_accel_cov**, **angular_velocity_cov**: sets the variance given to the Imu readings. For the T265, these values are being modified by the inner confidence value. - **hold_back_imu_for_frames**: Images processing takes time. Therefor there is a time gap between the moment the image arrives at the wrapper and the moment the image is published to the ROS environment. During this time, Imu messages keep on arriving and a situation is created where an image with earlier timestamp is published after Imu message with later timestamp. If that is a problem, setting *hold_back_imu_for_frames* to *true* will hold the Imu messages back while processing the images and then publish them all in a burst, thus keeping the order of publication as the order of arrival. Note that in either case, the timestamp in each message's header reflects the time of it's origin. From 4411dc09864cabea2865a8b29983bd627e0ea771 Mon Sep 17 00:00:00 2001 From: Pedro Machado Date: Tue, 26 May 2020 12:10:05 +0100 Subject: [PATCH 120/131] Update README.md added extra comments for installing in ROS melodic. --- README.md | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 780bc7c781..c99c7ee47c 100644 --- a/README.md +++ b/README.md @@ -16,7 +16,14 @@ The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but * ### Method 1: The ROS distribution: realsense2_camera is available as a debian package of ROS distribution. It can be installed by typing: - `sudo apt-get install ros-kinetic-realsense2-camera` + ``` + export ROS_VER=kinetic + ``` + or + ``` + export ROS_VER=melodic + ``` + sudo apt-get install ros-$ROS_VER-realsense2-camera` This will install both realsense2_camera and its dependents, including librealsense2 library. From 6a0da7791dfba1c765918e3858aea6d5f11fddc6 Mon Sep 17 00:00:00 2001 From: Brice Date: Fri, 29 May 2020 11:12:05 +0200 Subject: [PATCH 121/131] change state_publisher into robot_state_publisher --- realsense2_camera/launch/rs_d435_camera_with_model.launch | 2 +- realsense2_description/launch/view_d415_model.launch | 2 +- realsense2_description/launch/view_d435_model.launch | 2 +- realsense2_description/launch/view_multiple_d435_models.launch | 2 +- realsense2_description/launch/view_r410_model.launch | 2 +- realsense2_description/launch/view_r430_model.launch | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/launch/rs_d435_camera_with_model.launch b/realsense2_camera/launch/rs_d435_camera_with_model.launch index 2cc52f4b67..4212775a0f 100644 --- a/realsense2_camera/launch/rs_d435_camera_with_model.launch +++ b/realsense2_camera/launch/rs_d435_camera_with_model.launch @@ -104,6 +104,6 @@ - + diff --git a/realsense2_description/launch/view_d415_model.launch b/realsense2_description/launch/view_d415_model.launch index 46827f3e52..d131744a60 100644 --- a/realsense2_description/launch/view_d415_model.launch +++ b/realsense2_description/launch/view_d415_model.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/launch/view_d435_model.launch b/realsense2_description/launch/view_d435_model.launch index 2ef6b165f9..2b703398a2 100644 --- a/realsense2_description/launch/view_d435_model.launch +++ b/realsense2_description/launch/view_d435_model.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/launch/view_multiple_d435_models.launch b/realsense2_description/launch/view_multiple_d435_models.launch index 5f5d18b24d..407826ca87 100644 --- a/realsense2_description/launch/view_multiple_d435_models.launch +++ b/realsense2_description/launch/view_multiple_d435_models.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/launch/view_r410_model.launch b/realsense2_description/launch/view_r410_model.launch index 68d9addb26..923569b651 100644 --- a/realsense2_description/launch/view_r410_model.launch +++ b/realsense2_description/launch/view_r410_model.launch @@ -1,6 +1,6 @@ - + diff --git a/realsense2_description/launch/view_r430_model.launch b/realsense2_description/launch/view_r430_model.launch index a835dec4e2..1f9434f44a 100644 --- a/realsense2_description/launch/view_r430_model.launch +++ b/realsense2_description/launch/view_r430_model.launch @@ -1,6 +1,6 @@ - + From faa290f6d63b3e3e03e4aa90558fd6067ffdf584 Mon Sep 17 00:00:00 2001 From: Brice Date: Fri, 29 May 2020 11:14:04 +0200 Subject: [PATCH 122/131] fix noetic xacro macro call --- realsense2_description/tests/dual_d415.xacro | 8 ++++---- realsense2_description/tests/dual_d435.xacro | 8 ++++---- realsense2_description/tests/dual_r410.xacro | 8 ++++---- realsense2_description/tests/dual_r430.xacro | 8 ++++---- realsense2_description/tests/one_of_each.xacro | 16 ++++++++-------- .../urdf/test_d415_camera.urdf.xacro | 4 ++-- .../urdf/test_d435_camera.urdf.xacro | 4 ++-- .../urdf/test_d435_multiple_cameras.urdf.xacro | 8 ++++---- .../urdf/test_r410_camera.urdf.xacro | 4 ++-- .../urdf/test_r430_camera.urdf.xacro | 4 ++-- 10 files changed, 36 insertions(+), 36 deletions(-) diff --git a/realsense2_description/tests/dual_d415.xacro b/realsense2_description/tests/dual_d415.xacro index fa269f648b..a2c5870fb2 100644 --- a/realsense2_description/tests/dual_d415.xacro +++ b/realsense2_description/tests/dual_d415.xacro @@ -4,10 +4,10 @@ - + - - + + - + diff --git a/realsense2_description/tests/dual_d435.xacro b/realsense2_description/tests/dual_d435.xacro index 531aff2347..d062875249 100644 --- a/realsense2_description/tests/dual_d435.xacro +++ b/realsense2_description/tests/dual_d435.xacro @@ -4,10 +4,10 @@ - + - - + + - + diff --git a/realsense2_description/tests/dual_r410.xacro b/realsense2_description/tests/dual_r410.xacro index 79e5ad0258..686c52a5e3 100644 --- a/realsense2_description/tests/dual_r410.xacro +++ b/realsense2_description/tests/dual_r410.xacro @@ -4,10 +4,10 @@ - + - - + + - + diff --git a/realsense2_description/tests/dual_r430.xacro b/realsense2_description/tests/dual_r430.xacro index c2be68e736..463e76149c 100644 --- a/realsense2_description/tests/dual_r430.xacro +++ b/realsense2_description/tests/dual_r430.xacro @@ -4,10 +4,10 @@ - + - - + + - + diff --git a/realsense2_description/tests/one_of_each.xacro b/realsense2_description/tests/one_of_each.xacro index 910fd56f8a..58c1bdf016 100644 --- a/realsense2_description/tests/one_of_each.xacro +++ b/realsense2_description/tests/one_of_each.xacro @@ -7,16 +7,16 @@ - + - - + + - - + + - - + + - + diff --git a/realsense2_description/urdf/test_d415_camera.urdf.xacro b/realsense2_description/urdf/test_d415_camera.urdf.xacro index 524821f859..d16f43d518 100644 --- a/realsense2_description/urdf/test_d415_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d415_camera.urdf.xacro @@ -3,7 +3,7 @@ - + - + diff --git a/realsense2_description/urdf/test_d435_camera.urdf.xacro b/realsense2_description/urdf/test_d435_camera.urdf.xacro index dd5a6ee7eb..6d02adef64 100644 --- a/realsense2_description/urdf/test_d435_camera.urdf.xacro +++ b/realsense2_description/urdf/test_d435_camera.urdf.xacro @@ -3,7 +3,7 @@ - + - + diff --git a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro index 043ea35de3..02eefb1c87 100644 --- a/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro +++ b/realsense2_description/urdf/test_d435_multiple_cameras.urdf.xacro @@ -3,11 +3,11 @@ - + - + - + - + diff --git a/realsense2_description/urdf/test_r410_camera.urdf.xacro b/realsense2_description/urdf/test_r410_camera.urdf.xacro index e1876cf570..8d24892f76 100644 --- a/realsense2_description/urdf/test_r410_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r410_camera.urdf.xacro @@ -3,7 +3,7 @@ - + - + diff --git a/realsense2_description/urdf/test_r430_camera.urdf.xacro b/realsense2_description/urdf/test_r430_camera.urdf.xacro index 4f78ad8a4b..0a9d740a08 100644 --- a/realsense2_description/urdf/test_r430_camera.urdf.xacro +++ b/realsense2_description/urdf/test_r430_camera.urdf.xacro @@ -3,7 +3,7 @@ - + - + From 52452255a8d72732d52a59a73419d424497af2f7 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 3 Jun 2020 13:17:14 +0200 Subject: [PATCH 123/131] Add proper output of equidistant distortion model. --- realsense2_camera/src/base_realsense_node.cpp | 42 ++++++++++--------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 48634f8769..0cecda24c0 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -171,7 +171,7 @@ void BaseRealSenseNode::setupErrorCallback() { 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) + 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("Performing Hardware Reset."); @@ -334,14 +334,14 @@ void BaseRealSenseNode::set_sensor_auto_exposure_roi(rs2::sensor sensor) } } -void BaseRealSenseNode::readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, - const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, +void BaseRealSenseNode::readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, + const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, int* option_value) { nh1.param(option_name, *option_value, *option_value); //param (const std::string ¶m_name, T ¶m_val, const T &default_val) const if (*option_value < min_val) *option_value = min_val; if (*option_value > max_val) *option_value = max_val; - + ddynrec->registerVariable( option_name, *option_value, [this, sensor, option_name](int new_value){set_auto_exposure_roi(option_name, sensor, new_value);}, "auto-exposure " + option_name + " coordinate", min_val, max_val); @@ -1167,19 +1167,19 @@ void BaseRealSenseNode::FillImuData_LinearInterpolation(const CimuData imu_data, if ((type != ACCEL) || _imu_history.size() < 3) return; - + std::deque gyros_data; CimuData accel0, accel1, crnt_imu; - while (_imu_history.size()) + while (_imu_history.size()) { crnt_imu = _imu_history.front(); _imu_history.pop_front(); - if (!accel0.is_set() && crnt_imu.m_type == ACCEL) + if (!accel0.is_set() && crnt_imu.m_type == ACCEL) { accel0 = crnt_imu; - } - else if (accel0.is_set() && crnt_imu.m_type == ACCEL) + } + else if (accel0.is_set() && crnt_imu.m_type == ACCEL) { accel1 = crnt_imu; const double dt = accel1.m_time - accel0.m_time; @@ -1193,7 +1193,7 @@ void BaseRealSenseNode::FillImuData_LinearInterpolation(const CimuData imu_data, imu_msgs.push_back(CreateUnitedMessage(crnt_accel, crnt_gyro)); } accel0 = accel1; - } + } else if (accel0.is_set() && crnt_imu.m_time >= accel0.m_time && crnt_imu.m_type == GYRO) { gyros_data.push_back(crnt_imu); @@ -1385,7 +1385,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) tf::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w); tfv=tf::quatRotate(q,tfv); tf::vector3TFToMsg(tfv,v_msg.vector); - + geometry_msgs::Vector3Stamped om_msg; om_msg.vector.x = -pose.angular_velocity.z; om_msg.vector.y = -pose.angular_velocity.x; @@ -1393,7 +1393,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) tf::vector3MsgToTF(om_msg.vector,tfv); tfv=tf::quatRotate(q,tfv); tf::vector3TFToMsg(tfv,om_msg.vector); - + nav_msgs::Odometry odom_msg; _seq[stream_index] += 1; @@ -1425,7 +1425,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) void BaseRealSenseNode::frame_callback(rs2::frame frame) { _synced_imu_publisher->Pause(); - + try{ double frame_time = frame.get_timestamp(); @@ -1707,7 +1707,11 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v _camera_info[stream_index].P.at(10) = 1; _camera_info[stream_index].P.at(11) = 0; - _camera_info[stream_index].distortion_model = "plumb_bob"; + if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4) { + _camera_info[stream_index].distortion_model = "equidistant"; + } else { + _camera_info[stream_index].distortion_model = "plumb_bob"; + } // set R (rotation matrix) values to identity matrix _camera_info[stream_index].R.at(0) = 1.0; @@ -1957,8 +1961,8 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co if (use_texture) { 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) + + 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()) @@ -1998,7 +2002,7 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co _msg_pointcloud.is_dense = true; sensor_msgs::PointCloud2Modifier modifier(_msg_pointcloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.setPointCloud2FieldsByString(1, "xyz"); vertex = pc.get_vertices(); if (use_texture) @@ -2100,8 +2104,8 @@ rs2::stream_profile BaseRealSenseNode::getAProfile(const stream_index_pair& stre { const std::vector profiles = _sensors[stream].get_stream_profiles(); return *(std::find_if(profiles.begin(), profiles.end(), - [&stream] (const rs2::stream_profile& profile) { - return ((profile.stream_type() == stream.first) && (profile.stream_index() == stream.second)); + [&stream] (const rs2::stream_profile& profile) { + return ((profile.stream_type() == stream.first) && (profile.stream_index() == stream.second)); })); } From 602212a5370c11d6803765db5024c231b9641bba Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 3 Jun 2020 13:23:31 +0200 Subject: [PATCH 124/131] 4-space tabs --- realsense2_camera/src/base_realsense_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 0cecda24c0..e53e605ada 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1708,9 +1708,9 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v _camera_info[stream_index].P.at(11) = 0; if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4) { - _camera_info[stream_index].distortion_model = "equidistant"; + _camera_info[stream_index].distortion_model = "equidistant"; } else { - _camera_info[stream_index].distortion_model = "plumb_bob"; + _camera_info[stream_index].distortion_model = "plumb_bob"; } // set R (rotation matrix) values to identity matrix From 7c8922ddd7bc4430957bdb17ccea564cd3337f31 Mon Sep 17 00:00:00 2001 From: Helen Oleynikova Date: Wed, 3 Jun 2020 13:23:50 +0200 Subject: [PATCH 125/131] style --- realsense2_camera/src/base_realsense_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index e53e605ada..04ca464481 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1707,7 +1707,8 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v _camera_info[stream_index].P.at(10) = 1; _camera_info[stream_index].P.at(11) = 0; - if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4) { + if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4) + { _camera_info[stream_index].distortion_model = "equidistant"; } else { _camera_info[stream_index].distortion_model = "plumb_bob"; From 1cc944b757ded5f587ad3eeb7bf0d3240eaeca30 Mon Sep 17 00:00:00 2001 From: doronhi Date: Tue, 16 Jun 2020 12:41:53 +0300 Subject: [PATCH 126/131] Add new L515_PID Support L515 stream: infrared, 0 --- realsense2_camera/include/constants.h | 3 ++- .../include/realsense_node_factory.h | 3 ++- realsense2_camera/src/base_realsense_node.cpp | 23 ++++++++----------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index d6a561e638..e95384f715 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -34,7 +34,8 @@ namespace realsense2_camera const uint16_t RS435i_RGB_PID = 0x0B3A; // AWGC_MM const uint16_t RS405_PID = 0x0b0c; // DS5U const uint16_t RS_T265_PID = 0x0b37; // - const uint16_t RS_L515_PID = 0x0B3D; // + const uint16_t RS_L515_PID_PRE_PRQ = 0x0B3D; // + const uint16_t RS_L515_PID = 0x0B64; // const bool ALIGN_DEPTH = false; diff --git a/realsense2_camera/include/realsense_node_factory.h b/realsense2_camera/include/realsense_node_factory.h index 6b4110f923..9d757e459b 100644 --- a/realsense2_camera/include/realsense_node_factory.h +++ b/realsense2_camera/include/realsense_node_factory.h @@ -25,6 +25,7 @@ namespace realsense2_camera { const stream_index_pair COLOR{RS2_STREAM_COLOR, 0}; const stream_index_pair DEPTH{RS2_STREAM_DEPTH, 0}; + const stream_index_pair INFRA0{RS2_STREAM_INFRARED, 0}; const stream_index_pair INFRA1{RS2_STREAM_INFRARED, 1}; const stream_index_pair INFRA2{RS2_STREAM_INFRARED, 2}; const stream_index_pair FISHEYE{RS2_STREAM_FISHEYE, 0}; @@ -35,7 +36,7 @@ namespace realsense2_camera const stream_index_pair POSE{RS2_STREAM_POSE, 0}; - const std::vector IMAGE_STREAMS = {DEPTH, INFRA1, INFRA2, + const std::vector IMAGE_STREAMS = {DEPTH, INFRA0, INFRA1, INFRA2, COLOR, FISHEYE, FISHEYE1, FISHEYE2}; diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 6220c9e470..4342650e47 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -705,37 +705,34 @@ void BaseRealSenseNode::setupDevice() ROS_INFO_STREAM("Device Sensors: "); for(auto&& sensor : _dev_sensors) { + for (auto& profile : sensor.get_stream_profiles()) + { + auto video_profile = profile.as(); + stream_index_pair sip(video_profile.stream_type(), video_profile.stream_index()); + if (_sensors.find( sip ) != _sensors.end()) + continue; + _sensors[sip] = sensor; + } + std::string module_name = sensor.get_info(RS2_CAMERA_INFO_NAME); if (sensor.is()) { - _sensors[DEPTH] = sensor; - _sensors[INFRA1] = sensor; - _sensors[INFRA2] = sensor; _sensors_callback[module_name] = frame_callback_function; } else if (sensor.is()) { - _sensors[COLOR] = sensor; _sensors_callback[module_name] = frame_callback_function; } else if (sensor.is()) { - _sensors[FISHEYE] = sensor; _sensors_callback[module_name] = frame_callback_function; } else if (sensor.is()) { - _sensors[GYRO] = sensor; - _sensors[ACCEL] = sensor; _sensors_callback[module_name] = imu_callback_function; } else if (sensor.is()) { - _sensors[GYRO] = sensor; - _sensors[ACCEL] = sensor; - _sensors[POSE] = sensor; - _sensors[FISHEYE1] = sensor; - _sensors[FISHEYE2] = sensor; _sensors_callback[module_name] = multiple_message_callback_function; } else @@ -1808,7 +1805,7 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const stream_index_pair& s { if (!strcmp(e.what(), "Requested extrinsics are not available!")) { - ROS_WARN_STREAM(e.what() << " : using unity as default."); + ROS_WARN_STREAM("(" << rs2_stream_to_string(stream.first) << ", " << stream.second << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default."); ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}}); } else From 5d235b44f3be7812c29c047416e312c23f1b6b73 Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 17 Jun 2020 10:23:00 +0300 Subject: [PATCH 127/131] Setting enum dynamic parameters ignores options without description. --- realsense2_camera/src/base_realsense_node.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 6220c9e470..242e7b234f 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -242,14 +242,18 @@ bool is_checkbox(rs2::options sensor, rs2_option option) bool is_enum_option(rs2::options sensor, rs2_option option) { + static const int MAX_ENUM_OPTION_VALUES(100); + static const float EPSILON(0.05); + rs2::option_range op_range = sensor.get_option_range(option); - if (op_range.step < 0.001f) return false; + if (abs((op_range.step - 1)) > EPSILON || (op_range.max > MAX_ENUM_OPTION_VALUES)) 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; + continue; + return true; } - return true; + return false; } bool is_int_option(rs2::options sensor, rs2_option option) @@ -269,6 +273,8 @@ std::map get_enum_method(rs2::options sensor, rs2_option optio const auto op_range_step = int(op_range.step); for (auto val = op_range_min; val <= op_range_max; val += op_range_step) { + if (sensor.get_option_value_description(option, val) == nullptr) + continue; dict[sensor.get_option_value_description(option, val)] = val; } } From 4ecc0bbcc96a7f569499223d4800467665c77e9d Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 17 Jun 2020 14:14:59 +0300 Subject: [PATCH 128/131] restore redundant spaces for easy backward compatibility. --- realsense2_camera/src/base_realsense_node.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index c94d10be1c..39c1e7bfc2 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -183,7 +183,7 @@ void BaseRealSenseNode::setupErrorCallback() { 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) + 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("Performing Hardware Reset."); @@ -346,14 +346,14 @@ void BaseRealSenseNode::set_sensor_auto_exposure_roi(rs2::sensor sensor) } } -void BaseRealSenseNode::readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, - const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, +void BaseRealSenseNode::readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr ddynrec, + const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, int* option_value) { nh1.param(option_name, *option_value, *option_value); //param (const std::string ¶m_name, T ¶m_val, const T &default_val) const if (*option_value < min_val) *option_value = min_val; if (*option_value > max_val) *option_value = max_val; - + ddynrec->registerVariable( option_name, *option_value, [this, sensor, option_name](int new_value){set_auto_exposure_roi(option_name, sensor, new_value);}, "auto-exposure " + option_name + " coordinate", min_val, max_val); @@ -1179,19 +1179,19 @@ void BaseRealSenseNode::FillImuData_LinearInterpolation(const CimuData imu_data, if ((type != ACCEL) || _imu_history.size() < 3) return; - + std::deque gyros_data; CimuData accel0, accel1, crnt_imu; - while (_imu_history.size()) + while (_imu_history.size()) { crnt_imu = _imu_history.front(); _imu_history.pop_front(); - if (!accel0.is_set() && crnt_imu.m_type == ACCEL) + if (!accel0.is_set() && crnt_imu.m_type == ACCEL) { accel0 = crnt_imu; - } - else if (accel0.is_set() && crnt_imu.m_type == ACCEL) + } + else if (accel0.is_set() && crnt_imu.m_type == ACCEL) { accel1 = crnt_imu; const double dt = accel1.m_time - accel0.m_time; @@ -1205,7 +1205,7 @@ void BaseRealSenseNode::FillImuData_LinearInterpolation(const CimuData imu_data, imu_msgs.push_back(CreateUnitedMessage(crnt_accel, crnt_gyro)); } accel0 = accel1; - } + } else if (accel0.is_set() && crnt_imu.m_time >= accel0.m_time && crnt_imu.m_type == GYRO) { gyros_data.push_back(crnt_imu); @@ -1397,7 +1397,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) tf::Quaternion q(-msg.transform.rotation.x,-msg.transform.rotation.y,-msg.transform.rotation.z,msg.transform.rotation.w); tfv=tf::quatRotate(q,tfv); tf::vector3TFToMsg(tfv,v_msg.vector); - + geometry_msgs::Vector3Stamped om_msg; om_msg.vector.x = -pose.angular_velocity.z; om_msg.vector.y = -pose.angular_velocity.x; @@ -1405,7 +1405,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) tf::vector3MsgToTF(om_msg.vector,tfv); tfv=tf::quatRotate(q,tfv); tf::vector3TFToMsg(tfv,om_msg.vector); - + nav_msgs::Odometry odom_msg; _seq[stream_index] += 1; @@ -1437,7 +1437,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame) void BaseRealSenseNode::frame_callback(rs2::frame frame) { _synced_imu_publisher->Pause(); - + try{ double frame_time = frame.get_timestamp(); @@ -1974,8 +1974,8 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co if (use_texture) { 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) + + 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()) @@ -2015,7 +2015,7 @@ void BaseRealSenseNode::publishPointCloud(rs2::points pc, const ros::Time& t, co _msg_pointcloud.is_dense = true; sensor_msgs::PointCloud2Modifier modifier(_msg_pointcloud); - modifier.setPointCloud2FieldsByString(1, "xyz"); + modifier.setPointCloud2FieldsByString(1, "xyz"); vertex = pc.get_vertices(); if (use_texture) @@ -2117,8 +2117,8 @@ rs2::stream_profile BaseRealSenseNode::getAProfile(const stream_index_pair& stre { const std::vector profiles = _sensors[stream].get_stream_profiles(); return *(std::find_if(profiles.begin(), profiles.end(), - [&stream] (const rs2::stream_profile& profile) { - return ((profile.stream_type() == stream.first) && (profile.stream_index() == stream.second)); + [&stream] (const rs2::stream_profile& profile) { + return ((profile.stream_type() == stream.first) && (profile.stream_index() == stream.second)); })); } From 5c625fb6fd3792cfeda11d0e4ebd3b9305373f4c Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 18 Jun 2020 10:17:36 +0300 Subject: [PATCH 129/131] fixed README.md --- README.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 853bc5a4b6..6435a7542f 100644 --- a/README.md +++ b/README.md @@ -30,12 +30,9 @@ The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but Notice: * The version of librealsense2 is almost always behind the one availeable in RealSense™ official repository. * librealsense2 is not built to use native v4l2 driver but the less stable RS-USB protocol. That is because the last is more general and operational on a larger variety of platforms. + * realsense2_description is available as a separate debian package of ROS distribution. It includes the 3D-models of the devices and is necessary for running launch files that include these models (i.e. rs_d435_camera_with_model.launch). It can be installed by typing: + `sudo apt-get install ros-$ROS_VER-realsense2-description` - realsense2_description is available as a debian package of ROS distribution. It is necessary for running some of the RealSense™ d400 series. It can be installed by typing: - - `sudo apt-get install ros-kinetic-realsense2-description` - - This will install both realsense2_description and its dependents * ### Method 2: The RealSense™ distribution: From 7bbe006c02edc6583ca7fc6f8950d907b236e88b Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 18 Jun 2020 11:05:13 +0300 Subject: [PATCH 130/131] upgrade version to 2.2.14 --- README.md | 4 ++-- realsense2_camera/CHANGELOG.rst | 8 ++++++++ realsense2_camera/CMakeLists.txt | 2 +- realsense2_camera/include/constants.h | 2 +- realsense2_description/CHANGELOG.rst | 7 +++++++ 5 files changed, 19 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 6435a7542f..39b3d2a071 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ROS Wrapper for Intel® RealSense™ Devices These are packages for using Intel RealSense cameras (D400 series SR300 camera and T265 Tracking Module) with ROS. -LibRealSense supported version: v2.33.1 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) +LibRealSense supported version: v2.35.2 (see [realsense2_camera release notes](https://github.com/IntelRealSense/realsense-ros/releases)) ## Installation Instructions @@ -42,7 +42,7 @@ The following instructions are written for ROS Kinetic, on **Ubuntu 16.04** but - #### 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 and librealsense-dkms packages. #### OR - - #### Build from sources by downloading the latest [Intel® RealSense™ SDK 2.0](https://github.com/IntelRealSense/librealsense/releases/tag/v2.33.1) 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.35.2) and follow the instructions under [Linux Installation](https://github.com/IntelRealSense/librealsense/blob/master/doc/installation.md) ### Step 2: Install Intel® RealSense™ ROS from Sources diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index d445c8a175..d462744b88 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -4,6 +4,14 @@ Changelog for package realsense2_camera Forthcoming ----------- +* Fix compatibility with Librealsense2 Version 2.35.2. +* Fix support for L515. +* Fix urdf issues. +* Add noetic support: change state_publisher into robot_state_publisher +* fix distortion correction model for T265 (equidistant) +* fix stability issues. Stop sensors at program termination. +* Contributors: Brice, Helen Oleynikova, doronhi + * upgrade version to 2.2.13 * fix ctrl-C closing issues. * handle device creation exceptions. diff --git a/realsense2_camera/CMakeLists.txt b/realsense2_camera/CMakeLists.txt index 5327d5f1fd..14e6a6e5d3 100644 --- a/realsense2_camera/CMakeLists.txt +++ b/realsense2_camera/CMakeLists.txt @@ -34,7 +34,7 @@ if(SET_USER_BREAK_AT_STARTUP) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DBPDEBUG") endif() -find_package(realsense2 2.32.1) +find_package(realsense2 2.35.2) 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() diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index e95384f715..0f86e987f1 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -7,7 +7,7 @@ #define REALSENSE_ROS_MAJOR_VERSION 2 #define REALSENSE_ROS_MINOR_VERSION 2 -#define REALSENSE_ROS_PATCH_VERSION 13 +#define REALSENSE_ROS_PATCH_VERSION 14 #define STRINGIFY(arg) #arg #define VAR_ARG_STRING(arg) STRINGIFY(arg) diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 85fd1505e4..2997b39828 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -4,4 +4,11 @@ Changelog for package realsense2_description Forthcoming ----------- +* fix urdf issues (arg use_nominal_extrinsics). +* Add noetic support: + - urdf files. + - change state_publisher into robot_state_publisher +* correct offset between camera_link and base_link +* Contributors: Brice, Marco Camurri, doronhi + * upgrade version to 2.2.13 From bc41dcbe68c7ecb56d9e5e3f10a82db0d9575cba Mon Sep 17 00:00:00 2001 From: doronhi Date: Thu, 18 Jun 2020 11:06:53 +0300 Subject: [PATCH 131/131] 2.2.14 --- realsense2_camera/CHANGELOG.rst | 4 ++-- realsense2_camera/package.xml | 2 +- realsense2_description/CHANGELOG.rst | 4 ++-- realsense2_description/package.xml | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/realsense2_camera/CHANGELOG.rst b/realsense2_camera/CHANGELOG.rst index d462744b88..ed027199a2 100644 --- a/realsense2_camera/CHANGELOG.rst +++ b/realsense2_camera/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_camera ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.14 (2020-06-18) +------------------- * Fix compatibility with Librealsense2 Version 2.35.2. * Fix support for L515. * Fix urdf issues. diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index bac96d5709..b6924b4726 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -1,7 +1,7 @@ realsense2_camera - 2.2.13 + 2.2.14 RealSense Camera package allowing access to Intel T265 Tracking module and SR300 and D400 3D cameras Sergey Dorodnicov Doron Hirshberg diff --git a/realsense2_description/CHANGELOG.rst b/realsense2_description/CHANGELOG.rst index 2997b39828..c75f0a958c 100644 --- a/realsense2_description/CHANGELOG.rst +++ b/realsense2_description/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package realsense2_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +2.2.14 (2020-06-18) +------------------- * fix urdf issues (arg use_nominal_extrinsics). * Add noetic support: - urdf files. diff --git a/realsense2_description/package.xml b/realsense2_description/package.xml index 8aa10d4e16..4653763df0 100644 --- a/realsense2_description/package.xml +++ b/realsense2_description/package.xml @@ -1,7 +1,7 @@ realsense2_description - 2.2.13 + 2.2.14 RealSense Camera description package for Intel 3D D400 cameras Sergey Dorodnicov Doron Hirshberg