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