v2.5 merge (stereolabs#236)

* Updated wrapper compatibility with ZED SDK 2.5. * Updated "package.xml" to "Format 2" * Refactored wrapper code for clarity * Updated publishing rules to stream Odometry when subscribing to point cloud topics * Added launch example to run wrapper nodelet in a standalone nodelet manager * Added a publisher for internal IMU data * Added a publisher for disparity image * Added parameters for Pose Smoothing and Spatial Memory. * Added initial tracking position parameter * Added dynamic image/measures resolution setting to allow compute and bandwidth optimizations * Fixed minor bugs
zzx9636 · Jul 17, 2018 · 7d75cb4 · 7d75cb4
1 parent 3fe7f74
commit 7d75cb4
Show file tree

Hide file tree

Showing 26 changed files with 2,854 additions and 1,246 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -38,27 +38,47 @@ checkPackage("CUDA" "CUDA not found, install it from:\n https://developer.nvidia
 find_package(PCL)
 checkPackage("PCL" "PCL not found, try to install it with:\n sudo apt-get install libpcl1 ros-$(rosversion -d)-pcl-ros")
 
+find_package(OpenMP)
+checkPackage("OpenMP" "OpenMP not found, please install it to improve performances: 'sudo apt install libomp-dev'")
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
 find_package(catkin COMPONENTS
   image_transport
   roscpp
   rosconsole
   sensor_msgs
+  stereo_msgs
   dynamic_reconfigure
   tf2_ros
   pcl_conversions
   nodelet
   tf2_geometry_msgs
+  message_generation
 )
 
 checkPackage("image_transport" "")
 checkPackage("roscpp" "")
 checkPackage("rosconsole" "")
 checkPackage("sensor_msgs" "")
+checkPackage("stereo_msgs" "")
 checkPackage("dynamic_reconfigure" "")
 checkPackage("tf2_ros" "")
 checkPackage("pcl_conversions" "")
 checkPackage("nodelet" "")
 checkPackage("tf2_geometry_msgs" "")
+checkPackage("message_generation" "")
+
+add_service_files( FILES
+    set_initial_pose.srv
+    reset_odometry.srv
+    reset_tracking.srv
+  )
+
+generate_messages()
 
 generate_dynamic_reconfigure_options(
   cfg/Zed.cfg
@@ -69,23 +89,37 @@ catkin_package(
     roscpp
     rosconsole
     sensor_msgs
+    stereo_msgs
     opencv3
     image_transport
     dynamic_reconfigure
     tf2_ros
     tf2_geometry_msgs
-    pcl_conversions
+    pcl_conversions    
+    message_runtime
 )
+
+###############################################################################
+# SOURCES
+
+set(TOOLS_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/tools/src/sl_tools.cpp)
+set(NODE_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/zed_wrapper_node.cpp)
+set(NODELET_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/nodelet/src/zed_wrapper_nodelet.cpp)
+
+###############################################################################
+
 ###############################################################################
 # INCLUDES
 
 # Specify locations of header files.
 include_directories(
-	${catkin_INCLUDE_DIRS}
-	${CUDA_INCLUDE_DIRS}
-	${ZED_INCLUDE_DIRS}
-	${OpenCV_INCLUDE_DIRS}
-	${PCL_INCLUDE_DIRS}
+        ${catkin_INCLUDE_DIRS}
+        ${CUDA_INCLUDE_DIRS}
+        ${ZED_INCLUDE_DIRS}
+        ${OpenCV_INCLUDE_DIRS}
+        ${PCL_INCLUDE_DIRS}
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/tools/include
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/nodelet/include
 )
 
 link_directories(${ZED_LIBRARY_DIR})
@@ -98,7 +132,6 @@ link_directories(${PCL_LIBRARY_DIRS})
 ###############################################################################
 # EXECUTABLE
 
-
 if(NOT DEFINED CUDA_NPP_LIBRARIES_ZED)
     #To deal with cuda 9 nppi libs and previous versions of ZED SDK
     set(CUDA_NPP_LIBRARIES_ZED ${CUDA_npp_LIBRARY} ${CUDA_npps_LIBRARY} ${CUDA_nppi_LIBRARY})   
@@ -114,11 +147,11 @@ set(LINK_LIBRARIES
   ${OpenCV_LIBS}
   ${PCL_LIBRARIES})
 
-add_library(ZEDWrapper src/zed_wrapper_nodelet.cpp)
+add_library(ZEDWrapper ${TOOLS_SRC} ${NODELET_SRC})
 target_link_libraries(ZEDWrapper ${LINK_LIBRARIES})
 add_dependencies(ZEDWrapper ${PROJECT_NAME}_gencfg)
 
-add_executable(zed_wrapper_node src/zed_wrapper_node.cpp)
+add_executable(zed_wrapper_node ${NODE_SRC})
 target_link_libraries(zed_wrapper_node ZEDWrapper ${LINK_LIBRARIES})
 add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)
 

diff --git a/LICENSE b/LICENSE
@@ -1,4 +1,4 @@
-Copyright (c) 2017, Stereolabs
+Copyright (c) 2018, Stereolabs
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

diff --git a/README.md b/README.md
@@ -1,6 +1,6 @@
 # Stereolabs ZED Camera - ROS Integration
 
-This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, odometry information and supports the use of multiple ZED cameras.
+This package lets you use the ZED stereo camera with ROS. It outputs the camera left and right images, depth map, point cloud, pose information and supports the use of multiple ZED cameras.
 
 ## Getting started
 
@@ -25,7 +25,8 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package
    - roscpp
    - rosconsole
    - sensor_msgs
-   - opencv
+   - stereo_msgs
+   - opencv3
    - image_transport
    - dynamic_reconfigure
    - urdf

diff --git a/cfg/Zed.cfg b/cfg/Zed.cfg
@@ -9,5 +9,6 @@ gen.add("confidence", int_t, 0, "Confidence threshold, the lower the better", 80
 gen.add("exposure", int_t, 1, "Exposure value when manual controlled", 100, 0, 100);
 gen.add("gain", int_t, 2, "Gain value when manual controlled", 50, 0, 100);
 gen.add("auto_exposure", bool_t, 3, "Enable/Disable auto control of exposure and gain", True);
+gen.add("mat_resize_factor", double_t, 4, "Image/Measures resize factor", 1.0, 0.1, 1.0);
 
 exit(gen.generate(PACKAGE, "zed_wrapper", "Zed"))
diff --git a/launch/display.launch b/launch/display.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <!--
-Copyright (c) 2017, STEREOLABS.
+Copyright (c) 2018, STEREOLABS.
 
 All rights reserved.
 

diff --git a/launch/display_zedm.launch b/launch/display_zedm.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <!--
-Copyright (c) 2017, STEREOLABS.
+Copyright (c) 2018, STEREOLABS.
 
 All rights reserved.
 

diff --git a/launch/zed.launch b/launch/zed.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <!--
-Copyright (c) 2017, STEREOLABS.
+Copyright (c) 2018, STEREOLABS.
 
 All rights reserved.
 
@@ -17,18 +17,32 @@ THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 -->
 <launch>
-    <!-- Odometry coordinate frame -->
-    <arg name="odometry_frame"       default="map" />
-    <arg name="camera_model"          default="0" /> <!-- 0=ZED, 1=ZEDM-->
-    <arg name="serial_number"           default="0" /> 
+    <arg name="svo_file"              default="" /> <!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
+
+    <arg name="camera_model"         default="1" /> <!-- 0=ZED, 1=ZEDM-->
+    <arg name="serial_number"        default="0" />
+    <arg name="verbose"              default="true" />
+    <arg name="resolution"           default="2" /> <!--0=RESOLUTION_HD2K, 1=RESOLUTION_HD1080, 2=RESOLUTION_HD720, 3=RESOLUTION_VGA -->
+    <arg name="frame_rate"           default="30" />
+    <!-- RESOLUTION_HD2K   -> 2208*1242, available framerates: 15 fps.
+         RESOLUTION_HD1080 -> 1920*1080, available framerates: 15, 30 fps.
+         RESOLUTION_HD720  -> 1280*720,  available framerates: 15, 30, 60 fps.
+         RESOLUTION_VGA    -> 672*376,   available framerates: 15, 30, 60, 100 fps. -->
+
+    <!-- Coordinate frame -->
+    <arg name="pose_frame"           default="map" />
+    <arg name="odometry_frame"       default="odom" />
 
     <group ns="zed">
         <include file="$(find zed_wrapper)/launch/zed_camera.launch">
-            <!-- compliant mode for rviz -->
-            <arg name="odometry_frame"        value="$(arg odometry_frame)" />
+            <arg name="svo_file"            value="$(arg svo_file)" />
+            <arg name="pose_frame"          value="$(arg pose_frame)" />
+            <arg name="odometry_frame"      value="$(arg odometry_frame)" />
             <arg name="camera_model"        value="$(arg camera_model)" />
-            <arg name="serial_number"           default="$(arg serial_number)" /> 
+            <arg name="serial_number"       default="$(arg serial_number)" /> 
+            <arg name="resolution"          default="$(arg resolution)" />
+            <arg name="frame_rate"          default="$(arg frame_rate)" />
+            <arg name="verbose"             default="$(arg verbose)" />
         </include>
     </group>
-
 </launch>
diff --git a/launch/zed_camera.launch b/launch/zed_camera.launch
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <!--
-Copyright (c) 2017, STEREOLABS.
+Copyright (c) 2018, STEREOLABS.
 
 All rights reserved.
 
@@ -21,41 +21,68 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     <arg name="svo_file"              default="" /><!-- <arg name="svo_file" default="path/to/svo/file.svo"> -->
     <arg name="zed_id"                default="0" />
     <!-- If a ZED SN is given, zed_id is ignored and the wrapper will specifically look for the ZED with the corresponding serial number -->
-    <arg name="serial_number"                default="0" /> 
+    <arg name="serial_number"         default="0" />     
+    <arg name="verbose"               default="true" />
+
+    <arg name="resolution"            default="2" /> <!--0=RESOLUTION_HD2K, 1=RESOLUTION_HD1080, 2=RESOLUTION_HD720, 3=RESOLUTION_VGA -->
+    <arg name="frame_rate"            default="30" />
+    <!-- RESOLUTION_HD2K   -> 2208*1242, available framerates: 15 fps.
+         RESOLUTION_HD1080 -> 1920*1080, available framerates: 15, 30 fps.
+         RESOLUTION_HD720  -> 1280*720,  available framerates: 15, 30, 60 fps.
+         RESOLUTION_VGA    -> 672*376,   available framerates: 15, 30, 60, 100 fps. -->
 
     <!-- GPU ID-->
     <arg name="gpu_id"                default="-1" />
+
     <!-- Definition coordinate frames -->
-    <arg name="publish_tf"            default="true" />
-    <arg name="odometry_frame"        default="odom" />
-    <arg name="base_frame"            default="zed_center" />
-    <arg name="camera_frame"          default="zed_left_camera" />
-    <arg name="depth_frame"           default="zed_depth_camera" />
+    <arg name="publish_tf"                    default="true" />
+    <arg name="pose_frame"                    default="map" />
+    <arg name="odometry_frame"                default="odom" />
+    <arg name="base_frame"                    default="zed_camera_center" />
+    <arg name="left_camera_frame"             default="zed_left_camera_frame" />
+    <arg name="left_camera_optical_frame"     default="zed_left_camera_optical_frame" />
+    <arg name="right_camera_frame"            default="zed_right_camera_frame" />
+    <arg name="right_camera_optical_frame"    default="zed_right_camera_optical_frame" />
+    <arg name="imu_frame"                     default="imu_link" /> <!-- only used with ZED M -->
+
+    <arg name="initial_pose"                  default="[0.0,0.0,0.0, 0.0,0.0,0.0]" /> <!-- X, Y, Z, (meter) R, P, Y, (radians)-->
+
     <!-- Publish urdf zed -->
     <arg name="publish_urdf"          default="true" />
+
+    <arg name="rgb_topic"           default="rgb/image_rect_color" />
+    <arg name="rgb_info_topic"      default="rgb/camera_info" />
+    <arg name="depth_topic"         default="depth/depth_registered" />
 
-    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" --> 
+    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" output="screen" required="true"><!-- launch-prefix="valgrind" -->
+
+        <param name="camera_model"          value="$(arg camera_model)" />
 
-        <param name="camera_model" value="$(arg camera_model)" />
-        <!-- publish odometry frame -->
+        <!-- publish pose frame -->
         <param name="publish_tf"            value="$(arg publish_tf)" />
+
         <!-- Configuration frame camera -->    
-        <param name="odometry_frame"        value="$(arg odometry_frame)" />
-        <param name="base_frame"            value="$(arg base_frame)" />
-        <param name="camera_frame"          value="$(arg camera_frame)" />
-        <param name="depth_frame"           value="$(arg depth_frame)" />
+        <param name="pose_frame"                    value="$(arg pose_frame)" />
+        <param name="odometry_frame"                value="$(arg odometry_frame)" />
+        <param name="base_frame"                    value="$(arg base_frame)" />
+        <param name="left_camera_frame"             value="$(arg left_camera_frame)" />
+        <param name="left_camera_optical_frame"     value="$(arg left_camera_optical_frame)" />
+        <param name="right_camera_frame"            value="$(arg right_camera_frame)" />
+        <param name="right_camera_optical_frame"    value="$(arg right_camera_optical_frame)" />
 
         <!-- SVO file path -->
         <param name="svo_filepath"          value="$(arg svo_file)" />
 
         <!-- ZED parameters -->
         <param name="zed_id"                value="$(arg zed_id)" />
-        <param name="serial_number"      value="$(arg serial_number)" />
+        <param name="serial_number"         value="$(arg serial_number)" />
 
-        <param name="resolution"            value="2" />
+        <param name="resolution"            value="$(arg resolution)" />
+        <param name="verbose"               value="$(arg verbose)" />
+        <param name="mat_resize_factor"     value="1.0" />
         <param name="quality"               value="1" />
         <param name="sensing_mode"          value="0" />
-        <param name="frame_rate"            value="60" />
+        <param name="frame_rate"            value="$(arg frame_rate)" />
         <param name="odometry_db"           value="" />
         <param name="openni_depth_mode"     value="0" />
         <param name="gpu_id"                value="$(arg gpu_id)" />
@@ -64,29 +91,45 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
         <param name="exposure"              value="100" />
         <param name="auto_exposure"         value="true" />
         <param name="depth_stabilization"   value="1" />
+        <param name="pose_smoothing"        value="false" />
+        <param name="spatial_memory"        value="true" />
+
+        <rosparam param="initial_tracking_pose" subst_value="True">$(arg initial_pose)</rosparam>
 
         <!-- ROS topic names -->
-        <param name="rgb_topic"             value="rgb/image_rect_color" />
-        <param name="rgb_raw_topic"         value="rgb/image_raw_color" />
-        <param name="rgb_cam_info_topic"    value="rgb/camera_info" />
+        <param name="rgb_topic"                 value="$(arg rgb_topic)" />
+        <param name="rgb_raw_topic"             value="rgb/image_raw_color" />
+        <param name="rgb_cam_info_topic"        value="$(arg rgb_info_topic)" />
         <param name="rgb_cam_info_raw_topic"    value="rgb/camera_info_raw" />
 
-        <param name="left_topic"            value="left/image_rect_color" />
-        <param name="left_raw_topic"        value="left/image_raw_color" />
-        <param name="left_cam_info_topic"   value="left/camera_info" />
+        <param name="left_topic"                value="left/image_rect_color" />
+        <param name="left_raw_topic"            value="left/image_raw_color" />
+        <param name="left_cam_info_topic"       value="left/camera_info" />
         <param name="left_cam_info_raw_topic"   value="left/camera_info_raw" />
 
-        <param name="right_topic"           value="right/image_rect_color" />
-        <param name="right_raw_topic"       value="right/image_raw_color" />
-        <param name="right_cam_info_topic"  value="right/camera_info" />
+        <param name="right_topic"               value="right/image_rect_color" />
+        <param name="right_raw_topic"           value="right/image_raw_color" />
+        <param name="right_cam_info_topic"      value="right/camera_info" />
         <param name="right_cam_info_raw_topic"  value="right/camera_info_raw" />
 
-        <param name="depth_topic"           value="depth/depth_registered" />
-        <param name="depth_cam_info_topic"  value="depth/camera_info" />
+        <param name="depth_topic"               value="$(arg depth_topic)" />
+        <param name="depth_cam_info_topic"      value="depth/camera_info" />
+
+        <param name="point_cloud_topic"         value="point_cloud/cloud_registered" />        
+
+        <param name="disparity_topic"           value="disparity/disparity_image" />
+        <param name="confidence_img_topic"      value="confidence/confidence_image" />
+        <param name="confidence_map_topic"      value="confidence/confidence_map" />
+
+        <param name="pose_topic"                value="map" />
+        <param name="odometry_topic"            value="odom" />
+
+        <param name="init_odom_with_imu"        value="true" /> <!-- only used with ZED mini -->
 
-        <param name="point_cloud_topic"     value="point_cloud/cloud_registered" />
+        <param name="imu_topic"                 value="imu/data" /> <!-- only used with ZED M -->
+        <param name="imu_topic_raw"             value="imu/data_raw" /> <!-- only used with ZED M -->
 
-        <param name="odometry_topic"        value="odom" />
+        <param name="imu_pub_rate"              value="500.0" /> <!-- only used with ZED M -->
 
     </node>