165 lines
6.3 KiB
Markdown
165 lines
6.3 KiB
Markdown
# 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 -> <frame>`
|
|
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ố `<ten>.`):
|
|
|
|
| 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_<ten>` | Topic publish |
|
|
| `frame_id` | `<ten>` | 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=<prefix>` |
|
|
| `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
|
|
```
|