🤖 ROS2 机器人 少年创客营：Day 3
主题：赋予机器人"眼睛"与"大脑" ------ 从"盲目执行"到"智能闭环"
👋 欢迎回到实验室，高级见习工程师！
昨天回顾 ：我们写出了自动画正方形的代码。但是，我们的海龟是**"盲人"！它只知道"走2秒、转1.5秒"，却完全不知道自己实际**走了多远。如果地面打滑，正方形就会画变形。
今天目标 ：我们要给海龟装上**"眼睛"（传感器订阅）和 "大脑"（定时控制循环）！🧠👁️
我们将彻底抛弃 ROS 1 的while + sleep写法，学习 ROS 2 最核心的 定时器 (Timer) 机制，编写真正的异步闭环控制**代码，让海龟能够实时感知位置，精准导航到任意坐标点。
🗺️ 今日探险地图 (Checklist)
- 👁️ 视觉解锁：理解"话题订阅" (Subscription) 与回调函数。
- ⏱️ 核心升级 ：掌握
create_timer，告别rate.sleep()(ROS 2 正统写法)。 - 🧠 逻辑重构：将"死循环"改为"事件驱动"的控制循环。
- 🎯 任务实战 ：编写
go_to_goal.py，实现自适应速度的自动导航。 - 🏆 终极挑战：设计"多点巡逻"任务，让海龟依次访问多个坐标。
👁️ 第一关：理解 ROS 2 的"心跳" ------ Timer
在 ROS 1 中，我们习惯写：
python
# ❌ ROS 1 风格 (阻塞式)
rate = rospy.Rate(10)
while not done:
do_something()
rate.sleep() # 程序在这里卡住，无法处理其他消息！在 ROS 2 中，这种做法被认为是"不优雅"甚至危险的，因为它会阻塞节点处理其他消息（比如紧急停止信号）。
✅ ROS 2 推荐做法：定时器 (Timer)
python
# ✅ ROS 2 风格 (非阻塞/异步)
self.timer = self.create_timer(0.1, self.control_callback)
# 程序继续运行，每 0.1 秒，系统会自动调用 control_callback核心区别：
- ROS 1: 你主动去"睡"一会儿。
- ROS 2 : 你注册一个闹钟，然后去忙别的（
spin），闹钟响了系统自动叫你。
💻 第二关：编写"智能导航"节点 (完整代码)
我们将结合 订阅者 (Pose) 和 定时器 (Timer)，编写一个完美的导航脚本。
📄 文件路径
src/my_turtle_bot/my_turtle_bot/go_to_goal.py
🐍 完整代码 (方案 B：Timer 模式)
python
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
class GoToGoalNode(Node):
def __init__(self, goal_x, goal_y):
super().__init__('go_to_goal_node')
# --- 配置目标 ---
self.goal_x = goal_x
self.goal_y = goal_y
self.reached_goal = False
# --- 状态变量 ---
self.current_pose = None
# --- 1. 创建发布者 (嘴巴) ---
# 发送速度指令给海龟
self.cmd_pub = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
# --- 2. 创建订阅者 (眼睛) ---
# 实时接收海龟的位置信息
# 当 '/turtle1/pose' 有新消息时，自动触发 pose_callback
self.pose_sub = self.create_subscription(
Pose,
'turtle1/pose',
self.pose_callback,
10
)
# --- 3. 创建定时器 (大脑心跳) ---
# 🌟 核心亮点：每 0.1 秒 (10Hz) 自动调用 control_loop 函数
# 这替代了 while + sleep 的模式，是非阻塞的！
self.timer = self.create_timer(0.1, self.control_loop)
self.get_logger().info(f'🚀 导航系统启动！目标点：({goal_x}, {goal_y})')
def pose_callback(self, msg):
"""
回调函数：只要收到新位置，就更新本地变量。
这个函数执行非常快，不会阻塞主程序。
"""
self.current_pose = msg
def control_loop(self):
"""
控制循环：每 0.1 秒被定时器自动调用一次。
这里包含所有的决策逻辑。
"""
# 1. 安全检查：如果还没收到位置数据，先不行动
if self.current_pose is None:
self.get_logger().warn('⏳ 等待海龟位置数据...')
return
# 2. 如果已经到达，就不再计算
if self.reached_goal:
return
# --- 核心算法开始 ---
# A. 计算距离
dx = self.goal_x - self.current_pose.x
dy = self.goal_y - self.current_pose.y
distance = math.sqrt(dx*dx + dy*dy)
# B. 计算目标角度
# atan2 返回的是 -pi 到 pi 之间的弧度
desired_theta = math.atan2(dy, dx)
# C. 计算角度误差
angle_diff = desired_theta - self.current_pose.theta
# D. 角度标准化 (确保误差在 -pi 到 pi 之间，防止海龟疯狂旋转)
while angle_diff > math.pi:
angle_diff -= 2.0 * math.pi
while angle_diff < -math.pi:
angle_diff += 2.0 * math.pi
# E. 决策逻辑
cmd = Twist()
# 判断是否到达终点 (阈值 0.1 米)
if distance < 0.1:
self.get_logger().info('🎉 到达目标点！任务完成。')
self.stop_robot()
self.reached_goal = True
# 可选：在这里关闭节点 rclpy.shutdown()
return
# 策略：先对齐方向，再前进
if abs(angle_diff) > 0.1:
# 角度偏差大 -> 原地旋转
# 比例控制：偏差越大，转得越快
cmd.angular.z = 1.5 * (1.0 if angle_diff > 0 else -1.0)
cmd.linear.x = 0.0
# 每 10 次循环打印一次日志，避免刷屏
if int(self.get_clock().seconds_nanoseconds()[0]) % 1 == 0:
pass # 简化日志，保持终端整洁
else:
# 方向对了 -> 直线前进
# 自适应速度：离得越远越快，离得近自动减速 (防止冲过头)
speed = min(2.0, distance * 2.0)
cmd.linear.x = speed
cmd.angular.z = 0.0
# F. 发布指令
self.cmd_pub.publish(cmd)
def stop_robot(self):
"""发送停止指令"""
stop_msg = Twist()
stop_msg.linear.x = 0.0
stop_msg.angular.z = 0.0
self.cmd_pub.publish(stop_msg)
def main(args=None):
rclpy.init(args=args)
# 设定目标坐标 (可以在这里修改，或通过参数传入)
target_x = 5.5
target_y = 5.5
node = GoToGoalNode(target_x, target_y)
try:
# 🌟 ROS 2 标准运行方式
# spin(node) 会让节点一直活着，并在后台处理：
# 1. 订阅者的回调 (pose_callback)
# 2. 定时器的回调 (control_loop)
# 这一切都是异步并发进行的！
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info('🛑 用户中断，停止机器人。')
node.stop_robot()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()🔍 代码深度解析 (为什么这样写更好？)
1. 为什么用 create_timer 而不是 while？
- 非阻塞 ：在
control_loop执行计算时，如果海龟突然发了一个新位置，pose_callback依然可以立即执行 更新数据。而在while+sleep模式下，如果sleep正在计时，新数据只能排队等待，导致控制延迟。 - 资源管理 ：
spin(node)统一管理所有任务，CPU 利用率更合理。
2. 角度标准化 (while angle_diff > math.pi)
这是机器人学的经典陷阱。
- 假设海龟朝向
3.0rad，目标在-3.0rad。 - 直接相减：
-3.0 - 3.0 = -6.0。海龟会觉得要转一大圈。 - 实际上：
3.0和-3.0几乎是一样的方向，只需转0.28rad。 - 标准化代码 确保了海龟永远走最短路径旋转。
3. 自适应速度 min(2.0, distance * 2.0)
- 距离远时：速度限制在
2.0，快速接近。 - 距离近时 (如 0.2m)：速度自动降为
0.4。 - 效果：海龟会像老司机一样，快到时轻踩刹车，稳稳停在目标点，不会冲过去再倒回来。
🚀 第三关：编译与运行
1. 修改 setup.py
确保你的入口点已注册：
python
entry_points={
'console_scripts': [
'go_to_goal = my_turtle_bot.go_to_goal:main',
# 如果有之前的 listener 也可以保留
'listen_pose = my_turtle_bot.pose_listener:main',
],
},2. 构建与运行
bash
cd ~/ros2_ws
colcon build
source install/setup.bash
# 终端 1: 启动仿真器
ros2 run turtlesim turtlesim_node
# 终端 2: 启动智能导航
ros2 run my_turtle_bot go_to_goal👀 观察现象：
- 海龟先原地快速旋转，对准目标点 (5.5, 5.5)。
- 然后加速冲出，随着距离变近，肉眼可见地减速。
- 最后平滑停下。
- 测试干扰 ：在海龟前进时，用键盘 (
teleop_turtle) 强行把它推歪。你会发现它立刻感知到偏差 ，自动调整方向继续走向目标！这就是闭环的威力！
🎨 第四关：创意挑战赛
现在的海龟只能去一个点。你能让它变得更聪明吗？
🌟 挑战等级
-
🥉 青铜级：多点巡逻 🛡️
- 任务 ：修改
GoToGoalNode类，增加一个goals_list = [(2,2), (8,2), (8,8), (2,8)]。 - 逻辑 ：在
control_loop中，当reached_goal为真时，不要停止，而是切换到列表中的下一个点，重置reached_goal = False。
- 任务 ：修改
-
🥈 白银级：动态参数 🎯
-
任务：不使用代码里的硬编码坐标，通过命令行参数传入。
-
提示 ：
pythonself.declare_parameter('goal_x', 5.5) self.declare_parameter('goal_y', 5.5) x = self.get_parameter('goal_x').value -
运行命令 ：
ros2 run my_turtle_bot go_to_goal --ros-args -p goal_x:=9.0 -p goal_y:=1.0
-
-
🥇 黄金级：绘制图形 ✍️
- 任务：定义一系列点，连起来是一个三角形或正方形。
- 要求：点在切换时要有平滑过渡，不要停顿太久。
🆘 常见问题急救包
| 问题 | 原因 | 解决 |
|---|---|---|
| 海龟原地疯狂旋转 | 角度未标准化 | 检查 while angle_diff > math.pi 逻辑是否遗漏。 |
| 海龟走到一半停了 | 距离阈值太大或速度计算错误 | 检查 distance < 0.1 的判断，确认 current_pose 是否在更新。 |
| 程序不退出 | reached_goal 标记没生效 |
确认到达后设置了 self.reached_goal = True 并调用了 stop_robot。 |
报错 AttributeError: create_rate |
还在用 ROS 1 思维 | 记住：ROS 2 只有 create_timer，没有 create_rate！ |
📝 Day 3 总结清单
| 概念 | 关键词 | 作用 |
|---|---|---|
| 定时器 | create_timer |
ROS 2 的核心心跳，替代 while+sleep |
| 异步回调 | spin, callback |
同时处理传感器数据和逻辑控制，互不阻塞 |
| 闭环控制 | Feedback | 实时根据误差调整动作 (PID 思想雏形) |
| 数学工具 | atan2, 标准化 |
计算机器人运动学，解决角度跳变问题 |
| 自适应 | min() |
根据距离动态调整速度，实现平滑停止 |
🔮 明日预告 (Day 4)
单只海龟已经很聪明了，但如果我们有两只海龟，它们能合作吗？
明天，我们将进入多机协作 与服务通信的世界：
- Service (服务)：从"广播"升级为"打电话"。例如：发送一个请求"请重置海龟位置"，等待服务器返回"已完成"。
- 多话题管理 ：如何同时监听
turtle1/pose和turtle2/pose？- 自定义消息：发明一种新的数据结构，让机器人传递更复杂的信息。
准备好构建你的第一个机器人集群了吗？明天见！
© 2026 ROS 2 少年创客营 | 拒绝阻塞，拥抱异步，让智能流动起来