feat(hesai): add a ring section filter to Hesai sensors

docs/filters.md

@@ -0,0 +1,98 @@
+# Point Filters
+
+Filters run for every point, as soon as it is fully decoded. This can speed up the later parts of pointcloud processing pipelines by reducing the number of points sent and copied between modules.
+
+## Configuration
+
+Filters are configured via the ROS parameter `point_filters`. The parameter is a stringified JSOn object of the form:
+
+```json
+{
+  "filter_type1": configuration1,
+  "filter_type2": configuration2
+}
+```
+
+Where the `filter_type` field has to be one of the supported filters below, and where the `configuration` format depends on the filter type.
+
+Filters can also be set during runtime, e.g. via:
+
+```shell
+ros2 param set /hesai_ros_wrapper_node point_filters '"{\"filter_type1\": configuration1, ...}"'
+```
+
+## Supported Filters
+
+The following filter types are supported:
+
+| Filter Name         | Filter Type Field     |
+| ------------------- | --------------------- |
+| Ring Section Filter | `ring_section_filter` |
+
+Below, each filter type is documented in detail.
+
+### Ring Section Filter
+
+This filter can remove zero or more contiguous sections per ring.
+
+Configuration is done in the following format:
+
+```json
+  [
+    [channel_id1, start_deg1, end_deg1],
+    [channel_id2, start_deg2, end_deg2],
+    ...
+  ]
+```
+
+Things to keep in mind:
+
+- Not all channels have to be configured (unconfigured channels are not filtered).
+- A channel can be configured multiple times, in which case all of the configured sections are filtered (like a logical `OR` operation)
+- Sections are allowed to overlap
+- Sections can wrap around the `360/0` boundary (e.g. `[270, 90] deg`)
+- `start_deg` and `end_deg` are floating-point, so precise angles can be specified
+
+Examples:
+
+- `[50.25, 100.114] deg` on rings `[2, 4]`:
+
+  ```json
+  [
+    [2, 50.25, 100, 114],
+    [3, 50.25, 100, 114],
+    [4, 50.25, 100, 114]
+  ]
+  ```
+
+- complete rings `1, 3, 5`:
+
+  ```json
+  [
+    [1, 0, 360],
+    [3, 0, 360],
+    [5, 0, 360]
+  ]
+  ```
+
+- `[10, 20] deg`, `[30, 40] deg` and `[50, 60] deg` on ring `5`:
+
+  ```json
+  [
+    [5, 10, 20],
+    [5, 30, 40],
+    [5, 50, 60]
+  ]
+  ```
+
+## Compatibility Chart
+
+|           | Ring Section Filter |
+| --------- | ------------------- |
+| Hesai     | ✅                  |
+| Robosense | ❌                  |
+| Velodyne  | ❌                  |
+
+Compatibility:  
+✅: compatible  
+❌: incompatible

docs/index.md

@@ -25,6 +25,7 @@ Nebula works with ROS 2 and is the recommended sensor driver for the [Autoware](
 - [Design](design.md)
 - [Parameters](parameters.md)
 - [Point cloud types](point_types.md)
+- [Point filters](filters.md)
 
 ## Supported sensors
 

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -17,13 +17,15 @@
 
 #include "nebula_common/nebula_common.hpp"
 #include "nebula_common/nebula_status.hpp"
+#include "nebula_common/point_filters/point_filter.hpp"
 #include "nebula_common/util/string_conversions.hpp"
 
 #include <algorithm>
 #include <cmath>
 #include <fstream>
 #include <iostream>
 #include <map>
+#include <memory>
 #include <sstream>
 #include <stdexcept>
 #include <string>
@@ -50,6 +52,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase
   PtpTransportType ptp_transport_type;
   PtpSwitchType ptp_switch_type;
   uint8_t ptp_lock_threshold;
+  std::vector<std::shared_ptr<PointFilter>> point_filters;
 };
 /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
 /// @param os

nebula_common/include/nebula_common/nebula_common.hpp

@@ -15,17 +15,23 @@
 #ifndef NEBULA_COMMON_H
 #define NEBULA_COMMON_H
 
+#include "nebula_common/util/expected.hpp"
+
 #include <nebula_common/point_types.hpp>
 
 #include <boost/tokenizer.hpp>
 
 #include <algorithm>
+#include <cstdint>
 #include <ostream>
 #include <string>
 #include <vector>
 
 namespace nebula::drivers
 {
+
+using std::string_literals::operator""s;
+
 /// @brief Coordinate mode for Velodyne's setting (need to check)
 enum class CoordinateMode { UNKNOWN = 0, CARTESIAN, SPHERICAL, CYLINDRICAL };
 
@@ -634,6 +640,47 @@ inline std::string sensor_model_to_string(const SensorModel & sensor_model)
   }
 }
 
