Fix Family B angle decode + add LR-1FMI model
parse_family_b() dùng sai hệ số góc 0.25°/LSB; theo spec Olei chính hãng (Olei.LidarSensor/LidarDataBlock.GetAngleDegrees) AngleRaw là 0.01°/LSB. Sai 25× khiến điểm bị gán nhầm góc → một phòng bị bôi thành vòng tròn trên RViz. Đã verify với thiết bị thật OLELR-1FMI: sau khi sửa ra 2400 điểm/vòng, 0–359.9°, đúng hình học môi trường. - Đổi hệ số góc 0.25° → 0.01° trong parse_family_b(). - Bỏ qua block invalid (AngleRaw >= 0xFF00) theo spec. - Dò ranh giới vòng quay PER-POINT thay vì per-packet (một gói có thể chứa >1 vòng), tránh gộp nhiều vòng vào một scan. - Thêm model LR-1FMI (360°, 0.01°/LSB, ~2400 pts/rev) vào bảng model + kModelTable, đặt "1FMI" trước "1F" để khớp đúng chuỗi tên. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
238
src/json_mini.hpp
Normal file
238
src/json_mini.hpp
Normal file
@@ -0,0 +1,238 @@
|
||||
// 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.
|
||||
#pragma once
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <cctype>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace json {
|
||||
|
||||
enum class Type { Null, Bool, Number, String, Object, Array };
|
||||
|
||||
struct Value {
|
||||
Type type = Type::Null;
|
||||
bool b = false;
|
||||
double num = 0;
|
||||
std::string str;
|
||||
std::vector<Value> arr;
|
||||
std::vector<std::pair<std::string, Value>> obj;
|
||||
|
||||
static Value make_object() { Value v; v.type = Type::Object; return v; }
|
||||
static Value make_string(std::string s) { Value v; v.type = Type::String; v.str = std::move(s); return v; }
|
||||
static Value make_number(double n) { Value v; v.type = Type::Number; v.num = n; return v; }
|
||||
static Value make_bool(bool x) { Value v; v.type = Type::Bool; v.b = x; return v; }
|
||||
|
||||
void set(const std::string& key, Value v) {
|
||||
for (auto& kv : obj) if (kv.first == key) { kv.second = std::move(v); return; }
|
||||
obj.emplace_back(key, std::move(v));
|
||||
}
|
||||
|
||||
const Value* find(const std::string& key) const {
|
||||
for (auto& kv : obj) if (kv.first == key) return &kv.second;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
std::string get_string(const std::string& key, const std::string& def = "") const {
|
||||
const Value* v = find(key);
|
||||
return (v && v->type == Type::String) ? v->str : def;
|
||||
}
|
||||
double get_number(const std::string& key, double def = 0) const {
|
||||
const Value* v = find(key);
|
||||
return (v && v->type == Type::Number) ? v->num : def;
|
||||
}
|
||||
bool get_bool(const std::string& key, bool def = false) const {
|
||||
const Value* v = find(key);
|
||||
return (v && v->type == Type::Bool) ? v->b : def;
|
||||
}
|
||||
|
||||
std::string dump() const {
|
||||
std::string out;
|
||||
dump_to(out);
|
||||
return out;
|
||||
}
|
||||
|
||||
private:
|
||||
static void escape_into(const std::string& s, std::string& out) {
|
||||
out += '"';
|
||||
for (char c : s) {
|
||||
switch (c) {
|
||||
case '"': out += "\\\""; break;
|
||||
case '\\': out += "\\\\"; break;
|
||||
case '\n': out += "\\n"; break;
|
||||
default: out += c; break;
|
||||
}
|
||||
}
|
||||
out += '"';
|
||||
}
|
||||
|
||||
void dump_to(std::string& out) const {
|
||||
switch (type) {
|
||||
case Type::Null: out += "null"; break;
|
||||
case Type::Bool: out += b ? "true" : "false"; break;
|
||||
case Type::Number: {
|
||||
if (num == static_cast<long long>(num)) out += std::to_string(static_cast<long long>(num));
|
||||
else out += std::to_string(num);
|
||||
break;
|
||||
}
|
||||
case Type::String: escape_into(str, out); break;
|
||||
case Type::Array: {
|
||||
out += '[';
|
||||
for (size_t i = 0; i < arr.size(); ++i) {
|
||||
if (i) out += ',';
|
||||
arr[i].dump_to(out);
|
||||
}
|
||||
out += ']';
|
||||
break;
|
||||
}
|
||||
case Type::Object: {
|
||||
out += '{';
|
||||
for (size_t i = 0; i < obj.size(); ++i) {
|
||||
if (i) out += ',';
|
||||
escape_into(obj[i].first, out);
|
||||
out += ':';
|
||||
obj[i].second.dump_to(out);
|
||||
}
|
||||
out += '}';
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
// ── Parser ──────────────────────────────────────────────────────────────
|
||||
class ParseError : public std::runtime_error {
|
||||
public:
|
||||
explicit ParseError(const std::string& what) : std::runtime_error(what) {}
|
||||
};
|
||||
|
||||
namespace detail {
|
||||
|
||||
class Parser {
|
||||
public:
|
||||
explicit Parser(const std::string& s) : s_(s) {}
|
||||
|
||||
Value parse() {
|
||||
skip_ws();
|
||||
Value v = parse_value();
|
||||
skip_ws();
|
||||
return v;
|
||||
}
|
||||
|
||||
private:
|
||||
const std::string& s_;
|
||||
size_t pos_ = 0;
|
||||
|
||||
char peek() const {
|
||||
if (pos_ >= s_.size()) throw ParseError("unexpected end of JSON");
|
||||
return s_[pos_];
|
||||
}
|
||||
char next() { return s_[pos_++]; }
|
||||
void skip_ws() { while (pos_ < s_.size() && std::isspace(static_cast<unsigned char>(s_[pos_]))) ++pos_; }
|
||||
void expect(char c) {
|
||||
if (pos_ >= s_.size() || s_[pos_] != c)
|
||||
throw ParseError(std::string("expected '") + c + "'");
|
||||
++pos_;
|
||||
}
|
||||
bool starts_with(const char* lit) {
|
||||
size_t n = std::strlen(lit);
|
||||
if (s_.compare(pos_, n, lit) == 0) { pos_ += n; return true; }
|
||||
return false;
|
||||
}
|
||||
|
||||
Value parse_value() {
|
||||
skip_ws();
|
||||
char c = peek();
|
||||
if (c == '{') return parse_object();
|
||||
if (c == '[') return parse_array();
|
||||
if (c == '"') return Value::make_string(parse_string());
|
||||
if (starts_with("true")) return Value::make_bool(true);
|
||||
if (starts_with("false")) return Value::make_bool(false);
|
||||
if (starts_with("null")) { Value v; v.type = Type::Null; return v; }
|
||||
return parse_number();
|
||||
}
|
||||
|
||||
Value parse_object() {
|
||||
Value v = Value::make_object();
|
||||
expect('{');
|
||||
skip_ws();
|
||||
if (peek() == '}') { ++pos_; return v; }
|
||||
while (true) {
|
||||
skip_ws();
|
||||
std::string key = parse_string();
|
||||
skip_ws();
|
||||
expect(':');
|
||||
Value val = parse_value();
|
||||
v.obj.emplace_back(std::move(key), std::move(val));
|
||||
skip_ws();
|
||||
char c = next();
|
||||
if (c == ',') continue;
|
||||
if (c == '}') break;
|
||||
throw ParseError("expected ',' or '}' in object");
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
Value parse_array() {
|
||||
Value v; v.type = Type::Array;
|
||||
expect('[');
|
||||
skip_ws();
|
||||
if (peek() == ']') { ++pos_; return v; }
|
||||
while (true) {
|
||||
v.arr.push_back(parse_value());
|
||||
skip_ws();
|
||||
char c = next();
|
||||
if (c == ',') continue;
|
||||
if (c == ']') break;
|
||||
throw ParseError("expected ',' or ']' in array");
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
std::string parse_string() {
|
||||
expect('"');
|
||||
std::string out;
|
||||
while (true) {
|
||||
char c = next();
|
||||
if (c == '"') break;
|
||||
if (c == '\\') {
|
||||
char e = next();
|
||||
switch (e) {
|
||||
case 'n': out += '\n'; break;
|
||||
case 't': out += '\t'; break;
|
||||
case '"': out += '"'; break;
|
||||
case '\\': out += '\\'; break;
|
||||
case '/': out += '/'; break;
|
||||
default: out += e; break;
|
||||
}
|
||||
} else {
|
||||
out += c;
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
Value parse_number() {
|
||||
size_t start = pos_;
|
||||
if (pos_ < s_.size() && (s_[pos_] == '-' || s_[pos_] == '+')) ++pos_;
|
||||
while (pos_ < s_.size() &&
|
||||
(std::isdigit(static_cast<unsigned char>(s_[pos_])) || s_[pos_] == '.' ||
|
||||
s_[pos_] == 'e' || s_[pos_] == 'E' || s_[pos_] == '-' || s_[pos_] == '+'))
|
||||
++pos_;
|
||||
if (pos_ == start) throw ParseError("invalid number");
|
||||
return Value::make_number(std::strtod(s_.substr(start, pos_ - start).c_str(), nullptr));
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace detail
|
||||
|
||||
inline Value parse(const std::string& s) {
|
||||
detail::Parser p(s);
|
||||
return p.parse();
|
||||
}
|
||||
|
||||
} // namespace json
|
||||
156
src/olei_config.cpp
Normal file
156
src/olei_config.cpp
Normal file
@@ -0,0 +1,156 @@
|
||||
#include "lidarlib/config.hpp"
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
#include "json_mini.hpp"
|
||||
#include <algorithm>
|
||||
#include <fstream>
|
||||
#include <sstream>
|
||||
#include <stdexcept>
|
||||
#include <utility>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
namespace {
|
||||
|
||||
struct ModelEntry { const char* name; const ModelConfig* cfg; const char* brand; };
|
||||
|
||||
constexpr ModelEntry kModels[] = {
|
||||
{ "AUTO", &MODEL_AUTO, "OLEI" },
|
||||
{ "VB", &MODEL_VB, "OLEI" },
|
||||
{ "VF", &MODEL_VF, "OLEI" },
|
||||
{ "LR-1F", &MODEL_LR1F, "OLEI" },
|
||||
{ "LR-1FMI", &MODEL_LR1FMI, "OLEI" },
|
||||
{ "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" },
|
||||
};
|
||||
|
||||
json::Value to_json(const LidarConfig& c) {
|
||||
json::Value v = json::Value::make_object();
|
||||
v.set("name", json::Value::make_string(c.name));
|
||||
v.set("ip", json::Value::make_string(c.ip));
|
||||
v.set("port", json::Value::make_number(c.port));
|
||||
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));
|
||||
return v;
|
||||
}
|
||||
|
||||
LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) {
|
||||
LidarConfig c = def;
|
||||
c.name = v.get_string("name", def.name);
|
||||
c.ip = v.get_string("ip", def.ip);
|
||||
c.port = static_cast<uint16_t>(v.get_number("port", def.port));
|
||||
c.brand = v.get_string("brand", def.brand);
|
||||
c.model = v.get_string("model", def.model);
|
||||
c.inverted = v.get_bool("inverted", def.inverted);
|
||||
return c;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
const ModelConfig* model_by_name(const std::string& name) {
|
||||
for (const auto& e : kModels)
|
||||
if (name == e.name) return e.cfg;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
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;
|
||||
return nullptr;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
const std::vector<std::string>& model_names() {
|
||||
static const std::vector<std::string> names = [] {
|
||||
std::vector<std::string> v;
|
||||
for (const auto& e : kModels) v.push_back(e.name);
|
||||
return v;
|
||||
}();
|
||||
return names;
|
||||
}
|
||||
|
||||
const std::vector<std::string>& brand_names() {
|
||||
static const std::vector<std::string> names = {"OLEI", "SICK"};
|
||||
return names;
|
||||
}
|
||||
|
||||
const std::vector<std::string>& model_names_for_brand(const std::string& brand) {
|
||||
static const std::vector<std::string> empty;
|
||||
static const auto by_brand = [] {
|
||||
std::vector<std::pair<std::string, std::vector<std::string>>> m;
|
||||
for (const auto& e : kModels) {
|
||||
auto it = std::find_if(m.begin(), m.end(),
|
||||
[&](const auto& p) { return p.first == e.brand; });
|
||||
if (it == m.end()) { m.push_back({e.brand, {}}); it = m.end() - 1; }
|
||||
it->second.push_back(e.name);
|
||||
}
|
||||
return m;
|
||||
}();
|
||||
for (const auto& p : by_brand)
|
||||
if (p.first == brand) return p.second;
|
||||
return empty;
|
||||
}
|
||||
|
||||
Config load_config(const std::string& path) {
|
||||
Config cfg; // defaults
|
||||
std::ifstream f(path);
|
||||
if (!f) return cfg;
|
||||
|
||||
std::ostringstream ss;
|
||||
ss << f.rdbuf();
|
||||
json::Value root;
|
||||
try {
|
||||
root = json::parse(ss.str());
|
||||
} catch (const json::ParseError&) {
|
||||
return cfg; // malformed file -> fall back to defaults rather than crash
|
||||
}
|
||||
|
||||
const json::Value* lidars = root.find("lidars");
|
||||
if (!lidars || lidars->type != json::Type::Array) return cfg;
|
||||
|
||||
static const LidarConfig kBlankDefault{};
|
||||
cfg.lidars.clear();
|
||||
for (const auto& entry : lidars->arr)
|
||||
cfg.lidars.push_back(lidar_from_json(entry, kBlankDefault));
|
||||
return cfg;
|
||||
}
|
||||
|
||||
void save_config(const std::string& path, const Config& cfg) {
|
||||
json::Value root = json::Value::make_object();
|
||||
json::Value arr; arr.type = json::Type::Array;
|
||||
for (const auto& lidar : cfg.lidars) arr.arr.push_back(to_json(lidar));
|
||||
root.set("lidars", arr);
|
||||
|
||||
std::ofstream f(path, std::ios::trunc);
|
||||
if (!f) throw std::runtime_error("khong the ghi file config: " + path);
|
||||
f << root.dump() << "\n";
|
||||
}
|
||||
|
||||
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).
|
||||
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);
|
||||
}
|
||||
|
||||
} // namespace lidarlib
|
||||
488
src/olei_lidar.cpp
Normal file
488
src/olei_lidar.cpp
Normal file
@@ -0,0 +1,488 @@
|
||||
#include "lidarlib/lidar.hpp"
|
||||
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/select.h>
|
||||
|
||||
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.
|
||||
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]
|
||||
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) ──────────────────────────────────────
|
||||
static uint32_t crc32_olei(const uint8_t* data, size_t len) {
|
||||
uint32_t crc = 0xFFFFFFFF;
|
||||
for (size_t i = 0; i < len; ++i) {
|
||||
crc ^= static_cast<uint32_t>(data[i]) << 24;
|
||||
for (int b = 0; b < 8; ++b)
|
||||
crc = (crc & 0x80000000u) ? (crc << 1) ^ 0x04C11DB7u : (crc << 1);
|
||||
}
|
||||
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)
|
||||
{
|
||||
auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
|
||||
}
|
||||
|
||||
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
|
||||
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
|
||||
#endif
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
addr.sin_addr.s_addr = inet_addr(ip_.c_str());
|
||||
|
||||
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return false;
|
||||
}
|
||||
pending_angle_deg_.reserve(2048);
|
||||
pending_dist_m_.reserve(2048);
|
||||
pending_intensity_.reserve(2048);
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── close() ────────────────────────────────────────────────────────────────
|
||||
void Driver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
// ─── recv_scan() — blocks until one full revolution is available ──────────
|
||||
bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
scan_ready_ = false;
|
||||
|
||||
while (!scan_ready_) {
|
||||
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 false; // timeout or error
|
||||
}
|
||||
if (!spin_once()) return false;
|
||||
}
|
||||
out = std::move(ready_result_);
|
||||
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);
|
||||
|
||||
ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
|
||||
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]
|
||||
|
||||
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).
|
||||
void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
|
||||
float angle = signed_angle_deg;
|
||||
if (!pending_angle_deg_.empty()) {
|
||||
float prev = pending_angle_deg_.back();
|
||||
while (angle - prev > 180.f) angle -= 360.f;
|
||||
while (angle - prev < -180.f) angle += 360.f;
|
||||
}
|
||||
pending_angle_deg_.push_back(angle);
|
||||
pending_dist_m_.push_back(dist_m);
|
||||
pending_intensity_.push_back(intensity);
|
||||
}
|
||||
|
||||
// ─── flush_scan() — a revolution is complete ───────────────────────────────
|
||||
void Driver::flush_scan() {
|
||||
if (pending_angle_deg_.empty()) return;
|
||||
|
||||
const size_t n = pending_angle_deg_.size();
|
||||
|
||||
LaserScan& scan = ready_result_.scan;
|
||||
scan.timestamp_ms = pending_ts_;
|
||||
scan.angle_min = pending_angle_deg_.front() * kDeg2Rad;
|
||||
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.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());
|
||||
|
||||
ExtraInfo& info = ready_result_.info;
|
||||
info = pending_info_;
|
||||
info.detected_model = detected_model_name_;
|
||||
info.error_status = pending_err_;
|
||||
|
||||
pending_angle_deg_.clear();
|
||||
pending_dist_m_.clear();
|
||||
pending_intensity_.clear();
|
||||
pending_info_ = ExtraInfo{}; // reset per-revolution optional fields
|
||||
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)
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
// ── 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]);
|
||||
|
||||
const uint8_t* blk = buf + HEADER_LEN;
|
||||
for (uint16_t i = 0; i < num_pts; ++i, blk += BLOCK_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));
|
||||
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);
|
||||
}
|
||||
|
||||
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.
|
||||
bool Driver::parse_family_b(const uint8_t* buf, int len) {
|
||||
static constexpr int HEADER_LEN = 40;
|
||||
static constexpr int BLOCK_LEN = 8;
|
||||
|
||||
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_) {
|
||||
std::string raw(reinterpret_cast<const char*>(buf + 7), 10);
|
||||
size_t z = raw.find('\0');
|
||||
if (z != std::string::npos) raw.resize(z);
|
||||
|
||||
if (!raw.empty()) {
|
||||
detected_model_name_ = raw;
|
||||
model_locked_ = true;
|
||||
|
||||
static constexpr struct { const char* key; const ModelConfig* cfg; } kModelTable[] = {
|
||||
{ "1BS5", &MODEL_LR1BS5 },
|
||||
{ "16F", &MODEL_LR16F },
|
||||
{ "1FMI", &MODEL_LR1FMI }, // must precede "1F": "OLELR-1FMI" also contains "1F"
|
||||
{ "1F", &MODEL_LR1F },
|
||||
{ "VF", &MODEL_VF },
|
||||
{ "VB", &MODEL_VB },
|
||||
};
|
||||
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;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int num_pts = (len - HEADER_LEN) / BLOCK_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.
|
||||
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
|
||||
|
||||
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f); // [0,360)
|
||||
if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
|
||||
flush_scan(); // revolution complete
|
||||
}
|
||||
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
|
||||
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;
|
||||
|
||||
push_point(angle, dist_m, intensity);
|
||||
}
|
||||
|
||||
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).
|
||||
bool Driver::parse_family_c(const uint8_t* buf, int len) {
|
||||
static constexpr int HEADER_LEN = 48;
|
||||
if (len < HEADER_LEN) return false;
|
||||
|
||||
uint16_t header_size_field = le16(buf + 8);
|
||||
uint8_t distance_ratio_raw = buf[10];
|
||||
uint8_t types = buf[11];
|
||||
uint16_t scan_frequency_raw = le16(buf + 24);
|
||||
uint16_t num_pts_scan = le16(buf + 26);
|
||||
uint16_t input_status = le16(buf + 28);
|
||||
uint16_t output_status = le16(buf + 30);
|
||||
uint32_t field_status = le32(buf + 32);
|
||||
uint16_t first_index = le16(buf + 40);
|
||||
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
|
||||
|
||||
int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
|
||||
if (header_size < HEADER_LEN || header_size > len) return false;
|
||||
|
||||
int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
|
||||
if (bytes_per_point == 0) return false; // unknown Types, layout unclear
|
||||
|
||||
int payload_bytes = len - header_size;
|
||||
int num_pts = num_pts_packet;
|
||||
if (num_pts == 0 || num_pts * bytes_per_point > payload_bytes) {
|
||||
num_pts = payload_bytes / bytes_per_point;
|
||||
}
|
||||
if (num_pts <= 0) return false;
|
||||
|
||||
pending_info_.distance_ratio_raw = distance_ratio_raw;
|
||||
pending_info_.scan_frequency_raw = scan_frequency_raw;
|
||||
pending_info_.input_status = input_status;
|
||||
pending_info_.output_status = output_status;
|
||||
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.
|
||||
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;
|
||||
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) {
|
||||
flush_scan();
|
||||
}
|
||||
|
||||
const uint8_t* blk = buf + header_size;
|
||||
for (int i = 0; i < num_pts; ++i, blk += bytes_per_point) {
|
||||
uint16_t range_mm;
|
||||
uint16_t inten_raw = 0;
|
||||
bool has_inten = false;
|
||||
|
||||
if (types == 0x00) {
|
||||
range_mm = le16(blk);
|
||||
} else if (types == 0x01) {
|
||||
range_mm = le16(blk);
|
||||
inten_raw = le16(blk + 2);
|
||||
has_inten = true;
|
||||
} else { // 0x10
|
||||
range_mm = le16(blk + 2);
|
||||
}
|
||||
|
||||
float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f);
|
||||
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 */,
|
||||
has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
|
||||
}
|
||||
|
||||
last_angle_ = raw_first_angle;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace lidarlib
|
||||
319
src/sick_lidar.cpp
Normal file
319
src/sick_lidar.cpp
Normal file
@@ -0,0 +1,319 @@
|
||||
#include "lidarlib/sick_lidar.hpp"
|
||||
|
||||
#include <cctype>
|
||||
#include <cerrno>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
#include <fcntl.h>
|
||||
#include <sys/socket.h>
|
||||
#include <arpa/inet.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/select.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
|
||||
namespace lidarlib {
|
||||
|
||||
namespace {
|
||||
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
|
||||
constexpr char kStx = 0x02;
|
||||
constexpr char kEtx = 0x03;
|
||||
constexpr int kConnectTimeoutMs = 2000;
|
||||
|
||||
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.
|
||||
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;
|
||||
size_t i = 0, n = s.size();
|
||||
while (i < n) {
|
||||
while (i < n && std::isspace(static_cast<unsigned char>(s[i]))) ++i;
|
||||
size_t start = i;
|
||||
while (i < n && !std::isspace(static_cast<unsigned char>(s[i]))) ++i;
|
||||
if (i > start) out.push_back(s.substr(start, i - start));
|
||||
}
|
||||
return out;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
|
||||
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port) {}
|
||||
|
||||
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;
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
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).
|
||||
int flags = ::fcntl(sock_fd_, F_GETFL, 0);
|
||||
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
|
||||
if (rc < 0 && errno == EINPROGRESS) {
|
||||
fd_set wfds; FD_ZERO(&wfds); FD_SET(sock_fd_, &wfds);
|
||||
timeval tv{ kConnectTimeoutMs / 1000, (kConnectTimeoutMs % 1000) * 1000 };
|
||||
rc = ::select(sock_fd_ + 1, nullptr, &wfds, nullptr, &tv);
|
||||
if (rc > 0) {
|
||||
int err = 0; socklen_t errlen = sizeof(err);
|
||||
::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
|
||||
rc = (err == 0) ? 0 : -1;
|
||||
} else {
|
||||
rc = -1; // timeout, or select() itself failed
|
||||
}
|
||||
}
|
||||
::fcntl(sock_fd_, F_SETFL, flags); // restore blocking mode for send/recv below
|
||||
|
||||
if (rc < 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return false;
|
||||
}
|
||||
|
||||
int nodelay = 1;
|
||||
::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
|
||||
|
||||
recv_buf_.clear();
|
||||
|
||||
// The device stays passive until told otherwise — without this, no
|
||||
// LMDscandata telegram ever arrives.
|
||||
if (!send_telegram("sEN LMDscandata 1")) {
|
||||
close();
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ─── close() ────────────────────────────────────────────────────────────────
|
||||
void SickDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
send_telegram("sEN LMDscandata 0"); // best-effort, ignore failure
|
||||
::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;
|
||||
framed.reserve(body.size() + 2);
|
||||
framed.push_back(kStx);
|
||||
framed += body;
|
||||
framed.push_back(kEtx);
|
||||
|
||||
size_t sent = 0;
|
||||
while (sent < framed.size()) {
|
||||
ssize_t n = ::send(sock_fd_, framed.data() + sent, framed.size() - sent, 0);
|
||||
if (n <= 0) return false;
|
||||
sent += static_cast<size_t>(n);
|
||||
}
|
||||
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. ──────────────────────────────────────────────
|
||||
bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
|
||||
if (sock_fd_ < 0) return false;
|
||||
|
||||
for (;;) {
|
||||
size_t etx_pos = recv_buf_.find(kEtx);
|
||||
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;
|
||||
}
|
||||
out = recv_buf_.substr(stx_pos + 1, etx_pos - stx_pos - 1);
|
||||
recv_buf_.erase(0, etx_pos + 1);
|
||||
return true;
|
||||
}
|
||||
|
||||
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 false; // timeout or error
|
||||
}
|
||||
|
||||
char buf[4096];
|
||||
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
|
||||
if (n <= 0) return false; // closed or error
|
||||
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.
|
||||
}
|
||||
}
|
||||
|
||||
// ─── spin_once() ────────────────────────────────────────────────────────────
|
||||
bool SickDriver::spin_once() {
|
||||
std::string telegram;
|
||||
if (!read_telegram(telegram, 0)) return false; // 0 = block until next telegram
|
||||
|
||||
ScanResult result;
|
||||
if (!parse_lmdscandata(telegram, result)) return true; // ignore non-scan telegrams
|
||||
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.
|
||||
bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
|
||||
std::vector<std::string> tok = tokenize(telegram);
|
||||
if (tok.size() < 20) return false;
|
||||
if (tok[0] != "sSN" && tok[0] != "sRA") return false;
|
||||
if (tok[1] != "LMDscandata") return false;
|
||||
|
||||
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
|
||||
uint32_t time_of_transmission = hex_to_u32(next());
|
||||
uint32_t in0 = hex_to_u32(next());
|
||||
uint32_t in1 = hex_to_u32(next());
|
||||
uint32_t out0 = hex_to_u32(next());
|
||||
uint32_t out1 = hex_to_u32(next());
|
||||
next(); // Reserved
|
||||
uint32_t scanning_frequency = hex_to_u32(next());
|
||||
hex_to_u32(next()); // MeasurementFrequency — not exposed
|
||||
|
||||
uint32_t num_encoders = hex_to_u32(next());
|
||||
for (uint32_t e = 0; e < num_encoders; ++e) {
|
||||
next(); // EncoderPosition
|
||||
next(); // EncoderSpeed
|
||||
}
|
||||
|
||||
LaserScan& scan = out.scan;
|
||||
scan.ranges.clear();
|
||||
scan.intensities.clear();
|
||||
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"
|
||||
uint32_t scale_bits = hex_to_u32(next());
|
||||
hex_to_u32(next()); // ScalingOffset — unused
|
||||
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
|
||||
|
||||
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_inc_deg = static_cast<float>(step_width) * 0.0001f;
|
||||
scan.ranges.assign(num_data, 0.f);
|
||||
} else if (is_rssi && scan.intensities.empty()) {
|
||||
scan.intensities.assign(num_data, 0.f);
|
||||
}
|
||||
|
||||
for (uint32_t d = 0; d < num_data; ++d) {
|
||||
uint32_t raw = hex_to_u32(next());
|
||||
if (is_dist) {
|
||||
scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f; // mm -> m
|
||||
got_dist = true;
|
||||
} else if (is_rssi && d < scan.intensities.size()) {
|
||||
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);
|
||||
|
||||
uint32_t num_8bit_channels = hex_to_u32(next());
|
||||
for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block(true);
|
||||
|
||||
if (!got_dist || scan.ranges.empty()) return false;
|
||||
|
||||
scan.timestamp_ms = time_of_transmission;
|
||||
scan.angle_min = angle_min_deg * kDeg2Rad;
|
||||
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.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
|
||||
|
||||
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;
|
||||
info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency);
|
||||
info.input_status = static_cast<uint16_t>((in0 << 8) | in1);
|
||||
info.output_status = static_cast<uint16_t>((out0 << 8) | out1);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace lidarlib
|
||||
Reference in New Issue
Block a user