File angle_corrector_correction_based.hpp
File List > decoders > angle_corrector_correction_based.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/hesai/hesai_common.hpp"
#include "nebula_decoders/nebula_decoders_common/angles.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp"
#include <nebula_common/nebula_common.hpp>
#include <algorithm>
#include <array>
#include <cassert>
#include <cstdint>
#include <functional>
#include <memory>
#include <optional>
#include <ostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
namespace nebula::drivers
{
template <size_t ChannelN, size_t AngleUnit>
class AngleCorrectorCorrectionBased : public AngleCorrector<HesaiCorrection>
{
private:
static constexpr size_t max_azimuth = 360 * AngleUnit;
const std::shared_ptr<const HesaiCorrection> correction_;
rclcpp::Logger logger_;
std::array<float, max_azimuth> cos_{};
std::array<float, max_azimuth> sin_{};
struct FrameAngleInfo
{
static constexpr uint32_t unset = UINT32_MAX;
uint32_t fov_start = unset;
uint32_t fov_end = unset;
uint32_t timestamp_reset = unset;
uint32_t scan_emit = unset;
};
std::vector<FrameAngleInfo> frame_angle_info_;
int find_field(uint32_t azimuth)
{
// Assumes that:
// * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned)
// * the fields are arranged in ascending order (e.g. field 1: 20-140deg, field 2: 140-260deg
// etc.) These assumptions hold for AT128E2X.
int field = correction_->frameNumber - 1;
for (size_t i = 0; i < correction_->frameNumber; ++i) {
if (azimuth < correction_->startFrame[i]) return field;
field = i;
}
// This is never reached if correction_ is correct
return field;
}
bool are_corrected_angles_above_threshold(uint32_t azi, double threshold, bool any, bool eq_ok)
{
for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) {
auto azi_corr = get_corrected_angle_data(azi, channel_id).azimuth_rad;
if (!any && (azi_corr < threshold || (!eq_ok && azi_corr == threshold))) return false;
if (any && (azi_corr > threshold || (eq_ok && azi_corr == threshold))) return true;
}
return !any;
}
uint32_t bin_search(uint32_t start, uint32_t end, double threshold, bool any, bool eq_ok)
{
if (start > end) return FrameAngleInfo::unset;
if (end - start <= 1) {
bool result_start = are_corrected_angles_above_threshold(
normalize_angle<uint32_t>(start, max_azimuth), threshold, any, eq_ok);
if (result_start) return start;
return end;
}
uint32_t next = (start + end) / 2;
bool result_next = are_corrected_angles_above_threshold(
normalize_angle<uint32_t>(next, max_azimuth), threshold, any, eq_ok);
if (result_next) return bin_search(start, next, threshold, any, eq_ok);
return bin_search(next + 1, end, threshold, any, eq_ok);
}
public:
explicit AngleCorrectorCorrectionBased(
const std::shared_ptr<const HesaiCorrection> & sensor_correction, double fov_start_azimuth_deg,
double fov_end_azimuth_deg, double scan_cut_azimuth_deg)
: correction_(sensor_correction), logger_(rclcpp::get_logger("AngleCorrectorCorrectionBased"))
{
if (sensor_correction == nullptr) {
throw std::runtime_error(
"Cannot instantiate AngleCorrectorCorrectionBased without correction data");
}
logger_.set_level(rclcpp::Logger::Level::Debug);
// ////////////////////////////////////////
// Trigonometry lookup tables
// ////////////////////////////////////////
for (size_t i = 0; i < max_azimuth; ++i) {
float rad = 2.f * i * M_PIf / max_azimuth;
cos_[i] = cosf(rad);
sin_[i] = sinf(rad);
}
// ////////////////////////////////////////
// Scan start/end correction lookups
// ////////////////////////////////////////
auto fov_start_rad = deg2rad(fov_start_azimuth_deg);
auto fov_end_rad = deg2rad(fov_end_azimuth_deg);
auto scan_cut_rad = deg2rad(scan_cut_azimuth_deg);
// For each field (= mirror), find the raw block azimuths corresponding FoV start and end
for (size_t field_id = 0; field_id < correction_->frameNumber; ++field_id) {
auto frame_start = correction_->startFrame[field_id];
auto frame_end = correction_->endFrame[field_id];
if (frame_end < frame_start) frame_end += max_azimuth;
FrameAngleInfo & angle_info = frame_angle_info_.emplace_back();
angle_info.fov_start = bin_search(frame_start, frame_end, fov_start_rad, true, true);
angle_info.fov_end = bin_search(angle_info.fov_start, frame_end, fov_end_rad, false, true);
angle_info.scan_emit =
bin_search(angle_info.fov_start, angle_info.fov_end, scan_cut_rad, false, true);
angle_info.timestamp_reset =
bin_search(angle_info.fov_start, angle_info.fov_end, scan_cut_rad, true, true);
if (
angle_info.fov_start == FrameAngleInfo::unset ||
angle_info.fov_end == FrameAngleInfo::unset ||
angle_info.scan_emit == FrameAngleInfo::unset ||
angle_info.timestamp_reset == FrameAngleInfo::unset) {
throw std::runtime_error("Not all necessary angles found!");
}
if (fov_start_rad == scan_cut_rad) {
angle_info.timestamp_reset = angle_info.fov_start;
angle_info.scan_emit = angle_info.fov_start;
} else if (fov_end_rad == scan_cut_rad) {
angle_info.timestamp_reset = angle_info.fov_start;
angle_info.scan_emit = angle_info.fov_end;
}
}
}
CorrectedAngleData get_corrected_angle_data(uint32_t block_azimuth, uint32_t channel_id) override
{
int field = find_field(block_azimuth);
int32_t elevation = correction_->elevation[channel_id] +
correction_->get_elevation_adjust_v3(channel_id, block_azimuth) *
static_cast<int32_t>(AngleUnit / 100);
// Allow negative angles in the radian value. This makes visualization of this field nicer and
// should have no other mathematical implications in downstream modules.
float elevation_rad = 2.f * elevation * M_PI / max_azimuth;
// Then, normalize the integer value to the positive [0, MAX_AZIMUTH] range for array indexing
elevation = (max_azimuth + elevation) % max_azimuth;
int32_t azimuth = (block_azimuth + max_azimuth - correction_->startFrame[field]) * 2 -
correction_->azimuth[channel_id] +
correction_->get_azimuth_adjust_v3(channel_id, block_azimuth) *
static_cast<int32_t>(AngleUnit / 100);
azimuth = (max_azimuth + azimuth) % max_azimuth;
float azimuth_rad = 2.f * azimuth * M_PI / max_azimuth;
return {azimuth_rad, elevation_rad, sin_[azimuth],
cos_[azimuth], sin_[elevation], cos_[elevation]};
}
bool passed_emit_angle(uint32_t last_azimuth, uint32_t current_azimuth) override
{
for (const auto & frame_angles : frame_angle_info_) {
if (angle_is_between(last_azimuth, current_azimuth, frame_angles.scan_emit, false))
return true;
}
return false;
}
bool passed_timestamp_reset_angle(uint32_t last_azimuth, uint32_t current_azimuth) override
{
for (const auto & frame_angles : frame_angle_info_) {
if (angle_is_between(last_azimuth, current_azimuth, frame_angles.timestamp_reset, false))
return true;
}
return false;
}
bool is_inside_fov(uint32_t last_azimuth, uint32_t current_azimuth) override
{
for (const auto & frame_angles : frame_angle_info_) {
if (
angle_is_between(frame_angles.fov_start, frame_angles.fov_end, current_azimuth, false) ||
angle_is_between(frame_angles.fov_start, frame_angles.fov_end, last_azimuth, false))
return true;
}
return false;
}
bool is_inside_overlap(uint32_t last_azimuth, uint32_t current_azimuth) override
{
for (const auto & frame_angles : frame_angle_info_) {
if (
angle_is_between(frame_angles.timestamp_reset, frame_angles.scan_emit, current_azimuth) ||
angle_is_between(frame_angles.timestamp_reset, frame_angles.scan_emit, last_azimuth))
return true;
}
return false;
}
};
} // namespace nebula::drivers