refactor: restructure lidarlib into xlidar-driver plugin SDK
- LidarManager facade (liblidar_manager.so): dlopen plugin discovery, available_drivers map<driver_id, PluginRegistry>, create_lidar_device, config.json load/save with legacy lidarlib migration - Common LidarDriverInterface + DriverInfo/DeviceConfig plugin ABI (extern C get_driver_info / create_driver_instance) - Plugins: driver_rplidar (ported from xlocd, Slamtec SDK), driver_olei, driver_sick_code (TiM CoLa-A), driver_sick_safety (nanoScan3), driver_espe - Diagnostics extended with rplidar health + firmware; FOV filter window, range override and legacy remap window unified in DeviceConfig - Rewritten README, diagnostics doc and examples (list_drivers, example, lidar_app) Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
6
examples/CMakeLists.txt
Normal file
6
examples/CMakeLists.txt
Normal file
@@ -0,0 +1,6 @@
|
||||
# Demo binaries. Run from the build dir so the default plugins path
|
||||
# ("plugins") resolves to build/plugins.
|
||||
foreach(demo list_drivers example lidar_app)
|
||||
add_executable(${demo} ${demo}.cpp)
|
||||
target_link_libraries(${demo} PRIVATE lidar_manager)
|
||||
endforeach()
|
||||
@@ -1,39 +0,0 @@
|
||||
// ESPE LGA60 driver example (TCP, port 8080 theo cấu hình mặc định của hãng).
|
||||
#include "lidarlib/espe_lidar.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
int main() {
|
||||
lidarlib::EspeDriver drv(lidarlib::MODEL_ESPE_LGA60, "192.168.1.88", 8080);
|
||||
|
||||
lidarlib::ErrorCode err = drv.open();
|
||||
if (err != lidarlib::ErrorCode::Ok) {
|
||||
fprintf(stderr, "Không kết nối được tới lidar ESPE: %s\n",
|
||||
lidarlib::to_string(err));
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 1000)) {
|
||||
fprintf(stderr, "Timeout hoặc lỗi nhận dữ liệu: %s\n",
|
||||
lidarlib::to_string(drv.last_error()));
|
||||
continue;
|
||||
}
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
printf("Scan #%d: %zu điểm, model=%s, diag=%s\n",
|
||||
i, scan.ranges.size(), result.info.detected_model.c_str(),
|
||||
lidarlib::to_string(drv.get_diagnostics()).c_str());
|
||||
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
|
||||
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
||||
}
|
||||
}
|
||||
|
||||
drv.close();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Build:
|
||||
// g++ -std=c++17 -O2 -Iinclude -o espe_example examples/espe_example.cpp src/espe_lidar.cpp src/olei_lidar.cpp
|
||||
@@ -1,57 +1,71 @@
|
||||
// OLEI driver example.
|
||||
#include "lidarlib/lidar.hpp"
|
||||
// Open one lidar through the manager and read 10 scans.
|
||||
// ./example <driver_id> [plugins_dir]
|
||||
// ./example olei_lidar_driver
|
||||
// ./example rplidar_c1_driver
|
||||
#include "lidar_manager.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
int main() {
|
||||
lidarlib::Driver drv(lidarlib::MODEL_VB);
|
||||
int main(int argc, char** argv) {
|
||||
if (argc < 2) {
|
||||
fprintf(stderr, "usage: %s <driver_id> [plugins_dir]\n", argv[0]);
|
||||
return 1;
|
||||
}
|
||||
const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins";
|
||||
|
||||
lidarlib::ErrorCode err = drv.open();
|
||||
if (err != lidarlib::ErrorCode::Ok) {
|
||||
fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err));
|
||||
xlidar::LidarManager manager(plugins_dir);
|
||||
manager.load_all_plugins();
|
||||
|
||||
xlidar::DeviceConfig cfg;
|
||||
cfg.name = "demo";
|
||||
cfg.driver_id = argv[1];
|
||||
// Network drivers use cfg.ip / cfg.port (0 = driver default port);
|
||||
// the rplidar driver uses cfg.serial_port / cfg.baudrate instead.
|
||||
|
||||
auto lidar = manager.create_lidar_device(cfg);
|
||||
if (!lidar) return 1;
|
||||
|
||||
xlidar::DriverInfo info = lidar->get_driver_info();
|
||||
printf("driver: %s (%s %s)\n", info.driver_id.c_str(), info.vendor.c_str(),
|
||||
info.model.c_str());
|
||||
|
||||
xlidar::ErrorCode err = lidar->open();
|
||||
if (err != xlidar::ErrorCode::Ok) {
|
||||
fprintf(stderr, "open failed: %s\n", xlidar::to_string(err));
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Chờ cảm biến sẵn sàng: đã nhận được ít nhất một scan hoàn chỉnh
|
||||
// và thiết bị không báo lỗi (motor/điện áp/nhiệt độ...)
|
||||
if (!drv.wait_ready(5000)) {
|
||||
fprintf(stderr, "Cảm biến chưa sẵn sàng: %s (diag: %s)\n",
|
||||
lidarlib::to_string(drv.last_error()),
|
||||
lidarlib::to_string(drv.get_diagnostics()).c_str());
|
||||
drv.close();
|
||||
// Wait until the sensor is usable: at least one complete scan decoded
|
||||
// and the device reports no fault (motor/voltage/pollution/...).
|
||||
if (!lidar->wait_ready(5000)) {
|
||||
fprintf(stderr, "sensor not ready: %s (diag: %s)\n",
|
||||
xlidar::to_string(lidar->last_error()),
|
||||
xlidar::to_string(lidar->get_diagnostics()).c_str());
|
||||
lidar->close();
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 2000)) {
|
||||
fprintf(stderr, "Timeout hoặc lỗi nhận packet\n");
|
||||
xlidar::ScanResult result;
|
||||
if (!lidar->recv_scan(result, 2000)) {
|
||||
fprintf(stderr, "recv_scan failed: %s\n", xlidar::to_string(lidar->last_error()));
|
||||
break;
|
||||
}
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n",
|
||||
i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
|
||||
info.detected_model.c_str());
|
||||
const xlidar::LaserScan& scan = result.scan;
|
||||
printf("scan #%d: %zu points, ts=%u ms, model=%s\n",
|
||||
i, scan.ranges.size(), scan.timestamp_ms,
|
||||
result.info.detected_model.c_str());
|
||||
|
||||
// Chẩn đoán thiết bị từ scan mới nhất
|
||||
lidarlib::Diagnostics diag = drv.get_diagnostics();
|
||||
printf(" diag: %s\n", lidarlib::to_string(diag).c_str());
|
||||
if (diag.has_fault()) {
|
||||
if (diag.monitor_fault()) printf(" !! lỗi monitor/motor\n");
|
||||
if (diag.voltage_fault()) printf(" !! điện áp bất thường\n");
|
||||
if (diag.temperature_fault()) printf(" !! nhiệt độ bất thường\n");
|
||||
}
|
||||
// Device self-diagnostics from the newest scan.
|
||||
xlidar::Diagnostics diag = lidar->get_diagnostics();
|
||||
printf(" diag: %s\n", xlidar::to_string(diag).c_str());
|
||||
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
for (size_t j = 0; j < 5 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
|
||||
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
||||
}
|
||||
}
|
||||
|
||||
drv.close();
|
||||
lidar->close();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Build:
|
||||
// g++ -std=c++17 -O2 -Iinclude -o example examples/example.cpp src/olei_lidar.cpp
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
// Headless skeleton app: loads config.json, one reader thread per lidar.
|
||||
// ./lidar_app [config.json]
|
||||
#include "lidarlib/lidarlib.hpp"
|
||||
// ./lidar_app [config.json] [plugins_dir]
|
||||
#include "lidar_manager.hpp"
|
||||
#include <atomic>
|
||||
#include <csignal>
|
||||
#include <cstdio>
|
||||
@@ -13,33 +13,31 @@ 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));
|
||||
void run_lidar(xlidar::LidarManager& manager, xlidar::DeviceConfig cfg) {
|
||||
std::unique_ptr<xlidar::LidarDriverInterface> lidar = manager.create_lidar_device(cfg);
|
||||
if (!lidar) return;
|
||||
|
||||
xlidar::ErrorCode err = lidar->open();
|
||||
if (err != xlidar::ErrorCode::Ok) {
|
||||
fprintf(stderr, "[%s] open failed %s (%s)\n",
|
||||
cfg.name.c_str(), cfg.driver_id.c_str(), xlidar::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);
|
||||
printf("[%s] opened %s (model=%s, inverted=%d)\n",
|
||||
cfg.name.c_str(), cfg.driver_id.c_str(), cfg.model.c_str(), cfg.inverted);
|
||||
|
||||
while (g_running) {
|
||||
lidarlib::ScanResult result;
|
||||
xlidar::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);
|
||||
printf("[%s] %zu points | ts=%u ms | model=%s | diag=%s\n",
|
||||
cfg.name.c_str(), result.scan.ranges.size(), result.scan.timestamp_ms,
|
||||
result.info.detected_model.c_str(),
|
||||
xlidar::to_string(lidar->get_diagnostics()).c_str());
|
||||
}
|
||||
|
||||
lidar->close();
|
||||
printf("[%s] da dong\n", cfg.name.c_str());
|
||||
printf("[%s] closed\n", cfg.name.c_str());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -48,23 +46,30 @@ int main(int argc, char** argv) {
|
||||
setvbuf(stdout, nullptr, _IOLBF, 0);
|
||||
|
||||
const std::string config_path = (argc > 1) ? argv[1] : "config.json";
|
||||
const std::string plugins_dir = (argc > 2) ? argv[2] : "plugins";
|
||||
|
||||
lidarlib::Config cfg = lidarlib::load_config(config_path);
|
||||
lidarlib::save_config(config_path, cfg); // ensure the file exists
|
||||
xlidar::LidarManager manager(plugins_dir);
|
||||
printf("%zu driver(s) available\n", manager.load_all_plugins());
|
||||
|
||||
xlidar::ManagerConfig cfg = xlidar::load_config(config_path);
|
||||
xlidar::save_config(config_path, cfg); // ensure the file exists (and migrate legacy keys)
|
||||
|
||||
if (cfg.lidars.empty()) {
|
||||
fprintf(stderr, "Khong co lidar nao trong %s\n", config_path.c_str());
|
||||
fprintf(stderr, "no lidars in %s\n", config_path.c_str());
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::signal(SIGINT, on_signal);
|
||||
std::signal(SIGTERM, on_signal);
|
||||
|
||||
// One thread per device — instances are fully independent. The manager
|
||||
// outlives every thread (join below), as the plugin contract requires.
|
||||
std::vector<std::thread> threads;
|
||||
threads.reserve(cfg.lidars.size());
|
||||
for (const auto& lc : cfg.lidars) threads.emplace_back(run_lidar, lc);
|
||||
for (const auto& lc : cfg.lidars)
|
||||
threads.emplace_back(run_lidar, std::ref(manager), lc);
|
||||
|
||||
printf("Dang chay %zu lidar tu %s. Ctrl-C de dung.\n",
|
||||
printf("running %zu lidar(s) from %s. Ctrl-C to stop.\n",
|
||||
cfg.lidars.size(), config_path.c_str());
|
||||
for (auto& t : threads) t.join();
|
||||
return 0;
|
||||
|
||||
28
examples/list_drivers.cpp
Normal file
28
examples/list_drivers.cpp
Normal file
@@ -0,0 +1,28 @@
|
||||
// Discover plugins and print every registered driver.
|
||||
// ./list_drivers [plugins_dir] (default: ./plugins)
|
||||
#include "lidar_manager.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
const std::string plugins_dir = (argc > 1) ? argv[1] : "plugins";
|
||||
|
||||
xlidar::LidarManager manager(plugins_dir);
|
||||
size_t count = manager.load_all_plugins();
|
||||
printf("%zu driver(s) in %s\n\n", count, plugins_dir.c_str());
|
||||
|
||||
for (const auto& [driver_id, plugin] : manager.available_drivers()) {
|
||||
const xlidar::DriverInfo& info = plugin.info;
|
||||
printf("%s\n", driver_id.c_str());
|
||||
printf(" vendor: %s\n", info.vendor.c_str());
|
||||
printf(" model: %s\n", info.model.c_str());
|
||||
printf(" transport: %s%s\n", xlidar::to_string(info.transport),
|
||||
info.transport_selectable ? " (selectable)" : "");
|
||||
printf(" models: ");
|
||||
for (size_t i = 0; i < info.supported_models.size(); ++i)
|
||||
printf("%s%s", i ? ", " : "", info.supported_models[i].c_str());
|
||||
printf("\n");
|
||||
printf(" description: %s\n", info.description.c_str());
|
||||
printf(" file: %s\n\n", plugin.file_path.c_str());
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -1,39 +0,0 @@
|
||||
// SICK nanoScan3 example. The sensor's UDP output target must be configured
|
||||
// in SICK Safety Designer; this driver only binds a local UDP port.
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
int main() {
|
||||
lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
|
||||
|
||||
lidarlib::ErrorCode err = drv.open();
|
||||
if (err != lidarlib::ErrorCode::Ok) {
|
||||
fprintf(stderr, "Không mở được UDP socket cho nanoScan3: %s\n",
|
||||
lidarlib::to_string(err));
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 2000)) {
|
||||
fprintf(stderr, "Timeout hoặc lỗi nhận UDP datagram\n");
|
||||
break;
|
||||
}
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
printf("Scan #%d: %zu điểm, ts=%u, model=%s\n",
|
||||
i, scan.ranges.size(), scan.timestamp_ms, info.detected_model.c_str());
|
||||
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
|
||||
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
||||
}
|
||||
}
|
||||
|
||||
drv.close();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Build:
|
||||
// g++ -std=c++17 -O2 -Iinclude -o nanoscan_example examples/nanoscan_example.cpp src/sick_lidar.cpp
|
||||
@@ -1,39 +0,0 @@
|
||||
// SICK TiM driver example (SOPAS/CoLa-A, TCP).
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include <cstdio>
|
||||
|
||||
int main() {
|
||||
lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
|
||||
|
||||
lidarlib::ErrorCode err = drv.open();
|
||||
if (err != lidarlib::ErrorCode::Ok) {
|
||||
fprintf(stderr, "Không kết nối được TCP tới lidar SICK: %s\n",
|
||||
lidarlib::to_string(err));
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 2000)) {
|
||||
fprintf(stderr, "Timeout hoặc lỗi nhận telegram\n");
|
||||
break;
|
||||
}
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X, model=%s\n",
|
||||
i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
|
||||
info.detected_model.c_str());
|
||||
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
|
||||
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
||||
}
|
||||
}
|
||||
|
||||
drv.close();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Build:
|
||||
// g++ -std=c++17 -O2 -Iinclude -o sick_example examples/sick_example.cpp src/sick_lidar.cpp
|
||||
@@ -1,47 +0,0 @@
|
||||
// Test 2 Olei lidars (front + rear) concurrently.
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdio>
|
||||
#include <thread>
|
||||
|
||||
static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
|
||||
const std::string& local_ip, uint16_t port, bool inverted, int n_scans) {
|
||||
lidarlib::Driver drv(cfg, local_ip, port, inverted);
|
||||
lidarlib::ErrorCode err = drv.open();
|
||||
if (err != lidarlib::ErrorCode::Ok) {
|
||||
fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n",
|
||||
tag, local_ip.c_str(), port, lidarlib::to_string(err));
|
||||
return;
|
||||
}
|
||||
printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);
|
||||
|
||||
for (int i = 0; i < n_scans; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 2000)) {
|
||||
fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i);
|
||||
continue;
|
||||
}
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n",
|
||||
tag, i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
|
||||
info.detected_model.c_str());
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f dist=%.3fm intensity=%.0f\n",
|
||||
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
||||
}
|
||||
}
|
||||
drv.close();
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Real headers (UDP sniff): front = "OLELR-1BS2", rear = "OLELR-1BS5".
|
||||
std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO,
|
||||
"192.168.100.100", 2368, false, 5);
|
||||
std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO,
|
||||
"192.168.100.100", 2369, true, 5);
|
||||
|
||||
t_front.join();
|
||||
t_rear.join();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user