3. ROS驱动开发：编写底盘驱动节点、订阅/cmd_vel话题、发布/odom话题

好，咱们进入正题。阿克曼底盘要动起来，得有个“大脑”来指挥。这个大脑就是ROS驱动节点。说白了，它就是个程序，一边听指令，一边汇报状态。

我个人习惯把驱动节点拆成三块：通信层、控制层、反馈层。通信层负责跟ROS打交道，控制层算电机指令，反馈层算里程计。今天咱们重点讲通信层——怎么订阅/cmd_vel，怎么发布/odom。

3.1 节点架构设计

先画个图，让你对整个流程有个直观印象。我习惯在写代码前先画框架图，这样不容易跑偏。

你看，整个流程很清晰。上层节点发速度指令，驱动节点接收后解算，然后通过串口或CAN发给硬件。硬件执行完，编码器数据返回，驱动节点再算出里程计发回去。

3.2 订阅 /cmd_vel 话题

/cmd_vel 是ROS里标准的运动指令话题。消息类型是 geometry_msgs/Twist，里面包含线速度和角速度。对于阿克曼底盘，我们只关心 linear.x（前进速度）和 angular.z（转向角速度）。

嗯，这里要注意：阿克曼底盘不能像差速底盘那样原地旋转。所以收到角速度后，得先转成前轮转角。我见过有人直接把角速度当转角发出去，结果车子原地打转——那肯定不对。

来看代码怎么写。这是订阅器的核心部分：

// cmd_vel 回调函数
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
    // 提取线速度和角速度
    double linear_x = msg->linear.x;
    double angular_z = msg->angular.z;
    
    // 阿克曼运动学解算
    double steering_angle = 0.0;
    double speed = linear_x;
    
    // 避坑：线速度太小时，转角直接置零
    if (fabs(linear_x) > 0.01)
    {
        steering_angle = atan2(wheelbase_ * angular_z, linear_x);
    }
    
    // 限制最大转角（防止机械碰撞）
    if (steering_angle > max_steering_angle_)
        steering_angle = max_steering_angle_;
    else if (steering_angle < -max_steering_angle_)
        steering_angle = -max_steering_angle_;
    
    // 发送到底层硬件
    sendToMotorDriver(speed, steering_angle);
}

我曾经在一个项目里遇到过，转角限幅没做好，结果前轮打死把转向拉杆给别断了。从那以后，我每次都会检查机械限位，代码里也加两层保护——软限位和硬限位。

3.3 发布 /odom 话题

/odom 话题发布的是里程计信息，消息类型是 nav_msgs/Odometry。它包含两部分：位姿（位置+朝向）和速度。位姿靠编码器积分推算，速度可以直接用编码器测得的实时值。

说白了，里程计就是个“积分器”。你每隔一小段时间读一次编码器，算出位移增量，累加起来就是当前位置。但积分会累积误差，所以长时间运行后位置会漂。这是所有里程计的通病，没办法。

发布里程计的代码框架：

void publishOdometry(double dt)
{
    // 从编码器读取左右轮速度
    double left_wheel_speed = readLeftEncoderSpeed();
    double right_wheel_speed = readRightEncoderSpeed();
    
    // 计算底盘线速度和角速度
    double v = (left_wheel_speed + right_wheel_speed) / 2.0;
    double omega = (right_wheel_speed - left_wheel_speed) / wheel_separation_;
    
    // 积分更新位姿
    double delta_x = v * cos(theta_) * dt;
    double delta_y = v * sin(theta_) * dt;
    double delta_theta = omega * dt;
    
    x_ += delta_x;
    y_ += delta_y;
    theta_ += delta_theta;
    
    // 填充 Odometry 消息
    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_footprint";
    
    // 位姿
    odom_msg.pose.pose.position.x = x_;
    odom_msg.pose.pose.position.y = y_;
    odom_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta_);
    
    // 速度
    odom_msg.twist.twist.linear.x = v;
    odom_msg.twist.twist.angular.z = omega;
    
    // 发布
    odom_pub_.publish(odom_msg);
    
    // 同时发布 tf 变换（导航需要）
    geometry_msgs::TransformStamped tf_msg;
    tf_msg.header = odom_msg.header;
    tf_msg.child_frame_id = "base_footprint";
    tf_msg.transform.translation.x = x_;
    tf_msg.transform.translation.y = y_;
    tf_msg.transform.translation.z = 0.0;
    tf_msg.transform.rotation = odom_msg.pose.pose.orientation;
    tf_broadcaster_.sendTransform(tf_msg);
}

你想想看，这段代码里最关键的变量是什么？是 wheel_separation_（轮距）。这个参数如果量错了，里程计算出来全是歪的。我建议你拿尺子亲自量，别信图纸上的标称值。图纸和实物之间，经常差个几毫米。

3.4 参数配置与调试

驱动节点里有很多参数需要配置。我习惯把它们放在一个YAML文件里，启动时加载。这样调参不用重新编译代码。

参数名	类型	说明	典型值
wheelbase	double	轴距（前后轮中心距）	0.325 m
wheel_separation	double	轮距（左右轮中心距）	0.285 m
max_steering_angle	double	最大转向角（弧度）	0.45 rad
encoder_ticks_per_meter	int	编码器每米脉冲数	3200
control_frequency	double	控制循环频率（Hz）	50.0

3.5 完整节点框架

最后，给你一个完整的节点框架。把上面几块拼起来，就是一个能用的驱动节点了。

class AckermannDriveNode
{
public:
    AckermannDriveNode() : nh_("~"), x_(0.0), y_(0.0), theta_(0.0)
    {
        // 加载参数
        nh_.param("wheelbase", wheelbase_, 0.325);
        nh_.param("wheel_separation", wheel_separation_, 0.285);
        nh_.param("max_steering_angle", max_steering_angle_, 0.45);
        
        // 订阅 /cmd_vel
        cmd_vel_sub_ = nh_.subscribe("/cmd_vel", 10, 
                                     &AckermannDriveNode::cmdVelCallback, this);
        
        // 发布 /odom
        odom_pub_ = nh_.advertise<nav_msgs::Odometry>("/odom", 10);
        
        // 定时器：控制循环
        control_timer_ = nh_.createTimer(ros::Duration(0.02), 
                                         &AckermannDriveNode::controlLoop, this);
    }
    
private:
    void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg);
    void controlLoop(const ros::TimerEvent& event);
    void publishOdometry(double dt);
    
    // ROS 句柄
    ros::NodeHandle nh_;
    ros::Subscriber cmd_vel_sub_;
    ros::Publisher odom_pub_;
    ros::Timer control_timer_;
    tf::TransformBroadcaster tf_broadcaster_;
    
    // 参数
    double wheelbase_, wheel_separation_, max_steering_angle_;
    
    // 里程计状态
    double x_, y_, theta_;
};

写到这里，我想起刚入行时的一个教训。那时候我写驱动节点，把订阅和发布的频率设得不一样——订阅50Hz，发布10Hz。结果导航模块老是报“里程计更新太慢”。后来统一成50Hz，问题就解决了。所以，控制循环的频率要一致，别自己给自己挖坑。

好了，驱动节点的核心逻辑就这些。你照着这个框架写，把串口或CAN通信的部分填进去，就能让阿克曼底盘跑起来了。记住，调试时先用手推着车走，看里程计数据对不对，再上电跑。安全第一。

---