📚 课程目录第1章单机器人基础控制

1. 单机器人基础控制

任务目标

本案例实现一个最小可运行的ROS2速度控制闭环：向机器人发布cmd_vel速度指令，同时订阅odom里程计话题，在终端中观察机器人是否按指令运动。该案例是后续多机器人调度的基础单元。

环境与依赖

Ubuntu 22.04 + ROS2 Humble（或更高版本）
已安装turtlesim（用于模拟验证）或具备真实机器人驱动节点
Python3 + rclpy

核心代码

创建ROS2 Python包后，在src/basic_control/basic_control/目录下新建velocity_controller.py：

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

class VelocityController(Node):
    def __init__(self):
        super().__init__('velocity_controller')
        # 发布速度指令到/cmd_vel
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
        # 订阅里程计话题/odom
        self.odom_sub = self.create_subscription(
            Odometry, '/odom', self.odom_callback, 10)
        # 控制参数
        self.target_linear = 0.5      # 目标线速度 m/s
        self.target_angular = 0.0     # 目标角速度 rad/s
        self.duration = 5.0           # 运动时长 s
        self.timer = self.create_timer(0.1, self.timer_callback)
        self.start_time = self.get_clock().now()
        self.current_pose = None

    def odom_callback(self, msg):
        # 提取位置信息用于验证
        self.current_pose = msg.pose.pose

    def timer_callback(self):
        elapsed = (self.get_clock().now() - self.start_time).nanoseconds / 1e9
        if elapsed < self.duration:
            cmd = Twist()
            cmd.linear.x = self.target_linear
            cmd.angular.z = self.target_angular
            self.cmd_pub.publish(cmd)
        else:
            # 时间到，停止运动
            cmd = Twist()
            cmd.linear.x = 0.0
            cmd.angular.z = 0.0
            self.cmd_pub.publish(cmd)
            self.get_logger().info('运动完成，已停止')
            # 打印最终位置（若已收到里程计）
            if self.current_pose is not None:
                x = self.current_pose.position.x
                y = self.current_pose.position.y
                self.get_logger().info(f'最终位置: x={x:.2f}, y={y:.2f}')
            # 关闭定时器
            self.timer.cancel()

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

if __name__ == '__main__':
    main()

在setup.py中注册入口点：

entry_points={
    'console_scripts': [
        'velocity_controller = basic_control.velocity_controller:main',
    ],
},

运行与验证

编译并source环境：在ROS2工作空间根目录执行colcon build --packages-select basic_control，然后source install/setup.bash。
启动模拟器（无真实机器人时）：ros2 run turtlesim turtlesim_node。注意：turtlesim默认话题为/turtle1/cmd_vel和/turtle1/pose，需做话题重映射：ros2 run turtlesim turtlesim_node --ros-args --remap cmd_vel:=/cmd_vel --remap pose:=/odom（pose消息类型与Odometry不同，仅用于验证运动方向，位置读取需改用turtle1/pose）。
启动控制节点：另开终端，ros2 run basic_control velocity_controller。
观察结果：在turtlesim窗口中，乌龟应沿x轴正向直线移动约5秒后停止。终端中应打印“运动完成，已停止”。若使用真实机器人，需确保/cmd_vel和/odom话题名称与驱动节点一致。

通过判定标准：

节点启动无报错，ros2 topic echo /cmd_vel能看到线速度0.5 m/s持续发布约5秒，随后变为0。
若使用turtlesim，乌龟明显移动一段距离后停下；若使用真实机器人，里程计位置变化约2.5米（0.5 m/s × 5 s），方向无偏转。
终端输出“运动完成，已停止”日志。

常见问题

话题不匹配：确认发布/订阅话题名称与机器人驱动一致，可用ros2 topic list检查。
权限问题：真实机器人串口需sudo chmod 666 /dev/ttyUSB0（按实际设备号）。
时间同步：若使用仿真时间，需在启动节点前设置ros2 param set /velocity_controller use_sim_time true。