4. 编写传感器发布节点：以激光雷达为例

好，咱们进入实战环节。前面聊了那么多理论，现在该动手写代码了。这一章，我带你创建一个ROS节点，专门用来发布激光雷达的LaserScan消息。

你可能会问：为什么非要用激光雷达举例？因为在实际的机器人项目中，激光雷达几乎是标配。我在做移动机器人导航时，激光雷达的数据质量直接决定了建图和定位的成败。说白了，没有靠谱的激光数据，MoveIt规划出来的路径就是纸上谈兵。

4.1 理解LaserScan消息结构

在写代码之前，咱们得先搞清楚LaserScan消息长什么样。你可以在终端里用这个命令查看：

rosmsg show sensor_msgs/LaserScan

输出结果是这样的：

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities

我来给你拆解一下这些字段的含义：

字段 说明 我的经验
header 消息头，包含时间戳和坐标系ID frame_id一定要设对，我见过太多人因为设成空字符串导致TF报错
angle_min / angle_max 扫描的起始和终止角度（弧度） 注意单位是弧度，不是角度
angle_increment 相邻两个激光点的角度间隔 这个值决定了激光的分辨率
time_increment 每个激光点的时间间隔 对于静止雷达，可以设为0
scan_time 完成一次完整扫描的时间 通常等于 time_increment * 点数
range_min / range_max 有效测量距离范围 超出这个范围的值会被认为是无效
ranges 距离数据数组 长度由角度范围决定
intensities 反射强度数组（可选） 有些雷达不提供这个数据

核心要点：ranges数组的长度 = (angle_max - angle_min) / angle_increment。这个公式你写代码时一定会用到。

4.2 编写激光雷达发布节点

好，现在咱们开始写代码。我会创建一个名为lidar_publisher.py的节点，它会模拟一个激光雷达，不断发布LaserScan消息。

我个人习惯用Python写这类节点，因为调试方便。当然，如果你对C++更熟悉，也可以用C++实现，逻辑是一样的。

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import LaserScan
import math

def publish_laser_scan():
    # 初始化节点
    rospy.init_node('lidar_publisher', anonymous=True)
    
    # 创建发布者
    pub = rospy.Publisher('/scan', LaserScan, queue_size=10)
    
    # 设置发布频率
    rate = rospy.Rate(10)  # 10Hz
    
    # 创建LaserScan消息对象
    scan = LaserScan()
    
    # 设置消息头
    scan.header.frame_id = 'laser_frame'
    
    # 设置扫描参数
    scan.angle_min = -math.pi / 2      # -90度
    scan.angle_max = math.pi / 2       # 90度
    scan.angle_increment = math.pi / 180  # 1度分辨率
    scan.time_increment = 0.0
    scan.scan_time = 0.1
    scan.range_min = 0.1
    scan.range_max = 10.0
    
    # 计算点数
    num_points = int((scan.angle_max - scan.angle_min) / scan.angle_increment)
    
    rospy.loginfo("激光雷达发布节点已启动")
    
    while not rospy.is_shutdown():
        # 更新时间戳
        scan.header.stamp = rospy.Time.now()
        
        # 生成模拟数据
        scan.ranges = []
        for i in range(num_points):
            # 模拟一个简单的障碍物检测
            angle = scan.angle_min + i * scan.angle_increment
            # 假设在0度方向有一个2米远的障碍物
            if abs(angle) < 0.1:
                scan.ranges.append(2.0)
            else:
                scan.ranges.append(float('inf'))
        
        # 发布消息
        pub.publish(scan)
        
        # 按照设定频率休眠
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_laser_scan()
    except rospy.ROSInterruptException:
        pass

小提示：代码中的anonymous=True参数允许你同时运行多个同名节点，调试时特别有用。

4.3 代码逐段解析

这段代码看起来不长，但每个部分都有讲究。我来给你拆开讲讲：

4.3.1 初始化与发布者创建

