File hesai_sensor.hpp

File List > decoders > hesai_sensor.hpp

Go to the documentation of this file

// 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_hesai/decoders/angle_corrector_calibration_based.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_correction_based.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"

#include <nebula_common/nebula_common.hpp>

#include <algorithm>
#include <type_traits>
#include <vector>

namespace nebula::drivers
{

enum class AngleCorrectionType { CALIBRATION, CORRECTION };

template <typename PacketT, AngleCorrectionType AngleCorrection = AngleCorrectionType::CALIBRATION>
class HesaiSensor
{
private:
  static bool is_strongest_return(
    uint32_t return_idx,
    const std::vector<const typename PacketT::body_t::block_t::unit_t *> & return_units)
  {
    for (unsigned int i = 0; i < return_units.size(); ++i) {
      if (i == return_idx) {
        continue;
      }

      if (return_units[return_idx]->reflectivity < return_units[i]->reflectivity) {
        return false;
      }
    }

    return true;
  };

  static bool is_duplicate_return(
    uint32_t return_idx,
    const std::vector<const typename PacketT::body_t::block_t::unit_t *> & return_units)
  {
    for (unsigned int i = 0; i < return_units.size(); ++i) {
      if (i == return_idx) {
        continue;
      }

      if (
        return_units[return_idx]->distance == return_units[i]->distance &&
        return_units[return_idx]->reflectivity == return_units[i]->reflectivity) {
        return true;
      }
    }

    return false;
  };

public:
  using packet_t = PacketT;
  using angle_corrector_t = typename std::conditional<
    (AngleCorrection == AngleCorrectionType::CALIBRATION),
    AngleCorrectorCalibrationBased<PacketT::n_channels, PacketT::degree_subdivisions>,
    AngleCorrectorCorrectionBased<PacketT::n_channels, PacketT::degree_subdivisions>>::type;

  HesaiSensor() = default;
  virtual ~HesaiSensor() = default;

  virtual int get_packet_relative_point_time_offset(
    uint32_t block_id, uint32_t channel_id, const PacketT & packet) = 0;

  int get_earliest_point_time_offset_for_block(uint32_t start_block_id, const PacketT & packet)
  {
    unsigned int n_returns = hesai_packet::get_n_returns(packet.tail.return_mode);
    int min_offset_ns = 0x7FFFFFFF;  // MAXINT (max. positive value)

    for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) {
      for (uint32_t channel_id = 0; channel_id < PacketT::n_channels; ++channel_id) {
        min_offset_ns = std::min(
          min_offset_ns, get_packet_relative_point_time_offset(block_id, channel_id, packet));
      }
    }

    return min_offset_ns;
  }

  virtual ReturnType get_return_type(
    hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
    const std::vector<const typename PacketT::body_t::block_t::unit_t *> & return_units)
  {
    if (is_duplicate_return(return_idx, return_units)) {
      return ReturnType::IDENTICAL;
    }

    switch (return_mode) {
      case hesai_packet::return_mode::SINGLE_FIRST:
        return ReturnType::FIRST;
      case hesai_packet::return_mode::SINGLE_SECOND:
        return ReturnType::SECOND;
      case hesai_packet::return_mode::SINGLE_STRONGEST:
        return ReturnType::STRONGEST;
      case hesai_packet::return_mode::SINGLE_LAST:
        return ReturnType::LAST;
      case hesai_packet::return_mode::DUAL_LAST_STRONGEST:
        if (is_strongest_return(return_idx, return_units)) {
          return return_idx == 0 ? ReturnType::LAST_STRONGEST : ReturnType::STRONGEST;
        } else {
          return return_idx == 0 ? ReturnType::LAST : ReturnType::SECONDSTRONGEST;
        }
      case hesai_packet::return_mode::DUAL_FIRST_SECOND:
        return return_idx == 0 ? ReturnType::FIRST : ReturnType::SECOND;
      case hesai_packet::return_mode::DUAL_FIRST_LAST:
        return return_idx == 0 ? ReturnType::FIRST : ReturnType::LAST;
      case hesai_packet::return_mode::DUAL_FIRST_STRONGEST:
        if (is_strongest_return(return_idx, return_units)) {
          return return_idx == 0 ? ReturnType::FIRST_STRONGEST : ReturnType::STRONGEST;
        } else {
          return return_idx == 0 ? ReturnType::FIRST : ReturnType::SECONDSTRONGEST;
        }
      case hesai_packet::return_mode::DUAL_STRONGEST_SECONDSTRONGEST:
        return return_idx == 0 ? ReturnType::STRONGEST : ReturnType::SECONDSTRONGEST;
      case hesai_packet::return_mode::TRIPLE_FIRST_LAST_STRONGEST:
        switch (return_idx) {
          case 0:
            return ReturnType::FIRST;
          case 1:
            return ReturnType::LAST;
          case 2:
            return ReturnType::STRONGEST;
          default:
            return ReturnType::UNKNOWN;
        }
      default:
        return ReturnType::UNKNOWN;
    }
  }
};

}  // namespace nebula::drivers