QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Refactor lidar library: rename olei_config to lidar_config, add nanoscan example and shared byte helpers

Browse Source 
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
QUYVN
2026-07-03 09:27:43 +07:00
parent 59880871b0
commit 323715eab0
16 changed files with 621 additions and 814 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/json_mini.hpp
View File

@@ -1,6 +1,4 @@
// json_mini.hpp — minimal header-only JSON parse/serialize, just enough for
// flat-ish config objects (no comments, no streaming, no error recovery).
// Not a general-purpose JSON library — kept tiny on purpose.
// Minimal header-only JSON parse/serialize, just enough for config objects.
#pragma once
#include <string>
#include <vector>
@@ -104,7 +102,6 @@ private:
}
};
// ── Parser ──────────────────────────────────────────────────────────────
class ParseError : public std::runtime_error {
public:
explicit ParseError(const std::string& what) : std::runtime_error(what) {}

42
src/lidar_bytes.hpp Normal file
View File

@@ -0,0 +1,42 @@
// Internal helpers shared by the driver TUs — not part of the public API.
#pragma once
#include "lidarlib/lidar.hpp"
#include <cstdint>
#include <cstring>
namespace lidarlib {
inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
// angle_min/angle_max/angle_increment are rewritten; points are untouched.
inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
const float new_min = min_deg * kDeg2Rad;
const float new_max = max_deg * kDeg2Rad;
const float old_span = scan.angle_max - scan.angle_min;
if (old_span > 0.f)
scan.angle_increment *= (new_max - new_min) / old_span;
scan.angle_min = new_min;
scan.angle_max = new_max;
}
// Little-endian readers (bounds are the caller's responsibility).
inline uint8_t le_u8 (const uint8_t* p) { return p[0]; }
inline uint16_t le16(const uint8_t* p) {
return static_cast<uint16_t>(p[0] | (p[1] << 8));
}
inline uint32_t le32(const uint8_t* p) {
return static_cast<uint32_t>(p[0])
| (static_cast<uint32_t>(p[1]) << 8)
| (static_cast<uint32_t>(p[2]) << 16)
| (static_cast<uint32_t>(p[3]) << 24);
}
inline int32_t le_i32(const uint8_t* p) { return static_cast<int32_t>(le32(p)); }
inline float bits_to_float(uint32_t bits) {
float f;
std::memcpy(&f, &bits, sizeof(f));
return f;
}
} // namespace lidarlib

43
src/olei_config.cpp → src/lidar_config.cpp
View File

@@ -22,9 +22,10 @@ constexpr ModelEntry kModels[] = {
{ "LR-1BS5", &MODEL_LR1BS5, "OLEI" },
{ "LR-16F", &MODEL_LR16F, "OLEI" },
{ "GS1-5", &MODEL_GS15, "OLEI" },
{ "SICK-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" },
{ "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" },
{ "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" },
{ "SICK-TIM5xx", &MODEL_SICK_TIM5XX, "SICK" },
{ "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" },
{ "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" },
{ "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" },
};
json::Value to_json(const LidarConfig& c) {
@@ -35,6 +36,8 @@ json::Value to_json(const LidarConfig& c) {
v.set("brand", json::Value::make_string(c.brand));
v.set("model", json::Value::make_string(c.model));
v.set("inverted", json::Value::make_bool(c.inverted));
v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg));
v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg));
return v;
}
@@ -46,6 +49,8 @@ LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) {
c.brand = v.get_string("brand", def.brand);
c.model = v.get_string("model", def.model);
c.inverted = v.get_bool("inverted", def.inverted);
c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", def.angle_min_deg));
c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", def.angle_max_deg));
return c;
}
@@ -58,10 +63,6 @@ const ModelConfig* model_by_name(const std::string& name) {
}
namespace {
// Like model_by_name() but only accepts a model that actually belongs to
// `brand` — so a mis-paired brand+model (e.g. brand="OLEI", model="SICK-TIM571")
// doesn't resolve to the other brand's preset. Returns nullptr if the name
// isn't a valid model for that brand.
const ModelConfig* model_by_name_for_brand(const std::string& name, const std::string& brand) {
for (const auto& e : kModels)
if (name == e.name && brand == e.brand) return e.cfg;
@@ -101,7 +102,7 @@ const std::vector<std::string>& model_names_for_brand(const std::string& brand)
}
Config load_config(const std::string& path) {
Config cfg; // defaults
Config cfg;
std::ifstream f(path);
if (!f) return cfg;
@@ -111,7 +112,7 @@ Config load_config(const std::string& path) {
try {
root = json::parse(ss.str());
} catch (const json::ParseError&) {
return cfg; // malformed file -> fall back to defaults rather than crash
return cfg; // malformed file -> defaults
}
const json::Value* lidars = root.find("lidars");
@@ -136,21 +137,25 @@ void save_config(const std::string& path, const Config& cfg) {
}
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
// Anything other than the exact string "SICK" is treated as OLEI (this also
// keeps old config.json files without a `brand` field working).
// Anything but the exact string "SICK" is OLEI (keeps brand-less configs working).
const bool is_sick = (cfg.brand == "SICK");
// Resolve the model *for this brand*: an unknown/empty name, OR a name that
// belongs to the other brand, falls back to the brand default (OLEI
// auto-detects from the packet; SICK has no model string in the wire
// protocol so we pick a mid-range preset). This prevents a mis-paired
// brand+model from silently configuring the wrong driver/FOV.
const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI");
if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO;
if (is_sick)
return std::make_unique<SickDriver>(*model, cfg.ip, cfg.port);
return std::make_unique<Driver>(*model, cfg.ip, cfg.port, cfg.inverted);
ModelConfig mc = *model;
if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
mc.remap_angles = true;
mc.out_angle_min = cfg.angle_min_deg;
mc.out_angle_max = cfg.angle_max_deg;
}
if (is_sick) {
if (model == &MODEL_SICK_NANOSCAN3)
return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port);
return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port);
}
return std::make_unique<Driver>(mc, cfg.ip, cfg.port, cfg.inverted);
}
} // namespace lidarlib