rospy.init_node('lidar_publisher', anonymous=True)这行代码初始化了节点。节点名是lidar_publisher，你可以在启动时通过__name:=xxx来重命名。

然后我们创建了一个发布者：

pub = rospy.Publisher('/scan', LaserScan, queue_size=10)

这里/scan是话题名，LaserScan是消息类型，queue_size=10表示消息队列长度。我曾经犯过一个错误，把queue_size设成0，结果消息全丢了，排查了半天才发现问题。

4.3.2 消息填充

填充消息时，有几点需要注意：

  • frame_id：必须和你的TF树一致。我习惯用laser_frame，然后在URDF中定义这个坐标系。
  • 角度范围：我这里设了-90度到90度，模拟一个180度扫描的雷达。实际雷达可能是360度的，比如RPLIDAR。
  • 分辨率：1度分辨率对于模拟来说够用了。实际雷达的分辨率通常更高，比如0.5度甚至0.25度。

4.3.3 数据生成与发布

在循环中，我模拟了一个简单的场景：在正前方2米处有一个障碍物。实际项目中，你可能会从真实的激光雷达硬件读取数据，或者从bag文件中回放。

注意rospy.Time.now()的使用。时间戳必须实时更新，否则下游节点可能会认为数据过期。我记得有一次调试导航算法，发现机器人总是撞墙，最后发现是时间戳没更新，MoveIt认为数据太旧直接忽略了。

4.4 运行与测试

写完了代码，咱们来跑一下看看效果。首先确保你的ROS环境已经配置好：

# 启动roscore
roscore

# 在新终端中运行节点
chmod +x lidar_publisher.py
rosrun your_package lidar_publisher.py

然后打开另一个终端，用rostopic echo查看消息：

rostopic echo /scan

你应该能看到类似这样的输出：

header: 
  seq: 123
  stamp: 
    secs: 1695000000
    nsecs: 123456789
  frame_id: "laser_frame"
angle_min: -1.57079632679
angle_max: 1.57079632679
angle_increment: 0.0174532925199
time_increment: 0.0
scan_time: 0.1
range_min: 0.1
range_max: 10.0
ranges: [inf, inf, ..., 2.0, ..., inf, inf]
intensities: []

注意：如果你用rviz可视化，记得把Fixed Frame设为laser_frame，否则看不到激光数据。

4.5 知识体系总览

为了让你更直观地理解这一章的内容，我画了一张图：

激光雷达发布节点知识体系 LaserScan消息结构 Python节点实现 运行与测试验证 header 角度参数 距离参数 ranges数组 初始化节点 创建发布者 填充消息 循环发布 启动roscore 运行节点 rostopic echo rviz可视化 三个核心步骤：理解消息 → 编写代码 → 运行验证

这张图把本章的知识点串起来了。你从理解消息结构开始，然后动手写代码，最后运行验证。每一步都踩实了，后面接入真实传感器时才不会手忙脚乱。

4.6 常见问题与避坑

写这个节点时，有几个坑我帮你提前踩了：

  • frame_id不匹配：我曾经把frame_id写成laser，但TF树里是laser_frame，结果rviz里死活看不到数据。记住，这两个名字必须完全一致。
  • 角度单位搞错：ROS里角度单位是弧度，不是度。我见过有人直接填了90，结果雷达只扫了90弧度，相当于转了14圈多。
  • ranges数组长度不对：如果长度和角度范围不匹配，下游节点会报错。用公式(angle_max - angle_min) / angle_increment算一下，确保整数。
  • 忘记更新时间戳：这个我前面提过，时间戳不更新，MoveIt会认为数据过期，直接不处理。

调试小技巧：如果你不确定消息是否正确，可以用rostopic info /scan查看话题信息，用rostopic hz /scan查看发布频率。这两个命令是我调试时的左膀右臂。

好了，这一章的内容就到这儿。你跟着代码敲一遍，跑起来看看效果，比光看文章强十倍。下一章咱们会聊如何让这个节点和MoveIt对接，到时候你就知道今天写的代码有多重要了。