From ac1c162d0995c226111f08caf22948b57b299627 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 9 Oct 2020 12:58:24 +0200 Subject: [PATCH 1/8] Fix to RGB/Depth desync issue --- .../include/zed_wrapper_nodelet.hpp | 14 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 386 +++++++++--------- 2 files changed, 203 insertions(+), 197 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp index 5adba6c3..877e42bb 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/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 @@ -231,27 +231,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_pubSens(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 @@ -646,6 +646,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; diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 2558284a..c944805a 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -67,6 +67,7 @@ void ZEDWrapperNodelet::onInit() { mStopNode = false; mPcDataReady = false; + mRgbDepthDataRetrieved = true; #ifndef NDEBUG @@ -235,7 +236,7 @@ void ZEDWrapperNodelet::onInit() { mZedParams.enable_image_enhancement = true; // Always active - mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::updateDiagnostic); + mDiagUpdater.add("ZED Diagnostic", this, &ZEDWrapperNodelet::callback_updateDiagnostic); mDiagUpdater.setHardwareID("ZED camera"); mConnStatus = sl::ERROR_CODE::CAMERA_NOT_DETECTED; @@ -359,7 +360,7 @@ void ZEDWrapperNodelet::onInit() { // ----> Dynamic Reconfigure parameters mDynRecServer = boost::make_shared>(mDynServerMutex); dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&ZEDWrapperNodelet::dynamicReconfCallback, this, _1, _2); + f = boost::bind(&ZEDWrapperNodelet::callback_dynamicReconf, this, _1, _2); mDynRecServer->setCallback(f); // Update parameters zed_nodelets::ZedConfig config; @@ -467,7 +468,7 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubMapPath.getTopic()); mPathTimer = mNhNs.createTimer(ros::Duration(1.0 / mPathPubRate), - &ZEDWrapperNodelet::pubPathCallback, this); + &ZEDWrapperNodelet::callback_pubPath, this); if (mPathMaxCount != -1) { NODELET_DEBUG_STREAM("Path vectors reserved " << mPathMaxCount << " poses."); @@ -518,7 +519,7 @@ void ZEDWrapperNodelet::onInit() { mFrameTimestamp = ros::Time::now(); mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), - &ZEDWrapperNodelet::pubSensCallback, this); + &ZEDWrapperNodelet::callback_pubSens, this); mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); @@ -577,7 +578,7 @@ 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::pubVideoDepthCallback, + mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); } @@ -1298,7 +1299,7 @@ bool ZEDWrapperNodelet::start_3d_mapping() { mMappingRunning = true; - mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::pubFusedPointCloudCallback, + mFusedPcTimer = mNhNs.createTimer(ros::Duration(1.0 / mFusedPcPubFreq), &ZEDWrapperNodelet::callback_pubFusedPointCloud, this); NODELET_INFO_STREAM(" * Resolution: " << params.resolution_meter << " m"); @@ -1757,13 +1758,11 @@ void ZEDWrapperNodelet::pointcloud_thread_func() { if (elapsed_msec < pc_period_msec) { std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(pc_period_msec - elapsed_msec))); } - // <---- Check publishing frequency last_time = std::chrono::steady_clock::now(); publishPointCloud(); - mPcDataReady = false; } @@ -1817,7 +1816,7 @@ void ZEDWrapperNodelet::publishPointCloud() { mPubCloud.publish(pointcloudMsg); } -void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubFusedPointCloud(const ros::TimerEvent& e) { sensor_msgs::PointCloud2Ptr pointcloudFusedMsg = boost::make_shared(); @@ -2105,7 +2104,7 @@ void ZEDWrapperNodelet::updateDynamicReconfigure() { //NODELET_DEBUG_STREAM( "updateDynamicReconfigure MUTEX UNLOCK"); } -void ZEDWrapperNodelet::dynamicReconfCallback(zed_nodelets::ZedConfig& config, uint32_t level) { +void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, uint32_t level) { //NODELET_DEBUG_STREAM( "dynamicReconfCallback MUTEX LOCK"); mDynParMutex.lock(); DynParams param = static_cast(level); @@ -2270,244 +2269,237 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_nodelets::ZedConfig& config, u } } -void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); - - if(rgbSubnumber+rgbRawSubnumber+ - leftSubnumber+leftRawSubnumber+ - rightSubnumber+rightRawSubnumber+ - rgbGraySubnumber+rgbGrayRawSubnumber+ - leftGraySubnumber+leftGrayRawSubnumber+ - rightGraySubnumber+rightGrayRawSubnumber+ - depthSubnumber+disparitySubnumber+confMapSubnumber+ - stereoSubNumber+stereoRawSubNumber == 0) { + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); // + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); // + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); // + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); // + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); // + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); // + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); // + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); // + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); // + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); // + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); // + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); // + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); // + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); // + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); // + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); // + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); // + + bool retrieved = false; + + sl::Mat mat_left,mat_left_raw; + sl::Mat mat_right,mat_right_raw; + sl::Mat mat_left_gray,mat_left_raw_gray; + sl::Mat mat_right_gray,mat_right_raw_gray; + sl::Mat mat_depth,mat_disp,mat_conf; + + sl::Timestamp ts_rgb=0; // used to check RGB/Depth sync + sl::Timestamp ts_depth=0; // used to check RGB/Depth sync + sl::Timestamp grab_ts=0; + + // ----> Retrieve all required data + std::unique_lock lock(mCamDataMutex, std::defer_lock); + + if (lock.try_lock()) { + if(rgbSubnumber+leftSubnumber>0) { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + ts_rgb=mat_left.timestamp; + grab_ts=mat_left.timestamp; + } + if(rgbRawSubnumber+leftRawSubnumber>0) { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw.timestamp; + } + if(rightSubnumber>0) { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right.timestamp; + } + if(rightRawSubnumber>0) { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw.timestamp; + } + if(rgbGraySubnumber+leftGraySubnumber>0) { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_gray.timestamp; + } + if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw_gray.timestamp; + } + if(rightGraySubnumber>0) { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_gray.timestamp; + } + if(rightGrayRawSubnumber>0) { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw_gray.timestamp; + } + if(depthSubnumber>0) { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_depth.timestamp; + + ts_depth = mat_depth.timestamp; + if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { + NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); + } + } + if(disparitySubnumber>0) { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_disp.timestamp; + } + if(confMapSubnumber) { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_conf.timestamp; + } + } + // <---- Retrieve all required data + + // ----> Notify grab thread that all data are synchronized and a new grab can be done + mRgbDepthDataRetrievedCondVar.notify_one(); + mRgbDepthDataRetrieved = true; + // <---- Notify grab thread that all data are synchronized and a new grab can be done + + if(!retrieved) { mPublishingData = false; lastZedTs = 0; return; } - mPublishingData = true; - sl::Mat leftZEDMat, rightZEDMat, leftGrayZEDMat, rightGrayZEDMat, - depthZEDMat, disparityZEDMat, confMapZEDMat; - - // Retrieve RGBA Left image and use Timestamp to check if image is new - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - if(leftZEDMat.timestamp==lastZedTs) { - return; // No new image! + // ----> Check if a grab has been done before publishing the same images + if( grab_ts.data_ns==lastZedTs.data_ns ) { + // Data not updated by a grab calling in the grab thread + return; } if(lastZedTs.data_ns!=0) { - double period_sec = static_cast(leftZEDMat.timestamp.data_ns - lastZedTs.data_ns)/1e9; + double period_sec = static_cast(grab_ts.data_ns - lastZedTs.data_ns)/1e9; //NODELET_DEBUG_STREAM( "PUBLISHING PERIOD: " << period_sec << " sec @" << 1./period_sec << " Hz") ; mVideoDepthPeriodMean_sec->addValue(period_sec); //NODELET_DEBUG_STREAM( "MEAN PUBLISHING PERIOD: " << mVideoDepthPeriodMean_sec->getMean() << " sec @" << 1./mVideoDepthPeriodMean_sec->getMean() << " Hz") ; } - lastZedTs = leftZEDMat.timestamp; - - // Timestamp to be used for topics headers - ros::Time stamp = mFrameTimestamp; - - // Publish the left == rgb image if someone has subscribed to - if (leftSubnumber > 0 || rgbSubnumber > 0) { - if (leftSubnumber > 0) { - sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + lastZedTs = grab_ts; + // <---- Check if a grab has been done before publishing the same images - publishImage(leftImgMsg, leftZEDMat, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); - } - - if (rgbSubnumber > 0) { - sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - // rgb is the left image - publishImage(rgbImgMsg, leftZEDMat, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); - } + // Publish the left = rgb image if someone has subscribed to + if (leftSubnumber > 0) { + sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); + publishImage(leftImgMsg, mat_left, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); } - - // Publish the left == rgb GRAY image if someone has subscribed to - if (leftGraySubnumber > 0 || rgbGraySubnumber > 0) { - - // Retrieve RGBA Left image - mZed.retrieveImage(leftGrayZEDMat, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); - - if (leftGraySubnumber > 0) { - sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); - - publishImage(leftGrayImgMsg, leftGrayZEDMat, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbGraySubnumber > 0) { - sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); - - publishImage(rgbGrayImgMsg, leftGrayZEDMat, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, - stamp); // rgb is the left image - } + if (rgbSubnumber > 0) { + sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared(); + publishImage(rgbImgMsg, mat_left, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp); } - // Publish the left_raw == rgb_raw image if someone has subscribed to - if (leftRawSubnumber > 0 || rgbRawSubnumber > 0) { - - // Retrieve RGBA Left image - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - - if (leftRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); - - publishImage(rawLeftImgMsg, leftZEDMat, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + // Publish the left = rgb GRAY image if someone has subscribed to + if (leftGraySubnumber > 0) { + sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared(); + publishImage(leftGrayImgMsg, mat_left_gray, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGraySubnumber > 0) { + sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared(); + publishImage(rgbGrayImgMsg, mat_left_gray, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId, stamp); + } - publishImage(rawRgbImgMsg, leftZEDMat, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, - stamp); - } + // Publish the left_raw = rgb_raw image if someone has subscribed to + if (leftRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared(); + publishImage(rawLeftImgMsg, mat_left_raw, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared(); + publishImage(rawRgbImgMsg, mat_left_raw, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); } // Publish the left_raw == rgb_raw GRAY image if someone has subscribed to - if (leftGrayRawSubnumber > 0 || rgbGrayRawSubnumber > 0) { - - // Retrieve RGBA Left image - mZed.retrieveImage(leftGrayZEDMat, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + if (leftGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - if (leftGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared(); - - publishImage(rawLeftGrayImgMsg, leftGrayZEDMat, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); - - publishImage(rawRgbGrayImgMsg, leftGrayZEDMat, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, - stamp); - } + publishImage(rawLeftGrayImgMsg, mat_left_raw_gray, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, stamp); + } + if (rgbGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); + publishImage(rawRgbGrayImgMsg, mat_left_raw_gray, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId, stamp); } // Publish the right image if someone has subscribed to if (rightSubnumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rightImgMsg = boost::make_shared(); - - publishImage(rightImgMsg, rightZEDMat, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + publishImage(rightImgMsg, mat_right, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp); } // Publish the right image GRAY if someone has subscribed to if (rightGraySubnumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightGrayZEDMat, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared(); - - publishImage(rightGrayImgMsg, rightGrayZEDMat, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); + publishImage(rightGrayImgMsg, mat_right_gray, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp); } // Publish the right raw image if someone has subscribed to if (rightRawSubnumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared(); - - publishImage(rawRightImgMsg, rightZEDMat, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + publishImage(rawRightImgMsg, mat_right_raw, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); } // Publish the right raw image GRAY if someone has subscribed to if (rightGrayRawSubnumber > 0) { - // Retrieve RGBA Right image - mZed.retrieveImage(rightGrayZEDMat, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared(); - - publishImage(rawRightGrayImgMsg, rightGrayZEDMat, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); + publishImage(rawRightGrayImgMsg, mat_right_raw_gray, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp); } // Stereo couple side-by-side if (stereoSubNumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared(); - - sl_tools::imagesToROSmsg(stereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp); - + sl_tools::imagesToROSmsg(stereoImgMsg, mat_left, mat_right, mCameraFrameId, stamp); mPubStereo.publish(stereoImgMsg); } // Stereo RAW couple side-by-side if (stereoRawSubNumber > 0) { - - // Retrieve RGBA Right image - mZed.retrieveImage(rightZEDMat, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - mZed.retrieveImage(leftZEDMat, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared(); - - sl_tools::imagesToROSmsg(rawStereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp); - + sl_tools::imagesToROSmsg(rawStereoImgMsg, mat_left_raw, mat_right_raw, mCameraFrameId, stamp); mPubRawStereo.publish(rawStereoImgMsg); } // Publish the depth image if someone has subscribed to - if (depthSubnumber > 0 || disparitySubnumber > 0) { - - mZed.retrieveMeasure(depthZEDMat, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); - + if (depthSubnumber > 0) { sensor_msgs::ImagePtr depthImgMsg = boost::make_shared(); - - publishDepth(depthImgMsg, depthZEDMat, stamp); // in meters + publishDepth(depthImgMsg, mat_depth, stamp); } // Publish the disparity image if someone has subscribed to if (disparitySubnumber > 0) { - - mZed.retrieveMeasure(disparityZEDMat, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - publishDisparity(disparityZEDMat, stamp); + publishDisparity(mat_disp, stamp); } // Publish the confidence map if someone has subscribed to if (confMapSubnumber > 0) { - - mZed.retrieveMeasure(confMapZEDMat, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); - sensor_msgs::ImagePtr confMapMsg = boost::make_shared(); - - sl_tools::imageToROSmsg(confMapMsg, confMapZEDMat, mConfidenceOptFrameId, stamp); - + sl_tools::imageToROSmsg(confMapMsg, mat_conf, mConfidenceOptFrameId, stamp); mPubConfMap.publish(confMapMsg); } } -void ZEDWrapperNodelet::pubPathCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); @@ -2579,7 +2571,7 @@ void ZEDWrapperNodelet::pubPathCallback(const ros::TimerEvent& e) { } } -void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { @@ -3169,6 +3161,21 @@ void ZEDWrapperNodelet::device_poll_thread_func() { runParams.enable_depth = false; // Ask to not compute the depth } + // ----> Wait for RGB/Depth synchronization before grabbing + std::unique_lock datalock(mCamDataMutex); + while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups + if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { + // Check thread stopping + if (mStopNode) { + return; + } else { + continue; + } + } + } + // <---- Wait for RGB/Depth synchronization before grabbing + + mRgbDepthDataRetrieved = false; mGrabStatus = mZed.grab(runParams); std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); @@ -3368,8 +3375,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } // <---- Camera Settings - mCamDataMutex.lock(); - // Publish the point cloud if someone has subscribed to if (cloudSubnumber > 0) { @@ -3392,7 +3397,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } else { mPcPublishing = false; } - mCamDataMutex.unlock(); mObjDetMutex.lock(); if (mObjDetRunning) { @@ -3606,24 +3610,24 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { - // Publish pose tf only if enabled - if(mPublishTf) { - // Note, the frame is published, but its values will only change if - // someone has subscribed to odom - publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame - - if(mPublishMapTf) { + // Publish pose tf only if enabled + if(mPublishTf) { // Note, the frame is published, but its values will only change if - // someone has subscribed to map - publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame - } + // someone has subscribed to odom + publishOdomFrame(mOdom2BaseTransf, stamp); // publish the base Frame in odometry frame + + if(mPublishMapTf) { + // Note, the frame is published, but its values will only change if + // someone has subscribed to map + publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame + } - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } } } - } #if 0 //#ifndef NDEBUG // Enable for TF checking // Double check: map_to_pose must be equal to mMap2BaseTransf @@ -3729,7 +3733,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { NODELET_DEBUG("ZED pool thread finished"); } -void ZEDWrapperNodelet::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { +void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) { if (mConnStatus != sl::ERROR_CODE::SUCCESS) { stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, sl::toString(mConnStatus).c_str()); From 6d26090baefae2b3c816edfb4d76ec97f3a1755c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Fri, 9 Oct 2020 16:32:41 +0200 Subject: [PATCH 2/8] Fix stereo couple publishing --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index c944805a..727551da 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2272,23 +2272,23 @@ void ZEDWrapperNodelet::callback_dynamicReconf(zed_nodelets::ZedConfig& config, void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency - uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); // - uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); // - uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); // - uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); // - uint32_t rightSubnumber = mPubRight.getNumSubscribers(); // - uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); // - uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); // - uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); // - uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); // - uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); // - uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); // - uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); // - uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); // - uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); // - uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); // - uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); // - uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); // + uint32_t rgbSubnumber = mPubRgb.getNumSubscribers(); + uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers(); + uint32_t leftSubnumber = mPubLeft.getNumSubscribers(); + uint32_t leftRawSubnumber = mPubRawLeft.getNumSubscribers(); + uint32_t rightSubnumber = mPubRight.getNumSubscribers(); + uint32_t rightRawSubnumber = mPubRawRight.getNumSubscribers(); + uint32_t rgbGraySubnumber = mPubRgbGray.getNumSubscribers(); + uint32_t rgbGrayRawSubnumber = mPubRawRgbGray.getNumSubscribers(); + uint32_t leftGraySubnumber = mPubLeftGray.getNumSubscribers(); + uint32_t leftGrayRawSubnumber = mPubRawLeftGray.getNumSubscribers(); + uint32_t rightGraySubnumber = mPubRightGray.getNumSubscribers(); + uint32_t rightGrayRawSubnumber = mPubRawRightGray.getNumSubscribers(); + uint32_t depthSubnumber = mPubDepth.getNumSubscribers(); + uint32_t disparitySubnumber = mPubDisparity.getNumSubscribers(); + uint32_t confMapSubnumber = mPubConfMap.getNumSubscribers(); + uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); + uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); bool retrieved = false; @@ -2306,23 +2306,23 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { std::unique_lock lock(mCamDataMutex, std::defer_lock); if (lock.try_lock()) { - if(rgbSubnumber+leftSubnumber>0) { + if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); retrieved = true; ts_rgb=mat_left.timestamp; grab_ts=mat_left.timestamp; } - if(rgbRawSubnumber+leftRawSubnumber>0) { + if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); retrieved = true; grab_ts=mat_left_raw.timestamp; } - if(rightSubnumber>0) { + if(rightSubnumber+stereoSubNumber>0) { mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); retrieved = true; grab_ts=mat_right.timestamp; } - if(rightRawSubnumber>0) { + if(rightRawSubnumber+stereoRawSubNumber>0) { mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); retrieved = true; grab_ts=mat_right_raw.timestamp; From c4842fc87c8618c9af4bc7612846966b42190b11 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 26 Oct 2020 14:15:17 +0100 Subject: [PATCH 3/8] minor fix --- zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 727551da..ef12f99d 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2825,7 +2825,7 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { // } // } - if( (imu_SubNumber > 0 || mPublishImuTf) && new_imu_data) { + if( imu_SubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; sensor_msgs::ImuPtr imuMsg = boost::make_shared(); From 292dfab2e078e16b53b75231ed2643cc326d1235 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Tue, 27 Oct 2020 17:53:15 +0100 Subject: [PATCH 4/8] Sensor/Camera desync potentially fixed. More test and QA to do --- .../include/zed_wrapper_nodelet.hpp | 14 +- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 438 +++++++----------- zed_wrapper/params/zed2.yaml | 2 +- 3 files changed, 191 insertions(+), 263 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp index 877e42bb..03da5dc4 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -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 @@ -246,7 +251,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet { /*! \brief Callback to publish Sensor Data with a ROS publisher. * \param e : the ros::TimerEvent binded to the callback */ - void callback_pubSens(const ros::TimerEvent& e); + void callback_pubSensorsData(const ros::TimerEvent& e); /*! \brief Callback to update node diagnostic status * \param stat : node status @@ -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; diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index ef12f99d..897b509e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -482,47 +482,29 @@ void ZEDWrapperNodelet::onInit() { } // Sensor publishers - /*if (!mSvoMode)*/ { - if (mSensPubRate > 0 && mZedRealCamModel != sl::MODEL::ZED) { - // IMU Publishers - mPubImu = mNhNs.advertise(imu_topic, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic() << " @ " - << mSensPubRate << " Hz"); - mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic() << " @ " - << mSensPubRate << " Hz"); - mPubImuMag = mNhNs.advertise(imu_mag_topic, 1/*MAG_FREQ*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic() << " @ " - << std::min(MAG_FREQ,mSensPubRate) << " Hz"); - //mPubImuMagRaw = mNhNs.advertise(imu_mag_topic_raw, static_cast(MAG_FREQ)); - //NODELET_INFO_STREAM("Advertised on topic " << mPubImuMagRaw.getTopic() << " @ " - // << std::min(MAG_FREQ,mSensPubRate) << " Hz"); - - if( mZedRealCamModel == sl::MODEL::ZED2 ) { - // IMU temperature sensor - mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1/*static_cast(mSensPubRate)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic() << " @ " << mSensPubRate << " Hz"); - - // Atmospheric pressure - mPubPressure = mNhNs.advertise(pressure_topic, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic() << " @ " - << std::min(BARO_FREQ,mSensPubRate ) << " Hz"); - - // CMOS sensor temperatures - mPubTempL = mNhNs.advertise(temp_topic_left, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic() << " @ " - << std::min(BARO_FREQ,mSensPubRate ) << " Hz"); - mPubTempR = mNhNs.advertise(temp_topic_right, 1/*static_cast(BARO_FREQ)*/); - NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic() << " @ " - << std::min(BARO_FREQ,mSensPubRate ) << " Hz"); - } - - mFrameTimestamp = ros::Time::now(); - mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), - &ZEDWrapperNodelet::callback_pubSens, this); - mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); - - + if (mZedRealCamModel != sl::MODEL::ZED) { + // IMU Publishers + mPubImu = mNhNs.advertise(imu_topic, 1/*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImu.getTopic() ); + mPubImuRaw = mNhNs.advertise(imu_topic_raw, 1/*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuRaw.getTopic() ); + mPubImuMag = mNhNs.advertise(imu_mag_topic, 1/*MAG_FREQ*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuMag.getTopic() ); + + if( mZedRealCamModel == sl::MODEL::ZED2 ) { + // IMU temperature sensor + mPubImuTemp = mNhNs.advertise(imu_temp_topic, 1/*static_cast(mSensPubRate)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubImuTemp.getTopic() ); + + // Atmospheric pressure + mPubPressure = mNhNs.advertise(pressure_topic, 1/*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubPressure.getTopic() ); + + // CMOS sensor temperatures + mPubTempL = mNhNs.advertise(temp_topic_left, 1/*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempL.getTopic() ); + mPubTempR = mNhNs.advertise(temp_topic_right, 1/*static_cast(BARO_FREQ)*/); + NODELET_INFO_STREAM("Advertised on topic " << mPubTempR.getTopic() ); } // Publish camera imu transform in a latched topic @@ -551,6 +533,15 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } + + if(!mSvoMode) { + mFrameTimestamp = ros::Time::now(); + mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), + &ZEDWrapperNodelet::callback_pubSensorsData, this); + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); + } else { + mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mCamFrameRate / 2)); + } } // Services @@ -2302,78 +2293,96 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { sl::Timestamp ts_depth=0; // used to check RGB/Depth sync sl::Timestamp grab_ts=0; - // ----> Retrieve all required data - std::unique_lock lock(mCamDataMutex, std::defer_lock); - - if (lock.try_lock()) { - if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { - mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - ts_rgb=mat_left.timestamp; - grab_ts=mat_left.timestamp; - } - if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { - mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_raw.timestamp; - } - if(rightSubnumber+stereoSubNumber>0) { - mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right.timestamp; - } - if(rightRawSubnumber+stereoRawSubNumber>0) { - mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_raw.timestamp; + mCamDataMutex.lock(); + + // ----> Retrieve all required image data + if(rgbSubnumber+leftSubnumber+stereoSubNumber>0) { + mZed.retrieveImage(mat_left, sl::VIEW::LEFT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + ts_rgb=mat_left.timestamp; + grab_ts=mat_left.timestamp; + } + if(rgbRawSubnumber+leftRawSubnumber+stereoRawSubNumber>0) { + mZed.retrieveImage(mat_left_raw, sl::VIEW::LEFT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw.timestamp; + } + if(rightSubnumber+stereoSubNumber>0) { + mZed.retrieveImage(mat_right, sl::VIEW::RIGHT, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right.timestamp; + } + if(rightRawSubnumber+stereoRawSubNumber>0) { + mZed.retrieveImage(mat_right_raw, sl::VIEW::RIGHT_UNRECTIFIED, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw.timestamp; + } + if(rgbGraySubnumber+leftGraySubnumber>0) { + mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_gray.timestamp; + } + if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { + mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_left_raw_gray.timestamp; + } + if(rightGraySubnumber>0) { + mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_gray.timestamp; + } + if(rightGrayRawSubnumber>0) { + mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); + retrieved = true; + grab_ts=mat_right_raw_gray.timestamp; + } + if(depthSubnumber>0) { + mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_depth.timestamp; + + ts_depth = mat_depth.timestamp; + + if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { + NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); } - if(rgbGraySubnumber+leftGraySubnumber>0) { - mZed.retrieveImage(mat_left_gray, sl::VIEW::LEFT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_gray.timestamp; - } - if(rgbGrayRawSubnumber+leftGrayRawSubnumber>0) { - mZed.retrieveImage(mat_left_raw_gray, sl::VIEW::LEFT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_left_raw_gray.timestamp; - } - if(rightGraySubnumber>0) { - mZed.retrieveImage(mat_right_gray, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_gray.timestamp; - } - if(rightGrayRawSubnumber>0) { - mZed.retrieveImage(mat_right_raw_gray, sl::VIEW::RIGHT_UNRECTIFIED_GRAY, sl::MEM::CPU, mMatResolVideo); - retrieved = true; - grab_ts=mat_right_raw_gray.timestamp; - } - if(depthSubnumber>0) { - mZed.retrieveMeasure(mat_depth, sl::MEASURE::DEPTH, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_depth.timestamp; + } + if(disparitySubnumber>0) { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_disp.timestamp; + } + if(confMapSubnumber) { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_conf.timestamp; + } + // <---- Retrieve all required image data - ts_depth = mat_depth.timestamp; + // ----> Data ROS timestamp + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); + if(mSvoMode) { + stamp = ros::Time::now(); + } + // <---- Data ROS timestamp - if( ts_rgb.data_ns!=0 && (ts_depth.data_ns!=ts_rgb.data_ns) ) { - NODELET_WARN_STREAM( "!!!!! DEPTH/RGB ASYNC !!!!! - Delta: " << 1e-9*static_cast(ts_depth-ts_rgb) << " sec"); + // ----> Publish sensor data if sync is required by user or SVO + if( mZedRealCamModel!=sl::MODEL::ZED ) + { + if(mSensTimestampSync || mSvoMode) { + if(retrieved) { + publishSensData(stamp); } } - if(disparitySubnumber>0) { - mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_disp.timestamp; - } - if(confMapSubnumber) { - mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); - retrieved = true; - grab_ts=mat_conf.timestamp; - } } - // <---- Retrieve all required data + // <---- Publish sensor data if sync is required by user or SVO + + mCamDataMutex.unlock(); // ----> Notify grab thread that all data are synchronized and a new grab can be done - mRgbDepthDataRetrievedCondVar.notify_one(); - mRgbDepthDataRetrieved = true; + //mRgbDepthDataRetrievedCondVar.notify_one(); + //mRgbDepthDataRetrieved = true; // <---- Notify grab thread that all data are synchronized and a new grab can be done if(!retrieved) { @@ -2398,9 +2407,6 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { lastZedTs = grab_ts; // <---- Check if a grab has been done before publishing the same images - ros::Time stamp = sl_tools::slTime2Ros(grab_ts); - - // Publish the left = rgb image if someone has subscribed to if (leftSubnumber > 0) { sensor_msgs::ImagePtr leftImgMsg = boost::make_shared(); @@ -2571,18 +2577,24 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { } } -void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { + NODELET_DEBUG("callback_pubSensorsData"); std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { return; } + publishSensData(); +} + +void ZEDWrapperNodelet::publishSensData(ros::Time t) { + NODELET_DEBUG("publishSensData"); + uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); uint32_t imu_TempSubNumber = 0; uint32_t imu_MagSubNumber = 0; - //uint32_t imu_MagRawSubNumber = 0; uint32_t pressSubNumber = 0; uint32_t tempLeftSubNumber = 0; uint32_t tempRightSubNumber = 0; @@ -2590,66 +2602,73 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { if( mZedRealCamModel == sl::MODEL::ZED2 ) { imu_TempSubNumber = mPubImuTemp.getNumSubscribers(); imu_MagSubNumber = mPubImuMag.getNumSubscribers(); - //imu_MagRawSubNumber = mPubImuMagRaw.getNumSubscribers(); pressSubNumber = mPubPressure.getNumSubscribers(); tempLeftSubNumber = mPubTempL.getNumSubscribers(); tempRightSubNumber = mPubTempR.getNumSubscribers(); } - int totSub = imu_SubNumber + imu_RawSubNumber + imu_TempSubNumber + imu_MagSubNumber + /*imu_MagRawSubNumber +*/ - pressSubNumber + tempLeftSubNumber + tempRightSubNumber; - ros::Time ts_imu; ros::Time ts_baro; ros::Time ts_mag; - //ros::Time ts_mag_raw; static ros::Time lastTs_imu = ros::Time(); static ros::Time lastTs_baro = ros::Time(); static ros::Time lastT_mag = ros::Time(); - //static ros::Time lastT_mag_raw = ros::Time(); sl::SensorsData sens_data; - if(mSvoMode) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) + if(mSvoMode || mSensTimestampSync) { + if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) { + NODELET_DEBUG("Not retrieved sensors data in IMAGE REFERENCE TIME"); return; + } } else { - if ( mSensTimestampSync && mGrabActive) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS ) - return; - } else if ( !mSensTimestampSync ) { - if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS ) - return; - } else { + if( mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::CURRENT) != sl::ERROR_CODE::SUCCESS ) { + NODELET_DEBUG("Not retrieved sensors data in CURRENT REFERENCE TIME"); return; } } - if (mSvoMode) { + if(mSvoMode) { ts_imu = ros::Time::now(); ts_baro = ros::Time::now(); ts_mag = ros::Time::now(); //ts_mag_raw = ros::Time::now(); } else { - if (mSensTimestampSync && mGrabActive) { - ts_imu = mFrameTimestamp; - ts_baro = mFrameTimestamp; - ts_mag = mFrameTimestamp; - //ts_mag_raw = mFrameTimestamp; + if(t!=ros::Time(0)) { + ts_imu = t; + ts_baro = t; + ts_mag = t; } else { ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); - //ts_mag_raw = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); } } + // ----> Publish odometry tf only if enabled + if (mPublishTf && mTrackingReady) { + NODELET_DEBUG("Publishing TF"); + + publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame + + if (mPublishMapTf) { + publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame + } + + if(mPublishImuTf && !mStaticImuFramePublished ) + { + publishStaticImuFrame(); + } + } + // <---- Publish odometry tf only if enabled + bool new_imu_data = ts_imu!=lastTs_imu; bool new_baro_data = ts_baro!=lastTs_baro; bool new_mag_data = ts_mag!=lastT_mag; if( !new_imu_data && !new_baro_data && !new_mag_data) { + NODELET_DEBUG("No updated sensors data"); return; } @@ -2659,13 +2678,9 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); } - if(totSub<1 && !mPublishImuTf) { - return; // Nothing to publish - } - if( imu_SubNumber > 0 || imu_RawSubNumber > 0 || imu_TempSubNumber > 0 || pressSubNumber > 0 || - imu_MagSubNumber > 0 /*|| imu_MagRawSubNumber > 0*/ ) { + imu_MagSubNumber > 0 ) { // Publish freq calculation static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); @@ -2700,8 +2715,11 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { imuTempMsg->variance = 0.0; mPubImuTemp.publish(imuTempMsg); + } else { + NODELET_DEBUG("No new IMU temp."); } + if( sens_data.barometer.is_available && new_baro_data ) { lastTs_baro = ts_baro; @@ -2763,6 +2781,8 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { mPubTempR.publish(tempRightMsg); } + } else { + NODELET_DEBUG("No new BAROM. DATA"); } if( imu_MagSubNumber>0) { @@ -2797,34 +2817,10 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { mPubImuMag.publish(magMsg); } + } else { + NODELET_DEBUG("No new MAG. DATA"); } - // if( imu_MagRawSubNumber>0 ) { - // if( sens_data.magnetometer.is_available && lastT_mag_raw != ts_mag_raw ) { - // lastT_mag_raw = ts_mag_raw; - - // sensor_msgs::MagneticFieldPtr mMagRawMsg = boost::make_shared(); - - - // mMagRawMsg->header.stamp = ts_mag; - // mMagRawMsg->header.frame_id = mImuFrameId; - // mMagRawMsg->magnetic_field.x = sens_data.magnetometer.magnetic_field_uncalibrated.x*1e-6; // Tesla - // mMagRawMsg->magnetic_field.y = sens_data.magnetometer.magnetic_field_uncalibrated.y*1e-6; // Tesla - // mMagRawMsg->magnetic_field.z = sens_data.magnetometer.magnetic_field_uncalibrated.z*1e-6; // Tesla - // mMagRawMsg->magnetic_field_covariance[0] = 0.039e-6; - // mMagRawMsg->magnetic_field_covariance[1] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[2] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[3] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[4] = 0.037e-6; - // mMagRawMsg->magnetic_field_covariance[5] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[6] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[7] = 0.0f; - // mMagRawMsg->magnetic_field_covariance[8] = 0.047e-6; - - // mPubImuMagRaw.publish(mMagRawMsg); - // } - // } - if( imu_SubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; @@ -2892,9 +2888,9 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } - if(imu_SubNumber > 0) { - mPubImu.publish(imuMsg); - } + mPubImu.publish(imuMsg); + } else { + NODELET_DEBUG("No new IMU DATA"); } if (imu_RawSubNumber > 0 && new_imu_data) { @@ -2935,85 +2931,14 @@ void ZEDWrapperNodelet::callback_pubSens(const ros::TimerEvent& e) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } - imuRawMsg->orientation_covariance[0] = - -1; // Orientation data is not available in "data_raw" -> See ROS REP145 + // Orientation data is not available in "data_raw" -> See ROS REP145 // http://www.ros.org/reps/rep-0145.html#topics + imuRawMsg->orientation_covariance[0] = + -1; mPubImuRaw.publish(imuRawMsg); } - - if(new_imu_data && mPublishImuTf) { - // Publish odometry tf only if enabled - if (mPublishTf) { - if(!mTrackingReady) { - return; - } - - publishOdomFrame(mOdom2BaseTransf, ts_imu); // publish the base Frame in odometry frame - - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } - - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } - } - } - - // // Publish IMU tf - // // Left camera to odom transform from TF buffer - // tf2::Transform cam_to_odom; - - // //std::string poseFrame; - // static bool first_error = false; - - // // Look up the transformation from imu frame to odom link - // try { - // // Save the transformation from base to frame - // geometry_msgs::TransformStamped c2o = - // mTfBuffer->lookupTransform(mOdometryFrameId, mCameraFrameId, ros::Time(0), ros::Duration(0.1)); - // // Get the TF2 transformation - // tf2::fromMsg(c2o.transform, cam_to_odom); - // } catch (tf2::TransformException& ex) { - // if(!first_error) { - // NODELET_DEBUG_THROTTLE(1.0, "Transform error: %s", ex.what()); - // NODELET_WARN_THROTTLE(1.0, "The tf from '%s' to '%s' is not available.", - // mCameraFrameId.c_str(), mMapFrameId.c_str()); - // NODELET_WARN_THROTTLE(1.0, "Note: one of the possible cause of the problem is the absense of a publisher " - // "of the base_link -> odom transform. " - // "This happens if `publish_tf` is `false` and no other nodes publish the " - // "TF chain '%s' -> '%s' -> '%s'", - // mOdometryFrameId.c_str(), mBaseFrameId.c_str(), mCameraFrameId.c_str()); - // first_error=false; - // } - - // return; - // } - - // // ----> Update IMU pose for TF - - // // IMU Quaternion in Map frame - // tf2::Quaternion imu_q; - // imu_q.setX(sens_data.imu.pose.getOrientation()[0]); - // imu_q.setY(sens_data.imu.pose.getOrientation()[1]); - // imu_q.setZ(sens_data.imu.pose.getOrientation()[2]); - // imu_q.setW(sens_data.imu.pose.getOrientation()[3]); - - // // Pose Quaternion from ZED Camera - // tf2::Quaternion odom_q = cam_to_odom.getRotation(); - // // Difference between IMU and ZED Quaternion - // tf2::Quaternion delta_q = imu_q * odom_q.inverse(); - - // mLastImuPose.setIdentity(); - // mLastImuPose.setRotation(delta_q); - - // publishImuFrame(mLastImuPose, ts_imu); - // // <---- Update IMU pose for TF - // } } - void ZEDWrapperNodelet::device_poll_thread_func() { ros::Rate loop_rate(mCamFrameRate); @@ -3162,21 +3087,23 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } // ----> Wait for RGB/Depth synchronization before grabbing - std::unique_lock datalock(mCamDataMutex); - while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups - if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { - // Check thread stopping - if (mStopNode) { - return; - } else { - continue; - } - } - } + // std::unique_lock datalock(mCamDataMutex); + // while (!mRgbDepthDataRetrieved) { // loop to avoid spurious wakeups + // if (mRgbDepthDataRetrievedCondVar.wait_for(datalock, std::chrono::milliseconds(500)) == std::cv_status::timeout) { + // // Check thread stopping + // if (mStopNode) { + // return; + // } else { + // continue; + // } + // } + // } // <---- Wait for RGB/Depth synchronization before grabbing + mCamDataMutex.lock(); mRgbDepthDataRetrieved = false; mGrabStatus = mZed.grab(runParams); + mCamDataMutex.unlock(); std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); @@ -3609,7 +3536,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() { oldStatus = mPosTrackingStatus; } - if(mZedRealCamModel == sl::MODEL::ZED || !mPublishImuTf) { + if(mZedRealCamModel == sl::MODEL::ZED) { // Publish pose tf only if enabled if(mPublishTf) { // Note, the frame is published, but its values will only change if @@ -3621,11 +3548,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { // someone has subscribed to map publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame } - - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } } } @@ -3817,7 +3739,7 @@ void ZEDWrapperNodelet::callback_updateDiagnostic(diagnostic_updater::Diagnostic stat.add("IMU", "Topics not subscribed"); } - if( mSensPubRate > 0 && mZedRealCamModel == sl::MODEL::ZED2 ) { + if( mZedRealCamModel == sl::MODEL::ZED2 ) { stat.addf("Left CMOS Temp.", "%.1f °C", mTempLeft); stat.addf("Right CMOS Temp.", "%.1f °C", mTempRight); diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 2122cde3..816a71d4 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -13,7 +13,7 @@ pos_tracking: imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. sensors: - sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame + sensors_timestamp_sync: true # Synchronize Sensors messages timestamp with latest received frame publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: From 7a761bb4207798b637101421a8c3b12508570596 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Oct 2020 11:16:02 +0100 Subject: [PATCH 5/8] Fix Sensors data freq diagnostic info --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 48 ++++++++++++------- zed_wrapper/params/zed2.yaml | 2 +- 2 files changed, 32 insertions(+), 18 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 897b509e..571f75d5 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2607,6 +2607,17 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { tempRightSubNumber = mPubTempR.getNumSubscribers(); } + uint32_t tot_sub = imu_SubNumber+imu_RawSubNumber+imu_TempSubNumber+imu_MagSubNumber+pressSubNumber+ + tempLeftSubNumber+tempRightSubNumber; + + if(tot_sub>0) { + mSensPublishing=true; + } else { + mSensPublishing=false; + } + + bool sensors_data_published = false; + ros::Time ts_imu; ros::Time ts_baro; ros::Time ts_mag; @@ -2678,23 +2689,6 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { sens_data.temperature.get( sl::SensorsData::TemperatureData::SENSOR_LOCATION::ONBOARD_RIGHT, mTempRight); } - if( imu_SubNumber > 0 || imu_RawSubNumber > 0 || - imu_TempSubNumber > 0 || pressSubNumber > 0 || - imu_MagSubNumber > 0 ) { - // Publish freq calculation - static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); - std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); - - double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); - last_time = now; - - mSensPeriodMean_usec->addValue(elapsed_usec); - - mSensPublishing = true; - } else { - mSensPublishing = false; - } - if (imu_TempSubNumber>0) { sensor_msgs::TemperaturePtr imuTempMsg = boost::make_shared(); @@ -2714,6 +2708,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { imuTempMsg->temperature = static_cast(imu_temp); imuTempMsg->variance = 0.0; + sensors_data_published = true; mPubImuTemp.publish(imuTempMsg); } else { NODELET_DEBUG("No new IMU temp."); @@ -2739,6 +2734,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal pressMsg->variance = 1.0585e-2; + sensors_data_published = true; mPubPressure.publish(pressMsg); } @@ -2759,6 +2755,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { tempLeftMsg->temperature = static_cast(mTempLeft); tempLeftMsg->variance = 0.0; + sensors_data_published = true; mPubTempL.publish(tempLeftMsg); } @@ -2779,6 +2776,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { tempRightMsg->temperature = static_cast(mTempRight); tempRightMsg->variance = 0.0; + sensors_data_published = true; mPubTempR.publish(tempRightMsg); } } else { @@ -2815,6 +2813,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { magMsg->magnetic_field_covariance[7] = 0.0f; magMsg->magnetic_field_covariance[8] = 0.047e-6; + sensors_data_published = true; mPubImuMag.publish(magMsg); } } else { @@ -2888,6 +2887,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } + sensors_data_published = true; mPubImu.publish(imuMsg); } else { NODELET_DEBUG("No new IMU DATA"); @@ -2935,8 +2935,22 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { // http://www.ros.org/reps/rep-0145.html#topics imuRawMsg->orientation_covariance[0] = -1; + sensors_data_published = true; mPubImuRaw.publish(imuRawMsg); } + + // ----> Update Diagnostic + if( sensors_data_published ) { + // Publish freq calculation + static std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now(); + std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now(); + + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; + + mSensPeriodMean_usec->addValue(elapsed_usec); + } + // <---- Update Diagnostic } void ZEDWrapperNodelet::device_poll_thread_func() { diff --git a/zed_wrapper/params/zed2.yaml b/zed_wrapper/params/zed2.yaml index 816a71d4..2122cde3 100644 --- a/zed_wrapper/params/zed2.yaml +++ b/zed_wrapper/params/zed2.yaml @@ -13,7 +13,7 @@ pos_tracking: imu_fusion: true # enable/disable IMU fusion. When set to false, only the optical odometry will be used. sensors: - sensors_timestamp_sync: true # Synchronize Sensors messages timestamp with latest received frame + sensors_timestamp_sync: false # Synchronize Sensors messages timestamp with latest received frame publish_imu_tf: true # publish `IMU -> _left_camera_frame` TF object_detection: From 170723d9c8240af5652ab59311b409933e3fbee4 Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Oct 2020 11:42:42 +0100 Subject: [PATCH 6/8] Fix Sensors pub freq when Camera/Sensors sync is active --- .../src/zed_nodelet/src/zed_wrapper_nodelet.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index 571f75d5..e1c013db 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -534,7 +534,7 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } - if(!mSvoMode) { + if(!mSvoMode&&!mSensTimestampSync) { mFrameTimestamp = ros::Time::now(); mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate*1.5) ), &ZEDWrapperNodelet::callback_pubSensorsData, this); @@ -2281,6 +2281,11 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { uint32_t stereoSubNumber = mPubStereo.getNumSubscribers(); uint32_t stereoRawSubNumber = mPubRawStereo.getNumSubscribers(); + uint32_t tot_sub = rgbSubnumber+rgbRawSubnumber+leftSubnumber+leftRawSubnumber+rightSubnumber+rightRawSubnumber+ + rgbGraySubnumber+rgbGrayRawSubnumber+leftGraySubnumber+leftGrayRawSubnumber+ + rightGraySubnumber+rightGrayRawSubnumber+depthSubnumber+disparitySubnumber+confMapSubnumber+ + stereoSubNumber+stereoRawSubNumber; + bool retrieved = false; sl::Mat mat_left,mat_left_raw; @@ -2353,7 +2358,7 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { retrieved = true; grab_ts=mat_disp.timestamp; } - if(confMapSubnumber) { + if(confMapSubnumber>0) { mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); retrieved = true; grab_ts=mat_conf.timestamp; @@ -2371,7 +2376,9 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { if( mZedRealCamModel!=sl::MODEL::ZED ) { if(mSensTimestampSync || mSvoMode) { - if(retrieved) { + //NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); + if(tot_sub>0 && retrieved && (grab_ts.data_ns!=lastZedTs.data_ns)) { + //NODELET_INFO("CALLBACK"); publishSensData(stamp); } } @@ -2578,7 +2585,7 @@ void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { } void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { - NODELET_DEBUG("callback_pubSensorsData"); + //NODELET_INFO("callback_pubSensorsData"); std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { @@ -2589,7 +2596,7 @@ void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { } void ZEDWrapperNodelet::publishSensData(ros::Time t) { - NODELET_DEBUG("publishSensData"); + //NODELET_INFO("publishSensData"); uint32_t imu_SubNumber = mPubImu.getNumSubscribers(); uint32_t imu_RawSubNumber = mPubImuRaw.getNumSubscribers(); From e88122d26e61217338a6606d8e2bfd183bd830ec Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Wed, 28 Oct 2020 12:08:42 +0100 Subject: [PATCH 7/8] Fix sensors data timestamp with SVO --- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 25 ++++++++----------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp index e1c013db..990d594e 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -2375,12 +2375,14 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) { // ----> Publish sensor data if sync is required by user or SVO if( mZedRealCamModel!=sl::MODEL::ZED ) { - if(mSensTimestampSync || mSvoMode) { + if(mSensTimestampSync) { //NODELET_INFO_STREAM("tot_sub: " << tot_sub << " - retrieved: " << retrieved << " - grab_ts.data_ns!=lastZedTs.data_ns: " << (grab_ts.data_ns!=lastZedTs.data_ns)); if(tot_sub>0 && retrieved && (grab_ts.data_ns!=lastZedTs.data_ns)) { //NODELET_INFO("CALLBACK"); publishSensData(stamp); } + } else if(mSvoMode) { + publishSensData(stamp); } } // <---- Publish sensor data if sync is required by user or SVO @@ -2647,21 +2649,14 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) { } } - if(mSvoMode) { - ts_imu = ros::Time::now(); - ts_baro = ros::Time::now(); - ts_mag = ros::Time::now(); - //ts_mag_raw = ros::Time::now(); + if(t!=ros::Time(0)) { + ts_imu = t; + ts_baro = t; + ts_mag = t; } else { - if(t!=ros::Time(0)) { - ts_imu = t; - ts_baro = t; - ts_mag = t; - } else { - ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); - ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); - ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); - } + ts_imu = sl_tools::slTime2Ros(sens_data.imu.timestamp); + ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); + ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); } // ----> Publish odometry tf only if enabled From a52af066198c0f2c585235984412d6b687b7354c Mon Sep 17 00:00:00 2001 From: Walter Lucetti Date: Mon, 2 Nov 2020 17:22:02 +0100 Subject: [PATCH 8/8] Update latest_changes.md --- latest_changes.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/latest_changes.md b/latest_changes.md index 0ad18d89..a7d97ed7 100644 --- a/latest_changes.md +++ b/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