# 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
```