道术合一

Appearance

激光雷达 ROS2 驱动全流程详解 ​

一、系统架构总览 ​

┌─────────────────┐    USB     ┌─────────────────┐    ROS 2     ┌─────────────────┐
│  激光雷达硬件    │───────────►│   串口驱动      │─────────────►│   ROS 2节点      │
│  (N10Plus)      │  460800bps │  (/dev/ttyACM0) │              │  (lslidar_driver)│
└─────────────────┘            └─────────────────┘              └─────────┬───────┘
                                                                           │
                                                                           ▼
                                                                   ┌─────────────────┐
                                                                   │   话题发布       │
                                                                   │  /scan (LaserScan)│
                                                                   │  /lslidar_point_cloud│
                                                                   └─────────────────┘

二、代码结构解析 ​

1. 工作空间结构 ​

~/bob_ws/JP6.2_xxx_ros2_src_20260106/
├── src/
│   └── xxx_lidar_ros2/
│       └── lslidar_ros2/
│           └── lslidar_driver/
│               ├── CMakeLists.txt          # 编译配置
│               ├── package.xml            # 包定义
│               └── src/
│                   ├── lslidar_x10_driver.cpp    # 主驱动文件（X10系列）
│                   ├── input.cpp                 # 输入处理（串口/网络）
│                   ├── lsiosr.cpp               # I/O 处理
│                   └── ... 其他文件
├── build/          # 编译中间文件
├── install/        # 安装目录（含可执行文件）
└── log/           # 编译日志

2. 核心文件解析 ​

lslidar_x10_driver.cpp - 主驱动 ​

cpp
// 1. 节点初始化
LslidarX10Driver::LslidarX10Driver() {
    // 声明参数
    node_->declare_parameter<std::string>("serial_port", "");
    node_->declare_parameter<std::string>("lidar_model", "N10Plus");
    // ...
}

// 2. 主循环
bool LslidarX10Driver::polling() {
    while (rclcpp::ok()) {
        // 获取数据包
        lslidar_msgs::msg::LslidarPacket::UniquePtr packet;
        int packet_size = data_acquisition_strategy_->getPacket(packet);

        // 验证数据包
        if (!checkPacketValidity(packet, packet_size)) {
            continue; // 这里出了问题！
        }

        // 解析数据并发布
        decodePacket(packet);
        publishScan();
    }
}

// 3. 数据包验证（问题所在！）
bool LslidarX10Driver::checkPacketValidity(...) {
    // 原始验证逻辑过于严格，导致N10Plus数据包被拒绝
    return false; // 总是返回false！
}

input.cpp - 输入处理 ​

cpp
class Input {
    // 支持串口和网络两种模式
    bool getPacket(lslidar_msgs::msg::LslidarPacket::UniquePtr& packet);
};

class SerialStrategy : public Input {
    // 串口策略：从 /dev/ttyACM0 读取
};

三、数据流详解 ​

1. 启动流程 ​

bash
# 用户命令
ros2 run lslidar_driver lslidar_driver_node --ros-args \
  -p serial_port:="/dev/ttyACM0" \
  -p lidar_type:="X10" \
  -p lidar_model:="N10Plus"

# 内部执行顺序：
# 1. ROS 2 加载节点
# 2. 初始化参数
# 3. 创建 SerialStrategy 对象
# 4. 打开串口 /dev/ttyACM0
# 5. 进入 polling() 主循环

2. 数据流路径 ​

雷达硬件 → 串口数据 → SerialStrategy::getPacket() → checkPacketValidity() → decodePacket() → publishScan()
      ↓                ↓                   ↓                   ↓                   ↓
  原始数据          字节流              LslidarPacket       验证通过/失败      解析为LaserScan

3. 问题诊断流程 ​

问题：/scan 话题无数据
排查：
1. 检查串口连接 ✅
2. 检查串口数据流 ✅ (2秒256字节)
3. 检查节点启动日志 ✅ (Successfully opened serial port)
4. 检查数据包验证 ❌ (checkPacketValidity 返回 false)
5. 检查解析函数 ❌ (从未执行到 decodePacket)

四、调试技巧 ​

1. 添加调试日志 ​

