这篇介绍下在上位机上如何接收和发送数据。
ROS平台与底盘通信协议
ROS平台与stm32通过串口进行通讯
(1)stm32部分
串口接收
(1)内容:小车左右轮速度,单位:mm/s(所有数据都为 float型,float型占4字节)
(2)格式:10字节
[右轮速度4字节][左轮速度4字节][结束符”\r\n”2字节]
串口发送
(1)内容:里程计(x,y坐标、线速度、角速度和方向角,单位依次为:mm,mm,mm/s,rad/s,rad,所有数据都为float型,float型占4字节)
(2)格式:21字节
[x坐标4字节][y坐标4字节][方向角4字节][线速度4字节][角速度4字节][结束符”\n”1字节]
(2)ROS平台串口节点部分
写入串口
(1)内容:左右轮速度,单位为mm/s
(2)格式:10字节,[右轮速度4字节][左轮速度4字节][结束符"\r\n"2字节]
读取串口
(1)内容:小车x,y坐标,方向角,线速度,角速度,单位依次为:mm,mm,rad,mm/s,rad/s
(2)格式:21字节,[X坐标4字节][Y坐标4字节][方向角4字节][线速度4字节][角速度4字节][结束符"\n"1字节]
串口通信处理程序
(1)小车底盘串口处理程序。
底盘串口处理的程序主要写在了 main.c 文件中。
底盘向串口发送里程计代码:
if(main_sta&0x01)//执行发送里程计数据步骤
{
//里程计数据获取
x_data.odoemtry_float=position_x;//单位mm
y_data.odoemtry_float=position_y;//单位mm
theta_data.odoemtry_float=oriention;//单位rad
vel_linear.odoemtry_float=velocity_linear;//单位mm/s
vel_angular.odoemtry_float=velocity_angular;//单位rad/s
//将所有里程计数据存到要发送的数组
for(j=0;j<4;j++)
{
odometry_data[j]=x_data.odometry_char[j];
odometry_data[j+4]=y_data.odometry_char[j];
odometry_data[j+8]=theta_data.odometry_char[j];
odometry_data[j+12]=vel_linear.odometry_char[j];
odometry_data[j+16]=vel_angular.odometry_char[j];
}
odometry_data[20]='\n';//添加结束符
//发送数据要串口
for(i=0;i<21;i++)
{
USART_ClearFlag(USART1,USART_FLAG_TC); //在发送第一个数据前加此句,解决第一个数据不能正常发送的问题
USART_SendData(USART1,odometry_data[i]);//发送一个字节到串口
while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待发送结束
}
main_sta&=0xFE;//执行计算里程计数据步骤
}
底盘接收串口发来的速度代码:
if(USART_RX_STA&0x8000) // 串口1接收函数
{
//接收左右轮速度
for(t=0;t<4;t++)
{
rightdata.data[t]=USART_RX_BUF[t];
leftdata.data[t]=USART_RX_BUF[t+4];
}
//储存左右轮速度
odometry_right=rightdata.d;//单位mm/s
odometry_left=leftdata.d;//单位mm/s
USART_RX_STA=0;//清楚接收标志位
}
(2)ROS平台串口处理程序
ROS平台串口处理程序主要写在 base_controller.cpp 文件中。
ROS平台向串口发送速度代码:
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
{
string port("/dev/ttyUSB0"); //小车串口号
unsigned long baud = 115200; //小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000)); //配置串口
angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s
//将转换好的小车速度分量为左右轮速度
left_speed_data.d = linear_temp - 0.5f*angular_temp*D ;
right_speed_data.d = linear_temp + 0.5f*angular_temp*D ;
//存入数据到要发布的左右轮速度消息
left_speed_data.d*=ratio; //放大1000倍,mm/s
right_speed_data.d*=ratio;//放大1000倍,mm/s
for(int i=0;i<4;i++) //将左右轮速度存入数组中发送给串口
{
speed_data[i]=right_speed_data.data[i];
speed_data[i+4]=left_speed_data.data[i];
}
//在写入串口的左右轮速度数据后加入”/r/n“
speed_data[8]=data_terminal0;
speed_data[9]=data_terminal1;
//写入数据到串口
my_serial.write(speed_data,10);
}
ROS平台接收串口发来的里程计代码:
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据