ROS2 机器人 少年创客营：Day 4

🤖 ROS2 机器人 少年创客营：Day 4

主题：从"广播"到"对话" ------ 服务 (Service) 与多机协作

👋 欢迎回来，机器人架构师！

昨天回顾 ：我们给海龟装上了"眼睛"和"大脑"，利用 Timer 实现了精准的闭环导航。海龟不再盲目，它能感知位置并自动修正路线。

今天挑战：

1. 单点通信的局限 ：话题 (Topic) 是"广播"，发了就不管了。如果我想让海龟**"立刻瞬移"或者 "重置世界"，并且等待确认**后再做下一步，该怎么办？
2. 多机时代 ：如果屏幕上有两只海龟 (turtle1 和 turtle2)，如何让它们互相配合？

今日核心 ：学习 Service (服务) 通信机制，掌握 多节点命名空间 (Namespace)，并编写一个**"多海龟巡逻队"**！

---

🗺️ 今日探险地图 (Checklist)

• 📞 通信升级：理解 Service (请求/响应) 与 Topic (发布/订阅) 的区别。
• 🛠️ 工具实战 ：使用 ros2 service 命令行调用服务。
• 🐢 多机管理 ：利用 Namespace 同时控制 turtle1 和 turtle2。
• 🧠 逻辑进阶：在代码中异步调用 Service (Spawn & Reset)。
• 🏆 终极任务 ：编写 swarm_patrol.py，自动生成新海龟并指挥它们组队巡逻。

---

📞 第一关：Topic vs Service ------ 广播 vs 打电话

这是 ROS 通信的两大基石，必须分清！

特性	Topic (话题)	Service (服务)
模式	发布/订阅 (Pub/Sub)	请求/响应 (Req/Rep)
比喻	📻 收音机/广播主播发消息，听众随便听。主播不知道谁听了，也不等反馈。	📞 打电话/客服 客户端问："现在几点了？"服务端答："现在是 3 点。"必须有问有答，同步阻塞。
数据流	单向 (One-way)	双向 (Two-way)
典型场景	传感器数据 (激光雷达、摄像头)持续的速度指令	开关机、重置位置、查询状态"请帮我算个结果"
ROS 2 命令	ros2 topic echo ...	ros2 service call ...

💡 什么时候用 Service？

• 当你需要确保任务完成时（例如：重置仿真器）。
• 当你需要获取一个计算结果时（例如：两个数相加）。
• 当动作不频繁发生时（Service 建立连接开销比 Topic 大，不适合高频数据）。

---

🛠️ 第二关：动手玩转 Service

turtlesim 节点内置了几个非常实用的 Service。让我们打开终端试试！

1. 查看有哪些服务

ros2 service list

你会看到类似 /spawn, /reset, /turtle1/set_pen 等服务。

2. 查看服务类型

ros2 service type /spawn
# 输出: turtlesim/srv/Spawn

3. 召唤一只新海龟！🐢✨

我们要调用 /spawn 服务，参数包括 x, y, theta (角度), name。

# 语法：ros2 service call <服务名> <服务类型> "{参数}"
ros2 service call /spawn turtlesim/srv/Spawn "{x: 2.0, y: 2.0, theta: 0.0, name: 'turtle2'}"

👀 观察：

• 终端会返回新海龟的名字。
• 仿真窗口里瞬间多了一只海龟！
• 关键点 ：命令行等待了服务器的回应才结束，这就是同步调用。

4. 重置世界 🌍

ros2 service call /reset std_srvs/srv/Empty "{}"

所有海龟消失，世界清空。

---

💻 第三关：代码实战 ------ 多海龟巡逻队

我们将编写一个 Python 节点，完成以下高难度动作：

1. 调用服务 ：自动在随机位置生成 turtle2。
2. 多命名空间 ：同时订阅 turtle1 和 turtle2 的位置。
3. 协同控制 ：让 turtle1 去左上角，turtle2 去右下角，互不干扰。

📄 文件路径

src/my_turtle_bot/my_turtle_bot/swarm_patrol.py

🐍 完整代码 (Service + Multi-Turtle)

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from turtlesim.srv import Spawn
from std_srvs.srv import Empty
import math
import random

