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:
@@ -1,15 +1,16 @@
|
||||
#pragma once
|
||||
// xlidar-driver — device self-diagnostics decoded from the data stream.
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
namespace lidarlib {
|
||||
namespace xlidar {
|
||||
|
||||
struct ExtraInfo; // lidar.hpp
|
||||
struct ExtraInfo; // lidar_interface.hpp
|
||||
|
||||
// Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are reserved
|
||||
// on the wire; a nonzero reserved bit is still reported as a fault.
|
||||
// OLEI Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are
|
||||
// reserved on the wire; a nonzero reserved bit is still reported as a fault.
|
||||
inline constexpr uint8_t kFaultMonitor = 1u << 0; // monitor / motor abnormal
|
||||
inline constexpr uint8_t kFaultVoltage = 1u << 1; // supply voltage out of range
|
||||
inline constexpr uint8_t kFaultTemperature = 1u << 2; // internal temperature abnormal
|
||||
@@ -28,25 +29,32 @@ inline constexpr uint8_t kNanoStateContaminationError = 1u << 3;
|
||||
inline constexpr uint8_t kNanoStateReferenceContour = 1u << 4;
|
||||
inline constexpr uint8_t kNanoStateManipulation = 1u << 5;
|
||||
|
||||
// Device self-diagnostics decoded from the data stream. Fields the family
|
||||
// doesn't carry stay std::nullopt (see docs/diagnostics.md for the per-family
|
||||
// wire layout). valid stays false until the driver has decoded one full scan.
|
||||
// RPLIDAR SDK health status values (sl_lidar_response_device_health_t.status).
|
||||
inline constexpr uint8_t kRplidarHealthOk = 0;
|
||||
inline constexpr uint8_t kRplidarHealthWarning = 1;
|
||||
inline constexpr uint8_t kRplidarHealthError = 2;
|
||||
|
||||
// Device self-diagnostics decoded from the data stream. Fields the device
|
||||
// family doesn't carry stay std::nullopt (see docs/diagnostics.md for the
|
||||
// per-family wire layout). valid stays false until the driver has decoded one
|
||||
// full scan.
|
||||
struct Diagnostics {
|
||||
bool valid = false;
|
||||
std::string model = "AUTO";
|
||||
std::string firmware; // e.g. "fw 1.32 hw 18"; empty if unknown
|
||||
uint32_t device_timestamp_ms = 0; // device clock; 0 if not on the wire
|
||||
|
||||
// Family A error byte (0 = no fault; Family B/C don't carry it)
|
||||
// OLEI Family A error byte (0 = no fault; Family B/C don't carry it)
|
||||
uint8_t error_status = 0;
|
||||
|
||||
bool monitor_fault() const { return (error_status & kFaultMonitor) != 0; }
|
||||
bool voltage_fault() const { return (error_status & kFaultVoltage) != 0; }
|
||||
bool temperature_fault() const { return (error_status & kFaultTemperature) != 0; }
|
||||
|
||||
// Family A only — raw motor speed field, unit unverified
|
||||
// OLEI Family A only — raw motor speed field, unit unverified
|
||||
std::optional<uint16_t> rotation_raw;
|
||||
|
||||
// Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified
|
||||
// OLEI Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified
|
||||
std::optional<uint16_t> scan_frequency_raw;
|
||||
std::optional<uint16_t> input_status;
|
||||
std::optional<uint16_t> output_status;
|
||||
@@ -73,17 +81,30 @@ struct Diagnostics {
|
||||
|
||||
bool espe_fault() const { return espe_error_status && *espe_error_status != 0; }
|
||||
|
||||
// RPLIDAR — SDK getHealth() status (refreshed at open(); the streaming
|
||||
// protocol carries no health) plus the device error code that goes with it.
|
||||
std::optional<uint8_t> rplidar_health_status;
|
||||
std::optional<uint16_t> rplidar_error_code;
|
||||
|
||||
bool rplidar_fault() const { return rplidar_health_status && *rplidar_health_status == kRplidarHealthError; }
|
||||
bool rplidar_warning() const { return rplidar_health_status && *rplidar_health_status == kRplidarHealthWarning; }
|
||||
|
||||
// Fault = device says something is wrong now; warning = degraded but
|
||||
// still measuring (dirty optics) — schedule cleaning/service.
|
||||
// still measuring (dirty optics, weak motor) — schedule cleaning/service.
|
||||
bool has_fault() const {
|
||||
return error_status != 0 || sick_error() || pollution_error()
|
||||
|| contamination_error() || manipulation() || espe_fault();
|
||||
|| contamination_error() || manipulation() || espe_fault()
|
||||
|| rplidar_fault();
|
||||
}
|
||||
bool has_warning() const {
|
||||
return pollution_warning() || contamination_warning() || rplidar_warning();
|
||||
}
|
||||
bool has_warning() const { return pollution_warning() || contamination_warning(); }
|
||||
bool healthy() const { return valid && !has_fault(); }
|
||||
};
|
||||
|
||||
// Decode the diagnostic fields of one scan; sets valid = true.
|
||||
// Defined inline in lidar_interface.hpp (needs the ExtraInfo definition, and
|
||||
// every plugin .so must carry its own copy).
|
||||
Diagnostics decode_diagnostics(const ExtraInfo& info);
|
||||
|
||||
// One-line log summary: "no data" / "ok" / "WARN: pollution" /
|
||||
@@ -93,6 +114,7 @@ inline std::string to_string(const Diagnostics& d) {
|
||||
if (!d.has_fault()) return d.has_warning()
|
||||
? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
|
||||
+ (d.contamination_warning() ? " contamination" : "")
|
||||
+ (d.rplidar_warning() ? " rplidar" : "")
|
||||
: "ok";
|
||||
|
||||
std::string s = "FAULT:";
|
||||
@@ -108,6 +130,12 @@ inline std::string to_string(const Diagnostics& d) {
|
||||
std::snprintf(buf, sizeof(buf), " espe(0x%04X)", *d.espe_error_status);
|
||||
s += buf;
|
||||
}
|
||||
if (d.rplidar_fault()) {
|
||||
char buf[32];
|
||||
std::snprintf(buf, sizeof(buf), " rplidar(0x%04X)",
|
||||
d.rplidar_error_code ? *d.rplidar_error_code : 0);
|
||||
s += buf;
|
||||
}
|
||||
if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
|
||||
char buf[24];
|
||||
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
|
||||
@@ -116,4 +144,4 @@ inline std::string to_string(const Diagnostics& d) {
|
||||
return s;
|
||||
}
|
||||
|
||||
} // namespace lidarlib
|
||||
} // namespace xlidar
|
||||
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"
|
||||
110
include/lidar_manager.hpp
Normal file
110
include/lidar_manager.hpp
Normal file
@@ -0,0 +1,110 @@
|
||||
#pragma once
|
||||
// xlidar-driver — LidarManager: the host-facing facade of the SDK.
|
||||
//
|
||||
// The manager scans a plugin directory for driver .so files, registers their
|
||||
// DriverInfo, and creates driver instances by driver_id:
|
||||
//
|
||||
// xlidar::LidarManager manager("/opt/xlidar/plugins");
|
||||
// manager.load_all_plugins();
|
||||
// for (const auto& [id, plugin] : manager.available_drivers())
|
||||
// printf("%s — %s\n", id.c_str(), plugin.info.description.c_str());
|
||||
//
|
||||
// xlidar::DeviceConfig cfg;
|
||||
// cfg.driver_id = "rplidar_c1_driver";
|
||||
// cfg.serial_port = "/dev/ttyUSB0";
|
||||
// auto lidar = manager.create_lidar_device(cfg);
|
||||
//
|
||||
// Plugins stay loaded until the manager is destroyed; the manager must
|
||||
// outlive every device instance it created (instances execute code that
|
||||
// lives inside the plugin .so).
|
||||
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// One discovered plugin: its registered identity plus where it came from.
|
||||
struct PluginRegistry {
|
||||
DriverInfo info;
|
||||
std::string file_path; // absolute path of the plugin .so
|
||||
};
|
||||
|
||||
// Configuration document (config.json): a named list of lidar devices.
|
||||
struct ManagerConfig {
|
||||
std::vector<DeviceConfig> lidars;
|
||||
};
|
||||
|
||||
// Returns defaults if the file doesn't exist (without creating it).
|
||||
// Legacy lidarlib entries ({"brand": "OLEI"|"SICK"|"ESPE", ...}) are
|
||||
// migrated on the fly: brand+model resolve to the matching driver_id and the
|
||||
// old angle window keys map to the remap fields.
|
||||
ManagerConfig load_config(const std::string& path);
|
||||
|
||||
void save_config(const std::string& path, const ManagerConfig& cfg);
|
||||
|
||||
class LidarManager {
|
||||
public:
|
||||
// plugins_dir: directory holding the driver .so files. Nothing is
|
||||
// touched until load_all_plugins() runs.
|
||||
explicit LidarManager(std::string plugins_dir);
|
||||
~LidarManager();
|
||||
|
||||
LidarManager(const LidarManager&) = delete;
|
||||
LidarManager& operator=(const LidarManager&) = delete;
|
||||
|
||||
// Scan plugins_dir for *.so, dlopen each, resolve get_driver_info /
|
||||
// create_driver_instance, and register the driver. Files that are not
|
||||
// valid plugins (missing symbols, dlopen failure) are skipped with a
|
||||
// message on stderr. Safe to call again to pick up newly added files
|
||||
// (already-loaded driver_ids are kept, not reloaded). Returns the number
|
||||
// of drivers registered in total.
|
||||
size_t load_all_plugins();
|
||||
|
||||
// Registered drivers keyed by driver_id. Stable while the manager lives;
|
||||
// load_all_plugins() may add entries.
|
||||
const std::map<std::string, PluginRegistry>& available_drivers() const {
|
||||
return available_;
|
||||
}
|
||||
|
||||
bool has_driver(const std::string& driver_id) const {
|
||||
return available_.count(driver_id) != 0;
|
||||
}
|
||||
|
||||
// Create a device instance from the plugin registered for driver_id.
|
||||
// nullptr if driver_id is unknown. The instance is independent and
|
||||
// thread-safe to pump from its own thread; it must be destroyed before
|
||||
// the manager.
|
||||
std::unique_ptr<LidarDriverInterface>
|
||||
create_lidar_device(const std::string& driver_id, const DeviceConfig& cfg);
|
||||
|
||||
// Convenience: driver_id taken from cfg.driver_id.
|
||||
std::unique_ptr<LidarDriverInterface> create_lidar_device(const DeviceConfig& cfg) {
|
||||
return create_lidar_device(cfg.driver_id, cfg);
|
||||
}
|
||||
|
||||
// Convenience: one instance per entry of a config.json document (see
|
||||
// load_config). Entries whose driver_id is not available are skipped
|
||||
// with a message on stderr.
|
||||
std::vector<std::unique_ptr<LidarDriverInterface>>
|
||||
create_from_config_file(const std::string& path);
|
||||
|
||||
const std::string& plugins_dir() const { return plugins_dir_; }
|
||||
|
||||
private:
|
||||
struct LoadedPlugin {
|
||||
void* handle = nullptr; // dlopen handle
|
||||
xlidar_create_driver_instance_fn create = nullptr;
|
||||
};
|
||||
|
||||
std::string plugins_dir_;
|
||||
std::map<std::string, PluginRegistry> available_;
|
||||
std::map<std::string, LoadedPlugin> loaded_;
|
||||
mutable std::mutex mutex_;
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
@@ -1,63 +0,0 @@
|
||||
#pragma once
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// Settings for one lidar. `name` is the unique key across saves.
|
||||
struct LidarConfig {
|
||||
std::string name = "lidar";
|
||||
std::string ip = "0.0.0.0";
|
||||
uint16_t port = 2368;
|
||||
std::string model = "AUTO";
|
||||
bool inverted = false; // unit mounted upside-down → mirror the scan
|
||||
std::string brand = "OLEI"; // "OLEI", "SICK" or "ESPE"
|
||||
bool use_udp = false; // ESPE only: UDP instead of TCP transport
|
||||
|
||||
// Output angle window (deg): scan angles are remapped onto
|
||||
// [angle_min_deg, angle_max_deg] without dropping points.
|
||||
// Defaults (±360) = off.
|
||||
float angle_min_deg = -360.f;
|
||||
float angle_max_deg = 360.f;
|
||||
|
||||
friend bool operator==(const LidarConfig& a, const LidarConfig& b) {
|
||||
return a.name == b.name && a.ip == b.ip && a.port == b.port &&
|
||||
a.model == b.model && a.inverted == b.inverted && a.brand == b.brand &&
|
||||
a.use_udp == b.use_udp &&
|
||||
a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg;
|
||||
}
|
||||
friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); }
|
||||
};
|
||||
|
||||
struct Config {
|
||||
std::vector<LidarConfig> lidars = {
|
||||
{"front", "0.0.0.0", 2368, "AUTO", false},
|
||||
{"rear", "0.0.0.0", 2369, "AUTO", true},
|
||||
};
|
||||
};
|
||||
|
||||
// nullptr if `name` doesn't match any known model.
|
||||
const ModelConfig* model_by_name(const std::string& name);
|
||||
|
||||
const std::vector<std::string>& model_names();
|
||||
|
||||
const std::vector<std::string>& brand_names();
|
||||
|
||||
// Subset of model_names() valid for `brand`; empty if unknown.
|
||||
const std::vector<std::string>& model_names_for_brand(const std::string& brand);
|
||||
|
||||
// Returns defaults if the file doesn't exist (without creating it).
|
||||
Config load_config(const std::string& path);
|
||||
|
||||
void save_config(const std::string& path, const Config& cfg);
|
||||
|
||||
// Build a ready-to-open lidar from one LidarConfig — the only entry point an
|
||||
// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP
|
||||
// NanoScanDriver, others → TCP SickDriver); brand "ESPE" → TCP EspeDriver;
|
||||
// anything else → OLEI UDP.
|
||||
// Unknown/cross-brand model falls back to the brand default. Never nullptr.
|
||||
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg);
|
||||
|
||||
} // namespace lidarlib
|
||||
@@ -1,46 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// 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
|
||||
|
||||
// Runtime failures
|
||||
Timeout, // no (complete) scan within timeout_ms
|
||||
DeviceDisconnected, // peer closed the connection / socket recv error
|
||||
};
|
||||
|
||||
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::Timeout: return "Timeout";
|
||||
case ErrorCode::DeviceDisconnected: return "DeviceDisconnected";
|
||||
}
|
||||
return "Unknown";
|
||||
}
|
||||
|
||||
} // namespace lidarlib
|
||||
@@ -1,83 +0,0 @@
|
||||
#pragma once
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ESPE LGA60-320: 320° FOV, device sweeps 20°..340° with 0° at the rear
|
||||
// (angle_offset_deg = -180 so output 0° = ahead). Range per datasheet page;
|
||||
// the wire caps distance at 50000 mm.
|
||||
inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.05f, 50.f, -180.f };
|
||||
|
||||
// ESPE LGA60 over TCP (default port 8080) or UDP, ported from the vendor's
|
||||
// ROS driver; NOT verified on real hardware. open() sends the "RAuto" start
|
||||
// command; device parameters (spin rate, resolution, filters) are whatever
|
||||
// the vendor Windows config tool programmed — this driver does not set them.
|
||||
class EspeDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip: device address; use_udp selects the transport the device is
|
||||
// configured for (vendor default is TCP); inverted: unit mounted
|
||||
// upside-down → mirror the scan.
|
||||
explicit EspeDriver(const ModelConfig& cfg,
|
||||
const std::string& ip,
|
||||
uint16_t port = 8080,
|
||||
bool use_udp = false,
|
||||
bool inverted = false);
|
||||
~EspeDriver();
|
||||
|
||||
EspeDriver(const EspeDriver&) = delete;
|
||||
EspeDriver& operator=(const EspeDriver&) = delete;
|
||||
|
||||
// Connect + send the start-capture command.
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
bool fill_buffer(int timeout_ms); // one recv() into recv_buf_
|
||||
bool parse_buffer(); // consume frames; true when a scan completed
|
||||
void handle_range_frame(const uint8_t* frame, uint16_t data_size);
|
||||
void finish_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool use_udp_ = false;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Stream bytes carried across frame boundaries; per-instance.
|
||||
std::string recv_buf_;
|
||||
|
||||
// Per-revolution accumulation
|
||||
std::vector<float> pending_ranges_;
|
||||
std::vector<float> pending_intensities_;
|
||||
float rev_start_deg_ = 0.f; // device angle of the revolution's first point
|
||||
float angle_inc_deg_ = 0.f;
|
||||
uint32_t points_total_ = 0; // measure_size from the header; 0 = no rev open
|
||||
uint16_t pending_time_ = 0; // header "time" field, unit unverified
|
||||
|
||||
// Latched from the newest "WSimu" area frame, if the device sends any.
|
||||
std::optional<uint16_t> espe_error_status_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by finish_scan().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
};
|
||||
|
||||
} // namespace lidarlib
|
||||
@@ -1,236 +0,0 @@
|
||||
#pragma once
|
||||
#include "lidarlib/diagnostics.hpp"
|
||||
#include "lidarlib/error.hpp"
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <functional>
|
||||
#include <optional>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// 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 devices, always 0
|
||||
float scan_time = 0.f; // sec — not exposed by devices, always 0
|
||||
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 family doesn't carry stay std::nullopt.
|
||||
struct ExtraInfo {
|
||||
std::string detected_model = "AUTO";
|
||||
uint8_t error_status = 0; // Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
|
||||
uint8_t distance_scale_mm = 0; // 0 = not reported
|
||||
|
||||
// Family A only
|
||||
std::optional<uint16_t> rotation_raw;
|
||||
|
||||
// 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;
|
||||
};
|
||||
|
||||
struct ScanResult {
|
||||
LaserScan scan;
|
||||
ExtraInfo info;
|
||||
};
|
||||
|
||||
// Per-model configuration. scan_angle_* use the signed system [-180,180]:
|
||||
// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
|
||||
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 (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 make_lidar / remap_scan_window): 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
|
||||
};
|
||||
|
||||
inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270°
|
||||
inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
||||
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B)
|
||||
inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line
|
||||
inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
||||
|
||||
// Model unknown ahead of time: Family B/C packets carry enough to auto-detect;
|
||||
// Family A doesn't, so the wide default FOV is kept.
|
||||
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
|
||||
|
||||
using ScanCallback = std::function<void(const ScanResult&)>;
|
||||
|
||||
// Unified driver interface returned by make_lidar(); OLEI and SICK drivers
|
||||
// both derive from it.
|
||||
class Lidar {
|
||||
public:
|
||||
virtual ~Lidar() = default;
|
||||
|
||||
// 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 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 while the device is
|
||||
// silent); fires the scan callback when a scan completed. False on error.
|
||||
virtual bool spin_once() = 0;
|
||||
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_{};
|
||||
};
|
||||
|
||||
// OLEI UDP driver.
|
||||
class Driver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip: local bind address; port: UDP port the lidar sends to;
|
||||
// inverted: unit mounted upside-down → mirror every angle.
|
||||
explicit Driver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 2368,
|
||||
bool inverted = false);
|
||||
~Driver();
|
||||
|
||||
Driver(const Driver&) = delete;
|
||||
Driver& operator=(const Driver&) = delete;
|
||||
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// Model name read from the Family B/C header; "AUTO" until one is seen.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
bool poll_packet(); // one recvfrom() + dispatch to the family parser
|
||||
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
|
||||
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
|
||||
bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5)
|
||||
|
||||
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
|
||||
void flush_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-revolution accumulation buffers (index-aligned)
|
||||
std::vector<float> pending_angle_deg_;
|
||||
std::vector<float> pending_dist_m_;
|
||||
std::vector<uint8_t> pending_intensity_;
|
||||
uint32_t pending_ts_ = 0;
|
||||
uint8_t pending_err_ = 0;
|
||||
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
||||
|
||||
ExtraInfo pending_info_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by flush_scan().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
|
||||
// Per-instance so two drivers on two threads don't race.
|
||||
uint8_t recv_buf_[4096];
|
||||
|
||||
bool auto_detect_ = false;
|
||||
bool model_locked_ = false;
|
||||
std::string detected_model_name_ = "AUTO";
|
||||
};
|
||||
|
||||
} // namespace lidarlib
|
||||
@@ -1,7 +0,0 @@
|
||||
#pragma once
|
||||
// One-include convenience header for the whole public API.
|
||||
#include "lidarlib/error.hpp"
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include "lidarlib/espe_lidar.hpp"
|
||||
#include "lidarlib/config.hpp"
|
||||
@@ -1,118 +0,0 @@
|
||||
#pragma once
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
|
||||
// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
|
||||
// frame puts 90° at the device front.
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571, 270°, 25m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781, 270°, 25m
|
||||
|
||||
// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
|
||||
// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
|
||||
// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
|
||||
class SickDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// inverted: unit mounted upside-down → mirror the scan.
|
||||
explicit SickDriver(const ModelConfig& cfg,
|
||||
const std::string& ip,
|
||||
uint16_t port = 2111,
|
||||
bool inverted = false);
|
||||
~SickDriver();
|
||||
|
||||
SickDriver(const SickDriver&) = delete;
|
||||
SickDriver& operator=(const SickDriver&) = delete;
|
||||
|
||||
// Connect + send "sEN LMDscandata 1" to start continuous scan output.
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
bool send_telegram(const std::string& body);
|
||||
bool read_telegram(std::string& out, int timeout_ms);
|
||||
bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
// Leftover TCP bytes carried across telegram boundaries; per-instance.
|
||||
std::string recv_buf_;
|
||||
};
|
||||
|
||||
inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
|
||||
|
||||
// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
|
||||
// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
|
||||
// Passive UDP receiver: the sensor's UDP target must be configured up front in
|
||||
// SICK Safety Designer — this class does no CoLa2/TCP handshake.
|
||||
class NanoScanDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip: local bind address; port: local UDP port the sensor sends to;
|
||||
// inverted: unit mounted upside-down → mirror the scan.
|
||||
explicit NanoScanDriver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 6060,
|
||||
bool inverted = false);
|
||||
~NanoScanDriver();
|
||||
|
||||
NanoScanDriver(const NanoScanDriver&) = delete;
|
||||
NanoScanDriver& operator=(const NanoScanDriver&) = delete;
|
||||
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
int recv_datagram(int timeout_ms);
|
||||
bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by parse_packet().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
|
||||
std::vector<uint8_t> recv_buf_;
|
||||
};
|
||||
|
||||
} // namespace lidarlib
|
||||
Reference in New Issue
Block a user