Skip to content

Commit

Permalink
Fix timestamp (#570)
Browse files Browse the repository at this point in the history 
* Fix IMU link
 * IMU link published even if `publish_tf` and `publish_map_tf` are set
to `false`

* Minor fix

* Improve MR #557 about problems with timestamp coherency

* Update changelog
  • Loading branch information
@Myzhar
Myzhar authored Jun 3, 2020
1 parent 98702be commit 636f019
Show file tree
Hide file tree
Showing 4 changed files with 56 additions and 39 deletions.
4 changes: 4 additions & 0 deletions latest_changes.md
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@
LATEST CHANGES
==============

Timestamp fix (2020-06-03)
--------------------------
- Fix timestamp update coherency due to parallel threads. Thanks to @matlabbe

IMU fix (2020-05-24)
--------------------
- Fix issue with IMU frame link when `publish_tf` and `publish_map_tf` are disabled
Expand Down
1 change: 1 addition & 0 deletions zed_nodelets/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ catkin_package(
zed_interfaces
)


###############################################################################
# SOURCES

Expand Down
2 changes: 1 addition & 1 deletion zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -367,7 +367,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet {

/* \brief Perform object detection and publish result
*/
void detectObjects(bool publishObj, bool publishViz);
void detectObjects(bool publishObj, bool publishViz, ros::Time t);

/* \brief Generates an univoque color for each object class ID
*/
Expand Down
88 changes: 50 additions & 38 deletions zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1505,7 +1505,7 @@ void ZEDWrapperNodelet::publishPose(ros::Time t) {
}

std_msgs::Header header;
header.stamp = mFrameTimestamp;
header.stamp = t;
header.frame_id = mMapFrameId;
geometry_msgs::Pose pose;

Expand Down Expand Up @@ -1813,7 +1813,7 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) {
return;
}

pointcloudFusedMsg->header.stamp = mFrameTimestamp;
//pointcloudFusedMsg->header.stamp = t;
mZed.requestSpatialMapAsync();

while (mZed.getSpatialMapRequestStatusAsync() == sl::ERROR_CODE::FAILURE) {
Expand All @@ -1833,7 +1833,7 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) {

if (pointcloudFusedMsg->width != ptsCount || pointcloudFusedMsg->height != 1) {
// Initialize Point Cloud message
// https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
// https://github.com/ros/common_msgs/blob/jade-devel/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h
pointcloudFusedMsg->header.frame_id = mMapFrameId; // Set the header values of the ROS message
pointcloudFusedMsg->is_bigendian = false;
pointcloudFusedMsg->is_dense = false;
Expand All @@ -1854,12 +1854,15 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) {

//NODELET_INFO_STREAM("Chunks: " << mFusedPC.chunks.size());



int index = 0;
float* ptCloudPtr = (float*)(&pointcloudFusedMsg->data[0]);
int updated = 0;

for (int c = 0; c < mFusedPC.chunks.size(); c++) {
if (mFusedPC.chunks[c].has_been_updated || resized) {

updated++;

size_t chunkSize = mFusedPC.chunks[c].vertices.size();
Expand All @@ -1871,6 +1874,8 @@ void ZEDWrapperNodelet::pubFusedPointCloudCallback(const ros::TimerEvent& e) {
memcpy(ptCloudPtr, cloud_pts, 4 * chunkSize * sizeof(float));

ptCloudPtr += 4 * chunkSize;

pointcloudFusedMsg->header.stamp = sl_tools::slTime2Ros(mFusedPC.chunks[c].timestamp);
}

} else {
Expand Down Expand Up @@ -1971,6 +1976,7 @@ void ZEDWrapperNodelet::fillCamInfo(sl::Camera& zed, sensor_msgs::CameraInfoPtr
#else
if (rawParam) {
if(mUseOldExtrinsic) { // Camera frame (Z forward, Y down, X right)

std::vector<float> R_ = sl_tools::convertRodrigues(zedParam.R);
float* p = R_.data();

Expand Down Expand Up @@ -2245,7 +2251,7 @@ void ZEDWrapperNodelet::dynamicReconfCallback(zed_nodelets::ZedConfig& config, u
}

void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
static sl::Timestamp lastTs = 0;
static sl::Timestamp lastZedTs = 0; // Used to calculate stable publish frequency

uint32_t rgbSubnumber = mPubRgb.getNumSubscribers();
uint32_t rgbRawSubnumber = mPubRawRgb.getNumSubscribers();
Expand Down Expand Up @@ -2275,7 +2281,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
stereoSubNumber+stereoRawSubNumber == 0) {

mPublishingData = false;
lastTs = 0;
lastZedTs = 0;
return;
}

Expand All @@ -2284,35 +2290,36 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
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==lastTs) {
if(leftZEDMat.timestamp==lastZedTs) {
return; // No new image!
}
if(lastTs.data_ns!=0) {
double period_sec = static_cast<double>(leftZEDMat.timestamp.data_ns - lastTs.data_ns)/1e9;
if(lastZedTs.data_ns!=0) {
double period_sec = static_cast<double>(leftZEDMat.timestamp.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") ;
}
lastTs = leftZEDMat.timestamp;
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<sensor_msgs::Image>();

publishImage(leftImgMsg, leftZEDMat, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, mFrameTimestamp);
publishImage(leftImgMsg, leftZEDMat, mPubLeft, mLeftCamInfoMsg, mLeftCamOptFrameId, stamp);
}

if (rgbSubnumber > 0) {
sensor_msgs::ImagePtr rgbImgMsg = boost::make_shared<sensor_msgs::Image>();

// rgb is the left image
publishImage(rgbImgMsg, leftZEDMat, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, mFrameTimestamp);
publishImage(rgbImgMsg, leftZEDMat, mPubRgb, mRgbCamInfoMsg, mDepthOptFrameId, stamp);

}
}
Expand All @@ -2327,14 +2334,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
sensor_msgs::ImagePtr leftGrayImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(leftGrayImgMsg, leftGrayZEDMat, mPubLeftGray, mLeftCamInfoMsg, mLeftCamOptFrameId,
mFrameTimestamp);
stamp);
}

if (rgbGraySubnumber > 0) {
sensor_msgs::ImagePtr rgbGrayImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rgbGrayImgMsg, leftGrayZEDMat, mPubRgbGray, mRgbCamInfoMsg, mDepthOptFrameId,
mFrameTimestamp); // rgb is the left image
stamp); // rgb is the left image
}
}

Expand All @@ -2348,14 +2355,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
sensor_msgs::ImagePtr rawLeftImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rawLeftImgMsg, leftZEDMat, mPubRawLeft, mLeftCamInfoRawMsg, mLeftCamOptFrameId,
mFrameTimestamp);
stamp);
}

