loctv
abf7bab917
Slamtec devices count their angle clockwise viewed from the top (Interface Protocol spec; the official rplidar_ros node mirrors the angles for the same reason), but the decode used the raw ascending angles as CCW for a right-side-up unit and mirrored them for an upside-down one — exactly backwards. Field-verified 2026-07-23: a right-side-up C1 produced a left-right mirrored sweep against a verified right-handed reference lidar. Now: right-side-up mirrors (angle' = 2π − raw, nodes walked backwards); upside-down uses the raw ascending angles, because the physical flip already reverses the apparent rotation. The mountedUpsideDown flag finally carries its true physical meaning. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
344 lines
14 KiB
C++
344 lines
14 KiB
C++
// Slamtec RPLIDAR over serial (C1 defaults), built on the vendored SDK
|
||
// (third_party/rplidar_sdk, sl_lidar.h). Scan math: angle/distance decoding,
|
||
// inversion, FOV window, invalid points as NaN.
|
||
//
|
||
// Unlike the network drivers, angles arrive in the DEVICE frame [0, 2π),
|
||
// 0 = ahead, ascending (the SDK's ascendScanData order) — but Slamtec's
|
||
// angle scale runs CLOCKWISE viewed from the top, so decode mirrors it into
|
||
// our right-handed convention (see the handedness comment in recv_scan).
|
||
#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{};
|
||
// Handedness: Slamtec devices count their angle CLOCKWISE viewed from
|
||
// the top (Interface Protocol spec; the official rplidar_ros node
|
||
// mirrors the angles for exactly this reason). Our output convention
|
||
// is right-handed (CCW, + = left), so:
|
||
// - right-side-up (inverted_ == false): mirror (angle' = 2π − raw)
|
||
// and walk the nodes backwards to keep ascending order;
|
||
// - upside-down (inverted_ == true): the flip makes the rotation
|
||
// appear CCW from above, so the raw ascending angles are already
|
||
// right-handed — use them as-is.
|
||
// Field-verified (2026-07-23): a right-side-up C1 decoded as CCW
|
||
// produced a left-right mirrored sweep against a verified
|
||
// right-handed reference lidar; invisible with the C1 alone because
|
||
// a single-sensor SLAM map is self-consistently mirrored.
|
||
if (inverted_) {
|
||
scan.angle_min = angle_first;
|
||
scan.angle_max = angle_last;
|
||
} else {
|
||
scan.angle_min = kTwoPi - angle_last;
|
||
scan.angle_max = kTwoPi - angle_first;
|
||
}
|
||
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) {
|
||
// Mirrored branches walk backwards (see the handedness comment
|
||
// above): backwards for a right-side-up unit, forwards when the
|
||
// physical flip already reversed the apparent rotation.
|
||
const std::size_t node_index = inverted_ ? i : count - 1 - 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;
|
||
if (!transport_supported(kDriverInfo, *cfg))
|
||
return new InvalidConfigDriver(kDriverInfo,
|
||
std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
|
||
// "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);
|
||
}
|