239 lines
6.8 KiB
C++
239 lines
6.8 KiB
C++
#pragma once
|
|
#include <cstdint>
|
|
#include <vector>
|
|
#include <mutex>
|
|
#include <atomic>
|
|
#include <deque>
|
|
#include <algorithm>
|
|
#include <Eigen/Dense>
|
|
|
|
// Estruturas principais (estado “de operação”)
|
|
|
|
struct AttitudeRPY {
|
|
uint64_t timestamp = 0;
|
|
float roll = 0.f; // rad
|
|
float pitch = 0.f; // rad
|
|
float yaw = 0.f; // rad
|
|
float latencia = 0.f;
|
|
float frequencia = 0.f;
|
|
};
|
|
|
|
struct ImuRaw {
|
|
float acc_x = 0.f; // em g (ou m/s^2, conforme seu pipeline)
|
|
float acc_y = 0.f;
|
|
float acc_z = 0.f;
|
|
float gyro_x = 0.f; // rad/s
|
|
float gyro_y = 0.f;
|
|
float gyro_z = 0.f;
|
|
uint64_t t_ms = 0;
|
|
};
|
|
|
|
struct Distance6 {
|
|
// distâncias médias (m) à frente/trás/esq/dir/cima/baixo (0 = sem obstáculo)
|
|
float front = 0.f;
|
|
float back = 0.f;
|
|
float left = 0.f;
|
|
float right = 0.f;
|
|
float up = 0.f;
|
|
float down = 0.f;
|
|
uint64_t t_ms = 0;
|
|
};
|
|
|
|
struct BBox3D {
|
|
// caixa alinhada aos eixos do frame do sensor
|
|
float min_x, min_y, min_z;
|
|
float max_x, max_y, max_z;
|
|
uint64_t t_ms = 0;
|
|
|
|
// ---- enriquecimentos (novos) ----
|
|
uint32_t voxels = 0; // quantos voxels no componente
|
|
// centro (m)
|
|
float cx = 0.f, cy = 0.f, cz = 0.f;
|
|
// dimensões (m)
|
|
float w = 0.f, h = 0.f, d = 0.f; // w = |y|, h = |z|, d = |x|
|
|
// distâncias úteis (m)
|
|
float dist_m = 0.f; // euclidiana 3D até o centro
|
|
float dist_xy_m = 0.f; // projeção no plano chão
|
|
float frente_m = 0.f; // “range” à frente do sensor (eixo X positivo)
|
|
};
|
|
|
|
struct RGBPoint {
|
|
float x, y, z;
|
|
uint8_t r, g, b;
|
|
};
|
|
|
|
struct CloudRGB {
|
|
std::vector<RGBPoint> pts;
|
|
uint64_t t_ms = 0;
|
|
};
|
|
|
|
struct TemporalCloudAccumulator {
|
|
// janela de tempo (ms) e limite máximo de pontos para a nuvem mesclada
|
|
std::atomic<uint64_t> window_ms{ 200 }; // ex.: 150 ms de cauda
|
|
std::atomic<size_t> max_points{ 200000 }; // limite de pontos
|
|
|
|
struct Block {
|
|
std::vector<RGBPoint> pts; // usamos RGBPoint do Core (x,y,z,r,g,b)
|
|
uint64_t t_ms{ 0 };
|
|
};
|
|
|
|
std::mutex mtx;
|
|
std::deque<Block> ring;
|
|
|
|
// insere um pacote; assume CloudRGB.x/y/z em metros
|
|
void push(const CloudRGB& c) {
|
|
if (c.pts.empty()) return;
|
|
Block b;
|
|
b.t_ms = c.t_ms;
|
|
b.pts = c.pts; // cópia (rápida): RGBPoint é pequeno
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
ring.push_back(std::move(b));
|
|
// poda imediata pelo tempo atual do pacote
|
|
prune_unlocked(c.t_ms);
|
|
}
|
|
|
|
// gera nuvem mesclada (aplica janela e max_points)
|
|
CloudRGB buildMerged(uint64_t now_ms) {
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
prune_unlocked(now_ms);
|
|
|
|
CloudRGB out;
|
|
if (ring.empty()) return out;
|
|
|
|
// estime total e corte pelo max_points (começando dos mais recentes)
|
|
size_t total = 0;
|
|
for (auto it = ring.rbegin(); it != ring.rend(); ++it) {
|
|
total += it->pts.size();
|
|
if (total > max_points.load()) break;
|
|
}
|
|
out.pts.reserve(std::min(total, max_points.load()));
|
|
out.t_ms = ring.back().t_ms; // timestamp do mais recente
|
|
|
|
size_t budget = max_points.load();
|
|
for (auto it = ring.rbegin(); it != ring.rend() && budget > 0; ++it) {
|
|
const auto& v = it->pts;
|
|
size_t take = std::min(budget, v.size());
|
|
out.pts.insert(out.pts.end(), v.end() - take, v.end());
|
|
budget -= take;
|
|
}
|
|
return out;
|
|
}
|
|
|
|
void clear() {
|
|
std::lock_guard<std::mutex> lk(mtx);
|
|
ring.clear();
|
|
}
|
|
|
|
|
|
private:
|
|
void prune_unlocked(uint64_t ref_ms) {
|
|
const uint64_t W = window_ms.load();
|
|
if (W == 0) return; // mapa ilimitado no tempo
|
|
while (!ring.empty() && (ref_ms - ring.front().t_ms) > W) {
|
|
ring.pop_front();
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
namespace Core {
|
|
|
|
// ---- estado global acessível pelos módulos ----
|
|
extern std::atomic<uint32_t> handle;
|
|
extern std::atomic<bool> connected;
|
|
extern std::atomic<bool> running;
|
|
extern std::atomic<bool> debug;
|
|
extern std::atomic<uint64_t> last_imu_ms;
|
|
extern std::atomic<double> freq_imu;
|
|
extern std::atomic<uint64_t> last_pcl_ms;
|
|
extern std::atomic<double> freq_pcl;
|
|
extern std::atomic<bool> sensor_ok;
|
|
extern std::atomic<bool> streaming;
|
|
|
|
void init();
|
|
|
|
// -------- IP --------
|
|
void set_lidar_ip(const std::string& ip);
|
|
void set_host_ip(const std::string& ip);
|
|
const std::string& get_lidar_ip();
|
|
const std::string& get_host_ip();
|
|
|
|
// -------- Versao --------
|
|
void set_fw_version(const std::string& v);
|
|
const std::string& get_fw_version();
|
|
|
|
// -------- Descricao --------
|
|
void set_dev_type(const std::string& t);
|
|
const std::string& get_dev_type();
|
|
|
|
// -------- IMU / atitude --------
|
|
void set_last_imu(const ImuRaw& s);
|
|
ImuRaw get_last_imu();
|
|
|
|
void set_attitude(const AttitudeRPY& rpy);
|
|
AttitudeRPY get_attitude();
|
|
|
|
// -------- tempo de dados (para watchdog) --------
|
|
uint64_t get_last_imu_time_ms();
|
|
void set_last_points_time_ms(uint64_t t);
|
|
uint64_t get_last_points_time_ms();
|
|
|
|
// -------- temperatura / contagem de power-on --------
|
|
void set_temperature_c(float t);
|
|
float get_temperature_c();
|
|
|
|
void set_power_count(uint32_t n);
|
|
uint32_t get_power_count();
|
|
|
|
// -------- pose global (ICP/odometria) --------
|
|
void set_pose_Twm(const Eigen::Matrix4f& Twm);
|
|
Eigen::Matrix4f get_pose_Twm();
|
|
|
|
// -------- nuvens (live e global/map) --------
|
|
void set_cloud_live(const CloudRGB& c);
|
|
CloudRGB get_cloud_live();
|
|
|
|
void set_cloud_map(const CloudRGB& c);
|
|
CloudRGB get_cloud_map();
|
|
|
|
// -------- distâncias 6-eixos & bboxes (resultado de percepção) --------
|
|
void set_distances(const Distance6& d);
|
|
Distance6 get_distances();
|
|
|
|
void set_bboxes(const std::vector<BBox3D>& boxes);
|
|
std::vector<BBox3D> get_bboxes();
|
|
|
|
// -------- utilitários --------
|
|
float rad2deg(float r);
|
|
float deg2rad(float d);
|
|
float wrapPi(float a);
|
|
|
|
// Cloud global (MAP)
|
|
void set_cloud_map(const CloudRGB& c);
|
|
CloudRGB get_cloud_map();
|
|
void clear_cloud_map();
|
|
|
|
// Configuração
|
|
void AccConfigLive(uint64_t window_ms, size_t max_points);
|
|
void AccConfigMap(uint64_t window_ms, size_t max_points);
|
|
|
|
// Push + build
|
|
void AccPushLive(const CloudRGB& pkt, uint64_t t_ms);
|
|
CloudRGB AccBuildLive(uint64_t ref_ms);
|
|
|
|
void AccPushMap(const CloudRGB& pkt, uint64_t t_ms);
|
|
CloudRGB AccBuildMap(uint64_t ref_ms);
|
|
|
|
// Resetar o MAP global (nuvem + acumulador)
|
|
void ResetGlobalMap();
|
|
|
|
// Zera acumulador live (ring de pacotes) e limpa frame atual/bboxes
|
|
void AccClearLive();
|
|
void ClearLiveFrame(uint64_t now_ms);
|
|
|
|
// (opcional) helper: seta distâncias inválidas/NaN para o snapshot saber que caiu
|
|
void InvalidateDistances();
|
|
|
|
|
|
} // namespace Core
|