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. 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:

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:

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

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)

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.

3.2 Chạy node trực tiếp (không launch)

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:

ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link front

3.3 Kiểm tra dữ liệu

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

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