cpp
// 在 lslidar_x10_driver.cpp 中添加
RCLCPP_INFO(node_->get_logger(), "Packet size: %d", packet_size);
RCLCPP_INFO(node_->get_logger(), "Check validity: %s",
            checkPacketValidity(packet, packet_size) ? "true" : "false");

2. 使用 gdb 调试 ​

bash
# 编译带调试信息
colcon build --packages-select lslidar_driver --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo

# 使用 gdb 运行
gdb --args install/lslidar_driver/lib/lslidar_driver/lslidar_driver_node \
  --ros-args -p serial_port:="/dev/ttyACM0" -p lidar_model:="N10Plus"

# 在 gdb 中
(gdb) break checkPacketValidity
(gdb) run

3. 数据包分析 ​

python
# Python 脚本分析原始数据
import serial
import struct

ser = serial.Serial('/dev/ttyACM0', 460800, timeout=1)
data = ser.read(1206)  # 标准数据包大小
print(f"Packet size: {len(data)}")
print(f"First 16 bytes: {data[:16].hex()}")

五、参数系统 ​

1. 可配置参数 ​

yaml
# 可通过 --ros-args -p 设置
serial_port: "/dev/ttyACM0" # 串口设备
lidar_type: "X10" # 雷达类型（关键！）
lidar_model: "N10Plus" # 雷达型号
frame_id: "laser" # TF坐标系
publish_scan: true # 是否发布激光数据
baud_rate: 460800 # 波特率（如果支持）
min_range: 0.15 # 最小测量距离
max_range: 50.0 # 最大测量距离

2. 参数传递机制 ​

bash
# ROS 2 参数传递
ros2 run package executable --ros-args -p name:=value

# 实际传递过程：
# 命令行 → rclcpp::Node::declare_parameter() → 节点内部变量

六、编译系统 ​

1. 编译流程 ​

bash
colcon build --packages-select lslidar_driver
# 执行步骤：
# 1. 读取 CMakeLists.txt
# 2. 编译 .cpp 文件为 .o
# 3. 链接为可执行文件
# 4. 安装到 install/ 目录

2. 文件位置 ​

bash
# 可执行文件位置
install/lslidar_driver/lib/lslidar_driver/lslidar_driver_node

# 库文件位置
install/lslidar_driver/lib/

# 启动文件位置（如果有）
install/lslidar_driver/share/lslidar_driver/launch/

七、当前问题与修复 ​

问题根源 ​

cpp
// lslidar_x10_driver.cpp 中的问题函数
bool LslidarX10Driver::checkPacketValidity(...) {
    // N10Plus 雷达的数据包格式与驱动预期不匹配
    // 导致验证总是失败，数据无法进入后续处理
    return false; // ← 这里需要改为 return true
}

修复方案 ​

bash
# 1. 定位函数
grep -n "bool.*checkPacketValidity" src/.../lslidar_x10_driver.cpp

# 2. 修改函数
sed -i '/bool.*checkPacketValidity/,/^[[:space:]]*}/c\
bool LslidarX10Driver::checkPacketValidity(...) {\
    return true; // 临时修复：绕过验证\
}' src/.../lslidar_x10_driver.cpp

# 3. 重新编译
rm -rf build/lslidar_driver install/lslidar_driver
colcon build --packages-select lslidar_driver

# 4. 测试
source install/setup.bash
ros2 run lslidar_driver lslidar_driver_node --ros-args ...

八、完整工作流程 ​

正常流程 ​

1. 硬件连接：雷达 → USB → /dev/ttyACM0
2. 权限设置：chmod 666 /dev/ttyACM0
3. 波特率设置：stty -F /dev/ttyACM0 460800
4. 启动驱动：ros2 run lslidar_driver ...
5. 数据流：串口读取 → 数据包验证 → 解析 → 发布
6. 可视化：rviz2 添加 LaserScan 显示

调试流程 ​

1. 确认硬件：ls /dev/ttyACM*
2. 测试数据流：sudo cat /dev/ttyACM0 | wc -c
3. 查看启动日志：--log-level debug
4. 检查话题：ros2 topic list
5. 检查数据：ros2 topic echo /scan
6. 修改代码 → 重新编译 → 测试