前言

在上两篇博客中我介绍了一下上位机与下位机的通信以及两轮差速机器人里程计的编写，其实这一切都是为了这篇底层代码的实现打下的基础。

那么上位机底层都包含哪些内容呢，又该如何进行简单实现呢？

应包含至少以下几个功能：

1.订阅控制命令并实现向下位机发送

2.接受下位机数据的反馈，并进行处理

3.完成里程计的计算和发布，同时发布TF坐标变换

正文

那么就让我们一起来看具体步骤吧：

1. 创建一个用于底层的cpp节点文件：

在创建上位机底层代码时，需要在ROS的工作空间基础上创建一个ROS节点，并指定节点名称和节点类型。这里我初始化节点名称为“car_base”,同时新建了类，我新建了一个头文件并把类放在了头文件中，然后进行类的初始化。

2. 根据所需要用到的消息决定头文件：

这里我们用到了TF信息，IMU信息、Odometry信息、Twist格式信息以及四元数，并使用了ros的串口包，所以包含下面头文件。

#include <iostream>
#include <serial/serial.h> //ros串口包
#include "ros/ros.h"
#include"tf2_ros/transform_broadcaster.h"
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <sensor_msgs/Range.h>

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Quaternion.h>

3.定义通信时用于发送和接受数据的结构体

定义了头文件之后便要定义一些常用的变量

这里我的报文格式如下：

其中包头为“0xFE”和"0xEE"，L_mode、和R_mode分别为左右电机运行模式，最后一位都是校验位，其中我在下位机到上位机的通信中还预留了IMU的数据位。

那么最终用于发送的数据结构体就如下：

//发送字节数
#define SEND_DATA_Num 12    
//接收字节数
#define Receive_Num 30 

struct cmd {
    unsigned char null1;
    unsigned char H1;
    unsigned char H2;
    unsigned char l_mode;
    unsigned char r_mode;
    uint16_t Servo_PWM_l;
    uint16_t Servo_PWM_r;
    unsigned char null2;
    unsigned char null3;
    unsigned char CS;
};
union Send_Cmd{
    struct cmd cmd0;
    unsigned char data[SEND_DATA_Num];
};
struct RECEIVE_DATA
{
    unsigned char receive[Receive_Num];
};

4.在类的构造函数中进行初始化并打开串口

在刚定义的car_base类的构造函数中初始化节点句柄、订阅者发布者，以及打开串口或者其他通信接口，以及设置一些参数服务器。在打开端口时需要注意设置正确的参数，例如串口名、波特率、数据位、停止位、奇偶校验等。

try
    { 
        //尝试初始化与开启串口
        Stm32_Serial.setPort(uart_port_name); //选择要开启的串口号
        Stm32_Serial.setBaudrate(serial_baud_rate); //设置波特率
        serial::Timeout _time = serial::Timeout::simpleTimeout(2000);  //超时等待
        Stm32_Serial.setTimeout(_time);
        Stm32_Serial.open();  //开启串口
        if(Stm32_Serial.isOpen())
        {
            ROS_INFO_STREAM("car_robot serial port opened");  //串口开启成功提示
        }
    }
catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("car_robot can not open serial port,Please check the serial port cable! "); //如果开启串口失败，打印错误信息
    }

5.数据的发送

这里呢写了这样的一个函数用于数据的发送，其中传入参数Motor_PWM1和Motor_PWM2为左右电机翻转电平数量。然后通过serial库的write函数向下位机发送数据。

unsigned char Car_base::uart_send_cmd(uint16_t Motor_PWM1,uint16_t Motor_PWM2)
{
    send_cmd.cmd0.H1 = 0xFF;
	send_cmd.cmd0.H2 = 0xEE;
    send_cmd.cmd0.l_mode = l_mode;
    send_cmd.cmd0.r_mode = r_mode;
    send_cmd.cmd0.Servo_PWM_l = Motor_PWM1;
    send_cmd.cmd0.Servo_PWM_r = Motor_PWM2;
    send_cmd.cmd0.CS = check_uint(Motor_PWM1) + check_uint(Motor_PWM2); //和校验
    try
    {
        Stm32_Serial.write(send_cmd.data,sizeof(send_cmd.data)); //通过串口向下位机发送数据 
    }
    catch (serial::IOException& e)   
    {
        ROS_ERROR_STREAM("Unable to send data through serial port");  //如果发送数据失败，打印错误信息
    }
    return send_cmd.cmd0.CS;
}

其中传入参数Motor_PWM1和Motor_PWM2的值通过订阅到的速度消息得来。

void Car_base::controlLoopCB_send(const ros::TimerEvent&)
{
    double v = twist.linear.x;
    double w = twist.angular.z;
    double _r = v / w;
    double _vl = w * (_r - L/2.0);
    double _vr = w * (_r + L/2.0);
    if(_vl>=0) l_mode = 1;
    else l_mode = 0;
    if(_vr>=0) r_mode = 1;
    else r_mode = 0;
    uint16_t Motor_PWM1 = period_/20.0*_vl*(Encoder_pre/(2.0*Pi*wheel_radius))*20.0;
    uint16_t Motor_PWM2 = period_/20.0*_vr*(Encoder_pre/(2.0*Pi*wheel_radius))*20.0;
    uart_send_cmd(Motor_PWM1,Motor_PWM2); 
}

6.数据的接收

