# 通讯协议

### STM32端 调试串口通信 ax\_uart\_db.c

```c
/**			                                                    
		   ____                    _____ _____  _____        XTARK@塔克创新
		  / __ \                  / ____|  __ \|  __ \  
		 | |  | |_ __   ___ _ __ | |    | |__) | |__) |
		 | |  | | '_ \ / _ \ '_ \| |    |  _  /|  ___/ 
		 | |__| | |_) |  __/ | | | |____| | \ \| |     
		  \____/| .__/ \___|_| |_|\_____|_|  \_\_|     
		    		| |                                    
		    		|_|  OpenCRP 树莓派 专用ROS机器人控制器                                   
									 
  ****************************************************************************** 
  *           
  * 版权所有： XTARK@塔克创新  版权所有，盗版必究
  * 官网网站： www.xtark.cn
  * 淘宝店铺： https://shop246676508.taobao.com  
  * 塔克媒体： www.cnblogs.com/xtark（博客）
	* 塔克微信： 微信公众号：塔克创新（获取最新资讯）
  *      
  ******************************************************************************
  * @作  者  Musk Han@XTARK
  * @版  本  V1.0
  * @日  期  2019-7-26
  * @内  容  调试串口通信
  *
  ******************************************************************************
  * @说  明
  *
  * 1.USB串口通信，printf函数已定向到该串口，可以输出调试信息
  * 2.可以使用具有X-Protocol协议进行数据发送。
  * 3.开启UART的串口数据接收功能，使用中断方式，X-Protocol协议通信
	* 4.可通过AX_UART_DB_GetRxData()函数判断是否有数据接收
  *
  * X-Protocol协议介绍（变帧长）
  * 帧定义：AA 55 | 0B  | 01  | 03 E8 FC 18 00 0A | 14
  *        帧头   帧长   帧码  数据                校验和
  * 帧  头：双帧头，抗干扰强
  * 帧  长：根据数据长度设定
  * 帧  码：用户根据功能设定，标识帧的唯一性
  * 数  据：高位在前，长度可变，内容自由组合8位，16位，32位数据
  * 校验和：前面数据累加和的低8位
  * 帧示例：( AA 55 0B 01 03 E8 FC 18 00 0A 14 ) 内容：1000，-1000,10,
  * 
  ******************************************************************************
  */

#include "ax_uart_db.h"
#include <stdio.h>

static uint8_t uart_db_flag_rx_ok = 0; //接收成功标志
static uint8_t uart_db_rx_con=0;       //接收计数器
static uint8_t uart_db_rx_checksum;    //帧头部分校验和
static uint8_t uart_db_rx_buf[40];     //接收缓冲，数据内容小于等于32Byte
static uint8_t uart_db_tx_buf[40];     //发送缓冲

/**
  * @简  述  UART DB调试串口初始化
  * @参  数  baud： 波特率设置
  * @返回值	 无
  */
void AX_UART_DB_Init(uint32_t baud)
{
	GPIO_InitTypeDef GPIO_InitStructure;
  USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;
	
	//**调试串口USART配置******
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);  //打开串口GPIO的时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);  //打开串口外设的时钟
	
	//将USART Tx的GPIO配置为推挽复用模式
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	//将USART Rx的GPIO配置为浮空输入模式
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	//配置USART参数
	USART_InitStructure.USART_BaudRate = baud; //波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_Init(USART1, &USART_InitStructure);
	
	//配置USART为中断源
	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; //抢断优先级	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;	//子优先级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	//使能中断
	NVIC_Init(&NVIC_InitStructure);//初始化配置NVIC
	
	//使能串口接收中断
	USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
	
	//使能 USART， 配置完毕
	USART_Cmd(USART1, ENABLE);
}

/**
  * @简  述  DBUART 串口中断服务函数
  * @参  数  无 
  * @返回值  无
  */
void USART1_IRQHandler(void)
{
	uint8_t Res;
	
	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断
	{
		Res =USART_ReceiveData(USART1);	
		
		if(uart_db_rx_con < 3)    //==接收帧头 + 长度
		{
			if(uart_db_rx_con == 0)  //接收帧头1 0xAA
			{
				if(Res == 0xAA)
				{
					uart_db_rx_buf[0] = Res;
					uart_db_rx_con = 1;					
				}
				else
				{
					
				}
			}else if(uart_db_rx_con == 1) //接收帧头2 0x55
			{
				if(Res == 0x55)
				{
					uart_db_rx_buf[1] = Res;
					uart_db_rx_con = 2;
				}
				else
				{
					uart_db_rx_con = 0;						
				}				
			}
			else  //接收数据长度
			{
				uart_db_rx_buf[2] = Res;
				uart_db_rx_con = 3;
				uart_db_rx_checksum = (0xAA+0x55) + Res;	//计算校验和
			}
		}
		else    //==接收数据
		{
			if(uart_db_rx_con < (uart_db_rx_buf[2]-1) )
			{
				uart_db_rx_buf[uart_db_rx_con] = Res;
				uart_db_rx_con++;
				uart_db_rx_checksum = uart_db_rx_checksum + Res;					
			}
			else    //判断最后1位
			{
				//数据校验
				if( Res == uart_db_rx_checksum )  //校验正确
				{	
					//此处进行数据解析
					uart_db_flag_rx_ok = 1;
					
					//接收完成，恢复初始状态
					uart_db_rx_con = 0;					
				}
			}
		}
		
    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
	} 
}

/**
  * @简  述  获取接收的数据
  * @参  数  *pbuf：接收数据指针,第1个字节为帧编码，后面为数据
  * @返回值	 0-无数据接收，other-需要读取的数据字节个数
  */
uint8_t AX_UART_DB_GetData(uint8_t *pbuf)
{
	uint8_t cnt,i;
	
	if(uart_db_flag_rx_ok != 0)
	{
		cnt = uart_db_rx_buf[2]-4;
		
		for(i=0; i<cnt; i++)
		{
			*(pbuf+i) = uart_db_rx_buf[3+i];
		}
		
		uart_db_flag_rx_ok = 0;
		return cnt;
	}
	else
	{
		return 0;
	}	
}

/**
  * @简  述  UART 发送数据（X-Protocol协议）
  * @参  数  *pbuf：发送数据指针
  *          len：发送数据长度个数，≤27 (32-5)
  *          num：帧号，帧编码
  * @返回值	 无
  */
void AX_UART_DB_SendPacket(uint8_t *pbuf, uint8_t len, uint8_t num)
{
	uint8_t i,cnt;	
  uint8_t tx_checksum = 0;//发送校验和
	
	if(len <= 32)
	{
		/******获取数据******/
		uart_db_tx_buf[0] = 0xAA;    //帧头
		uart_db_tx_buf[1] = 0x55;    //
		uart_db_tx_buf[2] = len+5;  //根据输出长度计算帧长度
		uart_db_tx_buf[3] = num;    //帧编码
		
		for(i=0; i<len; i++)
		{
			uart_db_tx_buf[4+i] = *(pbuf+i);
		}
		
		/******计算校验和******/	
		cnt = 4+len;
		for(i=0; i<cnt; i++)
		{
			tx_checksum = tx_checksum + uart_db_tx_buf[i];
		}
		uart_db_tx_buf[i] = tx_checksum;
		
		
		/******发送数据******/	
		cnt = 5+len;
		
		//查询传输方式
		for(i=0; i<cnt; i++)
		{
			USART_SendData(USART1, uart_db_tx_buf[i]);
			while(USART_GetFlagStatus(USART1,USART_FLAG_TC) != SET);
		}
	}
}

/**************************串口打印相关函数重定义********************************/
/**
  * @简  述  重定义putc函数（USART1）	
  */
int fputc(int ch, FILE *f)
{
	/* Place your implementation of fputc here */
	/* e.g. write a character to the USART */
	USART_SendData(USART1, (uint8_t) ch);

	/* Loop until the end of transmission */
	while (USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET)
	{}

	return ch;
}

/**
  * @简  述  重定义getc函数（USART1）	
  */
int fgetc(FILE *f)
{
	/* 等待串口1输入数据 */
	while (USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET)
	{}

	return (int)USART_ReceiveData(USART1);
}

/******************* (C) 版权 2019 XTARK **************************************/

```

