4. 控制指令订阅与执行：cmd_vel话题订阅、Joint Trajectory Controller、差速驱动模型实现
好，咱们进入第四章。
这一章，说白了就是让机器人动起来。你给机器人发个速度指令，它得乖乖执行。这背后涉及三个核心环节：指令接收、控制器调度、底层执行。
我个人习惯把这一章叫做「机器人的运动神经」。没有它，机器人就是个铁疙瘩。
4.1 cmd_vel话题订阅：机器人的耳朵
机器人怎么听指令？靠的就是 cmd_vel 话题。这是ROS里最经典的话题之一，类型是 geometry_msgs/Twist。
你想想看，一个 Twist 消息里就两个字段：linear（线速度）和 angular（角速度）。就这么简单。
核心要点：
- 话题名：
/cmd_vel（通常带命名空间） - 消息类型：
geometry_msgs/Twist - 发布频率：一般在10Hz~50Hz之间
我在项目中遇到过一个问题：订阅了 cmd_vel 但机器人不动。查了半天，发现是话题名写错了——多了一个斜杠。嗯，这种低级错误，谁还没犯过呢？
下面是一个简单的订阅器示例：
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
// 拿到线速度和角速度
double linear_x = msg->linear.x;
double angular_z = msg->angular.z;
ROS_INFO("收到指令: 线速度=%.2f, 角速度=%.2f", linear_x, angular_z);
// 这里可以调用你的驱动函数
// motor_driver.setSpeed(linear_x, angular_z);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cmd_vel_subscriber");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("/cmd_vel", 10, cmdVelCallback);
ros::spin();
return 0;
}
小技巧：回调函数里别做耗时操作。我习惯只做数据转换和缓存，真正的驱动调用放到单独的控制循环里。否则容易丢包。
4.2 Joint Trajectory Controller：关节的指挥官
如果你用的是关节型机器人（比如机械臂），那 cmd_vel 就不够用了。你需要 Joint Trajectory Controller。
这东西是 ros_control 框架里的核心组件。它接收 trajectory_msgs/JointTrajectory 消息，然后按照时间戳规划出每个关节的位置、速度、加速度。
说白了，它就是个「关节动作编排器」。你告诉它「1秒后，关节1转到30度，关节2转到45度」，它就会自动算好中间怎么走。
| 参数 | 说明 | 我常用的值 |
|---|---|---|
| joints | 控制的关节名称列表 | ["joint1", "joint2", ...] |
| constraints | 位置/速度/加速度约束 | goal_time: 0.5, stopped_velocity_tolerance: 0.05 |
| gains | PID参数 | p: 100, i: 0.1, d: 10 |
注意：Joint Trajectory Controller 对时间戳非常敏感。我曾经因为系统时间不同步，导致轨迹规划器算出来的时间全是负数——机器人直接抽搐。后来加了 ros::Time::now() 同步才解决。
4.3 差速驱动模型实现：两轮机器人的灵魂
差速驱动，这是移动机器人最经典的底盘模型。两个独立驱动的轮子，通过速度差实现转向。
公式其实很简单：
// 已知：机器人轮距 wheel_separation，轮半径 wheel_radius
// 输入：cmd_vel 中的 linear.x 和 angular.z
double v = msg->linear.x; // 机器人中心线速度
double w = msg->angular.z; // 机器人角速度
// 计算左右轮速度
double left_wheel_speed = (v - w * wheel_separation / 2.0) / wheel_radius;
double right_wheel_speed = (v + w * wheel_separation / 2.0) / wheel_radius;
// 然后发给电机驱动
motor_left.setSpeed(left_wheel_speed);
motor_right.setSpeed(right_wheel_speed);
你看，就这么几行代码。但实际项目中坑不少。
避坑指南（我曾经踩过的）：
- 轮距单位：一定要用米。我见过有人用毫米算，结果机器人原地转圈圈。
- 速度限幅：电机有最大转速，别忘了做
clamp。否则指令发出去，电机直接过载。 - 死区处理：小速度时电机可能不转。我习惯加个阈值，低于0.01m/s就直接发0。
4.4 整体架构：一张图说清楚
下面这张图，是我自己画的控制流。你看一眼就明白了：
这张图里，我特意把反馈回路也画出来了。实际项目中，你订阅 cmd_vel 后，还要发布 odom（里程计）回来。这样才能形成闭环控制。
4.5 实战建议：先仿真再真机
我个人强烈建议：先在Gazebo里跑通这套流程。Gazebo里有个 diff_drive_controller 插件，你配好参数就能直接用。
配置示例（YAML）：
diff_drive_controller:
type: "diff_drive_controller/DiffDriveController"
left_wheel: ['left_wheel_joint']
right_wheel: ['right_wheel_joint']
wheel_separation: 0.35
wheel_radius: 0.1
publish_rate: 50.0
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
cmd_vel_timeout: 0.5
enable_odom_tf: true
提示：cmd_vel_timeout 这个参数很关键。如果超过0.5秒没收到新指令，控制器会自动刹车。我见过有人没设这个，机器人跑出去后遥控器断连，结果它一路撞墙。
好了，这一章的内容就这些。核心就是：订阅指令 → 模型计算 → 驱动执行。你把这个链条打通了，机器人就能听你使唤了。
可可精品资料库