3、发布者节点实现：编写Python发布者节点、设置发布频率、发布消息数据
好，咱们接着聊。上一节我们把话题通信的底层逻辑理清了，现在该动手了。
这一节，我带你亲手写一个发布者节点。说白了，就是让机器人里的某个程序，能定时往外喊话：「我这里有地图数据，谁要谁拿走！」
我个人习惯，学ROS2先从Python入手。为什么？因为快。你写几行代码就能跑起来，看到效果，信心就来了。C++当然更高效，但调试起来麻烦不少。咱们先跑通，再优化。
3.1 创建功能包与节点文件
在开始写代码之前，你得先有个「家」——也就是功能包。我建议你直接在终端里敲命令，别用IDE新建，那样容易漏掉依赖配置。
ros2 pkg create map_publisher_pkg --build-type ament_python --dependencies rclpy std_msgs nav_msgs这条命令干了三件事：
- 创建了一个叫
map_publisher_pkg的功能包 - 指定了Python作为开发语言
- 自动添加了三个依赖：
rclpy（ROS2客户端库）、std_msgs（标准消息类型）、nav_msgs（导航相关消息，地图数据就在这里面）
嗯，这里要注意：nav_msgs 里的 OccupancyGrid 消息类型，就是咱们传输地图数据的核心。它本质上是一个二维数组，每个格子存一个值：0表示空闲，100表示障碍，-1表示未知。
3.2 编写发布者节点代码
接下来，在 map_publisher_pkg/map_publisher_pkg/ 目录下新建一个文件，叫 map_publisher.py。我直接贴代码，然后一行行给你拆解。
import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from std_msgs.msg import Header
import numpy as np
class MapPublisher(Node):
def __init__(self):
super().__init__('map_publisher')
self.publisher_ = self.create_publisher(OccupancyGrid, 'map_topic', 10)
timer_period = 1.0 # 每秒发布一次
self.timer = self.create_timer(timer_period, self.timer_callback)
self.get_logger().info('地图发布节点已启动！')
def timer_callback(self):
msg = OccupancyGrid()
msg.header = Header()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.frame_id = 'map'
# 设置地图元信息
msg.info.resolution = 0.05 # 每个格子0.05米
msg.info.width = 100
msg.info.height = 100
msg.info.origin.position.x = -2.5
msg.info.origin.position.y = -2.5
msg.info.origin.position.z = 0.0
msg.info.origin.orientation.w = 1.0
# 生成模拟地图数据
map_data = np.zeros((100, 100), dtype=np.int8)
# 在中间画一个障碍物方块
map_data[40:60, 40:60] = 100
# 随机加一些噪声点
map_data[20:30, 70:80] = 50
msg.data = map_data.flatten().tolist()
self.publisher_.publish(msg)
self.get_logger().info(f'发布了地图数据，尺寸：{msg.info.width}x{msg.info.height}')
def main(args=None):
rclpy.init(args=args)
map_publisher = MapPublisher()
rclpy.spin(map_publisher)
map_publisher.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
这段代码，我拆成几个关键点来讲。
3.3 核心机制拆解
第一，创建发布者。
self.create_publisher(OccupancyGrid, 'map_topic', 10) 这行，指定了消息类型是 OccupancyGrid，话题名叫 map_topic，队列深度是10。队列深度什么意思？就是如果订阅者处理不过来，最多缓存10条消息，再新的就丢掉。我在项目中遇到过，如果地图数据特别大，队列深度设太大反而会撑爆内存。10是个比较稳妥的值。
第二，定时器驱动。
self.create_timer(timer_period, self.timer_callback) 这行，设置了一个1秒的定时器。每过1秒，ROS2就会自动调用 timer_callback 函数。这就是发布频率的控制方式。你想10Hz？那就设0.1秒。想0.5Hz？设2秒。很简单。
第三，消息填充。
地图消息的核心是 msg.info 和 msg.data。info 告诉接收方：地图有多大、每个格子代表多少米、原点在哪。data 就是实实在在的格子数据。我这里用numpy生成了一个100x100的模拟地图，中间画了个障碍物方块。实际项目中，你会从SLAM算法或者加载文件里拿数据。
重要提醒： msg.data 必须是一维列表。如果你用二维数组，记得用 flatten() 压平。我曾经有个同事，忘了这步，调试了一下午才发现数据发出去全是乱的。
3.4 设置发布频率的讲究
发布频率不是随便设的。我整理了一张表，方便你对照选择：
| 应用场景 | 推荐频率 | 说明 |
|---|---|---|
| 静态地图加载 | 1 Hz | 地图不变，发布一次就够了，但为了容错可以低频重复 |
| 实时SLAM建图 | 5-10 Hz | 机器人移动时，地图需要快速更新 |
| 高精度定位 | 20-50 Hz | 配合激光雷达或视觉，需要高频地图匹配 |
| 大尺寸地图传输 | 0.5-2 Hz | 地图太大，频率高了网络带宽扛不住 |
你想想看，如果你发布一个1000x1000的地图，每个格子占1字节，那就是1MB的数据。10Hz发布，每秒就是10MB流量。WiFi扛得住吗？不一定。所以频率和地图尺寸要权衡。
3.5 运行与验证
代码写好了，怎么跑？两步走：
- 编译功能包：在工作空间根目录执行
colcon build --packages-select map_publisher_pkg - 运行节点：
ros2 run map_publisher_pkg map_publisher
你会看到终端每隔1秒打印一条日志：「发布了地图数据，尺寸：100x100」。这时候再开一个终端，用 ros2 topic echo /map_topic 就能看到原始数据流。
小技巧： 用 ros2 topic hz /map_topic 可以查看实际发布频率。如果发现频率低于你设定的值，说明你的回调函数执行时间太长了。这时候要么优化代码，要么降低频率。
3.6 避坑指南
我曾经在项目里犯过一个低级错误：忘了设置 msg.header.stamp 的时间戳。结果订阅端拿到的地图数据，时间全是0，导致整个定位系统时序错乱。所以记住：时间戳一定要用 self.get_clock().now().to_msg() 来生成，别自己手动填。
还有一个坑：frame_id 要跟你的TF树对得上。如果你写死了 'map'，但实际TF树里叫 'global_map'，那订阅端做坐标变换时会直接报错。我建议你把这个参数化，别写死。
3.7 本章知识体系
为了让你更直观地理解发布者节点的整体结构，我画了一张流程图：
这张图把整个流程串起来了：节点启动 → 初始化发布者和定时器 → 定时触发回调 → 构建消息 → 发布出去。每一步都环环相扣，缺一不可。
好，发布者节点就讲到这里。代码不多，但背后的机制值得你多琢磨。下一节我们讲订阅者怎么接收这些地图数据，到时候你会看到，接收端其实更简单，但坑也不少。