refactor(robosense): composable sensor behavior

.cspell.json

@@ -38,6 +38,7 @@
     "Difop",
     "gptp",
     "Idat",
-    "Vdat"
+    "Vdat",
+    "manc"
   ]
 }

nebula_common/include/nebula_common/nebula_common.hpp

@@ -121,6 +121,35 @@ inline ReturnType ReturnModeToReturnType(const ReturnMode & mode)
   }
 }
 
+inline uint8_t ReturnModeToNReturns(const ReturnMode & mode)
+{
+  switch (mode) {
+    case ReturnMode::SINGLE_STRONGEST:
+    case ReturnMode::SINGLE_FIRST:
+    case ReturnMode::SINGLE_LAST:
+    case ReturnMode::STRONGEST:
+    case ReturnMode::FIRST:
+    case ReturnMode::LAST:
+      return 1;
+    case ReturnMode::DUAL_STRONGEST_FIRST:
+    case ReturnMode::DUAL_FIRST_STRONGEST:
+    case ReturnMode::DUAL_STRONGEST_LAST:
+    case ReturnMode::DUAL_LAST_STRONGEST:
+    case ReturnMode::DUAL_LAST_FIRST:
+    case ReturnMode::DUAL_WEAK_FIRST:
+    case ReturnMode::DUAL_WEAK_LAST:
+    case ReturnMode::DUAL_FIRST:
+    case ReturnMode::DUAL_LAST:
+    case ReturnMode::DUAL_ONLY:
+    case ReturnMode::DUAL:
+      return 2;
+    case ReturnMode::TRIPLE:
+      return 3;
+    case ReturnMode::UNKNOWN:
+      throw std::runtime_error("Got unknown return mode");
+  }
+}
+
 /// @brief Convert ReturnMode enum to integer
 /// @param mode
 /// @return Corresponding number

nebula_common/include/nebula_common/robosense/robosense_common.hpp

@@ -58,18 +58,21 @@ inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode)
 size_t GetChannelSize(const SensorModel & model)
 {
   switch (model) {
+    case SensorModel::ROBOSENSE_BPEARL:
     case SensorModel::ROBOSENSE_BPEARL_V3:
+    case SensorModel::ROBOSENSE_BPEARL_V4:
       return 32;
     case SensorModel::ROBOSENSE_HELIOS:
       return 32;
+    default:
+      throw std::runtime_error("Unknown sensor model");
   }
 }
 
 struct ChannelCorrection
 {
   float azimuth{NAN};
   float elevation{NAN};
-  uint16_t channel{};
 
   [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); }
 };
@@ -185,17 +188,6 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase
   {
     return calibration[channel_id];
   }
-
-  void CreateCorrectedChannels()
-  {
-    for(auto& correction : calibration) {
-      uint16_t channel = 0;
-      for(const auto& compare:calibration) {
-        if(compare.elevation < correction.elevation) ++channel;
-      }
-      correction.channel = channel;
-    }
-  }
 };
 
 }  // namespace drivers

nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor.hpp

