update ErrorCode, status, lifecycle

This commit is contained in:
2026-07-06 15:55:14 +07:00
parent 323715eab0
commit 397b9ab3c5
12 changed files with 253 additions and 69 deletions

View File

@@ -41,10 +41,17 @@ target_link_libraries(my_app PRIVATE lidarlib::lidarlib)
lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"};
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c); std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(c);
lidar->open();
lidarlib::ErrorCode err = lidar->open();
if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "open that bai: %s\n", lidarlib::to_string(err));
return 1;
}
lidarlib::ScanResult r; lidarlib::ScanResult r;
lidar->recv_scan(r, 1000); if (!lidar->recv_scan(r, 1000)) {
// Timeout / DeviceDisconnected / NotOpen — xem lidar->last_error()
}
// r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar // r.scan : LaserScan — format sensor_msgs/LaserScan của ROS, chung mọi lidar
// r.info : ExtraInfo — thông tin thêm tuỳ family/model // r.info : ExtraInfo — thông tin thêm tuỳ family/model
printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str()); printf("%zu diem, model=%s\n", r.scan.ranges.size(), r.info.detected_model.c_str());
@@ -61,6 +68,33 @@ lidarlib::NanoScanDriver nano(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` + Ngoài `recv_scan()` blocking còn có callback: `set_scan_callback()` +
`spin_once()` trong vòng lặp riêng. `spin_once()` trong vòng lặp riêng.
### Error handling & lifecycle
`open()` trả về `lidarlib::ErrorCode` (header `lidarlib/error.hpp`,
`to_string()` để log). Các code chính:
| ErrorCode | Khi nào |
|---|---|
| `Ok` | Thành công |
| `AlreadyOpen` | Gọi `open()` khi đang mở — kết nối cũ giữ nguyên |
| `NotOpen` | Gọi `recv_scan()`/`spin_once()` khi chưa `open()` |
| `InvalidAddress` | Chuỗi IP không hợp lệ |
| `PortInUse` | Port local đã bị chiếm (bind `EADDRINUSE`/`EACCES`) |
| `BindFailed` / `SocketError` | Lỗi bind khác / không tạo được socket |
| `ConnectionRefused` / `ConnectionFailed` / `Timeout` | TCP connect (SICK TiM) bị từ chối / không tới được / quá 2s |
| `HandshakeFailed` | TCP nối được nhưng gửi `sEN LMDscandata 1` thất bại |
| `DeviceDisconnected` | Thiết bị đóng kết nối / lỗi recv giữa chừng |
Trạng thái instance:
- `is_open()` — socket đang mở hay không.
- `last_error()` — kết quả của lần `open()`/`recv_scan()`/`spin_once()` gần
nhất (`recv_scan()` trả `false` thì gọi hàm này để biết `Timeout` hay
`DeviceDisconnected`).
- Lifecycle chịu lỗi mọi thứ tự gọi: `close()` trước `open()` hoặc `close()`
hai lần là no-op; `open()` hai lần trả `AlreadyOpen` và không đụng kết nối
đang chạy; sau `close()` có thể `open()` lại (state scan dở được reset).
## Cấu hình (config.json) ## Cấu hình (config.json)
`lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng `lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng
@@ -150,6 +184,7 @@ parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer
| File | Vai trò | | File | Vai trò |
|---|---| |---|---|
| `include/lidarlib/error.hpp` | `enum class ErrorCode` + `to_string()` |
| `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI | | `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI |
| `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` | | `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` |
| `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` | | `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` |
@@ -160,7 +195,35 @@ parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer
## Ghi chú ## Ghi chú
- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` sẽ thất - Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` trả
bại. Kiểm tra: `ss -lunp | grep 2368`. `ErrorCode::PortInUse`. Kiểm tra: `ss -lunp | grep 2368`.
- `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm - `inverted` đã verify bằng sniff sống: `false` góc tăng dần, `true` góc giảm
dần cùng bước. dần cùng bước.
## Changelog
### 2026-07-06 — Error handling & lifecycle API
- **Header mới `lidarlib/error.hpp`**: `enum class ErrorCode``Ok`,
`AlreadyOpen`, `NotOpen`, `SocketError`, `InvalidAddress`, `PortInUse`,
`BindFailed`, `ConnectionRefused`, `ConnectionFailed`, `HandshakeFailed`,
`Timeout`, `DeviceDisconnected` — kèm `to_string()` để log.
- **`open()` đổi chữ ký `bool``ErrorCode`** trên cả 3 driver (`Driver`,
`SickDriver`, `NanoScanDriver`). Lỗi phân loại từ `errno` thật: bind
`EADDRINUSE``PortInUse`, connect TCP bị từ chối → `ConnectionRefused`,
quá 2s → `Timeout`, gửi lệnh start-stream fail → `HandshakeFailed`,
IP sai format → `InvalidAddress`.
- **API trạng thái mới trên interface `Lidar`**:
- `is_open()` — socket đang mở hay không (cả 3 driver implement);
- `last_error()` — kết quả lần `open()`/`recv_scan()`/`spin_once()` gần
nhất; `recv_scan()` trả `false` thì gọi hàm này để biết `Timeout` hay
`DeviceDisconnected`.
- **Lifecycle chịu lỗi mọi thứ tự gọi**: `close()` trước `open()` hoặc gọi
hai lần là no-op; `open()` khi đang mở trả `AlreadyOpen` và không đụng kết
nối đang chạy; `recv_scan()`/`spin_once()` khi chưa mở trả `false` +
`NotOpen`; sau `close()` có thể `open()` lại (state scan dở được reset).
- **`NanoScanDriver` bỏ `SO_REUSEADDR`**: với UDP không có tác dụng (không có
TIME_WAIT) mà còn che mất lỗi trùng port — giờ `PortInUse` báo được thật.
- ⚠️ **Breaking change**: code cũ viết `if (!lidar->open())` bị đảo ngược
logic vì `ErrorCode::Ok == 0` — phải đổi thành
`if (lidar->open() != lidarlib::ErrorCode::Ok)`.

View File

@@ -5,8 +5,9 @@
int main() { int main() {
lidarlib::Driver drv(lidarlib::MODEL_VB); lidarlib::Driver drv(lidarlib::MODEL_VB);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "Không mở được socket\n"); if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err));
return 1; return 1;
} }

