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:
299
plugins/driver_espe/espe_driver.cpp
Normal file
299
plugins/driver_espe/espe_driver.cpp
Normal file
@@ -0,0 +1,299 @@
|
||||
// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP.
|
||||
#include "espe_driver.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
namespace {
|
||||
// "RAuto" + fixed tail — puts the device into continuous measurement output.
|
||||
constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};
|
||||
|
||||
constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
|
||||
constexpr char kAreaMagic[5] = {'W', 'S', 'i', 'm', 'u'};
|
||||
|
||||
constexpr size_t kRangeHeaderSize = 16; // magic + 6 big-endian u16 fields
|
||||
constexpr size_t kAreaFrameSize = 13; // magic + 4 status bytes + err u16 + crc u16
|
||||
constexpr uint16_t kMaxDistanceMm = 50000; // wire sentinel: beyond = no return
|
||||
constexpr uint16_t kMaxIntensity = 30000;
|
||||
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
|
||||
constexpr int kConnectTimeoutMs = 2000;
|
||||
|
||||
uint16_t be16(const uint8_t* p) {
|
||||
return static_cast<uint16_t>((p[0] << 8) | p[1]);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
EspeDriver::EspeDriver(const ModelConfig& cfg, const std::string& ip,
|
||||
uint16_t port, bool use_udp, bool inverted)
|
||||
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip),
|
||||
port_(port), use_udp_(use_udp), inverted_(inverted) {}
|
||||
|
||||
EspeDriver::~EspeDriver() { close(); }
|
||||
|
||||
ErrorCode EspeDriver::open() {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
|
||||
return set_error(ErrorCode::InvalidAddress);
|
||||
|
||||
sock_fd_ = ::socket(AF_INET, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
|
||||
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
|
||||
|
||||
ErrorCode conn_err = ErrorCode::Ok;
|
||||
if (use_udp_) {
|
||||
// connect() on UDP just fixes the peer; replies come to our port.
|
||||
if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
|
||||
conn_err = ErrorCode::ConnectionFailed;
|
||||
} else {
|
||||
conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
|
||||
}
|
||||
|
||||
if (conn_err != ErrorCode::Ok) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return set_error(conn_err);
|
||||
}
|
||||
|
||||
recv_buf_.clear();
|
||||
points_total_ = 0;
|
||||
pending_time_ = 0;
|
||||
scan_ready_ = false;
|
||||
espe_error_status_.reset();
|
||||
latest_diag_ = Diagnostics{};
|
||||
|
||||
// Device is passive until told to stream.
|
||||
ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
|
||||
if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
|
||||
close();
|
||||
return set_error(ErrorCode::HandshakeFailed);
|
||||
}
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void EspeDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool EspeDriver::fill_buffer(int timeout_ms) {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
|
||||
if (timeout_ms > 0) {
|
||||
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
|
||||
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
|
||||
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
|
||||
if (r <= 0) {
|
||||
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
char buf[4096];
|
||||
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
|
||||
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
|
||||
recv_buf_.append(buf, static_cast<size_t>(n));
|
||||
return true;
|
||||
}
|
||||
|
||||
// Consume complete frames from recv_buf_; returns true once a full revolution
|
||||
// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
|
||||
bool EspeDriver::parse_buffer() {
|
||||
for (;;) {
|
||||
size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
|
||||
size_t area_pos = recv_buf_.find(kAreaMagic, 0, sizeof(kAreaMagic));
|
||||
size_t pos = std::min(range_pos, area_pos);
|
||||
if (pos == std::string::npos) {
|
||||
// No magic in sight: keep only a possible partial magic at the tail.
|
||||
if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
|
||||
recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
|
||||
return scan_ready_;
|
||||
}
|
||||
if (pos > 0) recv_buf_.erase(0, pos);
|
||||
|
||||
const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());
|
||||
|
||||
if (area_pos < range_pos) {
|
||||
if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
|
||||
// Zone/obstacle frame — only sent when the host polls areas, but
|
||||
// it carries the device fault word, so latch it if it appears.
|
||||
// Byte order unverified on hardware: the protocol is mixed-endian
|
||||
// (header fields big-endian, point payload little-endian) and no
|
||||
// spec covers this field; little-endian assumed like the payload.
|
||||
espe_error_status_ = le16(d + 9);
|
||||
recv_buf_.erase(0, kAreaFrameSize);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
|
||||
uint16_t data_size = be16(d + 8);
|
||||
uint16_t measure_size = be16(d + 12);
|
||||
if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
|
||||
recv_buf_.erase(0, sizeof(kRangeMagic)); // bogus header — resync
|
||||
continue;
|
||||
}
|
||||
if (data_size > measure_size) data_size = measure_size;
|
||||
|
||||
size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
|
||||
if (recv_buf_.size() < frame_size) return scan_ready_;
|
||||
|
||||
handle_range_frame(d, data_size);
|
||||
recv_buf_.erase(0, frame_size);
|
||||
// Stop as soon as a revolution completes — draining further frames
|
||||
// could finish a second revolution and overwrite ready_result_ before
|
||||
// the caller consumes it. Leftover bytes wait for the next call.
|
||||
if (scan_ready_) return true;
|
||||
}
|
||||
}
|
||||
|
||||
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
|
||||
// data_size (points in this frame), data_position (cumulative points incl.
|
||||
// this frame), measure_size (points per revolution), time; then data_size ×
|
||||
// 4 B little-endian (u16 distance mm, u16 intensity).
|
||||
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
|
||||
uint16_t start_angle = be16(frame + 4);
|
||||
uint16_t end_angle = be16(frame + 6);
|
||||
uint16_t data_position = be16(frame + 10);
|
||||
uint16_t measure_size = be16(frame + 12);
|
||||
pending_time_ = be16(frame + 14);
|
||||
|
||||
// First frame of a revolution (or geometry changed) → start a new one.
|
||||
if (points_total_ != measure_size || data_position <= data_size) {
|
||||
points_total_ = measure_size;
|
||||
rev_start_deg_ = static_cast<float>(start_angle);
|
||||
angle_inc_deg_ = static_cast<float>(end_angle - start_angle) / measure_size;
|
||||
pending_ranges_.assign(points_total_, 0.f);
|
||||
pending_intensities_.assign(points_total_, 0.f);
|
||||
}
|
||||
if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }
|
||||
|
||||
// start_angle is normally constant across the revolution, so this is just
|
||||
// the cumulative position; the angle term covers firmware that advances it.
|
||||
int32_t begin = static_cast<int32_t>(std::lround(
|
||||
(static_cast<float>(start_angle) - rev_start_deg_) / angle_inc_deg_))
|
||||
+ static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
|
||||
|
||||
const uint8_t* p = frame + kRangeHeaderSize;
|
||||
for (uint16_t i = 0; i < data_size; ++i, p += 4) {
|
||||
int32_t idx = begin + i;
|
||||
if (idx < 0 || idx >= static_cast<int32_t>(points_total_)) continue;
|
||||
uint16_t dist = le16(p + 0);
|
||||
uint16_t inten = le16(p + 2);
|
||||
pending_ranges_[idx] = (dist > kMaxDistanceMm)
|
||||
? std::numeric_limits<float>::infinity()
|
||||
: static_cast<float>(dist) * 1e-3f; // mm -> m
|
||||
// Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
|
||||
pending_intensities_[idx] =
|
||||
static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
|
||||
* (255.f / kMaxIntensity);
|
||||
}
|
||||
|
||||
if (data_position >= points_total_) finish_scan();
|
||||
}
|
||||
|
||||
void EspeDriver::finish_scan() {
|
||||
LaserScan& scan = ready_result_.scan;
|
||||
scan = LaserScan{};
|
||||
scan.timestamp_ms = pending_time_; // header "time" field, unit unverified
|
||||
scan.ranges = std::move(pending_ranges_);
|
||||
scan.intensities = std::move(pending_intensities_);
|
||||
scan.angle_min = (rev_start_deg_ + cfg_.angle_offset_deg) * kDeg2Rad;
|
||||
scan.angle_increment = angle_inc_deg_ * kDeg2Rad;
|
||||
scan.angle_max = scan.angle_min +
|
||||
scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
|
||||
scan.range_min = cfg_.range_min_m;
|
||||
scan.range_max = cfg_.range_max_m;
|
||||
|
||||
finalize_scan(scan, cfg_, inverted_);
|
||||
|
||||
ExtraInfo& info = ready_result_.info;
|
||||
info = ExtraInfo{};
|
||||
info.detected_model = cfg_.name;
|
||||
info.espe_error_status = espe_error_status_;
|
||||
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
|
||||
mark_scan_decoded();
|
||||
|
||||
pending_ranges_.clear();
|
||||
pending_intensities_.clear();
|
||||
points_total_ = 0;
|
||||
scan_ready_ = true;
|
||||
}
|
||||
|
||||
bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
for (;;) {
|
||||
if (parse_buffer()) {
|
||||
scan_ready_ = false;
|
||||
out = std::move(ready_result_);
|
||||
set_error(ErrorCode::Ok);
|
||||
return true;
|
||||
}
|
||||
if (!fill_buffer(timeout_ms)) return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool EspeDriver::spin_once() {
|
||||
if (!parse_buffer()) {
|
||||
if (!fill_buffer(0)) return false;
|
||||
parse_buffer();
|
||||
}
|
||||
if (scan_ready_) {
|
||||
scan_ready_ = false;
|
||||
if (cb_) cb_(ready_result_);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ── plugin registration ─────────────────────────────────────────────────────
|
||||
|
||||
namespace {
|
||||
|
||||
const DriverInfo kDriverInfo = [] {
|
||||
DriverInfo info;
|
||||
info.vendor = "ESPE";
|
||||
info.model = "LGA60";
|
||||
info.driver_id = "espe_lga60_driver";
|
||||
info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
|
||||
"use_udp; open() sends the RAuto start command; device "
|
||||
"parameters come from the vendor Windows tool. Default "
|
||||
"port 8080 (vendor default IP 192.168.1.88). Ported from "
|
||||
"the vendor ROS driver; not verified on real hardware.";
|
||||
info.transport = Transport::Tcp;
|
||||
info.transport_selectable = true; // use_udp switches to UDP
|
||||
info.supported_models = {"ESPE-LGA60"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
} // namespace
|
||||
|
||||
DriverInfo EspeDriver::get_driver_info() const { return kDriverInfo; }
|
||||
|
||||
} // namespace xlidar
|
||||
|
||||
XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
||||
*out = xlidar::kDriverInfo;
|
||||
}
|
||||
|
||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||
using namespace xlidar;
|
||||
const uint16_t port = cfg->port ? cfg->port : 8080;
|
||||
return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
|
||||
cfg->ip, port, cfg->use_udp, cfg->inverted);
|
||||
}
|
||||
Reference in New Issue
Block a user