From 9ac911e54155c63f64749dc72b7e6505b3dbd278 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 15 Aug 2023 23:03:52 +0200 Subject: [PATCH 1/5] Improve error handling in LaserScanDisplay MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../displays/laser_scan/laser_scan_display.hpp | 3 +++ .../displays/laser_scan/laser_scan_display.cpp | 13 +++++++++++++ 2 files changed, 16 insertions(+) diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp index f3454c552..db4e1d25d 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp @@ -81,6 +81,9 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC LaserScanDisplay : public /** @brief Process a single message. Overridden from MessageFilterDisplay. */ void processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; + /** create a status warning when tolerance is larger than 1s */ + void checkTolerance(int tolerance); + std::unique_ptr point_cloud_common_; std::unique_ptr projector_; rclcpp::Duration filter_tolerance_; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp index 5db275fb4..df8a351da 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp @@ -35,6 +35,7 @@ #include "tf2_ros/buffer.h" #include "rviz_common/properties/int_property.hpp" +#include "rviz_common/properties/status_property.hpp" #include "rviz_common/transformation/transformation_manager.hpp" #include "rviz_common/validate_floats.hpp" #include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" @@ -61,6 +62,16 @@ void LaserScanDisplay::onInitialize() transformer_guard_->initialize(context_); } +void LaserScanDisplay::checkTolerance(int tolerance) +{ + if (tolerance > 1) + setStatus(rviz_common::properties::StatusProperty::Warn, "Scan Time", + QString( + "Laser scan time, computed from time_increment * len(ranges), is rather large: " + " %1s.\nThe display of any message will be delayed by this amount of time!") + .arg(tolerance)); +} + void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) { // Compute tolerance necessary for this scan @@ -72,6 +83,7 @@ void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPt if (tolerance > filter_tolerance_) { filter_tolerance_ = tolerance; tf_filter_->setTolerance(filter_tolerance_); + checkTolerance(filter_tolerance_.nanoseconds() * 1e-9); } auto cloud = std::make_shared(); auto tf_wrapper = std::dynamic_pointer_cast( @@ -108,6 +120,7 @@ void LaserScanDisplay::reset() { MFDClass::reset(); point_cloud_common_->reset(); + checkTolerance(filter_tolerance_.nanoseconds() * 1e-9); } void LaserScanDisplay::onDisable() From 861a119d21b0d7cfd91c3f8580e60be3d4078d97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 17 Aug 2023 14:15:15 +0200 Subject: [PATCH 2/5] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../displays/laser_scan/laser_scan_display.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp index df8a351da..07c4a0292 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp @@ -64,12 +64,14 @@ void LaserScanDisplay::onInitialize() void LaserScanDisplay::checkTolerance(int tolerance) { - if (tolerance > 1) - setStatus(rviz_common::properties::StatusProperty::Warn, "Scan Time", - QString( - "Laser scan time, computed from time_increment * len(ranges), is rather large: " - " %1s.\nThe display of any message will be delayed by this amount of time!") - .arg(tolerance)); + if (tolerance > 1) { + setStatus( + rviz_common::properties::StatusProperty::Warn, "Scan Time", + QString( + "Laser scan time, computed from time_increment * len(ranges), is rather large: " + " %1s.\nThe display of any message will be delayed by this amount of time!") + .arg(tolerance)); + } } void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) From 24bc0f5788fef62e7906da35d6ad12ef69e85a16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 23 Aug 2023 09:46:40 +0200 Subject: [PATCH 3/5] Feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../displays/laser_scan/laser_scan_display.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp index 07c4a0292..9661410c8 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp @@ -85,7 +85,7 @@ void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPt if (tolerance > filter_tolerance_) { filter_tolerance_ = tolerance; tf_filter_->setTolerance(filter_tolerance_); - checkTolerance(filter_tolerance_.nanoseconds() * 1e-9); + checkTolerance(RCL_NS_TO_S(filter_tolerance_.nanoseconds())); } auto cloud = std::make_shared(); auto tf_wrapper = std::dynamic_pointer_cast( @@ -122,7 +122,7 @@ void LaserScanDisplay::reset() { MFDClass::reset(); point_cloud_common_->reset(); - checkTolerance(filter_tolerance_.nanoseconds() * 1e-9); + checkTolerance(RCL_NS_TO_S(filter_tolerance_.nanoseconds())); } void LaserScanDisplay::onDisable() From 220f86697b523cb86868f3e0e19ad2f1352b846f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 5 Sep 2023 16:43:41 +0200 Subject: [PATCH 4/5] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../displays/laser_scan/laser_scan_display.hpp | 4 +++- .../displays/laser_scan/laser_scan_display.cpp | 13 ++++++++----- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp index db4e1d25d..98c12d5bb 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/laser_scan/laser_scan_display.hpp @@ -47,6 +47,8 @@ #include "rviz_default_plugins/transformation/tf_frame_transformer.hpp" #include "rviz_default_plugins/visibility_control.hpp" +#include + namespace rviz_common { class QueueSizeProperty; @@ -82,7 +84,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC LaserScanDisplay : public void processMessage(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) override; /** create a status warning when tolerance is larger than 1s */ - void checkTolerance(int tolerance); + void checkTolerance(rclcpp::Duration tolerance); std::unique_ptr point_cloud_common_; std::unique_ptr projector_; diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp index 9661410c8..bd3d12fc9 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp @@ -41,6 +41,8 @@ #include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" #include "rviz_default_plugins/transformation/tf_wrapper.hpp" +#include + namespace rviz_default_plugins { namespace displays @@ -62,15 +64,16 @@ void LaserScanDisplay::onInitialize() transformer_guard_->initialize(context_); } -void LaserScanDisplay::checkTolerance(int tolerance) +void LaserScanDisplay::checkTolerance(rclcpp::Duration tolerance) { - if (tolerance > 1) { + auto tolerance_seg = RCL_NS_TO_S(filter_tolerance_.nanoseconds()); + if (tolerance_seg > 1) { setStatus( rviz_common::properties::StatusProperty::Warn, "Scan Time", QString( "Laser scan time, computed from time_increment * len(ranges), is rather large: " " %1s.\nThe display of any message will be delayed by this amount of time!") - .arg(tolerance)); + .arg(tolerance_seg)); } } @@ -85,7 +88,7 @@ void LaserScanDisplay::processMessage(sensor_msgs::msg::LaserScan::ConstSharedPt if (tolerance > filter_tolerance_) { filter_tolerance_ = tolerance; tf_filter_->setTolerance(filter_tolerance_); - checkTolerance(RCL_NS_TO_S(filter_tolerance_.nanoseconds())); + checkTolerance(filter_tolerance_); } auto cloud = std::make_shared(); auto tf_wrapper = std::dynamic_pointer_cast( @@ -122,7 +125,7 @@ void LaserScanDisplay::reset() { MFDClass::reset(); point_cloud_common_->reset(); - checkTolerance(RCL_NS_TO_S(filter_tolerance_.nanoseconds())); + checkTolerance(filter_tolerance_); } void LaserScanDisplay::onDisable() From d952bb7dd47ca9e1142665edc8e9e547c713cccd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Tue, 5 Sep 2023 18:16:27 +0200 Subject: [PATCH 5/5] Fixed unused variable MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../displays/laser_scan/laser_scan_display.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp index bd3d12fc9..2a538bf14 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/laser_scan/laser_scan_display.cpp @@ -66,7 +66,7 @@ void LaserScanDisplay::onInitialize() void LaserScanDisplay::checkTolerance(rclcpp::Duration tolerance) { - auto tolerance_seg = RCL_NS_TO_S(filter_tolerance_.nanoseconds()); + auto tolerance_seg = RCL_NS_TO_S(tolerance.nanoseconds()); if (tolerance_seg > 1) { setStatus( rviz_common::properties::StatusProperty::Warn, "Scan Time",