loctv
5b2c74bd36
- 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>
320 lines
11 KiB
C++
320 lines
11 KiB
C++
// 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);
|
|
}
|