if (rgbRawSubnumber > 0) {
sensor_msgs::ImagePtr rawRgbImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rawRgbImgMsg, leftZEDMat, mPubRawRgb, mRgbCamInfoRawMsg, mDepthOptFrameId,
mFrameTimestamp);
stamp);
}
}

Expand All @@ -2369,14 +2376,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
sensor_msgs::ImagePtr rawLeftGrayImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rawLeftGrayImgMsg, leftGrayZEDMat, mPubRawLeftGray, mLeftCamInfoRawMsg, mLeftCamOptFrameId,
mFrameTimestamp);
stamp);
}

if (rgbGrayRawSubnumber > 0) {
sensor_msgs::ImagePtr rawRgbGrayImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rawRgbGrayImgMsg, leftGrayZEDMat, mPubRawRgbGray, mRgbCamInfoRawMsg, mDepthOptFrameId,
mFrameTimestamp);
stamp);
}
}

Expand All @@ -2388,7 +2395,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr rightImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rightImgMsg, rightZEDMat, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, mFrameTimestamp);
publishImage(rightImgMsg, rightZEDMat, mPubRight, mRightCamInfoMsg, mRightCamOptFrameId, stamp);
}

// Publish the right image GRAY if someone has subscribed to
Expand All @@ -2398,7 +2405,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {
mZed.retrieveImage(rightGrayZEDMat, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU, mMatResolVideo);
sensor_msgs::ImagePtr rightGrayImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rightGrayImgMsg, rightGrayZEDMat, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, mFrameTimestamp);
publishImage(rightGrayImgMsg, rightGrayZEDMat, mPubRightGray, mRightCamInfoMsg, mRightCamOptFrameId, stamp);
}

