feat(ring_section_filter): check if the provided filters exceed the s…

…ensor's number of channels

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();
       }

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);
     }
 

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -213,7 +213,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());
   }
@@ -352,7 +352,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(
   }
 
   if (_return_mode.length() > 0)
-    new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode);
+    new_cfg.return_mode =
+      nebula::drivers::ReturnModeFromStringHesai(_return_mode, sensor_cfg_ptr_->sensor_model);
 
   if (new_cfg.cut_angle == 360.0) {
     RCLCPP_WARN_STREAM(
@@ -362,7 +363,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange(
   }
 
   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;