From 5d20af337050f9a3a98463ad7b7960fcfe01a45b Mon Sep 17 00:00:00 2001
From: Max SCHMELLER <max.schmeller@tier4.jp>
Date: Tue, 3 Sep 2024 18:47:05 +0900
Subject: [PATCH] feat(ring_section_filter): check if the provided filters
 exceed  the sensor's number of channels

---
 .../point_filters/parser.hpp                  |  5 +++--
 .../point_filters/ring_section_filter.hpp     | 20 ++++++++++++++++++-
 nebula_ros/src/hesai/hesai_ros_wrapper.cpp    |  5 +++--
 3 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp
index 433f2a195..778d8105a 100644
--- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp
+++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp
@@ -16,6 +16,7 @@
 
 #include "nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp"
 
+#include <nebula_common/nebula_common.hpp>
 #include <nebula_common/point_filters/point_filter.hpp>
 #include <nebula_common/util/expected.hpp>
 #include <nlohmann/json.hpp>
@@ -31,7 +32,7 @@ using nlohmann::json;
 using namespace std::string_literals;  // NOLINT
 
 nebula::util::expected<std::vector<std::shared_ptr<PointFilter>>, std::string> parse_point_filters(
-  const std::string & s)
+  const std::string & s, SensorModel sensor_model)
 {
   std::vector<std::shared_ptr<PointFilter>> parsed_filters;
 
@@ -48,7 +49,7 @@ nebula::util::expected<std::vector<std::shared_ptr<PointFilter>>, std::string> p
 
   for (const auto & [key, value] : j.items()) {
     if (key == "ring_section_filter") {
-      auto parsed = RingSectionFilter::fromJson(value);
+      auto parsed = RingSectionFilter::fromJson(value, sensor_model);
       if (!parsed.has_value()) {
         return "Could not parse " + key + ": " + parsed.error();
       }
diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp
index 0be3729f7..9d342532b 100644
--- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp
+++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/ring_section_filter.hpp
@@ -34,9 +34,19 @@ namespace nebula::drivers
 
 using namespace std::string_literals;  // NOLINT
 
+/**
+ * @brief Filters out angular sections on the configured rings.
+ */
 class RingSectionFilter : public PointFilter
 {
 public:
+  /**
+   * @brief Construct a new ring section filter given a list of (channel_id, start_deg, end_deg)
+   * tuples.
+   *
+   * @param excluded_sections_deg A list of (channel_id, start_deg, end_deg) tuples which should be
+   * excluded. Sections wrapping around the 360/0 deg boundary are handled correctly.
+   */
   explicit RingSectionFilter(
     const std::vector<std::tuple<uint32_t, float, float>> & excluded_sections_deg)
   {
@@ -82,8 +92,12 @@ class RingSectionFilter : public PointFilter
    * filter if parsed successfully, an error message otherwise.
    */
   static nebula::util::expected<RingSectionFilter, std::string> fromJson(
-    const nlohmann::json & json)
+    const nlohmann::json & json, SensorModel sensor_model)
   {
+    auto sensor_n_channels_result = get_n_channels(sensor_model);
+    if (!sensor_n_channels_result.has_value()) return sensor_n_channels_result.error();
+    uint32_t sensor_n_channels = sensor_n_channels_result.value();
+
     std::vector<std::tuple<uint32_t, float, float>> parsed_sections;
 
     if (!json.is_array()) {
@@ -111,6 +125,10 @@ class RingSectionFilter : public PointFilter
       float start_deg = sector[1];
       float end_deg = sector[2];
 
+      if (channel_id >= sensor_n_channels) {
+        return "sensor supports at most " + std::to_string(sensor_n_channels) + " channels";
+      }
+
       parsed_sections.emplace_back(channel_id, start_deg, end_deg);
     }
 
diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp
index ccdacc481..6308339c1 100644
--- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp
+++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp
@@ -197,7 +197,7 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams()
   }
 
   auto point_filters_raw = declare_parameter<std::string>("point_filters", param_read_write());
-  auto point_filters = drivers::parse_point_filters(point_filters_raw);
+  auto point_filters = drivers::parse_point_filters(point_filters_raw, config.sensor_model);
   if (!point_filters.has_value()) {
     throw std::runtime_error("Could not parse point filters: " + point_filters.error());
   }
@@ -340,7 +340,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(
       nebula::drivers::ReturnModeFromStringHesai(_return_mode, sensor_cfg_ptr_->sensor_model);
 
   if (!point_filters_raw.empty()) {
-    auto point_filters = drivers::parse_point_filters(point_filters_raw);
+    auto point_filters =
+      drivers::parse_point_filters(point_filters_raw, sensor_cfg_ptr_->sensor_model);
     if (!point_filters.has_value()) {
       SetParametersResult result{};
       result.successful = false;