View File

@@ -15,9 +15,11 @@ void on_signal(int) { g_running = false; }
void run_lidar(lidarlib::LidarConfig cfg) { void run_lidar(lidarlib::LidarConfig cfg) {
std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg); std::unique_ptr<lidarlib::Lidar> lidar = lidarlib::make_lidar(cfg);
if (!lidar->open()) { lidarlib::ErrorCode err = lidar->open();
fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n", if (err != lidarlib::ErrorCode::Ok) {
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port); fprintf(stderr, "[%s] khong mo duoc %s %s:%u (%s)\n",
cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port,
lidarlib::to_string(err));
return; return;
} }
printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n", printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n",

View File

@@ -6,8 +6,10 @@
int main() { int main() {
lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "Không mở được UDP socket cho nanoScan3\n"); if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không mở được UDP socket cho nanoScan3: %s\n",
lidarlib::to_string(err));
return 1; return 1;
} }

View File

@@ -5,8 +5,10 @@
int main() { int main() {
lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "Không kết nối được TCP tới lidar SICK\n"); if (err != lidarlib::ErrorCode::Ok) {
fprintf(stderr, "Không kết nối được TCP tới lidar SICK: %s\n",
lidarlib::to_string(err));
return 1; return 1;
} }

View File

@@ -6,9 +6,10 @@
static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg, static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
const std::string& local_ip, uint16_t port, bool inverted, int n_scans) { const std::string& local_ip, uint16_t port, bool inverted, int n_scans) {
lidarlib::Driver drv(cfg, local_ip, port, inverted); lidarlib::Driver drv(cfg, local_ip, port, inverted);
if (!drv.open()) { lidarlib::ErrorCode err = drv.open();
fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n", if (err != lidarlib::ErrorCode::Ok) {
tag, local_ip.c_str(), port); fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n",
tag, local_ip.c_str(), port, lidarlib::to_string(err));
return; return;
} }
printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port); printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);

