193 lines
7.6 KiB
C++
193 lines
7.6 KiB
C++
#pragma once
|
|
#include "lidarlib/diagnostics.hpp"
|
|
#include "lidarlib/error.hpp"
|
|
#include <cstdint>
|
|
#include <vector>
|
|
#include <string>
|
|
#include <functional>
|
|
#include <optional>
|
|
|
|
namespace lidarlib {
|
|
|
|
// ROS sensor_msgs/LaserScan-shaped output (radians, meters, seconds).
|
|
// ranges[i] is at angle_min + i*angle_increment, in sweep order.
|
|
struct LaserScan {
|
|
uint32_t timestamp_ms = 0; // device clock (ms); 0 if not on the wire
|
|
float angle_min = 0.f; // rad
|
|
float angle_max = 0.f; // rad
|
|
float angle_increment = 0.f; // rad
|
|
float time_increment = 0.f; // sec — not exposed by devices, always 0
|
|
float scan_time = 0.f; // sec — not exposed by devices, always 0
|
|
float range_min = 0.f; // m — from ModelConfig, not measured
|
|
float range_max = 0.f; // m — from ModelConfig, not measured
|
|
std::vector<float> ranges; // m
|
|
std::vector<float> intensities; // 0-255 as float
|
|
};
|
|
|
|
// Diagnostic/header fields; fields the family doesn't carry stay std::nullopt.
|
|
struct ExtraInfo {
|
|
std::string detected_model = "AUTO";
|
|
uint8_t error_status = 0; // Family A: BIT0=Monitor, BIT1=Voltage, BIT2=Temp
|
|
uint8_t distance_scale_mm = 0; // 0 = not reported
|
|
|
|
// Family A only
|
|
std::optional<uint16_t> rotation_raw;
|
|
|
|
// Family C / V3 (GS1-5) only — raw passthroughs, unverified
|
|
std::optional<uint8_t> distance_ratio_raw;
|
|
std::optional<uint16_t> scan_frequency_raw;
|
|
std::optional<uint16_t> input_status;
|
|
std::optional<uint16_t> output_status;
|
|
std::optional<uint32_t> field_status;
|
|
std::optional<uint32_t> status_flags;
|
|
|
|
// SICK TiM only — LMDscandata status pair (word0<<8)|word1:
|
|
// 0 ok, 1 error, 2 pollution warning, 4 pollution error.
|
|
std::optional<uint16_t> sick_device_status;
|
|
|
|
// SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
|
|
std::optional<uint8_t> nano_general_state;
|
|
};
|
|
|
|
struct ScanResult {
|
|
LaserScan scan;
|
|
ExtraInfo info;
|
|
};
|
|
|
|
// Per-model configuration. scan_angle_* use the signed system [-180,180]:
|
|
// 0 = ahead, + = left, - = right. range_min/max are datasheet placeholders.
|
|
struct ModelConfig {
|
|
const char* name;
|
|
float scan_angle_min; // deg
|
|
float scan_angle_max; // deg
|
|
float range_min_m = 0.05f;
|
|
float range_max_m = 30.f;
|
|
// Added to the raw device angle so output 0° = ahead (LR-1F/1FMI report 0°
|
|
// at the back: +180; SICK TiM puts the front at 90°: -90).
|
|
float angle_offset_deg = 0.f;
|
|
// Output remap window (see make_lidar / remap_scan_window): shifts the
|
|
// scan's angles onto [out_angle_min, out_angle_max] without dropping points.
|
|
bool remap_angles = false;
|
|
float out_angle_min = 0.f; // deg
|
|
float out_angle_max = 0.f; // deg
|
|
};
|
|
|
|
inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270°
|
|
inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
|
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear
|
|
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear
|
|
inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B)
|
|
inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line
|
|
inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
|
|
|
// Model unknown ahead of time: Family B/C packets carry enough to auto-detect;
|
|
// Family A doesn't, so the wide default FOV is kept.
|
|
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
|
|
|
|
using ScanCallback = std::function<void(const ScanResult&)>;
|
|
|
|
// Unified driver interface returned by make_lidar(); OLEI and SICK drivers
|
|
// both derive from it.
|
|
class Lidar {
|
|
public:
|
|
virtual ~Lidar() = default;
|
|
|
|
// ErrorCode::Ok on success. Calling open() on an already-open instance
|
|
// returns AlreadyOpen and leaves the connection untouched.
|
|
virtual ErrorCode open() = 0;
|
|
// Idempotent: safe to call before open() or more than once.
|
|
virtual void close() = 0;
|
|
// Block until one full scan; false on error/timeout (see last_error()).
|
|
// 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;
|
|
virtual void set_scan_callback(ScanCallback cb) = 0;
|
|
virtual bool spin_once() = 0;
|
|
virtual const char* detected_model() const = 0;
|
|
|
|
virtual bool is_open() const = 0;
|
|
// Status of the most recent open()/recv_scan()/spin_once() call.
|
|
ErrorCode last_error() const { return last_error_; }
|
|
|
|
// Device self-diagnostics from the newest fully decoded scan. valid stays
|
|
// false until one scan has been seen — and permanently on drivers whose
|
|
// wire format carries no diagnostic fields (SICK, for now). Updated by
|
|
// recv_scan()/spin_once(); call from the same thread that pumps them.
|
|
virtual Diagnostics get_diagnostics() const { return {}; }
|
|
|
|
protected:
|
|
ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
|
|
|
|
private:
|
|
ErrorCode last_error_ = ErrorCode::Ok;
|
|
};
|
|
|
|
// OLEI UDP driver.
|
|
class Driver : public Lidar {
|
|
public:
|
|
using ScanCallback = lidarlib::ScanCallback;
|
|
|
|
// ip: local bind address; port: UDP port the lidar sends to;
|
|
// inverted: unit mounted upside-down → mirror every angle.
|
|
explicit Driver(const ModelConfig& cfg,
|
|
const std::string& ip = "0.0.0.0",
|
|
uint16_t port = 2368,
|
|
bool inverted = false);
|
|
~Driver();
|
|
|
|
Driver(const Driver&) = delete;
|
|
Driver& operator=(const Driver&) = delete;
|
|
|
|
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; }
|
|
|
|
// Model name read from the Family B/C header; "AUTO" until one is seen.
|
|
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
|
|
|
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
|
|
|
private:
|
|
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)
|
|
|
|
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
|
|
void flush_scan();
|
|
|
|
ModelConfig cfg_;
|
|
std::string ip_;
|
|
uint16_t port_;
|
|
bool inverted_ = false;
|
|
int sock_fd_ = -1;
|
|
ScanCallback cb_;
|
|
|
|
// Per-revolution accumulation buffers (index-aligned)
|
|
std::vector<float> pending_angle_deg_;
|
|
std::vector<float> pending_dist_m_;
|
|
std::vector<uint8_t> pending_intensity_;
|
|
uint32_t pending_ts_ = 0;
|
|
uint8_t pending_err_ = 0;
|
|
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
|
|
|
ExtraInfo pending_info_;
|
|
|
|
// Snapshot for get_diagnostics(); refreshed by flush_scan().
|
|
Diagnostics latest_diag_;
|
|
|
|
ScanResult ready_result_;
|
|
bool scan_ready_ = false;
|
|
|
|
// Per-instance so two drivers on two threads don't race.
|
|
uint8_t recv_buf_[4096];
|
|
|
|
bool auto_detect_ = false;
|
|
bool model_locked_ = false;
|
|
std::string detected_model_name_ = "AUTO";
|
|
};
|
|
|
|
} // namespace lidarlib
|