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。