通讯协议
STM32端 调试串口通信 ax_uart_db.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
/**
____ _____ _____ _____ 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端
#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文件
#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_;
};
Last updated