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:
383
include/lidar_interface.hpp
Normal file
383
include/lidar_interface.hpp
Normal file
@@ -0,0 +1,383 @@
|
||||
#pragma once
|
||||
// xlidar-driver — public driver interface.
|
||||
//
|
||||
// Every lidar driver plugin implements xlidar::LidarDriverInterface and
|
||||
// exports two extern "C" entry points (see "Plugin ABI" at the bottom):
|
||||
//
|
||||
// get_driver_info(xlidar::DriverInfo*) — static metadata
|
||||
// create_driver_instance(const xlidar::DeviceConfig*)
|
||||
// — new driver instance
|
||||
//
|
||||
// Host applications never include plugin headers; they talk to plugins
|
||||
// exclusively through this header + lidar_manager.hpp.
|
||||
|
||||
#include "lidar_diagnostics.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// ── Error codes ─────────────────────────────────────────────────────────────
|
||||
|
||||
// Result of open() and the sticky status behind last_error(). Ok == 0 so
|
||||
// `if (err != ErrorCode::Ok)` reads naturally at call sites.
|
||||
enum class ErrorCode {
|
||||
Ok = 0,
|
||||
|
||||
// Lifecycle misuse — the call was refused, the instance state is unchanged.
|
||||
AlreadyOpen, // open() called while already open
|
||||
NotOpen, // recv_scan()/spin_once() called before open()
|
||||
|
||||
// open() failures
|
||||
SocketError, // socket() creation failed
|
||||
InvalidAddress, // ip string is not a valid IPv4 address
|
||||
PortInUse, // bind: local port already taken (EADDRINUSE/EACCES)
|
||||
BindFailed, // bind failed for another reason
|
||||
ConnectionRefused, // TCP connect refused (device up, port closed)
|
||||
ConnectionFailed, // TCP connect failed (unreachable, no route, ...)
|
||||
HandshakeFailed, // connected, but the start-stream command failed
|
||||
SerialError, // serial port open/configure failed (serial drivers)
|
||||
DeviceError, // device rejected a command / reported a hard fault
|
||||
|
||||
// Runtime failures
|
||||
Timeout, // no (complete) scan within timeout_ms
|
||||
DeviceDisconnected, // peer closed the connection / socket or serial error
|
||||
|
||||
// Configuration errors
|
||||
InvalidConfig, // DeviceConfig is not usable by this driver
|
||||
};
|
||||
|
||||
inline const char* to_string(ErrorCode e) {
|
||||
switch (e) {
|
||||
case ErrorCode::Ok: return "Ok";
|
||||
case ErrorCode::AlreadyOpen: return "AlreadyOpen";
|
||||
case ErrorCode::NotOpen: return "NotOpen";
|
||||
case ErrorCode::SocketError: return "SocketError";
|
||||
case ErrorCode::InvalidAddress: return "InvalidAddress";
|
||||
case ErrorCode::PortInUse: return "PortInUse";
|
||||
case ErrorCode::BindFailed: return "BindFailed";
|
||||
case ErrorCode::ConnectionRefused: return "ConnectionRefused";
|
||||
case ErrorCode::ConnectionFailed: return "ConnectionFailed";
|
||||
case ErrorCode::HandshakeFailed: return "HandshakeFailed";
|
||||
case ErrorCode::SerialError: return "SerialError";
|
||||
case ErrorCode::DeviceError: return "DeviceError";
|
||||
case ErrorCode::Timeout: return "Timeout";
|
||||
case ErrorCode::DeviceDisconnected: return "DeviceDisconnected";
|
||||
case ErrorCode::InvalidConfig: return "InvalidConfig";
|
||||
}
|
||||
return "Unknown";
|
||||
}
|
||||
|
||||
// ── Scan data ───────────────────────────────────────────────────────────────
|
||||
|
||||
// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
|
||||
// ranges[i] is at angle_min + i*angle_increment, in sweep order.
|
||||
struct LaserScan {
|
||||
uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
|
||||
float angle_min = 0.f; // rad
|
||||
float angle_max = 0.f; // rad
|
||||
float angle_increment = 0.f; // rad
|
||||
float time_increment = 0.f; // sec — not exposed by most devices, 0 then
|
||||
float scan_time = 0.f; // sec — not exposed by most devices, 0 then
|
||||
float range_min = 0.f; // m — from ModelConfig, not measured
|
||||
float range_max = 0.f; // m — from ModelConfig, not measured
|
||||
std::vector<float> ranges; // m
|
||||
std::vector<float> intensities; // 0-255 as float
|
||||
};
|
||||
|
||||
// Diagnostic/header fields; fields the device family doesn't carry stay
|
||||
// std::nullopt (see docs/diagnostics.md for the per-family wire layout).
|
||||
struct ExtraInfo {
|
||||
std::string detected_model = "AUTO";
|
||||
uint8_t error_status = 0; // OLEI Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
|
||||
uint8_t distance_scale_mm = 0; // 0 = not reported
|
||||
|
||||
// OLEI Family A only
|
||||
std::optional<uint16_t> rotation_raw;
|
||||
|
||||
// OLEI Family C / V3 (GS1-5) only — raw passthroughs, unverified
|
||||
std::optional<uint8_t> distance_ratio_raw;
|
||||
std::optional<uint16_t> scan_frequency_raw;
|
||||
std::optional<uint16_t> input_status;
|
||||
std::optional<uint16_t> output_status;
|
||||
std::optional<uint32_t> field_status;
|
||||
std::optional<uint32_t> status_flags;
|
||||
|
||||
// SICK TiM only — LMDscandata status pair (word0<<8)|word1:
|
||||
// 0 ok, 1 error, 2 pollution warning, 4 pollution error.
|
||||
std::optional<uint16_t> sick_device_status;
|
||||
|
||||
// SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
|
||||
std::optional<uint8_t> nano_general_state;
|
||||
|
||||
// ESPE LGA60 only — fault word from the newest "WSimu" area frame; the
|
||||
// device only sends those when area data is polled, so usually nullopt.
|
||||
std::optional<uint16_t> espe_error_status;
|
||||
|
||||
// RPLIDAR only — SDK health status (0 ok, 1 warning, 2 error) and the
|
||||
// device error code that goes with it.
|
||||
std::optional<uint8_t> rplidar_health_status;
|
||||
std::optional<uint16_t> rplidar_error_code;
|
||||
};
|
||||
|
||||
struct ScanResult {
|
||||
LaserScan scan;
|
||||
ExtraInfo info;
|
||||
};
|
||||
|
||||
// Decode the diagnostic fields of one scan; sets valid = true.
|
||||
inline Diagnostics decode_diagnostics(const ExtraInfo& info) {
|
||||
Diagnostics d;
|
||||
d.valid = true;
|
||||
d.model = info.detected_model;
|
||||
d.error_status = info.error_status;
|
||||
d.rotation_raw = info.rotation_raw;
|
||||
d.scan_frequency_raw = info.scan_frequency_raw;
|
||||
d.input_status = info.input_status;
|
||||
d.output_status = info.output_status;
|
||||
d.field_status = info.field_status;
|
||||
d.status_flags = info.status_flags;
|
||||
d.sick_device_status = info.sick_device_status;
|
||||
d.nano_general_state = info.nano_general_state;
|
||||
d.espe_error_status = info.espe_error_status;
|
||||
d.rplidar_health_status = info.rplidar_health_status;
|
||||
d.rplidar_error_code = info.rplidar_error_code;
|
||||
return d;
|
||||
}
|
||||
|
||||
// Per-model configuration. scan_angle_* use the signed system [-180,180]:
|
||||
// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
|
||||
// The per-vendor MODEL_* presets live in each plugin.
|
||||
struct ModelConfig {
|
||||
const char* name;
|
||||
float scan_angle_min; // deg
|
||||
float scan_angle_max; // deg
|
||||
float range_min_m = 0.05f;
|
||||
float range_max_m = 30.f;
|
||||
// Added to the raw device angle so output 0° = ahead (e.g. LR-1F/1FMI
|
||||
// report 0° at the back: +180; SICK TiM puts the front at 90°: -90).
|
||||
float angle_offset_deg = 0.f;
|
||||
// Output remap window (see remap_scan_window in plugins/common): shifts
|
||||
// the scan's angles onto [out_angle_min, out_angle_max] without dropping
|
||||
// points.
|
||||
bool remap_angles = false;
|
||||
float out_angle_min = 0.f; // deg
|
||||
float out_angle_max = 0.f; // deg
|
||||
// Valid FOV window from DeviceConfig::angle_min/max_deg: points outside
|
||||
// become NaN, geometry unchanged (apply_fov_window in plugins/common).
|
||||
bool fov_filter = false;
|
||||
float fov_min_deg = -360.f;
|
||||
float fov_max_deg = 360.f;
|
||||
};
|
||||
|
||||
using ScanCallback = std::function<void(const ScanResult&)>;
|
||||
|
||||
// ── Driver metadata & instance configuration ────────────────────────────────
|
||||
|
||||
// Transport a driver uses to reach the device. A driver declares exactly one
|
||||
// primary transport; drivers that can switch (e.g. ESPE TCP/UDP) declare the
|
||||
// default and honor DeviceConfig::use_udp.
|
||||
enum class Transport { Serial, Udp, Tcp };
|
||||
|
||||
inline const char* to_string(Transport t) {
|
||||
switch (t) {
|
||||
case Transport::Serial: return "serial";
|
||||
case Transport::Udp: return "udp";
|
||||
case Transport::Tcp: return "tcp";
|
||||
}
|
||||
return "unknown";
|
||||
}
|
||||
|
||||
// Static identity a plugin registers about itself (get_driver_info entry
|
||||
// point and LidarDriverInterface::get_driver_info()).
|
||||
struct DriverInfo {
|
||||
std::string vendor; // "Slamtec", "OLEI", "SICK", "ESPE"
|
||||
std::string model; // device category the driver targets, e.g. "C1"
|
||||
// or "TiM5xx/TiM7xx" — one driver may cover a
|
||||
// whole series
|
||||
std::string driver_id; // unique stable id, e.g. "rplidar_c1_driver"
|
||||
std::string description; // short doc: covered devices, transport, notes
|
||||
|
||||
// Extra metadata for hosts/UIs (not part of the required identity):
|
||||
Transport transport = Transport::Udp; // primary transport
|
||||
bool transport_selectable = false; // true → use_udp switches TCP/UDP
|
||||
std::vector<std::string> supported_models; // valid DeviceConfig::model values
|
||||
};
|
||||
|
||||
// Settings for one lidar instance. `name` is the unique key across saves.
|
||||
// Which fields matter depends on the driver's transport:
|
||||
// serial → serial_port + baudrate; udp/tcp → ip + port.
|
||||
struct DeviceConfig {
|
||||
std::string name = "lidar";
|
||||
std::string driver_id; // plugin that owns this device
|
||||
std::string model = "AUTO"; // one of DriverInfo::supported_models
|
||||
|
||||
// Network transports (udp: local bind address / tcp: device address)
|
||||
std::string ip = "0.0.0.0";
|
||||
uint16_t port = 0; // 0 = driver default
|
||||
|
||||
// Serial transport
|
||||
std::string serial_port = "/dev/ttyUSB0";
|
||||
uint32_t baudrate = 460800;
|
||||
|
||||
bool inverted = false; // unit mounted upside-down → mirror the scan
|
||||
bool use_udp = false; // only for transport-selectable drivers (ESPE)
|
||||
|
||||
// Valid field-of-view window (deg, signed system: 0 = ahead, + = left).
|
||||
// Points outside are reported as NaN (invalid), the scan geometry is
|
||||
// unchanged. Defaults (±360) = off.
|
||||
float angle_min_deg = -360.f;
|
||||
float angle_max_deg = 360.f;
|
||||
|
||||
// Range override (m); 0 = keep the driver/model default.
|
||||
float range_min_m = 0.f;
|
||||
float range_max_m = 0.f;
|
||||
|
||||
// Legacy output remap window (deg): scan angles are linearly remapped
|
||||
// onto [remap_angle_min_deg, remap_angle_max_deg] without dropping
|
||||
// points. Defaults (±360) = off. Kept for pre-plugin lidarlib configs.
|
||||
float remap_angle_min_deg = -360.f;
|
||||
float remap_angle_max_deg = 360.f;
|
||||
|
||||
// Driver-specific options that don't warrant a first-class field
|
||||
// (documented per plugin).
|
||||
std::map<std::string, std::string> extra;
|
||||
|
||||
friend bool operator==(const DeviceConfig& a, const DeviceConfig& b) {
|
||||
return a.name == b.name && a.driver_id == b.driver_id && a.model == b.model &&
|
||||
a.ip == b.ip && a.port == b.port &&
|
||||
a.serial_port == b.serial_port && a.baudrate == b.baudrate &&
|
||||
a.inverted == b.inverted && a.use_udp == b.use_udp &&
|
||||
a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg &&
|
||||
a.range_min_m == b.range_min_m && a.range_max_m == b.range_max_m &&
|
||||
a.remap_angle_min_deg == b.remap_angle_min_deg &&
|
||||
a.remap_angle_max_deg == b.remap_angle_max_deg &&
|
||||
a.extra == b.extra;
|
||||
}
|
||||
friend bool operator!=(const DeviceConfig& a, const DeviceConfig& b) { return !(a == b); }
|
||||
};
|
||||
|
||||
// ── Driver interface ────────────────────────────────────────────────────────
|
||||
|
||||
// Unified driver interface implemented by every plugin. Instances come from
|
||||
// LidarManager::create_lidar_device() (or a plugin's create_driver_instance
|
||||
// entry point directly). One instance == one physical device; instances are
|
||||
// fully independent — run each on its own thread without locking.
|
||||
class LidarDriverInterface {
|
||||
public:
|
||||
virtual ~LidarDriverInterface() = default;
|
||||
|
||||
// Static metadata of the driver that produced this instance.
|
||||
virtual DriverInfo get_driver_info() const = 0;
|
||||
|
||||
// ErrorCode::Ok on success. Calling open() on an already-open instance
|
||||
// returns AlreadyOpen and leaves the connection untouched.
|
||||
virtual ErrorCode open() = 0;
|
||||
// Idempotent: safe to call before open() or more than once.
|
||||
virtual void close() = 0;
|
||||
// Block until one full scan; false on error/timeout (see last_error()).
|
||||
// timeout_ms = 0 → block indefinitely. No default on purpose: drivers
|
||||
// differ (OLEI 1000, SICK TiM 2000).
|
||||
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
|
||||
// The callback fires only from spin_once() — recv_scan() never invokes
|
||||
// it. Pick one pump style: recv_scan() to poll, or callback + spin_once().
|
||||
virtual void set_scan_callback(ScanCallback cb) = 0;
|
||||
// Process one unit of input (may block on the socket/port while the
|
||||
// device is silent); fires the scan callback when a scan completed.
|
||||
// False on error.
|
||||
virtual bool spin_once() = 0;
|
||||
// Model name read from the wire where the protocol carries one
|
||||
// ("AUTO"/configured name until then).
|
||||
virtual const char* detected_model() const = 0;
|
||||
|
||||
virtual bool is_open() const = 0;
|
||||
// Status of the most recent open()/recv_scan()/spin_once() call.
|
||||
ErrorCode last_error() const { return last_error_; }
|
||||
|
||||
// Device self-diagnostics from the newest fully decoded scan. valid stays
|
||||
// false until one scan has been seen. Updated by recv_scan()/spin_once();
|
||||
// call from the same thread that pumps them.
|
||||
virtual Diagnostics get_diagnostics() const { return {}; }
|
||||
|
||||
// True when the sensor is usable right now: connection open, at least one
|
||||
// fault-free scan decoded, and that scan no older than max_age_ms
|
||||
// (0 = skip the age check). Diagnostics only refresh from
|
||||
// recv_scan()/spin_once(), so unless something is pumping them this goes
|
||||
// stale and reports not-ready; call from the pump thread.
|
||||
bool is_ready(int max_age_ms = 3000) const {
|
||||
if (!is_open() || !get_diagnostics().healthy()) return false;
|
||||
if (max_age_ms <= 0) return true;
|
||||
return last_scan_time_.time_since_epoch().count() != 0
|
||||
&& std::chrono::steady_clock::now() - last_scan_time_
|
||||
<= std::chrono::milliseconds(max_age_ms);
|
||||
}
|
||||
|
||||
// Pump recv_scan() until is_ready() or timeout_ms elapses; false on
|
||||
// timeout (see last_error() for the underlying failure). Scans consumed
|
||||
// while waiting are discarded and the scan callback does not fire —
|
||||
// intended for startup, before handing the pump to the main loop.
|
||||
bool wait_ready(int timeout_ms = 5000) {
|
||||
const auto deadline = std::chrono::steady_clock::now()
|
||||
+ std::chrono::milliseconds(timeout_ms);
|
||||
ScanResult tmp;
|
||||
while (!is_ready()) {
|
||||
if (!is_open()) return false;
|
||||
const auto left = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
deadline - std::chrono::steady_clock::now()).count();
|
||||
if (left <= 0) return false;
|
||||
recv_scan(tmp, static_cast<int>(left));
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
protected:
|
||||
ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
|
||||
// Drivers call this each time a full scan is decoded; feeds the freshness
|
||||
// side of is_ready().
|
||||
void mark_scan_decoded() { last_scan_time_ = std::chrono::steady_clock::now(); }
|
||||
|
||||
private:
|
||||
ErrorCode last_error_ = ErrorCode::Ok;
|
||||
std::chrono::steady_clock::time_point last_scan_time_{};
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
|
||||
// ── Plugin ABI ──────────────────────────────────────────────────────────────
|
||||
//
|
||||
// Each plugin .so exports exactly these two symbols (C linkage, default
|
||||
// visibility — plugins are otherwise built with -fvisibility=hidden):
|
||||
//
|
||||
// XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out);
|
||||
// XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||
// create_driver_instance(const xlidar::DeviceConfig* cfg);
|
||||
//
|
||||
// create_driver_instance returns a heap-allocated instance (never nullptr for
|
||||
// a structurally valid cfg; config problems surface from open() as
|
||||
// InvalidConfig/SerialError/...). The host deletes it through the
|
||||
// LidarDriverInterface vtable, so the plugin must stay loaded for the
|
||||
// instance's whole lifetime — LidarManager guarantees that by keeping every
|
||||
// plugin open until the manager itself is destroyed.
|
||||
//
|
||||
// C linkage keeps the symbol names unmangled for dlsym(); the types crossing
|
||||
// the boundary are C++ (same toolchain for manager and plugins is required —
|
||||
// they are always built together in this repo).
|
||||
|
||||
#define XLIDAR_PLUGIN_EXPORT extern "C" __attribute__((visibility("default")))
|
||||
|
||||
extern "C" {
|
||||
using xlidar_get_driver_info_fn = void (*)(xlidar::DriverInfo*);
|
||||
using xlidar_create_driver_instance_fn =
|
||||
xlidar::LidarDriverInterface* (*)(const xlidar::DeviceConfig*);
|
||||
}
|
||||
|
||||
// Symbol names LidarManager resolves in every plugin.
|
||||
#define XLIDAR_GET_DRIVER_INFO_SYMBOL "get_driver_info"
|
||||
#define XLIDAR_CREATE_DRIVER_INSTANCE_SYMBOL "create_driver_instance"
|
||||
Reference in New Issue
Block a user