Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Optionally use stored timestamps when publishing from svo files #763

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Optionally use stored timestamps when publishing from svo files #763

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions zed_nodelets/CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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