# 通讯协议

### 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_;

};



```


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://oceanaparts.gitbook.io/halnote/tong-xun-xie-yi.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.