上应大机协之书

AChassis接口

sit_chassis/include/sit_chassis/AChassis.h

该文件定义了二维平面机器人的抽象接口，该接口存在两个待实现的纯虚函数。

    /**
     * @brief 对底盘设置一个期望速度
     * 
     * @param vx x轴方向的期望速度
     * @param vy y轴方向上的期望速度
     * @param vw 环绕z轴方向的转速
     */
    virtual void setSpeed(double vx,double vy,double vw) = 0;

    /**
     * @brief 获取底盘当前时刻的速度
     * 
     * @param vx x轴方向的实际速度
     * @param vy y轴方向上的实际速度
     * @param vw 环绕z轴方向的转速
     */
    virtual void getSpeed(double& vx,double& vy,double& vw) = 0;

AThreeChassis抽象类

sit_chassis/include/sit_chassis/AThreeChassis.h

该类定义了一个抽象的三轮全向底盘的运动学实现的抽象类，它提供了两个纯虚函数，

    /**
     * @brief 设置车轮的期望线速度，具体通过怎样的方式下发速度指令，继续由子类实现，
     * 车轮线速度单位为 m/s
     * 
     * @param v1 1号车轮的线速度
     * @param v2 2号车轮的线速度
     * @param v3 3号车轮的线速度
     */
    virtual void setWheelSpeed(double v1, double v2, double v3) = 0;

    /**
     * @brief 获取当前时刻车轮速度
     * 
     * @param v1 1号车轮的线速度
     * @param v2 2号车轮的线速度
     * @param v3 3号车轮的线速度
     */
    virtual void getWheelSpeed(double& v1,double& v2,double& v3) = 0;

在本类中，我们假设调用这些纯虚函数即可实现设置轮速与获取轮速。

根据运动学公式，即可实现如下底盘的速度获取与设置的成员函数。

    void setSpeed(double vx, double vy, double vw) override
    {
        const double M_PI_6 = M_PI / 6;
        //ROS_INFO("Speed:[%f, %f, %f]", vx, vy, vw);
        //需要将速度进行分解到三个轮子上
        double vb = -vy + vw * L;
        double vl = -vx * cos(M_PI_6) + vy * sin(M_PI_6) + vw * L;
        double vr = vx * cos(M_PI_6) + vy * sin(M_PI_6) + vw * L;

        //设置轮速
        setWheelSpeed(vb, vr, vl);
    }

    void getSpeed(double& vx,double& vy,double& vw) override
    {
        //需要获取三个轮子的轮速，合成出轮子速度

        //获取三个轮子速度
        double v1,v2,v3;
        getWheelSpeed(v1,v2,v3);

        //速度合成
        vx = (v2 - v3) * (sqrt(3) / 3);
        vy = (-2 * v1+ v2+ v3) / 3;
        vw = (v1 + v2 + v3) / (3 * L);
    }

ThreeChassis实现类

sit_chassis/include/sit_chassis/ThreeChassis.h

该类通过sit_protocol功能包实现向下位机获取与设置原始轮速，本类继续通过引入比例系数K，结合原始轮速实现了AThreeChassis抽象类中的设置与获取轮速的纯虚函数。

ChassisNode类

sit_chassis/include/sit_chassis/ChassisNode.h

该类实现了AChassis接口与ros的发布订阅机制的结合，前面之所以划分出较为复杂的抽象层次结构，最精彩的便是在本类中体现，观察本类发现，其并不依赖于任何具体实现类，而仅依赖了AChassis接口。

熟悉设计模式的同学可以看出，这里实现了依赖倒置原则。它的好处就是，借助c++的动态多态性，我们可以实现更多的运动模型底盘实现类，其均可作为AChassis传入本类的构造函数，从而将底盘接入整个ros生态，这大大提高了软件的可复用性，降低了耦合度。

主入口类

sit_chassis/src/three_chassis_node.cpp

该类为主入口类，其实现较为简单，代码如下，

#include <ros/ros.h>
#include <sit_chassis/ThreeChassis.h>
#include <sit_chassis/ChassisNode.h>

int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "three_chassis"); //初始化三轮底盘节点
    ros::NodeHandle nh("~");
    ThreeChassis chassis(nh);

    ChassisNode cn(nh,&chassis);
    for (ros::Rate rate(20); ros::ok(); rate.sleep())
    {
        cn.spinOnce();
        ros::spinOnce();
    }
    return 0;
}

launch文件

sit_chassis/launch/chassis.launch

<?xml version="1.0"?>
<launch>
    <arg name="k" default="-4772.44"/>
    <arg name="radius" default="0.1900"/>
    <node pkg="sit_chassis" type="three_chassis_node" name="chassis_node" output="screen">
        <param name="k" value="$(arg k)"/>
        <param name="radius" value="$(arg radius)"/>
    </node>
</launch>

该文件启动一个三轮底盘节点，需要传入两个标定系数k与底盘半径radius。

底盘半径radius一般为手动测量得出。

标定系数k可由脚本自动标定得出。

script脚本

sit_chassis/scripts/calibrator.py

本脚本用于标定出底盘系数k。