diff --git a/src/rviz/default_plugin/laser_scan_display.cpp b/src/rviz/default_plugin/laser_scan_display.cpp index 8e2801641..6330b0a14 100644 --- a/src/rviz/default_plugin/laser_scan_display.cpp +++ b/src/rviz/default_plugin/laser_scan_display.cpp @@ -66,6 +66,16 @@ void LaserScanDisplay::onInitialize() point_cloud_common_->initialize(context_, scene_node_); } +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::PointCloud2Ptr cloud(new sensor_msgs::PointCloud2); @@ -76,6 +86,7 @@ void LaserScanDisplay::processMessage(const sensor_msgs::LaserScanConstPtr& scan { filter_tolerance_ = tolerance; tf_filter_->setTolerance(filter_tolerance_); + checkTolerance(filter_tolerance_.toSec()); } try @@ -105,6 +116,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 09bb33b2a..374a812f3 100644 --- a/src/rviz/default_plugin/laser_scan_display.h +++ b/src/rviz/default_plugin/laser_scan_display.h @@ -63,6 +63,9 @@ class LaserScanDisplay : public MessageFilterDisplay /** @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); + PointCloudCommon* point_cloud_common_; laser_geometry::LaserProjection* projector_; diff --git a/src/rviz/frame_manager.cpp b/src/rviz/frame_manager.cpp index ce758fcde..eb9c38e33 100644 --- a/src/rviz/frame_manager.cpp +++ b/src/rviz/frame_manager.cpp @@ -328,7 +328,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*/,