4、激光雷达驱动开发：编写ROS2驱动节点、配置CMakeLists.txt、发布原始点云数据

好，咱们进入第四章节。前面几章我们把ROS2的基础环境搭好了，也聊了聊激光雷达的选型。今天要动真格的了——写一个真正的ROS2驱动节点，把雷达的原始点云数据发出来。

说实话，我第一次写激光雷达驱动的时候，踩了不少坑。那时候还是ROS1，现在换成ROS2，很多习惯都得改。但核心逻辑没变：读数据、解析数据、发数据。就这么简单。

4.1 驱动节点的整体架构

先画个图，让你对整个流程有个直观印象。我个人习惯在写代码前先画架构图，这样思路清晰，不容易写偏。

嗯，这张图基本说清楚了。驱动节点就是中间的桥梁，一边连硬件，一边连ROS2生态。

4.2 创建ROS2功能包

动手之前，先创建功能包。我习惯用C++写驱动，性能好，控制力强。当然Python也行，但遇到高频雷达时容易丢包。

# 进入工作空间
cd ~/ros2_ws/src

# 创建功能包
ros2 pkg create --build-type ament_cmake lidar_driver \
  --dependencies rclcpp sensor_msgs std_msgs

# 查看生成的结构
tree lidar_driver/

创建完成后，你会看到这样的目录结构：

文件/目录	作用
CMakeLists.txt	编译配置文件，核心中的核心
package.xml	包描述文件，声明依赖
src/	存放C++源文件
include/lidar_driver/	存放头文件
launch/	启动文件目录（手动创建）

4.3 编写驱动节点代码

好，开始写核心代码。我们以常见的单线激光雷达为例，假设它通过串口输出数据，格式是：起始字节 + 角度 + 距离 + 强度。

先写头文件，声明我们的驱动类：

// include/lidar_driver/lidar_driver_node.hpp
#ifndef LIDAR_DRIVER_NODE_HPP
#define LIDAR_DRIVER_NODE_HPP

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <string>
#include <vector>

class LidarDriverNode : public rclcpp::Node
{
public:
  explicit LidarDriverNode(const std::string &node_name);
  ~LidarDriverNode();

private:
  // 串口通信相关
  int serial_fd_;
  std::string port_;
  int baudrate_;

  // 数据解析
  void parseRawData(const std::vector<uint8_t> &buffer);
  void publishScan(const std::vector<float> &ranges,
                   const std::vector<float> &intensities);

  // 定时器驱动
  rclcpp::TimerBase::SharedPtr timer_;
  void timerCallback();

  // 发布器
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_;
};

#endif

嗯，这里要注意：LaserScan消息是ROS2里专门为单线激光雷达设计的。它包含角度范围、距离数组、强度数组等字段。说白了，就是一圈扫描点的集合。

接下来是实现文件。我挑重点说：

// src/lidar_driver_node.cpp
#include "lidar_driver/lidar_driver_node.hpp"
#include <fcntl.h>
#include <unistd.h>
#include <termios.h>

LidarDriverNode::LidarDriverNode(const std::string &node_name)
  : Node(node_name)
{
  // 声明参数
  this->declare_parameter<std::string>("port", "/dev/ttyUSB0");
  this->declare_parameter<int>("baudrate", 115200);

  port_ = this->get_parameter("port").as_string();
  baudrate_ = this->get_parameter("baudrate").as_int();

  // 创建发布器
  scan_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>(
    "scan", rclcpp::QoS(10));

  // 打开串口
  serial_fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY);
  if (serial_fd_ < 0) {
    RCLCPP_ERROR(this->get_logger(), "无法打开串口: %s", port_.c_str());
    return;
  }

  // 配置串口参数（省略具体设置）
  // ...

  // 启动定时器，10ms读取一次
  timer_ = this->create_wall_timer(
    std::chrono::milliseconds(10),
    std::bind(&LidarDriverNode::timerCallback, this));

  RCLCPP_INFO(this->get_logger(), "激光雷达驱动节点已启动");
}

定时器回调函数里做数据读取和解析：

void LidarDriverNode::timerCallback()
{
  std::vector<uint8_t> buffer(1024);
  int len = read(serial_fd_, buffer.data(), buffer.size());

  if (len > 0) {
    buffer.resize(len);
    parseRawData(buffer);
  }
}

