From 9e28eb2b31f94cfdae5cf235b7f6444092769f0b Mon Sep 17 00:00:00 2001 From: QUYVN Date: Fri, 3 Jul 2026 15:56:11 +0700 Subject: [PATCH] add md --- README.md | 164 ++++++++++++++++++++++++++++++++++++++++++++++ config/lidar.yaml | 16 ++++- 2 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000..e74b1ab --- /dev/null +++ b/README.md @@ -0,0 +1,164 @@ +# lidarlib_ros + +Gói ROS 2 (Humble, ament_cmake/C++17) cầu nối thư viện **lidarlib** (`liblidarlib.so`, +OLEI/UDP + SICK/TCP) sang `sensor_msgs/LaserScan`. + +Luồng dữ liệu: + +``` +lidarlib::make_lidar(cfg) -> open() -> recv_scan(r, timeout) + (1 thread / lidar) | + v + publish sensor_msgs/LaserScan -> RViz +``` + +Hỗ trợ **nhiều lidar cùng lúc**: param `lidars` là danh sách tên, mỗi tên có một +block tham số riêng trong [config/lidar.yaml](config/lidar.yaml). Mỗi lidar chạy +trên một thread riêng, publish topic + frame riêng, tự **reconnect** khi mất kết +nối (5 lần timeout liên tiếp → close + open lại). + +## 1. Yêu cầu + +- ROS 2 Humble (`rclcpp`, `sensor_msgs`, `tf2_ros`, `rviz2`) +- Thư viện `lidarlib` đã cài vào hệ thống (mặc định `/usr/local`) + +### Cài thư viện lidarlib + +Từ thư mục source của thư viện: + +```bash +cmake -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build -j +sudo cmake --install build --prefix /usr/local +sudo ldconfig # BẮT BUỘC - xem mục Troubleshooting +``` + +Kiểm tra thư viện đã thấy được: + +```bash +ldconfig -p | grep lidarlib +# -> liblidarlib.so.1 (libc6,x86-64) => /usr/local/lib/liblidarlib.so.1 +``` + +> Nếu cài vào `$HOME/.local` thay vì `/usr/local`, thêm khi build package: +> `colcon build --cmake-args -DCMAKE_PREFIX_PATH=$HOME/.local` + +## 2. Build package + +```bash +cd ~/ros2_ws +colcon build --packages-select lidarlib_ros +source install/setup.bash +``` + +## 3. Chạy & test + +### 3.1 Chạy bằng launch (kèm static TF + RViz) + +```bash +ros2 launch lidarlib_ros lidar.launch.py # có RViz +ros2 launch lidarlib_ros lidar.launch.py rviz:=false # không RViz +``` + +Launch file tự đọc `config/lidar.yaml`, in bảng thông tin từng lidar (brand, ip, +port, topic, frame, TF) ra màn hình, và tự sinh **static TF** `base_link -> ` +cho mọi frame có trong config. Vị trí lắp đặt thực tế sửa trong `MOUNT_POSES` +tại [launch/lidar.launch.py](launch/lidar.launch.py). + +### 3.2 Chạy node trực tiếp (không launch) + +```bash +ros2 run lidarlib_ros lidarlib_node --ros-args \ + --params-file ~/ros2_ws/src/lidarlib_ros/config/lidar.yaml +``` + +Lưu ý: chạy kiểu này **không có TF**, RViz sẽ báo frame lỗi trừ khi tự publish TF: + +```bash +ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link front +``` + +### 3.3 Kiểm tra dữ liệu + +```bash +# Liệt kê topic scan +ros2 topic list | grep scan + +# Tần số publish (kỳ vọng ~10-15 Hz tùy model) +ros2 topic hz /scan_front + +# Xem nội dung 1 message +ros2 topic echo /scan_front --once + +# Số điểm hợp lệ trong 1 scan +ros2 topic echo /scan_front --once --field ranges | tr ',' '\n' | grep -vc inf +``` + +Log của node in mỗi 2 giây một dòng tóm tắt cho từng lidar: + +``` +[gs15] scan: 1440 diem, ts=... ms, FOV[-180.0..180.0]deg | detected_model=GS1-5 error=0x0 ... +``` + +`error` khác `0x0` nghĩa là thiết bị báo lỗi (bit 0=Monitor, 1=Voltage, 2=Temp). + +### 3.4 Test khi CHƯA có phần cứng + +Node không sập nếu một lidar không mở được — nó log lỗi và bỏ qua con đó, các con +khác vẫn chạy. Node chỉ thoát khi **không mở được lidar nào**. + +Với OLEI (UDP, bind `0.0.0.0`) có thể giả lập bằng cách phát lại gói tin đã bắt: + +```bash +# Bắt gói từ lidar thật (chạy khi có phần cứng) +sudo tcpdump -i any udp port 2368 -w /tmp/olei.pcap + +# Phát lại để test node offline +sudo tcpreplay -i lo /tmp/olei.pcap +``` + +## 4. Tham số (config/lidar.yaml) + +Tham số chung: + +| Tham số | Mặc định | Ý nghĩa | +|---|---|---| +| `lidars` | `["front"]` | Danh sách tên lidar cần chạy; mỗi tên có block riêng | +| `qos_reliability` | `reliable` | `reliable` (khớp RViz mặc định) hoặc `best_effort` | + +Tham số cho từng lidar (tiền tố `.`): + +| Tham số | Mặc định | Ý nghĩa | +|---|---|---| +| `brand` | `OLEI` | `OLEI` = UDP, `SICK` = TCP/SOPAS | +| `model` | `AUTO` | OLEI: `AUTO`/`VB`/`VF`/`LR-1F`/`LR-1BS5`/`LR-16F`/`GS1-5`; SICK: `SICK-TIM5XX`/`SICK-TIM571`/`SICK-TIM7XX`/`SICK-nanoScan3` | +| `ip` | `0.0.0.0` | OLEI: địa chỉ bind của host (thường `0.0.0.0`); SICK: IP của lidar | +| `port` | `2368` | OLEI: 2368/2369; SICK TiM: 2111; nanoScan3: port UDP đã cấu hình | +| `inverted` | `false` | Lidar lắp úp ngược. OLEI: thư viện xử lý; brand khác: node đảo chiều lúc publish | +| `topic` | `scan_` | Topic publish | +| `frame_id` | `` | Frame TF | +| `timeout_ms` | `1000` | Timeout mỗi lần `recv_scan` | +| `range_min` / `range_max` | `0.0` | Mét; `0.0` = dùng giá trị thiết bị báo | +| `angle_min_deg` / `angle_max_deg` | `±360.0` | Remap tuyến tính nhãn góc của scan sang cửa sổ này (không bỏ điểm nào); `±360` = tắt, giữ nguyên góc thiết bị | + +## 5. Troubleshooting + +| Triệu chứng | Nguyên nhân / cách sửa | +|---|---| +| `liblidarlib.so.1 => not found` khi chạy node | Quên `sudo ldconfig` sau khi cài thư viện. Kiểm tra bằng `ldd $(ros2 pkg prefix lidarlib_ros)/lib/lidarlib_ros/lidarlib_node` | +| CMake: `Could not find lidarlib` | Thư viện chưa cài, hoặc cài vào prefix khác → thêm `-DCMAKE_PREFIX_PATH=` | +| `Khong mo duoc lidar (...)` | Sai IP/port, chưa cắm mạng, hoặc port đang bị tiến trình khác giữ (`sudo ss -ulpn | grep 2368`) | +| `Chua nhan duoc scan (timeout ...)` lặp lại | Lidar chưa gửi dữ liệu tới host: kiểm tra IP đích cấu hình trên lidar, subnet (host cần IP cùng dải, vd `192.168.100.x`), firewall | +| RViz không hiện điểm, topic có dữ liệu | Sai `Fixed Frame` (chọn `base_link`), hoặc QoS không khớp → để `qos_reliability: reliable` | +| Scan bị lật gương | Bật/tắt `inverted` của lidar tương ứng | + +## 6. Cấu trúc package + +``` +lidarlib_ros/ +├── src/lidarlib_node.cpp # node cầu nối, multi-lidar, auto-reconnect +├── config/lidar.yaml # danh sách lidar + tham số từng con +├── launch/lidar.launch.py # node + static TF (MOUNT_POSES) + RViz +├── rviz/lidar.rviz # config RViz có sẵn +└── CMakeLists.txt # link lidarlib::lidarlib +``` diff --git a/config/lidar.yaml b/config/lidar.yaml index 3ad091f..b39bcd9 100644 --- a/config/lidar.yaml +++ b/config/lidar.yaml @@ -1,7 +1,7 @@ lidarlib_node: ros__parameters: # Danh sach lidar chay cung luc. Them/bot ten tuy y; moi ten co 1 block rieng ben duoi. - lidars: ["tim7", "nano", "gs15", "1f"] + lidars: ["tim7", "nano", "nano1", "gs15", "1f"] gs15: @@ -58,4 +58,18 @@ lidarlib_node: range_min: 0.0 # met; 0.0 = tat (dung gia tri thiet bi bao) range_max: 0.0 angle_min_deg: -360.0 # do; +/-360 = tat (giu nguyen goc thiet bi) + angle_max_deg: 360.0 + + nano1: + brand: "SICK" + model: "SICK-nanoScan3" + ip: "0.0.0.0" + port: 6062 + inverted: false # true neu lap up nguoc (node dao chieu scan) + topic: "scan_front2" + frame_id: "front2" + timeout_ms: 1000 + range_min: 0.0 # met; 0.0 = tat (dung gia tri thiet bi bao) + range_max: 0.0 + angle_min_deg: -360.0 # do; +/-360 = tat (giu nguyen goc thiet bi) angle_max_deg: 360.0 \ No newline at end of file