DriverLIdar/include/lidarlib/sick_lidar.hpp

#pragma once
#include "lidarlib/lidar.hpp"
#include <string>

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

// ─── 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.
class SickDriver : public Lidar {
public:
    using ScanCallback = lidarlib::ScanCallback;  // same callback shape, ScanResult-compatible

    // ip/port: SICK device's TCP endpoint (SOPAS default port 2111).
    explicit SickDriver(const ModelConfig& cfg,
                         const std::string& ip,
                         uint16_t            port = 2111);
    ~SickDriver();

    SickDriver(const SickDriver&)            = delete;
    SickDriver& operator=(const SickDriver&) = delete;

    // 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.
    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 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_;
    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.
    std::string recv_buf_;
};

} // namespace lidarlib