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:
29
plugins/CMakeLists.txt
Normal file
29
plugins/CMakeLists.txt
Normal file
@@ -0,0 +1,29 @@
|
||||
# Driver plugins. Every plugin is a self-contained MODULE library named
|
||||
# <dir>.so (no "lib" prefix), exporting exactly the two C entry points
|
||||
# declared in include/lidar_interface.hpp. Plugins land in build/plugins/.
|
||||
|
||||
set(XLIDAR_PLUGIN_OUTPUT_DIR ${CMAKE_BINARY_DIR}/plugins)
|
||||
set(XLIDAR_PLUGIN_COMMON_DIR ${CMAKE_CURRENT_SOURCE_DIR}/common)
|
||||
|
||||
# xlidar_add_plugin(<name> <sources...>) — shared boilerplate for one plugin.
|
||||
function(xlidar_add_plugin name)
|
||||
add_library(${name} MODULE ${ARGN})
|
||||
set_target_properties(${name} PROPERTIES
|
||||
PREFIX "" # driver_olei.so, not libdriver_olei.so
|
||||
LIBRARY_OUTPUT_DIRECTORY ${XLIDAR_PLUGIN_OUTPUT_DIR}
|
||||
CXX_VISIBILITY_PRESET hidden # only the two entry points are visible
|
||||
VISIBILITY_INLINES_HIDDEN ON
|
||||
POSITION_INDEPENDENT_CODE ON
|
||||
)
|
||||
target_include_directories(${name} PRIVATE
|
||||
${CMAKE_SOURCE_DIR}/include
|
||||
${XLIDAR_PLUGIN_COMMON_DIR}
|
||||
)
|
||||
target_link_libraries(${name} PRIVATE Threads::Threads)
|
||||
endfunction()
|
||||
|
||||
add_subdirectory(driver_olei)
|
||||
add_subdirectory(driver_sick_code)
|
||||
add_subdirectory(driver_sick_safety)
|
||||
add_subdirectory(driver_espe)
|
||||
add_subdirectory(driver_rplidar)
|
||||
152
plugins/common/plugin_helpers.hpp
Normal file
152
plugins/common/plugin_helpers.hpp
Normal file
@@ -0,0 +1,152 @@
|
||||
// Internal helpers shared by the plugin TUs — not part of the public API.
|
||||
// Header-only on purpose: every plugin .so carries its own copy, so plugins
|
||||
// never link against each other or against liblidar_manager.
|
||||
#pragma once
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cerrno>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <limits>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
// Mirror a finished scan for a unit mounted upside-down: reverse the point
|
||||
// order and negate the angular window. Apply before remap_scan_window().
|
||||
inline void invert_scan(LaserScan& scan) {
|
||||
std::reverse(scan.ranges.begin(), scan.ranges.end());
|
||||
std::reverse(scan.intensities.begin(), scan.intensities.end());
|
||||
const float new_min = -scan.angle_max;
|
||||
scan.angle_max = -scan.angle_min;
|
||||
scan.angle_min = new_min;
|
||||
}
|
||||
|
||||
// Valid FOV window (DeviceConfig::angle_min/max_deg): points whose signed
|
||||
// angle falls outside [min_deg, max_deg] become NaN; the scan geometry is
|
||||
// unchanged. Apply after invert_scan(), before remap_scan_window() (it needs
|
||||
// the real angles).
|
||||
inline void apply_fov_window(LaserScan& scan, float min_deg, float max_deg) {
|
||||
const float min_rad = min_deg * kDeg2Rad;
|
||||
const float max_rad = max_deg * kDeg2Rad;
|
||||
constexpr float kPi = 3.14159265358979323846f;
|
||||
for (size_t i = 0; i < scan.ranges.size(); ++i) {
|
||||
// Normalize into (-pi, pi]: OLEI scans unwrap continuously and may
|
||||
// exceed the seam.
|
||||
float a = scan.angle_min + static_cast<float>(i) * scan.angle_increment;
|
||||
a = std::fmod(a, 2.f * kPi);
|
||||
if (a > kPi) a -= 2.f * kPi;
|
||||
if (a < -kPi) a += 2.f * kPi;
|
||||
if (a < min_rad || a > max_rad)
|
||||
scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
|
||||
}
|
||||
}
|
||||
|
||||
// Apply the generic DeviceConfig windows/overrides onto a model preset —
|
||||
// every plugin's create_driver_instance() funnels through this.
|
||||
inline ModelConfig apply_device_config(const ModelConfig& preset, const DeviceConfig& cfg) {
|
||||
ModelConfig mc = preset;
|
||||
if (cfg.range_min_m > 0.f) mc.range_min_m = cfg.range_min_m;
|
||||
if (cfg.range_max_m > 0.f) mc.range_max_m = cfg.range_max_m;
|
||||
if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
|
||||
mc.fov_filter = true;
|
||||
mc.fov_min_deg = cfg.angle_min_deg;
|
||||
mc.fov_max_deg = cfg.angle_max_deg;
|
||||
}
|
||||
if (cfg.remap_angle_min_deg > -360.f || cfg.remap_angle_max_deg < 360.f) {
|
||||
mc.remap_angles = true;
|
||||
mc.out_angle_min = cfg.remap_angle_min_deg;
|
||||
mc.out_angle_max = cfg.remap_angle_max_deg;
|
||||
}
|
||||
return mc;
|
||||
}
|
||||
|
||||
// Standard finalize sequence shared by the drivers; call once per completed
|
||||
// scan, after ranges/intensities/angles are filled in device order.
|
||||
inline void finalize_scan(LaserScan& scan, const ModelConfig& cfg, bool inverted) {
|
||||
if (inverted)
|
||||
invert_scan(scan);
|
||||
if (cfg.fov_filter)
|
||||
apply_fov_window(scan, cfg.fov_min_deg, cfg.fov_max_deg);
|
||||
if (cfg.remap_angles)
|
||||
remap_scan_window(scan, cfg.out_angle_min, cfg.out_angle_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;
|
||||
}
|
||||
|
||||
// Non-blocking connect with a bounded timeout — a blocking connect() to an
|
||||
// unreachable device would stall for the OS default (~2 min on Linux).
|
||||
// Enables TCP_NODELAY on success; the fd is returned to blocking mode either
|
||||
// way. The caller owns the fd and closes it on failure.
|
||||
inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) {
|
||||
int flags = ::fcntl(fd, F_GETFL, 0);
|
||||
::fcntl(fd, F_SETFL, flags | O_NONBLOCK);
|
||||
|
||||
ErrorCode conn_err = ErrorCode::Ok;
|
||||
int rc = ::connect(fd, reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
|
||||
if (rc < 0 && errno != EINPROGRESS) {
|
||||
conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
|
||||
: ErrorCode::ConnectionFailed;
|
||||
} else if (rc < 0) {
|
||||
fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds);
|
||||
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
|
||||
rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv);
|
||||
if (rc == 0) {
|
||||
conn_err = ErrorCode::Timeout;
|
||||
} else if (rc < 0) {
|
||||
conn_err = ErrorCode::ConnectionFailed;
|
||||
} else {
|
||||
int err = 0; socklen_t errlen = sizeof(err);
|
||||
::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen);
|
||||
if (err != 0)
|
||||
conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
|
||||
: ErrorCode::ConnectionFailed;
|
||||
}
|
||||
}
|
||||
::fcntl(fd, F_SETFL, flags);
|
||||
|
||||
if (conn_err == ErrorCode::Ok) {
|
||||
int nodelay = 1;
|
||||
::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
|
||||
}
|
||||
return conn_err;
|
||||
}
|
||||
|
||||
} // namespace xlidar
|
||||
1
plugins/driver_espe/CMakeLists.txt
Normal file
1
plugins/driver_espe/CMakeLists.txt
Normal file
@@ -0,0 +1 @@
|
||||
xlidar_add_plugin(driver_espe espe_driver.cpp)
|
||||
299
plugins/driver_espe/espe_driver.cpp
Normal file
299
plugins/driver_espe/espe_driver.cpp
Normal file
@@ -0,0 +1,299 @@
|
||||
// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP.
|
||||
#include "espe_driver.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#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 {
|
||||
// "RAuto" + fixed tail — puts the device into continuous measurement output.
|
||||
constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};
|
||||
|
||||
constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
|
||||
constexpr char kAreaMagic[5] = {'W', 'S', 'i', 'm', 'u'};
|
||||
|
||||
constexpr size_t kRangeHeaderSize = 16; // magic + 6 big-endian u16 fields
|
||||
constexpr size_t kAreaFrameSize = 13; // magic + 4 status bytes + err u16 + crc u16
|
||||
constexpr uint16_t kMaxDistanceMm = 50000; // wire sentinel: beyond = no return
|
||||
constexpr uint16_t kMaxIntensity = 30000;
|
||||
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
|
||||
constexpr int kConnectTimeoutMs = 2000;
|
||||
|
||||
uint16_t be16(const uint8_t* p) {
|
||||
return static_cast<uint16_t>((p[0] << 8) | p[1]);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
EspeDriver::EspeDriver(const ModelConfig& cfg, const std::string& ip,
|
||||
uint16_t port, bool use_udp, bool inverted)
|
||||
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip),
|
||||
port_(port), use_udp_(use_udp), inverted_(inverted) {}
|
||||
|
||||
EspeDriver::~EspeDriver() { close(); }
|
||||
|
||||
ErrorCode EspeDriver::open() {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
|
||||
return set_error(ErrorCode::InvalidAddress);
|
||||
|
||||
sock_fd_ = ::socket(AF_INET, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
|
||||
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
|
||||
|
||||
ErrorCode conn_err = ErrorCode::Ok;
|
||||
if (use_udp_) {
|
||||
// connect() on UDP just fixes the peer; replies come to our port.
|
||||
if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
|
||||
conn_err = ErrorCode::ConnectionFailed;
|
||||
} else {
|
||||
conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
|
||||
}
|
||||
|
||||
if (conn_err != ErrorCode::Ok) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return set_error(conn_err);
|
||||
}
|
||||
|
||||
recv_buf_.clear();
|
||||
points_total_ = 0;
|
||||
pending_time_ = 0;
|
||||
scan_ready_ = false;
|
||||
espe_error_status_.reset();
|
||||
latest_diag_ = Diagnostics{};
|
||||
|
||||
// Device is passive until told to stream.
|
||||
ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
|
||||
if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
|
||||
close();
|
||||
return set_error(ErrorCode::HandshakeFailed);
|
||||
}
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void EspeDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool EspeDriver::fill_buffer(int timeout_ms) {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
|
||||
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 false;
|
||||
}
|
||||
}
|
||||
|
||||
char buf[4096];
|
||||
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
|
||||
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
|
||||
recv_buf_.append(buf, static_cast<size_t>(n));
|
||||
return true;
|
||||
}
|
||||
|
||||
// Consume complete frames from recv_buf_; returns true once a full revolution
|
||||
// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
|
||||
bool EspeDriver::parse_buffer() {
|
||||
for (;;) {
|
||||
size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
|
||||
size_t area_pos = recv_buf_.find(kAreaMagic, 0, sizeof(kAreaMagic));
|
||||
size_t pos = std::min(range_pos, area_pos);
|
||||
if (pos == std::string::npos) {
|
||||
// No magic in sight: keep only a possible partial magic at the tail.
|
||||
if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
|
||||
recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
|
||||
return scan_ready_;
|
||||
}
|
||||
if (pos > 0) recv_buf_.erase(0, pos);
|
||||
|
||||
const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());
|
||||
|
||||
if (area_pos < range_pos) {
|
||||
if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
|
||||
// Zone/obstacle frame — only sent when the host polls areas, but
|
||||
// it carries the device fault word, so latch it if it appears.
|
||||
// Byte order unverified on hardware: the protocol is mixed-endian
|
||||
// (header fields big-endian, point payload little-endian) and no
|
||||
// spec covers this field; little-endian assumed like the payload.
|
||||
espe_error_status_ = le16(d + 9);
|
||||
recv_buf_.erase(0, kAreaFrameSize);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
|
||||
uint16_t data_size = be16(d + 8);
|
||||
uint16_t measure_size = be16(d + 12);
|
||||
if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
|
||||
recv_buf_.erase(0, sizeof(kRangeMagic)); // bogus header — resync
|
||||
continue;
|
||||
}
|
||||
if (data_size > measure_size) data_size = measure_size;
|
||||
|
||||
size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
|
||||
if (recv_buf_.size() < frame_size) return scan_ready_;
|
||||
|
||||
handle_range_frame(d, data_size);
|
||||
recv_buf_.erase(0, frame_size);
|
||||
// Stop as soon as a revolution completes — draining further frames
|
||||
// could finish a second revolution and overwrite ready_result_ before
|
||||
// the caller consumes it. Leftover bytes wait for the next call.
|
||||
if (scan_ready_) return true;
|
||||
}
|
||||
}
|
||||
|
||||
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
|
||||
// data_size (points in this frame), data_position (cumulative points incl.
|
||||
// this frame), measure_size (points per revolution), time; then data_size ×
|
||||
// 4 B little-endian (u16 distance mm, u16 intensity).
|
||||
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
|
||||
uint16_t start_angle = be16(frame + 4);
|
||||
uint16_t end_angle = be16(frame + 6);
|
||||
uint16_t data_position = be16(frame + 10);
|
||||
uint16_t measure_size = be16(frame + 12);
|
||||
pending_time_ = be16(frame + 14);
|
||||
|
||||
// First frame of a revolution (or geometry changed) → start a new one.
|
||||
if (points_total_ != measure_size || data_position <= data_size) {
|
||||
points_total_ = measure_size;
|
||||
rev_start_deg_ = static_cast<float>(start_angle);
|
||||
angle_inc_deg_ = static_cast<float>(end_angle - start_angle) / measure_size;
|
||||
pending_ranges_.assign(points_total_, 0.f);
|
||||
pending_intensities_.assign(points_total_, 0.f);
|
||||
}
|
||||
if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }
|
||||
|
||||
// start_angle is normally constant across the revolution, so this is just
|
||||
// the cumulative position; the angle term covers firmware that advances it.
|
||||
int32_t begin = static_cast<int32_t>(std::lround(
|
||||
(static_cast<float>(start_angle) - rev_start_deg_) / angle_inc_deg_))
|
||||
+ static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
|
||||
|
||||
const uint8_t* p = frame + kRangeHeaderSize;
|
||||
for (uint16_t i = 0; i < data_size; ++i, p += 4) {
|
||||
int32_t idx = begin + i;
|
||||
if (idx < 0 || idx >= static_cast<int32_t>(points_total_)) continue;
|
||||
uint16_t dist = le16(p + 0);
|
||||
uint16_t inten = le16(p + 2);
|
||||
pending_ranges_[idx] = (dist > kMaxDistanceMm)
|
||||
? std::numeric_limits<float>::infinity()
|
||||
: static_cast<float>(dist) * 1e-3f; // mm -> m
|
||||
// Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
|
||||
pending_intensities_[idx] =
|
||||
static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
|
||||
* (255.f / kMaxIntensity);
|
||||
}
|
||||
|
||||
if (data_position >= points_total_) finish_scan();
|
||||
}
|
||||
|
||||
void EspeDriver::finish_scan() {
|
||||
LaserScan& scan = ready_result_.scan;
|
||||
scan = LaserScan{};
|
||||
scan.timestamp_ms = pending_time_; // header "time" field, unit unverified
|
||||
scan.ranges = std::move(pending_ranges_);
|
||||
scan.intensities = std::move(pending_intensities_);
|
||||
scan.angle_min = (rev_start_deg_ + cfg_.angle_offset_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.range_min = cfg_.range_min_m;
|
||||
scan.range_max = cfg_.range_max_m;
|
||||
|
||||
finalize_scan(scan, cfg_, inverted_);
|
||||
|
||||
ExtraInfo& info = ready_result_.info;
|
||||
info = ExtraInfo{};
|
||||
info.detected_model = cfg_.name;
|
||||
info.espe_error_status = espe_error_status_;
|
||||
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
|
||||
mark_scan_decoded();
|
||||
|
||||
pending_ranges_.clear();
|
||||
pending_intensities_.clear();
|
||||
points_total_ = 0;
|
||||
scan_ready_ = true;
|
||||
}
|
||||
|
||||
bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
for (;;) {
|
||||
if (parse_buffer()) {
|
||||
scan_ready_ = false;
|
||||
out = std::move(ready_result_);
|
||||
set_error(ErrorCode::Ok);
|
||||
return true;
|
||||
}
|
||||
if (!fill_buffer(timeout_ms)) return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool EspeDriver::spin_once() {
|
||||
if (!parse_buffer()) {
|
||||
if (!fill_buffer(0)) return false;
|
||||
parse_buffer();
|
||||
}
|
||||
if (scan_ready_) {
|
||||
scan_ready_ = false;
|
||||
if (cb_) cb_(ready_result_);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// ── plugin registration ─────────────────────────────────────────────────────
|
||||
|
||||
namespace {
|
||||
|
||||
const DriverInfo kDriverInfo = [] {
|
||||
DriverInfo info;
|
||||
info.vendor = "ESPE";
|
||||
info.model = "LGA60";
|
||||
info.driver_id = "espe_lga60_driver";
|
||||
info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
|
||||
"use_udp; open() sends the RAuto start command; device "
|
||||
"parameters come from the vendor Windows tool. Default "
|
||||
"port 8080 (vendor default IP 192.168.1.88). Ported from "
|
||||
"the vendor ROS driver; not verified on real hardware.";
|
||||
info.transport = Transport::Tcp;
|
||||
info.transport_selectable = true; // use_udp switches to UDP
|
||||
info.supported_models = {"ESPE-LGA60"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
} // namespace
|
||||
|
||||
DriverInfo EspeDriver::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 : 8080;
|
||||
return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
|
||||
cfg->ip, port, cfg->use_udp, cfg->inverted);
|
||||
}
|
||||
87
plugins/driver_espe/espe_driver.hpp
Normal file
87
plugins/driver_espe/espe_driver.hpp
Normal file
@@ -0,0 +1,87 @@
|
||||
// ESPE LGA60 laser scanner over TCP or UDP — plugin-private header.
|
||||
#pragma once
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// ESPE LGA60-320: 320° FOV, device sweeps 20°..340° with 0° at the rear
|
||||
// (angle_offset_deg = -180 so output 0° = ahead). Range per datasheet page;
|
||||
// the wire caps distance at 50000 mm.
|
||||
inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.05f, 50.f, -180.f };
|
||||
|
||||
// ESPE LGA60 over TCP (default port 8080) or UDP, ported from the vendor's
|
||||
// ROS driver; NOT verified on real hardware. open() sends the "RAuto" start
|
||||
// command; device parameters (spin rate, resolution, filters) are whatever
|
||||
// the vendor Windows config tool programmed — this driver does not set them.
|
||||
class EspeDriver : public LidarDriverInterface {
|
||||
public:
|
||||
// ip: device address; use_udp selects the transport the device is
|
||||
// configured for (vendor default is TCP); inverted: unit mounted
|
||||
// upside-down → mirror the scan.
|
||||
explicit EspeDriver(const ModelConfig& cfg,
|
||||
const std::string& ip,
|
||||
uint16_t port = 8080,
|
||||
bool use_udp = false,
|
||||
bool inverted = false);
|
||||
~EspeDriver();
|
||||
|
||||
EspeDriver(const EspeDriver&) = delete;
|
||||
EspeDriver& operator=(const EspeDriver&) = delete;
|
||||
|
||||
DriverInfo get_driver_info() const override;
|
||||
|
||||
// Connect + send the start-capture command.
|
||||
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:
|
||||
bool fill_buffer(int timeout_ms); // one recv() into recv_buf_
|
||||
bool parse_buffer(); // consume frames; true when a scan completed
|
||||
void handle_range_frame(const uint8_t* frame, uint16_t data_size);
|
||||
void finish_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool use_udp_ = false;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Stream bytes carried across frame boundaries; per-instance.
|
||||
std::string recv_buf_;
|
||||
|
||||
// Per-revolution accumulation
|
||||
std::vector<float> pending_ranges_;
|
||||
std::vector<float> pending_intensities_;
|
||||
float rev_start_deg_ = 0.f; // device angle of the revolution's first point
|
||||
float angle_inc_deg_ = 0.f;
|
||||
uint32_t points_total_ = 0; // measure_size from the header; 0 = no rev open
|
||||
uint16_t pending_time_ = 0; // header "time" field, unit unverified
|
||||
|
||||
// Latched from the newest "WSimu" area frame, if the device sends any.
|
||||
std::optional<uint16_t> espe_error_status_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by finish_scan().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
1
plugins/driver_olei/CMakeLists.txt
Normal file
1
plugins/driver_olei/CMakeLists.txt
Normal file
@@ -0,0 +1 @@
|
||||
xlidar_add_plugin(driver_olei olei_driver.cpp)
|
||||
457
plugins/driver_olei/olei_driver.cpp
Normal file
457
plugins/driver_olei/olei_driver.cpp
Normal file
@@ -0,0 +1,457 @@
|
||||
// OLEI 2D lidars over UDP — Family A (0xFAF0), Family B (0xFEF0) and
|
||||
// Family C / Protocol V3 (0xFEAC, GS1-5) packet parsing.
|
||||
#include "olei_driver.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <cerrno>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// 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;
|
||||
if (deg > 180.f) deg -= 360.f;
|
||||
return deg;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
OleiDriver::OleiDriver(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);
|
||||
}
|
||||
|
||||
OleiDriver::~OleiDriver() { close(); }
|
||||
|
||||
ErrorCode OleiDriver::open() {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
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);
|
||||
|
||||
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
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
// Reset per-revolution state so a close()/open() cycle starts clean.
|
||||
pending_angle_deg_.clear();
|
||||
pending_dist_m_.clear();
|
||||
pending_intensity_.clear();
|
||||
pending_info_ = ExtraInfo{};
|
||||
latest_diag_ = Diagnostics{};
|
||||
last_angle_ = -1.f;
|
||||
scan_ready_ = false;
|
||||
|
||||
pending_angle_deg_.reserve(2048);
|
||||
pending_dist_m_.reserve(2048);
|
||||
pending_intensity_.reserve(2048);
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void OleiDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool OleiDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
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) {
|
||||
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (!poll_packet()) return false;
|
||||
}
|
||||
out = std::move(ready_result_);
|
||||
set_error(ErrorCode::Ok);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OleiDriver::spin_once() {
|
||||
if (!poll_packet()) return false;
|
||||
if (scan_ready_) {
|
||||
scan_ready_ = false;
|
||||
if (cb_) cb_(ready_result_);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OleiDriver::poll_packet() {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
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) { set_error(ErrorCode::DeviceDisconnected); return false; }
|
||||
|
||||
// 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));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
|
||||
void OleiDriver::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);
|
||||
}
|
||||
|
||||
void OleiDriver::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;
|
||||
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());
|
||||
|
||||
// Inversion already happened per point (maybe_invert), so inverted=false.
|
||||
finalize_scan(scan, cfg_, /*inverted=*/false);
|
||||
|
||||
ExtraInfo& info = ready_result_.info;
|
||||
info = pending_info_;
|
||||
info.detected_model = detected_model_name_;
|
||||
info.error_status = pending_err_;
|
||||
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
|
||||
mark_scan_decoded();
|
||||
|
||||
pending_angle_deg_.clear();
|
||||
pending_dist_m_.clear();
|
||||
pending_intensity_.clear();
|
||||
pending_info_ = ExtraInfo{};
|
||||
scan_ready_ = true;
|
||||
}
|
||||
|
||||
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
|
||||
bool OleiDriver::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;
|
||||
|
||||
uint8_t dist_scale = buf[4]; // mm per count
|
||||
uint8_t err_status = buf[5];
|
||||
float ang_start = static_cast<float>(buf[6]);
|
||||
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);
|
||||
|
||||
int block_bytes = len - HEADER_LEN;
|
||||
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;
|
||||
|
||||
if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
|
||||
flush_scan();
|
||||
}
|
||||
|
||||
pending_ts_ = timestamp;
|
||||
pending_err_ = err_status;
|
||||
pending_info_.distance_scale_mm = dist_scale;
|
||||
pending_info_.rotation_raw = rotation_raw;
|
||||
|
||||
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];
|
||||
|
||||
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) + cfg_.angle_offset_deg);
|
||||
angle = maybe_invert(angle, inverted_);
|
||||
|
||||
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
|
||||
|
||||
push_point(angle, dist_raw * scale_mm * 0.001f, intensity);
|
||||
}
|
||||
|
||||
last_angle_ = ang_start;
|
||||
return true;
|
||||
}
|
||||
|
||||
// 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 OleiDriver::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];
|
||||
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;
|
||||
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int num_pts = (len - HEADER_LEN) / BLOCK_LEN;
|
||||
if (num_pts <= 0) return false;
|
||||
|
||||
const uint8_t* blk = buf + HEADER_LEN;
|
||||
// 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;
|
||||
|
||||
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f);
|
||||
if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
|
||||
flush_scan();
|
||||
}
|
||||
last_angle_ = dev_deg;
|
||||
|
||||
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;
|
||||
|
||||
push_point(angle, dist_m, intensity);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// 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 OleiDriver::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;
|
||||
|
||||
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;
|
||||
|
||||
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 == 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);
|
||||
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 + 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,
|
||||
has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
|
||||
}
|
||||
|
||||
last_angle_ = raw_first_angle;
|
||||
return true;
|
||||
}
|
||||
|
||||
// ── plugin registration ─────────────────────────────────────────────────────
|
||||
|
||||
namespace {
|
||||
|
||||
const DriverInfo kDriverInfo = [] {
|
||||
DriverInfo info;
|
||||
info.vendor = "OLEI";
|
||||
info.model = "2D series (VB/VF/LR-1x/GS1-5)";
|
||||
info.driver_id = "olei_lidar_driver";
|
||||
info.description = "OLEI 2D lidars over UDP — auto-detects the Family A/B/C "
|
||||
"protocol per packet; model AUTO self-detects from the "
|
||||
"stream (Family B/C). Default port 2368.";
|
||||
info.transport = Transport::Udp;
|
||||
info.supported_models = {"AUTO", "VB", "VF", "LR-1F", "LR-1FMI", "LR-1BS5",
|
||||
"LR-16F", "GS1-5"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
const ModelConfig* model_by_name(const std::string& name) {
|
||||
static constexpr const ModelConfig* kModels[] = {
|
||||
&MODEL_AUTO, &MODEL_VB, &MODEL_VF, &MODEL_LR1F, &MODEL_LR1FMI,
|
||||
&MODEL_LR1BS5, &MODEL_LR16F, &MODEL_GS15,
|
||||
};
|
||||
for (const ModelConfig* m : kModels)
|
||||
if (name == m->name) return m;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DriverInfo OleiDriver::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 ModelConfig* preset = model_by_name(cfg->model);
|
||||
if (!preset) preset = &MODEL_AUTO; // unknown model → auto-detect
|
||||
const uint16_t port = cfg->port ? cfg->port : 2368;
|
||||
return new OleiDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
|
||||
}
|
||||
89
plugins/driver_olei/olei_driver.hpp
Normal file
89
plugins/driver_olei/olei_driver.hpp
Normal file
@@ -0,0 +1,89 @@
|
||||
// OLEI 2D lidars over UDP (Family A / B / C protocols) — plugin-private header.
|
||||
#pragma once
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// Model presets. Family B/C packets carry enough to auto-detect the model;
|
||||
// Family A doesn't, so MODEL_AUTO keeps the wide default FOV.
|
||||
inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270°
|
||||
inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
||||
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B)
|
||||
inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line
|
||||
inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family C/V3)
|
||||
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
|
||||
|
||||
// OLEI UDP driver.
|
||||
class OleiDriver : public LidarDriverInterface {
|
||||
public:
|
||||
// ip: local bind address; port: UDP port the lidar sends to;
|
||||
// inverted: unit mounted upside-down → mirror every angle.
|
||||
explicit OleiDriver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 2368,
|
||||
bool inverted = false);
|
||||
~OleiDriver();
|
||||
|
||||
OleiDriver(const OleiDriver&) = delete;
|
||||
OleiDriver& operator=(const OleiDriver&) = 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; }
|
||||
|
||||
// Model name read from the Family B/C header; "AUTO" until one is seen.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
bool poll_packet(); // one recvfrom() + dispatch to the family parser
|
||||
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
|
||||
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
|
||||
bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5)
|
||||
|
||||
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
|
||||
void flush_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-revolution accumulation buffers (index-aligned)
|
||||
std::vector<float> pending_angle_deg_;
|
||||
std::vector<float> pending_dist_m_;
|
||||
std::vector<uint8_t> pending_intensity_;
|
||||
uint32_t pending_ts_ = 0;
|
||||
uint8_t pending_err_ = 0;
|
||||
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
||||
|
||||
ExtraInfo pending_info_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by flush_scan().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
|
||||
// Per-instance so two drivers on two threads don't race.
|
||||
uint8_t recv_buf_[4096];
|
||||
|
||||
bool auto_detect_ = false;
|
||||
bool model_locked_ = false;
|
||||
std::string detected_model_name_ = "AUTO";
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
35
plugins/driver_rplidar/CMakeLists.txt
Normal file
35
plugins/driver_rplidar/CMakeLists.txt
Normal file
@@ -0,0 +1,35 @@
|
||||
# The rplidar plugin compiles the vendor SDK straight into the plugin .so.
|
||||
# XLIDAR_RPLIDAR_SDK_DIR must point at a directory holding include/ + src/;
|
||||
# when unset, common local layouts are probed. Without an SDK the plugin is
|
||||
# skipped (the rest of the build is unaffected).
|
||||
|
||||
if(NOT XLIDAR_RPLIDAR_SDK_DIR)
|
||||
foreach(candidate
|
||||
${CMAKE_SOURCE_DIR}/third_party/rplidar_sdk
|
||||
${CMAKE_SOURCE_DIR}/../rplidar_sdk/sdk
|
||||
${CMAKE_SOURCE_DIR}/../xloc-monorepo/xlocd/deps/rplidar_sdk)
|
||||
if(EXISTS ${candidate}/include/sl_lidar.h)
|
||||
set(XLIDAR_RPLIDAR_SDK_DIR ${candidate})
|
||||
break()
|
||||
endif()
|
||||
endforeach()
|
||||
endif()
|
||||
|
||||
if(NOT XLIDAR_RPLIDAR_SDK_DIR OR NOT EXISTS ${XLIDAR_RPLIDAR_SDK_DIR}/include/sl_lidar.h)
|
||||
message(WARNING "driver_rplidar: Slamtec SDK not found "
|
||||
"(set -DXLIDAR_RPLIDAR_SDK_DIR=...) — plugin skipped")
|
||||
return()
|
||||
endif()
|
||||
|
||||
message(STATUS "driver_rplidar: using SDK at ${XLIDAR_RPLIDAR_SDK_DIR}")
|
||||
|
||||
file(GLOB_RECURSE RPLIDAR_SDK_SOURCES CONFIGURE_DEPENDS
|
||||
${XLIDAR_RPLIDAR_SDK_DIR}/src/*.cpp)
|
||||
# The SDK ships win32/macOS arch files; keep Linux only.
|
||||
list(FILTER RPLIDAR_SDK_SOURCES EXCLUDE REGEX "arch/(win32|macOS)/")
|
||||
|
||||
xlidar_add_plugin(driver_rplidar rplidar_driver.cpp ${RPLIDAR_SDK_SOURCES})
|
||||
target_include_directories(driver_rplidar SYSTEM PRIVATE
|
||||
${XLIDAR_RPLIDAR_SDK_DIR}/include
|
||||
${XLIDAR_RPLIDAR_SDK_DIR}/src
|
||||
)
|
||||
326
plugins/driver_rplidar/rplidar_driver.cpp
Normal file
326
plugins/driver_rplidar/rplidar_driver.cpp
Normal file
@@ -0,0 +1,326 @@
|
||||
// Slamtec RPLIDAR over serial (C1 defaults), built on the vendor SDK
|
||||
// (sl_lidar.h). Ported from xlocd's embedded rplidar driver — the scan math
|
||||
// (angle/distance decoding, inversion, FOV window, NaN invalid points) is
|
||||
// kept identical.
|
||||
//
|
||||
// Unlike the network drivers, angles are reported in the DEVICE frame
|
||||
// [0, 2π), 0 = ahead, ascending — exactly what the SDK's ascendScanData
|
||||
// yields (and what xlocd's engine expects).
|
||||
#include "lidar_interface.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <filesystem>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "sl_lidar.h"
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
namespace {
|
||||
|
||||
// Node buffer for one grab. 8192 is the SDK-recommended size, far above the
|
||||
// ~400-500 points/rev of a C1 in DenseBoost mode.
|
||||
constexpr std::size_t kMaxNodesPerScan = 8192;
|
||||
constexpr float kPi = 3.14159265358979323846F;
|
||||
constexpr float kTwoPi = 2.0F * kPi;
|
||||
// C1 default range (datasheet: 12 m on white; 16 m ceiling matches the
|
||||
// common rplidar_ros configuration). Overridable via DeviceConfig range_*.
|
||||
constexpr float kDefaultRangeMinM = 0.05F;
|
||||
constexpr float kDefaultRangeMaxM = 16.0F;
|
||||
// Nominal rotation period (~10 Hz) for the first frame, before a real
|
||||
// grab-to-grab interval has been measured.
|
||||
constexpr float kDefaultScanTimeS = 0.1F;
|
||||
constexpr int kDefaultGrabTimeoutMs = 2000; // SDK default
|
||||
|
||||
inline constexpr ModelConfig MODEL_RPLIDAR_C1 { "C1", -180.f, 180.f, kDefaultRangeMinM, kDefaultRangeMaxM };
|
||||
|
||||
// HQ node angle: angle_z_q14 is [0..360) fixed-point Q14 on a 90° scale.
|
||||
float node_angle_rad(const sl_lidar_response_measurement_node_hq_t& node) {
|
||||
return static_cast<float>(node.angle_z_q14) * 90.0F / (1 << 14) * kDeg2Rad;
|
||||
}
|
||||
|
||||
// HQ node distance: dist_mm_q2 is mm in Q2 (1/4 mm) -> metres.
|
||||
float node_distance_m(const sl_lidar_response_measurement_node_hq_t& node) {
|
||||
return static_cast<float>(node.dist_mm_q2) / 4.0F / 1000.0F;
|
||||
}
|
||||
|
||||
// Device angle [0, 2π) -> signed (-180, 180] degrees (0 = ahead, + = left),
|
||||
// to compare against the configured FOV window.
|
||||
float to_signed_deg(float angle_rad) {
|
||||
float deg = angle_rad / kDeg2Rad;
|
||||
if (deg > 180.0F) deg -= 360.0F;
|
||||
return deg;
|
||||
}
|
||||
|
||||
const DriverInfo kDriverInfo = [] {
|
||||
DriverInfo info;
|
||||
info.vendor = "Slamtec";
|
||||
info.model = "C1";
|
||||
info.driver_id = "rplidar_c1_driver";
|
||||
info.description = "Slamtec RPLIDAR over serial, built on the vendor SDK — "
|
||||
"defaults match the C1 (CP2102N UART bridge, baud "
|
||||
"460800); other SDK-compatible serial models (A/S "
|
||||
"series) work with the matching baud rate. Health check "
|
||||
"at open(); model/firmware auto-detected.";
|
||||
info.transport = Transport::Serial;
|
||||
info.supported_models = {"AUTO", "C1"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
} // namespace
|
||||
|
||||
class RplidarDriver : public LidarDriverInterface {
|
||||
public:
|
||||
RplidarDriver(const ModelConfig& cfg, std::string serial_port, uint32_t baudrate,
|
||||
bool inverted)
|
||||
: cfg_(cfg), serial_port_(std::move(serial_port)), baudrate_(baudrate),
|
||||
inverted_(inverted) {}
|
||||
|
||||
~RplidarDriver() override { close(); }
|
||||
|
||||
RplidarDriver(const RplidarDriver&) = delete;
|
||||
RplidarDriver& operator=(const RplidarDriver&) = delete;
|
||||
|
||||
DriverInfo get_driver_info() const override { return kDriverInfo; }
|
||||
|
||||
// Full connect sequence; each step maps to one ErrorCode:
|
||||
// device present (SerialError) -> serial channel (SerialError) -> SDK
|
||||
// driver (SerialError) -> connect (ConnectionFailed) -> device info
|
||||
// (non-fatal, fills model/firmware) -> health check (DeviceError on
|
||||
// fault) -> motor + startScan typical mode (HandshakeFailed).
|
||||
ErrorCode open() override {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
std::error_code fs_error;
|
||||
if (!std::filesystem::exists(serial_port_, fs_error))
|
||||
return set_error(ErrorCode::SerialError);
|
||||
|
||||
auto channel = sl::createSerialPortChannel(serial_port_, static_cast<int>(baudrate_));
|
||||
if (!channel) return set_error(ErrorCode::SerialError);
|
||||
channel_ = *channel;
|
||||
|
||||
auto lidar = sl::createLidarDriver();
|
||||
if (!lidar) { disconnect(); return set_error(ErrorCode::SerialError); }
|
||||
lidar_ = *lidar;
|
||||
|
||||
if (!SL_IS_OK(lidar_->connect(channel_))) {
|
||||
disconnect();
|
||||
return set_error(ErrorCode::ConnectionFailed);
|
||||
}
|
||||
|
||||
// Identification — failure here is non-fatal (fields stay empty).
|
||||
detected_model_name_ = cfg_.name;
|
||||
firmware_.clear();
|
||||
sl_lidar_response_device_info_t info{};
|
||||
if (SL_IS_OK(lidar_->getDeviceInfo(info))) {
|
||||
char model_buf[32];
|
||||
std::snprintf(model_buf, sizeof(model_buf), "slamtec-0x%02X",
|
||||
static_cast<unsigned>(info.model));
|
||||
char firmware_buf[48];
|
||||
std::snprintf(firmware_buf, sizeof(firmware_buf), "fw %u.%02u hw %u",
|
||||
static_cast<unsigned>(info.firmware_version >> 8),
|
||||
static_cast<unsigned>(info.firmware_version & 0xFF),
|
||||
static_cast<unsigned>(info.hardware_version));
|
||||
detected_model_name_ = model_buf;
|
||||
firmware_ = firmware_buf;
|
||||
}
|
||||
|
||||
// Mandatory health check: a self-reported Fault means the data is not
|
||||
// usable; Warning still runs but stays visible in diagnostics.
|
||||
sl_lidar_response_device_health_t health{};
|
||||
if (!SL_IS_OK(lidar_->getHealth(health)) || health.status == SL_LIDAR_STATUS_ERROR) {
|
||||
health_status_ = SL_LIDAR_STATUS_ERROR;
|
||||
health_error_code_ = static_cast<uint16_t>(health.error_code);
|
||||
refresh_diag_from_health();
|
||||
disconnect();
|
||||
return set_error(ErrorCode::DeviceError);
|
||||
}
|
||||
health_status_ = health.status;
|
||||
health_error_code_ = static_cast<uint16_t>(health.error_code);
|
||||
refresh_diag_from_health();
|
||||
|
||||
// C1 spins the motor on the scan command; setMotorSpeed stays for
|
||||
// DTR-controlled models (A-series).
|
||||
(void)lidar_->setMotorSpeed();
|
||||
sl::LidarScanMode scan_mode{};
|
||||
if (!SL_IS_OK(lidar_->startScan(false, true, 0, &scan_mode))) {
|
||||
(void)lidar_->setMotorSpeed(0);
|
||||
disconnect();
|
||||
return set_error(ErrorCode::HandshakeFailed);
|
||||
}
|
||||
|
||||
have_last_grab_ = false;
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void close() override {
|
||||
if (lidar_ != nullptr) {
|
||||
(void)lidar_->stop();
|
||||
(void)lidar_->setMotorSpeed(0);
|
||||
}
|
||||
disconnect();
|
||||
}
|
||||
|
||||
// Blocks until the SDK hands over one full revolution.
|
||||
bool recv_scan(ScanResult& out, int timeout_ms) override {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
|
||||
std::vector<sl_lidar_response_measurement_node_hq_t> nodes(kMaxNodesPerScan);
|
||||
std::size_t count = nodes.size();
|
||||
const auto grabbed = lidar_->grabScanDataHq(
|
||||
nodes.data(), count,
|
||||
timeout_ms > 0 ? static_cast<sl_u32>(timeout_ms) : kDefaultGrabTimeoutMs);
|
||||
if (!SL_IS_OK(grabbed) || count < 2) {
|
||||
set_error(grabbed == SL_RESULT_OPERATION_TIMEOUT ? ErrorCode::Timeout
|
||||
: ErrorCode::DeviceDisconnected);
|
||||
return false;
|
||||
}
|
||||
(void)lidar_->ascendScanData(nodes.data(), count);
|
||||
|
||||
// Real rotation period = interval between consecutive grabs (~86 ms
|
||||
// on a C1); the first frame uses the nominal value.
|
||||
const auto grab_time = std::chrono::steady_clock::now();
|
||||
const float scan_time = have_last_grab_
|
||||
? std::chrono::duration<float>(grab_time - last_grab_).count()
|
||||
: kDefaultScanTimeS;
|
||||
last_grab_ = grab_time;
|
||||
have_last_grab_ = true;
|
||||
|
||||
const float angle_first = node_angle_rad(nodes.front());
|
||||
const float angle_last = node_angle_rad(nodes[count - 1]);
|
||||
if (angle_last <= angle_first) {
|
||||
set_error(ErrorCode::Timeout); // malformed revolution — treat as a miss
|
||||
return false;
|
||||
}
|
||||
|
||||
LaserScan& scan = out.scan;
|
||||
scan = LaserScan{};
|
||||
// Inverted mount -> mirror the angles (angle' = 2π - angle) and walk
|
||||
// the nodes backwards to keep ascending order.
|
||||
if (inverted_) {
|
||||
scan.angle_min = kTwoPi - angle_last;
|
||||
scan.angle_max = kTwoPi - angle_first;
|
||||
} else {
|
||||
scan.angle_min = angle_first;
|
||||
scan.angle_max = angle_last;
|
||||
}
|
||||
scan.angle_increment = (scan.angle_max - scan.angle_min) / static_cast<float>(count - 1);
|
||||
scan.scan_time = scan_time;
|
||||
scan.time_increment = scan_time / static_cast<float>(count);
|
||||
scan.range_min = cfg_.range_min_m;
|
||||
scan.range_max = cfg_.range_max_m;
|
||||
|
||||
// Valid FOV window — only filter when narrower than the full circle.
|
||||
const bool apply_angle_window =
|
||||
cfg_.fov_filter && (cfg_.fov_min_deg > -180.0F || cfg_.fov_max_deg < 180.0F);
|
||||
|
||||
scan.ranges.reserve(count);
|
||||
scan.intensities.reserve(count);
|
||||
for (std::size_t i = 0; i < count; ++i) {
|
||||
const std::size_t node_index = inverted_ ? count - 1 - i : i;
|
||||
// dist = 0 is the SDK's "no return" sentinel; together with
|
||||
// out-of-range / out-of-window points it becomes NaN.
|
||||
const float distance = node_distance_m(nodes[node_index]);
|
||||
bool valid = nodes[node_index].dist_mm_q2 != 0 &&
|
||||
distance >= scan.range_min && distance <= scan.range_max;
|
||||
if (valid && apply_angle_window) {
|
||||
const float grid_angle = scan.angle_min + scan.angle_increment * static_cast<float>(i);
|
||||
const float signed_deg = to_signed_deg(grid_angle);
|
||||
valid = signed_deg >= cfg_.fov_min_deg && signed_deg <= cfg_.fov_max_deg;
|
||||
}
|
||||
scan.ranges.push_back(valid ? distance : std::numeric_limits<float>::quiet_NaN());
|
||||
scan.intensities.push_back(static_cast<float>(nodes[node_index].quality));
|
||||
}
|
||||
|
||||
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 = detected_model_name_;
|
||||
info.rplidar_health_status = health_status_;
|
||||
info.rplidar_error_code = health_error_code_;
|
||||
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.firmware = firmware_;
|
||||
mark_scan_decoded();
|
||||
|
||||
set_error(ErrorCode::Ok);
|
||||
return true;
|
||||
}
|
||||
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
|
||||
// One unit of input == one revolution for this device.
|
||||
bool spin_once() override {
|
||||
ScanResult result;
|
||||
if (!recv_scan(result, kDefaultGrabTimeoutMs)) return false;
|
||||
if (cb_) cb_(result);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool is_open() const override { return lidar_ != nullptr; }
|
||||
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
// Health snapshot -> diagnostics, so a fault is visible before the first
|
||||
// scan (valid = true means "health was read", not "a scan was decoded").
|
||||
void refresh_diag_from_health() {
|
||||
ExtraInfo info;
|
||||
info.detected_model = detected_model_name_;
|
||||
info.rplidar_health_status = health_status_;
|
||||
info.rplidar_error_code = health_error_code_;
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.firmware = firmware_;
|
||||
}
|
||||
|
||||
// The SDK factories hand out raw pointers and require the caller to
|
||||
// delete them (see sl_lidar_driver.h) — this is the only place doing so.
|
||||
void disconnect() {
|
||||
if (lidar_ != nullptr) { delete lidar_; lidar_ = nullptr; }
|
||||
if (channel_ != nullptr) { delete channel_; channel_ = nullptr; }
|
||||
}
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string serial_port_;
|
||||
uint32_t baudrate_;
|
||||
bool inverted_ = false;
|
||||
ScanCallback cb_;
|
||||
|
||||
std::string detected_model_name_ = "AUTO";
|
||||
std::string firmware_;
|
||||
std::optional<uint8_t> health_status_;
|
||||
std::optional<uint16_t> health_error_code_;
|
||||
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
std::chrono::steady_clock::time_point last_grab_{};
|
||||
bool have_last_grab_ = false;
|
||||
|
||||
sl::ILidarDriver* lidar_ = nullptr;
|
||||
sl::IChannel* channel_ = nullptr;
|
||||
};
|
||||
|
||||
} // 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;
|
||||
// "AUTO" and "C1" share the same preset; the real model is read from the
|
||||
// device at open().
|
||||
const uint32_t baud = cfg->baudrate ? cfg->baudrate : 460800;
|
||||
return new RplidarDriver(apply_device_config(MODEL_RPLIDAR_C1, *cfg),
|
||||
cfg->serial_port, baud, cfg->inverted);
|
||||
}
|
||||
1
plugins/driver_sick_code/CMakeLists.txt
Normal file
1
plugins/driver_sick_code/CMakeLists.txt
Normal file
@@ -0,0 +1 @@
|
||||
xlidar_add_plugin(driver_sick_code sick_code_driver.cpp)
|
||||
319
plugins/driver_sick_code/sick_code_driver.cpp
Normal file
319
plugins/driver_sick_code/sick_code_driver.cpp
Normal file
@@ -0,0 +1,319 @@
|
||||
// SICK TiM 5xx/7xx — SOPAS/CoLa-A ASCII telegrams over TCP ("sSN/sRA
|
||||
// LMDscandata" parsing).
|
||||
#include "sick_code_driver.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <cctype>
|
||||
#include <cerrno>
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
#include <arpa/inet.h>
|
||||
#include <netinet/in.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
namespace {
|
||||
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 fields as plain hex of the 2's-complement bits.
|
||||
return static_cast<int32_t>(hex_to_u32(tok));
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
SickCodeDriver::SickCodeDriver(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) {}
|
||||
|
||||
SickCodeDriver::~SickCodeDriver() { close(); }
|
||||
|
||||
ErrorCode SickCodeDriver::open() {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
|
||||
return set_error(ErrorCode::InvalidAddress);
|
||||
|
||||
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
|
||||
|
||||
ErrorCode conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
|
||||
if (conn_err != ErrorCode::Ok) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return set_error(conn_err);
|
||||
}
|
||||
|
||||
recv_buf_.clear();
|
||||
latest_diag_ = Diagnostics{};
|
||||
|
||||
// Device is passive until told to stream.
|
||||
if (!send_telegram("sEN LMDscandata 1")) {
|
||||
close();
|
||||
return set_error(ErrorCode::HandshakeFailed);
|
||||
}
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void SickCodeDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
send_telegram("sEN LMDscandata 0"); // best-effort
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool SickCodeDriver::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;
|
||||
}
|
||||
|
||||
// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
|
||||
// carries leftover bytes across calls.
|
||||
bool SickCodeDriver::read_telegram(std::string& out, int timeout_ms) {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); 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) {
|
||||
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) {
|
||||
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
char buf[4096];
|
||||
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
|
||||
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
|
||||
recv_buf_.append(buf, static_cast<size_t>(n));
|
||||
}
|
||||
}
|
||||
|
||||
bool SickCodeDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
for (;;) {
|
||||
std::string telegram;
|
||||
if (!read_telegram(telegram, timeout_ms)) return false;
|
||||
if (parse_lmdscandata(telegram, out)) { set_error(ErrorCode::Ok); return true; }
|
||||
// Non-scan telegram (e.g. an ack) — keep waiting.
|
||||
}
|
||||
}
|
||||
|
||||
bool SickCodeDriver::spin_once() {
|
||||
std::string telegram;
|
||||
if (!read_telegram(telegram, 0)) return false;
|
||||
|
||||
ScanResult result;
|
||||
if (!parse_lmdscandata(telegram, result)) return true;
|
||||
if (cb_) cb_(result);
|
||||
return true;
|
||||
}
|
||||
|
||||
// CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
|
||||
// per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
|
||||
bool SickCodeDriver::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
|
||||
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());
|
||||
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
|
||||
|
||||
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;
|
||||
|
||||
// 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
|
||||
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;
|
||||
|
||||
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 + 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()) {
|
||||
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()) {
|
||||
// Clamp to the 0-255 LaserScan contract (16-bit RSSI can exceed it).
|
||||
float v = static_cast<float>(raw) * scale;
|
||||
scan.intensities[d] = v > 255.f ? 255.f : v;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
uint32_t num_16bit_channels = hex_to_u32(next());
|
||||
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();
|
||||
|
||||
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;
|
||||
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);
|
||||
|
||||
finalize_scan(scan, cfg_, inverted_);
|
||||
|
||||
ExtraInfo& info = out.info;
|
||||
info = ExtraInfo{};
|
||||
info.detected_model = cfg_.name;
|
||||
info.sick_device_status = static_cast<uint16_t>(((status0 & 0xFF) << 8) | (status1 & 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);
|
||||
|
||||
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 = "TiM5xx/TiM7xx";
|
||||
info.driver_id = "sick_tim_driver";
|
||||
info.description = "SICK TiM 2D lidars (TiM551/561/571/781, ...) over "
|
||||
"SOPAS/CoLa-A ASCII telegrams on TCP. open() starts the "
|
||||
"LMDscandata stream. Default port 2111. Verified on a "
|
||||
"real TiM781S.";
|
||||
info.transport = Transport::Tcp;
|
||||
info.supported_models = {"SICK-TIM5xx", "SICK-TIM571", "SICK-TIM7xx"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
const ModelConfig* model_by_name(const std::string& name) {
|
||||
static constexpr const ModelConfig* kModels[] = {
|
||||
&MODEL_SICK_TIM5XX, &MODEL_SICK_TIM571, &MODEL_SICK_TIM7XX,
|
||||
};
|
||||
for (const ModelConfig* m : kModels)
|
||||
if (name == m->name) return m;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DriverInfo SickCodeDriver::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 ModelConfig* preset = model_by_name(cfg->model);
|
||||
if (!preset) preset = &MODEL_SICK_TIM571; // brand default
|
||||
const uint16_t port = cfg->port ? cfg->port : 2111;
|
||||
return new SickCodeDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
|
||||
}
|
||||
67
plugins/driver_sick_code/sick_code_driver.hpp
Normal file
67
plugins/driver_sick_code/sick_code_driver.hpp
Normal file
@@ -0,0 +1,67 @@
|
||||
// SICK TiM 5xx/7xx over SOPAS/CoLa-A (TCP) — plugin-private header.
|
||||
#pragma once
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
|
||||
// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
|
||||
// frame puts 90° at the device front.
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571, 270°, 25m
|
||||
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781, 270°, 25m
|
||||
|
||||
// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
|
||||
// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
|
||||
// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
|
||||
class SickCodeDriver : public LidarDriverInterface {
|
||||
public:
|
||||
// inverted: unit mounted upside-down → mirror the scan.
|
||||
explicit SickCodeDriver(const ModelConfig& cfg,
|
||||
const std::string& ip,
|
||||
uint16_t port = 2111,
|
||||
bool inverted = false);
|
||||
~SickCodeDriver();
|
||||
|
||||
SickCodeDriver(const SickCodeDriver&) = delete;
|
||||
SickCodeDriver& operator=(const SickCodeDriver&) = delete;
|
||||
|
||||
DriverInfo get_driver_info() const override;
|
||||
|
||||
// Connect + send "sEN LMDscandata 1" to start continuous scan output.
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// No model string on the wire — returns the configured name.
|
||||
const char* detected_model() const override { return detected_model_name_.c_str(); }
|
||||
|
||||
Diagnostics get_diagnostics() const override { return latest_diag_; }
|
||||
|
||||
private:
|
||||
bool send_telegram(const std::string& body);
|
||||
bool read_telegram(std::string& out, int timeout_ms);
|
||||
bool parse_lmdscandata(const std::string& telegram, ScanResult& out);
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string detected_model_name_; // owned copy of cfg_.name (stable lifetime)
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
// Leftover TCP bytes carried across telegram boundaries; per-instance.
|
||||
std::string recv_buf_;
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
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