4、编写第一个运动控制节点：使用Python编写速度发布节点、让机器人动起来
好，到了真正动手的时候了。
前面几节我们聊了Gazebo的界面、话题、还有怎么用命令行发指令。但说实话，每次手动敲rostopic pub太累了，也不够“自动化”。真正的机器人控制，得靠节点来干。
这一节，我们就来写第一个运动控制节点。用Python，发布速度指令，让Gazebo里的小车动起来。
4.1 先搞清楚我们要做什么
说白了，我们要写一个Python脚本。这个脚本会做三件事：
- 连接到ROS Master（就是roscore）
- 创建一个Publisher，往
/cmd_vel话题上发消息 - 循环发送速度指令，让机器人前进、后退、转弯
嗯，就这么简单。但别小看它，这是所有运动控制的起点。
核心知识点：
- Publisher的创建与使用
- Twist消息的结构
- 循环与频率控制（Rate）
4.2 先看一眼Twist消息长什么样
在ROS里，速度指令用的是geometry_msgs/Twist消息。我刚开始学的时候，总觉得这个名字挺唬人。其实拆开看，就两个部分：
| 字段 | 类型 | 含义 |
|---|---|---|
| linear.x | float64 | 前进/后退速度（米/秒） |
| linear.y | float64 | 横向移动（差速小车一般不用） |
| linear.z | float64 | 垂直速度（地面机器人用不到） |
| angular.z | float64 | 旋转角速度（弧度/秒） |
你看，真正常用的就两个：linear.x和angular.z。前进靠前者，转弯靠后者。
4.3 写代码：第一个速度发布节点
我个人习惯，先建一个文件夹放脚本。在终端里执行：
cd ~/catkin_ws/src
mkdir -p my_robot_control/scripts
cd my_robot_control/scripts
touch move_robot.py
chmod +x move_robot.py然后打开move_robot.py，把下面的代码贴进去：
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import Twist
def move_robot():
# 初始化节点
rospy.init_node('move_robot_node', anonymous=True)
# 创建发布者，发布到 /cmd_vel 话题
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
# 设置发布频率：10Hz
rate = rospy.Rate(10)
# 创建一个Twist消息对象
vel_msg = Twist()
# 设置线速度和角速度
vel_msg.linear.x = 0.5 # 前进 0.5 m/s
vel_msg.angular.z = 0.0 # 不转弯
# 记录一下开始时间
start_time = rospy.Time.now()
rospy.loginfo("机器人开始前进...")
# 循环发布，持续5秒
while not rospy.is_shutdown():
# 检查是否已经过了5秒
elapsed = (rospy.Time.now() - start_time).to_sec()
if elapsed > 5.0:
break
# 发布速度指令
pub.publish(vel_msg)
# 按设定频率休眠
rate.sleep()
# 停止机器人
vel_msg.linear.x = 0.0
pub.publish(vel_msg)
rospy.loginfo("机器人已停止")
if __name__ == '__main__':
try:
move_robot()
except rospy.ROSInterruptException:
pass小提示：我刚开始写的时候，经常忘记在最后发一个零速度指令。结果机器人一直往前冲，撞到墙才停。记得一定要在退出前把速度归零。
4.4 运行起来看看效果
代码写好了，怎么跑？三步走：
- 启动roscore（新开终端）：
roscore - 启动Gazebo世界（再开一个终端）：
roslaunch my_robot_gazebo empty_world.launch - 运行我们的节点（第三个终端）：
rosrun my_robot_control move_robot.py
你会看到，小车开始往前走了。5秒后，自动停下。
为什么会这样？因为我们的节点每0.1秒（10Hz）发一次速度指令。Gazebo里的仿真模型收到后，就会更新位置。说白了，就是靠这种“高频指令”来驱动仿真。
4.5 让机器人画个圈
光走直线太无聊了。我们改一下参数，让机器人画个圈：
vel_msg.linear.x = 0.5 # 前进速度
vel_msg.angular.z = 0.5 # 旋转速度同时把循环时间改成10秒。再跑一次，你会发现机器人走出了一个圆弧。如果线速度和角速度配合得当，理论上能画出完美的圆。
我在项目中遇到过一个问题：角速度设太大，机器人原地打转，根本走不出去。后来才意识到，线速度和角速度的比例决定了转弯半径。比例越大，转弯越平缓。
注意：不同机器人的最大速度不一样。差速小车的最大线速度一般不超过1m/s，角速度不超过1rad/s。设太大，仿真里的运动会变得很假，甚至出现穿模。
4.6 用SVG理清整个流程
下面这张图，是我自己画的知识结构。你看一眼，就能明白整个运动控制节点的逻辑：
你看，整个流程其实就三步：脚本创建发布者 → 发布Twist消息 → Gazebo接收并执行。然后循环回去，继续发下一条指令。
4.7 避坑指南：我踩过的几个坑
写这个节点的时候，我曾经犯过几个低级错误，分享出来，你们别走弯路：
- 忘记启动roscore：节点一跑就报错
Unable to communicate with master。嗯，ROS的规矩，先有roscore，才有其他节点。 - 话题名拼错：我写过
/cmd_vel少了个下划线，结果机器人纹丝不动。用rostopic list检查一下，最保险。 - 频率太低：一开始设了1Hz，机器人走一步停一步，像卡顿一样。后来改成10Hz，运动就平滑了。
- 忘记停止：节点退出前没发零速度，机器人一直往前冲。后来我养成了习惯，在
rospy.on_shutdown里注册一个停止回调。
总结一下：
这一节我们干了三件事：理解了Twist消息、写了一个Python发布节点、让机器人在Gazebo里动了起来。说白了，运动控制的本质就是“发速度指令”。后面所有复杂的控制算法，都是在这个基础上叠加的。
好，代码写完了，跑通了，你也算正式踏入ROS机器人控制的大门了。接下来，你可以试着改改参数，让机器人走个正方形、走个8字形。玩熟了，自然就理解了。
一个小练习：修改代码，让机器人先前进2秒，再左转2秒，再前进2秒，再右转2秒。看看能不能走出一条“S”形路线。
可可精品资料库