update ErrorCode, status, lifecycle
This commit is contained in:
71
README.md
71
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"};
|
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)`.
|
||||||
|
|||||||
@@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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",
|
||||||
|
|||||||
@@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
46
include/lidarlib/error.hpp
Normal file
46
include/lidarlib/error.hpp
Normal 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
|
||||||
@@ -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(); }
|
||||||
|
|||||||
@@ -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"
|
||||||
|
|||||||
@@ -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(); }
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
@@ -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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user