refactor: restructure lidarlib into xlidar-driver plugin SDK
- LidarManager facade (liblidar_manager.so): dlopen plugin discovery, available_drivers map<driver_id, PluginRegistry>, create_lidar_device, config.json load/save with legacy lidarlib migration - Common LidarDriverInterface + DriverInfo/DeviceConfig plugin ABI (extern C get_driver_info / create_driver_instance) - Plugins: driver_rplidar (ported from xlocd, Slamtec SDK), driver_olei, driver_sick_code (TiM CoLa-A), driver_sick_safety (nanoScan3), driver_espe - Diagnostics extended with rplidar health + firmware; FOV filter window, range override and legacy remap window unified in DeviceConfig - Rewritten README, diagnostics doc and examples (list_drivers, example, lidar_app) Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
1
plugins/driver_sick_safety/CMakeLists.txt
Normal file
1
plugins/driver_sick_safety/CMakeLists.txt
Normal file
@@ -0,0 +1 @@
|
||||
xlidar_add_plugin(driver_sick_safety sick_safety_driver.cpp)
|
||||
261
plugins/driver_sick_safety/sick_safety_driver.cpp
Normal file
261
plugins/driver_sick_safety/sick_safety_driver.cpp
Normal file
@@ -0,0 +1,261 @@
|
||||
// SICK nanoScan3 / microScan3 — binary safety-data UDP packets, with
|
||||
// application-layer "MS3 " fragment reassembly.
|
||||
#include "sick_safety_driver.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
namespace {
|
||||
constexpr size_t kNanoRecvBufSize = 65536;
|
||||
// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
|
||||
constexpr double kNanoAngleResolution = 4194304.0;
|
||||
} // namespace
|
||||
|
||||
SickSafetyDriver::SickSafetyDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
|
||||
bool inverted)
|
||||
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
|
||||
inverted_(inverted), recv_buf_(kNanoRecvBufSize) {}
|
||||
|
||||
SickSafetyDriver::~SickSafetyDriver() { close(); }
|
||||
|
||||
ErrorCode SickSafetyDriver::open() {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
if (ip_ == "0.0.0.0" || ip_.empty()) {
|
||||
addr.sin_addr.s_addr = INADDR_ANY;
|
||||
} else if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1) {
|
||||
return set_error(ErrorCode::InvalidAddress);
|
||||
}
|
||||
|
||||
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
|
||||
|
||||
// No SO_REUSEADDR: UDP has no TIME_WAIT, and on Linux it would let two
|
||||
// sockets bind the same port, hiding PortInUse from the second app.
|
||||
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
|
||||
int err = errno;
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
|
||||
: ErrorCode::BindFailed);
|
||||
}
|
||||
latest_diag_ = Diagnostics{};
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void SickSafetyDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
int SickSafetyDriver::recv_datagram(int timeout_ms) {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); 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) {
|
||||
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
|
||||
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return -1; }
|
||||
return 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 SickSafetyDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
std::vector<uint8_t> tele;
|
||||
std::vector<uint8_t> have; // per-byte coverage so duplicate fragments don't count twice
|
||||
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)) { set_error(ErrorCode::Ok); 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);
|
||||
have.assign(total, 0);
|
||||
assembling = true;
|
||||
}
|
||||
|
||||
if (static_cast<uint64_t>(foff) + pl_len <= total) {
|
||||
std::memcpy(tele.data() + foff, pl, pl_len);
|
||||
for (uint32_t b = 0; b < pl_len; ++b)
|
||||
if (!have[foff + b]) { have[foff + b] = 1; ++got; }
|
||||
}
|
||||
|
||||
if (got >= total) {
|
||||
assembling = false;
|
||||
if (parse_packet(tele.data(), static_cast<int>(total), out)) {
|
||||
set_error(ErrorCode::Ok);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool SickSafetyDriver::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 SickSafetyDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
|
||||
if (len < 52) return false;
|
||||
|
||||
uint16_t gss_off = le16(buf + 32); // General System State block
|
||||
uint16_t gss_size = le16(buf + 34);
|
||||
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);
|
||||
|
||||
finalize_scan(scan, cfg_, inverted_);
|
||||
|
||||
ExtraInfo& info = out.info;
|
||||
info = ExtraInfo{};
|
||||
info.detected_model = cfg_.name;
|
||||
// Byte 0 holds the run/standby/contamination/manipulation flags
|
||||
// (kNanoState*); the block is absent when not configured in the sensor.
|
||||
if (gss_off != 0 && gss_size != 0 && static_cast<int>(gss_off) < len)
|
||||
info.nano_general_state = buf[gss_off];
|
||||
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
|
||||
mark_scan_decoded();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// ── plugin registration ─────────────────────────────────────────────────────
|
||||
|
||||
namespace {
|
||||
|
||||
const DriverInfo kDriverInfo = [] {
|
||||
DriverInfo info;
|
||||
info.vendor = "SICK";
|
||||
info.model = "nanoScan3/microScan3";
|
||||
info.driver_id = "sick_nanoscan3_driver";
|
||||
info.description = "SICK safety laser scanners (nanoScan3/microScan3 family) "
|
||||
"— passive receiver of the binary safety-data UDP output; "
|
||||
"the sensor's UDP target must be configured in SICK Safety "
|
||||
"Designer. Default local port 6060. Not verified on real "
|
||||
"hardware.";
|
||||
info.transport = Transport::Udp;
|
||||
info.supported_models = {"SICK-nanoScan3"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
} // namespace
|
||||
|
||||
DriverInfo SickSafetyDriver::get_driver_info() const { return kDriverInfo; }
|
||||
|
||||
} // namespace xlidar
|
||||
|
||||
XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
||||
*out = xlidar::kDriverInfo;
|
||||
}
|
||||
|
||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||
using namespace xlidar;
|
||||
const uint16_t port = cfg->port ? cfg->port : 6060;
|
||||
return new SickSafetyDriver(apply_device_config(MODEL_SICK_NANOSCAN3, *cfg),
|
||||
cfg->ip, port, cfg->inverted);
|
||||
}
|
||||
63
plugins/driver_sick_safety/sick_safety_driver.hpp
Normal file
63
plugins/driver_sick_safety/sick_safety_driver.hpp
Normal file
@@ -0,0 +1,63 @@
|
||||
// SICK nanoScan3 / microScan3 safety scanners over UDP — plugin-private header.
|
||||
#pragma once
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };
|
||||
|
||||
// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
|
||||
// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
|
||||
// Passive UDP receiver: the sensor's UDP target must be configured up front in
|
||||
// SICK Safety Designer — this class does no CoLa2/TCP handshake.
|
||||
class SickSafetyDriver : public LidarDriverInterface {
|
||||
public:
|
||||
// ip: local bind address; port: local UDP port the sensor sends to;
|
||||
// inverted: unit mounted upside-down → mirror the scan.
|
||||
explicit SickSafetyDriver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 6060,
|
||||
bool inverted = false);
|
||||
~SickSafetyDriver();
|
||||
|
||||
SickSafetyDriver(const SickSafetyDriver&) = delete;
|
||||
SickSafetyDriver& operator=(const SickSafetyDriver&) = delete;
|
||||
|
||||
DriverInfo get_driver_info() const override;
|
||||
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
int recv_datagram(int timeout_ms);
|
||||
bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by parse_packet().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
|
||||
std::vector<uint8_t> recv_buf_;
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
Reference in New Issue
Block a user