diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index 4a97c3cf5c..ce758fcde4 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -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; } } @@ -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; } @@ -119,8 +116,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) @@ -128,6 +125,7 @@ void FrameManager::syncTime(ros::Time time) switch (sync_mode_) { case SyncOff: + case SyncFrame: break; case SyncExact: sync_time_ = time; @@ -135,13 +133,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 { @@ -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; } @@ -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_); @@ -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; @@ -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()); @@ -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); diff --git a/src/rviz/frame_manager.h b/src/rviz/frame_manager.h index c4c469c957..4d8e501906 100644 --- a/src/rviz/frame_manager.h +++ b/src/rviz/frame_manager.h @@ -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 @@ -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 void messageCallback(const ros::MessageEvent& msg_evt, Display* display) @@ -306,7 +307,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_; @@ -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 diff --git a/src/rviz/time_panel.cpp b/src/rviz/time_panel.cpp index bde802d238..7a10023a28 100644 --- a/src/rviz/time_panel.cpp +++ b/src/rviz/time_panel.cpp @@ -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."); @@ -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))); @@ -134,10 +126,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 @@ -145,7 +133,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) @@ -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 @@ -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(); } 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_; diff --git a/src/test/send_tf_timing.py b/src/test/send_tf_timing.py new file mode 100644 index 0000000000..f353f3802e --- /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