File robosense_common.hpp

File List > include > nebula_common > robosense > robosense_common.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_common/nebula_common.hpp"
#include "nebula_common/nebula_status.hpp"

#include <cmath>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

namespace nebula
{
namespace drivers
{

// Flag for detecting Bpearl version
constexpr uint8_t BPEARL_V4_FLAG = 0x04;

struct RobosenseSensorConfiguration : LidarConfigurationBase
{
  uint16_t gnss_port{};  // difop
  double scan_phase{};   // start/end angle
  double dual_return_distance_threshold{};
};

inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg)
{
  os << "Robosense Sensor Configuration:" << '\n';
  os << (LidarConfigurationBase)(arg) << '\n';
  os << "GNSS Port: " << arg.gnss_port << '\n';
  os << "Scan Phase: " << arg.scan_phase;
  return os;
}

inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode)
{
  if (return_mode == "Dual") return ReturnMode::DUAL;
  if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST;
  if (return_mode == "Last") return ReturnMode::SINGLE_LAST;
  if (return_mode == "First") return ReturnMode::SINGLE_FIRST;

  return ReturnMode::UNKNOWN;
}

struct ChannelCorrection
{
  float azimuth{NAN};
  float elevation{NAN};
  uint16_t channel{};

  [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); }
};

struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase
{
  std::vector<ChannelCorrection> calibration;

  void SetChannelSize(const size_t channel_num) { calibration.resize(channel_num); }

  template <typename stream_t>
  inline nebula::Status LoadFromStream(stream_t & stream)
  {
    std::string header;
    std::getline(stream, header);

    char sep;
    size_t laser_id;
    float elevation;
    float azimuth;
    Status load_status = Status::OK;
    for (size_t i = 0; i < calibration.size(); ++i) {
      stream >> laser_id >> sep >> elevation >> sep >> azimuth;

      if (laser_id <= 0 || laser_id > calibration.size()) {
        std::cout << "Invalid laser id: " << laser_id << std::endl;
        load_status = Status::INVALID_CALIBRATION_FILE;
      }
      if (std::isnan(elevation) || std::isnan(azimuth)) {
        std::cout << "Invalid calibration data" << laser_id << "," << elevation << "," << azimuth
                  << std::endl;
        load_status = Status::INVALID_CALIBRATION_FILE;
      }
      if (
        calibration[laser_id - 1].has_value() && calibration[laser_id - 1].elevation != elevation &&
        calibration[laser_id - 1].azimuth != azimuth) {
        std::cout << "Duplicate calibration data for laser id: " << laser_id << std::endl;
        load_status = Status::INVALID_CALIBRATION_FILE;
      }

      ChannelCorrection correction{azimuth, elevation};
      calibration[laser_id - 1] = correction;
    }

    for (auto & calib : calibration) {
      if (!calib.has_value()) {
        std::cout << calib.elevation << "," << calib.azimuth << std::endl;
        std::cout << "Missing calibration data" << std::endl;
        load_status = Status::INVALID_CALIBRATION_FILE;
      }
    }

    if (load_status != Status::OK) {
      for (auto & correction : calibration) {
        correction.elevation = NAN;
        correction.azimuth = NAN;
      }
    }

    return load_status;
  }

  inline nebula::Status LoadFromFile(const std::string & calibration_file)
  {
    std::ifstream ifs(calibration_file);
    if (!ifs) {
      return Status::INVALID_CALIBRATION_FILE;
    }

    const auto status = LoadFromStream(ifs);
    ifs.close();
    return status;
  }

  inline nebula::Status LoadFromString(const std::string & calibration_content)
  {
    std::stringstream ss;
    ss << calibration_content;

    const auto status = LoadFromStream(ss);
    return status;
  }

  //  inline nebula::Status LoadFromDifop(const std::string & calibration_file)

  inline nebula::Status SaveFile(const std::string & calibration_file)
  {
    std::ofstream ofs(calibration_file);
    if (!ofs) {
      return Status::CANNOT_SAVE_FILE;
    }
    ofs << "Laser id,Elevation,Azimuth" << std::endl;

    for (size_t i = 0; i < calibration.size(); ++i) {
      auto laser_id = i + 1;
      float elevation = calibration[i].elevation;
      float azimuth = calibration[i].azimuth;
      ofs << laser_id << "," << elevation << "," << azimuth << std::endl;
    }

    ofs.close();
    return Status::OK;
  }

  [[nodiscard]] inline ChannelCorrection GetCorrection(const size_t channel_id) const
  {
    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
}  // namespace nebula