avoid latency for video & depth data with pub_frame_rate = 0

- allow setting pub_frame_rate to 0
- when 0
  - disable timer based publishing
  - instead publish immidiately after grabbing

Within current worklow publishing and grabbing threads work separately.

With publishing rate == grabbing rate this
- adds average FRAME_TIME/2 latency
- adds pessimistic FRAME_TIME latency

The fix removes latency by notifying ROS thread to publish after succesful grabbing

Fix for:
- stereolabs#895

zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp

@@ -72,6 +72,7 @@
 #include <sensor_msgs/point_cloud2_iterator.h>
 #include <stereo_msgs/DisparityImage.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <std_msgs/Empty.h>
 
 #include <condition_variable>
 #include <memory>
@@ -245,6 +246,12 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
    */
     void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level);
 
+    /*! \brief Internal notifification on grabbed data as thread sync mechanism
+   * \param e : empty message
+   */
+    void callback_grabbedData(const std_msgs::Empty &e);
+
+
     /*! \brief Callback to publish Video and Depth data
    * \param e : the ros::TimerEvent binded to the callback
    */
@@ -461,7 +468,10 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
     ros::Publisher mPubMarker; // Publisher for Rviz markers
     ros::Publisher mPubPlane; // Publisher for detected planes
 
+    ros::Publisher mPubGrabbed; // internal notification about grabbed data
+
     // Subscribers
+    ros::Subscriber mGrabbedSub; // internal notification about grabbed data
     ros::Subscriber mClickedPtSub;
 
     // Timers

zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp

@@ -162,6 +162,9 @@ void ZEDWrapperNodelet::onInit()
     std::string marker_topic = "plane_marker";
     std::string plane_topic = "plane";
 
+    // Grabbed data topic (internal notification)
+    std::string grabbed_topic = "grabbed";
+
     // Create camera info
     mRgbCamInfoMsg.reset(new sensor_msgs::CameraInfo());
     mLeftCamInfoMsg.reset(new sensor_msgs::CameraInfo());
@@ -533,7 +536,11 @@ void ZEDWrapperNodelet::onInit()
         }
     }
 
+    // Internal notification about grabbed data
+    mPubGrabbed = mNhNs.advertise<std_msgs::Empty>(grabbed_topic, 1);
+
     // Subscribers
+    mGrabbedSub = mNhNs.subscribe(grabbed_topic, 1, &ZEDWrapperNodelet::callback_grabbedData, this);
     mClickedPtSub = mNhNs.subscribe(mClickedPtTopic, 10, &ZEDWrapperNodelet::clickedPtCallback, this);
 
     NODELET_INFO_STREAM("Subscribed to topic " << mClickedPtTopic.c_str());
@@ -566,7 +573,8 @@ void ZEDWrapperNodelet::onInit()
     mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this);
 
     // Start data publishing timer
-    mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this);
+    ros::Duration timerPeriod = mVideoDepthFreq ? ros::Duration(1.0 / mVideoDepthFreq) : ros::DURATION_MAX;
+    mVideoDepthTimer = mNhNs.createTimer(timerPeriod, &ZEDWrapperNodelet::callback_pubVideoDepth, this);
 
     // Start Sensors thread
     mSensThread = std::thread(&ZEDWrapperNodelet::sensors_thread_func, this);
@@ -2295,7 +2303,7 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config,
             NODELET_INFO("Reconfigure Video and Depth pub. frequency: %g", mVideoDepthFreq);
         }
 
-        mVideoDepthTimer.setPeriod(ros::Duration(1.0 / mVideoDepthFreq));
+        mVideoDepthTimer.setPeriod(mVideoDepthFreq ? ros::Duration(1.0 / mVideoDepthFreq) : ros::DURATION_MAX);
 
         mDynParMutex.unlock();
         // NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX UNLOCK");
@@ -2436,7 +2444,12 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config,
     }
 }
 
-void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e)
+void ZEDWrapperNodelet::callback_grabbedData(const std_msgs::Empty& /*e*/)
+{
+    callback_pubVideoDepth(ros::TimerEvent());
+}
+
+void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& /*e*/)
 {
     static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency
 
@@ -3344,6 +3357,16 @@ void ZEDWrapperNodelet::device_poll_thread_func()
 
             mFrameCount++;
 
+            // Internal grab notification (if not running timer based publishing)
+            double videoDepthFreq;
+
+            mDynParMutex.lock();
+            videoDepthFreq = mVideoDepthFreq;
+            mDynParMutex.unlock();
+
+            if(!videoDepthFreq)
+                mPubGrabbed.publish(std_msgs::Empty());
+
             // SVO recording
             mRecMutex.lock();
 

zed_wrapper/cfg/Zed.cfg

@@ -6,7 +6,7 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 gen = ParameterGenerator()
 
 group_general = gen.add_group("general")
-group_general.add("pub_frame_rate",         double_t,   0, "Video and Depth data frequency",                        15.0,   0.1,    60.0);
+group_general.add("pub_frame_rate",         double_t,   0, "Video and Depth data frequency (0 immidiately after grabbing)", 15.0,   0,    60.0);
 
 group_depth = gen.add_group("depth")
 group_depth.add("depth_confidence",         int_t,      1, "Depth confidence threshold",                            50,     1,      100)

zed_wrapper/params/common.yaml

@@ -16,7 +16,7 @@ auto_whitebalance:          true                                # Dynamic
 whitebalance_temperature:   42                                  # Dynamic - works only if `auto_whitebalance` is false
 depth_confidence:           30                                  # Dynamic
 depth_texture_conf:         100                                 # Dynamic
-pub_frame_rate:             15.0                                # Dynamic - frequency of publishing of video and depth data
+pub_frame_rate:             15.0                                # Dynamic - frequency of publishing of video and depth data, 0 to publish immidiately after grabbing
 point_cloud_freq:           10.0                                # Dynamic - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value)
 
 general: