上位机控制相关

base_control.cpp代码如下：

/******************************************************************
基于串口通信的ROS小车基础控制器，功能如下：
1.实现ros控制数据通过固定的格式和串口通信，从而达到控制小车的移动
2.订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动
3.发布里程计主题/odm 串口通信说明：
1.写入串口 （1）内容：左右轮速度，单位为mm/s （2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]
2.读取串口 （1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s （2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]
*******************************************************************/

#include "ros/ros.h"  //导入ROS系统包含核心公共头文件
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>//我们需要实现“odom”参考系到“base_link”参考系的变换
#include <nav_msgs/Odometry.h>//以及nav_msgs/Odometry消息的发布

//以下为串口通讯需要的头文件

#include <string>        
#include <iostream>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "serial/serial.h"

/****************************************************************************/
using std::string;       //使用C＋＋标准程序库的任何标识符
using std::exception;    //还可以有以下两种写法
using std::cout;         //直接指定标识符。例如std::ostream而不是ostream
using std::cerr;         //如std::cout << std::hex << 3.4 << std::endl;
using std::endl;         //最方便的就是使用using namespace std;这样命名空间std内定义的所有标识符都有效（曝光）。就好像它们被声明为全局变量一样
using std::vector;       //#include <iostream>，#include <sstream>，#include <string>，using namespace std;
/*****************************************************************************/

float ratio = 1000.0f ;   //转速转换比例，执行速度调整比例
float D = 0.2680859f ;    //两轮间距，单位是m
float linear_temp=0,angular_temp=0;//暂存的线速度和角速度

/****************************************************/
unsigned char data_terminal0=0x0d;  //“/r"字符回车
unsigned char data_terminal1=0x0a;  //“/n"字符换行
                                    // 0d 0a这是自行定义的一种数据帧结束标志
unsigned char speed_data[10]={0};   //要发给串口的数据
string rec_buffer;  //串口数据接收变量
                    //String：适用于少量的字符串操作的情况
