From 0219d5c3943c69b32c68a040d3a676c64a4339f8 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Wed, 14 Jul 2021 12:23:49 -0700 Subject: [PATCH] Optionally publish a sim clock and use timestamps stored in svo in message headers rather than current wall time. Sim clock appears to be working, may have some odd glitches, may mess up non svo-playback modes Adjusting some of the timestamps, not sure if all pubs are using good timestamps, some of the static frames are timestamp 0 (but that's okay for static frames?), need to figure out if faster than real time playback is possible- there must be an api that allows read frames quickly, but maybe ruled out here. Make a function to centralize getting of time from the image or current time, clean up some other stamp usage Not clear on use of timestamps CURRENT vs ros::Time::now() --- zed_nodelets/CMakeLists.txt | 1 + zed_nodelets/package.xml | 1 + .../include/zed_wrapper_nodelet.hpp | 21 ++- .../zed_nodelet/src/zed_wrapper_nodelet.cpp | 128 ++++++++++++------ 4 files changed, 111 insertions(+), 40 deletions(-) diff --git a/zed_nodelets/CMakeLists.txt b/zed_nodelets/CMakeLists.txt index b7ee9438..77b08a9c 100644 --- a/zed_nodelets/CMakeLists.txt +++ b/zed_nodelets/CMakeLists.txt @@ -42,6 +42,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp image_transport rosconsole + rosgraph_msgs sensor_msgs stereo_msgs std_msgs diff --git a/zed_nodelets/package.xml b/zed_nodelets/package.xml index e0d5a7f7..3462daf3 100644 --- a/zed_nodelets/package.xml +++ b/zed_nodelets/package.xml @@ -18,6 +18,7 @@ nav_msgs roscpp rosconsole + rosgraph_msgs sensor_msgs stereo_msgs message_filters 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 6459be59..945fe859 100644 --- a/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_nodelets/src/zed_nodelet/include/zed_wrapper_nodelet.hpp @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -117,6 +118,21 @@ class ZEDWrapperNodelet : public nodelet::Nodelet */ void readParameters(); + ros::WallTimer wall_timer_; + ros::Time sim_clock_base_time; + /*! \brief Publish the current time when use_sim_time is true + * \param t : the ros::Time to send in the clock message + */ + void publishSimClock(const ros::Time& stamp); + + /*! \brief sim clock update thread + */ + void sim_clock_update(const ros::WallTimerEvent& e); + + /*! \brief get ZED image time or current time depending on params + */ + ros::Time getTimestamp(); + /*! \brief ZED camera polling thread function */ void device_poll_thread_func(); @@ -156,7 +172,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet /*! * \brief Publish IMU frame once as static TF */ - void publishStaticImuFrame(); + void publishStaticImuFrame(const ros::Time& t); /*! \brief Publish a sl::Mat image with a ros Publisher * \param imgMsgPtr : the image message to publish @@ -417,6 +433,8 @@ class ZEDWrapperNodelet : public nodelet::Nodelet image_transport::CameraPublisher mPubRightGray; image_transport::CameraPublisher mPubRawRightGray; + ros::Publisher mPubSimClock; + ros::Publisher mPubConfMap; // ros::Publisher mPubDisparity; // ros::Publisher mPubCloud; @@ -610,6 +628,7 @@ class ZEDWrapperNodelet : public nodelet::Nodelet double mCamDepthResizeFactor = 1.0; // flags + bool mUseSimTime = false; bool mTriggerAutoExposure = true; bool mTriggerAutoWB = true; bool mComputeDepth; 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 894e77d0..1231e551 100644 --- a/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_nodelets/src/zed_nodelet/src/zed_wrapper_nodelet.cpp @@ -20,6 +20,7 @@ #include "zed_wrapper_nodelet.hpp" +#include #include #include @@ -472,6 +473,11 @@ void ZEDWrapperNodelet::onInit() mPubRawStereo = it_zed.advertise(stereo_raw_topic, 1); NODELET_INFO_STREAM("Advertised on topic " << mPubRawStereo.getTopic()); + if (mUseSimTime) + { + mPubSimClock = mNhNs.advertise("/clock", 2); + } + // Confidence Map publisher mPubConfMap = mNhNs.advertise(conf_map_topic, 1); // confidence map NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic()); @@ -589,7 +595,8 @@ void ZEDWrapperNodelet::onInit() if (!mSvoMode && !mSensTimestampSync) { - mFrameTimestamp = ros::Time::now(); + // init so sensor callback will have a timestamp to work with on first publish + mFrameTimestamp = getTimestamp(); mImuTimer = mNhNs.createTimer(ros::Duration(1.0 / (mSensPubRate * 1.5)), &ZEDWrapperNodelet::callback_pubSensorsData, this); mSensPeriodMean_usec.reset(new sl_tools::CSmartMean(mSensPubRate / 2)); @@ -626,6 +633,11 @@ void ZEDWrapperNodelet::onInit() // Start pool thread mDevicePollThread = std::thread(&ZEDWrapperNodelet::device_poll_thread_func, this); + if (mUseSimTime) { + // TODO(lucasw) 0.02-0.05 are probably fine + wall_timer_ = mNhNs.createWallTimer(ros::WallDuration(0.01), &ZEDWrapperNodelet::sim_clock_update, this); + } + // Start data publishing timer mVideoDepthTimer = mNhNs.createTimer(ros::Duration(1.0 / mVideoDepthFreq), &ZEDWrapperNodelet::callback_pubVideoDepth, this); @@ -866,6 +878,9 @@ void ZEDWrapperNodelet::readParameters() mNhNs.param("svo_file", mSvoFilepath, std::string()); NODELET_INFO_STREAM(" * SVO input file: \t\t-> " << mSvoFilepath.c_str()); + mNhNs.getParam("/use_sim_time", mUseSimTime); + NODELET_INFO_STREAM(" * Use Sim Time\t\t\t-> " << mUseSimTime); + int svo_compr = 0; mNhNs.getParam("general/svo_compression", svo_compr); @@ -1776,7 +1791,7 @@ void ZEDWrapperNodelet::publishPose(ros::Time t) } } -void ZEDWrapperNodelet::publishStaticImuFrame() +void ZEDWrapperNodelet::publishStaticImuFrame(const ros::Time& stamp) { // Publish IMU TF as static TF if (!mPublishImuTf) @@ -1789,7 +1804,8 @@ void ZEDWrapperNodelet::publishStaticImuFrame() return; } - mStaticImuTransformStamped.header.stamp = ros::Time::now(); + NODELET_WARN_STREAM_ONCE("static imu transform " << stamp); + mStaticImuTransformStamped.header.stamp = stamp; mStaticImuTransformStamped.header.frame_id = mLeftCamFrameId; mStaticImuTransformStamped.child_frame_id = mImuFrameId; sl::Translation sl_tr = mSlCamImuTransf.getTranslation(); @@ -1805,7 +1821,7 @@ void ZEDWrapperNodelet::publishStaticImuFrame() // Publish transformation mStaticTransformImuBroadcaster.sendTransform(mStaticImuTransformStamped); - NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "'"); + NODELET_INFO_STREAM("Published static transform '" << mImuFrameId << "' -> '" << mLeftCamFrameId << "' " << stamp); mStaticImuFramePublished = true; } @@ -2700,8 +2716,10 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) ros::Time stamp = sl_tools::slTime2Ros(grab_ts); if (mSvoMode) { - stamp = ros::Time::now(); + // grab_ts is 0 when in svo playback mode + stamp = e.current_real; } + NODELET_INFO_STREAM_ONCE("time " << stamp); // <---- Data ROS timestamp // ----> Publish sensor data if sync is required by user or SVO @@ -2875,6 +2893,7 @@ void ZEDWrapperNodelet::callback_pubVideoDepth(const ros::TimerEvent& e) void ZEDWrapperNodelet::callback_pubPath(const ros::TimerEvent& e) { + NODELET_INFO_STREAM_ONCE("pub path " << e.current_real); uint32_t mapPathSub = mPubMapPath.getNumSubscribers(); uint32_t odomPathSub = mPubOdomPath.getNumSubscribers(); @@ -3012,6 +3031,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) sl::SensorsData sens_data; + // TODO(lucasw) is a mUseSimTime check needed in here? if (mSvoMode || mSensTimestampSync) { if (mZed.getSensorsData(sens_data, sl::TIME_REFERENCE::IMAGE) != sl::ERROR_CODE::SUCCESS) @@ -3041,6 +3061,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) ts_baro = sl_tools::slTime2Ros(sens_data.barometer.timestamp); ts_mag = sl_tools::slTime2Ros(sens_data.magnetometer.timestamp); } + // TODO(lucasw) in svo mode the above timestamps need to be set to ros::Time::now()? bool new_imu_data = ts_imu != lastTs_imu; bool new_baro_data = ts_baro != lastTs_baro; @@ -3066,7 +3087,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) if (mPublishImuTf && !mStaticImuFramePublished) { - publishStaticImuFrame(); + publishStaticImuFrame(ts_imu); } } // <---- Publish odometry tf only if enabled @@ -3370,6 +3391,51 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t) // <---- Update Diagnostic } +void ZEDWrapperNodelet::publishSimClock(const ros::Time& stamp) +{ + { + boost::posix_time::ptime posix_time = stamp.toBoost(); + const std::string iso_time_str = boost::posix_time::to_iso_extended_string(posix_time); + // NODELET_INFO_STREAM_THROTTLE(1.0, "time " << iso_time_str); + NODELET_INFO_STREAM_ONCE("time " << iso_time_str); + } + + NODELET_WARN_STREAM_ONCE("using sim clock " << stamp); + rosgraph_msgs::Clock clock; + clock.clock = stamp; + mPubSimClock.publish(clock); +} + +ros::Time ZEDWrapperNodelet::getTimestamp() +{ + ros::Time stamp; + if (mSvoMode && !mUseSimTime) + { + // TODO(lucasw) does it matter which one? + stamp = ros::Time::now(); + // mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); + NODELET_WARN_STREAM_ONCE("Using current time instead of image time " << stamp); + } + else + { + // TODO(lucasw) if no images have arrived yet, this is zero, or something else? + // In the svo file it appears to be something else, slightly in advance of the first + // frame. + stamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); + NODELET_WARN_STREAM_ONCE("Using zed image time " << stamp); + } + return stamp; +} + +void ZEDWrapperNodelet::sim_clock_update(const ros::WallTimerEvent& e) +{ + // TODO(lucasw) mutex + publishSimClock(sim_clock_base_time); + // TODO(lucasw) have ability to roll forward faster than real time + sim_clock_base_time += ros::Duration(0.001); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); +} + void ZEDWrapperNodelet::device_poll_thread_func() { ros::Rate loop_rate(mCamFrameRate); @@ -3383,14 +3449,10 @@ void ZEDWrapperNodelet::device_poll_thread_func() mObjDetPeriodMean_msec.reset(new sl_tools::CSmartMean(mCamFrameRate)); // Timestamp initialization - if (mSvoMode) - { - mFrameTimestamp = ros::Time::now(); - } - else - { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } + mFrameTimestamp = getTimestamp(); + + // TODO(lucasw) mutex + sim_clock_base_time = mFrameTimestamp; mPrevFrameTimestamp = mFrameTimestamp; @@ -3429,6 +3491,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() sl::RuntimeParameters runParams; runParams.sensing_mode = static_cast(mCamSensingMode); + // TODO(lucasw) is there a call to mZed in here that rolls it forward a frame in mSvoMode, or does it + // have a timer and keeps time independently, can only do real time? // Main loop while (mNhNs.ok()) { @@ -3536,6 +3600,8 @@ void ZEDWrapperNodelet::device_poll_thread_func() // the zed have been disconnected) and // re-initialize the ZED + // TODO(lucasw) if the status is END OF SVO FILE REACHED then loop it optionally, + // or exit. NODELET_INFO_STREAM_ONCE(toString(mGrabStatus)); std::this_thread::sleep_for(std::chrono::milliseconds(1)); @@ -3631,18 +3697,10 @@ void ZEDWrapperNodelet::device_poll_thread_func() mGrabPeriodMean_usec->addValue(elapsed_usec); // NODELET_INFO_STREAM("Grab time: " << elapsed_usec / 1000 << " msec"); + mFrameTimestamp = getTimestamp(); - // Timestamp - if (mSvoMode) - { - mFrameTimestamp = ros::Time::now(); - } - else - { - mFrameTimestamp = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::IMAGE)); - } - - ros::Time stamp = mFrameTimestamp; // Timestamp + const ros::Time stamp = mFrameTimestamp; // Timestamp + sim_clock_base_time = stamp; // ----> Camera Settings if (!mSvoMode && mFrameCount % 5 == 0) @@ -3974,7 +4032,7 @@ void ZEDWrapperNodelet::device_poll_thread_func() if (odomSubnumber > 0) { // Publish odometry message - publishOdom(mOdom2BaseTransf, mLastZedPose, mFrameTimestamp); + publishOdom(mOdom2BaseTransf, mLastZedPose, stamp); } mInitOdomWithPose = false; @@ -4098,27 +4156,19 @@ void ZEDWrapperNodelet::device_poll_thread_func() // Publish odometry tf only if enabled if (mPublishTf) { - ros::Time t; - - if (mSvoMode) - { - t = ros::Time::now(); - } - else - { - t = sl_tools::slTime2Ros(mZed.getTimestamp(sl::TIME_REFERENCE::CURRENT)); - } + // TODO(lucasw) or just ust mFrameTimestamp? + const ros::Time t = getTimestamp(); - publishOdomFrame(mOdom2BaseTransf, mFrameTimestamp); // publish the base Frame in odometry frame + publishOdomFrame(mOdom2BaseTransf, t); // publish the base Frame in odometry frame if (mPublishMapTf) { - publishPoseFrame(mMap2OdomTransf, mFrameTimestamp); // publish the odometry Frame in map frame + publishPoseFrame(mMap2OdomTransf, t); // publish the odometry Frame in map frame } if (mPublishImuTf && !mStaticImuFramePublished) { - publishStaticImuFrame(); + publishStaticImuFrame(t); } } }