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

.cspell.json

@@ -11,6 +11,7 @@
     "block_id",
     "Bpearl",
     "calib",
+    "centi",
     "DHAVE",
     "Difop",
     "extrinsics",

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

@@ -27,6 +27,7 @@ For more information, please refer to [About Nebula](about.md).
 - [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,15 +17,19 @@
 
 #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>
+#include <tuple>
 #include <vector>
 namespace nebula
 {
@@ -35,16 +39,18 @@ namespace drivers
 struct HesaiSensorConfiguration : public LidarConfigurationBase
 {
   uint16_t gnss_port{};
-  double scan_phase{};
+  uint16_t sync_angle{};
+  double cut_angle{};
   double dual_return_distance_threshold{};
-  std::string calibration_path{};
+  std::string calibration_path;
   uint16_t rotation_speed;
   uint16_t cloud_min_angle;
   uint16_t cloud_max_angle;
   PtpProfile ptp_profile;
   uint8_t ptp_domain;
   PtpTransportType ptp_transport_type;
   PtpSwitchType ptp_switch_type;
+  std::vector<std::shared_ptr<PointFilter>> point_filters;
 };
 /// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
 /// @param os
@@ -55,11 +61,13 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
   os << "Hesai Sensor Configuration:" << '\n';
   os << (LidarConfigurationBase)(arg) << '\n';
   os << "GNSS Port: " << arg.gnss_port << '\n';
-  os << "Scan Phase: " << arg.scan_phase << '\n';
   os << "Rotation Speed: " << arg.rotation_speed << '\n';
+  os << "Sync Angle: " << arg.sync_angle << '\n';
+  os << "Cut Angle: " << arg.cut_angle << '\n';
   os << "FoV Start: " << arg.cloud_min_angle << '\n';
   os << "FoV End: " << arg.cloud_max_angle << '\n';
   os << "Dual Return Distance Threshold: " << arg.dual_return_distance_threshold << '\n';
+  os << "Calibration Path: " << arg.calibration_path << '\n';
   os << "PTP Profile: " << arg.ptp_profile << '\n';
   os << "PTP Domain: " << std::to_string(arg.ptp_domain) << '\n';
   os << "PTP Transport Type: " << arg.ptp_transport_type << '\n';
@@ -73,6 +81,8 @@ struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase
   virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0;
   virtual nebula::Status SaveToFileFromBytes(
     const std::string & calibration_file, const std::vector<uint8_t> & buf) = 0;
+
+  [[nodiscard]] virtual std::tuple<float, float> getFovPadding() const = 0;
 };
 
 /// @brief struct for Hesai calibration configuration
@@ -174,6 +184,19 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase
     ofs.close();
     return Status::OK;
   }
+
+  [[nodiscard]] std::tuple<float, float> getFovPadding() const override
+  {
+    float min = INFINITY;
+    float max = -INFINITY;
+
+    for (const auto & item : azimuth_offset_map) {
+      min = std::min(min, item.second);
+      max = std::max(max, item.second);
+    }
+
+    return {-max, -min};
+  }
 };
 
 /// @brief struct for Hesai correction configuration (for AT)
@@ -357,7 +380,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   /// @param ch The channel id
   /// @param azi The precision azimuth in (0.01 / 256) degree unit
   /// @return The azimuth adjustment in 0.01 degree unit
-  int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const
+  [[nodiscard]] int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const
   {
     unsigned int i = std::floor(1.f * azi / STEP3);
     unsigned int l = azi - i * STEP3;
@@ -369,13 +392,19 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase
   /// @param ch The channel id
   /// @param azi The precision azimuth in (0.01 / 256) degree unit
   /// @return The elevation adjustment in 0.01 degree unit
-  int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const
+  [[nodiscard]] int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const
   {
     unsigned int i = std::floor(1.f * azi / STEP3);
     unsigned int l = azi - i * STEP3;
     float k = 1.f * l / STEP3;
     return round((1 - k) * elevationOffset[ch * 180 + i] + k * elevationOffset[ch * 180 + i + 1]);
   }
+
+  [[nodiscard]] std::tuple<float, float> getFovPadding() const override
+  {
+    // TODO(mojomex): calculate instead of hard-coding
+    return {-5, 5};
+  }
 };
 
 /*

nebula_common/include/nebula_common/nebula_common.hpp

@@ -15,11 +15,14 @@
 #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>
@@ -28,6 +31,9 @@ namespace nebula
 {
 namespace 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 };
 
@@ -636,6 +642,47 @@ inline std::string SensorModelToString(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,7 +15,9 @@
 #pragma once
 
 #include <exception>
+#include <stdexcept>
 #include <string>
+#include <utility>
 #include <variant>
 
 namespace nebula
@@ -97,9 +99,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_(std::move(value)) {}  // NOLINT(runtime/explicit)
+  expected(const E & error) : value_(error) {}        // NOLINT(runtime/explicit)
+  expected(E && error) : value_(std::move(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()