QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update ErrorCode, status, lifecycle

Browse Source
This commit is contained in:
QUYVN
2026-07-06 15:55:14 +07:00
parent 323715eab0
commit 397b9ab3c5
12 changed files with 253 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
src/olei_lidar.cpp
View File

@@ -1,6 +1,7 @@
#include "lidarlib/lidar.hpp"
#include "lidar_bytes.hpp"
#include <cerrno>
#include <cstring>
#include <cmath>
#include <stdexcept>
@@ -46,9 +47,17 @@ Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, boo
Driver::~Driver() { close(); }
bool Driver::open() {
ErrorCode Driver::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 false;
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
@@ -56,20 +65,26 @@ bool Driver::open() {
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
#endif
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
addr.sin_addr.s_addr = inet_addr(ip_.c_str());
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
int err = errno;
::close(sock_fd_);
sock_fd_ = -1;
return false;
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{};
last_angle_ = -1.f;
scan_ready_ = false;
pending_angle_deg_.reserve(2048);
pending_dist_m_.reserve(2048);
pending_intensity_.reserve(2048);
return true;
return set_error(ErrorCode::Ok);
}
void Driver::close() {
@@ -80,6 +95,7 @@ void Driver::close() {
}
bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
scan_ready_ = false;
while (!scan_ready_) {
@@ -87,22 +103,27 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
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) return false;
if (r <= 0) {
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
return false;
}
}
if (!spin_once()) return false;
}
out = std::move(ready_result_);
set_error(ErrorCode::Ok);
return true;
}
bool Driver::spin_once() {
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) return false;
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;

99
src/sick_lidar.cpp
View File

@@ -53,39 +53,50 @@ SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t p
SickDriver::~SickDriver() { close(); }
bool SickDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
if (sock_fd_ < 0) return false;
ErrorCode SickDriver::open() {
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
addr.sin_addr.s_addr = inet_addr(ip_.c_str());
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);
// Non-blocking connect with a bounded timeout — a blocking connect() to an
// unreachable device would stall for the OS default (~2 min on Linux).
int flags = ::fcntl(sock_fd_, F_GETFL, 0);
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
ErrorCode conn_err = ErrorCode::Ok;
int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
if (rc < 0 && errno == EINPROGRESS) {
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(sock_fd_, &wfds);
timeval tv{ kConnectTimeoutMs / 1000, (kConnectTimeoutMs % 1000) * 1000 };
rc = ::select(sock_fd_ + 1, nullptr, &wfds, nullptr, &tv);
if (rc > 0) {
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(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
rc = (err == 0) ? 0 : -1;
} else {
rc = -1;
if (err != 0)
conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
}
}
::fcntl(sock_fd_, F_SETFL, flags);
if (rc < 0) {
if (conn_err != ErrorCode::Ok) {
::close(sock_fd_);
sock_fd_ = -1;
return false;
return set_error(conn_err);
}
int nodelay = 1;
@@ -96,9 +107,9 @@ bool SickDriver::open() {
// Device is passive until told to stream.
if (!send_telegram("sEN LMDscandata 1")) {
close();
return false;
return set_error(ErrorCode::HandshakeFailed);
}
return true;
return set_error(ErrorCode::Ok);
}
void SickDriver::close() {
@@ -129,7 +140,7 @@ bool SickDriver::send_telegram(const std::string& body) {
// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
// carries leftover bytes across calls.
bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
if (sock_fd_ < 0) return false;
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
for (;;) {
size_t etx_pos = recv_buf_.find(kEtx);
@@ -148,12 +159,15 @@ bool SickDriver::read_telegram(std::string& out, int timeout_ms) {
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) return false;
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) return false;
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
recv_buf_.append(buf, static_cast<size_t>(n));
}
}
@@ -162,7 +176,7 @@ bool SickDriver::recv_scan(ScanResult& out, int timeout_ms) {
for (;;) {
std::string telegram;
if (!read_telegram(telegram, timeout_ms)) return false;
if (parse_lmdscandata(telegram, out)) return true;
if (parse_lmdscandata(telegram, out)) { set_error(ErrorCode::Ok); return true; }
// Non-scan telegram (e.g. an ack) — keep waiting.
}
}
@@ -294,25 +308,31 @@ NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, ui
NanoScanDriver::~NanoScanDriver() { close(); }
bool NanoScanDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
if (sock_fd_ < 0) return false;
int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
ErrorCode NanoScanDriver::open() {
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
sockaddr_in addr{};
addr.sin_family = AF_INET;
addr.sin_port = htons(port_);
addr.sin_addr.s_addr = (ip_ == "0.0.0.0" || ip_.empty()) ? INADDR_ANY
: inet_addr(ip_.c_str());
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 false;
return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
: ErrorCode::BindFailed);
}
return true;
return set_error(ErrorCode::Ok);
}
void NanoScanDriver::close() {
@@ -323,17 +343,21 @@ void NanoScanDriver::close() {
}
int NanoScanDriver::recv_datagram(int timeout_ms) {
if (sock_fd_ < 0) return -1;
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) return -1;
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);
return (n <= 0) ? -1 : static_cast<int>(n);
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
@@ -351,7 +375,7 @@ bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
const uint8_t* d = recv_buf_.data();
if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
if (parse_packet(d, n, out)) return true;
if (parse_packet(d, n, out)) { set_error(ErrorCode::Ok); return true; }
continue;
}
@@ -375,7 +399,10 @@ bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
if (got >= total) {
assembling = false;
if (parse_packet(tele.data(), static_cast<int>(total), out)) return true;
if (parse_packet(tele.data(), static_cast<int>(total), out)) {
set_error(ErrorCode::Ok);
return true;
}
}
}
}