View File

@@ -0,0 +1,46 @@
#pragma once
namespace lidarlib {
// Result of open() and the sticky status behind last_error(). Ok == 0 so
// `if (err != ErrorCode::Ok)` reads naturally at call sites.
enum class ErrorCode {
Ok = 0,
// Lifecycle misuse — the call was refused, the instance state is unchanged.
AlreadyOpen, // open() called while already open
NotOpen, // recv_scan()/spin_once() called before open()
// open() failures
SocketError, // socket() creation failed
InvalidAddress, // ip string is not a valid IPv4 address
PortInUse, // bind: local port already taken (EADDRINUSE/EACCES)
BindFailed, // bind failed for another reason
ConnectionRefused, // TCP connect refused (device up, port closed)
ConnectionFailed, // TCP connect failed (unreachable, no route, ...)
HandshakeFailed, // connected, but the start-stream command failed
// Runtime failures
Timeout, // no (complete) scan within timeout_ms
DeviceDisconnected, // peer closed the connection / socket recv error
};
inline const char* to_string(ErrorCode e) {
switch (e) {
case ErrorCode::Ok: return "Ok";
case ErrorCode::AlreadyOpen: return "AlreadyOpen";
case ErrorCode::NotOpen: return "NotOpen";
case ErrorCode::SocketError: return "SocketError";
case ErrorCode::InvalidAddress: return "InvalidAddress";
case ErrorCode::PortInUse: return "PortInUse";
case ErrorCode::BindFailed: return "BindFailed";
case ErrorCode::ConnectionRefused: return "ConnectionRefused";
case ErrorCode::ConnectionFailed: return "ConnectionFailed";
case ErrorCode::HandshakeFailed: return "HandshakeFailed";
case ErrorCode::Timeout: return "Timeout";
case ErrorCode::DeviceDisconnected: return "DeviceDisconnected";
}
return "Unknown";
}
} // namespace lidarlib

View File

@@ -1,4 +1,5 @@
#pragma once #pragma once
#include "lidarlib/error.hpp"
#include <cstdint> #include <cstdint>
#include <vector> #include <vector>
#include <string> #include <string>
@@ -83,14 +84,28 @@ class Lidar {
public: public:
virtual ~Lidar() = default; virtual ~Lidar() = default;
virtual bool open() = 0; // ErrorCode::Ok on success. Calling open() on an already-open instance
// returns AlreadyOpen and leaves the connection untouched.
virtual ErrorCode open() = 0;
// Idempotent: safe to call before open() or more than once.
virtual void close() = 0; virtual void close() = 0;
// Block until one full scan; false on error/timeout. timeout_ms = 0 → block // Block until one full scan; false on error/timeout (see last_error()).
// indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000). // timeout_ms = 0 → block indefinitely. No default on purpose: drivers
// differ (OLEI 1000, SICK 2000).
virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0; virtual bool recv_scan(ScanResult& out, int timeout_ms) = 0;
virtual void set_scan_callback(ScanCallback cb) = 0; virtual void set_scan_callback(ScanCallback cb) = 0;
virtual bool spin_once() = 0; virtual bool spin_once() = 0;
virtual const char* detected_model() const = 0; virtual const char* detected_model() const = 0;
virtual bool is_open() const = 0;
// Status of the most recent open()/recv_scan()/spin_once() call.
ErrorCode last_error() const { return last_error_; }
protected:
ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
private:
ErrorCode last_error_ = ErrorCode::Ok;
}; };
// OLEI UDP driver. // OLEI UDP driver.
@@ -109,11 +124,12 @@ public:
Driver(const Driver&) = delete; Driver(const Driver&) = delete;
Driver& operator=(const Driver&) = delete; Driver& operator=(const Driver&) = delete;
bool open() override; ErrorCode open() override;
void close() override; void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override; bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// Model name read from the Family B/C header; "AUTO" until one is seen. // Model name read from the Family B/C header; "AUTO" until one is seen.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }

