ROS平台构建：（二）ROS与stm32通信

这篇介绍下在上位机上如何接收和发送数据。https://blog.csdn.net/weixin_40641902/article/details/78873340

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）格式：１０字节,[右轮速度４字节][左轮速度４字节][结束符"\r\n"２字节]

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

串口通信处理程序

（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;   //放大１０００倍，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);
}

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

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