QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(config,diagnostics): explicit transport in DeviceConfig, vendor-neutral diagnostics

Browse Source 
DeviceConfig now carries an optional transport (serial/udp/tcp) instead of
the ESPE-only use_udp bool. Plugins validate it in create_driver_instance:
a fixed-transport driver configured with the wrong transport fails open()
with InvalidConfig (via InvalidConfigDriver — the plugin ABI forbids
returning nullptr) rather than silently ignoring the setting. Selectable
drivers (ESPE) switch TCP/UDP through the same field. config.json
load/save round-trips "transport" for every transport, including serial,
and migrates legacy use_udp:true entries.

Diagnostics drops the per-vendor accessors (espe_fault, rplidar_fault,
monitor_fault, sick_error, pollution_*, contamination_*, manipulation) for
one common shape: a list of DiagnosticIssue{severity, code, detail} with
cross-vendor codes, plus a raw map of vendor passthrough values and
to_json() for hosts that prefer a string. Vendor bit decoding now lives in
one place (decode_diagnostics); has_fault/has_warning/healthy keep their
meaning, so is_ready()/wait_ready() are unchanged.

Also: README regains the model/protocol and ExtraInfo tables lost in the
lidarlib->xlidar refactor (verified against current code), and the empty
xlocd/ tree left by a stray sync run is gone.

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
Trương Văn Lộc
2026-07-13 09:44:41 +07:00
parent f02d81c031
commit ef217bdca8
12 changed files with 455 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
include/lidar_diagnostics.hpp
View File

