280 lines
7.4 KiB
C++
280 lines
7.4 KiB
C++
#include "core.hpp"
|
|
#include <cmath>
|
|
#include <deque>
|
|
#include <numbers>
|
|
using std::numbers::pi_v;
|
|
|
|
namespace {
|
|
std::mutex g_mtx;
|
|
|
|
std::string lidar_ip;
|
|
std::string host_ip;
|
|
std::string firmware_version;
|
|
std::string dev_type;
|
|
|
|
ImuRaw g_imu;
|
|
AttitudeRPY g_rpy;
|
|
uint64_t g_last_imu_ms = 0;
|
|
uint64_t g_last_pts_ms = 0;
|
|
|
|
float g_temp_c = 0.f;
|
|
uint32_t g_power_count = 0;
|
|
|
|
Eigen::Matrix4f g_Twm = Eigen::Matrix4f::Identity();
|
|
|
|
CloudRGB g_cloud_live;
|
|
CloudRGB g_cloud_map;
|
|
|
|
Distance6 g_dist6;
|
|
std::vector<BBox3D> g_boxes;
|
|
|
|
std::mutex g_mtx_map;
|
|
|
|
static TemporalCloudAccumulator g_live_acc;
|
|
static TemporalCloudAccumulator g_map_acc;
|
|
} // anon
|
|
|
|
namespace Core {
|
|
|
|
// ==== DEFINIÇÕES CORRETAS (sem Core::) ====
|
|
std::atomic<uint32_t> handle{ 0 };
|
|
std::atomic<bool> connected{ false };
|
|
std::atomic<bool> running{ true };
|
|
std::atomic<bool> debug{ false };
|
|
std::atomic<uint64_t> last_imu_ms{ 0 };
|
|
std::atomic<double> freq_imu{ 0 };
|
|
std::atomic<uint64_t> last_pcl_ms{ 0 };
|
|
std::atomic<double> freq_pcl{ 0 };
|
|
std::atomic<bool> sensor_ok{ false };
|
|
std::atomic<bool> streaming{ false };
|
|
|
|
void init() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_Twm = Eigen::Matrix4f::Identity();
|
|
g_cloud_live = {};
|
|
g_cloud_map = {};
|
|
g_dist6 = {};
|
|
g_boxes.clear();
|
|
|
|
g_live_acc.window_ms = 150; // live com rastro curto (ms)
|
|
g_live_acc.max_points = 200000; // limite de pontos no live
|
|
|
|
g_map_acc.window_ms = 0; // 0 = sem poda temporal (mapa)
|
|
g_map_acc.max_points = 2000000; // cap de segurança pro mapa (ajuste à GPU)
|
|
}
|
|
|
|
void set_lidar_ip(const std::string& ip) {
|
|
lidar_ip = ip;
|
|
}
|
|
void set_host_ip(const std::string& ip) {
|
|
host_ip = ip;
|
|
}
|
|
const std::string& get_lidar_ip() {
|
|
return lidar_ip;
|
|
}
|
|
const std::string& get_host_ip() {
|
|
return host_ip;
|
|
}
|
|
|
|
void set_fw_version(const std::string& v) {
|
|
firmware_version = v;
|
|
}
|
|
const std::string& get_fw_version() {
|
|
return firmware_version;
|
|
}
|
|
|
|
void set_dev_type(const std::string& t) {
|
|
dev_type = t;
|
|
}
|
|
const std::string& get_dev_type() {
|
|
return dev_type;
|
|
}
|
|
|
|
void set_last_imu(const ImuRaw& s) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_imu = s;
|
|
g_last_imu_ms = s.t_ms;
|
|
}
|
|
ImuRaw get_last_imu() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_imu;
|
|
}
|
|
|
|
void set_attitude(const AttitudeRPY& rpy) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_rpy = rpy;
|
|
}
|
|
AttitudeRPY get_attitude() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_rpy;
|
|
}
|
|
|
|
uint64_t get_last_imu_time_ms() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_last_imu_ms;
|
|
}
|
|
void set_last_points_time_ms(uint64_t t) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_last_pts_ms = t;
|
|
}
|
|
uint64_t get_last_points_time_ms() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_last_pts_ms;
|
|
}
|
|
|
|
void set_temperature_c(float t) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_temp_c = t;
|
|
}
|
|
float get_temperature_c() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_temp_c;
|
|
}
|
|
|
|
void set_power_count(uint32_t n) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_power_count = n;
|
|
}
|
|
uint32_t get_power_count() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_power_count;
|
|
}
|
|
|
|
void set_pose_Twm(const Eigen::Matrix4f& Twm) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_Twm = Twm;
|
|
}
|
|
Eigen::Matrix4f get_pose_Twm() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_Twm;
|
|
}
|
|
|
|
void set_cloud_live(const CloudRGB& c) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_cloud_live = c;
|
|
}
|
|
CloudRGB get_cloud_live() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_cloud_live;
|
|
}
|
|
|
|
void set_distances(const Distance6& d) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_dist6 = d;
|
|
}
|
|
Distance6 get_distances() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_dist6;
|
|
}
|
|
|
|
void set_bboxes(const std::vector<BBox3D>& boxes) {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
g_boxes = boxes;
|
|
}
|
|
std::vector<BBox3D> get_bboxes() {
|
|
std::lock_guard<std::mutex> lk(g_mtx);
|
|
return g_boxes;
|
|
}
|
|
|
|
void set_cloud_map(const CloudRGB& c) {
|
|
std::lock_guard<std::mutex> lk(g_mtx_map);
|
|
g_cloud_map = c;
|
|
}
|
|
|
|
CloudRGB get_cloud_map() {
|
|
std::lock_guard<std::mutex> lk(g_mtx_map);
|
|
return g_cloud_map;
|
|
}
|
|
|
|
void clear_cloud_map() {
|
|
std::lock_guard<std::mutex> lk(g_mtx_map);
|
|
g_cloud_map = CloudRGB{};
|
|
}
|
|
|
|
void ResetGlobalMap()
|
|
{
|
|
clear_cloud_map();
|
|
g_map_acc.clear();
|
|
g_map_acc.window_ms = 0;
|
|
g_map_acc.max_points = 2000000;
|
|
}
|
|
|
|
void AccConfigLive(uint64_t window_ms, size_t max_points) {
|
|
g_live_acc.window_ms = window_ms;
|
|
g_live_acc.max_points = max_points;
|
|
}
|
|
|
|
void AccConfigMap(uint64_t window_ms, size_t max_points) {
|
|
g_map_acc.window_ms = window_ms;
|
|
g_map_acc.max_points = max_points;
|
|
}
|
|
|
|
void AccPushLive(const CloudRGB& pkt, uint64_t t_ms) {
|
|
g_live_acc.push(pkt);
|
|
// opcional: se quiser podar no push por ref_ms de fora:
|
|
(void)t_ms; // não precisa aqui; a prune já ocorre no push interno
|
|
}
|
|
|
|
CloudRGB AccBuildLive(uint64_t ref_ms) {
|
|
return g_live_acc.buildMerged(ref_ms);
|
|
}
|
|
|
|
void AccPushMap(const CloudRGB& pkt, uint64_t t_ms) {
|
|
g_map_acc.push(pkt);
|
|
(void)t_ms;
|
|
}
|
|
|
|
CloudRGB AccBuildMap(uint64_t ref_ms) {
|
|
return g_map_acc.buildMerged(ref_ms);
|
|
}
|
|
|
|
|
|
void AccClearLive() {
|
|
// se seu acumulador live usa um deque protegido por mutex:
|
|
std::lock_guard<std::mutex> lk(g_live_acc.mtx); // ajuste ao seu nome real
|
|
g_live_acc.ring.clear();
|
|
}
|
|
|
|
void ClearLiveFrame(uint64_t now_ms) {
|
|
// limpa cloud atual
|
|
CloudRGB empty;
|
|
empty.t_ms = now_ms;
|
|
empty.pts.clear();
|
|
set_cloud_live(empty);
|
|
|
|
// limpa bboxes
|
|
set_bboxes({});
|
|
|
|
// (opcional) invalida distâncias
|
|
InvalidateDistances();
|
|
|
|
// (opcional) zera a “idade” dos pontos para o snapshot/HUD ver que não há dados novos
|
|
set_last_points_time_ms(now_ms);
|
|
}
|
|
|
|
void InvalidateDistances() {
|
|
Distance6 d;
|
|
d.front = std::numeric_limits<double>::quiet_NaN();
|
|
d.back = std::numeric_limits<double>::quiet_NaN();
|
|
d.left = std::numeric_limits<double>::quiet_NaN();
|
|
d.right = std::numeric_limits<double>::quiet_NaN();
|
|
d.up = std::numeric_limits<double>::quiet_NaN();
|
|
d.down = std::numeric_limits<double>::quiet_NaN();
|
|
set_distances(d);
|
|
}
|
|
|
|
|
|
// utils
|
|
float Core::rad2deg(float r) { return r * 180.0f / pi_v<float>; }
|
|
float Core::deg2rad(float d) { return d * pi_v<float> / 180.0f; }
|
|
|
|
float wrapPi(float a) {
|
|
const float pi = pi_v<float>;
|
|
const float twoPi = 2.f * pi;
|
|
while (a > pi) a -= twoPi;
|
|
while (a < -pi) a += twoPi;
|
|
return a;
|
|
}
|
|
|
|
} // namespace Core
|