差分轮算法－两个轮子计算速度的方法-阿克曼四轮小车计算方法

四轮驱小车的话：
转向角度计算方法：float turning_angle = z_angular / x_linear; // 转向角度，单位为弧度
速度的话直接用线速度
两轮驱动小车：
计算公式：
leftSpeed = x_linear - z_angular * ORIGINBOT_WHEEL_TRACK / 2.0; #左轮速度
rightSpeed = x_linear + z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;#右轮速度
其中x_linear和z_angular是由del_cmd话题发出来的twist数据

void OriginbotBase::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
    DataFrame cmdFrame;
    float leftSpeed = 0.0, rightSpeed = 0.0;

    float x_linear = msg->linear.x; 
    float z_angular = msg->angular.z;

    //差分轮运动学模型求解
    leftSpeed  = x_linear - z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;
    rightSpeed = x_linear + z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;

    // RCLCPP_INFO(this->get_logger(), "leftSpeed = '%f' rightSpeed = '%f'", leftSpeed * 100, rightSpeed * 100);

    if (leftSpeed < 0)
        cmdFrame.data[0] = 0x00;
    else
        cmdFrame.data[0] = 0xff;
    cmdFrame.data[1] = int(abs(leftSpeed) * 1000) & 0xff;         //速度值从m/s变为mm/s
    cmdFrame.data[2] = (int(abs(leftSpeed) * 1000) >> 8) & 0xff;

    if (rightSpeed < 0)
        cmdFrame.data[3] = 0x00;
    else
        cmdFrame.data[3] = 0xff;
    cmdFrame.data[4] = int(abs(rightSpeed) * 1000) & 0xff;        //速度值从m/s变为mm/s
    cmdFrame.data[5] = (int(abs(rightSpeed) * 1000) >> 8) & 0xff;

    cmdFrame.check = (cmdFrame.data[0] + cmdFrame.data[1] + cmdFrame.data[2] + 
                      cmdFrame.data[3] + cmdFrame.data[4] + cmdFrame.data[5]) & 0xff;

    // 封装速度命令的数据帧
    cmdFrame.header = 0x55;
    cmdFrame.id     = 0x01;
    cmdFrame.length = 0x06;
    cmdFrame.tail   = 0xbb;
    try
    {
        serial_.write(&cmdFrame.header, sizeof(cmdFrame)); //向串口发数据
    }

    catch (serial::IOException &e)
    {
        RCLCPP_ERROR(this->get_logger(), "Unable to send data through serial port"); //如果发送数据失败,打印错误信息
    }

    // 考虑平稳停车的计数值
    if((fabs(x_linear)>0.0001) || (fabs(z_angular)>0.0001))
        auto_stop_count_ = 0;

    // printf("Frame raw data: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x \n", 
    //         cmdFrame.header, cmdFrame.id, cmdFrame.length, cmdFrame.data[0], cmdFrame.data[1], cmdFrame.data[2], 
    //         cmdFrame.data[3], cmdFrame.data[4], cmdFrame.data[5], cmdFrame.check, cmdFrame.tail);
}