@@ -1,14 +1,23 @@
#pragma once
// xlidar-driver — device self-diagnostics decoded from the data stream.
//
// The public surface is vendor-neutral: every driver reports through the same
// Diagnostics struct — a list of DiagnosticIssue with stable cross-vendor
// codes, plus a raw field map for vendor-specific passthrough. Hosts never
// need per-vendor accessors; serialize with to_json() when a string API is
// more convenient.
#include <cstdint>
#include <cstdio>
#include <optional>
#include <map>
#include <string>
#include <vector>
namespace xlidar {
struct ExtraInfo; // lidar_interface.hpp
// ── Raw wire constants (document the values in Diagnostics::raw) ────────────
// 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
@@ -34,72 +43,60 @@ 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.
// ── Common diagnostics structure ────────────────────────────────────────────
// Fault = device says something is wrong now, stop trusting the data;
// Warning = degraded but still measuring (dirty optics, weak motor) —
// schedule cleaning/service.
enum class DiagSeverity { Warning, Fault };
inline const char* to_string(DiagSeverity s) {
return s == DiagSeverity::Fault ? "fault" : "warning";
}
// One decoded device issue. `code` is a stable, machine-readable identifier
// shared across vendors:
// "motor" — motor/monitor subsystem abnormal
// "voltage" — supply voltage out of range
// "temperature" — internal temperature abnormal
// "optics_dirty" — pollution/contamination of the optics window
// (warning: clean soon; fault: data no longer reliable)
// "manipulation" — safety scanner suspects tampering/covering
// "device_error" — device-level fault the vendor doesn't break down
// "device_warning" — device-level warning the vendor doesn't break down
// `detail` is human-readable, names the vendor, and may carry the raw value.
struct DiagnosticIssue {
DiagSeverity severity = DiagSeverity::Fault;
std::string code;
std::string detail;
};
// Device self-diagnostics decoded from the data stream. valid stays false
// until the driver has decoded one full scan; issues is empty while the
// device reports healthy. Vendor-specific raw fields appear in `raw` keyed
// by stable names ("olei.error_status", "sick.device_status",
// "nano.general_state", "espe.error_status", "rplidar.health_status",
// "rplidar.error_code", ...) — only fields present on the wire are set.
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
// OLEI Family A error byte (0 = no fault; Family B/C don't carry it)
uint8_t error_status = 0;
std::vector<DiagnosticIssue> issues;
std::map<std::string, uint32_t> raw;
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; }
// OLEI Family A only — raw motor speed field, unit unverified
std::optional<uint16_t> rotation_raw;
// 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;
std::optional<uint32_t> field_status;
std::optional<uint32_t> status_flags;
// SICK TiM — LMDscandata status pair (word0<<8)|word1
std::optional<uint16_t> sick_device_status;
bool sick_error() const { return sick_device_status && (*sick_device_status & kSickStatusError); }
bool pollution_warning() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionWarning); }
bool pollution_error() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionError); }
// SICK nanoScan3 — General System State byte 0
std::optional<uint8_t> nano_general_state;
bool contamination_warning() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationWarning); }
bool contamination_error() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); }
bool manipulation() const { return nano_general_state && (*nano_general_state & kNanoStateManipulation); }
// ESPE LGA60 — raw fault word from area frames (bit meanings unverified);
// only present when the host polls area data.
std::optional<uint16_t> espe_error_status;
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, weak motor) — schedule cleaning/service.
bool has_fault() const {
return error_status != 0 || sick_error() || pollution_error()
|| contamination_error() || manipulation() || espe_fault()
|| rplidar_fault();
for (const auto& i : issues)
if (i.severity == DiagSeverity::Fault) return true;
return false;
}
bool has_warning() const {
return pollution_warning() || contamination_warning() || rplidar_warning();
for (const auto& i : issues)
if (i.severity == DiagSeverity::Warning) return true;
return false;
}
bool healthy() const { return valid && !has_fault(); }
bool healthy() const { return valid && !has_fault(); }
};
// Decode the diagnostic fields of one scan; sets valid = true.
@@ -107,40 +104,77 @@ struct Diagnostics {
// every plugin .so must carry its own copy).
Diagnostics decode_diagnostics(const ExtraInfo& info);
// One-line log summary: "no data" / "ok" / "WARN: pollution" /
// "FAULT: voltage temperature".
// One-line log summary: "no data" / "ok" / "WARN: optics_dirty" /
// "FAULT: voltage temperature | WARN: optics_dirty".
inline std::string to_string(const Diagnostics& d) {
if (!d.valid) return "no data";
if (!d.has_fault()) return d.has_warning()
? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
+ (d.contamination_warning() ? " contamination" : "")
+ (d.rplidar_warning() ? " rplidar" : "")
: "ok";
if (!d.valid) return "no data";
if (d.issues.empty()) return "ok";
std::string faults, warnings;
for (const auto& i : d.issues)
(i.severity == DiagSeverity::Fault ? faults : warnings) += " " + i.code;
std::string s;
if (!faults.empty()) s += "FAULT:" + faults;
if (!warnings.empty()) s += (s.empty() ? "WARN:" : " | WARN:") + warnings;
return s;
}
std::string s = "FAULT:";
if (d.monitor_fault()) s += " monitor";
if (d.voltage_fault()) s += " voltage";
if (d.temperature_fault()) s += " temperature";
if (d.sick_error()) s += " device";
if (d.pollution_error()) s += " pollution";
if (d.contamination_error()) s += " contamination";
if (d.manipulation()) s += " manipulation";
if (d.espe_fault()) {
char buf[24];
std::snprintf(buf, sizeof(buf), " espe(0x%04X)", *d.espe_error_status);
s += buf;
namespace detail {
// Minimal JSON string escaping (quotes, backslash, control characters) —
// model/firmware come off the wire and may hold arbitrary bytes.
inline std::string json_escape(const std::string& in) {
std::string out;
out.reserve(in.size());
for (unsigned char c : in) {
switch (c) {
case '"': out += "\\\""; break;
case '\\': out += "\\\\"; break;
case '\n': out += "\\n"; break;
case '\r': out += "\\r"; break;
case '\t': out += "\\t"; break;
default:
if (c < 0x20) {
char buf[8];
std::snprintf(buf, sizeof(buf), "\\u%04X", c);
out += buf;
} else {
out += static_cast<char>(c);
}
}
}
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;
return out;
}
} // namespace detail
// Full JSON snapshot, e.g. for a REST/telemetry payload:
// {"valid":true,"model":"C1","firmware":"fw 1.32 hw 18",
// "device_timestamp_ms":0,"healthy":false,
// "issues":[{"severity":"fault","code":"voltage","detail":"..."}],
// "raw":{"olei.error_status":2}}
inline std::string to_json(const Diagnostics& d) {
std::string s = "{\"valid\":";
s += d.valid ? "true" : "false";
s += ",\"model\":\"" + detail::json_escape(d.model) + "\"";
s += ",\"firmware\":\"" + detail::json_escape(d.firmware) + "\"";
s += ",\"device_timestamp_ms\":" + std::to_string(d.device_timestamp_ms);
s += ",\"healthy\":";
s += d.healthy() ? "true" : "false";
s += ",\"issues\":[";
for (size_t i = 0; i < d.issues.size(); ++i) {
const DiagnosticIssue& issue = d.issues[i];
if (i) s += ',';
s += "{\"severity\":\"";
s += to_string(issue.severity);
s += "\",\"code\":\"" + detail::json_escape(issue.code) + "\"";
s += ",\"detail\":\"" + detail::json_escape(issue.detail) + "\"}";
}
if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
char buf[24];
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
s += buf;
s += "],\"raw\":{";
bool first = true;
for (const auto& [key, value] : d.raw) {
if (!first) s += ',';
first = false;
s += "\"" + detail::json_escape(key) + "\":" + std::to_string(value);
}
s += "}}";
return s;
}

121
include/lidar_interface.hpp
View File

@@ -132,23 +132,89 @@ struct ScanResult {
ExtraInfo info;
};
// Decode the diagnostic fields of one scan; sets valid = true.
// Decode the diagnostic fields of one scan into the vendor-neutral
// Diagnostics structure; sets valid = true. Vendor bit layouts are decoded
// here (constants in lidar_diagnostics.hpp) so hosts only ever see common
// issue codes; the raw values ride along in Diagnostics::raw.
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;
d.valid = true;
d.model = info.detected_model;
const auto add = [&d](DiagSeverity severity, const char* code, std::string detail) {
d.issues.push_back({severity, code, std::move(detail)});
};
char buf[48];
// OLEI Family A error byte (Family B/C don't carry it — stays 0).
if (info.error_status != 0) {
d.raw["olei.error_status"] = info.error_status;
if (info.error_status & kFaultMonitor)
add(DiagSeverity::Fault, "motor", "OLEI monitor/motor abnormal");
if (info.error_status & kFaultVoltage)
add(DiagSeverity::Fault, "voltage", "OLEI supply voltage out of range");
if (info.error_status & kFaultTemperature)
add(DiagSeverity::Fault, "temperature", "OLEI internal temperature abnormal");
if (const uint8_t rest = info.error_status
& static_cast<uint8_t>(~(kFaultMonitor | kFaultVoltage | kFaultTemperature))) {
std::snprintf(buf, sizeof(buf), "OLEI reserved error bits 0x%02X", rest);
add(DiagSeverity::Fault, "device_error", buf);
}
}
// OLEI raw passthroughs (meanings unverified — no issue decoding).
if (info.rotation_raw) d.raw["olei.rotation"] = *info.rotation_raw;
if (info.distance_ratio_raw) d.raw["olei.distance_ratio"] = *info.distance_ratio_raw;
if (info.scan_frequency_raw) d.raw["olei.scan_frequency"] = *info.scan_frequency_raw;
if (info.input_status) d.raw["olei.input_status"] = *info.input_status;
if (info.output_status) d.raw["olei.output_status"] = *info.output_status;
if (info.field_status) d.raw["olei.field_status"] = *info.field_status;
if (info.status_flags) d.raw["olei.status_flags"] = *info.status_flags;
// SICK TiM device status pair.
if (info.sick_device_status) {
d.raw["sick.device_status"] = *info.sick_device_status;
if (*info.sick_device_status & kSickStatusError)
add(DiagSeverity::Fault, "device_error", "SICK TiM device error");
if (*info.sick_device_status & kSickStatusPollutionWarning)
add(DiagSeverity::Warning, "optics_dirty", "SICK TiM pollution warning");
if (*info.sick_device_status & kSickStatusPollutionError)
add(DiagSeverity::Fault, "optics_dirty", "SICK TiM pollution error");
}
// SICK nanoScan3 general system state.
if (info.nano_general_state) {
d.raw["nano.general_state"] = *info.nano_general_state;
if (*info.nano_general_state & kNanoStateContaminationWarning)
add(DiagSeverity::Warning, "optics_dirty", "nanoScan3 contamination warning");
if (*info.nano_general_state & kNanoStateContaminationError)
add(DiagSeverity::Fault, "optics_dirty", "nanoScan3 contamination error");
if (*info.nano_general_state & kNanoStateManipulation)
add(DiagSeverity::Fault, "manipulation", "nanoScan3 manipulation suspected");
}
// ESPE fault word (bit meanings unverified).
if (info.espe_error_status) {
d.raw["espe.error_status"] = *info.espe_error_status;
if (*info.espe_error_status != 0) {
std::snprintf(buf, sizeof(buf), "ESPE fault word 0x%04X", *info.espe_error_status);
add(DiagSeverity::Fault, "device_error", buf);
}
}
// RPLIDAR SDK health.
if (info.rplidar_health_status) {
d.raw["rplidar.health_status"] = *info.rplidar_health_status;
if (info.rplidar_error_code) d.raw["rplidar.error_code"] = *info.rplidar_error_code;
if (*info.rplidar_health_status == kRplidarHealthError) {
std::snprintf(buf, sizeof(buf), "RPLIDAR health error, code 0x%04X",
info.rplidar_error_code ? *info.rplidar_error_code : 0);
add(DiagSeverity::Fault, "device_error", buf);
} else if (*info.rplidar_health_status == kRplidarHealthWarning) {
add(DiagSeverity::Warning, "device_warning", "RPLIDAR health warning");
}
}
return d;
}
@@ -183,7 +249,7 @@ using ScanCallback = std::function<void(const ScanResult&)>;
// 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.
// default and honor DeviceConfig::transport.
enum class Transport { Serial, Udp, Tcp };
inline const char* to_string(Transport t) {
@@ -195,6 +261,14 @@ inline const char* to_string(Transport t) {
return "unknown";
}
// Parse the strings written by to_string(Transport); nullopt for anything else.
inline std::optional<Transport> transport_from_string(const std::string& s) {
if (s == "serial") return Transport::Serial;
if (s == "udp") return Transport::Udp;
if (s == "tcp") return Transport::Tcp;
return std::nullopt;
}
// Static identity a plugin registers about itself (get_driver_info entry
// point and LidarDriverInterface::get_driver_info()).
struct DriverInfo {
@@ -207,18 +281,25 @@ struct DriverInfo {
// 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
bool transport_selectable = false; // true → DeviceConfig::transport
// may pick either TCP or 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:
// Which fields matter depends on the transport in effect:
// 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
// Transport to reach the device. nullopt = the driver's declared default
// (DriverInfo::transport). A fixed-transport driver rejects a mismatch
// from open() with InvalidConfig; transport-selectable drivers (ESPE)
// switch between TCP and UDP through this field.
std::optional<Transport> transport;
// Network transports (udp: local bind address / tcp: device address)
std::string ip = "0.0.0.0";
uint16_t port = 0; // 0 = driver default
@@ -228,7 +309,6 @@ struct DeviceConfig {
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
@@ -252,9 +332,10 @@ struct DeviceConfig {
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.transport == b.transport &&
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.inverted == b.inverted &&
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 &&