// Publish the right raw image if someone has subscribed to
Expand All @@ -2409,7 +2416,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr rawRightImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rawRightImgMsg, rightZEDMat, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, mFrameTimestamp);
publishImage(rawRightImgMsg, rightZEDMat, mPubRawRight, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp);
}

// Publish the right raw image GRAY if someone has subscribed to
Expand All @@ -2419,7 +2426,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr rawRightGrayImgMsg = boost::make_shared<sensor_msgs::Image>();

publishImage(rawRightGrayImgMsg, rightGrayZEDMat, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, mFrameTimestamp);
publishImage(rawRightGrayImgMsg, rightGrayZEDMat, mPubRawRightGray, mRightCamInfoRawMsg, mRightCamOptFrameId, stamp);
}

// Stereo couple side-by-side
Expand All @@ -2431,7 +2438,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr stereoImgMsg = boost::make_shared<sensor_msgs::Image>();

sl_tools::imagesToROSmsg(stereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, mFrameTimestamp);
sl_tools::imagesToROSmsg(stereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp);

mPubStereo.publish(stereoImgMsg);
}
Expand All @@ -2445,7 +2452,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr rawStereoImgMsg = boost::make_shared<sensor_msgs::Image>();

sl_tools::imagesToROSmsg(rawStereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, mFrameTimestamp);
sl_tools::imagesToROSmsg(rawStereoImgMsg, leftZEDMat, rightZEDMat, mCameraFrameId, stamp);

mPubRawStereo.publish(rawStereoImgMsg);
}
Expand All @@ -2457,14 +2464,14 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr depthImgMsg = boost::make_shared<sensor_msgs::Image>();

publishDepth(depthImgMsg, depthZEDMat, mFrameTimestamp); // in meters
publishDepth(depthImgMsg, depthZEDMat, stamp); // in meters
}

// Publish the disparity image if someone has subscribed to
if (disparitySubnumber > 0) {

mZed.retrieveMeasure(disparityZEDMat, sl::MEASURE::DISPARITY, sl::MEM::CPU, mMatResolDepth);
publishDisparity(disparityZEDMat, mFrameTimestamp);
publishDisparity(disparityZEDMat, stamp);
}

