4. 控制接口仿真

好，我们进入第四讲。前面几章我们把仿真环境搭起来了，机器人模型也跑起来了。但说实话，光看着机器人站在那里不动，跟个模型似的，没啥意思。这一章，我们就来真正“动”它——写一个ROS2节点，通过话题发布控制指令，让仿真机器人跑起来。

我个人习惯，学一个新东西，先搞清楚它到底在干什么。说白了，这一章的核心就两件事：发布速度指令和订阅机器人状态。一个发，一个收，ROS2最基础的通信方式。

4.1 理解话题通信：发布者与订阅者

ROS2里，节点之间怎么说话？靠话题（Topic）。一个节点发布消息到某个话题，另一个节点从同一个话题订阅消息。就像电台广播，发布者是电台，订阅者是听众。你想想看，这种模式的好处是什么？解耦。发布者不需要知道谁在听，订阅者也不需要知道谁在发。

在机器人控制里，最常见的场景就是：

• /cmd_vel 话题：发布速度指令（线速度 + 角速度）
• /odom 话题：订阅里程计信息（位置、速度）

嗯，这里要注意，/cmd_vel 用的是 geometry_msgs/msg/Twist 消息类型。这个类型很简单，就两个字段：linear（线速度）和 angular（角速度）。每个字段又有 x、y、z 三个分量。对于差速轮机器人，我们一般只用 linear.x 和 angular.z。

4.2 编写第一个控制节点：让机器人走直线

好，我们直接上手写代码。我会用Python，因为调试方便，适合教学。C++版本其实逻辑一样，就是语法不同。

先创建一个新的Python文件，比如 robot_controller.py。放在你的功能包 src 目录下。

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

class RobotController(Node):
    def __init__(self):
        super().__init__('robot_controller')
        # 创建发布者，发布到 /cmd_vel 话题
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        # 设置定时器，每0.1秒发布一次
        self.timer = self.create_timer(0.1, self.timer_callback)
        self.get_logger().info('机器人控制器已启动')

    def timer_callback(self):
        msg = Twist()
        # 设置线速度 0.2 m/s，角速度 0
        msg.linear.x = 0.2
        msg.angular.z = 0.0
        self.publisher_.publish(msg)
        self.get_logger().info(f'发布速度: linear.x={msg.linear.x}')

def main(args=None):
    rclpy.init(args=args)
    node = RobotController()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

这段代码很简单。我刚开始学的时候，也写过类似的。但有个坑——记得给文件加执行权限。我曾经有一次在真机上部署，忘了加权限，节点死活起不来，排查了半天。

4.3 订阅机器人状态：读取里程计数据

光发指令不够，我们还得知道机器人实际走了多少。这就需要订阅 /odom 话题。里程计消息类型是 nav_msgs/msg/Odometry，里面包含了位置、姿态、速度等信息。

我在项目中遇到过一个问题：仿真里的里程计数据很完美，但真机上噪声很大。所以，从仿真开始，就要养成检查数据质量的习惯。

下面我们扩展一下上面的节点，增加订阅功能：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry

class RobotController(Node):
    def __init__(self):
        super().__init__('robot_controller')
        # 发布者
        self.publisher_ = self.create_publisher(Twist, '/cmd_vel', 10)
        # 订阅者
        self.subscription = self.create_subscription(
            Odometry, '/odom', self.odom_callback, 10)
        self.timer = self.create_timer(0.1, self.timer_callback)
        self.get_logger().info('机器人控制器已启动，并订阅了里程计')

    def timer_callback(self):
        msg = Twist()
        msg.linear.x = 0.2
        self.publisher_.publish(msg)

    def odom_callback(self, msg):
        # 提取位置信息
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        self.get_logger().info(f'当前位置: x={x:.2f}, y={y:.2f}')

def main(args=None):
    rclpy.init(args=args)
    node = RobotController()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

你看，订阅回调函数里，我们拿到了机器人的实时位置。这在做闭环控制时非常有用。

4.4 让机器人走正方形：组合运动

走直线太单调了。我们来点组合动作——让机器人走一个正方形。逻辑很简单：直走一段，转弯90度，再直走，再转弯，循环四次。

这里有个关键点：如何控制转弯角度？你不能直接说“转90度”，因为ROS2的 /cmd_vel 只接受速度指令。你需要根据角速度和时间来估算转过的角度。

我个人的做法是：用里程计反馈来精确控制。但为了演示，我们先做一个开环的版本：

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import time

class SquareController(Node):
    def __init__(self):
        super().__init__('square_controller')
        self.pub = self.create_publisher(Twist, '/cmd_vel', 10)
        self.timer = self.create_timer(0.1, self.timer_callback)
        self.state = 'forward'
        self.start_time = self.get_clock().now()
        self.get_logger().info('开始走正方形')

    def timer_callback(self):
        msg = Twist()
        now = self.get_clock().now()
        elapsed = (now - self.start_time).nanoseconds / 1e9

        if self.state == 'forward':
            msg.linear.x = 0.2
            if elapsed > 2.0:  # 直走2秒
                self.state = 'turn'
                self.start_time = now
        elif self.state == 'turn':
            msg.angular.z = 0.5
            if elapsed > 3.14:  # 大约转90度 (0.5 rad/s * 3.14s ≈ 1.57 rad)
                self.state = 'forward'
                self.start_time = now
                self.get_logger().info('完成一次转弯')

        self.pub.publish(msg)

def main(args=None):
    rclpy.init(args=args)
    node = SquareController()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

4.5 可视化调试：用rqt工具查看话题

代码写完了，怎么验证对不对？除了看终端日志，ROS2提供了强大的可视化工具。我个人最常用的是 rqt 和 rqt_graph。

• rqt_graph：显示节点和话题的拓扑图。一眼就能看出谁在发布，谁在订阅。
• rqt_plot：实时绘制话题数据曲线。比如你可以看 /odom 里的位置变化。
• rqt_console：查看日志输出，方便调试。

启动方法很简单：

# 在另一个终端运行
ros2 run rqt_graph rqt_graph
# 或者直接
rqt

你想想看，有了这些工具，调试效率能提升多少？不用再靠猜了。

4.6 本章知识体系

为了让你更直观地理解本章的核心逻辑，我画了一张图：

这张图清晰地展示了本章的核心逻辑：控制器节点发布 /cmd_vel 话题，仿真机器人订阅并执行，同时机器人发布 /odom 话题反馈状态，控制器订阅后用于闭环控制。

4.7 避坑指南与经验总结

最后，我总结几个实战中容易踩的坑：

• 话题名称拼写错误：/cmd_vel 不是 /cmd_velo，/odom 不是 /odometry。ROS2不会报错，但你的节点就是收不到数据。我曾经花了一下午查这个问题。
• 消息类型不匹配：发布者和订阅者必须使用完全相同的消息类型。比如 Twist 和 TwistStamped 是不同的。
• 忘记 spin()：ROS2节点需要 rclpy.spin() 才能持续运行。如果你发现回调函数不执行，八成是忘了 spin。
• 仿真时间 vs 真实时间：在仿真中，时间可能被加速或暂停。如果你用 time.sleep() 来控制运动，可能会出问题。建议使用ROS2的时钟 API。