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:
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
|
||||
Reference in New Issue
Block a user