### STM32端 串口通信ax\_uart\_pi.c

```c
/**			                                                    
		   ____                    _____ _____  _____        XTARK@塔克创新
		  / __ \                  / ____|  __ \|  __ \  
		 | |  | |_ __   ___ _ __ | |    | |__) | |__) |
		 | |  | | '_ \ / _ \ '_ \| |    |  _  /|  ___/ 
		 | |__| | |_) |  __/ | | | |____| | \ \| |     
		  \____/| .__/ \___|_| |_|\_____|_|  \_\_|     
		    		| |                                    
		    		|_|  OpenCRP 树莓派 专用ROS机器人控制器                                   
									 
  ******************************************************************************  
  *           
  * 版权所有： XTARK@塔克创新  版权所有，盗版必究
  * 官网网站： www.xtark.cn
  * 淘宝店铺： https://shop246676508.taobao.com  
  * 塔克媒体： www.cnblogs.com/xtark（博客）
	* 塔克微信： 微信公众号：塔克创新（获取最新资讯）
  *      
  ******************************************************************************
  * @作  者  Musk Han@XTARK
  * @版  本  V1.0
  * @日  期  2019-7-26
  * @内  容  串口通信
  *
  ******************************************************************************
  * @说  明
  *
  * 1.树莓派通信串口
  * 2.可以使用具有X-Protocol协议进行数据发送
  * 3.开启UART的串口数据接收功能，使用中断方式，X-Protocol协议通信
	* 4.可通过AX_UART_XX_GetRxData()函数判断是否有数据接收
  *
  * X-Protocol协议介绍（变帧长）
  * 帧定义：AA 55 | 0B  | 01  | 03 E8 FC 18 00 0A | 14
  *        帧头   帧长   帧码  数据                校验和
  * 帧  头：双帧头，抗干扰强
  * 帧  长：根据数据长度设定
  * 帧  码：用户根据功能设定，标识帧的唯一性
  * 数  据：高位在前，长度可变，内容自由组合8位，16位，32位数据
  * 校验和：前面数据累加和的低8位
  * 帧示例：( AA 55 0B 01 03 E8 FC 18 00 0A 14 ) 内容：1000，-1000,10,
  * 
  ******************************************************************************
  */

#include "ax_uart_pi.h"
#include <stdio.h>
#include "ax_kinematics.h"

static uint8_t uart2_rx_con=0;  //接收计数器
static uint8_t uart2_rx_checksum; //帧头部分校验和
static uint8_t uart2_rx_buf[60];  //接收缓冲，数据内容小于等于32Byte
static uint8_t uart2_tx_buf[60];  //发送缓冲

//外部变量
extern int16_t ax_motor_kp;
extern int16_t ax_motor_ki;
extern int16_t ax_motor_kd;

extern int16_t robot_target_speed[3];  // X Y Yaw
extern int16_t robot_params[2];


/**
  * @简  述  串口初始化
  * @参  数  baud： 波特率设置
  * @返回值	 无
  */
void AX_UART_PI_Init(uint32_t baud)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	//**USART配置******
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);  //打开串口GPIO的时钟

	//将USART Tx的GPIO配置为推挽复用模式
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	//将USART Rx的GPIO配置为浮空输入模式
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
	GPIO_Init(GPIOA, &GPIO_InitStructure);

	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);  //打开串口外设的时钟

	//配置USART参数
	USART_InitStructure.USART_BaudRate = baud;		//波特率
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_Init(USART2, &USART_InitStructure);

	//配置USART为中断源
	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2; //抢断优先级	
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;	//子优先级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;	//使能中断
	NVIC_Init(&NVIC_InitStructure);//初始化配置NVIC

	//使能串口接收中断
	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);

	//使能 USART， 配置完毕
	USART_Cmd(USART2, ENABLE);
}



/**
  * @简  述  串口中断服务程序
  * @参  数  无
  * @返回值  无
  */
void USART2_IRQHandler(void)                	
{
	uint8_t Res;
	
	if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)  //接收中断
	{
		  //printf("Get Data!\r\n");
			Res =USART_ReceiveData(USART2);	
		
			if(uart2_rx_con < 3)    //==接收帧头 + 长度
			{
				if(uart2_rx_con == 0)  //接收帧头1 0xAA
				{
					if(Res == 0xAA)
					{
						uart2_rx_buf[0] = Res;
						uart2_rx_con = 1;					
					}
					else
					{
						
					}
				}else if(uart2_rx_con == 1) //接收帧头2 0x55
				{
					if(Res == 0x55)
					{
						uart2_rx_buf[1] = Res;
						uart2_rx_con = 2;
					}
					else
					{
						uart2_rx_con = 0;						
					}				
				}
				else  //接收数据长度
				{
					uart2_rx_buf[2] = Res;
					uart2_rx_con = 3;
					uart2_rx_checksum = (0xAA+0x55) + Res;	//计算校验和
				}
			}
			else    //==接收数据
			{
				if(uart2_rx_con < (uart2_rx_buf[2]-1) )
				{
					uart2_rx_buf[uart2_rx_con] = Res;
					uart2_rx_con++;
					uart2_rx_checksum = uart2_rx_checksum + Res;					
				}
				else    //判断最后1位
				{
					//数据校验
					if( Res == uart2_rx_checksum )  //校验正确
					{	
						//=====此处进行数据解析=========
						//运动控制帧
						if(uart2_rx_buf[3] == 0x11)
						{
							robot_target_speed[0] = (int16_t)((uart2_rx_buf[4]<<8) | uart2_rx_buf[5]);
							robot_target_speed[1] = (int16_t)((uart2_rx_buf[6]<<8) | uart2_rx_buf[7]);
							robot_target_speed[2] = (int16_t)((uart2_rx_buf[8]<<8) | uart2_rx_buf[9]);
						  
							//速度限制
							if(robot_target_speed[0] > ROBOT_LINEAR_SPEED_LIMIT)    robot_target_speed[0] = ROBOT_LINEAR_SPEED_LIMIT;
							if(robot_target_speed[0] < (-ROBOT_LINEAR_SPEED_LIMIT)) robot_target_speed[0] = (-ROBOT_LINEAR_SPEED_LIMIT);
							if(robot_target_speed[1] > ROBOT_LINEAR_SPEED_LIMIT)    robot_target_speed[1] = ROBOT_LINEAR_SPEED_LIMIT;
							if(robot_target_speed[1] < (-ROBOT_LINEAR_SPEED_LIMIT)) robot_target_speed[1] = (-ROBOT_LINEAR_SPEED_LIMIT);
							if(robot_target_speed[2] > ROBOT_ANGULAR_SPEED_LIMIT)    robot_target_speed[2] = ROBOT_ANGULAR_SPEED_LIMIT;
							if(robot_target_speed[2] < (-ROBOT_ANGULAR_SPEED_LIMIT)) robot_target_speed[2] = (-ROBOT_ANGULAR_SPEED_LIMIT);
						}
						else
						{
							//PID参数帧
							if(uart2_rx_buf[3] == 0x12)
							{
								ax_motor_kp = (int16_t)((uart2_rx_buf[4]<<8) | uart2_rx_buf[5]);
								ax_motor_ki = (int16_t)((uart2_rx_buf[6]<<8) | uart2_rx_buf[7]);
								ax_motor_kd = (int16_t)((uart2_rx_buf[8]<<8) | uart2_rx_buf[9]);
							}
							
							//机器人参数
							if(uart2_rx_buf[3] == 0x13)
							{
								robot_params[0] = (int16_t)((uart2_rx_buf[4]<<8) | uart2_rx_buf[5]);
								robot_params[1] = (int16_t)((uart2_rx_buf[6]<<8) | uart2_rx_buf[7]);
								
								AX_Kinematics_Init(robot_params);
							}
						}
						
						//接收完成，恢复初始状态
						uart2_rx_con = 0;					
					}	
					
				}
			}
			
      USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
	} 
}

/**
  * @简  述  发送数据（X-Protocol协议）
  * @参  数  *pbuf：发送数据指针
  *          len：发送数据长度个数，≤27 (32-5)
  *          num：帧号，帧编码
  * @返回值	 无
  */
void AX_UART_PI_SendPacket(uint8_t *pbuf, uint8_t len, uint8_t num)
{
	uint8_t i,cnt;	
  uint8_t tx_checksum = 0;//发送校验和
	
	if(len <= 50)
	{
		/******获取数据******/
		uart2_tx_buf[0] = 0xAA;    //帧头
		uart2_tx_buf[1] = 0x55;    //
		uart2_tx_buf[2] = len+5;  //根据输出长度计算帧长度
		uart2_tx_buf[3] = num;    //帧编码
		
		for(i=0; i<len; i++)
		{
			uart2_tx_buf[4+i] = *(pbuf+i);
		}
		
		/******计算校验和******/	
		cnt = 4+len;
		for(i=0; i<cnt; i++)
		{
			tx_checksum = tx_checksum + uart2_tx_buf[i];
		}
		uart2_tx_buf[i] = tx_checksum;
		
		
		/******发送数据******/	
		cnt = 5+len;
		
		//查询传输方式
		for(i=0; i<cnt; i++)
		{
			USART_SendData(USART2, uart2_tx_buf[i]);
			while(USART_GetFlagStatus(USART2,USART_FLAG_TC) != SET);
		}
	}
}

/******************* (C) 版权 2018 XTARK **************************************/

```