以该定时器的频率每隔一段时间接收一定字节，判断是否为包头包尾，如果是，标志位置1，反之置0，当标志位为1时，说明受到了正确的数据，便开始进行编码器数值的接收，然后进行结算，得到当前的左右轮速。

void Car_base::controlLoopCB_receive(const ros::TimerEvent&)
{
    ros::Time current_time = ros::Time::now();
	uint8_t buffer[1];
    len = Stm32_Serial.read(Receive_Data.receive,Receive_Num);	//获取长度
    if(Receive_Data.receive[0]==0xFF && Receive_Data.receive[1]==0xEE)
    {
        Serial_RxFlag=1;
    }
    else
    {
        len=Stm32_Serial.read(buffer,1);
        Serial_RxFlag=0;
    }
    if(Serial_RxFlag==1 && len==Receive_Num)
    {
        L_Encoder = (int(Receive_Data.receive[4])+int(Receive_Data.receive[5]*256)); 
        R_Encoder = (int(Receive_Data.receive[6])+int(Receive_Data.receive[7]*256)); 

        if(int(Receive_Data.receive[2])==1)  //l_mode为1是正转，反之反转  
        { 
            Vl = double(L_Encoder)/Encoder_pre*(2.0*Pi*wheel_radius)/1000.0*20.0;//m/s  Encoder_pre:编码器一圈脉冲  //20hz
        }
        else
        {
            Vl = -double(L_Encoder)/Encoder_pre*(2.0*Pi*wheel_radius)/1000.0*20.0;//m/s  Encoder_pre:编码器一圈脉冲  20hz
        }
        if(int(Receive_Data.receive[3])==1)  //r_mode为1是正转，反之反转  
        { 
            Vr =double(R_Encoder)/Encoder_pre*(2.0*Pi*wheel_radius)/1000.0*20.0;//m/s  Encoder_pre:编码器一圈脉冲  20hz 
        }
        else
        {
            Vr=-double(R_Encoder)/Encoder_pre*(2.0*Pi*wheel_radius)/1000.0*20.0;//m/s  Encoder_pre:编码器一圈脉冲  20hz
        }
    }
}

7.里程计的编写以及TF坐标变换的发布

下面就是里程计的代码实现啦，原理呢已经在上篇博客进行说明了，原理理解了，还是挺简单的，直接按照公式写就可以了，发布tf和里程计就相应的把变量列出来，然后一一赋值之后在发布就可以了。

void Car_base::timerOdomCB(const ros::TimerEvent&)
{
    //当前时间
    ros::Time current_time = ros::Time::now();
    //时间间隔
    double dt=(current_time-last_time).toSec();
    if( len==Receive_Num && Serial_RxFlag==1)
    {
        //计算d
        d = (Vr-Vl) * dt;
        d_ = 0.5 * (Vr + Vl) * dt;
        //计算转角
        delta_yaw = d / L;
        //pose
        if(delta_yaw == 0)
        {
            R = 0;
            pose_x = pose_x;
            pose_y = pose_y + d_;
        }
        else
        {
            R = d_ / delta_yaw;
            pose_x = pose_x + cos(pose_yaw) * R * (1-cos(delta_yaw)) - sin(pose_yaw) * R * sin(delta_yaw);
            pose_y = pose_y + sin(pose_yaw) * R * (1-cos(delta_yaw)) + cos(pose_yaw) * R * sin(delta_yaw);
        }
        
        //ROS_INFO("%lf,%lf,%lf,%lf",this->pose_x,pose_y,delta_yaw,R);
        
        pose_yaw = pose_yaw + delta_yaw;
        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose_yaw);
        static tf2_ros::TransformBroadcaster odom_broadcaster;
        geometry_msgs::TransformStamped odom_trans;
        //first, we'll publish the transform over tf
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = odom_ID;
        odom_trans.child_frame_id = base_ID;
        
        odom_trans.transform.translation.x = pose_x;
        odom_trans.transform.translation.y = pose_y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;
        //send the transform
        if(pub_tf)
            odom_broadcaster.sendTransform(odom_trans);
        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = odom_ID;
        odom.child_frame_id = base_ID;
        //set the position
        odom.pose.pose.position.x = pose_x;
        odom.pose.pose.position.y = pose_y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;
        //set the velocity
        odom.twist.twist.linear.x = 0.5 * (Vr + Vl);
        odom.twist.twist.linear.y = 0;
        odom.twist.twist.angular.z = steering;
        ROS_INFO("linear.x is %lf,%lf",0.5 * (Vr + Vl),steering);
        for(int i=0;i<36;i++)
        {
            odom.twist.covariance[i]=0;
        }
        odom.twist.covariance[0]=vx_cov;
        odom.twist.covariance[35]=steering_cov;
        odom.pose.covariance=odom.twist.covariance;
        odom_pub.publish(odom);
        last_time=current_time;
    } 
    else
    {
        ROS_ERROR("Error Value ,PLEASE CHECK!");
    }
}

把上述这些步骤都完成后，最终实现结果如下：

发布1m/s的线速度和1.5rad/s的角速度，里程计信息如上图，在rviz实时显示情况为，base_link做圆周运动。

这里我是直接用我电脑进行的演示，后面直接移植到旭日X3便可以了。

以上就是ROS上位机底层代码的简单实现流程，具体实现时需要根据通信协议和硬件平台进行相应的修改。同时，在实现过程中需要注意错误处理和异常情况的处理，保证代码的稳定性和可靠性。