@@ -0,0 +1,95 @@
+#pragma once
+
+#include "nebula_common/nebula_common.hpp"
+#include "nebula_decoders/nebula_decoders_common/util.hpp"
+
+#include <cstddef>
+#include <cstdint>
+
+namespace nebula
+{
+namespace drivers
+{
+
+/// @brief Base class for all LiDAR sensors that are compatible with the generic decoder.
+template <typename PacketT>
+struct SensorBase
+{
+  typedef PacketT packet_t;
+  typedef typename packet_t::body_t body_t;
+  typedef typename body_t::block_t block_t;
+  typedef typename block_t::unit_t unit_t;
+
+  SensorBase() = default;
+  virtual ~SensorBase() = default;
+
+  /// @brief Decide whereto to decode the given raw packet. This is only non-zero for sensors that
+  /// split return groups across multiple packets
+  /// @param raw_packet The raw bytes to be decoded
+  /// @return The index [0, num_decode_groups) within the decode group to which the packet belongs
+  virtual size_t getDecodeGroupIndex(const uint8_t * const /* raw_packet */) const { return 0; }
+
+  /// @brief Whether the unit given by return_idx is a duplicate of any other unit in return_units
+  /// @param return_idx The unit's index in the return_units vector
+  /// @param return_units The vector of all the units corresponding to the same return group (i.e.
+  /// length 2 for dual-return with both units having the same channel but coming from different
+  /// blocks)
+  /// @return true if the unit is identical to any other one in return_units, false otherwise
+  template <typename CollectionT>
+  static bool isDuplicate(const CollectionT & return_units, const size_t return_idx)
+  {
+    for (size_t i = 0; i < return_units.size(); ++i) {
+      if (i == return_idx) {
+        continue;
+      }
+
+      if (
+        getFieldValue(return_units[return_idx]->distance) ==
+          getFieldValue(return_units[i]->distance) &&
+        getFieldValue(return_units[return_idx]->reflectivity) ==
+          getFieldValue(return_units[i]->reflectivity)) {
+        return true;
+      }
+    }
+
+    return false;
+  }
+
+  /// @brief Get the return type of the point given by return_idx
+  ///
+  /// @param return_units The units corresponding to all the returns in the group. These are usually
+  /// from the same column across adjascent blocks.
+  /// @param return_idx The block index of the point within the group of blocks that make up the
+  /// return group (e.g. either 0 or 1 for dual return)
+  /// @param return_mode The sensor's currently active return mode
+  /// @return The return type of the point
+  template <typename CollectionT>
+  static ReturnType getReturnType(
+    const CollectionT & return_units, const size_t return_idx, const ReturnMode & return_mode)
+  {
+    if (isDuplicate(return_units, return_idx)) {
+      return ReturnType::IDENTICAL;
+    }
+
+    // TODO(mojomex): this is exhaustive only for Robosense. Extend to all ReturnModes in the future
+    switch (return_mode) {
+      case ReturnMode::SINGLE_FIRST:
+        return ReturnType::FIRST;
+      case ReturnMode::SINGLE_LAST:
+        return ReturnType::LAST;
+      case ReturnMode::SINGLE_STRONGEST:
+        return ReturnType::STRONGEST;
+      case ReturnMode::DUAL:
+        if (return_idx == 0) {
+          return ReturnType::STRONGEST;
+        } else {
+          return ReturnType::LAST;
+        }
+      default:
+        return ReturnType::UNKNOWN;
+    }
+  }
+};
+
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp

@@ -0,0 +1,87 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_common/util.hpp"
+
+#include <cstddef>
+#include <cstdint>
+#include <type_traits>
+
+namespace nebula
+{
+namespace drivers
+{
+namespace sensor_mixins
+{
+
+struct CorrectedAngleData
+{
+  float azimuth_rad;
+  float elevation_rad;
+  float sin_azimuth;
+  float cos_azimuth;
+  float sin_elevation;
+  float cos_elevation;
+};
+
+/// @brief Retrieves raw angles for a given unit. The angles do not have to be in any particular unit
+/// as long as they will be corrected by a suitable @ref AngleCorrectorMixin
+template <typename PacketT>
+struct AnglesMixin
+{
+  /// @brief Retrieves the raw (pre-correction) azimuth of a given unit
+  virtual int32_t getRawAzimuth(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0;
+
+  /// @brief Retrieves the raw (pre-correction) elevation of a given unit
+  virtual int32_t getRawElevation(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0;
+};
+
+/// @brief Converts raw angles into corrected ones, along with their trigonometric functions.
+template <typename PacketT>
+struct AngleCorrectorMixin
+{
+  /// @brief Corrects the raw input anglesand computes their sin/cos
+  virtual CorrectedAngleData getCorrectedAngleData(int32_t raw_azimuth, int32_t raw_elevation) const = 0;
+};
+
+/// @brief Reads raw azimuth from a block's `azimuth` field and returns the unit id as elevation
+template <typename PacketT>
+struct BlockAziChannelElevMixin : public AnglesMixin<PacketT>
+{
+  int32_t getRawAzimuth(
+    const PacketT & packet, const size_t block_id, const size_t /* unit_id */) const override
+  {
+    const auto * block = getBlock(packet, block_id);
+    return getFieldValue(block->azimuth);
+  }
+
+  int32_t getRawElevation(
+    const PacketT & /* packet */, const size_t /* block_id */, const size_t unit_id) const override
+  {
+    return static_cast<int32_t>(unit_id);
+  }
+};
+
+/// @brief Reads azimuth and elevation from fields in the given unit
+template <typename PacketT>
+struct AnglesInUnitMixin : public AnglesMixin<PacketT>
+{
+  int32_t getRawAzimuth(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const override
+  {
+    const auto * unit = getUnit(packet, block_id, unit_id);
+    return getFieldValue(unit->azimuth);
+  }
+
+  int32_t getRawElevation(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const override
+  {
+    const auto * unit = getUnit(packet, block_id, unit_id);
+    return getFieldValue(unit->elevation);
+  }
+};
+
+}  // namespace sensor_mixins
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp

@@ -0,0 +1,38 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_common/util.hpp"
+
+#include <cstddef>
+#include <cstdint>
+#include <type_traits>
+
+namespace nebula
+{
+namespace drivers
+{
+namespace sensor_mixins
+{
+
+/// @brief Retrieves the channel ID (laser ID) of a given unit
+template <typename PacketT>
+struct ChannelMixin
+{
+  /// @brief Retrieves the channel ID (laser ID) of a given unit
+  virtual uint8_t getChannel(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0;
+};
+
+/// @brief Returns the given unit ID as channel ID
+template <typename PacketT>
+struct ChannelIsUnitMixin: public ChannelMixin<PacketT>
+{
+  uint8_t getChannel(
+    const PacketT & /* packet */, const size_t /* block_id */, const size_t unit_id) const override
+  {
+    return unit_id;
+  }
+};
+
+}  // namespace sensor_mixins
+}  // namespace drivers
+}  // namespace nebula

nebula_decoders/include/nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp

@@ -0,0 +1,48 @@
+#pragma once
+
+#include "nebula_decoders/nebula_decoders_common/util.hpp"
+
+#include <cstddef>
+#include <cstdint>
+#include <type_traits>
+
+namespace nebula
+{
+namespace drivers
+{
+namespace sensor_mixins
+{
+
+/// @brief Retrieves the packet's distance unit and calculates unit distances in meters
+template <typename PacketT>
+struct DistanceMixin
+{
+  /// @brief Returns the value to multiply distance values in the packet with to get meters.
+  virtual double getDistanceUnit(const PacketT & packet) const = 0;
+
+  /// @brief Returns the distance value of the given unit in meters.
+  virtual double getDistance(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const = 0;
+};
+
+/// @brief Reads the distance unit from the packet header and distance from each unit's `distance`
+/// field.
+template <typename PacketT>
+struct HeaderMmDisUnitMixin : public DistanceMixin<PacketT>
+{
+  double getDistanceUnit(const PacketT & packet) const override
+  {
+    return packet.header.dis_unit / 1000.;
+  }
+
+  double getDistance(
+    const PacketT & packet, const size_t block_id, const size_t unit_id) const override
+  {
+    const auto * unit = getUnit(packet, block_id, unit_id);
+    return getFieldValue(unit->distance) * getDistanceUnit(packet);
+  }
+};
+
+}  // namespace sensor_mixins
+}  // namespace drivers
+}  // namespace nebula