Merge pull request #641 from stereolabs/fix_629

Fix 629

latest_changes.md

@@ -1,6 +1,11 @@
 LATEST CHANGES
 ==============
 
+RGB/Depth sync fix #629 (2020-11-02)
+-------------------------------
+- Fixed sync issue between RGB and Depth data (Thx @dennisVi)
+- Fixed issues with SVO and sensors data (Thx @dennisVi)
+
 ASYNC Object Detection (2020-09-18)
 -----------------------------------
 - Object Detection now runs asynchronously respect to data grabbing and Object Detected data are published only when available not affecting the frequency of the publishing of the other data types

zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp

@@ -183,7 +183,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
 
     /*! \brief Publish a fused pointCloud with a ros Publisher
          */
-    void pubFusedPointCloudCallback(const ros::TimerEvent& e);
+    void callback_pubFusedPointCloud(const ros::TimerEvent& e);
 
     /*! \brief Publish the informations of a camera with a ros Publisher
          * \param cam_info_msg : the information message to publish
@@ -199,6 +199,11 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
          */
     void publishDisparity(sl::Mat disparity, ros::Time t);
 
+    /*! \brief Publish sensors data and TF
+         * \param t : the ros::Time to stamp the depth image
+         */
+    void publishSensData(ros::Time t = ros::Time(0));
+
     /*! \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
@@ -231,27 +236,27 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
 
     /*! \brief Callback to handle dynamic reconfigure events in ROS
          */
-    void dynamicReconfCallback(zed_nodelets::ZedConfig& config, uint32_t level);
+    void callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level);
 
     /*! \brief Callback to publish Video and Depth data
          * \param e : the ros::TimerEvent binded to the callback
          */
-    void pubVideoDepthCallback(const ros::TimerEvent& e);
+    void callback_pubVideoDepth(const ros::TimerEvent& e);
 
     /*! \brief Callback to publish Path data with a ROS publisher.
          * \param e : the ros::TimerEvent binded to the callback
          */
-    void pubPathCallback(const ros::TimerEvent& e);
+    void callback_pubPath(const ros::TimerEvent& e);
 
     /*! \brief Callback to publish Sensor Data with a ROS publisher.
          * \param e : the ros::TimerEvent binded to the callback
          */
-    void pubSensCallback(const ros::TimerEvent& e);
+    void callback_pubSensorsData(const ros::TimerEvent& e);
 
     /*! \brief Callback to update node diagnostic status
          * \param stat : node status
          */
-    void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
+    void callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat);
 
     /*! \brief Service callback to reset_tracking service
          * Tracking pose is reinitialized to the value available in the ROS Param
@@ -396,7 +401,9 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
     std::thread mDevicePollThread;
     std::thread mPcThread; // Point Cloud thread
 
-    bool mStopNode;
+    bool mStopNode = false;
+
+    const double mSensPubRate = 400.0;
 
     // Publishers
     image_transport::CameraPublisher mPubRgb; //
@@ -522,8 +529,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
     int mDepthStabilization;
     std::string mAreaMemDbPath;
     std::string mSvoFilepath;
-    std::string mRemoteStreamAddr;
-    double mSensPubRate = 400.0;
+    std::string mRemoteStreamAddr;    
     bool mSensTimestampSync;
     double mPathPubRate;
     int mPathMaxCount;
@@ -646,6 +652,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {
     std::mutex mObjDetMutex;
     std::condition_variable mPcDataReadyCondVar;
     bool mPcDataReady;
+    std::condition_variable mRgbDepthDataRetrievedCondVar;
+    bool mRgbDepthDataRetrieved;
 
     // Point cloud variables
     sl::Mat mCloud;