diff --git a/README.md b/README.md index ffed89b..5dd2a84 100644 --- a/README.md +++ b/README.md @@ -264,6 +264,16 @@ không handshake CoLa2/TCP. −180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi. +Mỗi lượt quét được thiết bị chia thành NHIỀU packet `HISN`: các bộ đếm điểm +trong header là của riêng packet đó, còn vòng quét là cửa sổ cố định +20°→340° = 320°/bước điểm — driver ghép các packet theo `start_angle` và chỉ +phát scan khi packet đóng ở 340°. Điểm không packet nào gửi = NaN. Bước góc +lấy từ packet mở vòng (span/số điểm) rồi giữ nguyên. +Chu kỳ quay đo host-side giữa hai vòng: `scan_time` là thời gian QUÉT +(period × 320/360, đúng như driver ROS của hãng dùng để đóng dấu), nên spin +rate suy ra từ `1/scan_time` cao hơn cơ khí 360/320; vòng đầu chưa đo được +thì để 0. Trường `time` 16-bit trong header là bộ đếm thiết bị đơn vị chưa +xác định (header của hãng ghi "chưa kích hoạt") → `timestamp_ms` = 0. Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng. @@ -289,7 +299,7 @@ comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md). | `intensities` | Cường độ 0–255 | | `range_min` / `range_max` | Dải đo hợp lệ (m) | | `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có | -| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 | +| `time_increment` / `scan_time` | Chỉ rplidar (chu kỳ grab thực) và ESPE (chu kỳ vòng quét, × 320/360) đo được; driver khác = 0 | `ExtraInfo`: metadata thô tuỳ giao thức — field thiết bị không có giữ `std::nullopt`: diff --git a/plugins/driver_espe/espe_driver.cpp b/plugins/driver_espe/espe_driver.cpp index d692374..871905d 100644 --- a/plugins/driver_espe/espe_driver.cpp +++ b/plugins/driver_espe/espe_driver.cpp @@ -29,6 +29,25 @@ 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(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::quiet_NaN(); + uint16_t be16(const uint8_t* p) { return static_cast((p[0] << 8) | p[1]); } @@ -69,9 +88,12 @@ ErrorCode EspeDriver::open() { } recv_buf_.clear(); - points_total_ = 0; - pending_time_ = 0; - scan_ready_ = false; + 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{}; @@ -147,6 +169,7 @@ bool EspeDriver::parse_buffer() { 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(data_size) * 4; @@ -161,39 +184,86 @@ bool EspeDriver::parse_buffer() { } } -// 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 × +// 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(kMaxPointsPerRev)) return; // implausible step + if (angle_inc_deg_ > 0.f && static_cast(total) == points_total_) return; + + angle_inc_deg_ = inc_deg; + points_total_ = static_cast(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) { - 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); + 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. - // 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(start_angle); - angle_inc_deg_ = static_cast(end_angle - start_angle) / measure_size; - pending_ranges_.assign(points_total_, 0.f); - pending_intensities_.assign(points_total_, 0.f); + // 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(end_angle) - static_cast(start_angle); + if (span > 0.f && measure_size > 0) + set_resolution(span / static_cast(measure_size)); } - if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; } + if (angle_inc_deg_ <= 0.f) return; // step still unknown — nowhere to put the points - // 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(std::lround( - (static_cast(start_angle) - rev_start_deg_) / angle_inc_deg_)) - + static_cast(data_position) - static_cast(data_size); + // 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(std::lround( + (static_cast(start_angle) - static_cast(kSweepStartDeg)) / angle_inc_deg_)) + + static_cast(data_position) - static_cast(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(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(needed); + } 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(points_total_)) continue; - uint16_t dist = le16(p + 0); - uint16_t inten = le16(p + 2); + const int32_t idx = begin + i; + if (idx < 0 || idx >= static_cast(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::infinity() : static_cast(dist) * 1e-3f; // mm -> m @@ -203,22 +273,53 @@ void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) { * (255.f / kMaxIntensity); } - if (data_position >= points_total_) finish_scan(); + // 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{}; - scan.timestamp_ms = pending_time_; // header "time" field, unit unverified + // 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 = (rev_start_deg_ + cfg_.angle_offset_deg) * kDeg2Rad; + scan.angle_min = (static_cast(kSweepStartDeg) + cfg_.angle_offset_deg) * kDeg2Rad; scan.angle_increment = angle_inc_deg_ * kDeg2Rad; scan.angle_max = scan.angle_min + scan.angle_increment * static_cast(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(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(scan.ranges.size() - 1); + } + } + last_rev_end_ = now; + have_last_rev_ = true; + finalize_scan(scan, cfg_, inverted_); ExtraInfo& info = ready_result_.info; @@ -227,13 +328,10 @@ void EspeDriver::finish_scan() { 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; + begin_revolution(); // the vectors above were moved out — restore them + scan_ready_ = true; } bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) { diff --git a/plugins/driver_espe/espe_driver.hpp b/plugins/driver_espe/espe_driver.hpp index 8404282..7126664 100644 --- a/plugins/driver_espe/espe_driver.hpp +++ b/plugins/driver_espe/espe_driver.hpp @@ -2,6 +2,7 @@ #pragma once #include "lidar_interface.hpp" +#include #include #include #include @@ -18,6 +19,11 @@ inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.0 // 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. +// +// The device streams each 20°→340° sweep as several packets, so a scan is +// assembled across packets and only completes when the sweep closes at 340° +// (see handle_range_frame). Rotation timing is measured host-side: the wire +// carries no usable clock. class EspeDriver : public LidarDriverInterface { public: // ip: device address; use_udp selects the transport the device is @@ -52,6 +58,8 @@ 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 set_resolution(float inc_deg); // latch the step + (re)size the revolution + void begin_revolution(); // blank the buffer for a fresh sweep void finish_scan(); ModelConfig cfg_; @@ -66,13 +74,19 @@ private: // Stream bytes carried across frame boundaries; per-instance. std::string recv_buf_; - // Per-revolution accumulation + // Per-revolution accumulation. A revolution is the device's fixed + // 20°→340° sweep, so it holds 320°/angle_inc_deg_ points — several + // packets' worth. The header's point counters describe one PACKET and say + // nothing about the revolution's size. std::vector pending_ranges_; std::vector 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 + float angle_inc_deg_ = 0.f; // angular step; 0 = not latched yet + uint32_t points_total_ = 0; // points per revolution = 320° / step + + // Rotation period, measured host-side between completed revolutions: the + // wire carries no usable clock (see finish_scan()). + std::chrono::steady_clock::time_point last_rev_end_{}; + bool have_last_rev_ = false; // Latched from the newest "WSimu" area frame, if the device sends any. std::optional espe_error_status_;