72 lines
2.2 KiB
C++
72 lines
2.2 KiB
C++
// Headless skeleton app: loads config.json, one reader thread per lidar.
|
|
// ./lidar_app [config.json]
|
|
#include "lidarlib/lidarlib.hpp"
|
|
#include <atomic>
|
|
#include <csignal>
|
|
#include <cstdio>
|
|
#include <memory>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
namespace {
|
|
|
|
std::atomic<bool> g_running{true};
|
|
void on_signal(int) { g_running = false; }
|
|
|
|
void run_lidar(lidarlib::LidarConfig cfg) {
|
|
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg);
|
|
lidarlib::ErrorCode err = lidar->open();
|
|
if (err != lidarlib::ErrorCode::Ok) {
|
|
fprintf(stderr, "[%s] khong mo duoc %s %s:%u (%s)\n",
|
|
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
|
|
lidarlib::to_string(err));
|
|
return;
|
|
}
|
|
printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n",
|
|
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
|
|
cfg.model.c_str(), cfg.inverted);
|
|
|
|
while (g_running) {
|
|
lidarlib::ScanResult result;
|
|
if (!lidar->recv_scan(result, 1000)) continue;
|
|
|
|
const lidarlib::LaserScan& scan = result.scan;
|
|
const lidarlib::ExtraInfo& info = result.info;
|
|
|
|
printf("[%s] %zu diem | ts=%u ms | model=%s | err=0x%02X\n",
|
|
cfg.name.c_str(), scan.ranges.size(), scan.timestamp_ms,
|
|
info.detected_model.c_str(), info.error_status);
|
|
}
|
|
|
|
lidar->close();
|
|
printf("[%s] da dong\n", cfg.name.c_str());
|
|
}
|
|
|
|
} // namespace
|
|
|
|
int main(int argc, char** argv) {
|
|
setvbuf(stdout, nullptr, _IOLBF, 0);
|
|
|
|
const std::string config_path = (argc > 1) ? argv[1] : "config.json";
|
|
|
|
lidarlib::Config cfg = lidarlib::load_config(config_path);
|
|
lidarlib::save_config(config_path, cfg); // ensure the file exists
|
|
|
|
if (cfg.lidars.empty()) {
|
|
fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str());
|
|
return 1;
|
|
}
|
|
|
|
std::signal(SIGINT, on_signal);
|
|
std::signal(SIGTERM, on_signal);
|
|
|
|
std::vector<std::thread> threads;
|
|
threads.reserve(cfg.lidars.size());
|
|
for (const auto& lc : cfg.lidars) threads.emplace_back(run_lidar, lc);
|
|
|
|
printf("Dang chay %zu lidar tu %s. Ctrl-C de dung.\n",
|
|
cfg.lidars.size(), config_path.c_str());
|
|
for (auto& t : threads) t.join();
|
|
return 0;
|
|
}
|