ROS2 核心编程实战：从零搭建一个虚拟小车控制系统 Lv1
学完能做什么：理解并手写节点、话题、服务、动作、参数，能把它们组合使用
语言：Python 3
适用版本：ROS2 Humble / Jazzy（Ubuntu 22.04 / 24.04）
目录
- 五个核心概念的关系图——先建立全局认知
- 准备工作：创建工作空间和功能包
- 节点（Node）：系统的最小运行单元
- 话题（Topic）：持续的数据流
- 服务（Service）：一问一答的请求响应
- 动作（Action）：有进度反馈的长任务
- 参数（Parameter）：节点的配置中心
- 综合实战：把五个概念组合成完整系统
- 概念速查：什么情况用什么？
1、五个核心概念的关系图——先建立全局认知
在写第一行代码之前，先搞清楚这五个东西的关系。
一句话总结每个概念：
| 概念 | 一句话 | 类比 |
|---|---|---|
| Node（节点） | 一个独立运行的程序 | 公司里的一个员工 |
| Topic（话题） | 持续广播，不等回复 | 广播电台 |
| Service（服务） | 问一个问题，等一个答案 | 打电话咨询 |
| Action（动作） | 交代一个耗时任务，实时汇报进度 | 委托快递，可追踪 |
| Parameter（参数） | 节点运行时的可调配置 | 员工手册 |
2、准备工作：创建工作空间和功能包
2.1 创建工作空间
# 创建工作空间目录
mkdir -p ~/ros2_learning_ws/src
cd ~/ros2_learning_ws
# 初始化构建（先 build 一次生成必要文件）
colcon build
source install/setup.bash
2.2 创建 Python 功能包
cd ~/ros2_learning_ws/src
# 创建功能包（Python类型）
ros2 pkg create --build-type ament_python learning_pkg
# 查看生成的目录结构
tree learning_pkg/
生成的目录结构如下：
learning_pkg/
├── learning_pkg/ ← 放 Python 代码的地方
│ └── __init__.py
├── resource/
│ └── learning_pkg
├── setup.cfg
├── setup.py ← 注册可执行脚本的地方
└── package.xml ← 声明依赖的地方
2.3 修改 package.xml，添加依赖
打开 package.xml，在 <buildtool_depend> 下方添加：
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>action_msgs</exec_depend>
2.4 修改 setup.py，注册脚本入口
from setuptools import setup
package_name = 'learning_pkg'
setup(
name=package_name,
version='0.0.1',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='your_name',
maintainer_email='your@email.com',
description='ROS2 学习功能包',
license='Apache-2.0',
tests_require=['pytest'],
entry_points={
'console_scripts': [
# 格式：'命令名 = 包名.文件名:函数名'
'simple_node = learning_pkg.simple_node:main',
'speed_publisher = learning_pkg.speed_publisher:main',
'speed_subscriber = learning_pkg.speed_subscriber:main',
'stop_server = learning_pkg.stop_server:main',
'stop_client = learning_pkg.stop_client:main',
'drive_action_server = learning_pkg.drive_action_server:main',
'drive_action_client = learning_pkg.drive_action_client:main',
'param_node = learning_pkg.param_node:main',
'robot_system = learning_pkg.robot_system:main',
],
},
)
3、节点（Node）：系统的最小运行单元
什么是节点？
节点就是一个独立运行的 Python 程序，它能与其他节点通信。ROS2 系统里所有的功能都跑在节点里。
关键点：
- 一个进程 = 一个节点（通常）
- 节点有名字（唯一标识）
- 节点死了，它提供的所有服务/话题都消失
最简单的节点
新建文件 ~/ros2_learning_ws/src/learning_pkg/learning_pkg/simple_node.py：
import rclpy # ROS2 Python 客户端库
from rclpy.node import Node # 节点基类
class SimpleNode(Node): # 继承 Node 基类
def __init__(self):
# 调用父类初始化，传入节点名称
# 节点名称是这个节点在 ROS2 网络中的唯一标识
super().__init__('simple_node')
# 获取 ROS2 自带的日志器（比 print 更规范）
self.get_logger().info(' 节点已启动！节点名：simple_node')
# 创建一个定时器：每隔 1.0 秒执行一次 timer_callback
self.timer = self.create_timer(1.0, self.timer_callback)
# 计数器
self.count = 0
def timer_callback(self):
"""每秒钟执行一次的回调函数"""
self.count += 1
self.get_logger().info(f'我还活着！这是第 {self.count} 秒')
def main(args=None):
# ① 初始化 ROS2 通信
rclpy.init(args=args)
# ② 创建节点实例
node = SimpleNode()
# ③ 让节点持续运行（等待事件、处理回调）
# spin() 是阻塞的，直到用 Ctrl+C 退出
rclpy.spin(node)
# ④ 退出后清理资源
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
构建并运行
# 回到工作空间根目录构建
cd ~/ros2_learning_ws
colcon build --packages-select learning_pkg
# 加载环境变量（每次新开终端都要执行）
source install/setup.bash
# 运行节点
ros2 run learning_pkg simple_node
输出：
[INFO] [simple_node]: 节点已启动！节点名：simple_node
[INFO] [simple_node]: 我还活着！这是第 1 秒
[INFO] [simple_node]: 我还活着！这是第 2 秒
...
另开一个终端，验证节点存在：
ros2 node list # 查看所有运行中的节点
ros2 node info /simple_node # 查看节点详细信息
4、话题（Topic）：持续的数据流
什么是话题？
话题是 ROS2 中最常用的通信方式，适合持续、单向的数据传输。
核心规则：
- 发布者：把数据推送出去，不等待回应
- 订阅者：监听话题，有数据来就触发回调
- 多对多：多个发布者 + 多个订阅者都可以
- 话题名 + 消息类型必须匹配才能通信
什么时候用话题？
√ 传感器数据（激光雷达、IMU）：持续输出，不需要响应
√ 速度指令：控制器持续发送/cmd_vel
√ 状态广播：电量、位置、状态标志
× 需要等待结果的操作（用 Service）
× 耗时的长任务（用 Action）
4.1 发布者：持续广播小车速度
新建 speed_publisher.py：
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist # ROS2 标准速度消息类型
import math
class SpeedPublisher(Node):
def __init__(self):
super().__init__('speed_publisher')
# ─── 创建发布者 ───
# 参数1：消息类型（Twist 包含线速度和角速度）
# 参数2：话题名称（约定：控制小车速度用 /cmd_vel）
# 参数3：队列长度（消息缓冲区，一般设 10）
self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
# 每 0.1 秒发布一次速度指令（10Hz）
self.timer = self.create_timer(0.1, self.publish_speed)
self.t = 0.0 # 时间变量，用来生成变化的速度
self.get_logger().info('速度发布节点已启动，话题：/cmd_vel')
def publish_speed(self):
"""定时器回调：构造并发布速度消息"""
msg = Twist() # 创建空消息
# Twist 消息结构：
# msg.linear.x = 前进速度 (m/s)，正值=前进，负值=后退
# msg.linear.y = 横向速度 (m/s)，差速机器人通常为 0
# msg.angular.z = 旋转速度 (rad/s)，正值=左转，负值=右转
# 模拟一个走"S形"曲线的小车
self.t += 0.1
msg.linear.x = 0.5 # 前进速度：0.5 m/s
msg.angular.z = 0.3 * math.sin(self.t) # 左右摆动的角速度
# 发布消息
self.publisher_.publish(msg)
self.get_logger().debug(
f'发布速度 → 前进: {msg.linear.x:.2f} m/s, '
f'转向: {msg.angular.z:.3f} rad/s'
)
def main(args=None):
rclpy.init(args=args)
node = SpeedPublisher()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
4.2 订阅者：监听速度数据
新建 speed_subscriber.py：
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
class SpeedSubscriber(Node):
def __init__(self):
super().__init__('speed_subscriber')
# ─── 创建订阅者 ───
# 参数1：消息类型（必须和发布者一致！）
# 参数2：话题名称（必须和发布者一致！）
# 参数3：回调函数（有消息来就调用它）
# 参数4：队列长度
self.subscription = self.create_subscription(
Twist,
'/cmd_vel',
self.speed_callback, # 收到消息时自动调用这个函数
10
)
self.get_logger().info('速度订阅节点已启动，正在监听 /cmd_vel...')
# 统计信息
self.msg_count = 0
self.total_distance = 0.0 # 模拟累计行驶距离
def speed_callback(self, msg: Twist):
"""
每次收到速度消息时被调用。
msg 就是发布者发来的 Twist 消息对象。
"""
self.msg_count += 1
# 估算行驶距离（速度 × 时间间隔 ≈ 0.1s）
dt = 0.1
self.total_distance += abs(msg.linear.x) * dt
self.get_logger().info(
f'[第{self.msg_count}条] '
f'前进速度: {msg.linear.x:.2f} m/s | '
f'转向速度: {msg.angular.z:.3f} rad/s | '
f'估算总里程: {self.total_distance:.2f} m'
)
def main(args=None):
rclpy.init(args=args)
node = SpeedSubscriber()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
4.3 运行并验证
终端 1（发布者）：
ros2 run learning_pkg speed_publisher
终端 2（订阅者）：
ros2 run learning_pkg speed_subscriber
终端 3（命令行验证）：
# 查看所有话题
ros2 topic list
# 实时查看 /cmd_vel 话题的数据
ros2 topic echo /cmd_vel
# 查看话题的发布频率
ros2 topic hz /cmd_vel
# 查看话题的消息类型
ros2 topic info /cmd_vel
5、服务（Service）：一问一答的请求响应
什么是服务？
服务是同步的请求-响应通信。客户端发出请求，阻塞等待，服务端处理后返回结果。
什么时候用服务？
√ 需要立即得到结果：急停命令、查询当前状态
√ 操作时间很短（毫秒到秒级）
√ 一次性触发型操作：切换模式、复位、校准
× 耗时超过几秒的任务（客户端会超时，用 Action）
× 持续数据流（用 Topic）
5.1 定义服务接口
ROS2 使用 .srv 文件定义服务接口，格式：
请求字段
---
响应字段
这里我们先用 ROS2 自带的 std_srvs/srv/SetBool 来演示（请求一个 bool，响应一个 bool + 消息）：
# SetBool.srv 的内容（ROS2 自带，不需要自己创建）
bool data # 请求：true=停止, false=恢复
---
bool success # 响应：是否成功
string message # 响应：说明信息
5.2 服务端：处理急停请求
新建 stop_server.py：
import rclpy
from rclpy.node import Node
from std_srvs.srv import SetBool # 使用 ROS2 自带的 SetBool 服务类型
class EmergencyStopServer(Node):
def __init__(self):
super().__init__('emergency_stop_server')
# ─── 创建服务端 ───
# 参数1：服务类型
# 参数2：服务名称（约定：命名清晰，表明功能）
# 参数3：回调函数（收到请求时调用）
self.srv = self.create_service(
SetBool,
'/emergency_stop',
self.handle_stop_request
)
# 内部状态
self.is_stopped = False
self.get_logger().info('急停服务已就绪，等待请求...')
def handle_stop_request(self, request, response):
"""
每次收到服务请求时被调用。
request: 客户端发来的请求对象
response: 需要填写并返回的响应对象
"""
# 从请求中读取数据
# request.data = True → 触发急停
# request.data = False → 解除急停
if request.data:
# 触发急停
self.is_stopped = True
response.success = True
response.message = '⚠ 急停已触发！小车已停止运动'
self.get_logger().warn('急停命令执行：停止所有运动')
else:
# 解除急停
self.is_stopped = False
response.success = True
response.message = '√ 急停解除，小车可以继续运动'
self.get_logger().info('急停解除：恢复正常运行')
# 必须返回 response 对象！
return response
def main(args=None):
rclpy.init(args=args)
node = EmergencyStopServer()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
5.3 客户端：发起急停请求
新建 stop_client.py：
import sys
import rclpy
from rclpy.node import Node
from std_srvs.srv import SetBool
class EmergencyStopClient(Node):
def __init__(self):
super().__init__('emergency_stop_client')
# ─── 创建客户端 ───
# 参数1：服务类型（必须和服务端一致！）
# 参数2：服务名称（必须和服务端一致！）
self.client = self.create_client(SetBool, '/emergency_stop')
# 等待服务端上线（最多等 5 秒）
self.get_logger().info('等待急停服务上线...')
while not self.client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('服务未就绪，继续等待...')
self.get_logger().info('急停服务已连接！')
def send_stop_request(self, stop: bool):
"""发送急停/解除急停请求"""
# 创建请求对象并填写数据
request = SetBool.Request()
request.data = stop # True=停车，False=解除
# 异步发送请求（不阻塞）
future = self.client.call_async(request)
# 等待响应结果
rclpy.spin_until_future_complete(self, future)
# 获取响应
response = future.result()
if response is not None:
self.get_logger().info(
f'服务响应 → 成功: {response.success}, '
f'消息: {response.message}'
)
else:
self.get_logger().error('请求失败，未收到响应！')
return response
def main(args=None):
rclpy.init(args=args)
node = EmergencyStopClient()
# 从命令行读取参数，决定停车还是解除
# 用法：ros2 run learning_pkg stop_client stop
# ros2 run learning_pkg stop_client resume
if len(sys.argv) > 1 and sys.argv[1] == 'stop':
node.send_stop_request(True) # 触发急停
else:
node.send_stop_request(False) # 解除急停
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
5.4 运行并验证
终端 1（服务端）：
ros2 run learning_pkg stop_server
终端 2（客户端触发急停）：
ros2 run learning_pkg stop_client stop
# 输出: 服务响应 → 成功: True, 消息: ⚠️ 急停已触发！
终端 2（客户端解除急停）：
ros2 run learning_pkg stop_client resume
# 输出: 服务响应 → 成功: True, 消息: ✅ 急停解除，小车可以继续运动
命令行调用服务（无需写代码）：
# 查看服务列表
ros2 service list
# 直接命令行调用
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: true}"
6、动作（Action）：有进度反馈的长任务
什么是动作？
动作是 ROS2 中最复杂的通信机制，专门为耗时且需要反馈的任务设计。
什么时候用动作？
√ 导航到某个坐标点（可能需要几十秒）
√ 执行"前进 X 米"（需要实时反馈已走了多少）
√ 机械臂抓取物体（多步骤，需要进度）
√ 任何需要取消能力的长任务
× 简短的操作（用 Service）
× 持续数据流（用 Topic）
6.1 创建自定义 Action 接口
首先需要创建一个接口包来定义 Action 的数据结构：
cd ~/ros2_learning_ws/src
ros2 pkg create --build-type ament_cmake learning_interfaces
mkdir learning_interfaces/action
新建 learning_interfaces/action/DriveDistance.action：
# 目标（Goal）：客户端告诉服务端要做什么
float32 target_distance # 目标距离（米）
float32 speed # 行驶速度（m/s）
---
# 结果（Result）：任务完成时返回
float32 actual_distance # 实际行驶距离
bool success # 是否成功完成
string message # 结果说明
---
# 反馈（Feedback）：执行过程中持续发送
float32 current_distance # 已行驶距离
float32 percentage # 完成百分比（0.0~1.0）
修改 learning_interfaces/CMakeLists.txt，添加 Action 编译声明：
cmake_minimum_required(VERSION 3.8)
project(learning_interfaces)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/DriveDistance.action"
)
ament_package()
修改 learning_interfaces/package.xml，添加：
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
构建接口包：
cd ~/ros2_learning_ws
colcon build --packages-select learning_interfaces
source install/setup.bash
6.2 动作服务端：执行"前进 X 米"
新建 drive_action_server.py：
import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from learning_interfaces.action import DriveDistance # 我们自定义的 Action 类型
class DriveDistanceServer(Node):
def __init__(self):
super().__init__('drive_distance_server')
# ─── 创建动作服务端 ───
self._action_server = ActionServer(
self,
DriveDistance, # Action 类型
'/drive_distance', # Action 名称
self.execute_callback, # 执行回调（核心逻辑在这里）
goal_callback=self.goal_callback, # 是否接受目标
cancel_callback=self.cancel_callback, # 是否允许取消
)
self.get_logger().info('前进动作服务端已就绪，等待任务...')
def goal_callback(self, goal_request):
"""
收到新目标时调用，决定是否接受。
可以在这里做参数合法性检查。
"""
dist = goal_request.target_distance
speed = goal_request.speed
if dist <= 0 or speed <= 0:
self.get_logger().warn(f'拒绝任务：参数非法（距离={dist}, 速度={speed}）')
return GoalResponse.REJECT
self.get_logger().info(f'接受任务：前进 {dist} 米，速度 {speed} m/s')
return GoalResponse.ACCEPT
def cancel_callback(self, goal_handle):
"""收到取消请求时调用，决定是否允许取消"""
self.get_logger().info('收到取消请求，允许取消')
return CancelResponse.ACCEPT
async def execute_callback(self, goal_handle):
"""
任务执行的核心逻辑（异步函数）。
goal_handle 包含目标数据，并用于发送反馈和结果。
"""
# 读取目标参数
target_distance = goal_handle.request.target_distance
speed = goal_handle.request.speed
self.get_logger().info(
f'开始执行：前进 {target_distance} 米，速度 {speed} m/s'
)
# ─── 执行任务主循环 ───
current_distance = 0.0
dt = 0.1 # 时间步长：0.1 秒
while current_distance < target_distance:
# ① 检查是否收到取消请求
if goal_handle.is_cancel_requested:
goal_handle.canceled()
self.get_logger().info(f'任务已取消！已行驶 {current_distance:.2f} 米')
# 返回被取消的结果
result = DriveDistance.Result()
result.actual_distance = current_distance
result.success = False
result.message = f'任务被取消，已行驶 {current_distance:.2f} 米'
return result
# ② 模拟行驶（实际项目中，这里会发布 /cmd_vel 话题）
current_distance += speed * dt
# 防止超过目标距离
if current_distance > target_distance:
current_distance = target_distance
# ③ 发送反馈（让客户端知道进度）
feedback = DriveDistance.Feedback()
feedback.current_distance = current_distance
feedback.percentage = current_distance / target_distance
goal_handle.publish_feedback(feedback)
self.get_logger().info(
f'进度：{current_distance:.2f}/{target_distance:.2f} 米 '
f'({feedback.percentage * 100:.1f}%)'
)
# ④ 等待下一个时间步（模拟实时执行）
time.sleep(dt)
# ─── 任务完成 ───
goal_handle.succeed()
result = DriveDistance.Result()
result.actual_distance = current_distance
result.success = True
result.message = f'√ 成功！实际行驶 {current_distance:.2f} 米'
self.get_logger().info(f'任务完成！{result.message}')
return result
def main(args=None):
rclpy.init(args=args)
node = DriveDistanceServer()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
6.3 动作客户端：发送前进指令
新建 drive_action_client.py：
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from learning_interfaces.action import DriveDistance
class DriveDistanceClient(Node):
def __init__(self):
super().__init__('drive_distance_client')
# ─── 创建动作客户端 ───
self._action_client = ActionClient(
self,
DriveDistance,
'/drive_distance'
)
def send_goal(self, distance: float, speed: float):
"""发送行驶目标"""
self.get_logger().info('等待动作服务端上线...')
self._action_client.wait_for_server()
# 构造目标
goal = DriveDistance.Goal()
goal.target_distance = distance
goal.speed = speed
self.get_logger().info(f'发送目标：前进 {distance} 米，速度 {speed} m/s')
# 异步发送目标，注册反馈回调
self._send_goal_future = self._action_client.send_goal_async(
goal,
feedback_callback=self.feedback_callback # 收到反馈时调用
)
# 目标被接受/拒绝后的回调
self._send_goal_future.add_done_callback(self.goal_response_callback)
def goal_response_callback(self, future):
"""目标被服务端接受或拒绝时调用"""
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('× 目标被服务端拒绝！')
return
self.get_logger().info('√ 目标已被接受，任务执行中...')
# 注册最终结果的回调
self._get_result_future = goal_handle.get_result_async()
self._get_result_future.add_done_callback(self.result_callback)
def feedback_callback(self, feedback_msg):
"""收到服务端进度反馈时调用（持续触发）"""
feedback = feedback_msg.feedback
self.get_logger().info(
f' 进度反馈：{feedback.current_distance:.2f} 米 '
f'/ {feedback.percentage * 100:.1f}%'
)
def result_callback(self, future):
"""任务最终完成时调用"""
result = future.result().result
self.get_logger().info(
f'🏁 任务结束 → 成功: {result.success}, '
f'消息: {result.message}'
)
def main(args=None):
rclpy.init(args=args)
node = DriveDistanceClient()
# 发送任务：前进 5 米，速度 1.0 m/s
node.send_goal(distance=5.0, speed=1.0)
# spin 等待所有回调完成
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
6.4 运行并验证
终端 1（服务端）：
ros2 run learning_pkg drive_action_server
终端 2（客户端）：
ros2 run learning_pkg drive_action_client
# 观察进度反馈持续输出...
命令行操作：
# 查看动作服务
ros2 action list
ros2 action info /drive_distance
# 命令行发送目标
ros2 action send_goal /drive_distance \
learning_interfaces/action/DriveDistance \
"{target_distance: 3.0, speed: 0.5}" \
--feedback # 显示实时反馈
7、参数（Parameter）：节点的配置中心
什么是参数？
参数是节点自己的配置变量，存储在节点内部，可以在运行时动态修改，不需要重启节点。
什么时候用参数？
√ 配置类数据：最大速度、PID 参数、传感器偏移量
√ 需要运行时调整而不重启的值
√ 不同部署环境的差异化配置
× 高频变化的传感器数据（用 Topic）
× 触发操作（用 Service）
参数节点实现
新建 param_node.py：
import rclpy
from rclpy.node import Node
from rcl_interfaces.msg import ParameterDescriptor # 参数描述器（可选）
class RobotParamNode(Node):
def __init__(self):
super().__init__('robot_param_node')
# ─── 声明参数 ───
# declare_parameter(名称, 默认值, 描述)
# 所有参数必须先声明才能使用！
# 声明最大速度参数
self.declare_parameter(
'max_speed',
1.5, # 默认值：1.5 m/s
ParameterDescriptor(description='小车最大线速度 (m/s)')
)
# 声明控制频率参数
self.declare_parameter(
'control_frequency',
10, # 默认值：10 Hz
ParameterDescriptor(description='控制循环频率 (Hz)')
)
# 声明车辆名称参数
self.declare_parameter(
'robot_name',
'my_robot',
ParameterDescriptor(description='机器人名称')
)
# 声明调试模式参数
self.declare_parameter(
'debug_mode',
False,
ParameterDescriptor(description='是否开启调试模式')
)
# ─── 读取参数值 ───
self.max_speed = self.get_parameter('max_speed').value
freq = self.get_parameter('control_frequency').value
self.get_logger().info(
f'节点启动，参数：\n'
f' max_speed = {self.max_speed} m/s\n'
f' control_frequency = {freq} Hz\n'
f' robot_name = {self.get_parameter("robot_name").value}\n'
f' debug_mode = {self.get_parameter("debug_mode").value}'
)
# ─── 注册参数变化监听 ───
# 当外部修改参数时，自动调用这个回调函数
self.add_on_set_parameters_callback(self.parameter_changed_callback)
# 定时器：每秒检查一次当前参数并使用
timer_period = 1.0 / freq
self.timer = self.create_timer(timer_period, self.control_loop)
def parameter_changed_callback(self, params):
"""
当任意参数被修改时调用。
params 是被修改的参数列表。
"""
from rcl_interfaces.msg import SetParametersResult
for param in params:
if param.name == 'max_speed':
self.max_speed = param.value
self.get_logger().info(f' 参数已更新：max_speed = {self.max_speed} m/s')
elif param.name == 'debug_mode':
self.get_logger().info(
f' 参数已更新：debug_mode = {param.value}'
)
# 返回成功
return SetParametersResult(successful=True)
def control_loop(self):
"""控制循环：使用参数中的最大速度"""
debug = self.get_parameter('debug_mode').value
if debug:
# debug 模式才输出这条日志
self.get_logger().info(
f'控制循环运行中... 当前最大速度：{self.max_speed} m/s'
)
def main(args=None):
rclpy.init(args=args)
node = RobotParamNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
运行与参数操作
终端 1（启动节点，同时传入初始参数值）：
# 用 --ros-args -p 传入参数
ros2 run learning_pkg param_node \
--ros-args \
-p max_speed:=2.0 \
-p robot_name:=my_bot \
-p debug_mode:=true
终端 2（动态操作参数，无需重启节点）：
# 查看节点的所有参数
ros2 param list /robot_param_node
# 查询单个参数值
ros2 param get /robot_param_node max_speed
# 动态修改参数（节点不需要重启！）
ros2 param set /robot_param_node max_speed 3.0
ros2 param set /robot_param_node debug_mode true
# 导出参数到 YAML 文件
ros2 param dump /robot_param_node > my_robot_params.yaml
# 从 YAML 文件加载参数启动
ros2 run learning_pkg param_node \
--ros-args --params-file my_robot_params.yaml
8、综合实战：把五个概念组合成完整系统
现在把前面学的五个概念整合到一个节点里，模拟一个完整的"虚拟小车控制器"。
系统架构：
robot_system 节点
├── 参数：max_speed, control_frequency, robot_name
├── 话题：发布 /cmd_vel（速度指令）
├── 话题：订阅 /cmd_input（接收外部速度输入）
├── 服务：/emergency_stop（急停/恢复）
└── 动作：/drive_distance（前进指定距离）
新建 robot_system.py：
import time
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, GoalResponse, CancelResponse
from geometry_msgs.msg import Twist
from std_srvs.srv import SetBool
from learning_interfaces.action import DriveDistance
class RobotSystem(Node):
"""
综合示例：一个节点同时使用话题、服务、动作、参数。
这是实际机器人项目中的典型结构。
"""
def __init__(self):
super().__init__('robot_system')
# ══════════════════════════════════
# 1. 声明参数
# ══════════════════════════════════
self.declare_parameter('max_speed', 1.5)
self.declare_parameter('control_frequency', 10)
self.declare_parameter('robot_name', 'my_robot')
self.max_speed = self.get_parameter('max_speed').value
freq = self.get_parameter('control_frequency').value
self.robot_name = self.get_parameter('robot_name').value
self.add_on_set_parameters_callback(self._on_param_change)
# ══════════════════════════════════
# 2. 内部状态
# ══════════════════════════════════
self.is_emergency_stopped = False # 是否急停
self.current_speed = Twist() # 当前速度指令
self.total_distance = 0.0 # 累计行驶距离
# ══════════════════════════════════
# 3. 话题：发布速度指令
# ══════════════════════════════════
self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10)
# 话题：订阅外部速度输入
self.cmd_input_sub = self.create_subscription(
Twist, '/cmd_input', self._on_cmd_input, 10
)
# ══════════════════════════════════
# 4. 服务：急停服务
# ══════════════════════════════════
self.stop_srv = self.create_service(
SetBool, '/emergency_stop', self._handle_emergency_stop
)
# ══════════════════════════════════
# 5. 动作：前进指定距离
# ══════════════════════════════════
self._drive_action = ActionServer(
self,
DriveDistance,
'/drive_distance',
execute_callback=self._execute_drive,
goal_callback=self._goal_callback,
cancel_callback=self._cancel_callback,
)
# ══════════════════════════════════
# 6. 控制定时器
# ══════════════════════════════════
period = 1.0 / freq
self.control_timer = self.create_timer(period, self._control_loop)
self.get_logger().info(
f'🤖 [{self.robot_name}] 系统已就绪\n'
f' 最大速度: {self.max_speed} m/s\n'
f' 控制频率: {freq} Hz\n'
f' 话题: /cmd_vel (发布), /cmd_input (订阅)\n'
f' 服务: /emergency_stop\n'
f' 动作: /drive_distance'
)
# ──────────────────────────────────────
# 话题回调
# ──────────────────────────────────────
def _on_cmd_input(self, msg: Twist):
"""收到外部速度输入时，限制速度并存储"""
if self.is_emergency_stopped:
return # 急停状态，忽略速度指令
# 限制到最大速度
self.current_speed.linear.x = max(
min(msg.linear.x, self.max_speed), -self.max_speed
)
self.current_speed.angular.z = max(
min(msg.angular.z, self.max_speed), -self.max_speed
)
# ──────────────────────────────────────
# 控制循环（定时器触发）
# ──────────────────────────────────────
def _control_loop(self):
"""定时发布速度指令"""
if self.is_emergency_stopped:
# 急停状态：发布零速度
self.cmd_vel_pub.publish(Twist())
else:
# 正常状态：发布当前速度
self.cmd_vel_pub.publish(self.current_speed)
# 累计行驶距离（粗略估算）
dt = 1.0 / self.get_parameter('control_frequency').value
self.total_distance += abs(self.current_speed.linear.x) * dt
# ──────────────────────────────────────
# 服务回调：急停
# ──────────────────────────────────────
def _handle_emergency_stop(self, request, response):
"""处理急停请求"""
self.is_emergency_stopped = request.data
if request.data:
# 立即清零速度
self.current_speed = Twist()
self.cmd_vel_pub.publish(Twist())
response.success = True
response.message = f'⚠ [{self.robot_name}] 急停！已停止运动'
self.get_logger().warn(response.message)
else:
response.success = True
response.message = f'√ [{self.robot_name}] 急停解除，恢复运行'
self.get_logger().info(response.message)
return response
# ──────────────────────────────────────
# 动作回调：前进指定距离
# ──────────────────────────────────────
def _goal_callback(self, goal):
if self.is_emergency_stopped:
self.get_logger().warn('急停状态中，拒绝动作任务')
return GoalResponse.REJECT
return GoalResponse.ACCEPT
def _cancel_callback(self, goal_handle):
return CancelResponse.ACCEPT
async def _execute_drive(self, goal_handle):
"""执行前进任务"""
target = goal_handle.request.target_distance
speed = min(goal_handle.request.speed, self.max_speed) # 不超过最大速度
self.get_logger().info(f'动作任务：前进 {target} 米，速度 {speed} m/s')
driven = 0.0
dt = 0.1
while driven < target:
if goal_handle.is_cancel_requested:
goal_handle.canceled()
result = DriveDistance.Result()
result.actual_distance = driven
result.success = False
result.message = '任务已取消'
return result
if self.is_emergency_stopped:
# 急停期间暂停执行
time.sleep(dt)
continue
# 模拟前进
driven = min(driven + speed * dt, target)
# 更新系统当前速度（让控制循环发布出去）
self.current_speed.linear.x = speed if driven < target else 0.0
# 发送反馈
fb = DriveDistance.Feedback()
fb.current_distance = driven
fb.percentage = driven / target
goal_handle.publish_feedback(fb)
time.sleep(dt)
# 完成后停车
self.current_speed = Twist()
goal_handle.succeed()
result = DriveDistance.Result()
result.actual_distance = driven
result.success = True
result.message = f'完成！行驶 {driven:.2f} 米'
self.get_logger().info(result.message)
return result
# ──────────────────────────────────────
# 参数变化回调
# ──────────────────────────────────────
def _on_param_change(self, params):
from rcl_interfaces.msg import SetParametersResult
for p in params:
if p.name == 'max_speed':
self.max_speed = p.value
self.get_logger().info(f'参数更新：max_speed = {self.max_speed}')
return SetParametersResult(successful=True)
def main(args=None):
rclpy.init(args=args)
node = RobotSystem()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
综合演示步骤
# 终端 1：启动完整系统
ros2 run learning_pkg robot_system \
--ros-args -p robot_name:=car_01 -p max_speed:=2.0
# 终端 2：发送速度指令（话题）
ros2 topic pub /cmd_input geometry_msgs/msg/Twist \
"{linear: {x: 1.0}, angular: {z: 0.2}}" --rate 10
# 终端 3：触发急停（服务）
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: true}"
# 解除急停
ros2 service call /emergency_stop std_srvs/srv/SetBool "{data: false}"
# 终端 4：执行前进任务（动作，带实时反馈）
ros2 action send_goal /drive_distance \
learning_interfaces/action/DriveDistance \
"{target_distance: 5.0, speed: 1.0}" --feedback
# 终端 5：动态修改最大速度（参数）
ros2 param set /robot_system max_speed 3.0
# 查看实时速度输出
ros2 topic echo /cmd_vel
9、概念速查：什么情况用什么？
决策流程图
完整对比表
| 维度 | Topic | Service | Action | Parameter |
|---|---|---|---|---|
| 通信方向 | 单向（发→收） | 双向（请求→响应） | 双向（多次反馈） | 读写配置 |
| 等待结果 | × 不等 | √ 阻塞等待 | √ 异步等待 | √ 立即返回 |
| 任务时长 | 持续 | 短（毫秒~秒） | 长（秒~分钟） | 瞬时 |
| 可取消 | × | × | √ | N/A |
| 进度反馈 | × | × | √ | N/A |
| 多对多 | √ | 一对多 | 一对多 | 节点内部 |
| 典型场景 | 激光雷达数据、速度指令 | 急停、切换模式、查询状态 | 导航、前进X米、抓取物体 | 速度限制、PID参数、调试开关 |
最终构建
所有代码写完后，构建整个工作空间：
cd ~/ros2_learning_ws
colcon build
source install/setup.bash
如果只想构建特定包：
colcon build --packages-select learning_pkg learning_interfaces
浙公网安备 33010602011771号