View File

@@ -1,5 +1,6 @@
#pragma once #pragma once
// One-include convenience header for the whole public API. // One-include convenience header for the whole public API.
#include "lidarlib/error.hpp"
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include "lidarlib/sick_lidar.hpp" #include "lidarlib/sick_lidar.hpp"
#include "lidarlib/config.hpp" #include "lidarlib/config.hpp"

View File

@@ -29,11 +29,12 @@ public:
SickDriver& operator=(const SickDriver&) = delete; SickDriver& operator=(const SickDriver&) = delete;
// Connect + send "sEN LMDscandata 1" to start continuous scan output. // Connect + send "sEN LMDscandata 1" to start continuous scan output.
bool open() override; ErrorCode open() override;
void close() override; void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 2000) override; bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override; bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// No model string on the wire — returns the configured name. // No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }
@@ -73,11 +74,12 @@ public:
NanoScanDriver(const NanoScanDriver&) = delete; NanoScanDriver(const NanoScanDriver&) = delete;
NanoScanDriver& operator=(const NanoScanDriver&) = delete; NanoScanDriver& operator=(const NanoScanDriver&) = delete;
bool open() override; ErrorCode open() override;
void close() override; void close() override;
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
bool spin_once() override; bool spin_once() override;
bool is_open() const override { return sock_fd_ >= 0; }
// No model string on the wire — returns the configured name. // No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); } const char* detected_model() const override { return detected_model_name_.c_str(); }

View File

@@ -1,6 +1,7 @@
#include "lidarlib/lidar.hpp" #include "lidarlib/lidar.hpp"
#include "lidar_bytes.hpp" #include "lidar_bytes.hpp"
#include <cerrno>
#include <cstring> #include <cstring>
#include <cmath> #include <cmath>
#include <stdexcept> #include <stdexcept>
@@ -46,9 +47,17 @@ Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, boo
Driver::~Driver() { close(); } 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); 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; int reuse = 1;
::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); ::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)); ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
#endif #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) { if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
int err = errno;
::close(sock_fd_); ::close(sock_fd_);
sock_fd_ = -1; 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_angle_deg_.reserve(2048);
pending_dist_m_.reserve(2048); pending_dist_m_.reserve(2048);
pending_intensity_.reserve(2048); pending_intensity_.reserve(2048);
return true; return set_error(ErrorCode::Ok);
} }
void Driver::close() { void Driver::close() {
@@ -80,6 +95,7 @@ void Driver::close() {
} }
bool Driver::recv_scan(ScanResult& out, int timeout_ms) { bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
scan_ready_ = false; scan_ready_ = false;
while (!scan_ready_) { 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); fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv); 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; if (!spin_once()) return false;
} }
out = std::move(ready_result_); out = std::move(ready_result_);
set_error(ErrorCode::Ok);
return true; return true;
} }
bool Driver::spin_once() { bool Driver::spin_once() {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
uint8_t* buf = recv_buf_; uint8_t* buf = recv_buf_;
sockaddr_in from{}; sockaddr_in from{};
socklen_t fromlen = sizeof(from); socklen_t fromlen = sizeof(from);
ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0, ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
reinterpret_cast<sockaddr*>(&from), &fromlen); 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]. // A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
if (n < 4) return true; if (n < 4) return true;

View File

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