class SwarmCommander(Node):
    def __init__(self):
        super().__init__('swarm_commander')
        
        self.turtles = {} # 存储每只海龟的状态 {"turtle1": pose, "turtle2": pose...}
        self.goals = {}   # 存储每只海龟的目标
        
        # --- 1. 设置目标 ---
        # turtle1 去左上，turtle2 去右下
        self.goals['turtle1'] = {'x': 2.0, 'y': 9.0}
        self.goals['turtle2'] = {'x': 9.0, 'y': 2.0}
        
        # --- 2. 创建发布者 (为每只海龟单独创建) ---
        # 注意话题名包含海龟名字
        self.cmd_pub_1 = self.create_publisher(Twist, 'turtle1/cmd_vel', 10)
        self.cmd_pub_2 = self.create_publisher(Twist, 'turtle2/cmd_vel', 10)
        
        self.pubs = {
            'turtle1': self.cmd_pub_1,
            'turtle2': self.cmd_pub_2
        }

        # --- 3. 创建订阅者 (使用回调区分) ---
        # 订阅 turtle1
        self.create_subscription(Pose, 'turtle1/pose', lambda msg: self.pose_callback('turtle1', msg), 10)
        # 订阅 turtle2
        self.create_subscription(Pose, 'turtle2/pose', lambda msg: self.pose_callback('turtle2', msg), 10)
        
        # --- 4. 创建服务客户端 (用来召唤新海龟) ---
        self.spawn_client = self.create_client(Spawn, '/spawn')
        
        # --- 5. 定时器 (指挥中心心跳) ---
        self.timer = self.create_timer(0.1, self.control_loop)
        
        self.get_logger().info('🚀 巡逻队指挥官已就位！正在召唤队友...')
        
        # 尝试召唤 turtle2 (如果已存在会失败，忽略错误)
        self.spawn_turtle('turtle2', 5.0, 5.0)

    def spawn_turtle(self, name, x, y):
        """异步调用 Spawn 服务"""
        if not self.spawn_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().warn(f'服务 /spawn 不可用，稍后重试...')
            return

        req = Spawn.Request()
        req.x = x
        req.y = y
        req.theta = 0.0
        req.name = name
        
        self.get_logger().info(f'📞 正在呼叫服务：生成 {name}...')
        # future 代表未来的结果，不会阻塞主程序
        future = self.spawn_client.call_async(req)
        future.add_done_callback(self.handle_spawn_response)

    def handle_spawn_response(self, future):
        try:
            response = future.result()
            self.get_logger().info(f'✅ 成功生成海龟：{response.name}')
        except Exception as e:
            self.get_logger().warn(f'⚠️ 生成失败 (可能已存在): {e}')

    def pose_callback(self, turtle_name, msg):
        """通用回调：更新任意海龟的位置"""
        self.turtles[turtle_name] = msg

    def control_loop(self):
        """每 0.1 秒检查所有海龟的状态并发出指令"""
        
        for name, goal in self.goals.items():
            # 如果还没收到该海龟的数据，跳过
            if name not in self.turtles:
                continue
                
            current_pose = self.turtles[name]
            pub = self.pubs[name]
            
            # --- 导航算法 (复用 Day 3 逻辑) ---
            dx = goal['x'] - current_pose.x
            dy = goal['y'] - current_pose.y
            distance = math.sqrt(dx*dx + dy*dy)
            
            # 到达判断
            if distance < 0.2: # 阈值稍微大一点，防止抖动
                # 到达后，随机换个新目标 (无限巡逻)
                new_x = random.uniform(1.0, 10.0)
                new_y = random.uniform(1.0, 10.0)
                self.goals[name] = {'x': new_x, 'y': new_y}
                self.get_logger().info(f'🎯 {name} 到达目标！新目标：({new_x:.1f}, {new_y:.1f})')
                # 发送停止指令
                pub.publish(Twist())
                continue
            
            # 计算角度
            desired_theta = math.atan2(dy, dx)
            angle_diff = desired_theta - current_pose.theta
            
            # 标准化角度
            while angle_diff > math.pi: angle_diff -= 2.0 * math.pi
            while angle_diff < -math.pi: angle_diff += 2.0 * math.pi
            
            cmd = Twist()
            if abs(angle_diff) > 0.1:
                cmd.angular.z = 1.5 * (1.0 if angle_diff > 0 else -1.0)
            else:
                cmd.linear.x = min(2.0, distance * 1.5)
            
            pub.publish(cmd)

def main(args=None):
    rclpy.init(args=args)
    node = SwarmCommander()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

---

🔍 代码深度解析

1. 异步服务调用 (call_async)

在 spawn_turtle 中，我们没有直接 result()，而是用了 call_async。