　                  //StringBuilder：适用于单线程下在字符缓冲区进行大量操作的情况
                    //StringBuffer：适用多线程下在字符缓冲区进行大量操作的情况
    //发送给下位机的左右轮速度，里程计的坐标和方向
    union floatData //union的作用为实现char数组和float之间的转换
    {
        float d;
        unsigned char data[4];
    }right_speed_data,left_speed_data,position_x,position_y,oriention,vel_linear,vel_angular;
    /************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
  //Twist消息，它的Topic是/cmd_vel，base controler订阅Twist消息来控制电机。
    {
        string port("/dev/ttyUSB0");    //小车串口号
        unsigned long baud = 115200;    //小车串口波特率
        serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口
     
angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s

//将转换好的小车速度分量为左右轮速度
left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;
right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;

//存入数据到要发布的左右轮速度消息
left_speed_data.d*=ratio;   //放大１０００倍，mm/s
right_speed_data.d*=ratio;//放大１０００倍，mm/s

for(int i=0;i<4;i++)    //将左右轮速度存入数组中发送给串口
{
    speed_data[i]=right_speed_data.data[i];
    speed_data[i+4]=left_speed_data.data[i];
}

//在写入串口的左右轮速度数据后加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//写入数据到串口
my_serial.write(speed_data,10);
}
 
int main(int argc, char **argv)// argv 是一个指针数组，他的元素个数是argc，存放的是指向每一个参数的指针
{
    string port("/dev/ttyUSB0");//小车串口号
    unsigned long baud = 115200;//小车串口波特率
    serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口


ros::init(argc, argv, "base_controller");//初始化串口节点
ros::NodeHandle n;  //定义节点进程句柄

ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题
ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); 
//发布一个消息类型为nav_msgs，命名为odom的话题。定义消息队列大小为20，即超过20条消息之后，旧的消息就会丢弃

static tf::TransformBroadcaster odom_broadcaster;//定义一个tf广播，来发布tf变换信息。
geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息
nav_msgs::Odometry odom;//定义里程计对象
geometry_msgs::Quaternion odom_quat; //四元数变量
//定义covariance协方差矩阵，作用为解决文职和速度的不同测量的不确定性
float covariance[36] = {0.01,   0,    0,     0,     0,     0,  // covariance on gps_x
                        0,  0.01, 0,     0,     0,     0,  // covariance on gps_y
                        0,  0,    99999, 0,     0,     0,  // covariance on gps_z
                        0,  0,    0,     99999, 0,     0,  // large covariance on rot x
                        0,  0,    0,     0,     99999, 0,  // large covariance on rot y
                        0,  0,    0,     0,     0,     0.01};  // large covariance on rot z 
//载入covariance矩阵
for(int i = 0; i < 36; i++)
{
    odom.pose.covariance[i] = covariance[i];;
}       

ros::Rate loop_rate(10);
//设置周期休眠时间，指定发布消息的频率，这里指10Hz，也即每秒10次
//通过 Rate::sleep()来处理睡眠的时间来控制对应的发布频率。
    while(ros::ok())
    //默认roscpp会植入一个SIGINT处理机制，当按下Ctrl-C，就会让ros::ok()返回false,那循环就会结束。
    //ros::ok() 返回false的几种情况：
    //SIGINT收到(Ctrl-C)信号
    //另一个同名节点启动，会先中止之前的同名节点
    //ros::shutdown()被调用
    //所有的ros::NodeHandles被销毁
        {
            rec_buffer =my_serial.readline(25,"\n");    //获取串口发送来的数据
            //readLine()是读取流读数据的时候用的，当读到换行标记'\n'、'\r'（回车）时，会跟着换行，同时会以字符串形式返回这一行的数据，当读取完所有的数据时会返回null
            const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
            if(rec_buffer.length()==21) //串口接收的数据长度正确就处理并发布里程计数据消息
            {
                for(int i=0;i<4;i++)//提取X，Y坐标，方向，线速度，角速度
                {
                    position_x.data[i]=receive_data[i];
                    position_y.data[i]=receive_data[i+4];
                    oriention.data[i]=receive_data[i+8];
                    vel_linear.data[i]=receive_data[i+12];
                    vel_angular.data[i]=receive_data[i+16];
                }
                //将X，Y坐标，线速度缩小1000倍
                position_x.d/=1000; //m
                position_y.d/=1000; //m
                vel_linear.d/=1000; //m/s


        // 为了兼容二维和三维的功能包，让消息结构更加通用，里程计的偏航角需要转换成四元数才能发布
  odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//将偏航角转换成四元数

        //载入坐标（tf）变换时间戳
        //时间戳是一个时间标记，用以表明该信息的唯一性。
        odom_trans.header.stamp = ros::Time::now();
        //发布坐标变换的父子坐标系
        odom_trans.header.frame_id = "odom";     
        odom_trans.child_frame_id = "base_footprint";       
        //tf位置数据：x,y,z,方向
        odom_trans.transform.translation.x = position_x.d;
        odom_trans.transform.translation.y = position_y.d;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;        
        //发布tf坐标变化
        odom_broadcaster.sendTransform(odom_trans);

        //载入里程计时间戳
        odom.header.stamp = ros::Time::now(); 
        //里程计的父子坐标系
        odom.header.frame_id = "odom";
        odom.child_frame_id = "base_footprint";       
        //里程计位置数据：x,y,z,方向
        odom.pose.pose.position.x = position_x.d;     
        odom.pose.pose.position.y = position_y.d;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;       
        //载入线速度和角速度
        odom.twist.twist.linear.x = vel_linear.d;
        //odom.twist.twist.linear.y = odom_vy;
        odom.twist.twist.angular.z = vel_angular.d;    
        //发布里程计
        odom_pub.publish(odom);

        ros::spinOnce();//周期执行如果程序里也有订阅话题，就必需，否则回调函数不能起作用。
  loop_rate.sleep();//周期休眠
    }
    //程序周期性调用
    //ros::spinOnce();  //callback函数必须处理所有问题时，才可以用到
}
return 0;

}

odom：
里程计坐标系，这里要区分开odom topic，这是两个概念，一个是坐标系，一个是根据编码器（或者视觉等）计算的里程计。但是两者也有关系，odom topic 转化得位姿矩阵是odom–>base_link的tf关系。这时可有会有疑问，odom和map坐标系是不是重合的？（这也是我写这个博客解决的主要问题）可以很肯定的告诉你，机器人运动开始是重合的。但是，随着时间的推移是不重合的，而出现的偏差就是里程计的累积误差。那map–>odom的tf怎么得到?就是在一些校正传感器合作校正的package比如gmapping会给出一个位置估计（localization），这可以得到map–>base_link的tf，所以估计位置和里程计位置的偏差也就是odom与map的坐标系偏差。所以，如果你的odom计算没有错误，那么map–>odom的tf就是0.
quat：
oculus获取的pose数据包含坐标和四元数信息，但所处的坐标系和CryEngine坐标系不同

oculus坐标系 ：+x向右，+y向上，+z向外（屏幕外）

cryengine坐标系：+x向右，+y向内（屏幕以内），+z向上

回调函数ros::spin()与ros::spinOnce()
ros::spin()

这句话的意思是循环且监听反馈函数（callback）。循环就是指程序运行到这里，就会一直在这里循环了。监听反馈函数的意思是，如果这个节点有callback函数，那写一句ros::spin()在这里，就可以在有对应消息到来的时候，运行callback函数里面的内容。
就目前而言，这句话适用于写在程序的末尾（因为写在这句话后面的代码不会被执行），适用于订阅节点，且订阅速度没有限制的情况。

ros::spinOnce()

这句话的意思是监听反馈函数（callback）。只能监听反馈，不能循环。所以当你需要监听一下的时候，就调用一下这个函数。
这个函数比较灵活，尤其是我想控制接收速度的时候。配合ros::ok()效果极佳

ROS的主循环中需要不断调用ros::spin()或ros::spinOnce()，两者区别在于前者调用后不会再返回，而后者在调用后还可以继续执行之后的程序。

在使用ros::spin()的情况下，一般来说在初始化时已经设置好所有消息的回调，并且不需要其他背景程序运行。这样一来，每次消息到达时会执行用户的回调函数进行操作，相当于程序是消息事件驱动的；而在使用ros::spinOnce()的情况下，一般来说仅仅使用回调不足以完成任务，还需要其他辅助程序的执行：比如定时任务、数据处理、用户界面等。

原理：除了用户的主程序以外，ROS的socket连接控制进程会在后台接收订阅的消息，所有接收到的消息并不是立即处理，而是等到spin()或者spinOnce()执行时才集中处理。

I. 对于速度较快的消息，需要注意合理控制消息队列及spinOnce()的时间。例如，如果消息到达的频率是100Hz，而spinOnce()的执行频率是10Hz，那么就要至少保证消息队列中预留的大小大于10。

II. 如果对于用户自己的周期性任务，最好和spinOnce()并列调用。即使该任务是周期性的对于数据进行处理，例如对接收到的IMU数据进行Kalman滤波，也不建议直接放在回调函数中：因为存在通信接收的不确定性，不能保证该回调执行在时间上的稳定性。