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

update brand ESPE

Browse Source
This commit is contained in:
QUYVN
2026-07-07 10:38:42 +07:00
parent 1c347a4918
commit 917b4fe4c5
15 changed files with 741 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
include/lidarlib/config.hpp
View File

@@ -12,8 +12,9 @@ struct LidarConfig {
std::string ip = "0.0.0.0";
uint16_t port = 2368;
std::string model = "AUTO";
bool inverted = false; // OLEI only
std::string brand = "OLEI"; // "OLEI" or "SICK"
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.
@@ -24,6 +25,7 @@ struct LidarConfig {
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); }
@@ -53,7 +55,8 @@ 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); anything else → OLEI 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);

13
include/lidarlib/diagnostics.hpp
View File

@@ -67,11 +67,17 @@ struct Diagnostics {
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; }
// Fault = device says something is wrong now; warning = degraded but
// still measuring (dirty optics) — schedule cleaning/service.
bool has_fault() const {
return error_status != 0 || sick_error() || pollution_error()
|| contamination_error() || manipulation();
|| contamination_error() || manipulation() || espe_fault();
}
bool has_warning() const { return pollution_warning() || contamination_warning(); }
bool healthy() const { return valid && !has_fault(); }
@@ -97,6 +103,11 @@ inline std::string to_string(const Diagnostics& d) {
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;
}
if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
char buf[24];
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);

83
include/lidarlib/espe_lidar.hpp Normal file
View File

@@ -0,0 +1,83 @@
#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

9
include/lidarlib/lidar.hpp
View File

@@ -47,6 +47,10 @@ struct ExtraInfo {
// 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 {
@@ -101,7 +105,11 @@ public:
// 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;
@@ -151,6 +159,7 @@ public:
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)

1
include/lidarlib/lidarlib.hpp
View File

@@ -3,4 +3,5 @@
#include "lidarlib/error.hpp"
#include "lidarlib/lidar.hpp"
#include "lidarlib/sick_lidar.hpp"
#include "lidarlib/espe_lidar.hpp"
#include "lidarlib/config.hpp"

14
include/lidarlib/sick_lidar.hpp
View File

@@ -20,9 +20,11 @@ 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);
uint16_t port = 2111,
bool inverted = false);
~SickDriver();
SickDriver(const SickDriver&) = delete;
@@ -50,6 +52,7 @@ private:
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_;
@@ -70,10 +73,12 @@ class NanoScanDriver : public Lidar {
public:
using ScanCallback = lidarlib::ScanCallback;
// ip: local bind address; port: local UDP port the sensor sends to.
// 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);
const std::string& ip = "0.0.0.0",
uint16_t port = 6060,
bool inverted = false);
~NanoScanDriver();
NanoScanDriver(const NanoScanDriver&) = delete;
@@ -99,6 +104,7 @@ private:
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_;