403 lines
17 KiB
C++
403 lines
17 KiB
C++
// 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;
|
||
|
||
// The head measures over a fixed window of every turn — from 20° to 340° in
|
||
// device angles, 0° at the rear — and is blind over the remaining 40°. Both
|
||
// the revolution's size and every packet's position are anchored to that
|
||
// window (the vendor ROS driver hard-codes the same two numbers). Consistent
|
||
// with MODEL_ESPE_LGA60's -160…160 preset once angle_offset_deg (-180) is
|
||
// applied.
|
||
constexpr uint16_t kSweepStartDeg = 20;
|
||
constexpr uint16_t kSweepEndDeg = 340;
|
||
constexpr float kSweepSpanDeg = static_cast<float>(kSweepEndDeg - kSweepStartDeg);
|
||
constexpr float kFullTurnDeg = 360.f;
|
||
|
||
// Rotation periods above this are stalls/reconnects, not a spin rate (the
|
||
// device runs at 10 or 20 Hz): don't publish timing derived from them.
|
||
constexpr float kMaxRevPeriodS = 1.f;
|
||
|
||
// A point no packet ever delivered — "invalid", distinct from the infinity
|
||
// that means the device looked and got no return.
|
||
const float kMissingPoint = std::numeric_limits<float>::quiet_NaN();
|
||
|
||
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();
|
||
pending_ranges_.clear();
|
||
pending_intensities_.clear();
|
||
angle_inc_deg_ = 0.f;
|
||
points_total_ = 0;
|
||
have_last_rev_ = false;
|
||
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;
|
||
}
|
||
// Both counters describe this packet; the clamp is the vendor's.
|
||
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;
|
||
}
|
||
}
|
||
|
||
// Latch the angular step and size the revolution around it. Kept stable once
|
||
// latched: the wire angles are whole degrees, so a step re-derived from a
|
||
// mid-sweep packet jitters, and re-sizing would drop the sweep in flight.
|
||
void EspeDriver::set_resolution(float inc_deg) {
|
||
if (!(inc_deg > 0.f)) return;
|
||
const long total = std::lround(kSweepSpanDeg / inc_deg);
|
||
if (total < 2 || total > static_cast<long>(kMaxPointsPerRev)) return; // implausible step
|
||
if (angle_inc_deg_ > 0.f && static_cast<uint32_t>(total) == points_total_) return;
|
||
|
||
angle_inc_deg_ = inc_deg;
|
||
points_total_ = static_cast<uint32_t>(total);
|
||
begin_revolution();
|
||
}
|
||
|
||
void EspeDriver::begin_revolution() {
|
||
if (points_total_ == 0) return;
|
||
pending_ranges_.assign(points_total_, kMissingPoint);
|
||
pending_intensities_.assign(points_total_, 0.f);
|
||
}
|
||
|
||
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (the
|
||
// angular window THIS packet covers, whole degrees), data_size (points in
|
||
// this packet's payload), data_position and measure_size (the vendor's
|
||
// "position"/"count" of the current packet's points), time; then data_size ×
|
||
// 4 B little-endian (u16 distance mm, u16 intensity).
|
||
//
|
||
// A packet is a slice of the sweep, not a revolution: the device splits every
|
||
// 20°→340° sweep into several of them, the first opening at 20° and the last
|
||
// closing at 340°. So the revolution holds 320°/step points, NOT measure_size
|
||
// — reading measure_size as the revolution size (as this driver first did)
|
||
// emits one scan per packet, each covering only that packet's few degrees.
|
||
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
|
||
const uint16_t start_angle = be16(frame + 4);
|
||
const uint16_t end_angle = be16(frame + 6);
|
||
const uint16_t data_position = be16(frame + 10);
|
||
const uint16_t measure_size = be16(frame + 12);
|
||
// frame + 14 is a 16-bit device counter the vendor header marks as "time
|
||
// flag (not enabled)"; see finish_scan() for why it is not a timestamp.
|
||
|
||
// Step = this packet's angular span / its point count. Taken from the
|
||
// packet that opens a sweep — the one the vendor driver trusts — or from
|
||
// whatever arrives first while nothing is latched yet.
|
||
if (start_angle == kSweepStartDeg || angle_inc_deg_ <= 0.f) {
|
||
const float span = static_cast<float>(end_angle) - static_cast<float>(start_angle);
|
||
if (span > 0.f && measure_size > 0)
|
||
set_resolution(span / static_cast<float>(measure_size));
|
||
}
|
||
if (angle_inc_deg_ <= 0.f) return; // step still unknown — nowhere to put the points
|
||
|
||
// First packet of a sweep: drop anything a lost closing packet left behind.
|
||
if (start_angle == kSweepStartDeg && data_position <= data_size) begin_revolution();
|
||
|
||
// Index of this packet's first point within the sweep, verbatim from the
|
||
// vendor driver: the angular offset from 20°, plus what the header's own
|
||
// counters carry. Exactly one of the two terms moves, whichever way the
|
||
// firmware numbers its packets — either start_angle walks the sweep while
|
||
// data_position stays at this packet's own count, or start_angle stays at
|
||
// 20° while data_position accumulates — so the sum is the packet's true
|
||
// start index in both cases.
|
||
const int32_t begin =
|
||
static_cast<int32_t>(std::lround(
|
||
(static_cast<float>(start_angle) - static_cast<float>(kSweepStartDeg)) / angle_inc_deg_))
|
||
+ static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
|
||
|
||
// Integer wire angles make 320°/step land a point or two short of what the
|
||
// device actually streams; grow rather than clip the tail (the vendor
|
||
// driver does the same).
|
||
const size_t needed = static_cast<size_t>(begin > 0 ? begin : 0) + data_size;
|
||
if (needed > pending_ranges_.size() && needed <= kMaxPointsPerRev) {
|
||
pending_ranges_.resize(needed, kMissingPoint);
|
||
pending_intensities_.resize(needed, 0.f);
|
||
points_total_ = static_cast<uint32_t>(needed);
|
||
}
|
||
|
||
const uint8_t* p = frame + kRangeHeaderSize;
|
||
for (uint16_t i = 0; i < data_size; ++i, p += 4) {
|
||
const int32_t idx = begin + i;
|
||
if (idx < 0 || idx >= static_cast<int32_t>(pending_ranges_.size())) continue;
|
||
const uint16_t dist = le16(p + 0);
|
||
const 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);
|
||
}
|
||
|
||
// The packet that closes the sweep at 340°, with its point counter full,
|
||
// ends the revolution — the vendor driver's condition unchanged. It holds
|
||
// whichever way the firmware numbers packets: per-packet counters make
|
||
// data_position == measure_size true on every packet (so the 340° edge
|
||
// decides), cumulative ones make it true only on the sweep's last packet.
|
||
if (end_angle == kSweepEndDeg && data_position == measure_size) finish_scan();
|
||
}
|
||
|
||
void EspeDriver::finish_scan() {
|
||
if (pending_ranges_.size() < 2 || angle_inc_deg_ <= 0.f) return;
|
||
|
||
LaserScan& scan = ready_result_.scan;
|
||
scan = LaserScan{};
|
||
// The header's 16-bit "time" field is a device counter of unverified unit
|
||
// (the vendor header calls it "not enabled" and its ROS driver never
|
||
// stamps a scan with it), while this field is contracted to be a device
|
||
// clock in ms — leave it at 0 and report timing from the rotation below.
|
||
scan.timestamp_ms = 0;
|
||
scan.ranges = std::move(pending_ranges_);
|
||
scan.intensities = std::move(pending_intensities_);
|
||
scan.angle_min = (static_cast<float>(kSweepStartDeg) + 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;
|
||
|
||
// Timing: revolutions complete one rotation period apart, but the points
|
||
// in one only span the 320° the head measures — the remaining 40° is dead
|
||
// time before the next sweep starts. scan_time is what a consumer
|
||
// subtracts from the arrival time to date the FIRST point, so it must be
|
||
// the sweep, not the period (the vendor ROS driver stamps with the same
|
||
// 320/360 factor). Consequence: a spin rate read back as 1/scan_time is
|
||
// 360/320 higher than the mechanical one. The first revolution has
|
||
// nothing to measure against — leave the fields at 0 and let the consumer
|
||
// fall back.
|
||
const auto now = std::chrono::steady_clock::now();
|
||
if (have_last_rev_) {
|
||
const float period = std::chrono::duration<float>(now - last_rev_end_).count();
|
||
if (period > 0.f && period < kMaxRevPeriodS) {
|
||
scan.scan_time = period * (kSweepSpanDeg / kFullTurnDeg);
|
||
scan.time_increment = scan.scan_time / static_cast<float>(scan.ranges.size() - 1);
|
||
}
|
||
}
|
||
last_rev_end_ = now;
|
||
have_last_rev_ = true;
|
||
|
||
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);
|
||
mark_scan_decoded();
|
||
|
||
begin_revolution(); // the vectors above were moved out — restore them
|
||
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 "
|
||
"DeviceConfig::transport; 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; // transport = 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;
|
||
if (!transport_supported(kDriverInfo, *cfg))
|
||
return new InvalidConfigDriver(kDriverInfo,
|
||
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||
const uint16_t port = cfg->port ? cfg->port : 8080;
|
||
const bool use_udp = cfg->transport == Transport::Udp;
|
||
return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
|
||
cfg->ip, port, use_udp, cfg->inverted);
|
||
}
|