Refactor lidar library: rename olei_config to lidar_config, add nanoscan example and shared byte helpers
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
@@ -6,27 +6,29 @@
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ─── Settings for one lidar ─────────────────────────────────────────────────
|
||||
// `name` is the unique key used to match entries across saves (rename = old
|
||||
// key removed, new key added) — useful if your GUI reconciles a running set of
|
||||
// lidars against an edited config list.
|
||||
// Settings for one lidar. `name` is the unique key across saves.
|
||||
struct LidarConfig {
|
||||
std::string name = "lidar";
|
||||
std::string ip = "0.0.0.0";
|
||||
uint16_t port = 2368;
|
||||
std::string model = "AUTO"; // must match an entry in model_names_for_brand(brand)
|
||||
bool inverted = false; // only applies to brand "OLEI" — SickDriver has no equivalent
|
||||
std::string brand = "OLEI"; // must match an entry in brand_names() — "OLEI" or "SICK"
|
||||
std::string model = "AUTO";
|
||||
bool inverted = false; // OLEI only
|
||||
std::string brand = "OLEI"; // "OLEI" or "SICK"
|
||||
|
||||
// Output angle window (deg): scan angles are remapped onto
|
||||
// [angle_min_deg, angle_max_deg] without dropping points.
|
||||
// Defaults (±360) = off.
|
||||
float angle_min_deg = -360.f;
|
||||
float angle_max_deg = 360.f;
|
||||
|
||||
friend bool operator==(const LidarConfig& a, const LidarConfig& b) {
|
||||
return a.name == b.name && a.ip == b.ip && a.port == b.port &&
|
||||
a.model == b.model && a.inverted == b.inverted && a.brand == b.brand;
|
||||
a.model == b.model && a.inverted == b.inverted && a.brand == b.brand &&
|
||||
a.angle_min_deg == b.angle_min_deg && a.angle_max_deg == b.angle_max_deg;
|
||||
}
|
||||
friend bool operator!=(const LidarConfig& a, const LidarConfig& b) { return !(a == b); }
|
||||
};
|
||||
|
||||
// Any number of lidars — managed as a list so a consuming app/GUI can
|
||||
// add/remove entries freely instead of being locked to a fixed front/rear pair.
|
||||
struct Config {
|
||||
std::vector<LidarConfig> lidars = {
|
||||
{"front", "0.0.0.0", 2368, "AUTO", false},
|
||||
@@ -34,47 +36,25 @@ struct Config {
|
||||
};
|
||||
};
|
||||
|
||||
// Known model name -> ModelConfig (matches the constants in lidar.hpp/sick_lidar.hpp).
|
||||
// Returns nullptr if name doesn't match any entry.
|
||||
// nullptr if `name` doesn't match any known model.
|
||||
const ModelConfig* model_by_name(const std::string& name);
|
||||
|
||||
// Names accepted by model_by_name(), for populating a UI dropdown.
|
||||
const std::vector<std::string>& model_names();
|
||||
|
||||
// Brand names accepted in LidarConfig::brand ("OLEI", "SICK"), for populating
|
||||
// a UI dropdown.
|
||||
const std::vector<std::string>& brand_names();
|
||||
|
||||
// Subset of model_names() valid for a given brand (e.g. "SICK" -> the
|
||||
// MODEL_SICK_* names) — empty if `brand` doesn't match any entry in
|
||||
// brand_names(). Used to filter the model dropdown once a brand is picked,
|
||||
// and to validate that LidarConfig::model actually belongs to its brand.
|
||||
// Subset of model_names() valid for `brand`; empty if unknown.
|
||||
const std::vector<std::string>& model_names_for_brand(const std::string& brand);
|
||||
|
||||
// Load config.json at `path`. If the file doesn't exist, returns defaults
|
||||
// (and does NOT create the file — caller decides whether to save it).
|
||||
// Returns defaults if the file doesn't exist (without creating it).
|
||||
Config load_config(const std::string& path);
|
||||
|
||||
// Overwrite `path` with `cfg` serialized as JSON.
|
||||
void save_config(const std::string& path, const Config& cfg);
|
||||
|
||||
// ─── THE single config function ─────────────────────────────────────────────
|
||||
// Build a ready-to-open lidar from one LidarConfig. This is the one entry point
|
||||
// a GUI/app needs: `brand` selects the transport — exactly "SICK" → TCP, any
|
||||
// other value (incl. "OLEI", empty, or an old config without the field) → OLEI
|
||||
// UDP. `model` is resolved *for that brand*: a name that is unknown OR belongs
|
||||
// to the other brand falls back to the brand's sensible default (MODEL_AUTO for
|
||||
// OLEI, MODEL_SICK_TIM571 for SICK), so a mis-paired brand+model can't silently
|
||||
// configure the wrong driver. `inverted` applies to OLEI only. Returns a unique
|
||||
// handle to the unified Lidar interface — call ->open() then
|
||||
// ->recv_scan(out, timeout_ms) to get ScanResult { LaserScan scan; ExtraInfo
|
||||
// info; }. Never returns nullptr.
|
||||
//
|
||||
// lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
|
||||
// auto lidar = lidarlib::make_lidar(c);
|
||||
// lidar->open();
|
||||
// lidarlib::ScanResult r;
|
||||
// lidar->recv_scan(r, 1000); // r.scan = LaserScan, r.info = ExtraInfo
|
||||
// Build a ready-to-open lidar from one LidarConfig — the only entry point an
|
||||
// app needs. brand "SICK" → SICK driver (model "SICK-nanoScan3" → UDP
|
||||
// NanoScanDriver, others → TCP SickDriver); anything else → OLEI UDP.
|
||||
// Unknown/cross-brand model falls back to the brand default. Never nullptr.
|
||||
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg);
|
||||
|
||||
} // namespace lidarlib
|
||||
|
||||
@@ -7,45 +7,31 @@
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ─── Default output: ROS sensor_msgs/LaserScan-shaped ──────────────────────
|
||||
// Same field names/semantics as ROS's LaserScan message (radians, meters,
|
||||
// seconds) so this can be bridged into a ROS node with a near-1:1 field copy.
|
||||
// ranges[i]/intensities[i] correspond to angle = angle_min + i*angle_increment;
|
||||
// the array spans exactly one revolution (or the model's FOV window) in the
|
||||
// order the device actually swept it — angle_min/angle_max are NOT clamped to
|
||||
// [-pi,pi], they just describe whatever contiguous window this revolution
|
||||
// covered (matches how continuously-rotating lidars without a phase reset
|
||||
// behave: the starting angle drifts slightly scan to scan).
|
||||
// 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 since power-on); 0 if the
|
||||
// family doesn't expose one (see ExtraInfo)
|
||||
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 — device doesn't expose per-point timing, always 0
|
||||
float scan_time = 0.f; // sec — device doesn't expose per-scan timing, always 0
|
||||
float range_min = 0.f; // m — from ModelConfig, NOT measured per-scan
|
||||
float range_max = 0.f; // m — from ModelConfig, NOT measured per-scan
|
||||
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 read back as float, like ROS does
|
||||
std::vector<float> intensities; // 0-255 as float
|
||||
};
|
||||
|
||||
// ─── Extra info: whatever diagnostic/header fields THIS family/model exposes ─
|
||||
// Fields the protocol family doesn't carry stay unset (std::nullopt). Several
|
||||
// of these are raw, undecoded passthroughs of header bytes whose exact
|
||||
// meaning hasn't been verified against real hardware/datasheet — see comments
|
||||
// in olei_lidar.cpp next to where each is read.
|
||||
// Diagnostic/header fields; fields the family doesn't carry stay std::nullopt.
|
||||
struct ExtraInfo {
|
||||
std::string detected_model = "AUTO"; // real model name read from the packet, or "AUTO"
|
||||
uint8_t error_status = 0; // Family A only; BIT0=Monitor, BIT1=Voltage, BIT2=Temp
|
||||
uint8_t distance_scale_mm = 0; // mm/count used to decode ranges this scan (0 = not reported)
|
||||
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 (0xFAF0) only — raw 16-bit "rotation info" header field,
|
||||
// meaning not decoded/verified.
|
||||
// Family A only
|
||||
std::optional<uint16_t> rotation_raw;
|
||||
|
||||
// Family C / protocol V3 (0xFEAC, GS1-5) only — ported from the C# driver
|
||||
// header layout, NOT cross-checked against real GS1-5 hardware.
|
||||
// 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;
|
||||
@@ -54,144 +40,90 @@ struct ExtraInfo {
|
||||
std::optional<uint32_t> status_flags;
|
||||
};
|
||||
|
||||
// One complete revolution, in both forms at once.
|
||||
struct ScanResult {
|
||||
LaserScan scan;
|
||||
ExtraInfo info;
|
||||
};
|
||||
|
||||
// ─── Per-model configuration ───────────────────────────────────────────────
|
||||
// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right.
|
||||
// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it.
|
||||
// range_min_m/range_max_m are sensor-spec placeholders (NOT read from any
|
||||
// packet) used to fill LaserScan::range_min/range_max — adjust to the real
|
||||
// datasheet values for each model if precision matters to your consumer.
|
||||
// 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 — VB/LR-16F: -135, 360° models: -180
|
||||
float scan_angle_max; // deg — VB/LR-16F: 135, 360° models: 180
|
||||
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
|
||||
};
|
||||
|
||||
// Table of known models — the driver auto-detects the packet family (A=0xFAF0 /
|
||||
// B=0xFEF0 / C=0xFEAC) per packet, so this config mainly decides the angular
|
||||
// window (FOV) that gets kept.
|
||||
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 }; // 2D 360° 50m
|
||||
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°, 0.01°/LSB ~2400 pts/rev (Family B)
|
||||
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°
|
||||
|
||||
// Sentinel: model unknown ahead of time. Family B (0xFEF0) carries an ASCII
|
||||
// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP
|
||||
// sniff) → the driver auto-detects it and narrows the FOV per the table
|
||||
// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has
|
||||
// no such string, so on a Family-A device MODEL_AUTO keeps the wide default
|
||||
// FOV (-180..180, no points dropped) until the user specifies a concrete model.
|
||||
// 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 };
|
||||
|
||||
// callback invoked whenever a complete scan is ready — shared by every driver
|
||||
// (lidarlib::Driver, lidarlib::SickDriver) and the unified Lidar interface below.
|
||||
using ScanCallback = std::function<void(const ScanResult&)>;
|
||||
|
||||
// ─── Unified driver interface ───────────────────────────────────────────────
|
||||
// Common handle returned by lidarlib::make_lidar() (the single config function in
|
||||
// config.hpp). Both the OLEI Driver (UDP) and the SICK SickDriver (TCP) derive
|
||||
// from this, so a GUI/app can drive any supported lidar through one type and
|
||||
// never branch on brand. Every call yields the same ScanResult { LaserScan
|
||||
// scan; ExtraInfo info; } — output #1 (ROS-shaped LaserScan, identical across
|
||||
// all models) and output #2 (ExtraInfo, model-specific extra fields).
|
||||
// Unified driver interface returned by make_lidar(); OLEI and SICK drivers
|
||||
// both derive from it.
|
||||
class Lidar {
|
||||
public:
|
||||
virtual ~Lidar() = default;
|
||||
|
||||
// Open the transport (UDP socket / TCP connection) and start receiving.
|
||||
virtual bool open() = 0;
|
||||
// Close the transport.
|
||||
virtual void close() = 0;
|
||||
// Block until one full scan is received; false on error/timeout.
|
||||
// timeout_ms = 0 → block indefinitely. (No default here on purpose: the
|
||||
// concrete drivers differ — OLEI 1000 ms, SICK 2000 ms — so callers using
|
||||
// the interface must state the timeout they want.)
|
||||
// Block until one full scan; false on error/timeout. 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;
|
||||
// Or set a callback and drive it from your own loop via spin_once().
|
||||
virtual void set_scan_callback(ScanCallback cb) = 0;
|
||||
// Receive + dispatch the callback once (non-owning loop step).
|
||||
virtual bool spin_once() = 0;
|
||||
// Real model name read from the packet, or the configured name if the
|
||||
// family carries none. See Driver::detected_model() for OLEI specifics.
|
||||
virtual const char* detected_model() const = 0;
|
||||
};
|
||||
|
||||
// ─── Driver ─────────────────────────────────────────────────────────────────
|
||||
// OLEI UDP driver.
|
||||
class Driver : public Lidar {
|
||||
public:
|
||||
// callback invoked whenever a complete scan is ready
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip : receiving host's bind address, usually "0.0.0.0"
|
||||
// port : UDP port the lidar sends to (default 2368)
|
||||
// cfg : model config
|
||||
// inverted : set true if this physical unit is mounted upside-down
|
||||
// (flipped 180° about its forward-facing axis). Mirrors every
|
||||
// point's angle (angle = -angle) so output stays in the
|
||||
// vehicle's frame regardless of mounting orientation — useful
|
||||
// when e.g. front is mounted normally but rear is flipped.
|
||||
// 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();
|
||||
|
||||
// Non-copyable
|
||||
Driver(const Driver&) = delete;
|
||||
Driver& operator=(const Driver&) = delete;
|
||||
|
||||
// Open the socket and start receiving
|
||||
bool open() override;
|
||||
|
||||
// Close the socket
|
||||
void close() override;
|
||||
|
||||
// Blocks until a full revolution has been received; returns false on error/timeout
|
||||
// timeout_ms = 0 → block indefinitely
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
|
||||
// Or use the callback (drive it from your own non-blocking loop)
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
|
||||
// Receive + dispatch callback (call from your own loop)
|
||||
bool spin_once() override;
|
||||
|
||||
// The REAL model name read from the Family B/C header (only meaningful
|
||||
// when the Driver was constructed with MODEL_AUTO). Always the actual
|
||||
// string found in the packet (e.g. "OLELR-1BS2"), even when that model
|
||||
// has no specific FOV entry in the table (FOV then stays at the 360°
|
||||
// default). Returns "AUTO" if no Family B/C packet has been seen yet.
|
||||
// Mirrored per-scan in ScanResult::info::detected_model.
|
||||
// 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(); }
|
||||
|
||||
private:
|
||||
// ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ──
|
||||
bool parse_family_a(const uint8_t* buf, int len);
|
||||
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)
|
||||
|
||||
// ── parse Family B packet (ID=0xFEF0): 40B header, 8B block ──
|
||||
bool parse_family_b(const uint8_t* buf, int len);
|
||||
|
||||
// ── parse Family C / protocol V3 packet (Magic=0xFEAC, GS1-5): 48B header ──
|
||||
bool parse_family_c(const uint8_t* buf, int len);
|
||||
|
||||
// Appends one point's angle (already signed+inverted+FOV-filtered by the
|
||||
// caller), unwrapping it against the previous point in this revolution so
|
||||
// the accumulated sequence stays continuous across the ±180° seam instead
|
||||
// of jumping — required for LaserScan::angle_min/angle_max/ranges to stay
|
||||
// monotonic for 360° devices.
|
||||
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
|
||||
|
||||
// Once a full revolution is ready → flush into ready_result_ and fire the callback
|
||||
void flush_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
@@ -201,27 +133,22 @@ private:
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-revolution accumulation buffers (parallel arrays, index-aligned)
|
||||
std::vector<float> pending_angle_deg_; // unwrapped, continuous
|
||||
// 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-around (revolution-boundary) detection, device space [0,360)
|
||||
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
||||
|
||||
// Per-revolution ExtraInfo accumulation — overwritten as packets for the
|
||||
// in-progress revolution are parsed, then copied into ready_result_ on flush.
|
||||
ExtraInfo pending_info_;
|
||||
|
||||
// recv_scan()'s output, gated by a simple ready flag
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
|
||||
// Per-instance receive buffer — NOT static, so that 2 lidars running on 2
|
||||
// threads don't overwrite each other's data (data race).
|
||||
// Per-instance so two drivers on two threads don't race.
|
||||
uint8_t recv_buf_[4096];
|
||||
|
||||
// Model auto-detection from the Family B/C header (see MODEL_AUTO)
|
||||
bool auto_detect_ = false;
|
||||
bool model_locked_ = false;
|
||||
std::string detected_model_name_ = "AUTO";
|
||||
|
||||
@@ -1,13 +1,5 @@
|
||||
#pragma once
|
||||
// ─── One-include convenience header ─────────────────────────────────────────
|
||||
// Pull in the whole public API in a single line:
|
||||
//
|
||||
// #include "lidarlib/lidarlib.hpp"
|
||||
//
|
||||
// Gives you the data model (LaserScan / ExtraInfo / ScanResult), the unified
|
||||
// Lidar interface, both concrete drivers (Driver = OLEI/UDP, SickDriver =
|
||||
// SICK/TCP), the model/brand tables, config.json load/save, and the single
|
||||
// config function lidarlib::make_lidar().
|
||||
// One-include convenience header for the whole public API.
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include "lidarlib/config.hpp"
|
||||
|
||||
@@ -1,45 +1,25 @@
|
||||
#pragma once
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
// ─── SICK TiM5xx/7xx model presets ──────────────────────────────────────────
|
||||
// FOV/range taken from SICK's public datasheets (NOT read from any packet —
|
||||
// same placeholder convention as the OLEI ModelConfig constants in lidar.hpp).
|
||||
// Scanning angle is 270° on every TiM5xx/7xx variant; only the rated range
|
||||
// differs by model.
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f }; // TiM551/561, 270°, 10m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f }; // TiM571, 270°, 25m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f }; // TiM781, 270°, 25m
|
||||
// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
|
||||
// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
|
||||
// frame puts 90° at the device front.
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571, 270°, 25m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781, 270°, 25m
|
||||
|
||||
// ─── SickDriver — SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111) ─
|
||||
//
|
||||
// VERIFIED against a real SICK TiM781S (FW V5.11-14.10.24, MODEL_SICK_TIM7XX)
|
||||
// on port 2111: 811 pts/scan over -45..225° (270° FOV), angle_increment =
|
||||
// 0.333° (matches the 781's rated angular resolution), stable ranges, and a
|
||||
// non-zero RSSI1 channel — confirms the DIST1/RSSI1 channel layout, the
|
||||
// IEEE-754 scaling factor, and the start-angle/step-width decode in
|
||||
// parse_lmdscandata() (sick_lidar.cpp) are correct on real hardware, not just
|
||||
// per SICK's "Telegram Listing" doc.
|
||||
// NOT yet verified: the angle-zero reference vs physical mounting direction,
|
||||
// devices reporting NumEncoders > 0, the 8-bit-channel branch (the test unit
|
||||
// only emitted 16-bit channels), and MODEL_SICK_TIM5XX/MODEL_SICK_TIM571's
|
||||
// FOV/range numbers (only TiM7xx was tested).
|
||||
//
|
||||
// Protocol differences from lidarlib::Driver that justify a separate class
|
||||
// instead of extending Driver:
|
||||
// - Transport is TCP (a connection, request/response + streamed telegrams),
|
||||
// not connectionless UDP broadcast.
|
||||
// - Telegrams are ASCII (CoLa-A), framed by STX(0x02)/ETX(0x03) instead of
|
||||
// the OLEI binary header+block layout — no CRC32 like Family A.
|
||||
// - The device is passive until told to start: must send "sEN LMDscandata 1"
|
||||
// before any scan telegram arrives.
|
||||
// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
|
||||
// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
|
||||
// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
|
||||
class SickDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback; // same callback shape, ScanResult-compatible
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip/port: SICK device's TCP endpoint (SOPAS default port 2111).
|
||||
explicit SickDriver(const ModelConfig& cfg,
|
||||
const std::string& ip,
|
||||
uint16_t port = 2111);
|
||||
@@ -50,30 +30,17 @@ public:
|
||||
|
||||
// Connect + send "sEN LMDscandata 1" to start continuous scan output.
|
||||
bool open() override;
|
||||
|
||||
// Best-effort "sEN LMDscandata 0" then close the socket.
|
||||
void close() override;
|
||||
|
||||
// Blocks until one LMDscandata telegram has been parsed; returns false on
|
||||
// error/timeout. timeout_ms = 0 → block indefinitely.
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
|
||||
|
||||
// Or use the callback (drive it from your own non-blocking loop)
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
|
||||
// Receive + parse + dispatch callback (call from your own loop)
|
||||
bool spin_once() override;
|
||||
|
||||
// SICK telegrams carry no model string — returns the configured model name
|
||||
// (e.g. "SICK-TIM7xx") so the unified Lidar interface stays consistent.
|
||||
// Backed by an owned std::string (not cfg_.name, a borrowed const char*) so
|
||||
// the pointer stays valid even if the ModelConfig was built from temporary
|
||||
// storage — matching Driver::detected_model()'s ownership.
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
private:
|
||||
bool send_telegram(const std::string& body); // wraps body with STX/ETX, writes to socket
|
||||
bool read_telegram(std::string& out, int timeout_ms); // returns next STX..ETX frame, STX/ETX stripped
|
||||
bool send_telegram(const std::string& body);
|
||||
bool read_telegram(std::string& out, int timeout_ms);
|
||||
bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
@@ -83,9 +50,51 @@ private:
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Accumulates bytes read from the TCP stream between telegram boundaries —
|
||||
// per-instance (not static) so 2 SickDrivers on 2 threads don't race.
|
||||
// Leftover TCP bytes carried across telegram boundaries; per-instance.
|
||||
std::string recv_buf_;
|
||||
};
|
||||
|
||||
inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
|
||||
|
||||
// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
|
||||
// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
|
||||
// Passive UDP receiver: the sensor's UDP target must be configured up front in
|
||||
// SICK Safety Designer — this class does no CoLa2/TCP handshake.
|
||||
class NanoScanDriver : public Lidar {
|
||||
public:
|
||||
using ScanCallback = lidarlib::ScanCallback;
|
||||
|
||||
// ip: local bind address; port: local UDP port the sensor sends to.
|
||||
explicit NanoScanDriver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 6060);
|
||||
~NanoScanDriver();
|
||||
|
||||
NanoScanDriver(const NanoScanDriver&) = delete;
|
||||
NanoScanDriver& operator=(const NanoScanDriver&) = delete;
|
||||
|
||||
bool 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;
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
private:
|
||||
int recv_datagram(int timeout_ms);
|
||||
bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
|
||||
std::vector<uint8_t> recv_buf_;
|
||||
};
|
||||
|
||||
} // namespace lidarlib
|
||||
|
||||
Reference in New Issue
Block a user