Skip to content

Commit

Permalink
Optionally publish a sim clock and use timestamps stored in svo in me…
Browse files Browse the repository at this point in the history 
…ssage 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()
  • Loading branch information
@lucasw
lucasw committed Nov 4, 2021
1 parent a666d67 commit 0219d5c
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 40 deletions.
1 change: 1 addition & 0 deletions zed_nodelets/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
image_transport
rosconsole
rosgraph_msgs
sensor_msgs
stereo_msgs
std_msgs
Expand Down
1 change: 1 addition & 0 deletions zed_nodelets/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<depend>nav_msgs</depend>
<depend>roscpp</depend>
<depend>rosconsole</depend>
<depend>rosgraph_msgs</depend>
<depend>sensor_msgs</depend>
<depend>stereo_msgs</depend>
<depend>message_filters</depend>
Expand Down
21 changes: 20 additions & 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 @@ -31,6 +31,7 @@
#include <image_transport/image_transport.h>
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include <rosgraph_msgs/Clock.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/static_transform_broadcaster.h>
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
128 changes: 89 additions & 39 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 @@ -20,6 +20,7 @@

#include "zed_wrapper_nodelet.hpp"

#include <boost/date_time/posix_time/posix_time.hpp>
#include <chrono>
#include <csignal>

Expand Down Expand Up @@ -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<rosgraph_msgs::Clock>("/clock", 2);
}

// Confidence Map publisher
mPubConfMap = mNhNs.advertise<sensor_msgs::Image>(conf_map_topic, 1); // confidence map
NODELET_INFO_STREAM("Advertised on topic " << mPubConfMap.getTopic());
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -866,6 +878,9 @@ void ZEDWrapperNodelet::readParameters()
mNhNs.param<std::string>("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);

Expand Down Expand Up @@ -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)
Expand All @@ -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();
Expand All @@ -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;
}
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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();

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand All @@ -3066,7 +3087,7 @@ void ZEDWrapperNodelet::publishSensData(ros::Time t)

if (mPublishImuTf && !mStaticImuFramePublished)
{
publishStaticImuFrame();
publishStaticImuFrame(ts_imu);
}
}
// <---- Publish odometry tf only if enabled
Expand Down Expand Up @@ -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);
Expand All @@ -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;

Expand Down Expand Up @@ -3429,6 +3491,8 @@ void ZEDWrapperNodelet::device_poll_thread_func()
sl::RuntimeParameters runParams;
runParams.sensing_mode = static_cast<sl::SENSING_MODE>(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())
{
Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}
}
}
Expand Down

0 comments on commit 0219d5c

Please sign in to comment.