Point cloud integration, removed multiple .launch

CMakeLists.txt

@@ -32,6 +32,7 @@ ENDIF(WIN32)
 
 find_package(CUDA 6.5 REQUIRED)
 find_package(OpenCV 2.4 COMPONENTS core highgui imgproc REQUIRED)
+find_package(PCL)
 
 find_package(catkin REQUIRED COMPONENTS
   image_transport
@@ -64,13 +65,15 @@ catkin_package(
 include_directories(
 	${catkin_INCLUDE_DIRS} 
 	${CUDA_INCLUDE_DIRS}
-    ${ZED_INCLUDE_DIRS}
-    ${OpenCV_INCLUDE_DIRS}
+	${ZED_INCLUDE_DIRS}
+	${OpenCV_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
 )
 
 link_directories(${ZED_LIBRARY_DIR})
 link_directories(${CUDA_LIBRARY_DIRS})
 link_directories(${OpenCV_LIBRARY_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
 
 ###############################################################################
 
@@ -90,7 +93,8 @@ target_link_libraries(
         ${catkin_LIBRARIES}
         ${ZED_LIBRARIES}
 	${CUDA_LIBRARIES} ${CUDA_nppi_LIBRARY} ${CUDA_npps_LIBRARY}
-        ${OpenCV_LIBS}  
+        ${OpenCV_LIBS}
+        ${PCL_LIBRARIES}  
     )
 
 add_dependencies(zed_wrapper_node ${PROJECT_NAME}_gencfg)

README.md

@@ -3,15 +3,20 @@ Ros wrapper for the ZED Stereo Camera SDK
 
 **This sample is designed to work with the ZED stereo camera only and requires the ZED SDK. For more information: https://www.stereolabs.com**
 
-This sample is a wrapper for the ZED library in order to use the ZED Camera with ROS.
-You can publish Left+Depth or Left+Right images and camera info on the topics of your choice.
-
-A set of parameters can be specified in the launch file. Two launch files are provided in the launch directory:
-
-       - zed_depth.launch: publish left and depth images with their camera info.
-       - zed_stereo.launch: publish left and right images with their camera info.
-
-The zed_depth_stereo_wrapper is a catkin package made to run on ROS Indigo, and depends
+This sample is a wrapper for the ZED library in order to use the ZED Camera with ROS. It can provide the camera images, the depth map, and a 3D point cloud
+Published topics:
+    - /camera/point_cloud/cloud
+    - /camera/depth/camera_info
+    - /camera/depth/image_rect_color
+    - /camera/left/camera_info
+    - /camera/left/image_rect_color
+    - /camera/rgb/camera_info
+    - /camera/rgb/image_rect_color
+
+A set of parameters can be specified in the launch file provided in the launch directory.
+    - zed.launch
+
+The zed_ros_wrapper is a catkin package made to run on ROS Indigo, and depends
 on the following ROS packages:
    - roscpp
    - rosconsole
@@ -33,31 +38,58 @@ Open a terminal :
 
 ## Run the program
 
-   Open a terminal :
+   Open a terminal to launch the wrapper:
+
+   	$ roslaunch zed_wrapper zed.launch
+
+   Open an other terminal to display images:
 
-   	$ roslaunch zed_wrapper zed_depth.launch
+   	$ rosrun image_view image_view image:=/camera/rgb/image_rect_color
 
-**WARNING : to get the depth in meters (it's a requirement for ROS), the baseline has to be set manually to 0.12 using ZED Settings App**
+   If you want to see the point cloud, launch rviz, select zed_optical_frame in Displays->Global Options->Fixed Frame->zed_optical_frame.
+   Then click on 'add' (bottom left), select the 'By Topic' tab, select point_cloud->cloud->PointCloud2 and click 'OK'.
+
+   	$ rosrun rviz rviz
+
+   Note that rviz isn't very good at displaying a camera feed and a point cloud at the same time. You should use an other instance of rviz or the `rosrun` command.
 
 ## Launch file parameters
 
-Parameter       |           Description           |              Value               
-------------------------|---------------------------------|---------------------------------
- computeDepth          | Toggle depth computation.       | '0': depth not computed, Left+Right images published 
-                       |                                 | '1': depth computed, Left+Depth images published 
- svo_file              | SVO filename                    | path to an SVO file              
- resolution            | ZED Camera resolution           | '0': HD2K                        
-                       |                                 | '1': HD1080                      
-                       |                                 | '2': HD720                       
-                       |                                 | '3': VGA                         
- quality               | Disparity Map quality           | '1': PERFORMANCE                 
-                      |                                 | '2': QUALITY                     
- sensing_mode          | Depth sensing mode              | '0': FULL                        
-                       |                                 | '1': RAW                         
-frame_rate            | Rate at which images are published  | int                              
- left_topic            | Topic to which left images are published | string                           
- second_topic          | Topic to which depth or right images are published   | string                           
- left_cam_info_topic   | Topic to which left camera info are published | string                           
- second_cam_info_topic | Topic to which right or depth camera info are published  | string                           
- left_frame_id         | ID specified in the left image message header | string                           
- second_frame_id       | ID specified in the depth or right image message header   | string                           
+ Parameter              |           Description           |              Value                
+------------------------|---------------------------------|---------------------------------  
+ svo_file               | SVO filename                    | path to an SVO file               
+ resolution             | ZED Camera resolution           | '0': HD2K                         
+ _                      | _                               | '1': HD1080                       
+ _                      | _                               | '2': HD720                        
+ _                      | _                               | '3': VGA                          
+ quality                | Disparity Map quality           | '1': PERFORMANCE                  
+ _                      | _                               | '2': QUALITY                      
+ sensing_mode           | Depth sensing mode              | '0': FULL                         
+ _                      | _                               | '1': RAW                          
+ frame_rate             | Rate at which images are published                          | int   
+ rgb_topic              | Topic to which rgb==default==left images are published      | string
+ rgb_cam_info_topic     | Topic to which rgb==default==left camera info are published | string
+ rgb_frame_id           | ID specified in the rgb==default==left image message header | string
+ left_topic             | Topic to which left images are published                    | string
+ left_cam_info_topic    | Topic to which left camera info are published               | string
+ left_frame_id          | ID specified in the left image message header               | string
+ right_topic            | Topic to which right images are published                   | string
+ right_cam_info_topic   | Topic to which right camera info are published              | string
+ right_frame_id         | ID specified in the right image message header              | string
+ depth_topic            | Topic to which depth map images are published               | string
+ depth_cam_info_topic   | Topic to which depth camera info are published              | string
+ depth_frame_id         | ID specified in the depth image message header              | string
+ point_cloud_topic      | Topic to which point clouds are published                   | string
+ cloud_frame_id         | ID specified in the point cloud message header              | string
+
+
+
+
+
+
+
+
+
+
+
+

zed.launch

@@ -0,0 +1,36 @@
+<launch>
+
+  <include file="$(find zed_wrapper)/launch/zed_tf.launch" />
+
+  <arg name="svo_file" default=""/>
+
+  <group ns="camera">
+    <node name="zed_wrapper_node" pkg="zed_wrapper" type="zed_wrapper_node" args="$(arg svo_file)" output="screen">
+
+      <param name="resolution"            value="2" />
+      <param name="quality"               value="2" />
+      <param name="sensing_mode"          value="1" />
+      <param name="frame_rate"            value="30" />
+
+      <param name="rgb_topic"            value="rgb/image_rect_color" />
+      <param name="rgb_cam_info_topic"   value="rgb/camera_info" />
+      <param name="rgb_frame_id"         value="/zed_optical_frame" />
+
+      <param name="left_topic"            value="left/image_rect_color" />
+      <param name="left_cam_info_topic"   value="left/camera_info" />
+      <param name="left_frame_id"         value="/zed_optical_frame" />
+
+      <param name="right_topic"            value="right/image_rect_color" />
+      <param name="right_cam_info_topic"   value="right/camera_info" />
+      <param name="right_frame_id"         value="/zed_optical_frame" />
+
+      <param name="depth_topic"            value="depth/image_rect_color" />
+      <param name="depth_cam_info_topic"   value="depth/camera_info" />
+      <param name="depth_frame_id"         value="/zed_optical_frame" />
+
+      <param name="point_cloud_topic"     value="point_cloud/cloud" />
+      <param name="cloud_frame_id"        value="/zed_optical_frame" />
+
+    </node>
+  </group>
+</launch>

zed_tf.launch

@@ -0,0 +1,13 @@
+<launch>
+  <arg name="camera" default="zed"/>
+  <arg name="tf_prefix" default="" />
+  <arg name="pi/2" value="1.5707963267948966" />
+  <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
+
+  <node pkg="tf" type="static_transform_publisher" name="$(arg camera)_base_link1"
+    args="0 0 0 0 0 0 1 camera_link $(arg tf_prefix)/$(arg camera)_frame 100" />
+
+  <node pkg="tf" type="static_transform_publisher" name="$(arg camera)_base_link2"
+    args="$(arg optical_rotate) $(arg tf_prefix)/$(arg camera)_frame $(arg tf_prefix)/$(arg camera)_optical_frame  100" />
+
+</launch>