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 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..03da5dc4 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 @@ -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; 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..990d594e 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."); @@ -481,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::pubSensCallback, 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 @@ -550,6 +533,15 @@ void ZEDWrapperNodelet::onInit() { NODELET_INFO_STREAM("Advertised on topic " << mPubCamImuTransf.getTopic() << " [LATCHED]"); } + + if(!mSvoMode&&!mSensTimestampSync) { + 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 @@ -577,7 +569,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 +1290,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 +1749,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 +1807,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 +2095,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,7 +2260,7 @@ 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(); @@ -2291,223 +2281,240 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) { 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 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; + 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; + + 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(disparitySubnumber>0) { + mZed.retrieveMeasure(mat_disp, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_disp.timestamp; + } + if(confMapSubnumber>0) { + mZed.retrieveMeasure(mat_conf, sl::MEASURE::CONFIDENCE, sl::MEM::CPU, mMatResolDepth); + retrieved = true; + grab_ts=mat_conf.timestamp; + } + // <---- Retrieve all required image data + + // ----> Data ROS timestamp + ros::Time stamp = sl_tools::slTime2Ros(grab_ts); + if(mSvoMode) { + stamp = ros::Time::now(); + } + // <---- Data ROS timestamp + + // ----> Publish sensor data if sync is required by user or SVO + if( mZedRealCamModel!=sl::MODEL::ZED ) + { + 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 + + mCamDataMutex.unlock(); + + // ----> 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(); - - // 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(); - - publishImage(rawLeftGrayImgMsg, leftGrayZEDMat, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId, - stamp); - } - - if (rgbGrayRawSubnumber > 0) { - sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared(); + if (leftGrayRawSubnumber > 0) { + sensor_msgs::ImagePtr rawLeftGrayImgMsg = 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,18 +2586,24 @@ void ZEDWrapperNodelet::pubPathCallback(const ros::TimerEvent& e) { } } -void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { +void ZEDWrapperNodelet::callback_pubSensorsData(const ros::TimerEvent& e) { + //NODELET_INFO("callback_pubSensorsData"); std::lock_guard lock(mCloseZedMutex); if (!mZed.isOpened()) { return; } + publishSensData(); +} + +void ZEDWrapperNodelet::publishSensData(ros::Time t) { + //NODELET_INFO("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; @@ -2598,66 +2611,77 @@ void ZEDWrapperNodelet::pubSensCallback(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; + 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; - //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) { - 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 (mSensTimestampSync && mGrabActive) { - ts_imu = mFrameTimestamp; - ts_baro = mFrameTimestamp; - ts_mag = mFrameTimestamp; - //ts_mag_raw = mFrameTimestamp; - } 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); + 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 + 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; } @@ -2667,27 +2691,6 @@ void ZEDWrapperNodelet::pubSensCallback(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*/ ) { - // 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(); @@ -2707,9 +2710,13 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { imuTempMsg->temperature = static_cast(imu_temp); imuTempMsg->variance = 0.0; + sensors_data_published = true; mPubImuTemp.publish(imuTempMsg); + } else { + NODELET_DEBUG("No new IMU temp."); } + if( sens_data.barometer.is_available && new_baro_data ) { lastTs_baro = ts_baro; @@ -2729,6 +2736,7 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { pressMsg->fluid_pressure = sens_data.barometer.pressure * 1e-2; // Pascal pressMsg->variance = 1.0585e-2; + sensors_data_published = true; mPubPressure.publish(pressMsg); } @@ -2749,6 +2757,7 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { tempLeftMsg->temperature = static_cast(mTempLeft); tempLeftMsg->variance = 0.0; + sensors_data_published = true; mPubTempL.publish(tempLeftMsg); } @@ -2769,8 +2778,11 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { tempRightMsg->temperature = static_cast(mTempRight); tempRightMsg->variance = 0.0; + sensors_data_published = true; mPubTempR.publish(tempRightMsg); } + } else { + NODELET_DEBUG("No new BAROM. DATA"); } if( imu_MagSubNumber>0) { @@ -2803,37 +2815,14 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { magMsg->magnetic_field_covariance[7] = 0.0f; magMsg->magnetic_field_covariance[8] = 0.047e-6; + sensors_data_published = true; 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 || mPublishImuTf) && new_imu_data) { + if( imu_SubNumber > 0 && new_imu_data) { lastTs_imu = ts_imu; sensor_msgs::ImuPtr imuMsg = boost::make_shared(); @@ -2900,9 +2889,10 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) { sens_data.imu.angular_velocity_covariance.r[r * 3 + 2] * DEG2RAD * DEG2RAD; } - if(imu_SubNumber > 0) { - mPubImu.publish(imuMsg); - } + sensors_data_published = true; + mPubImu.publish(imuMsg); + } else { + NODELET_DEBUG("No new IMU DATA"); } if (imu_RawSubNumber > 0 && new_imu_data) { @@ -2943,85 +2933,28 @@ void ZEDWrapperNodelet::pubSensCallback(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; + sensors_data_published = true; 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 + // ----> 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(); - if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, ts_imu); // publish the odometry Frame in map frame - } + double elapsed_usec = std::chrono::duration_cast(now - last_time).count(); + last_time = now; - if(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); - } - } + mSensPeriodMean_usec->addValue(elapsed_usec); } - - // // 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 - // } + // <---- Update Diagnostic } - void ZEDWrapperNodelet::device_poll_thread_func() { ros::Rate loop_rate(mCamFrameRate); @@ -3169,7 +3102,24 @@ 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 + + mCamDataMutex.lock(); + mRgbDepthDataRetrieved = false; mGrabStatus = mZed.grab(runParams); + mCamDataMutex.unlock(); std::chrono::steady_clock::time_point start_elab = std::chrono::steady_clock::now(); @@ -3368,8 +3318,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 +3340,6 @@ void ZEDWrapperNodelet::device_poll_thread_func() { } else { mPcPublishing = false; } - mCamDataMutex.unlock(); mObjDetMutex.lock(); if (mObjDetRunning) { @@ -3605,25 +3552,20 @@ void ZEDWrapperNodelet::device_poll_thread_func() { oldStatus = mPosTrackingStatus; } - 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) { + 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 - // 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(mPublishImuTf && !mStaticImuFramePublished ) - { - publishStaticImuFrame(); + 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 0 //#ifndef NDEBUG // Enable for TF checking // Double check: map_to_pose must be equal to mMap2BaseTransf @@ -3729,7 +3671,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()); @@ -3813,7 +3755,7 @@ void ZEDWrapperNodelet::updateDiagnostic(diagnostic_updater::DiagnosticStatusWra 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);