From b023cc58f400e87e40fa57a6b2b4497d02976b89 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 31 Mar 2023 16:43:57 +0200 Subject: [PATCH 1/2] clang-tidy fixup --- src/rviz/default_plugin/tf_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rviz/default_plugin/tf_display.cpp b/src/rviz/default_plugin/tf_display.cpp index 022ccd2bf2..8c53c47ee0 100644 --- a/src/rviz/default_plugin/tf_display.cpp +++ b/src/rviz/default_plugin/tf_display.cpp @@ -216,7 +216,7 @@ class RegexFilterProperty : public StringProperty } public: - RegexFilterProperty(const QString& name, const std::regex regex, Property* parent) + RegexFilterProperty(const QString& name, const std::regex& regex, Property* parent) : StringProperty(name, "", "regular expression", parent), default_(regex), regex_(regex) { QObject::connect(this, &RegexFilterProperty::changed, this, [this]() { onValueChanged(); }); From 96a76a40473bbc78dc29453891500e90e82fc50a Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 25 May 2023 15:01:22 +0200 Subject: [PATCH 2/2] Improve error handling in LaserScanDisplay (#1798) * Improve "unknown TF error" message * LaserScanDisplay: warn about large scan time --- src/rviz/default_plugin/laser_scan_display.cpp | 12 ++++++++++++ src/rviz/default_plugin/laser_scan_display.h | 3 +++ src/rviz/frame_manager.cpp | 2 +- 3 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/rviz/default_plugin/laser_scan_display.cpp b/src/rviz/default_plugin/laser_scan_display.cpp index 2377a7979a..07f8264669 100644 --- a/src/rviz/default_plugin/laser_scan_display.cpp +++ b/src/rviz/default_plugin/laser_scan_display.cpp @@ -77,6 +77,16 @@ void LaserScanDisplay::updateQueueSize() tf_filter_->setQueueSize((uint32_t)queue_size_property_->getInt()); } +void LaserScanDisplay::checkTolerance(int tolerance) +{ + if (tolerance > 1) + setStatus(StatusProperty::Warn, "Scan Time", + QString( + "Laser scan time, computed from time_increment * len(ranges), is rather large: %1s.\n" + "The display of any message will be delayed by this amount of time!") + .arg(tolerance)); +} + void LaserScanDisplay::processMessage(const sensor_msgs::LaserScanConstPtr& scan) { sensor_msgs::PointCloudPtr cloud(new sensor_msgs::PointCloud); @@ -89,6 +99,7 @@ void LaserScanDisplay::processMessage(const sensor_msgs::LaserScanConstPtr& scan { filter_tolerance_ = tolerance; tf_filter_->setTolerance(filter_tolerance_); + checkTolerance(filter_tolerance_.toSec()); } try @@ -127,6 +138,7 @@ void LaserScanDisplay::reset() { MFDClass::reset(); point_cloud_common_->reset(); + checkTolerance(filter_tolerance_.toSec()); } } // namespace rviz diff --git a/src/rviz/default_plugin/laser_scan_display.h b/src/rviz/default_plugin/laser_scan_display.h index 7d982ea4fb..535dcf92da 100644 --- a/src/rviz/default_plugin/laser_scan_display.h +++ b/src/rviz/default_plugin/laser_scan_display.h @@ -66,6 +66,9 @@ private Q_SLOTS: /** @brief Process a single message. Overridden from MessageFilterDisplay. */ void processMessage(const sensor_msgs::LaserScanConstPtr& scan) override; + /** create a status warning when tolerance is larger than 1s */ + void checkTolerance(int tolerance); + IntProperty* queue_size_property_; PointCloudCommon* point_cloud_common_; diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index afaf5a11ea..223a1b11f0 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -392,7 +392,7 @@ std::string FrameManager::discoverFailureReason(const std::string& frame_id, } } - return "Unknown reason for transform failure"; + return "Unknown reason for transform failure (frame=[" + frame_id + "])"; } void FrameManager::messageArrived(const std::string& /*frame_id*/,