### ROS端

```c
#include "../include/xtark_driver/xtark_driver.h"

XtarkDriver::XtarkDriver():msg_seq_(0),start_flag_(true),state_(waitingForHead1),first_init_(false){}
XtarkDriver::~XtarkDriver()
{
    boost::mutex::scoped_lock look(mutex_);
    recv_flag_ = false;
    if(sp_)
    {
        sp_->cancel();
        sp_->close();
        sp_.reset();
    }
    io_service_.stop();
    io_service_.reset();
}
/*动态参数配置服务回调函数*/
void XtarkDriver::dynamic_reconfig_callback(xtark_driver::PID_reconfigConfig &config,uint32_t level)
{
   if(first_init_)
   {
	    ROS_INFO("Set PID P:[%d], I:[%d], D:[%d]",config.Kp,config.Ki,config.Kd);
        SetPID(config.Kp,config.Ki,config.Kd);
        ros::Duration(0.02).sleep();
   }
    else
    {
	    first_init_=true;
	    ROS_INFO("Set PID P:[%d], I:[%d], D:[%d]",kp_,ki_,kd_);
    }   
}
/*主循环函数*/
void XtarkDriver::loop()
{
    uint8_t stop_buf[13];
    ros::NodeHandle nh_;
    ros::NodeHandle nh_p_("~");//私有句炳，用于获得节点下的私有参数

    /*从launch文件中配置的参数以及加载的参数文件中获取机器人参数，没有获得的话，使用最后面的默认参数*/
    nh_p_.param<std::string>("port_name",port_name_,std::string("/dev/ttyUSB0"));
    nh_p_.param<std::string>("odom_frame",odom_frame_,std::string("odom"));
    nh_p_.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    nh_p_.param<std::string>("imu_frame",imu_frame_,std::string("base_imu_link"));

    nh_p_.param<int>("baud_rate",baud_rate_,115200);
    nh_p_.param<int>("control_rate",control_rate_,50);
    
    nh_p_.param<double>("linear_correction_factor",linear_correction_factor_,1.0);
    nh_p_.param<double>("angular_correction_factor",angular_correction_factor_,1.0);

    nh_p_.param<bool>("publish_odom_transform",publish_odom_transform_,true);
    nh_p_.param<int>("Kp",kp_,300);
    nh_p_.param<int>("Ki",ki_,0);
    nh_p_.param<int>("Kd",kd_,200);
    

    odom_list_.resize(6,0.0);
    imu_list_.resize(9,0.0);
    wheelspeedSet_list_.resize(4,0);
    wheelspeedGet_list_.resize(4,0);
    /*初始化机器人硬件串口以及publisher发布器和定时器与动态配置参数服务器等的回调函数，然后进入ros::spin()循环*/
    if(initRobot())
    {
        //声明话题的发布器
        odom_pub_    = nh_.advertise<nav_msgs::Odometry>("odom",10);
        battery_pub_ = nh_.advertise<std_msgs::Float32>("voltage",1);
        imu_pub_     = nh_.advertise<sensor_msgs::Imu>("imu",10);
        avel_pub_    = nh_.advertise<std_msgs::Int32>("xtark/avel",10);
        bvel_pub_    = nh_.advertise<std_msgs::Int32>("xtark/bvel",10);
        cvel_pub_    = nh_.advertise<std_msgs::Int32>("xtark/cvel",10);
        dvel_pub_    = nh_.advertise<std_msgs::Int32>("xtark/dvel",10);

        aset_pub_    = nh_.advertise<std_msgs::Int32>("xtark/aset",10);
        bset_pub_    = nh_.advertise<std_msgs::Int32>("xtark/bset",10);
        cset_pub_    = nh_.advertise<std_msgs::Int32>("xtark/cset",10);
        dset_pub_    = nh_.advertise<std_msgs::Int32>("xtark/dset",10);

        //订阅的速度话题  用于发送给stm32让stm32控制底盘运动 this是C++下this指针，表明调用的是该类下的XtarkDriver::cmd_vel_callback作为毁掉汉书
        cmd_sub_     = nh_.subscribe<geometry_msgs::Twist>("cmd_vel",10,&XtarkDriver::cmd_vel_callback,this);
        
        SetParams(linear_correction_factor_,angular_correction_factor_);
        ros::Duration(0.02).sleep();
        SetPID(kp_,ki_,kd_);

        //实例化一个定时器 用于周期式向32发布速度指令
        ros::Timer send_speed_timer = nh_.createTimer(ros::Duration(1.0/control_rate_),&XtarkDriver::send_speed_callback,this);

        //线程库 实例化一个线程  多线程实现与OpenCRP串口通信接收数据
        boost::thread recv_thread(boost::bind(&XtarkDriver::recv_msg,this));

        //用于动态调整pid
	    dynamic_reconfigure::Server<xtark_driver::PID_reconfigConfig> reconfig_server;
	    dynamic_reconfigure::Server<xtark_driver::PID_reconfigConfig>::CallbackType f;
	    f = boost::bind(&XtarkDriver::dynamic_reconfig_callback,this,_1,_2);
	    reconfig_server.setCallback(f);

        ROS_INFO("Robot Running!");
        ros::spin();
    	SetVelocity(0,0,0);//向底盘发送速度 置0 小车停止
        return ;
    }
}
/*初始化硬件端口*/
bool XtarkDriver::initRobot()
{
    if(sp_)
    {
        ROS_ERROR("The SerialPort is already opened!");
        return false;
    }
     sp_ = serialp_ptr(new boost::asio::serial_port(io_service_));
     sp_->open(port_name_,ec_);
     if(ec_)
     {
        ROS_ERROR_STREAM("error : port_->open() failed...port_name=" << port_name_ << ", e=" << ec_.message().c_str());
        return false;
     }
     //设置串口参数
    sp_->set_option(boost::asio::serial_port_base::baud_rate(baud_rate_));
    sp_->set_option(boost::asio::serial_port_base::character_size(8));
    sp_->set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one));
    sp_->set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none));
    sp_->set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none));
    
    return true;
}
/*将收到的/cmd_vel上的速度话题消息通过串口发送给机器人*/
void XtarkDriver::send_speed_callback(const ros::TimerEvent&)
{
    double linear_x_speed, linear_y_speed,angular_speed;
    if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    {
            linear_x_speed = current_twist_.linear.x;
            linear_y_speed = current_twist_.linear.y;
            angular_speed = current_twist_.angular.z;
    }
    else
    {
            linear_x_speed  = 0;
            linear_y_speed  = 0;
            angular_speed = 0;
    }

    if((ros::Time::now() - now_).toSec() >=1)
    {
        ROS_WARN_THROTTLE(1,"Didn't received odom data,Please check your connection!");
    }
    //向底盘发送速度
    SetVelocity(linear_x_speed,linear_y_speed,angular_speed);
}
/*cmd_vel Subscriber的回调函数*/
void XtarkDriver::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();//将接受的消息存入类下的属性变量current_twist_，等待send_speed_callback函数调用（该函数通过定时器定时发生）向32发送
        cmd_vel_mutex_.unlock();
    }
    catch(...)
    {
        cmd_vel_mutex_.unlock();
    }
}
/*串口通信校验位计算辅助函数*/
void XtarkDriver::check_sum(uint8_t* data, size_t len, uint8_t& dest)
{
    dest = 0x00;
    for(int i=0;i<len;i++)
    {
        dest += *(data + i);
    }
}
/*与OpenCRP串口通信接收数据线程*/
void XtarkDriver::recv_msg()
{
    uint8_t payload_size, check_num, buffer_data[255],payload_type;
    state_ = waitingForHead1;
    recv_flag_ = true;
    while(recv_flag_)
    {
        switch (state_)
        {
            case waitingForHead1:
                check_num = 0x00;
                boost::asio::read(*sp_.get(), boost::asio::buffer(&buffer_data[0], 1), ec_);
                state_ = buffer_data[0] == head1 ? waitingForHead2 : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_DEBUG_STREAM("recv head1 error : ->"<<(int)buffer_data[0]);
                }
                break;
            case waitingForHead2:
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[1],1),ec_);
                state_ = buffer_data[1] == head2 ? waitingForPayloadSize : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_DEBUG_STREAM("recv head2 error : ->"<<(int)buffer_data[1]);
                }
                break;
            case waitingForPayloadSize:
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[2],1),ec_);
                payload_size = buffer_data[2] - 4;
                state_ = waitingForPayload;
                break;
            case waitingForPayload:
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[3],payload_size),ec_);
                payload_type = buffer_data[3];
                state_ = waitingForCheckSum;
                break;
            case waitingForCheckSum:
                boost::asio::read(*sp_.get(),boost::asio::buffer(&buffer_data[3+payload_size],1),ec_);
                check_sum(buffer_data,3+payload_size,check_num);
                state_ = buffer_data[3+payload_size] == check_num ? handlePayload : waitingForHead1;
                if(state_ == waitingForHead1)
                {
                    ROS_DEBUG_STREAM("check sum error! recv is  : ->"<<(int)buffer_data[3+payload_size]<<"  calc is "<<check_num);
                }
                break;
            case handlePayload:
                distribute_data(payload_type, buffer_data);
                state_ = waitingForHead1;
                break;
            default:
                state_ = waitingForHead1;
                break;
        }
    }
}
/*数据校验分发*/
void XtarkDriver::distribute_data(uint8_t msg_type, uint8_t* buffer_data)
{
     if(msg_type == foundType_Packages)
        handle_base_data(buffer_data);//从32接受的数据进行处理，与话题发布
}
/*PID参数发送函数*/
void XtarkDriver::SetPID(int p,int i, int d)
{
    static uint8_t pid_data[11];
    pid_data[0] = head1;
    pid_data[1] = head2;
    pid_data[2] = 0x0b;
    pid_data[3] = sendType_pid;
    pid_data[4] = (p>>8)&0xff;
    pid_data[5] = p&0xff;
    pid_data[6] = (i>>8)&0xff;
    pid_data[7] = i&0xff;
    pid_data[8] = (d>>8)&0xff;
    pid_data[9] = d&0xff;
    check_sum(pid_data,10,pid_data[10]);
    ROS_INFO("Send Time: %f",ros::Time::now().toSec());
    boost::asio::write(*sp_.get(),boost::asio::buffer(pid_data,11),ec_);
    ROS_INFO("Send End Time: %f",ros::Time::now().toSec());
}
/*底盘参数（线速度校准系数，角速度校准系数）发送函数*/
void XtarkDriver::SetParams(double linear_correction,double angular_correction) {
    printf("%f %f \r\n",linear_correction,angular_correction);
    static uint8_t param_data[20];
    param_data[0]  = head1;
    param_data[1]  = head2;
    param_data[2]  = 0x09;
    param_data[3]  = sendType_params;
    param_data[4]  = (int16_t)((int16_t)(linear_correction*1000)>>8)   &0xff;
    param_data[5]  = (int16_t)(linear_correction*1000)        &0xff;
    param_data[6]  = (int16_t)((int16_t)(angular_correction*1000)>>8)      &0xff;
    param_data[7]  = (int16_t)(angular_correction*1000)                    &0xff;
    check_sum(param_data,8,param_data[8]);
    boost::asio::write(*sp_.get(),boost::asio::buffer(param_data,9),ec_);
}
/*底盘速度发送函数*/
void XtarkDriver::SetVelocity(double x, double y, double yaw)
{
    static uint8_t vel_data[11];
    vel_data[0] = head1;
    vel_data[1] = head2;
    vel_data[2] = 0x0b;
    vel_data[3] = sendType_velocity;
    vel_data[4] = ((int16_t)(x*1000)>>8) & 0xff;
    vel_data[5] = ((int16_t)(x*1000)) & 0xff;
    vel_data[6] = ((int16_t)(y*1000)>>8) & 0xff;
    vel_data[7] = ((int16_t)(y*1000)) & 0xff;
    vel_data[8] = ((int16_t)(yaw*1000)>>8) & 0xff;
    vel_data[9] = ((int16_t)(yaw*1000)) & 0xff;
    //计算串口校验位
    check_sum(vel_data,10,vel_data[10]);
    //通过串口发生速度数据
    boost::asio::write(*sp_.get(),boost::asio::buffer(vel_data,11),ec_);
}
/*收到串口数据包解析函数*/
void XtarkDriver::handle_base_data(const uint8_t* buffer_data)
{
        now_ = ros::Time::now();
        //imu数据组织发布
        //gyro
        imu_list_[0]=((double)((int16_t)(buffer_data[4]*256+buffer_data[5]))/32768*2000/180*3.1415);
        imu_list_[1]=((double)((int16_t)(buffer_data[6]*256+buffer_data[7]))/32768*2000/180*3.1415);
        imu_list_[2]=((double)((int16_t)(buffer_data[8]*256+buffer_data[9]))/32768*2000/180*3.1415);
        //Acc 
        imu_list_[3]=((double)((int16_t)(buffer_data[10]*256+buffer_data[11]))/32768*2*9.8);
        imu_list_[4]=((double)((int16_t)(buffer_data[12]*256+buffer_data[13]))/32768*2*9.8);
        imu_list_[5]=((double)((int16_t)(buffer_data[14]*256+buffer_data[15]))/32768*2*9.8);
        //Angle 
        imu_list_[6]=((double)((int16_t)(buffer_data[16]*256+buffer_data[17]))/100);
        imu_list_[7]=((double)((int16_t)(buffer_data[18]*256+buffer_data[19]))/100);
        imu_list_[8]=((double)((int16_t)(buffer_data[20]*256+buffer_data[21]))/100);
        //欧拉角转四元数
        imu_pub_data_.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,(imu_list_[8]/180*3.1415926));
        imu_pub_data_.header.stamp = ros::Time::now();
        imu_pub_data_.header.frame_id = imu_frame_;
        //角速度
        imu_pub_data_.angular_velocity.x = imu_list_[0];
        imu_pub_data_.angular_velocity.y = imu_list_[1];
        imu_pub_data_.angular_velocity.z = imu_list_[2];
        //加速度
        imu_pub_data_.linear_acceleration.x = imu_list_[3];
        imu_pub_data_.linear_acceleration.y = imu_list_[4];
        imu_pub_data_.linear_acceleration.z = imu_list_[5];
        imu_pub_data_.orientation_covariance = {1e6, 0, 0,
                                            0, 1e6, 0,
                                            0, 0, 0.05};
        imu_pub_data_.angular_velocity_covariance = {1e6, 0, 0,
                                                 0, 1e6, 0,
                                                 0, 0, 1e6};
        imu_pub_data_.linear_acceleration_covariance = {1e-2, 0, 0,
                                                     0, 0, 0,
                                                     0, 0, 0};
       	//发布imu数据
        imu_pub_.publish(imu_pub_data_);


        //发布base_frame_到odom_frame_的坐标变换
        odom_list_[0]=((double)((int16_t)(buffer_data[22]*256+buffer_data[23]))/1000);
        odom_list_[1]=((double)((int16_t)(buffer_data[24]*256+buffer_data[25]))/1000);
        odom_list_[2]=((double)((int16_t)(buffer_data[26]*256+buffer_data[27]))/1000);
        //dx dy dyaw base_frame
        odom_list_[3]=((double)((int16_t)(buffer_data[28]*256+buffer_data[29]))/1000);
        odom_list_[4]=((double)((int16_t)(buffer_data[30]*256+buffer_data[31]))/1000);
        odom_list_[5]=((double)((int16_t)(buffer_data[32]*256+buffer_data[33]))/1000);

        transformStamped_.header.stamp    = now_;
        transformStamped_.header.frame_id = odom_frame_;
        transformStamped_.child_frame_id  = base_frame_;
        //xyz偏移（小车在odom_frame_的坐标位置）
        transformStamped_.transform.translation.x = odom_list_[0];
        transformStamped_.transform.translation.y = odom_list_[1];
        transformStamped_.transform.translation.z = 0.0;
        tf2::Quaternion q;
        //欧拉角转四元素
        q.setRPY(0,0,odom_list_[2]);
        transformStamped_.transform.rotation.x = q.x();
        transformStamped_.transform.rotation.y = q.y();
        transformStamped_.transform.rotation.z = q.z();
        transformStamped_.transform.rotation.w = q.w();
        //如果在xtark_params.yaml中将publish_odom_transform_设置为True则发布坐标变换
        if(publish_odom_transform_)
            //调用该类下的属性发布坐标变化
            br_.sendTransform(transformStamped_);

        //组织发布里程计数据 发布odom话题
        odom_.header.frame_id = odom_frame_;
        odom_.child_frame_id  = base_frame_;
        odom_.header.stamp    = now_;
        odom_.pose.pose.position.x = odom_list_[0];
        odom_.pose.pose.position.y = odom_list_[1];
        odom_.pose.pose.position.z = 0;
        odom_.pose.pose.orientation.x = q.getX();
        odom_.pose.pose.orientation.y = q.getY();
        odom_.pose.pose.orientation.z = q.getZ();
        odom_.pose.pose.orientation.w = q.getW();
        odom_.twist.twist.linear.x = odom_list_[3]/((now_-last_time_).toSec());
        odom_.twist.twist.linear.y = odom_list_[4]/((now_-last_time_).toSec());
        odom_.twist.twist.angular.z = odom_list_[5]/((now_-last_time_).toSec());
        odom_.twist.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 0.1 };
        odom_.pose.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e3 };
        odom_pub_.publish(odom_);
        last_time_ = now_;

        wheelspeedGet_list_[0]=((int16_t)(buffer_data[34]*256+buffer_data[35]));
        wheelspeedGet_list_[1]=((int16_t)(buffer_data[36]*256+buffer_data[37]));
        wheelspeedGet_list_[2]=((int16_t)(buffer_data[38]*256+buffer_data[39]));
        wheelspeedGet_list_[3]=((int16_t)(buffer_data[40]*256+buffer_data[41]));
        
        wheelspeedSet_list_[0]=((int16_t)(buffer_data[42]*256+buffer_data[43]));
        wheelspeedSet_list_[1]=((int16_t)(buffer_data[44]*256+buffer_data[45]));
        wheelspeedSet_list_[2]=((int16_t)(buffer_data[46]*256+buffer_data[47]));
        wheelspeedSet_list_[3]=((int16_t)(buffer_data[48]*256+buffer_data[49]));
       
        avel_pub_data_.data = wheelspeedGet_list_[0];
        bvel_pub_data_.data = wheelspeedGet_list_[1];
        cvel_pub_data_.data = wheelspeedGet_list_[2];
        dvel_pub_data_.data = wheelspeedGet_list_[3];

        aset_pub_data_.data = wheelspeedSet_list_[0];
        bset_pub_data_.data = wheelspeedSet_list_[1];
        cset_pub_data_.data = wheelspeedSet_list_[2];
        dset_pub_data_.data = wheelspeedSet_list_[3];

        avel_pub_.publish(avel_pub_data_);
        bvel_pub_.publish(bvel_pub_data_);
        cvel_pub_.publish(cvel_pub_data_);
        dvel_pub_.publish(dvel_pub_data_);

        aset_pub_.publish(aset_pub_data_);
        bset_pub_.publish(bset_pub_data_);
        cset_pub_.publish(cset_pub_data_);
        dset_pub_.publish(dset_pub_data_);

        battery_pub_data_.data = (double)(((buffer_data[50]<<8)+buffer_data[51]))/100;
        battery_pub_.publish(battery_pub_data_);
}
int main(int argc,char** argv)
{
    ros::init(argc,argv,"Xtark_driver_node");
    XtarkDriver driver;
    driver.loop();
    return 0;
}

```

