Merged PR #189 : Add publisher for disparity

CMakeLists.txt

@@ -43,6 +43,7 @@ find_package(catkin COMPONENTS
   roscpp
   rosconsole
   sensor_msgs
+  stereo_msgs
   dynamic_reconfigure
   tf2_ros
   pcl_conversions
@@ -54,6 +55,7 @@ checkPackage("image_transport" "")
 checkPackage("roscpp" "")
 checkPackage("rosconsole" "")
 checkPackage("sensor_msgs" "")
+checkPackage("stereo_msgs" "")
 checkPackage("dynamic_reconfigure" "")
 checkPackage("tf2_ros" "")
 checkPackage("pcl_conversions" "")
@@ -69,6 +71,7 @@ catkin_package(
     roscpp
     rosconsole
     sensor_msgs
+    stereo_msgs
     opencv3
     image_transport
     dynamic_reconfigure

README.md

@@ -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

launch/zed_camera.launch

@@ -32,6 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     <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="disparity_frame"       default="zed_depth_camera" />
     <arg name="imu_frame"             default="imu_link" />
 
     <!-- Publish urdf zed -->
@@ -49,6 +50,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
         <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="disparity_frame"       value="$(arg disparity_frame)" />
 
         <!-- SVO file path -->
         <param name="svo_filepath"          value="$(arg svo_file)" />

package.xml

@@ -16,6 +16,7 @@
   <depend>roscpp</depend>
   <depend>rosconsole</depend>
   <depend>sensor_msgs</depend>
+  <depend>stereo_msgs</depend>
   <depend>opencv3</depend>
   <depend>image_transport</depend>
   <depend>dynamic_reconfigure</depend>

src/nodelet/include/zed_wrapper_nodelet.h

@@ -40,6 +40,7 @@
 #include <sensor_msgs/image_encodings.h>
 #include <sensor_msgs/Imu.h>
 #include <image_transport/image_transport.h>
+#include <stereo_msgs/DisparityImage.h>
 #include <dynamic_reconfigure/server.h>
 #include <zed_wrapper/ZedConfig.h>
 #include <nav_msgs/Odometry.h>
@@ -138,6 +139,12 @@ namespace zed_wrapper {
          */
         void publishCamInfo(sensor_msgs::CameraInfoPtr cam_info_msg, ros::Publisher pub_cam_info, ros::Time t);
 
+        /* \brief Publish a cv::Mat disparity image with a ros Publisher
+         * \param disparity : the disparity image to publish
+         * \param t : the ros::Time to stamp the depth image
+         */
+        void publishDisparity(cv::Mat disparity, ros::Time t);
+
         /* \brief Get the information of the ZED cameras and store them in an information message
          * \param zed : the sl::zed::Camera* pointer to an instance
          * \param left_cam_info_msg : the information message to fill with the left camera informations
@@ -168,6 +175,7 @@ namespace zed_wrapper {
         image_transport::Publisher pub_right;
         image_transport::Publisher pub_raw_right;
         image_transport::Publisher pub_depth;
+        ros::Publisher pub_disparity; // TODO use image_transport?
         ros::Publisher pub_cloud;
         ros::Publisher pub_rgb_cam_info;
         ros::Publisher pub_left_cam_info;
@@ -186,6 +194,7 @@ namespace zed_wrapper {
         std::string right_frame_id;
         std::string rgb_frame_id;
         std::string depth_frame_id;
+        std::string disparity_frame_id;
         std::string cloud_frame_id;
         std::string odometry_frame_id;
         std::string base_frame_id;

src/nodelet/src/zed_wrapper_nodelet.cpp

@@ -75,6 +75,7 @@ namespace zed_wrapper {
         nh_ns.param<std::string>("base_frame", base_frame_id, "base_frame");
         nh_ns.param<std::string>("camera_frame", camera_frame_id, "camera_frame");
         nh_ns.param<std::string>("depth_frame", depth_frame_id, "depth_frame");
+        nh_ns.param<std::string>("disparity_frame", disparity_frame_id, "dispairty_frame");
         nh_ns.param<std::string>("imu_frame", imu_frame_id, "imu_link");
 
         // Get parameters from launch file
@@ -138,6 +139,8 @@ namespace zed_wrapper {
 
         string depth_cam_info_topic = "depth/camera_info";
 
+        string disparity_topic = "disparity/disparity_image";
+
         string point_cloud_topic = "point_cloud/cloud_registered";
         cloud_frame_id = camera_frame_id;
 
@@ -162,6 +165,8 @@ namespace zed_wrapper {
         nh_ns.getParam("depth_topic", depth_topic);
         nh_ns.getParam("depth_cam_info_topic", depth_cam_info_topic);
 
+        nh_ns.getParam("disparity_topic", disparity_topic);
+
         nh_ns.getParam("point_cloud_topic", point_cloud_topic);
 
         nh_ns.getParam("odometry_topic", odometry_topic);
@@ -267,6 +272,10 @@ namespace zed_wrapper {
         pub_depth = it_zed.advertise(depth_topic, 1); //depth
         NODELET_INFO_STREAM("Advertized on topic " << depth_topic);
 
+        // Disparity publisher
+        pub_disparity = nh.advertise<stereo_msgs::DisparityImage>(disparity_topic, 1);
+        NODELET_INFO_STREAM("Advertized on topic " << disparity_topic);
+
         //PointCloud publisher
         pub_cloud = nh.advertise<sensor_msgs::PointCloud2> (point_cloud_topic, 1);
         NODELET_INFO_STREAM("Advertized on topic " << point_cloud_topic);
@@ -464,6 +473,20 @@ namespace zed_wrapper {
         pub_depth.publish(imageToROSmsg(depth, encoding, depth_frame_id, t));
     }
 
+    void ZEDWrapperNodelet::publishDisparity(cv::Mat disparity, ros::Time t) {
+        sl::CameraInformation zedParam = zed.getCameraInformation();
+        sensor_msgs::ImagePtr disparity_image = imageToROSmsg(disparity,  sensor_msgs::image_encodings::TYPE_32FC1, disparity_frame_id, t);
+
+        stereo_msgs::DisparityImage msg;
+        msg.image = *disparity_image;
+        msg.header = msg.image.header;
+        msg.f = zedParam.calibration_parameters.left_cam.fx;
+        msg.T = zedParam.calibration_parameters.T.x;
+        msg.min_disparity = msg.f * msg.T / zed.getDepthMaxRangeValue();
+        msg.max_disparity = msg.f * msg.T / zed.getDepthMinRangeValue();
+        pub_disparity.publish(msg);
+    }
+
     void ZEDWrapperNodelet::publishPointCloud(int width, int height) {
         pcl::PointCloud<pcl::PointXYZRGB> point_cloud;
         point_cloud.width = width;
@@ -692,7 +715,7 @@ namespace zed_wrapper {
         trackParams.area_file_path = odometry_DB.c_str();
 
 
-        sl::Mat leftZEDMat, rightZEDMat, depthZEDMat;
+        sl::Mat leftZEDMat, rightZEDMat, depthZEDMat, disparityZEDMat;
         // Main loop
         while (nh_ns.ok()) {
             // Check for subscribers
@@ -703,9 +726,11 @@ namespace zed_wrapper {
             int right_SubNumber = pub_right.getNumSubscribers();
             int right_raw_SubNumber = pub_raw_right.getNumSubscribers();
             int depth_SubNumber = pub_depth.getNumSubscribers();
+            int disparity_SubNumber = pub_disparity.getNumSubscribers();
             int cloud_SubNumber = pub_cloud.getNumSubscribers();
             int odom_SubNumber = pub_odom.getNumSubscribers();
-            bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber + right_raw_SubNumber + depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;
+            bool runLoop = (rgb_SubNumber + rgb_raw_SubNumber + left_SubNumber + left_raw_SubNumber + right_SubNumber + 
+                            right_raw_SubNumber + depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber) > 0;
 
             runParams.enable_point_cloud = false;
             if (cloud_SubNumber > 0)
@@ -723,7 +748,7 @@ namespace zed_wrapper {
                     zed.disableTracking();
                     tracking_activated = false;
                 }
-                computeDepth = (depth_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
+                computeDepth = (depth_SubNumber + disparity_SubNumber + cloud_SubNumber + odom_SubNumber) > 0; // Detect if one of the subscriber need to have the depth information
                 ros::Time t = ros::Time::now(); // Get current time
 
                 grabbing = true;
@@ -849,6 +874,14 @@ namespace zed_wrapper {
                     publishDepth(toCVMat(depthZEDMat), t); // in meters
                 }
 
+                // Publish the disparity image if someone has subscribed to
+                if (disparity_SubNumber > 0) {
+                    zed.retrieveMeasure(disparityZEDMat, sl::MEASURE_DISPARITY);
+                    // Need to flip sign, but cause of this is not sure
+                    cv::Mat disparity = toCVMat(disparityZEDMat) * -1.0;
+                    publishDisparity(disparity, t);
+                }
+
                 // Publish the point cloud if someone has subscribed to
                 if (cloud_SubNumber > 0) {
                     // Run the point cloud conversion asynchronously to avoid slowing down all the program