// Publish the confidence map if someone has subscribed to
Expand All @@ -2474,7 +2481,7 @@ void ZEDWrapperNodelet::pubVideoDepthCallback(const ros::TimerEvent& e) {

sensor_msgs::ImagePtr confMapMsg = boost::make_shared<sensor_msgs::Image>();

sl_tools::imageToROSmsg(confMapMsg, confMapZEDMat, mConfidenceOptFrameId, mFrameTimestamp);
sl_tools::imageToROSmsg(confMapMsg, confMapZEDMat, mConfidenceOptFrameId, stamp);

mPubConfMap.publish(confMapMsg);
}
Expand Down Expand Up @@ -2813,7 +2820,7 @@ void ZEDWrapperNodelet::pubSensCallback(const ros::TimerEvent& e) {
if (imu_RawSubNumber > 0) {
sensor_msgs::ImuPtr imuRawMsg = boost::make_shared<sensor_msgs::Imu>();

imuRawMsg->header.stamp = ts_imu; // t;
imuRawMsg->header.stamp = ts_imu;
imuRawMsg->header.frame_id = mImuFrameId;
imuRawMsg->angular_velocity.x = sens_data.imu.angular_velocity[0] * DEG2RAD;
imuRawMsg->angular_velocity.y = sens_data.imu.angular_velocity[1] * DEG2RAD;
Expand Down Expand Up @@ -3038,7 +3045,11 @@ void ZEDWrapperNodelet::device_poll_thread_func() {

if (mComputeDepth) {
runParams.confidence_threshold = mCamDepthConfidence;
#if ZED_SDK_MAJOR_VERSION==3 && ZED_SDK_MINOR_VERSION<2
runParams.textureness_confidence_threshold = mCamDepthTextureConf;
#else
runParams.texture_confidence_threshold = mCamDepthTextureConf;
#endif
runParams.enable_depth = true; // Ask to compute the depth
} else {
runParams.enable_depth = false; // Ask to not compute the depth
Expand Down Expand Up @@ -3148,6 +3159,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE));
}

ros::Time stamp = mFrameTimestamp; // Timestamp

// ----> Camera Settings
if( !mSvoMode && mFrameCount%5 == 0 ) {
//NODELET_DEBUG_STREAM( "[" << mFrameCount << "] device_poll_thread_func MUTEX LOCK");
Expand Down Expand Up @@ -3255,7 +3268,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
mZed.retrieveMeasure(mCloud, sl::MEASURE::XYZBGRA, sl::MEM::CPU, mMatResolDepth);

mPointCloudFrameId = mDepthFrameId;
mPointCloudTime = mFrameTimestamp;
mPointCloudTime = stamp;

// Signal Pointcloud thread that a new pointcloud is ready
mPcDataReadyCondVar.notify_one();
Expand All @@ -3270,7 +3283,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
mObjDetMutex.lock();
if (mObjDetRunning) {
std::chrono::steady_clock::time_point start = std::chrono::steady_clock::now();
detectObjects(objDetSubnumber > 0, objDetVizSubnumber > 0);
detectObjects(objDetSubnumber > 0, objDetVizSubnumber > 0, stamp);
std::chrono::steady_clock::time_point now = std::chrono::steady_clock::now();

double elapsed_msec = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
Expand Down Expand Up @@ -3355,7 +3368,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() {

// Publish odometry message
if (odomSubnumber > 0) {
publishOdom(mOdom2BaseTransf, deltaOdom, mFrameTimestamp);
publishOdom(mOdom2BaseTransf, deltaOdom, stamp);
}

mTrackingReady = true;
Expand Down Expand Up @@ -3469,7 +3482,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() {

// Publish Pose message
if ((poseSubnumber + poseCovSubnumber) > 0) {
publishPose(mFrameTimestamp);
publishPose(stamp);
}

mTrackingReady = true;
Expand All @@ -3482,12 +3495,12 @@ void ZEDWrapperNodelet::device_poll_thread_func() {
if (mPublishTf) {
// Note, the frame is published, but its values will only change if
// someone has subscribed to odom
publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame
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, mFrameTimestamp); // publish the odometry Frame in map frame
publishPoseFrame(mMap2OdomTransf, stamp); // publish the odometry Frame in map frame
}
}

Expand Down Expand Up @@ -3982,7 +3995,7 @@ bool ZEDWrapperNodelet::on_stop_object_detection(zed_interfaces::stop_object_det
return res.done;
}

void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz) {
void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz, ros::Time t) {

sl::ObjectDetectionRuntimeParameters objectTracker_parameters_rt;
objectTracker_parameters_rt.detection_confidence_threshold = mObjDetConfidence;
Expand All @@ -4007,8 +4020,7 @@ void ZEDWrapperNodelet::detectObjects(bool publishObj, bool publishViz) {
objMsg.objects.resize(objCount);

std_msgs::Header header;
header.stamp = mFrameTimestamp;
//header.frame_id = mCameraFrameId;
header.stamp = t;
header.frame_id = mLeftCamFrameId;

visualization_msgs::MarkerArray objMarkersMsg;
Expand Down

0 comments on commit 636f019

Please sign in to comment.