327 lines
12 KiB
C++
327 lines
12 KiB
C++
|
#include <iostream>
|
|||
|
|
#include <filesystem>
|
|||
|
|
#include <chrono>
|
|||
|
|
#include <thread>
|
|||
|
|
#include <atomic>
|
|||
|
|
#include <vector>
|
|||
|
|
#include <cstring>
|
|||
|
|
|
|||
|
|
#include "core.hpp"
|
|||
|
|
#include "metrics.hpp"
|
|||
|
|
#include "viewer_debug.hpp"
|
|||
|
|
#include "status_snapshot.hpp"
|
|||
|
|
|
|||
|
|
#include "livox_lidar_api.h"
|
|||
|
|
#include "livox_lidar_def.h"
|
|||
|
|
|
|||
|
|
|
|||
|
|
namespace fs = std::filesystem;
|
|||
|
|
|
|||
|
|
inline void update_freq(std::atomic<double>& freq_hz, std::atomic<uint64_t>& last_ms, uint64_t now_ms, double alpha = 0.2) // 0<alpha<=1 (suavização)
|
|||
|
|
{
|
|||
|
|
uint64_t prev = last_ms.load(std::memory_order_relaxed);
|
|||
|
|
last_ms.store(now_ms, std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
if (prev == 0 || now_ms <= prev) {
|
|||
|
|
// primeira amostra ou relógio não avançou
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
const double dt_ms = static_cast<double>(now_ms - prev);
|
|||
|
|
if (dt_ms <= 0.0) return;
|
|||
|
|
|
|||
|
|
const double inst_hz = 1000.0 / dt_ms; // ms -> Hz
|
|||
|
|
double f = freq_hz.load(std::memory_order_relaxed);
|
|||
|
|
if (f <= 0.0) {
|
|||
|
|
// inicializa sem EMA na primeira frequência válida
|
|||
|
|
freq_hz.store(inst_hz, std::memory_order_relaxed);
|
|||
|
|
}
|
|||
|
|
else {
|
|||
|
|
// EMA: f = (1-alpha)*f + alpha*inst
|
|||
|
|
f = (1.0 - alpha) * f + alpha * inst_hz;
|
|||
|
|
freq_hz.store(f, std::memory_order_relaxed);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static std::string to_hex(const uint8_t* p, uint16_t n) {
|
|||
|
|
static const char* hex = "0123456789ABCDEF";
|
|||
|
|
std::string s; s.reserve(n * 2);
|
|||
|
|
for (uint16_t i = 0; i < n; ++i) {
|
|||
|
|
uint8_t b = p[i];
|
|||
|
|
s.push_back(hex[b >> 4]);
|
|||
|
|
s.push_back(hex[b & 0xF]);
|
|||
|
|
if (i + 1 < n) s.push_back(' ');
|
|||
|
|
}
|
|||
|
|
return s;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
static std::string ascii_trim_zeros(const uint8_t* p, uint16_t n) {
|
|||
|
|
while (n && p[n - 1] == 0) --n;
|
|||
|
|
return std::string(reinterpret_cast<const char*>(p), n);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
// ===================== Callbacks =====================
|
|||
|
|
|
|||
|
|
|
|||
|
|
void InternalInfoCallback(livox_status status, uint32_t handle, LivoxLidarDiagInternalInfoResponse* response, void*) {
|
|||
|
|
if (status != kLivoxLidarStatusSuccess || !response) return;
|
|||
|
|
|
|||
|
|
const uint8_t* ptr = response->data;
|
|||
|
|
for (uint16_t i = 0; i < response->param_num; ++i) {
|
|||
|
|
uint16_t id = *(uint16_t*)(ptr + 0);
|
|||
|
|
uint16_t length = *(uint16_t*)(ptr + 2);
|
|||
|
|
const uint8_t* val = ptr + 4;
|
|||
|
|
|
|||
|
|
//std::cout << "Param " << id << " len=" << length << " val=" << to_hex(val, length) << std::endl;
|
|||
|
|
|
|||
|
|
if (id == 0x0004 && length >= 12) {
|
|||
|
|
// 0–3: lidar IP, 4–7: subnet, 8–11: host IP
|
|||
|
|
char lidar_ip[16], host_ip[16];
|
|||
|
|
snprintf(lidar_ip, sizeof(lidar_ip), "%u.%u.%u.%u",
|
|||
|
|
val[0], val[1], val[2], val[3]);
|
|||
|
|
snprintf(host_ip, sizeof(host_ip), "%u.%u.%u.%u",
|
|||
|
|
val[8], val[9], val[10], val[11]);
|
|||
|
|
|
|||
|
|
Core::set_lidar_ip(lidar_ip);
|
|||
|
|
Core::set_host_ip(host_ip);
|
|||
|
|
}
|
|||
|
|
else if (id == 0x8001 && length >= 1) {
|
|||
|
|
auto info = ascii_trim_zeros(val, length);
|
|||
|
|
|
|||
|
|
// parse simples de FmVer e BuildTime
|
|||
|
|
auto dev_type = info.find("DevType:");
|
|||
|
|
if (dev_type != std::string::npos) {
|
|||
|
|
auto end = info.find(' ', dev_type);
|
|||
|
|
auto dvtype = info.substr(dev_type + 8, end - (dev_type + 8));
|
|||
|
|
Core::set_dev_type(dvtype);
|
|||
|
|
}
|
|||
|
|
auto fmver_pos = info.find("FmVer:");
|
|||
|
|
if (fmver_pos != std::string::npos) {
|
|||
|
|
auto end = info.find(' ', fmver_pos);
|
|||
|
|
auto fmver = info.substr(fmver_pos + 6, end - (fmver_pos + 6));
|
|||
|
|
Core::set_fw_version(fmver);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (id == 0x8007 && length >= 4) {
|
|||
|
|
uint32_t raw = *(uint32_t*)val;
|
|||
|
|
Core::set_temperature_c(static_cast<float>(raw) / 100.0f);
|
|||
|
|
}
|
|||
|
|
else if (id == 0x8008 && length >= 4) {
|
|||
|
|
Core::set_power_count(*(uint32_t*)val);
|
|||
|
|
}
|
|||
|
|
ptr += (4 + length);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LidarInfoChangeCallback(const uint32_t handle, const LivoxLidarInfo* info, void*) {
|
|||
|
|
std::cout << "[INFO] InfoChange handle=" << handle << "\n";
|
|||
|
|
if (!info) return;
|
|||
|
|
|
|||
|
|
Core::handle.store(handle, std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
// WorkMode + DataType + PointSend
|
|||
|
|
auto wm = SetLivoxLidarWorkMode(handle, kLivoxLidarNormal, nullptr, nullptr);
|
|||
|
|
auto dt = SetLivoxLidarPclDataType(handle, kLivoxLidarCartesianCoordinateHighData, nullptr, nullptr);
|
|||
|
|
auto en = EnableLivoxLidarPointSend(handle, nullptr, nullptr);
|
|||
|
|
std::cout << " WM=" << wm << " DT=" << dt << " EN=" << en << "\n";
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LidarImuDataCallback(const uint32_t handle, const uint8_t, LivoxLidarEthernetPacket* packet, void*) {
|
|||
|
|
Core::handle.store(handle, std::memory_order_relaxed);
|
|||
|
|
using namespace std::chrono;
|
|||
|
|
uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|||
|
|
if (!packet || packet->dot_num == 0) return;
|
|||
|
|
update_freq(Core::freq_imu, Core::last_imu_ms, now_ms);
|
|||
|
|
auto* imu_points = reinterpret_cast<LivoxLidarImuRawPoint*>(packet->data);
|
|||
|
|
const auto imu = imu_points[packet->dot_num - 1];
|
|||
|
|
Metrics::update_from_imu(imu.acc_x, imu.acc_y, imu.acc_z, imu.gyro_x, imu.gyro_y, imu.gyro_z, now_ms);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void LidarPointCloudCallback(const uint32_t handle, const uint8_t, LivoxLidarEthernetPacket* packet, void*) {
|
|||
|
|
Core::handle.store(handle, std::memory_order_relaxed);
|
|||
|
|
using namespace std::chrono;
|
|||
|
|
uint64_t now_ms = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|||
|
|
if (!packet || packet->dot_num == 0) return;
|
|||
|
|
update_freq(Core::freq_pcl, Core::last_pcl_ms, now_ms);
|
|||
|
|
Core::last_pcl_ms.store(now_ms, std::memory_order_relaxed);
|
|||
|
|
Core::streaming.store(true, std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
// 1) converte o pacote bruto em CloudRGB (sem coloração especial aqui)
|
|||
|
|
CloudRGB pkt;
|
|||
|
|
pkt.pts.reserve(packet->dot_num);
|
|||
|
|
pkt.t_ms = now_ms;
|
|||
|
|
|
|||
|
|
auto push = [&](float x, float y, float z) {
|
|||
|
|
RGBPoint p; p.x = x; p.y = y; p.z = z; p.r = 255; p.g = 255; p.b = 255;
|
|||
|
|
pkt.pts.push_back(p);
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
if (packet->data_type == kLivoxLidarCartesianCoordinateHighData) {
|
|||
|
|
auto* pts = reinterpret_cast<LivoxLidarCartesianHighRawPoint*>(packet->data);
|
|||
|
|
for (uint16_t i = 0; i < packet->dot_num; ++i)
|
|||
|
|
push(pts[i].x / 1000.f, pts[i].y / 1000.f, pts[i].z / 1000.f);
|
|||
|
|
}
|
|||
|
|
else if (packet->data_type == kLivoxLidarCartesianCoordinateLowData) {
|
|||
|
|
auto* pts = reinterpret_cast<LivoxLidarCartesianLowRawPoint*>(packet->data);
|
|||
|
|
for (uint16_t i = 0; i < packet->dot_num; ++i)
|
|||
|
|
push(pts[i].x / 100.f, pts[i].y / 100.f, pts[i].z / 100.f);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// 2) empilha no acumulador temporal
|
|||
|
|
Core::AccPushLive(pkt, now_ms);
|
|||
|
|
CloudRGB merged = Core::AccBuildLive(now_ms);
|
|||
|
|
Core::set_cloud_live(merged);
|
|||
|
|
Core::set_last_points_time_ms(now_ms);
|
|||
|
|
Metrics::enqueue_cloud(merged, now_ms);
|
|||
|
|
|
|||
|
|
if (Core::debug.load()) {
|
|||
|
|
if (ViewerDebug::get_mode_map()) {
|
|||
|
|
Core::AccPushMap(pkt, now_ms);
|
|||
|
|
CloudRGB map_merged = Core::AccBuildMap(now_ms);
|
|||
|
|
Core::set_cloud_map(map_merged);
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===================== Main =====================
|
|||
|
|
|
|||
|
|
struct LidarSupervisor {
|
|||
|
|
std::thread th;
|
|||
|
|
std::atomic<bool> alive{ false };
|
|||
|
|
|
|||
|
|
// thresholds (ajusta à vontade)
|
|||
|
|
int64_t pcl_timeout_ms = 2000; // consideramos "sem stream" se >2 s sem nuvem
|
|||
|
|
int64_t imu_timeout_ms = 3000; // IMU pode ser um pouco mais folgado
|
|||
|
|
int64_t diag_period_ms = 3000; // query de diagnóstico a cada 3 s quando ativo
|
|||
|
|
int64_t retry_period_ms = 500; // ciclo de verificação
|
|||
|
|
|
|||
|
|
void start() {
|
|||
|
|
alive = true;
|
|||
|
|
th = std::thread([this] { loop(); });
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void stop() {
|
|||
|
|
alive = false;
|
|||
|
|
if (th.joinable()) th.join();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void loop() {
|
|||
|
|
using namespace std::chrono;
|
|||
|
|
uint64_t last_diag = 0;
|
|||
|
|
|
|||
|
|
while (alive.load() && Core::running.load()) {
|
|||
|
|
const uint64_t now = duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
|
|||
|
|
|
|||
|
|
const auto last_pcl = Core::last_pcl_ms.load(std::memory_order_relaxed);
|
|||
|
|
const auto last_imu = Core::last_imu_ms.load(std::memory_order_relaxed);
|
|||
|
|
const auto h = Core::handle.load(std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
bool have_handle = (h != 0);
|
|||
|
|
|
|||
|
|
// Avalia saúde
|
|||
|
|
bool pcl_ok = have_handle && (last_pcl != 0) && (now - last_pcl <= (uint64_t)pcl_timeout_ms);
|
|||
|
|
bool imu_ok = have_handle && (last_imu != 0) && (now - last_imu <= (uint64_t)imu_timeout_ms);
|
|||
|
|
bool sensor_ok = have_handle && (pcl_ok || imu_ok);
|
|||
|
|
|
|||
|
|
// Atualiza flag pública
|
|||
|
|
bool prev_ok = Core::sensor_ok.exchange(sensor_ok, std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
// Se temos handle mas stream caiu, tenta reabilitar envio
|
|||
|
|
if (have_handle && !Core::connected.load(std::memory_order_relaxed)) {
|
|||
|
|
// força "enable point send" e tipo dos pontos de novo (idempotente)
|
|||
|
|
auto dt = SetLivoxLidarPclDataType(h, kLivoxLidarCartesianCoordinateHighData, nullptr, nullptr);
|
|||
|
|
auto en = EnableLivoxLidarPointSend(h, nullptr, nullptr);
|
|||
|
|
Core::connected.store((dt == kLivoxLidarStatusSuccess || en == kLivoxLidarStatusSuccess), std::memory_order_relaxed);
|
|||
|
|
if (dt != kLivoxLidarStatusSuccess || en != kLivoxLidarStatusSuccess) {
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Diagnóstico periódico (temp/power count)
|
|||
|
|
if (sensor_ok && have_handle && (now - last_diag >= (uint64_t)diag_period_ms)) {
|
|||
|
|
last_diag = now;
|
|||
|
|
QueryLivoxLidarInternalInfo(h, InternalInfoCallback, nullptr);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// Transições de estado úteis pra log
|
|||
|
|
if (sensor_ok && !prev_ok) {
|
|||
|
|
std::cout << "[SUP] LiDAR OK (stream ativo)\n";
|
|||
|
|
|
|||
|
|
if (Core::debug.load()) {
|
|||
|
|
ViewerDebug::start(false);
|
|||
|
|
std::cout << "[VIEWER] Debug viewer ON. Pressione Ctrl+C para sair.\n";
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else if (!sensor_ok && prev_ok) {
|
|||
|
|
std::cout << "[SUP] LiDAR OFF/DEGRADED (sem dados recentes)\n";
|
|||
|
|
Core::connected.store(false, std::memory_order_relaxed);
|
|||
|
|
Core::streaming.store(false, std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
// —— FLUSH de estado ativo ——
|
|||
|
|
Core::AccClearLive();
|
|||
|
|
Core::ClearLiveFrame(now);
|
|||
|
|
Core::freq_pcl.store(0.0, std::memory_order_relaxed);
|
|||
|
|
Core::freq_imu.store(0.0, std::memory_order_relaxed);
|
|||
|
|
Metrics::update_from_imu(0, 0, 0, 0, 0, 0, now);
|
|||
|
|
|
|||
|
|
if (Core::debug.load()) {
|
|||
|
|
ViewerDebug::stop();
|
|||
|
|
std::cout << "[VIEWER] Debug viewer OFF.\n";
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
StatusSnapshot snap = StatusSnapshot::collect_now();
|
|||
|
|
bool status = snap.publish_state_partial(10, 20);
|
|||
|
|
bool status_imu = snap.publish_state_imu();
|
|||
|
|
//std::string json = snap.to_json_string();
|
|||
|
|
//std::cout << "PUBLICADO: " << status << " - " << json << "\r\n";
|
|||
|
|
|
|||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(retry_period_ms));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
static LidarSupervisor g_sup;
|
|||
|
|
|
|||
|
|
int main(int argc, char** argv) {
|
|||
|
|
if (argc < 2) {
|
|||
|
|
std::cout << "Uso: app <config.json> [--debug]\n";
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
const std::string cfg = argv[1];
|
|||
|
|
Core::debug.store((argc >= 3 && std::strcmp(argv[2], "--debug") == 0), std::memory_order_relaxed);
|
|||
|
|
|
|||
|
|
Core::init();
|
|||
|
|
Metrics::init(MetricsParams{});
|
|||
|
|
Metrics::start_worker();
|
|||
|
|
StatusSnapshot::start_publisher();
|
|||
|
|
|
|||
|
|
if (!LivoxLidarSdkInit(cfg.c_str())) {
|
|||
|
|
std::cerr << "[ERR] LivoxLidarSdkInit falhou\n";
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
SetLivoxLidarPointCloudCallBack(LidarPointCloudCallback, nullptr);
|
|||
|
|
SetLivoxLidarImuDataCallback(LidarImuDataCallback, nullptr);
|
|||
|
|
SetLivoxLidarInfoChangeCallback(LidarInfoChangeCallback, nullptr);
|
|||
|
|
|
|||
|
|
if (!LivoxLidarSdkStart()) {
|
|||
|
|
std::cerr << "[ERR] LivoxLidarSdkStart falhou\n";
|
|||
|
|
LivoxLidarSdkUninit();
|
|||
|
|
return -1;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// supervisor ON (faz query inicial e monitora tudo)
|
|||
|
|
g_sup.start();
|
|||
|
|
|
|||
|
|
while (Core::running.load()) std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
|||
|
|
|
|||
|
|
g_sup.stop();
|
|||
|
|
Metrics::stop_worker();
|
|||
|
|
LivoxLidarSdkUninit();
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|