102 lines
4.2 KiB
C++
102 lines
4.2 KiB
C++
// ESPE LGA60 laser scanner over TCP or UDP — plugin-private header.
|
|
#pragma once
|
|
#include "lidar_interface.hpp"
|
|
|
|
#include <chrono>
|
|
#include <cstdint>
|
|
#include <optional>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
namespace xlidar {
|
|
|
|
// 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.
|
|
//
|
|
// 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
|
|
// 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;
|
|
|
|
DriverInfo get_driver_info() const override;
|
|
|
|
// 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 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_;
|
|
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. 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<float> pending_ranges_;
|
|
std::vector<float> pending_intensities_;
|
|
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<uint16_t> espe_error_status_;
|
|
|
|
// Snapshot for get_diagnostics(); refreshed by finish_scan().
|
|
Diagnostics latest_diag_;
|
|
|
|
ScanResult ready_result_;
|
|
bool scan_ready_ = false;
|
|
};
|
|
|
|
} // namespace xlidar
|