119 lines
4.7 KiB
C++
119 lines
4.7 KiB
C++
#pragma once
|
|
#include "lidarlib/lidar.hpp"
|
|
#include <cstdint>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
namespace lidarlib {
|
|
|
|
// 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
|
|
|
|
// 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;
|
|
|
|
// inverted: unit mounted upside-down → mirror the scan.
|
|
explicit SickDriver(const ModelConfig& cfg,
|
|
const std::string& ip,
|
|
uint16_t port = 2111,
|
|
bool inverted = false);
|
|
~SickDriver();
|
|
|
|
SickDriver(const SickDriver&) = delete;
|
|
SickDriver& operator=(const SickDriver&) = delete;
|
|
|
|
// Connect + send "sEN LMDscandata 1" to start continuous scan output.
|
|
ErrorCode open() override;
|
|
void close() override;
|
|
bool recv_scan(ScanResult& out, int timeout_ms = 2000) 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 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_;
|
|
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
|
std::string ip_;
|
|
uint16_t port_;
|
|
bool inverted_ = false;
|
|
int sock_fd_ = -1;
|
|
ScanCallback cb_;
|
|
|
|
// Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
|
|
Diagnostics latest_diag_;
|
|
|
|
// 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;
|
|
// inverted: unit mounted upside-down → mirror the scan.
|
|
explicit NanoScanDriver(const ModelConfig& cfg,
|
|
const std::string& ip = "0.0.0.0",
|
|
uint16_t port = 6060,
|
|
bool inverted = false);
|
|
~NanoScanDriver();
|
|
|
|
NanoScanDriver(const NanoScanDriver&) = delete;
|
|
NanoScanDriver& operator=(const NanoScanDriver&) = 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; }
|
|
|
|
// 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:
|
|
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_;
|
|
bool inverted_ = false;
|
|
int sock_fd_ = -1;
|
|
ScanCallback cb_;
|
|
|
|
// Snapshot for get_diagnostics(); refreshed by parse_packet().
|
|
Diagnostics latest_diag_;
|
|
|
|
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
|
|
std::vector<uint8_t> recv_buf_;
|
|
};
|
|
|
|
} // namespace lidarlib
|