机器人进阶学习（二）--上位机上的程序搭建

1万 · 2019-7-6 17:08

这篇介绍下在上位机上如何接收和发送数据。

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字节]

1万 · 2019-7-6 17:08

（2）ROS平台串口节点部分

写入串口
（1）内容：左右轮速度，单位为mm/s
（2）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]

读取串口
（1）内容：小车x,y坐标，方向角，线速度，角速度，单位依次为：mm,mm,rad,mm/s,rad/s
（2）格式：２１字节，[Ｘ坐标４字节][Ｙ坐标４字节][方向角４字节][线速度４字节][角速度４字节][结束符"\n"１字节]

1万 · 2019-7-6 17:08

串口通信处理程序

（1）小车底盘串口处理程序。

底盘串口处理的程序主要写在了 main.c 文件中。

底盘向串口发送里程计代码：
1
2
3
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;//执行计算里程计数据步骤
}

1万 · 2019-7-6 17:08

底盘接收串口发来的速度代码：

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;//清楚接收标志位
}

1万 · 2019-7-6 17:09

（2）ROS平台串口处理程序

ROS平台串口处理程序主要写在 base_controller.cpp 文件中。

ROS平台向串口发送速度代码：
1
2
3
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; //放大１０００倍，mm/s
right_speed_data.d*=ratio;//放大１０００倍，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);
}

1万 · 2019-7-6 17:09

ROS平台接收串口发来的里程计代码：

rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据