void LidarDriverNode::parseRawData(const std::vector<uint8_t> &buffer)
{
  // 这里根据雷达协议解析
  // 假设协议：0xAA 0x55 + 角度(2字节) + 距离(2字节) + 强度(1字节)
  // 实际项目中请替换为真实协议

  std::vector<float> ranges;
  std::vector<float> intensities;

  for (size_t i = 0; i + 5 < buffer.size(); i += 6) {
    if (buffer[i] == 0xAA && buffer[i+1] == 0x55) {
      // 解析角度（单位：0.01度）
      uint16_t angle_raw = (buffer[i+2] << 8) | buffer[i+3];
      float angle = angle_raw * 0.01f;

      // 解析距离（单位：毫米）
      uint16_t dist_raw = (buffer[i+4] << 8) | buffer[i+5];
      float distance = dist_raw / 1000.0f;  // 转为米

      // 这里简化处理，实际需要按角度索引填充
      ranges.push_back(distance);
      intensities.push_back(0.0f);
    }
  }

  publishScan(ranges, intensities);
}

最后是发布函数，把解析好的数据包装成LaserScan消息：

void LidarDriverNode::publishScan(
    const std::vector<float> &ranges,
    const std::vector<float> &intensities)
{
  auto scan_msg = sensor_msgs::msg::LaserScan();

  // 填充消息头
  scan_msg.header.stamp = this->now();
  scan_msg.header.frame_id = "laser_frame";

  // 角度参数（根据雷达实际参数设置）
  scan_msg.angle_min = -M_PI;      // -180度
  scan_msg.angle_max = M_PI;       // 180度
  scan_msg.angle_increment = 2 * M_PI / 360;  // 1度分辨率
  scan_msg.time_increment = 1.0f / 360.0f;    // 每点时间间隔
  scan_msg.scan_time = 1.0f / 10.0f;          // 10Hz扫描频率
  scan_msg.range_min = 0.1;        // 最小距离0.1米
  scan_msg.range_max = 10.0;       // 最大距离10米

  // 填充距离和强度数据
  scan_msg.ranges = ranges;
  scan_msg.intensities = intensities;

  // 发布
  scan_pub_->publish(scan_msg);
}

4.4 配置CMakeLists.txt

代码写完了，但编译不过去就尴尬了。CMakeLists.txt是ROS2 C++项目的命脉，配置不对，一切白搭。

我直接给你一个可用的模板：

cmake_minimum_required(VERSION 3.8)
project(lidar_driver)

# 使用C++17标准
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 查找依赖
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)

# 包含头文件目录
include_directories(include)

# 添加可执行文件
add_executable(lidar_driver_node
  src/lidar_driver_node.cpp
)

# 链接库
ament_target_dependencies(lidar_driver_node
  rclcpp
  sensor_msgs
  std_msgs
)

# 安装可执行文件
install(TARGETS
  lidar_driver_node
  DESTINATION lib/${PROJECT_NAME}
)

# 安装启动文件（如果有）
install(DIRECTORY
  launch
  DESTINATION share/${PROJECT_NAME}
)

# 安装配置文件（如果有）
install(DIRECTORY
  config
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

这里有几个坑，我当年都踩过：

常见错误	后果	解决办法
忘记find_package(sensor_msgs)	编译报错：找不到LaserScan类型	检查package.xml和CMakeLists.txt的依赖声明
头文件路径不对	找不到头文件	用include_directories(include)指定路径
忘记install TARGETS	ros2 run找不到节点	必须显式安装可执行文件
ament_target_dependencies拼错	链接失败	注意是ament_target_dependencies，不是target_link_libraries

4.5 编译与测试

代码和配置都准备好了，开始编译：

# 回到工作空间根目录
cd ~/ros2_ws

# 编译
colcon build --packages-select lidar_driver

# 如果编译成功，你会看到类似输出：
# Summary: 1 package finished [XX.Xs]

# 配置环境
source install/setup.bash

# 运行节点（假设雷达已连接）
ros2 run lidar_driver lidar_driver_node

运行后，用以下命令验证：

# 查看话题列表
ros2 topic list
# 应该能看到 /scan

# 查看话题数据
ros2 topic echo /scan
# 会输出LaserScan消息内容

# 查看发布频率
ros2 topic hz /scan
# 正常应该在10Hz左右

4.6 在Rviz2中可视化

看到数据还不够，得在Rviz2里亲眼看到点云才踏实：

# 启动Rviz2
rviz2

# 在Rviz2中：
# 1. 将Fixed Frame设置为 "laser_frame"
# 2. 添加显示类型：Add -> LaserScan
# 3. 在LaserScan属性中，选择话题 /scan

如果一切正常，你会看到一圈点云，代表雷达扫描到的障碍物。嗯，看到这个画面，说明你的驱动节点已经跑通了。

驱动节点开发好了，原始点云数据也能发布了。接下来，这些数据会流向哪里？嗯，那就是后面章节要聊的内容了。