diff --git a/README.md b/README.md index c003445..9ffdb99 100644 --- a/README.md +++ b/README.md @@ -41,10 +41,17 @@ target_link_libraries(my_app PRIVATE lidarlib::lidarlib) lidarlib::LidarConfig c{"front", "192.168.1.10", 2368, "AUTO", false, "OLEI"}; std::unique_ptr 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; -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.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()); @@ -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()` + `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) `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ò | |---|---| +| `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/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` | | `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ú -- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` sẽ thất - bại. Kiểm tra: `ss -lunp | grep 2368`. +- Nếu port UDP đã bị app khác giữ (không bật `SO_REUSEPORT`), `open()` trả + `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 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)`. diff --git a/examples/example.cpp b/examples/example.cpp index 010ee78..7ebda13 100644 --- a/examples/example.cpp +++ b/examples/example.cpp @@ -5,8 +5,9 @@ int main() { lidarlib::Driver drv(lidarlib::MODEL_VB); - if (!drv.open()) { - fprintf(stderr, "Không mở được socket\n"); + lidarlib::ErrorCode err = drv.open(); + if (err != lidarlib::ErrorCode::Ok) { + fprintf(stderr, "Không mở được socket: %s\n", lidarlib::to_string(err)); return 1; } diff --git a/examples/lidar_app.cpp b/examples/lidar_app.cpp index 6d90fb3..338c2e2 100644 --- a/examples/lidar_app.cpp +++ b/examples/lidar_app.cpp @@ -15,9 +15,11 @@ void on_signal(int) { g_running = false; } void run_lidar(lidarlib::LidarConfig cfg) { std::unique_ptr lidar = lidarlib::make_lidar(cfg); - if (!lidar->open()) { - fprintf(stderr, "[%s] khong mo duoc %s %s:%u\n", - cfg.name.c_str(), cfg.brand.c_str(), cfg.ip.c_str(), cfg.port); + lidarlib::ErrorCode err = lidar->open(); + if (err != lidarlib::ErrorCode::Ok) { + 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; } printf("[%s] da mo %s %s:%u (model=%s, inverted=%d)\n", diff --git a/examples/nanoscan_example.cpp b/examples/nanoscan_example.cpp index 0de09bc..4a91f02 100644 --- a/examples/nanoscan_example.cpp +++ b/examples/nanoscan_example.cpp @@ -6,8 +6,10 @@ int main() { lidarlib::NanoScanDriver drv(lidarlib::MODEL_SICK_NANOSCAN3, "0.0.0.0", 6060); - if (!drv.open()) { - fprintf(stderr, "Không mở được UDP socket cho nanoScan3\n"); + lidarlib::ErrorCode err = drv.open(); + if (err != lidarlib::ErrorCode::Ok) { + fprintf(stderr, "Không mở được UDP socket cho nanoScan3: %s\n", + lidarlib::to_string(err)); return 1; } diff --git a/examples/sick_example.cpp b/examples/sick_example.cpp index 518d223..f3a83c0 100644 --- a/examples/sick_example.cpp +++ b/examples/sick_example.cpp @@ -5,8 +5,10 @@ int main() { lidarlib::SickDriver drv(lidarlib::MODEL_SICK_TIM571, "192.168.0.1", 2111); - if (!drv.open()) { - fprintf(stderr, "Không kết nối được TCP tới lidar SICK\n"); + lidarlib::ErrorCode err = drv.open(); + 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; } diff --git a/examples/test_dual.cpp b/examples/test_dual.cpp index a96ffa6..e2c0400 100644 --- a/examples/test_dual.cpp +++ b/examples/test_dual.cpp @@ -6,9 +6,10 @@ static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg, const std::string& local_ip, uint16_t port, bool inverted, int n_scans) { lidarlib::Driver drv(cfg, local_ip, port, inverted); - if (!drv.open()) { - fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n", - tag, local_ip.c_str(), port); + lidarlib::ErrorCode err = drv.open(); + if (err != lidarlib::ErrorCode::Ok) { + fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (%s)\n", + tag, local_ip.c_str(), port, lidarlib::to_string(err)); return; } printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port); diff --git a/include/lidarlib/error.hpp b/include/lidarlib/error.hpp new file mode 100644 index 0000000..1ec8627 --- /dev/null +++ b/include/lidarlib/error.hpp @@ -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 diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp index 01792e6..21eb16a 100644 --- a/include/lidarlib/lidar.hpp +++ b/include/lidarlib/lidar.hpp @@ -1,4 +1,5 @@ #pragma once +#include "lidarlib/error.hpp" #include #include #include @@ -83,14 +84,28 @@ class Lidar { public: 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; - // Block until one full scan; false on error/timeout. timeout_ms = 0 → block - // indefinitely. No default on purpose: drivers differ (OLEI 1000, SICK 2000). + // Block until one full scan; false on error/timeout (see last_error()). + // 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 void set_scan_callback(ScanCallback cb) = 0; virtual bool spin_once() = 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. @@ -109,11 +124,12 @@ public: Driver(const Driver&) = delete; Driver& operator=(const Driver&) = delete; - bool open() override; + ErrorCode open() override; void close() override; bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } 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. const char* detected_model() const override { return detected_model_name_.c_str(); } diff --git a/include/lidarlib/lidarlib.hpp b/include/lidarlib/lidarlib.hpp index 766aa38..3b40bb1 100644 --- a/include/lidarlib/lidarlib.hpp +++ b/include/lidarlib/lidarlib.hpp @@ -1,5 +1,6 @@ #pragma once // One-include convenience header for the whole public API. +#include "lidarlib/error.hpp" #include "lidarlib/lidar.hpp" #include "lidarlib/sick_lidar.hpp" #include "lidarlib/config.hpp" diff --git a/include/lidarlib/sick_lidar.hpp b/include/lidarlib/sick_lidar.hpp index 320d71e..1f99629 100644 --- a/include/lidarlib/sick_lidar.hpp +++ b/include/lidarlib/sick_lidar.hpp @@ -29,11 +29,12 @@ public: SickDriver& operator=(const SickDriver&) = delete; // Connect + send "sEN LMDscandata 1" to start continuous scan output. - bool open() override; + ErrorCode open() override; void close() override; bool recv_scan(ScanResult& out, int timeout_ms = 2000) override; void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } bool spin_once() override; + bool is_open() const override { return sock_fd_ >= 0; } // No model string on the wire — returns the configured name. const char* detected_model() const override { return detected_model_name_.c_str(); } @@ -73,11 +74,12 @@ public: NanoScanDriver(const NanoScanDriver&) = delete; NanoScanDriver& operator=(const NanoScanDriver&) = delete; - bool open() override; + ErrorCode open() override; void close() override; bool recv_scan(ScanResult& out, int timeout_ms = 1000) override; void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); } bool spin_once() override; + bool is_open() const override { return sock_fd_ >= 0; } // No model string on the wire — returns the configured name. const char* detected_model() const override { return detected_model_name_.c_str(); } diff --git a/src/olei_lidar.cpp b/src/olei_lidar.cpp index 1425cea..4c639b6 100644 --- a/src/olei_lidar.cpp +++ b/src/olei_lidar.cpp @@ -1,6 +1,7 @@ #include "lidarlib/lidar.hpp" #include "lidar_bytes.hpp" +#include #include #include #include @@ -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(&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(&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; diff --git a/src/sick_lidar.cpp b/src/sick_lidar.cpp index 675738a..7755369 100644 --- a/src/sick_lidar.cpp +++ b/src/sick_lidar.cpp @@ -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(&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(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(&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(n); + if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return -1; } + return static_cast(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(total), out)) return true; + if (parse_packet(tele.data(), static_cast(total), out)) { + set_error(ErrorCode::Ok); + return true; + } } } }