### ROS端.h文件

```c
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int16.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/tf.h>
#include <dynamic_reconfigure/server.h>
#include <xtark_driver/PID_reconfigConfig.h>


#include <string>
#include <vector>
#include <math.h>

#include <boost/asio.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/system/error_code.hpp>
#include <boost/system/system_error.hpp>
#include <boost/bind.hpp>
#include <boost/thread.hpp>

#define G     9.8
#define head1 0xAA  //32发送的串口的数据帧头
#define head2 0x55
#define sendType_velocity    0x11//树莓派向32发生数据包的标识符
#define sendType_pid         0x12
#define sendType_params      0x13
#define sendType_wheelspeed  0x14

#define foundType_Packages    0x06//接受32数据包的标识符

//#define MAX_STEERING_ANGLE    0.87
//#define M_PI 3.1415926535

//枚举类型，用于串口状态机
enum packetFinderState
{
    waitingForHead1,
    waitingForHead2,
    waitingForPayloadSize,
    waitingForPayloadType,
    waitingForPayload,
    waitingForCheckSum,
    handlePayload
};



struct pid_param
{
    int kp;
    int ki;
    int kd;
};

struct imu_data
{
    float angle_x;
    float angle_y;
    float angle_z;
    float gyro_x;
    float gyro_y;
    float gyro_z;
    float accel_x;
    float accel_y;
    float accel_z;
    float q0;
    float q1;
    float q2;
    float q3;
};

typedef boost::shared_ptr<boost::asio::serial_port> serialp_ptr;

class XtarkDriver
{
    public:
        XtarkDriver();
        ~XtarkDriver();
        void loop();
    
    private:
        bool initRobot();

        void recv_msg();

        void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg);
        void send_speed_callback(const ros::TimerEvent&);
        void dynamic_reconfig_callback(xtark_driver::PID_reconfigConfig &config, uint32_t level);

        void handle_base_data(const uint8_t* buffer_data);
        void SetPID(int p,int i, int d);
        void SetParams(double linear_correction,double angular_correction);
        void SetVelocity(double x, double y, double yaw);

        void check_sum(uint8_t* data, size_t len, uint8_t& dest);
        void distribute_data(uint8_t msg_type, uint8_t* buffer_data);
        void upload_pid_param();

        packetFinderState state_;

        std_msgs::Float32  battery_pub_data_;

        boost::mutex cmd_vel_mutex_;
        boost::system::error_code ec_;
        boost::asio::io_service io_service_;
        boost::mutex mutex_;
        serialp_ptr sp_;

        bool recv_flag_;
        bool publish_odom_transform_;
        bool start_flag_;
        bool first_init_;
        uint8_t msg_seq_;

        geometry_msgs::Twist current_twist_;
        nav_msgs::Odometry odom_;
        geometry_msgs::TransformStamped transformStamped_;
        tf2_ros::TransformBroadcaster br_;

        ros::Time now_;
        ros::Time last_time_;
        ros::Time last_twist_time_;

        ros::Publisher odom_pub_;
        ros::Publisher battery_pub_;
        ros::Publisher imu_pub_;

        ros::Publisher avel_pub_;
        ros::Publisher bvel_pub_;
        ros::Publisher cvel_pub_;
        ros::Publisher dvel_pub_;

        ros::Publisher aset_pub_;
        ros::Publisher bset_pub_;
        ros::Publisher cset_pub_;
        ros::Publisher dset_pub_;

        std_msgs::Int32 avel_pub_data_;
        std_msgs::Int32 bvel_pub_data_;
        std_msgs::Int32 cvel_pub_data_;
        std_msgs::Int32 dvel_pub_data_;
       
        std_msgs::Int32 aset_pub_data_;
        std_msgs::Int32 bset_pub_data_;
        std_msgs::Int32 cset_pub_data_;
        std_msgs::Int32 dset_pub_data_;
        sensor_msgs::Imu imu_pub_data_;

        ros::Subscriber cmd_sub_;

        std::string port_name_;
        
        int baud_rate_;

        std::string odom_frame_;
        std::string imu_frame_;
        std::string base_frame_;
        std::string code_version_;
        int control_rate_;

        std::vector<double> imu_list_;
        std::vector<double> odom_list_;
        std::vector<int>    wheelspeedSet_list_;
        std::vector<int>    wheelspeedGet_list_;


        double wheel_track_;
        double wheel_diameter_;

	    double linear_correction_factor_;
	    double angular_correction_factor_;
        
        int kp_;
        int ki_;
        int kd_;

};



```