• 为什么？ 如果在 __init__ 或 control_loop 中直接等待服务结果（同步），整个节点会卡住，连 pose_callback 都收不到，海龟就"死"了。
• 做法 ：发送请求 -> 注册一个回调函数 (add_done_callback) -> 程序继续运行。等服务器回复了，回调函数会自动执行。

2. Lambda 表达式复用回调

lambda msg: self.pose_callback('turtle1', msg)

• 我们不想写两个几乎一样的函数 pose_callback_1 和 pose_callback_2。
• 使用 lambda 创建一个匿名小函数，把海龟的名字"硬编码"进去，然后统一交给 pose_callback 处理。这是 Python 的高级技巧，能让代码更简洁。

3. 字典管理多机器人

• self.turtles = {} 和 self.pubs = {}
• 这种设计具有可扩展性 。如果你想加 turtle3，只需在字典里加一项，循环逻辑完全不用改！

---

🚀 第四关：编译与运行

1. 更新 setup.py

添加新的入口点：

'swarm_patrol = my_turtle_bot.swarm_patrol:main',

2. 构建与运行

cd ~/ros2_ws
colcon build
source install/setup.bash

# 终端 1: 启动仿真器
ros2 run turtlesim turtlesim_node

# 终端 2: 启动巡逻队
ros2 run my_turtle_bot swarm_patrol

👀 观察现象：

1. 日志显示"正在呼叫服务：生成 turtle2"。
2. 一只新海龟出现在屏幕中央 (5,5)。
3. turtle1 冲向 (2, 9)，turtle2 冲向 (9, 2)。
4. 神奇时刻 ：当任何一只海龟到达目标后，它会自动随机选择下一个目标，两只海龟在屏幕上像有生命一样来回穿梭，互不碰撞（大概率）！

---

🎨 创意挑战赛

现在的巡逻队只是随机乱跑。你能给它们加点"纪律"吗？

🌟 挑战等级

• 🥉 青铜级：固定编队 🛡️

  • 任务 ：不让它们随机跑。设定一个正方形路径，让 turtle1 和 turtle2 沿着同样的四个点巡逻，但 turtle2 始终落后 turtle1 一个点。

• 🥈 白银级：避障逻辑 🚧

  • 任务 ：在 control_loop 中计算两只海龟之间的距离。如果距离小于 1.0 米，让它们暂时停下或反向移动，防止"撞车"。

• 🥇 黄金级：动态增员 📈

  • 任务 ：每隔 5 秒，自动调用一次 spawn 服务，生成 turtle3, turtle4... 直到屏幕上有 5 只海龟在跳舞！
  • 提示 ：在 control_loop 里加一个计时器 if time_now - last_spawn_time > 5.0: spawn_next()。

---

🆘 常见问题急救包

问题	原因	解决
报错 service not available	turtlesim_node 没启动或服务名错了	确保先运行了 turtlesim_node，检查服务名是否是 /spawn。
海龟名字冲突	试图生成已存在的名字	捕获异常，或者在生成前检查名字列表。代码中已处理忽略错误。
只有一只海龟动	订阅的话题名写错了	检查是否是 turtle2/pose 而不是 turtle1/pose。
海龟转圈不出来	初始角度正好背对目标且逻辑有误	检查角度标准化逻辑，确保 angle_diff 计算正确。

---

📝 Day 4 总结清单

概念	关键词	作用
Service	Request/Response, call_async	需确认结果的通信，如生成物体、重置系统
异步编程	future, callback	调用服务时不卡死主程序，保持系统流畅
多机协作	Namespace (turtle1/, turtle2/)	通过话题前缀区分不同机器人
数据结构	Dictionary (dict)	优雅地管理多个机器人的状态和控制器
Lambda	lambda x: ...	快速创建匿名函数，复用回调逻辑

---

🔮 明日预告 (Day 5)

海龟们已经能成群结队了，但如果环境中有障碍物怎么办？

明天，我们将引入 ROS 2 中最强大的功能之一：Action (动作)。

• Action vs Service ：如果任务要执行很久（比如"走 100 米"），Service 会一直等着，而 Action 允许你中途取消 或实时反馈进度（"已走 50%..."）。
• 实战：编写一个带有"进度条"的长距离导航任务，并实现"紧急中止"功能。

准备好掌握机器人控制的终极武器了吗？明天见！

---

© 2026 ROS2 机器人 少年创客营 | 从单兵作战到集群智能，通信让协作成为可能