基於ROS平臺的移動機器人-3-小車底盤與ROS的通訊

1.ROS平臺與底盤通訊協議

ROS平臺與小車底盤的通訊一般是通過串列埠或者CAN匯流排。我這裡採用的是串列埠，以下為我自定義的通訊資料格式：

（1）底盤串列埠部分

1.串列埠接收
 （1）內容：小車左右輪速度,單位:mm/s（所有資料都為   float型，float型佔4位元組）
 （2）格式：10位元組 
[右輪速度4位元組][左輪速度4位元組][結束符"\r\n"2位元組]

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.寫入串列埠
（1）內容：左右輪速度，單位為mm/s
（2）格式：１０位元組,[右輪速度４位元組][左輪速度４位元組][結束符"\r\n"２位元組]
2.讀取串列埠
（1）內容：小車x,y座標，方向角，線速度，角速度，單位依次為：mm,mm,rad,mm/s,rad/s
（2）格式：２１位元組，[Ｘ座標４位元組][Ｙ座標４位元組][方向角４位元組][線速度４位元組][角速度４位元組][結束符"\n"１位元組]

2.串列埠通訊處理程式

（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(); //儲存串列埠傳送來的資料