342 lines
12 KiB
C++
342 lines
12 KiB
C++
|
#pragma once
|
|||
|
|
#include <string>
|
|||
|
|
#include <vector>
|
|||
|
|
#include <atomic>
|
|||
|
|
#include <algorithm>
|
|||
|
|
#include <sstream>
|
|||
|
|
#include <iomanip>
|
|||
|
|
#include <chrono>
|
|||
|
|
#include <iostream>
|
|||
|
|
|
|||
|
|
// ===== Depende do teu projeto =====
|
|||
|
|
#include "core.hpp"
|
|||
|
|
#include "redis_publisher.hpp"
|
|||
|
|
|
|||
|
|
// Se quiser habilitar nlohmann/json, remova o comentário:
|
|||
|
|
// #include <nlohmann/json.hpp>
|
|||
|
|
|
|||
|
|
// ======= Estruturas =======
|
|||
|
|
struct SnapshotFreq {
|
|||
|
|
double pcl{ 0.0 };
|
|||
|
|
double imu{ 0.0 };
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
struct SnapshotIMU {
|
|||
|
|
uint64_t timestamp = 0;
|
|||
|
|
double roll_deg{ 0.0 };
|
|||
|
|
double pitch_deg{ 0.0 };
|
|||
|
|
double yaw_deg{ 0.0 };
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
struct SnapshotStatus {
|
|||
|
|
double temperatura_c{ 0.0 };
|
|||
|
|
uint32_t vezes_ligado{ 0 };
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
struct SnapshotDist {
|
|||
|
|
double frente{ 0.0 };
|
|||
|
|
double tras{ 0.0 };
|
|||
|
|
double esquerda{ 0.0 };
|
|||
|
|
double direita{ 0.0 };
|
|||
|
|
double sup{ 0.0 };
|
|||
|
|
double inf{ 0.0 };
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
struct StatusSnapshot {
|
|||
|
|
uint64_t timestamp_ms{ 0 };
|
|||
|
|
bool conectado{ true }; // placeholder por enquanto
|
|||
|
|
bool executando{ false };
|
|||
|
|
bool sensor_ativo{ false };
|
|||
|
|
std::string lidar_ip;
|
|||
|
|
std::string host_ip;
|
|||
|
|
std::string firmware_version;
|
|||
|
|
std::string dev_type;
|
|||
|
|
uint32_t handle{ 0 };
|
|||
|
|
SnapshotFreq freq;
|
|||
|
|
SnapshotIMU imu;
|
|||
|
|
SnapshotStatus status;
|
|||
|
|
SnapshotDist dist;
|
|||
|
|
std::vector<BBox3D> bboxes;
|
|||
|
|
|
|||
|
|
static std::unique_ptr<RedisPublisher> g_pub;
|
|||
|
|
|
|||
|
|
static void start_publisher() {
|
|||
|
|
const std::string uri = "tcp://localhost:6379";
|
|||
|
|
StatusSnapshot::g_pub = std::make_unique<RedisPublisher>(uri);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== Coletor: puxa dados do sistema e retorna um Snapshot pronto =====
|
|||
|
|
static StatusSnapshot collect_now() {
|
|||
|
|
StatusSnapshot s{};
|
|||
|
|
|
|||
|
|
// tempo
|
|||
|
|
using namespace std::chrono;
|
|||
|
|
s.timestamp_ms = duration_cast<milliseconds>(system_clock::now().time_since_epoch()).count();
|
|||
|
|
|
|||
|
|
// flags globais/atômicos do teu runtime
|
|||
|
|
s.conectado = Core::connected.load(std::memory_order_relaxed);
|
|||
|
|
s.executando = Core::running.load(std::memory_order_relaxed);
|
|||
|
|
s.sensor_ativo = Core::streaming.load(std::memory_order_relaxed);
|
|||
|
|
s.handle = Core::handle.load(std::memory_order_relaxed);
|
|||
|
|
s.host_ip = Core::get_host_ip();
|
|||
|
|
s.lidar_ip = Core::get_lidar_ip();
|
|||
|
|
s.firmware_version = Core::get_fw_version();
|
|||
|
|
s.dev_type = Core::get_dev_type();
|
|||
|
|
|
|||
|
|
// freq (Hz). Se teus g_freq_* guardam Hz, só converte p/ double:
|
|||
|
|
s.freq.pcl = static_cast<double>(Core::freq_pcl.load(std::memory_order_relaxed));
|
|||
|
|
s.freq.imu = static_cast<double>(Core::freq_imu.load(std::memory_order_relaxed));
|
|||
|
|
|
|||
|
|
// IMU (em graus)
|
|||
|
|
const auto att = Core::get_attitude();
|
|||
|
|
s.imu.timestamp = att.timestamp;
|
|||
|
|
s.imu.roll_deg = Core::rad2deg(att.roll);
|
|||
|
|
s.imu.pitch_deg = Core::rad2deg(att.pitch);
|
|||
|
|
s.imu.yaw_deg = Core::rad2deg(att.yaw);
|
|||
|
|
|
|||
|
|
// status (temperatura e contagem de ligadas)
|
|||
|
|
s.status.temperatura_c = Core::get_temperature_c();
|
|||
|
|
s.status.vezes_ligado = Core::get_power_count();
|
|||
|
|
|
|||
|
|
// distâncias (ajusta os nomes conforme teu Distance6)
|
|||
|
|
const auto d = Core::get_distances();
|
|||
|
|
s.dist.frente = d.front;
|
|||
|
|
s.dist.tras = d.back;
|
|||
|
|
s.dist.esquerda = d.left;
|
|||
|
|
s.dist.direita = d.right;
|
|||
|
|
s.dist.sup = d.up;
|
|||
|
|
s.dist.inf = d.down;
|
|||
|
|
|
|||
|
|
// bboxes — adapta para tua fonte. Exemplo genérico:
|
|||
|
|
// Supondo que exista Core::get_bboxes() -> std::vector<Deteccao>
|
|||
|
|
// com campos equivalentes.
|
|||
|
|
for (const auto& b : Core::get_bboxes()) {
|
|||
|
|
s.bboxes.push_back(std::move(b));
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return s;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// ===== Serialização manual (sem dependências) =====
|
|||
|
|
std::string to_json_string() const {
|
|||
|
|
std::ostringstream os;
|
|||
|
|
os << std::fixed << std::setprecision(3);
|
|||
|
|
os << "{";
|
|||
|
|
os << "\"timestamp\":" << timestamp_ms << ",";
|
|||
|
|
os << "\"conectado\":" << (conectado ? "true" : "false") << ",";
|
|||
|
|
os << "\"executando\":" << (executando ? "true" : "false") << ",";
|
|||
|
|
os << "\"sensor_ativo\":" << (sensor_ativo ? "true" : "false") << ",";
|
|||
|
|
os << "\"host_ip\":" << host_ip << ",";
|
|||
|
|
os << "\"lidar_ip\":" << lidar_ip << ",";
|
|||
|
|
os << "\"firmware_version\":" << firmware_version << ",";
|
|||
|
|
os << "\"dev_type\":" << dev_type << ",";
|
|||
|
|
os << "\"handle\":" << handle << ",";
|
|||
|
|
|
|||
|
|
os << "\"freq\":{"
|
|||
|
|
<< "\"pcl\":" << freq.pcl << ","
|
|||
|
|
<< "\"imu\":" << freq.imu << "},";
|
|||
|
|
|
|||
|
|
os << "\"imu\":{"
|
|||
|
|
<< "\"timestamp\":" << imu.timestamp << ","
|
|||
|
|
<< "\"roll\":" << imu.roll_deg << ","
|
|||
|
|
<< "\"pitch\":" << imu.pitch_deg << ","
|
|||
|
|
<< "\"yaw\":" << imu.yaw_deg << "},";
|
|||
|
|
|
|||
|
|
os << "\"status\":{"
|
|||
|
|
<< "\"temperatura\":" << status.temperatura_c << ","
|
|||
|
|
<< "\"vezes_ligado\":" << status.vezes_ligado << "},";
|
|||
|
|
|
|||
|
|
os << "\"distancias\":{"
|
|||
|
|
<< "\"frente\":" << dist.frente << ","
|
|||
|
|
<< "\"tras\":" << dist.tras << ","
|
|||
|
|
<< "\"esquerda\":" << dist.esquerda << ","
|
|||
|
|
<< "\"direita\":" << dist.direita << ","
|
|||
|
|
<< "\"sup\":" << dist.sup << ","
|
|||
|
|
<< "\"inf\":" << dist.inf << "},";
|
|||
|
|
|
|||
|
|
os << "\"bboxes\":[";
|
|||
|
|
for (size_t i = 0; i < bboxes.size(); ++i) {
|
|||
|
|
const auto& b = bboxes[i];
|
|||
|
|
os << "{"
|
|||
|
|
// limites da caixa
|
|||
|
|
<< "\"min_x\":" << b.min_x << ","
|
|||
|
|
<< "\"min_y\":" << b.min_y << ","
|
|||
|
|
<< "\"min_z\":" << b.min_z << ","
|
|||
|
|
<< "\"max_x\":" << b.max_x << ","
|
|||
|
|
<< "\"max_y\":" << b.max_y << ","
|
|||
|
|
<< "\"max_z\":" << b.max_z << ","
|
|||
|
|
|
|||
|
|
// tempo e voxel info
|
|||
|
|
<< "\"t_ms\":" << b.t_ms << ","
|
|||
|
|
<< "\"voxels\":" << b.voxels << ","
|
|||
|
|
|
|||
|
|
// centro e dimensões
|
|||
|
|
<< "\"cx\":" << b.cx << ","
|
|||
|
|
<< "\"cy\":" << b.cy << ","
|
|||
|
|
<< "\"cz\":" << b.cz << ","
|
|||
|
|
<< "\"w\":" << b.w << ","
|
|||
|
|
<< "\"h\":" << b.h << ","
|
|||
|
|
<< "\"d\":" << b.d << ","
|
|||
|
|
|
|||
|
|
// distâncias derivadas
|
|||
|
|
<< "\"dist_m\":" << b.dist_m << ","
|
|||
|
|
<< "\"dist_xy_m\":" << b.dist_xy_m << ","
|
|||
|
|
<< "\"frente_m\":" << b.frente_m
|
|||
|
|
|
|||
|
|
// fecha objeto
|
|||
|
|
<< "}";
|
|||
|
|
|
|||
|
|
if (i + 1 < bboxes.size())
|
|||
|
|
os << ",";
|
|||
|
|
}
|
|||
|
|
os << "]";
|
|||
|
|
|
|||
|
|
os << "}";
|
|||
|
|
return os.str();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// --- 2.1 Compactador de BBoxes ---
|
|||
|
|
std::string bboxes_to_json_compact(std::size_t max_boxes) const {
|
|||
|
|
if (bboxes.empty()) return "[]";
|
|||
|
|
|
|||
|
|
// ordena por proximidade frontal (menor frente_m primeiro)
|
|||
|
|
std::vector<BBox3D> v = bboxes;
|
|||
|
|
std::sort(v.begin(), v.end(),
|
|||
|
|
[](const BBox3D& a, const BBox3D& b) { return a.frente_m < b.frente_m; });
|
|||
|
|
|
|||
|
|
if (v.size() > max_boxes) v.resize(max_boxes);
|
|||
|
|
|
|||
|
|
std::ostringstream os;
|
|||
|
|
os.setf(std::ios::fixed); os << std::setprecision(3);
|
|||
|
|
os << "[";
|
|||
|
|
|
|||
|
|
for (std::size_t i = 0; i < v.size(); ++i) {
|
|||
|
|
const auto& b = v[i];
|
|||
|
|
os << "{"
|
|||
|
|
<< "\"min_x\":" << b.min_x << ","
|
|||
|
|
<< "\"min_y\":" << b.min_y << ","
|
|||
|
|
<< "\"min_z\":" << b.min_z << ","
|
|||
|
|
<< "\"max_x\":" << b.max_x << ","
|
|||
|
|
<< "\"max_y\":" << b.max_y << ","
|
|||
|
|
<< "\"max_z\":" << b.max_z << ","
|
|||
|
|
<< "\"t_ms\":" << b.t_ms << ","
|
|||
|
|
<< "\"voxels\":" << b.voxels << ","
|
|||
|
|
<< "\"cx\":" << b.cx << ","
|
|||
|
|
<< "\"cy\":" << b.cy << ","
|
|||
|
|
<< "\"cz\":" << b.cz << ","
|
|||
|
|
<< "\"w\":" << b.w << ","
|
|||
|
|
<< "\"h\":" << b.h << ","
|
|||
|
|
<< "\"d\":" << b.d << ","
|
|||
|
|
<< "\"dist_m\":" << b.dist_m << ","
|
|||
|
|
<< "\"dist_xy_m\":" << b.dist_xy_m << ","
|
|||
|
|
<< "\"frente_m\":" << b.frente_m
|
|||
|
|
<< "}";
|
|||
|
|
if (i + 1 < v.size()) os << ",";
|
|||
|
|
}
|
|||
|
|
os << "]";
|
|||
|
|
return os.str();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// --- 2.2 Publisher parcial (Hash + Stream) ---
|
|||
|
|
bool publish_state_partial(int ttl_sec_hash, std::size_t max_bboxes) const
|
|||
|
|
{
|
|||
|
|
if (!g_pub) {
|
|||
|
|
std::cerr << "[STATUS] RedisPublisher não inicializado\n";
|
|||
|
|
return false;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// monta pares para o JSON (paths com '.')
|
|||
|
|
std::vector<std::pair<std::string, std::string>> J;
|
|||
|
|
J.reserve(32);
|
|||
|
|
|
|||
|
|
auto addj_i = [&](std::string path, long long v) {
|
|||
|
|
J.emplace_back(std::move(path), std::to_string(v));
|
|||
|
|
};
|
|||
|
|
auto addj_b = [&](std::string path, bool v) {
|
|||
|
|
// booleans como true/false (sem aspas) viram bool real no JSON
|
|||
|
|
J.emplace_back(std::move(path), v ? "true" : "false");
|
|||
|
|
};
|
|||
|
|
auto addj_f = [&](std::string path, double v, int prec = 3) {
|
|||
|
|
std::ostringstream oss;
|
|||
|
|
oss.setf(std::ios::fixed); oss << std::setprecision(prec) << v;
|
|||
|
|
J.emplace_back(std::move(path), oss.str()); // número no JSON
|
|||
|
|
};
|
|||
|
|
auto addj_s = [&](std::string path, const std::string& v) {
|
|||
|
|
// adiciona string com aspas no JSON
|
|||
|
|
std::ostringstream oss;
|
|||
|
|
oss << "\"" << v << "\"";
|
|||
|
|
J.emplace_back(std::move(path), oss.str());
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
// ---- mesmos campos que você já preencheu no HASH ----
|
|||
|
|
addj_i("timestamp", static_cast<long long>(timestamp_ms));
|
|||
|
|
addj_b("conectado", conectado);
|
|||
|
|
addj_b("executando", executando);
|
|||
|
|
addj_b("sensor_ativo", sensor_ativo);
|
|||
|
|
addj_s("host_ip", host_ip);
|
|||
|
|
addj_s("lidar_ip", lidar_ip);
|
|||
|
|
addj_s("firmware_version", Core::get_fw_version());
|
|||
|
|
addj_s("dev_type", Core::get_dev_type());
|
|||
|
|
addj_i("handle", static_cast<long long>(handle));
|
|||
|
|
|
|||
|
|
addj_f("freq.pcl", freq.pcl, 2);
|
|||
|
|
addj_f("freq.imu", freq.imu, 2);
|
|||
|
|
|
|||
|
|
addj_f("imu.timestamp", imu.timestamp, 2);
|
|||
|
|
addj_f("imu.roll", imu.roll_deg, 2);
|
|||
|
|
addj_f("imu.pitch", imu.pitch_deg, 2);
|
|||
|
|
addj_f("imu.yaw", imu.yaw_deg, 2);
|
|||
|
|
|
|||
|
|
addj_f("status.temperatura", status.temperatura_c, 2);
|
|||
|
|
addj_i("status.vezes_ligado", static_cast<long long>(status.vezes_ligado));
|
|||
|
|
|
|||
|
|
addj_f("dist.frente", dist.frente, 2);
|
|||
|
|
addj_f("dist.tras", dist.tras, 2);
|
|||
|
|
addj_f("dist.esquerda", dist.esquerda, 2);
|
|||
|
|
addj_f("dist.direita", dist.direita, 2);
|
|||
|
|
addj_f("dist.sup", dist.sup, 2);
|
|||
|
|
addj_f("dist.inf", dist.inf, 2);
|
|||
|
|
|
|||
|
|
// opcional: bboxes como array JSON no snapshot (no mesmo documento)
|
|||
|
|
if (!bboxes.empty()) {
|
|||
|
|
std::string bb_json = bboxes_to_json_compact(max_bboxes); // deve ser JSON válido
|
|||
|
|
if (!bb_json.empty()) {
|
|||
|
|
// como o merge tenta json::parse(value), passar o array/objeto funciona direto
|
|||
|
|
J.emplace_back("bboxes", bb_json); // ex.: "[{...},{...}]"
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
// grava/mescla no snapshot JSON (sem apagar outros campos já existentes)
|
|||
|
|
bool ok_json = g_pub->set_json_merge_paths("ctx:dados_modulo_Lvx", J, ttl_sec_hash);
|
|||
|
|
//std::cout << "OK_JSON: " << ok_json << "\n";
|
|||
|
|
return ok_json;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
bool publish_state_imu() {
|
|||
|
|
const auto att = Core::get_attitude();
|
|||
|
|
|
|||
|
|
std::vector<std::pair<std::string, std::string>> J;
|
|||
|
|
J.reserve(32);
|
|||
|
|
|
|||
|
|
auto addj_i = [&](std::string path, long long v) {
|
|||
|
|
J.emplace_back(std::move(path), std::to_string(v));
|
|||
|
|
};
|
|||
|
|
auto addj_f = [&](std::string path, double v, int prec = 3) {
|
|||
|
|
std::ostringstream oss;
|
|||
|
|
oss.setf(std::ios::fixed); oss << std::setprecision(prec) << v;
|
|||
|
|
J.emplace_back(std::move(path), oss.str()); // número no JSON
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
addj_i("timestamp", static_cast<long long>(att.timestamp));
|
|||
|
|
addj_f("frequencia", att.frequencia, 4);
|
|||
|
|
addj_f("latencia", att.latencia, 4);
|
|||
|
|
addj_f("roll", Core::rad2deg(att.roll), 3);
|
|||
|
|
addj_f("pitch", Core::rad2deg(att.pitch), 3);
|
|||
|
|
addj_f("yaw", Core::rad2deg(att.yaw), 3);
|
|||
|
|
|
|||
|
|
// grava/mescla no snapshot JSON (sem apagar outros campos já existentes)
|
|||
|
|
bool ok_json = g_pub->set_json_merge_paths("ctx:dados_modulo_Imu", J, 3);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
};
|