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:
@@ -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