205
src/olei_lidar.cpp
View File

@@ -1,4 +1,5 @@
#include "lidarlib/lidar.hpp"
#include "lidar_bytes.hpp"
#include <cstring>
#include <cmath>
@@ -10,40 +11,19 @@
namespace lidarlib {
namespace {
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
}
// ── Little-endian helpers ────────────────────────────────────────────────────
static inline uint16_t le16(const uint8_t* p) {
return static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8);
}
static inline uint32_t le32(const uint8_t* p) {
return static_cast<uint32_t>(p[0])
| (static_cast<uint32_t>(p[1]) << 8)
| (static_cast<uint32_t>(p[2]) << 16)
| (static_cast<uint32_t>(p[3]) << 24);
}
// Normalize any angle into the SIGNED system (-180, 180]: 0 = straight ahead,
// + = left, - = right. This lets a model's FOV (e.g. VB -135…135) correctly
// filter lidars that report angles in 0–360 too.
// Normalize into (-180, 180]: 0 = ahead, + = left, - = right.
static inline float to_signed_deg(float deg) {
deg = std::fmod(deg, 360.f);
if (deg < 0.f) deg += 360.f; // → [0,360)
if (deg > 180.f) deg -= 360.f; // → (-180,180]
if (deg < 0.f) deg += 360.f;
if (deg > 180.f) deg -= 360.f;
return deg;
}
// Mirror the angle when the unit is mounted upside-down (flipped 180° about
// its forward axis), so output angle stays correct relative to the vehicle
// frame regardless of physical mounting. Must run AFTER to_signed_deg() and
// BEFORE the FOV filter, since the FOV window is defined in vehicle frame.
static inline float maybe_invert(float signed_deg, bool inverted) {
return inverted ? to_signed_deg(-signed_deg) : signed_deg;
}
// ── CRC32 (poly 0x04C11DB7, MSB-first) ──────────────────────────────────────
// CRC32 poly 0x04C11DB7, MSB-first
static uint32_t crc32_olei(const uint8_t* data, size_t len) {
uint32_t crc = 0xFFFFFFFF;
for (size_t i = 0; i < len; ++i) {
@@ -54,12 +34,10 @@ static uint32_t crc32_olei(const uint8_t* data, size_t len) {
return crc;
}
// ── Frame IDs ────────────────────────────────────────────────────────────────
static constexpr uint16_t FRAME_ID_A = 0xFAF0; // 2D Ethernet (VB, VF, LR-1F)
static constexpr uint16_t FRAME_ID_B = 0xFEF0; // LR-1BS5 / LR-1BS2 Ethernet variant
static constexpr uint16_t FRAME_ID_C = 0xFEAC; // Protocol V3 (GS1-5)
// ─── Constructor / Destructor ────────────────────────────────────────────────
Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
: cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
{
@@ -68,14 +46,10 @@ Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, boo
Driver::~Driver() { close(); }
// ─── open() ─────────────────────────────────────────────────────────────────
bool Driver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return false;
// Allow multiple sockets to bind the same port (run alongside another
// app / debugging). SO_REUSEPORT lets several listeners receive the same
// UDP stream — only works if EVERY socket on that port sets this flag.
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
#ifdef SO_REUSEPORT
@@ -98,7 +72,6 @@ bool Driver::open() {
return true;
}
// ─── close() ────────────────────────────────────────────────────────────────
void Driver::close() {
if (sock_fd_ >= 0) {
::close(sock_fd_);
@@ -106,7 +79,6 @@ void Driver::close() {
}
}
// ─── recv_scan() — blocks until one full revolution is available ──────────
bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
scan_ready_ = false;
@@ -115,7 +87,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return false; // timeout or error
if (r <= 0) return false;
}
if (!spin_once()) return false;
}
@@ -123,10 +95,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
return true;
}
// ─── spin_once() ────────────────────────────────────────────────────────────
bool Driver::spin_once() {
// buf is the recv_buf_ member, NOT static → each Driver has its own
// memory, safe when 2 lidars receive concurrently on 2 threads.
uint8_t* buf = recv_buf_;
sockaddr_in from{};
socklen_t fromlen = sizeof(from);
@@ -135,28 +104,19 @@ bool Driver::spin_once() {
reinterpret_cast<sockaddr*>(&from), &fromlen);
if (n < 0) return false;
// Distinguish protocol family by Frame ID (little-endian)
// Family A / C: Frame ID / magic sits right at bytes [0-1]
// Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3]
if (n < 4) return true; // too short, skip
uint16_t id_at_0 = le16(buf); // Family A (0xFAF0) or Family C (0xFEAC)
uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3]
// A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
if (n < 4) return true;
uint16_t id_at_0 = le16(buf);
uint16_t frame_id_b = le16(buf + 2);
if (id_at_0 == FRAME_ID_A) parse_family_a(buf, static_cast<int>(n));
else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast<int>(n));
else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast<int>(n));
// else: unknown family (3D LR-16F uses a different format, extend later)
return true;
}
// ─── push_point() — append with angle-unwrapping ───────────────────────────
// `signed_angle_deg` is already signed+inverted+FOV-filtered by the caller.
// Unwrapping against the previous point (rather than re-deriving from device
// raw angle) keeps this identical for all 3 families and survives the ±180°
// seam: a 360° device's points cross from +179.x to -179.x mid-revolution in
// the signed system, which push_point() turns back into a continuous ramp so
// LaserScan::angle_min/angle_max/ranges stay meaningful (monotonic, ROS-style).
// Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
float angle = signed_angle_deg;
if (!pending_angle_deg_.empty()) {
@@ -169,7 +129,6 @@ void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity)
pending_intensity_.push_back(intensity);
}
// ─── flush_scan() — a revolution is complete ───────────────────────────────
void Driver::flush_scan() {
if (pending_angle_deg_.empty()) return;
@@ -181,13 +140,16 @@ void Driver::flush_scan() {
scan.angle_max = pending_angle_deg_.back() * kDeg2Rad;
scan.angle_increment = (n > 1)
? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f;
scan.time_increment = 0.f; // device doesn't expose per-point timing
scan.scan_time = 0.f; // device doesn't expose per-scan timing
scan.time_increment = 0.f;
scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end());
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
ExtraInfo& info = ready_result_.info;
info = pending_info_;
info.detected_model = detected_model_name_;
@@ -196,63 +158,42 @@ void Driver::flush_scan() {
pending_angle_deg_.clear();
pending_dist_m_.clear();
pending_intensity_.clear();
pending_info_ = ExtraInfo{}; // reset per-revolution optional fields
pending_info_ = ExtraInfo{};
scan_ready_ = true;
if (cb_) cb_(ready_result_);
}
// ─── parse_family_a() ───────────────────────────────────────────────────────
// 20-byte header:
// [0-1] Frame ID = 0xFAF0
// [2-3] Protocol = 0x0200
// [4] Distance scale (mm/count)
// [5] Error status
// [6] Start angle (deg, uint8)
// [7] End angle (deg, uint8, exclusive)
// [8-9] Num points (uint16 LE)
// [10-11] Rotation info — raw, undecoded (exposed as ExtraInfo::rotation_raw)
// [12-15] Timestamp (uint32 LE, ms)
// [16-19] CRC32 of the block data
// 3-byte block × N:
// [0-1] Distance readout (uint16 LE)
// [2] Intensity (uint8)
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
bool Driver::parse_family_a(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 20;
static constexpr int BLOCK_LEN = 3;
if (len < HEADER_LEN) return false;
// ── read header ──
// uint16_t protocol = le16(buf + 2); // 0x0200
uint8_t dist_scale = buf[4]; // mm per count
uint8_t err_status = buf[5];
float ang_start = static_cast<float>(buf[6]);
// float ang_end = static_cast<float>(buf[7]); // exclusive
uint16_t num_pts = le16(buf + 8);
uint16_t rotation_raw = le16(buf + 10);
uint32_t timestamp = le32(buf + 12);
uint32_t crc_packet = le32(buf + 16);
// ── verify CRC (optional but recommended) ──
int block_bytes = len - HEADER_LEN;
if (block_bytes < num_pts * BLOCK_LEN) return false; // truncated packet
if (block_bytes < num_pts * BLOCK_LEN) return false;
uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<size_t>(num_pts * BLOCK_LEN));
if (crc_calc != crc_packet) return false; // CRC mismatch
if (crc_calc != crc_packet) return false;
// ── detect wrap-around → flush the previous revolution ──
if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
flush_scan();
}
// ── decode points ──
pending_ts_ = timestamp;
pending_err_ = err_status;
pending_info_.distance_scale_mm = dist_scale;
pending_info_.rotation_raw = rotation_raw;
// scale=0 means the firmware didn't report it → default to 1 mm/count to avoid dist=0.
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
const float ang_end = static_cast<float>(buf[7]);
@@ -261,36 +202,21 @@ bool Driver::parse_family_a(const uint8_t* buf, int len) {
uint16_t dist_raw = le16(blk);
uint8_t intensity = blk[2];
// Compute angle: linear interpolation within the packet's range (device-space)
float frac = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start));
float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start) + cfg_.angle_offset_deg);
angle = maybe_invert(angle, inverted_);
// Filter out anything outside the model's FOV (already in the signed -180…180 system)
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
push_point(angle, dist_raw * scale_mm * 0.001f /* mm → m */, intensity);
push_point(angle, dist_raw * scale_mm * 0.001f, intensity);
}
last_angle_ = ang_start;
return true;
}
// ─── parse_family_b() ───────────────────────────────────────────────────────
// 40-byte header:
// [0-1] 0x010F
// [2-3] 0xFEF0 (Frame ID)
// [4-5] 0x0200 (Protocol)
// [6] Distance scale
// [7-16] Model identifier string (e.g. "OLELR-1BS5")
// [17-39] Reserved
// 8-byte block × N:
// [0-1] AngleRaw (uint16 LE, × 0.01° → deg, 0–359.99); >= 0xFF00 = invalid point
// [2-3] Distance readout (uint16 LE); meters = value × DistanceScale / 1000
// [4-5] Signal strength (uint16 LE)
// [6-7] Reserved
// NOTE: this header carries no timestamp/error field, so ScanResult::scan's
// timestamp_ms and info.error_status stay at their defaults (0) for Family B.
// Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks
// (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire.
bool Driver::parse_family_b(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 40;
static constexpr int BLOCK_LEN = 8;
@@ -298,7 +224,6 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
if (len < HEADER_LEN) return false;
uint8_t dist_scale = buf[6];
// scale=0 → default to 1 mm/count so distances don't collapse to zero.
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
pending_info_.distance_scale_mm = dist_scale;
if (auto_detect_ && !model_locked_) {
@@ -320,10 +245,11 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
};
for (const auto& entry : kModelTable) {
if (raw.find(entry.key) != std::string::npos) {
cfg_.scan_angle_min = entry.cfg->scan_angle_min;
cfg_.scan_angle_max = entry.cfg->scan_angle_max;
cfg_.range_min_m = entry.cfg->range_min_m;
cfg_.range_max_m = entry.cfg->range_max_m;
cfg_.scan_angle_min = entry.cfg->scan_angle_min;
cfg_.scan_angle_max = entry.cfg->scan_angle_max;
cfg_.range_min_m = entry.cfg->range_min_m;
cfg_.range_max_m = entry.cfg->range_max_m;
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
break;
}
}
@@ -334,26 +260,21 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
if (num_pts <= 0) return false;
const uint8_t* blk = buf + HEADER_LEN;
// AngleRaw is 0.01°/LSB (0–359.99°), per the official Olei block spec —
// verified against real OLELR-1FMI geometry (a 0.25° scale smears a room
// into a circle). AngleRaw >= 0xFF00 marks an invalid point → skip it.
// The counter resets to 0 each revolution, but one packet is only a ~22°
// arc and the device can pack >1 revolution across packets, so the
// revolution boundary is detected PER POINT: a >90° drop between
// consecutive [0,360) angles ends the current revolution.
// A packet is only a ~22° arc and may span >1 rev, so the revolution
// boundary is detected per point: a >90° drop between consecutive angles.
static constexpr uint16_t INVALID_ANGLE = 0xFF00;
for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
uint16_t angle_raw = le16(blk);
if (angle_raw >= INVALID_ANGLE) continue; // invalid point
if (angle_raw >= INVALID_ANGLE) continue;
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f); // [0,360)
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f);
if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
flush_scan(); // revolution complete
flush_scan();
}
last_angle_ = dev_deg;
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f), inverted_); // -180…180
float dist_m = le16(blk + 2) * scale_mm * 0.001f; // readout × scale → m
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_);
float dist_m = le16(blk + 2) * scale_mm * 0.001f;
uint8_t intensity = static_cast<uint8_t>(le16(blk + 4) >> 2); // 10-bit → 8-bit
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
@@ -364,36 +285,9 @@ bool Driver::parse_family_b(const uint8_t* buf, int len) {
return true;
}
// ─── parse_family_c() ───────────────────────────────────────────────────────
// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C#
// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently
// sniffed/verified against real GS1-5 hardware (no device was available to
// test this while writing the code).
// 48-byte header:
// [0-1] Magic = 0xFEAC
// [2-3] Version
// [4-7] PacketSize (uint32 LE)
// [8-9] HeaderSize (uint16 LE, usually = 48)
// [10] Distance ratio — read by the original C# driver but NOT applied
// (distance is always raw mm / 1000); same behavior kept here.
// Exposed raw as ExtraInfo::distance_ratio_raw.
// [11] Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity),
// 0x10=4B/point (first 2 bytes unused, range at [+2,+4))
// [12-13] Scan number [14-15] Packet number
// [16-19] Timestamp decimal [20-23] Timestamp integer
// [24-25] Scan frequency raw [26-27] NumPointsScan (total points per revolution)
// [28-29] Input status [30-31] Output status
// [32-35] Field status
// [36-37] StartIndex [38-39] EndIndex
// [40-41] FirstIndex — index of this packet's first point within the full revolution
// [42-43] NumPointsPacket — number of points in this packet
// [44-47] Status flags
// All of [10], [24-25], [28-35], [44-47] are read and passed through raw in
// ExtraInfo — none of these are cross-verified against real hardware, same
// caveat as the rest of this family.
// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in
// the signed system (-180..180); no fmod needed like Family B since the
// index always stays within [0, NumPointsScan).
// Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending
// on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real
// hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180.
bool Driver::parse_family_c(const uint8_t* buf, int len) {
static constexpr int HEADER_LEN = 48;
if (len < HEADER_LEN) return false;
@@ -410,13 +304,15 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
uint16_t num_pts_packet = le16(buf + 42);
uint32_t status_flags = le32(buf + 44);
if (num_pts_scan == 0) return false; // avoid divide-by-zero
if (num_pts_scan == 0) return false;
int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
if (header_size < HEADER_LEN || header_size > len) return false;
// Types: 0x00 = 2B/point (range only), 0x01 = 4B (range+intensity),
// 0x10 = 4B (range at [+2,+4)).
int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
if (bytes_per_point == 0) return false; // unknown Types, layout unclear
if (bytes_per_point == 0) return false;
int payload_bytes = len - header_size;
int num_pts = num_pts_packet;
@@ -432,25 +328,18 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
pending_info_.field_status = field_status;
pending_info_.status_flags = status_flags;
// Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name
// string in the header like Family B, but recognizing this family is
// already enough to know the model, so auto-detect resolves immediately
// without reading any extra field.
// Magic 0xFEAC == exactly one model (GS1-5).
if (auto_detect_ && !model_locked_) {
cfg_.scan_angle_min = MODEL_GS15.scan_angle_min;
cfg_.scan_angle_max = MODEL_GS15.scan_angle_max;
cfg_.range_min_m = MODEL_GS15.range_min_m;
cfg_.range_max_m = MODEL_GS15.range_max_m;
cfg_.angle_offset_deg = MODEL_GS15.angle_offset_deg;
detected_model_name_ = MODEL_GS15.name;
model_locked_ = true;
}
const float angle_inc = 360.f / static_cast<float>(num_pts_scan);
// raw_angle is used for wrap-around detection: it does NOT have the -180
// offset that the externally-exposed angle gets, and stays in [0,360),
// monotonically increasing — matching the same convention used by
// Family A/B (last_angle_ >= 0 means "we already have a previous value");
// subtracting 180 here could go negative and break that sentinel check.
float raw_first_angle = static_cast<float>(first_index) * angle_inc;
if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) {
@@ -473,11 +362,11 @@ bool Driver::parse_family_c(const uint8_t* buf, int len) {
range_mm = le16(blk + 2);
}
float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f);
float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f + cfg_.angle_offset_deg);
angle = maybe_invert(angle, inverted_);
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
push_point(angle, range_mm * 0.001f /* mm → m */,
push_point(angle, range_mm * 0.001f,
has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
}

299
src/sick_lidar.cpp
View File

@@ -1,9 +1,12 @@
#include "lidarlib/sick_lidar.hpp"
#include "lidar_bytes.hpp"
#include <cctype>
#include <cerrno>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <limits>
#include <vector>
#include <fcntl.h>
#include <sys/socket.h>
@@ -16,7 +19,6 @@
namespace lidarlib {
namespace {
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
constexpr char kStx = 0x02;
constexpr char kEtx = 0x03;
constexpr int kConnectTimeoutMs = 2000;
@@ -25,14 +27,9 @@ uint32_t hex_to_u32(const std::string& tok) {
return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16));
}
int32_t hex_to_i32(const std::string& tok) {
// SICK encodes signed header fields as plain hex of the 2's-complement bits.
// SICK encodes signed fields as plain hex of the 2's-complement bits.
return static_cast<int32_t>(hex_to_u32(tok));
}
float bits_to_float(uint32_t bits) {
float f;
std::memcpy(&f, &bits, sizeof(f));
return f;
}
std::vector<std::string> tokenize(const std::string& s) {
std::vector<std::string> out;
@@ -45,6 +42,10 @@ std::vector<std::string> tokenize(const std::string& s) {
}
return out;
}
constexpr size_t kNanoRecvBufSize = 65536;
// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
constexpr double kNanoAngleResolution = 4194304.0;
} // namespace
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
@@ -52,7 +53,6 @@ SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t p
SickDriver::~SickDriver() { close(); }
// ─── open() — TCP connect + tell the device to start streaming ────────────
bool SickDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
if (sock_fd_ < 0) return false;
@@ -62,11 +62,8 @@ bool SickDriver::open() {
addr.sin_port = htons(port_);
addr.sin_addr.s_addr = inet_addr(ip_.c_str());
// Non-blocking connect with a bounded timeout: a SICK device that's
// powered off/unreachable leaves the SYN unanswered, and a plain blocking
// connect() would then stall this call — and whatever thread called it,
// e.g. a GUI's "connect" button handler — for the OS's default TCP retry
// timeout (~2 minutes on Linux).
// Non-blocking connect with a bounded timeout — a blocking connect() to an
// unreachable device would stall for the OS default (~2 min on Linux).
int flags = ::fcntl(sock_fd_, F_GETFL, 0);
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
@@ -80,10 +77,10 @@ bool SickDriver::open() {
::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
rc = (err == 0) ? 0 : -1;
} else {
rc = -1; // timeout, or select() itself failed
rc = -1;
}
}
::fcntl(sock_fd_, F_SETFL, flags); // restore blocking mode for send/recv below
::fcntl(sock_fd_, F_SETFL, flags);
if (rc < 0) {
::close(sock_fd_);
@@ -96,8 +93,7 @@ bool SickDriver::open() {
recv_buf_.clear();
// The device stays passive until told otherwise — without this, no
// LMDscandata telegram ever arrives.
// Device is passive until told to stream.
if (!send_telegram("sEN LMDscandata 1")) {
close();
return false;
@@ -105,16 +101,14 @@ bool SickDriver::open() {
return true;
}
// ─── close() ────────────────────────────────────────────────────────────────
void SickDriver::close() {
if (sock_fd_ >= 0) {
send_telegram("sEN LMDscandata 0"); // best-effort, ignore failure
send_telegram("sEN LMDscandata 0"); // best-effort
::close(sock_fd_);
sock_fd_ = -1;
}
}
// ─── send_telegram() — wrap with STX/ETX and write ─────────────────────────
bool SickDriver::send_telegram(const std::string& body) {
if (sock_fd_ < 0) return false;
std::string framed;
@@ -132,10 +126,8 @@ bool SickDriver::send_telegram(const std::string& body) {
return true;
}
// ─── read_telegram() — pull bytes off the TCP stream until one full
// STX..ETX frame is assembled. CoLa-A has no length prefix, so ETX is the
// only frame boundary; recv_buf_ carries any leftover bytes (start of the
// next telegram) across calls. ──────────────────────────────────────────────
// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
// carries leftover bytes across calls.
bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
if (sock_fd_ < 0) return false;
@@ -144,7 +136,6 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
if (etx_pos != std::string::npos) {
size_t stx_pos = recv_buf_.find(kStx);
if (stx_pos == std::string::npos || stx_pos > etx_pos) {
// Stray ETX with no matching STX before it — drop and retry.
recv_buf_.erase(0, etx_pos + 1);
continue;
}
@@ -157,58 +148,37 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return false; // timeout or error
if (r <= 0) return false;
}
char buf[4096];
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
if (n <= 0) return false; // closed or error
if (n <= 0) return false;
recv_buf_.append(buf, static_cast<size_t>(n));
}
}
// ─── recv_scan() ────────────────────────────────────────────────────────────
bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
for (;;) {
std::string telegram;
if (!read_telegram(telegram, timeout_ms)) return false;
if (parse_lmdscandata(telegram, out)) return true;
// Non-scan telegram (e.g. an "sEA"/access-mode ack) — keep waiting.
// Non-scan telegram (e.g. an ack) — keep waiting.
}
}
// ─── spin_once() ────────────────────────────────────────────────────────────
bool SickDriver::spin_once() {
std::string telegram;
if (!read_telegram(telegram, 0)) return false; // 0 = block until next telegram
if (!read_telegram(telegram, 0)) return false;
ScanResult result;
if (!parse_lmdscandata(telegram, result)) return true; // ignore non-scan telegrams
if (!parse_lmdscandata(telegram, result)) return true;
if (cb_) cb_(result);
return true;
}
// ─── parse_lmdscandata() ────────────────────────────────────────────────────
// CoLa-A "sSN LMDscandata"/"sRA LMDscandata" telegram, space-separated ASCII
// tokens (mostly hex). UNVERIFIED layout (see header comment) — ported from
// SICK's public Telegram Listing, field order below:
//
// sSN LMDscandata <Version> <DeviceNumber> <SerialNumber>
// <Status0> <Status1> <TelegramCounter> <ScanCounter>
// <TimeSinceStartup> <TimeOfTransmission>
// <In0> <In1> <Out0> <Out1> <Reserved>
// <ScanningFrequency> <MeasurementFrequency>
// <NumEncoders> [<EncoderPosition> <EncoderSpeed>]*
// <Num16BitChannels>
// { <ContentName> <ScalingFactor(IEEE754 hex)> <ScalingOffset(hex)>
// <StartAngle(1/10000 deg, signed hex)> <StepWidth(1/10000 deg, signed hex)>
// <NumData> <Data>* }*
// <Num8BitChannels> { ...same shape, 8-bit data... }*
// (position/name/comment/time/event fields follow — not needed for LaserScan, ignored)
//
// ContentName "DIST1" carries ranges (raw mm × ScalingFactor), "RSSI1"
// carries intensities (raw × ScalingFactor) — any other channel name is
// consumed (to keep the token cursor in sync) but its data discarded.
// CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
// per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
std::vector<std::string> tok = tokenize(telegram);
if (tok.size() < 20) return false;
@@ -218,15 +188,14 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
size_t i = 2;
auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); };
hex_to_u32(next()); // VersionNumber — not exposed
hex_to_u32(next()); // DeviceNumber — not exposed
hex_to_u32(next()); // SerialNumber — not exposed
uint32_t status0 = hex_to_u32(next()); // DeviceStatus: Error
uint32_t status1 = hex_to_u32(next()); // DeviceStatus: Pollution
uint32_t telegram_counter = hex_to_u32(next());
uint32_t scan_counter = hex_to_u32(next());
(void)telegram_counter; (void)scan_counter; // not carried by ExtraInfo today
hex_to_u32(next()); // TimeSinceStartup — not exposed
hex_to_u32(next()); // VersionNumber
hex_to_u32(next()); // DeviceNumber
hex_to_u32(next()); // SerialNumber
uint32_t status0 = hex_to_u32(next());
uint32_t status1 = hex_to_u32(next());
hex_to_u32(next()); // TelegramCounter
hex_to_u32(next()); // ScanCounter
hex_to_u32(next()); // TimeSinceStartup
uint32_t time_of_transmission = hex_to_u32(next());
uint32_t in0 = hex_to_u32(next());
uint32_t in1 = hex_to_u32(next());
@@ -234,7 +203,7 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
uint32_t out1 = hex_to_u32(next());
next(); // Reserved
uint32_t scanning_frequency = hex_to_u32(next());
hex_to_u32(next()); // MeasurementFrequency — not exposed
hex_to_u32(next()); // MeasurementFrequency
uint32_t num_encoders = hex_to_u32(next());
for (uint32_t e = 0; e < num_encoders; ++e) {
@@ -248,22 +217,23 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
float angle_min_deg = 0.f, angle_inc_deg = 0.f;
bool got_dist = false;
auto parse_channel_block = [&](bool eight_bit) {
std::string content = next(); // e.g. "DIST1", "RSSI1"
// 16-bit and 8-bit channel blocks share the same ASCII layout.
auto parse_channel_block = [&]() {
std::string content = next();
uint32_t scale_bits = hex_to_u32(next());
hex_to_u32(next()); // ScalingOffset — unused
hex_to_u32(next()); // ScalingOffset
int32_t start_angle = hex_to_i32(next()); // 1/10000 deg
int32_t step_width = hex_to_i32(next()); // 1/10000 deg
uint32_t num_data = hex_to_u32(next());
float scale = bits_to_float(scale_bits);
if (scale == 0.f) scale = 1.f; // guard against a zero/garbage scaling factor
if (scale == 0.f) scale = 1.f;
bool is_dist = content.rfind("DIST", 0) == 0;
bool is_rssi = content.rfind("RSSI", 0) == 0;
if (is_dist) {
angle_min_deg = static_cast<float>(start_angle) * 0.0001f;
angle_min_deg = static_cast<float>(start_angle) * 0.0001f + cfg_.angle_offset_deg;
angle_inc_deg = static_cast<float>(step_width) * 0.0001f;
scan.ranges.assign(num_data, 0.f);
} else if (is_rssi && scan.intensities.empty()) {
@@ -279,14 +249,13 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
scan.intensities[d] = static_cast<float>(raw) * scale;
}
}
(void)eight_bit;
};
uint32_t num_16bit_channels = hex_to_u32(next());
for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block(false);
for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block();
uint32_t num_8bit_channels = hex_to_u32(next());
for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block(true);
for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block();
if (!got_dist || scan.ranges.empty()) return false;
@@ -295,17 +264,18 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
scan.angle_increment = angle_inc_deg * kDeg2Rad;
scan.angle_max = scan.angle_min +
scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
scan.time_increment = 0.f; // device doesn't expose per-point timing
scan.scan_time = 0.f; // device doesn't expose per-scan timing
scan.time_increment = 0.f;
scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
if (scan.intensities.size() != scan.ranges.size())
scan.intensities.assign(scan.ranges.size(), 0.f); // RSSI channel wasn't enabled on the device
scan.intensities.assign(scan.ranges.size(), 0.f);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
ExtraInfo& info = out.info;
info = ExtraInfo{};
// LMDscandata carries no model-name string (unlike OLEI Family B) — SICK
// doesn't auto-detect, the caller's cfg names the model up front.
info.detected_model = cfg_.name;
info.error_status = static_cast<uint8_t>(status0 & 0xFF);
info.status_flags = (status0 << 8) | status1;
@@ -316,4 +286,181 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
return true;
}
// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
recv_buf_(kNanoRecvBufSize) {}
NanoScanDriver::~NanoScanDriver() { close(); }
bool NanoScanDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return false;
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY
: inet_addr(ip_.c_str());
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
::close(sock_fd_);
sock_fd_ = -1;
return false;
}
return true;
}
void NanoScanDriver::close() {
if (sock_fd_ >= 0) {
::close(sock_fd_);
sock_fd_ = -1;
}
}
int NanoScanDriver::recv_datagram(int timeout_ms) {
if (sock_fd_ < 0) return -1;
if (timeout_ms > 0) {
fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
if (r <= 0) return -1;
}
ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
return (n <= 0) ? -1 : static_cast<int>(n);
}
// A scan is split across datagrams at the application layer. Each starts with
// a 24-byte fragment header: "MS3 " @0, u32 totalLength @8, u32 scanNumber @12,
// u32 fragmentOffset @16. Reassemble until totalLength bytes; a lost fragment
// drops that scan and we resync on the next scanNumber.
bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
std::vector<uint8_t> tele;
uint32_t cur_scan = 0, total = 0, got = 0;
bool assembling = false;
for (;;) {
int n = recv_datagram(timeout_ms);
if (n < 0) return false;
const uint8_t* d = recv_buf_.data();
if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
if (parse_packet(d, n, out)) return true;
continue;
}
uint32_t tl = le32(d + 8);
uint32_t scan = le32(d + 12);
uint32_t foff = le32(d + 16);
const uint8_t* pl = d + 24;
uint32_t pl_len = static_cast<uint32_t>(n) - 24;
if (tl == 0 || tl > kNanoRecvBufSize) continue;
if (!assembling || scan != cur_scan || tl != total) {
cur_scan = scan; total = tl; got = 0;
tele.assign(total, 0);
assembling = true;
}
if (static_cast<uint64_t>(foff) + pl_len <= total) {
std::memcpy(tele.data() + foff, pl, pl_len);
got += pl_len;
}
if (got >= total) {
assembling = false;
if (parse_packet(tele.data(), static_cast<int>(total), out)) return true;
}
}
}
bool NanoScanDriver::spin_once() {
int n = recv_datagram(0);
if (n < 0) return false;
ScanResult result;
if (!parse_packet(recv_buf_.data(), n, result)) return true;
if (cb_) cb_(result);
return true;
}
// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners:
// DataHeader offset table at fixed offsets (derivedValues @36, measurementData
// @40); DerivedValues holds multiplicationFactor/startAngle/resolution;
// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status).
bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
if (len < 52) return false;
uint16_t dv_off = le16(buf + 36);
uint16_t dv_size = le16(buf + 38);
uint16_t md_off = le16(buf + 40);
uint16_t md_size = le16(buf + 42);
if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false;
if (static_cast<int>(dv_off) + 20 > len) return false;
if (static_cast<int>(md_off) + 4 > len) return false;
const uint8_t* dv = buf + dv_off;
uint16_t mult_factor = le16(dv + 0);
int32_t start_raw = le_i32(dv + 8);
int32_t res_raw = le_i32(dv + 12);
if (mult_factor == 0) mult_factor = 1;
double start_deg = static_cast<double>(start_raw) / kNanoAngleResolution;
double res_deg = static_cast<double>(res_raw) / kNanoAngleResolution;
const uint8_t* md = buf + md_off;
uint32_t num_beams = le32(md + 0);
if (num_beams == 0 || num_beams > 2751) return false; // 2751 = sensor max
if (static_cast<int64_t>(md_off) + 4 + static_cast<int64_t>(num_beams) * 4 > len)
return false;
LaserScan& scan = out.scan;
scan.ranges.assign(num_beams, 0.f);
scan.intensities.assign(num_beams, 0.f);
for (uint32_t i = 0; i < num_beams; ++i) {
const uint8_t* p = md + 4 + i * 4;
uint16_t distance = le16(p + 0);
uint8_t reflect = le_u8(p + 2);
uint8_t status = le_u8(p + 3);
bool valid = (status & 0x01) != 0;
bool infinite = (status & 0x02) != 0;
if (!valid || infinite) {
scan.ranges[i] = std::numeric_limits<float>::infinity();
} else {
scan.ranges[i] = static_cast<float>(distance) *
static_cast<float>(mult_factor) * 1e-3f; // mm -> m
}
scan.intensities[i] = static_cast<float>(reflect);
}
scan.angle_min = (static_cast<float>(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad;
scan.angle_increment = static_cast<float>(res_deg * kDeg2Rad);
scan.angle_max = scan.angle_min +
scan.angle_increment * static_cast<float>(num_beams - 1);
scan.time_increment = 0.f;
scan.scan_time = 0.f;
scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
// Raw device time from the DataHeader — an opaque tag, not ms since power-on.
scan.timestamp_ms = le32(buf + 28);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
ExtraInfo& info = out.info;
info = ExtraInfo{};
info.detected_model = cfg_.name;
return true;
}
} // namespace lidarlib