Skip to content

Commit

Permalink
Merge #1698: Improve TF time syncing
Browse files Browse the repository at this point in the history 
- New mode SyncFrame, syncing TF lookups within a visualization frame
- Remove experimental status of time syncing
- Simplify and document code
  • Loading branch information
@rhaschke
rhaschke committed May 27, 2022
2 parents bf85b57 + 1881e43 commit 02159c7
Show file tree
Hide file tree
Showing 7 changed files with 565 additions and 137 deletions.
110 changes: 43 additions & 67 deletions src/rviz/frame_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,32 +59,30 @@ FrameManager::~FrameManager()

void FrameManager::update()
{
boost::mutex::scoped_lock lock(cache_mutex_);
if (!pause_)
if (pause_)
return;
else
{
boost::mutex::scoped_lock lock(cache_mutex_);
cache_.clear();
}

if (!pause_)
{
switch (sync_mode_)
{
case SyncOff:
case SyncOff: // always use latest time
sync_time_ = ros::Time::now();
break;
case SyncExact:
case SyncExact: // sync to external source
// sync_time_ set via syncTime()
break;
case SyncApprox:
// adjust current time offset to sync source
current_delta_ = 0.7 * current_delta_ + 0.3 * sync_delta_;
try
{
sync_time_ = ros::Time::now() - ros::Duration(current_delta_);
}
catch (...)
{
sync_time_ = ros::Time::now();
}
// sync_delta is a sliding average of current_delta_, i.e.
// approximating the average delay of incoming sync messages w.r.t. current time
sync_delta_ = 0.7 * sync_delta_ + 0.3 * current_delta_;
// date back sync_time_ to ensure finding TFs that are as old as now() - sync_delta_
sync_time_ = ros::Time::now() - ros::Duration(sync_delta_);
break;
case SyncFrame: // sync to current time
// date back sync_time_ to ensure finding TFs that are as old as now() - sync_delta_
sync_time_ = ros::Time::now() - ros::Duration(sync_delta_);
break;
}
}
Expand All @@ -98,7 +96,6 @@ void FrameManager::setFixedFrame(const std::string& frame)
if (fixed_frame_ != frame)
{
fixed_frame_ = frame;
fixed_frame_id_ = tf_buffer_->_lookupFrameNumber(fixed_frame_);
cache_.clear();
should_emit = true;
}
Expand All @@ -119,29 +116,30 @@ void FrameManager::setSyncMode(SyncMode mode)
{
sync_mode_ = mode;
sync_time_ = ros::Time(0);
current_delta_ = 0;
sync_delta_ = 0;
current_delta_ = 0;
}

void FrameManager::syncTime(ros::Time time)
{
switch (sync_mode_)
{
case SyncOff:
case SyncFrame:
break;
case SyncExact:
sync_time_ = time;
break;
case SyncApprox:
if (time == ros::Time(0))
{
sync_delta_ = 0;
current_delta_ = 0;
return;
}
// avoid exception due to negative time
if (ros::Time::now() >= time)
if (ros::Time::now() >= time) // avoid exception due to negative time
{
sync_delta_ = (ros::Time::now() - time).toSec();
// estimate delay of sync message w.r.t. current time
current_delta_ = (ros::Time::now() - time).toSec();
}
else
{
Expand All @@ -151,48 +149,22 @@ void FrameManager::syncTime(ros::Time time)
}
}

bool FrameManager::adjustTime(const std::string& frame, ros::Time& time)
void FrameManager::adjustTime(ros::Time& time)
{
// we only need to act if we get a zero timestamp, which means "latest"
if (time != ros::Time())
{
return true;
}
return;

switch (sync_mode_)
{
case SyncOff:
break;
case SyncFrame:
case SyncExact:
case SyncApprox:
time = sync_time_;
break;
case SyncApprox:
{
// if we don't have tf info for the given timestamp, use the latest available
ros::Time latest_time;
std::string error_string;
int error_code;
if (fixed_frame_id_ == 0) // we couldn't resolve the fixed_frame_id yet
fixed_frame_id_ = tf_buffer_->_lookupFrameNumber(fixed_frame_);

error_code = tf_buffer_->_getLatestCommonTime(fixed_frame_id_, tf_buffer_->_lookupFrameNumber(frame),
latest_time, &error_string);

if (error_code != 0)
{
ROS_ERROR("Error getting latest time from frame '%s' to frame '%s': %s (Error code: %d)",
frame.c_str(), fixed_frame_.c_str(), error_string.c_str(), error_code);
return false;
}

if (latest_time > sync_time_)
{
time = sync_time_;
}
}
break;
}
return true;
}


Expand All @@ -201,10 +173,7 @@ bool FrameManager::getTransform(const std::string& frame,
Ogre::Vector3& position,
Ogre::Quaternion& orientation)
{
if (!adjustTime(frame, time))
{
return false;
}
adjustTime(time);

boost::mutex::scoped_lock lock(cache_mutex_);

Expand Down Expand Up @@ -243,10 +212,7 @@ bool FrameManager::transform(const std::string& frame,
Ogre::Vector3& position,
Ogre::Quaternion& orientation)
{
if (!adjustTime(frame, time))
{
return false;
}
adjustTime(time);

position = Ogre::Vector3::ZERO;
orientation = Ogre::Quaternion::IDENTITY;
Expand All @@ -261,7 +227,20 @@ bool FrameManager::transform(const std::string& frame,
{
tf2::doTransform(pose, pose, tf_buffer_->lookupTransform(fixed_frame_, frame, time));
}
catch (std::runtime_error& e)
catch (const tf2::ExtrapolationException& e)
{
if (sync_mode_ == SyncApprox)
return false;
// We don't have tf info for sync_time_. Reset sync_time_ to latest available time of frame
auto tf = tf_buffer_->lookupTransform(frame, frame, ros::Time());
if (sync_time_ > tf.header.stamp && tf.header.stamp != ros::Time())
{
sync_delta_ += (sync_time_ - tf.header.stamp).toSec(); // increase sync delta by observed amount
sync_time_ = tf.header.stamp;
}
return false;
}
catch (const std::runtime_error& e)
{
ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(),
fixed_frame_.c_str(), e.what());
Expand Down Expand Up @@ -292,10 +271,7 @@ bool FrameManager::frameHasProblems(const std::string& frame, ros::Time /*time*/

bool FrameManager::transformHasProblems(const std::string& frame, ros::Time time, std::string& error)
{
if (!adjustTime(frame, time))
{
return false;
}
adjustTime(time);

std::string tf_error;
bool transform_succeeded = tf_buffer_->canTransform(fixed_frame_, frame, time, &tf_error);
Expand Down
14 changes: 7 additions & 7 deletions src/rviz/frame_manager.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,10 @@ class FrameManager : public QObject
public:
enum SyncMode
{
SyncOff = 0,
SyncExact,
SyncApprox
SyncOff = 0, // use latest TF updates
SyncExact, // sync to incoming messages of a display (emitting timeSignal())
SyncApprox,
SyncFrame, // synchronize frame lookups to start of update() loop
};

/// Constructor, will create a TransformListener (and Buffer) automatically if not provided
Expand Down Expand Up @@ -229,7 +230,7 @@ class FrameManager : public QObject
void fixedFrameChanged();

private:
bool adjustTime(const std::string& frame, ros::Time& time);
void adjustTime(ros::Time& time);

template <class M>
void messageCallback(const ros::MessageEvent<M const>& msg_evt, Display* display)
Expand Down Expand Up @@ -306,7 +307,6 @@ class FrameManager : public QObject
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::string fixed_frame_;
tf2::CompactFrameID fixed_frame_id_;

bool pause_;

Expand All @@ -316,8 +316,8 @@ class FrameManager : public QObject
ros::Time sync_time_;

// used for approx. syncing
double sync_delta_;
double current_delta_;
double current_delta_; // current time delay between received sync msg's time stamp and now()
double sync_delta_; // sliding average of current_delta_, used to compute sync_time_
};

} // namespace rviz
Expand Down
82 changes: 25 additions & 57 deletions src/rviz/time_panel.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,17 +53,23 @@ TimePanel::TimePanel(QWidget* parent) : Panel(parent)
ros_time_label_ = makeTimeLabel();
ros_elapsed_label_ = makeTimeLabel();

experimental_cb_ = new QCheckBox("Experimental");
experimental_cb_->setSizePolicy(QSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum));

pause_button_ = new QPushButton("Pause");
pause_button_ = new QPushButton(QIcon::fromTheme("media-playback-pause"), "Pause");
pause_button_->setToolTip("Freeze ROS time.");
pause_button_->setCheckable(true);

sync_mode_selector_ = new QComboBox(this);
sync_mode_selector_->addItem("Off");
sync_mode_selector_->setItemData(FrameManager::SyncOff, "Display data using latest TF data",
Qt::ToolTipRole);
sync_mode_selector_->addItem("Exact");
sync_mode_selector_->setItemData(FrameManager::SyncExact, "Synchronize TF lookups to a source display",
Qt::ToolTipRole);
sync_mode_selector_->addItem("Approximate");
sync_mode_selector_->setItemData(
FrameManager::SyncApprox, "Synchronize to a source display in a smooth fashion", Qt::ToolTipRole);
sync_mode_selector_->addItem("Frame");
sync_mode_selector_->setItemData(FrameManager::SyncFrame, "Synchronize TF lookups within a frame",
Qt::ToolTipRole);
sync_mode_selector_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
sync_mode_selector_->setToolTip(
"Allows you to synchronize the ROS time and Tf transforms to a given source.");
Expand All @@ -73,41 +79,27 @@ TimePanel::TimePanel(QWidget* parent) : Panel(parent)
sync_source_selector_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
sync_source_selector_->setToolTip("Time source to use for synchronization.");

experimental_widget_ = new QWidget(this);
QHBoxLayout* experimental_layout = new QHBoxLayout(this);
experimental_layout->addWidget(pause_button_);
experimental_layout->addWidget(new QLabel("Synchronization:"));
experimental_layout->addWidget(sync_mode_selector_);
experimental_layout->addWidget(new QLabel("Source:"));
experimental_layout->addWidget(sync_source_selector_);
experimental_layout->addSpacing(20);
experimental_layout->setContentsMargins(0, 0, 20, 0);
experimental_widget_->setLayout(experimental_layout);

old_widget_ = new QWidget(this);
QHBoxLayout* old_layout = new QHBoxLayout(this);
old_layout->addWidget(new QLabel("ROS Elapsed:"));
old_layout->addWidget(ros_elapsed_label_);
old_layout->addWidget(new QLabel("Wall Time:"));
old_layout->addWidget(wall_time_label_);
old_layout->addWidget(new QLabel("Wall Elapsed:"));
old_layout->addWidget(wall_elapsed_label_);
old_layout->setContentsMargins(0, 0, 20, 0);
old_widget_->setLayout(old_layout);

QHBoxLayout* layout = new QHBoxLayout(this);
layout->addWidget(pause_button_);
layout->addSpacing(10);

layout->addWidget(new QLabel("Synchronization:"));
layout->addWidget(sync_mode_selector_);
layout->addWidget(sync_source_selector_);
layout->addSpacing(10);

layout->addWidget(experimental_widget_);
layout->addWidget(new QLabel("ROS Time:"));
layout->addWidget(ros_time_label_);
layout->addWidget(old_widget_);
layout->addStretch(100);
layout->addWidget(experimental_cb_);
layout->addWidget(new QLabel("ROS Elapsed:"));
layout->addWidget(ros_elapsed_label_);
layout->addWidget(new QLabel("Wall Time:"));
layout->addWidget(wall_time_label_);
layout->addWidget(new QLabel("Wall Elapsed:"));
layout->addWidget(wall_elapsed_label_);

layout->addStretch();
layout->setContentsMargins(11, 5, 11, 5);
this->setLayout(layout);

connect(experimental_cb_, SIGNAL(toggled(bool)), this, SLOT(experimentalToggled(bool)));
connect(pause_button_, SIGNAL(toggled(bool)), this, SLOT(pauseToggled(bool)));
connect(sync_mode_selector_, SIGNAL(activated(int)), this, SLOT(syncModeSelected(int)));
connect(sync_source_selector_, SIGNAL(activated(int)), this, SLOT(syncSourceSelected(int)));
Expand All @@ -134,18 +126,13 @@ void TimePanel::load(const Config& config)
syncModeSelected(sync_mode);
}
config.mapGetString("SyncSource", &config_sync_source_);
bool experimental = false;
config.mapGetBool("Experimental", &experimental);
experimental_cb_->setChecked(experimental);
experimentalToggled(experimental);
}

void TimePanel::save(Config config) const
{
Panel::save(config);
config.mapSetValue("SyncMode", sync_mode_selector_->currentIndex());
config.mapSetValue("SyncSource", sync_source_selector_->currentText());
config.mapSetValue("Experimental", experimental_cb_->checkState() == Qt::Checked);
}

void TimePanel::onDisplayAdded(Display* display)
Expand Down Expand Up @@ -235,25 +222,6 @@ void TimePanel::pauseToggled(bool checked)
vis_manager_->getFrameManager()->setPause(checked);
}

void TimePanel::experimentalToggled(bool checked)
{
old_widget_->setVisible(!checked);
experimental_widget_->setVisible(checked);
if (vis_manager_ && vis_manager_->getFrameManager())
{
if (!checked)
{
pauseToggled(false);
syncModeSelected(0);
}
else
{
pauseToggled(pause_button_->isChecked());
syncModeSelected(sync_mode_selector_->currentIndex());
}
}
}

void TimePanel::syncSourceSelected(int /*index*/)
{
// clear whatever was loaded from the config
Expand All @@ -264,7 +232,7 @@ void TimePanel::syncSourceSelected(int /*index*/)
void TimePanel::syncModeSelected(int mode)
{
vis_manager_->getFrameManager()->setSyncMode((FrameManager::SyncMode)mode);
sync_source_selector_->setEnabled(mode != FrameManager::SyncOff);
sync_source_selector_->setVisible(mode >= FrameManager::SyncExact && mode <= FrameManager::SyncApprox);
vis_manager_->notifyConfigChanged();
}

Expand Down
Loading

0 comments on commit 02159c7

Please sign in to comment.