+/**
+ * @brief Get the number of channels a specific sensor model has. A string error message is returned
+ * for sensors that do not have a meaningful channel number.
+ *
+ * @param sensor_model The sensor model of interest
+ * @return nebula::util::expected<uint32_t, std::string> The number of channels on success, or a
+ * string error message.
+ */
+inline nebula::util::expected<uint32_t, std::string> get_n_channels(
+  const SensorModel & sensor_model)
+{
+  switch (sensor_model) {
+    case SensorModel::VELODYNE_VLP16:
+      return 16;
+    case SensorModel::VELODYNE_VLP32:
+    case SensorModel::VELODYNE_VLP32MR:
+    case SensorModel::VELODYNE_HDL32:
+    case SensorModel::HESAI_PANDARXT32:
+    case SensorModel::HESAI_PANDARXT32M:
+    case SensorModel::ROBOSENSE_HELIOS:
+    case SensorModel::ROBOSENSE_BPEARL_V3:
+    case SensorModel::ROBOSENSE_BPEARL_V4:
+      return 32;
+    case SensorModel::HESAI_PANDAR40P:
+    case SensorModel::HESAI_PANDAR40M:
+      return 40;
+    case SensorModel::VELODYNE_HDL64:
+    case SensorModel::HESAI_PANDAR64:
+    case SensorModel::HESAI_PANDARQT64:
+      return 64;
+    case SensorModel::VELODYNE_VLS128:
+    case SensorModel::HESAI_PANDARQT128:
+    case SensorModel::HESAI_PANDARAT128:
+    case SensorModel::HESAI_PANDAR128_E3X:
+    case SensorModel::HESAI_PANDAR128_E4X:
+      return 128;
+    default:
+      return "unsupported sensor model"s;
+  }
+}
+
 /// @brief Convert return mode name to ReturnMode enum
 /// @param return_mode Return mode name (Upper and lower case letters must match)
 /// @return Corresponding ReturnMode

nebula_common/include/nebula_common/point_filters/point_filter.hpp

@@ -0,0 +1,30 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <nebula_common/point_types.hpp>
+
+namespace nebula::drivers
+{
+
+class PointFilter
+{
+public:
+  virtual ~PointFilter() = default;
+
+  virtual bool excluded(const NebulaPoint & point) = 0;
+};
+
+}  // namespace nebula::drivers

nebula_common/include/nebula_common/util/expected.hpp

@@ -15,6 +15,7 @@
 #pragma once
 
 #include <exception>
+#include <stdexcept>
 #include <string>
 #include <variant>
 
@@ -97,9 +98,10 @@ struct expected
     return default_;
   }
 
-  expected(const T & value) { value_ = value; }  // NOLINT(runtime/explicit)
-
-  expected(const E & error) { value_ = error; }  // NOLINT(runtime/explicit)
+  expected(const T & value) : value_(value) {}  // NOLINT(runtime/explicit)
+  expected(T && value) : value_(value) {}       // NOLINT(runtime/explicit)
+  expected(const E & error) : value_(error) {}  // NOLINT(runtime/explicit)
+  expected(E && error) : value_(error) {}       // NOLINT(runtime/explicit)
 
 private:
   std::variant<T, E> value_;

nebula_decoders/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(robosense_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(velodyne_msgs REQUIRED)
 find_package(yaml-cpp REQUIRED)
+find_package(nlohmann_json REQUIRED)
 
 include_directories(PUBLIC
     include
@@ -35,6 +36,7 @@ include_directories(PUBLIC
     ${pcl_conversions_INCLUDE_DIRS}
     ${rclcpp_INCLUDE_DIRS}
     ${sensor_msgs_INCLUDE_DIRS}
+    ${NLOHMANN_JSON_INCLUDE_DIRS}
 )
 
 link_libraries(
@@ -43,6 +45,7 @@ link_libraries(
     ${pcl_conversions_LIBRARIES}
     ${rclcpp_TARGETS}
     ${sensor_msgs_TARGETS}
+    ${NLOHMANN_JSON_LIBRARIES}
 )
 
 # Lidar Decoders
@@ -153,6 +156,7 @@ ament_export_dependencies(
     sensor_msgs
     velodyne_msgs
     yaml-cpp
+    nlohmann_json
 )
 
 ament_package()

nebula_decoders/include/nebula_decoders/nebula_decoders_common/point_filters/parser.hpp

@@ -0,0 +1,71 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#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>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace nebula::drivers
+{
+
+using nlohmann::json;
+using namespace std::string_literals;  // NOLINT
+
+inline nebula::util::expected<std::vector<std::shared_ptr<PointFilter>>, std::string>
+parse_point_filters(const std::string & s, SensorModel sensor_model)
+{
+  if (s.empty()) {
+    return std::vector<std::shared_ptr<PointFilter>>{};
+  }
+
+  std::vector<std::shared_ptr<PointFilter>> parsed_filters;
+
+  json j;
+  try {
+    j = json::parse(s);
+  } catch (json::parse_error & e) {
+    return "Could not parse JSON: "s + e.what();
+  }
+
+  if (!j.is_object()) {
+    return "expected a JSON object"s;
+  }
+
+  for (const auto & [key, value] : j.items()) {
+    if (key == "ring_section_filter") {
+      auto parsed = RingSectionFilter::fromJson(value, sensor_model);
+      if (!parsed.has_value()) {
+        return "Could not parse " + key + ": " + parsed.error();
+      }
+
+      auto filter = std::make_shared<RingSectionFilter>(parsed.value());
+      parsed_filters.emplace_back(filter);
+    } else {
+      return "unknown filter type: " + key;
+    }
+  }
+
+  return parsed_filters;
+}
+
+}  // namespace nebula::drivers