fix(dual_return_outlier_filter): change visibility diag initial value (…

…#471) * Change namespace of debug topics Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Update initial value of lidar visibility, separate err/warn threshold Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp> * Fix compile error Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
autowarefoundation · Mar 3, 2022 · 14afa55 · 14afa55
1 parent a3c200e
commit 14afa55
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 10 deletions.
diff --git a/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/dual-return-outlier-filter.md
@@ -57,7 +57,8 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref
 | `weak_first_distance_ratio`        | double | Threshold for ring_outlier_filter                                                                                         |
 | `general_distance_ratio`           | double | Threshold for ring_outlier_filter                                                                                         |
 | `weak_first_local_noise_threshold` | int    | The parameter for determining whether it is noise                                                                         |
-| `visibility_threshold`             | float  | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN |
+| `visibility_error_threshold`       | float  | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR  |
+| `visibility_warn_threshold`        | float  | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN |
 | `roi_mode`                         | string | The name of ROI mode for switching                                                                                        |
 | `min_azimuth_deg`                  | float  | The left limit of azimuth for `Fixed_azimuth_ROI` mode                                                                    |
 | `max_azimuth_deg`                  | float  | The right limit of azimuth for `Fixed_azimuth_ROI` mode                                                                   |

diff --git a/...sor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/...sor/include/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp
@@ -74,11 +74,12 @@ class DualReturnOutlierFilterComponent : public pointcloud_preprocessor::Filter
 private:
   void onVisibilityChecker(DiagnosticStatusWrapper & stat);
   Updater updater_{this};
-  double visibility_ = 1.f;
+  double visibility_ = -1.0f;
   double weak_first_distance_ratio_;
   double general_distance_ratio_;
   int weak_first_local_noise_threshold_;
-  double visibility_threshold_;
+  double visibility_error_threshold_;
+  double visibility_warn_threshold_;
   int vertical_bins_;
   float max_azimuth_diff_;
   std::string roi_mode_;

diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp
@@ -52,8 +52,11 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
 
     weak_first_local_noise_threshold_ =
       static_cast<int>(declare_parameter("weak_first_local_noise_threshold", 2));
-    visibility_threshold_ = static_cast<float>(declare_parameter("visibility_threshold", 0.5));
     roi_mode_ = static_cast<std::string>(declare_parameter("roi_mode", "Fixed_xyz_ROI"));
+    visibility_error_threshold_ =
+      static_cast<float>(declare_parameter("visibility_error_threshold", 0.5));
+    visibility_warn_threshold_ =
+      static_cast<float>(declare_parameter("visibility_warn_threshold", 0.7));
   }
   updater_.setHardwareID("dual_return_outlier_filter");
   updater_.add(
@@ -62,11 +65,11 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent(
   updater_.setPeriod(0.1);
 
   image_pub_ =
-    image_transport::create_publisher(this, "/dual_return_outlier_filter/frequency_image");
+    image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image");
   visibility_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
-    "/dual_return_outlier_filter/visibility", rclcpp::SensorDataQoS());
+    "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS());
   noise_cloud_pub_ = create_publisher<sensor_msgs::msg::PointCloud2>(
-    "/dual_return_outlier_filter/pointcloud_noise", rclcpp::SensorDataQoS());
+    "dual_return_outlier_filter/debug/pointcloud_noise", rclcpp::SensorDataQoS());
 
   using std::placeholders::_1;
   set_param_res_ = this->add_on_set_parameters_callback(
@@ -79,15 +82,27 @@ void DualReturnOutlierFilterComponent::onVisibilityChecker(DiagnosticStatusWrapp
   stat.add("value", std::to_string(visibility_));
 
   // Judge level
-  const auto level =
-    visibility_ > visibility_threshold_ ? DiagnosticStatus::OK : DiagnosticStatus::WARN;
+  auto level = DiagnosticStatus::OK;
+  if (visibility_ < 0) {
+    level = DiagnosticStatus::STALE;
+  } else if (visibility_ < visibility_error_threshold_) {
+    level = DiagnosticStatus::ERROR;
+  } else if (visibility_ < visibility_warn_threshold_) {
+    level = DiagnosticStatus::WARN;
+  } else {
+    level = DiagnosticStatus::OK;
+  }
 
   // Set message
   std::string msg;
   if (level == DiagnosticStatus::OK) {
     msg = "OK";
   } else if (level == DiagnosticStatus::WARN) {
-    msg = "low visibility in dual outlier filter";
+    msg = "WARNING: low visibility in dual outlier filter";
+  } else if (level == DiagnosticStatus::ERROR) {
+    msg = "ERROR: low visibility in dual outlier filter";
+  } else if (level == DiagnosticStatus::STALE) {
+    msg = "STALE";
   }
   stat.summary(level, msg);
 }