From b310e16c6c69a9c007fc8758b2df3c51eb5ebb01 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 19 Jan 2022 15:11:21 +0100 Subject: [PATCH 01/10] Simplify code --- src/rviz/frame_manager.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 9c3acd808f..4623f148e2 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -59,14 +59,12 @@ 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: From 95e187de76aab1c809891c2e992a53b90bc43438 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 19 Jan 2022 15:12:53 +0100 Subject: [PATCH 02/10] New sync mode SyncFrame ... to sync TF lookups to current rendering frame --- src/rviz/frame_manager.cpp | 8 ++++++-- src/rviz/frame_manager.h | 1 + src/rviz/time_panel.cpp | 6 +++++- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 4623f148e2..744d9ee39a 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -67,10 +67,12 @@ void FrameManager::update() cache_.clear(); switch (sync_mode_) { - case SyncOff: + case SyncOff: // always use latest time + case SyncFrame: // sync to current 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 @@ -126,6 +128,7 @@ void FrameManager::syncTime(ros::Time time) switch (sync_mode_) { case SyncOff: + case SyncFrame: break; case SyncExact: sync_time_ = time; @@ -161,6 +164,7 @@ bool FrameManager::adjustTime(const std::string& frame, ros::Time& time) { case SyncOff: break; + case SyncFrame: case SyncExact: time = sync_time_; break; diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h index 2ce359f3a8..eec38cea89 100644 --- a/src/rviz/frame_manager.h +++ b/src/rviz/frame_manager.h @@ -68,6 +68,7 @@ class FrameManager : public QObject enum SyncMode { SyncOff = 0, + SyncFrame, // synchronize frame lookups to start of update() loop SyncExact, SyncApprox }; diff --git a/src/rviz/time_panel.cpp b/src/rviz/time_panel.cpp index bde802d238..ba83038efd 100644 --- a/src/rviz/time_panel.cpp +++ b/src/rviz/time_panel.cpp @@ -62,7 +62,11 @@ TimePanel::TimePanel(QWidget* parent) : Panel(parent) sync_mode_selector_ = new QComboBox(this); sync_mode_selector_->addItem("Off"); + sync_mode_selector_->addItem("Frame"); + sync_mode_selector_->setItemData(1, "Synchronize TF lookups within a frame", Qt::ToolTipRole); sync_mode_selector_->addItem("Exact"); + sync_mode_selector_->setItemData(1, "Synchronize TF lookups to an external (Display) source", + Qt::ToolTipRole); sync_mode_selector_->addItem("Approximate"); sync_mode_selector_->setSizeAdjustPolicy(QComboBox::AdjustToContents); sync_mode_selector_->setToolTip( @@ -264,7 +268,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_->setEnabled(mode > FrameManager::SyncFrame); vis_manager_->notifyConfigChanged(); } From 293bac02e093a627c37011db900c95c8ec6acdde Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 19 Jan 2022 15:09:30 +0100 Subject: [PATCH 03/10] Maintain sync mode when toggling experimental TimePanel view --- src/rviz/time_panel.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/rviz/time_panel.cpp b/src/rviz/time_panel.cpp index ba83038efd..d64c4e9188 100644 --- a/src/rviz/time_panel.cpp +++ b/src/rviz/time_panel.cpp @@ -246,15 +246,9 @@ void TimePanel::experimentalToggled(bool checked) if (vis_manager_ && vis_manager_->getFrameManager()) { if (!checked) - { pauseToggled(false); - syncModeSelected(0); - } else - { pauseToggled(pause_button_->isChecked()); - syncModeSelected(sync_mode_selector_->currentIndex()); - } } } From db5cf02bfa152e42f20607cf3b12316862483579 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 26 Jan 2022 20:50:30 +0100 Subject: [PATCH 04/10] Switch roles of sync_time_ and current_delta_ current_delta_ should denote the currently measured time offset sync_delta_ should denote the offset used to compute sync_time_ --- src/rviz/frame_manager.cpp | 25 ++++++++++--------------- src/rviz/frame_manager.h | 4 ++-- 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 744d9ee39a..44d343f9fa 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -75,16 +75,11 @@ void FrameManager::update() // 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; } } @@ -119,8 +114,8 @@ 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) @@ -136,13 +131,13 @@ void FrameManager::syncTime(ros::Time time) 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 { diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h index eec38cea89..e9def5717a 100644 --- a/src/rviz/frame_manager.h +++ b/src/rviz/frame_manager.h @@ -315,8 +315,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 From 9a8f980b78f867b445f7e4c384c1a2ca765b002e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 26 Jan 2022 13:33:22 +0100 Subject: [PATCH 05/10] SyncFrame: Adapt sync_time_ to avoid TF extrapolation In SyncFrame mode, we use ros::Time::now() for syncing, which is typically more recent than any TF stamp available, causing extrapolationn exceptions. To avoid that, we date back sync_time_ by a small amount sync_delta_, that is determined from observed lookup time differences. --- src/rviz/frame_manager.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 44d343f9fa..6f66a42d37 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -67,8 +67,7 @@ void FrameManager::update() cache_.clear(); switch (sync_mode_) { - case SyncOff: // always use latest time - case SyncFrame: // sync to current time + case SyncOff: // always use latest time sync_time_ = ros::Time::now(); break; case SyncExact: // sync to external source @@ -81,6 +80,10 @@ void FrameManager::update() // 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; } } } @@ -255,7 +258,18 @@ 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) + { + // 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()); From d880e7d04037e502f544f424f2424ef934154ab7 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 27 Jan 2022 08:08:06 +0100 Subject: [PATCH 06/10] Simplify adjustTime() There is no need to explicitly call _getLatestCommonTime(). First, this only succeeds if both frames have exactly the same time stamp. Second, lookupTransform() will throw an extrapolation exception anyway, if the lookup fails because the requested timestamp is too new. --- src/rviz/frame_manager.cpp | 47 +++++++------------------------------- src/rviz/frame_manager.h | 2 +- 2 files changed, 9 insertions(+), 40 deletions(-) diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 6f66a42d37..31b9c4c050 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -150,13 +150,11 @@ 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_) { @@ -164,32 +162,10 @@ bool FrameManager::adjustTime(const std::string& frame, ros::Time& time) 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; - 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; } @@ -198,10 +174,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_); @@ -240,10 +213,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; @@ -260,6 +230,8 @@ bool FrameManager::transform(const std::string& frame, } 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()) @@ -300,10 +272,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); diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h index e9def5717a..2ba9492044 100644 --- a/src/rviz/frame_manager.h +++ b/src/rviz/frame_manager.h @@ -228,7 +228,7 @@ class FrameManager : public QObject void fixedFrameChanged(); private: - bool adjustTime(const std::string& frame, ros::Time& time); + void adjustTime(ros::Time& time); template void messageCallback(const ros::MessageEvent& msg_evt, Display* display) From 979c1382ff843e449c8add031b20d63bb3b8a3bf Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 26 Jan 2022 14:20:58 +0100 Subject: [PATCH 07/10] TimePanel: Integrate experimental view --- src/rviz/time_panel.cpp | 67 ++++++++++------------------------------- src/rviz/time_panel.h | 6 ---- 2 files changed, 16 insertions(+), 57 deletions(-) diff --git a/src/rviz/time_panel.cpp b/src/rviz/time_panel.cpp index d64c4e9188..7142caa2fc 100644 --- a/src/rviz/time_panel.cpp +++ b/src/rviz/time_panel.cpp @@ -53,10 +53,7 @@ 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); @@ -77,41 +74,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))); @@ -138,10 +121,6 @@ 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 @@ -149,7 +128,6 @@ 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) @@ -239,19 +217,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); - else - pauseToggled(pause_button_->isChecked()); - } -} - void TimePanel::syncSourceSelected(int /*index*/) { // clear whatever was loaded from the config @@ -262,7 +227,7 @@ void TimePanel::syncSourceSelected(int /*index*/) void TimePanel::syncModeSelected(int mode) { vis_manager_->getFrameManager()->setSyncMode((FrameManager::SyncMode)mode); - sync_source_selector_->setEnabled(mode > FrameManager::SyncFrame); + sync_source_selector_->setVisible(mode > FrameManager::SyncFrame); vis_manager_->notifyConfigChanged(); } diff --git a/src/rviz/time_panel.h b/src/rviz/time_panel.h index 617f6bb570..1fcafe8cdd 100644 --- a/src/rviz/time_panel.h +++ b/src/rviz/time_panel.h @@ -62,7 +62,6 @@ protected Q_SLOTS: void pauseToggled(bool checked); void syncModeSelected(int index); void syncSourceSelected(int index); - void experimentalToggled(bool checked); /** Read time values from VisualizationManager and update displays. */ void update(); @@ -82,13 +81,8 @@ protected Q_SLOTS: /** Fill a single time label with the given time value (in seconds). */ void fillTimeLabel(QLineEdit* label, double time); - QWidget* old_widget_; - QWidget* experimental_widget_; - QString config_sync_source_; - QCheckBox* experimental_cb_; - QPushButton* pause_button_; QComboBox* sync_source_selector_; QComboBox* sync_mode_selector_; From 794d9594f03cf9ea4dc8f08ba04ce8ac252edc03 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 27 Jan 2022 07:48:18 +0100 Subject: [PATCH 08/10] Testing TF time syncing --- src/test/send_tf_timing.py | 103 +++++++++++++++ src/test/tf_sync_display.rviz | 227 ++++++++++++++++++++++++++++++++++ src/test/tf_sync_frame.rviz | 160 ++++++++++++++++++++++++ 3 files changed, 490 insertions(+) create mode 100644 src/test/send_tf_timing.py create mode 100644 src/test/tf_sync_display.rviz create mode 100644 src/test/tf_sync_frame.rviz diff --git a/src/test/send_tf_timing.py b/src/test/send_tf_timing.py new file mode 100644 index 0000000000..efaf712fe5 --- /dev/null +++ b/src/test/send_tf_timing.py @@ -0,0 +1,103 @@ +#!/usr/bin/env python + +from __future__ import print_function + +import rospy +import math +import numpy as np + +import tf2_ros +from geometry_msgs.msg import TransformStamped +from sensor_msgs import point_cloud2 +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from std_msgs.msg import Header + +width = 100 +height = 100 + + +def create_pc(t): + fields = [ + PointField("x", 0, PointField.FLOAT32, 1), + PointField("y", 4, PointField.FLOAT32, 1), + PointField("z", 8, PointField.FLOAT32, 1), + PointField("intensity", 12, PointField.FLOAT32, 1), + ] + + header = Header() + header.frame_id = "base_link" + header.stamp = rospy.Time.now() + + # concentric waves + x, y = np.meshgrid(np.linspace(-2, 2, width), np.linspace(-2, 2, height)) + z = 0.2 * np.sin(3 * np.sqrt(x ** 2 + y ** 2) - t) + points = np.array([x, y, z, z]).reshape(4, -1).T + + return point_cloud2.create_cloud(header, fields, points) + + +if __name__ == "__main__": + rospy.init_node("send_tf_v2") + + # transform identical to rot + tf_static = TransformStamped() + tf_static.header.stamp = rospy.Time.now() + tf_static.header.frame_id = "rot" + tf_static.child_frame_id = "rot2" + tf_static.transform.rotation.w = 1 + br_static = tf2_ros.StaticTransformBroadcaster() + br_static.sendTransform(tf_static) + + br = tf2_ros.TransformBroadcaster() + fast_pub = rospy.Publisher("fast", PointCloud2, queue_size=10) + slow_pub = rospy.Publisher("slow", PointCloud2, queue_size=1) + old = create_pc(0.0) + + # base_link moving linearly back and forth + tf_base = TransformStamped() + tf_base.header.frame_id = "map" + tf_base.child_frame_id = "base_link" + tf_base.transform.rotation.w = 1 + + # transform rotating about base_link + tf_rot = TransformStamped() + tf_rot.header.frame_id = "base_link" + tf_rot.child_frame_id = "rot" + tf_rot.transform.rotation.w = 1 + + tf_slow = TransformStamped() + tf_slow.header.frame_id = "base_link" + tf_slow.child_frame_id = "slow" + tf_slow.transform.translation.z = 1 + tf_slow.transform.rotation.w = 1 + + fast_rate = rospy.Rate(100) + slow_rate = rospy.Rate(1) + frequency = 0.1 + radius = 2 + while not rospy.is_shutdown(): + tf_base.header.stamp = rospy.Time.now() + t = 2 * math.pi * tf_base.header.stamp.to_sec() * frequency + tf_base.transform.translation.x = 10 * math.cos(t) + br.sendTransform(tf_base) + + tf_rot.header.stamp = tf_base.header.stamp + tf_rot.transform.translation.x = radius * math.cos(2 * t) + tf_rot.transform.translation.y = radius * math.sin(2 * t) + tf_rot.transform.translation.z = 0 + br.sendTransform(tf_rot) + + pc = create_pc(2 * t) + fast_pub.publish(pc) + fast_rate.sleep() + print(".", end="") + if slow_rate.remaining() < rospy.Duration(): + # publish slow TF + tf_slow.header.stamp = old.header.stamp + br.sendTransform(tf_slow) + # re-publish old PC + slow_pub.publish(old) + old = pc # store current PC + slow_rate.last_time = rospy.Time.now() + print("+") diff --git a/src/test/tf_sync_display.rviz b/src/test/tf_sync_display.rviz new file mode 100644 index 0000000000..34c6f7a2cd --- /dev/null +++ b/src/test/tf_sync_display.rviz @@ -0,0 +1,227 @@ +Panels: + - Class: rviz/Displays + Help Height: 128 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /TF1/Frames1 + - /fast1 + Splitter Ratio: 0.5 + Tree Height: 714 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 2 + SyncSource: fast +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: BaseLink + Radius: 0.10000000149011612 + Reference Frame: base_link + Show Trail: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Rot + Radius: 0.10000000149011612 + Reference Frame: rot + Show Trail: false + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Rot2 + Radius: 0.10000000149011612 + Reference Frame: rot + Show Trail: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: fast + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: /fast + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: slow + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /slow + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: false + Length: 1 + Name: Slow + Radius: 0.10000000149011612 + Reference Frame: slow + Show Trail: false + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 18.42949676513672 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -7.236814975738525 + Y: -5.540378570556641 + Z: -5.354213714599609 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5453979969024658 + Target Frame: base_link + Yaw: 0.6553990840911865 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1061 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000387fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000387000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000387000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000034100fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000038700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 2560 + Y: 0 diff --git a/src/test/tf_sync_frame.rviz b/src/test/tf_sync_frame.rviz new file mode 100644 index 0000000000..404aed72ac --- /dev/null +++ b/src/test/tf_sync_frame.rviz @@ -0,0 +1,160 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 839 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 1 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: BaseLink + Radius: 0.10000000149011612 + Reference Frame: base_link + Show Trail: false + Value: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Alpha: 1 + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Rot + Radius: 0.10000000149011612 + Reference Frame: rot + Show Trail: false + Value: true + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 1 + Name: Rot2 + Radius: 0.10000000149011612 + Reference Frame: rot2 + Show Trail: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 10.879761695861816 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.7853981852531433 + Target Frame: base_link + Yaw: 0.7853981852531433 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000031200fffffffb0000000800540069006d00650100000000000004500000000000000000000004c7000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 47 + Y: 25 From 686ae16edb5b1e30cd9163380497ac728fdf19d5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 3 Feb 2022 14:48:41 +0100 Subject: [PATCH 09/10] Drop fixed_frame_id_ --- src/rviz/frame_manager.cpp | 1 - src/rviz/frame_manager.h | 1 - 2 files changed, 2 deletions(-) diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 31b9c4c050..ce758fcde4 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -96,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; } diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h index 2ba9492044..2aa50a935b 100644 --- a/src/rviz/frame_manager.h +++ b/src/rviz/frame_manager.h @@ -305,7 +305,6 @@ class FrameManager : public QObject std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; std::string fixed_frame_; - tf2::CompactFrameID fixed_frame_id_; bool pause_; From 37a28c3a8b918ded26d691910144a2c4c7a375f4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 27 May 2022 16:48:53 +0200 Subject: [PATCH 10/10] SyncFrame as last enum entry ... to keep existing config files valid --- src/rviz/frame_manager.h | 6 +++--- src/rviz/time_panel.cpp | 13 +++++++++---- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h index 2aa50a935b..32a7f2edf1 100644 --- a/src/rviz/frame_manager.h +++ b/src/rviz/frame_manager.h @@ -67,10 +67,10 @@ class FrameManager : public QObject public: enum SyncMode { - SyncOff = 0, + 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 - SyncExact, - SyncApprox }; /// Constructor, will create a TransformListener (and Buffer) automatically if not provided diff --git a/src/rviz/time_panel.cpp b/src/rviz/time_panel.cpp index 7142caa2fc..7a10023a28 100644 --- a/src/rviz/time_panel.cpp +++ b/src/rviz/time_panel.cpp @@ -59,12 +59,17 @@ TimePanel::TimePanel(QWidget* parent) : Panel(parent) sync_mode_selector_ = new QComboBox(this); sync_mode_selector_->addItem("Off"); - sync_mode_selector_->addItem("Frame"); - sync_mode_selector_->setItemData(1, "Synchronize TF lookups within a frame", Qt::ToolTipRole); + sync_mode_selector_->setItemData(FrameManager::SyncOff, "Display data using latest TF data", + Qt::ToolTipRole); sync_mode_selector_->addItem("Exact"); - sync_mode_selector_->setItemData(1, "Synchronize TF lookups to an external (Display) source", + 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."); @@ -227,7 +232,7 @@ void TimePanel::syncSourceSelected(int /*index*/) void TimePanel::syncModeSelected(int mode) { vis_manager_->getFrameManager()->setSyncMode((FrameManager::SyncMode)mode); - sync_source_selector_->setVisible(mode > FrameManager::SyncFrame); + sync_source_selector_->setVisible(mode >= FrameManager::SyncExact && mode <= FrameManager::SyncApprox); vis_manager_->notifyConfigChanged(); }