機(jī)器人里程計(jì)數(shù)據(jù)的上傳與接收

1.通信協(xié)議

里程計(jì)數(shù)據(jù)格式(19字節(jié))

2.STM32端

/**
 * @brief 發(fā)送里程計(jì)數(shù)據(jù)
 */
void DataTrans_Odom(void)
{
  uint8_t _cnt = 0;
  data_u _temp; // 聲明一個(gè)聯(lián)合體實(shí)例，使用它將待發(fā)送數(shù)據(jù)轉(zhuǎn)換為字節(jié)數(shù)組
  uint8_t data_to_send[100] = {0}; // 待發(fā)送的字節(jié)數(shù)組
  
  data_to_send[_cnt++]=0xAA;
  data_to_send[_cnt++]=0x55;
  
  uint8_t _start = _cnt;
  
  float datas[] = {kinematics.odom.vel.linear_x, 
                     kinematics.odom.vel.linear_y, 
                     kinematics.odom.vel.angular_z, 
                     kinematics.odom.pose.theta
                    };
  
  for(int i = 0; i < sizeof(datas) / sizeof(float); i++)
  {
    // 將要發(fā)送的數(shù)據(jù)賦值給聯(lián)合體的float成員
    // 相應(yīng)的就能更改字節(jié)數(shù)組成員的值
    _temp.data = datas[i];
    data_to_send[_cnt++]=_temp.data8[0];
    data_to_send[_cnt++]=_temp.data8[1];
    data_to_send[_cnt++]=_temp.data8[2];
    data_to_send[_cnt++]=_temp.data8[3]; // 最高位
  }
  
  uint8_t checkout = 0;
  for(int i = _start; i < _cnt; i++)
  {
    checkout += data_to_send[i];
  }
  data_to_send[_cnt++] = checkout;
  // 串口發(fā)送
  SendData(data_to_send, _cnt); 
}

3.ROS端

采用狀態(tài)機(jī)的方式來接收STM32端上傳的里程計(jì)數(shù)據(jù)，每讀取一字節(jié)數(shù)據(jù)，則在狀態(tài)機(jī)中處理一次，部分程序如下：

uint8_t buffer = 0;
ser.read(&buffer, 1); // ser是串口類的一個(gè)實(shí)例，該語句表示從串口中讀取一個(gè)字節(jié)
if(state == 0 && buffer == 0xAA)
{
    state++;
}
else if(state == 1 && buffer == 0x55)
{
    state++;
}
else if(state == 2)
{
    data_receive[data_cnt++]=buffer;
    if(data_cnt == 17)
    {
        /* 進(jìn)行數(shù)據(jù)校驗(yàn) */
        uint8_t checkout = 0;
        for(int k = 0; k < data_cnt - 1; k++)
        {
            checkout += data_receive[k];
        }
        if(checkout == data_receive[data_cnt - 1]) // 串口接收到的最后一個(gè)字節(jié)是校驗(yàn)碼 
        {
            /* 校驗(yàn)通過，進(jìn)行解碼 */
            float vx, vy, vth, th; // x軸線速度，y軸線速度，z軸角速度，偏航角
            float* datas_ptr[] = {&vx, &vy, &vth, &th};
            data_u temp;
            for(int i = 0; i < sizeof(datas_ptr) / sizeof(float*); i++)
            {
                temp.data8[0] = data_receive[4 * i + 0];
                temp.data8[1] = data_receive[4 * i + 1];
                temp.data8[2] = data_receive[4 * i + 2];
                temp.data8[3] = data_receive[4 * i + 3];              
                *(datas_ptr[i]) = temp.data;
            }
            th *= D2R; // 轉(zhuǎn)換為弧度
        }
        data_cnt = 0;
        state = 0;
    }
}
else state = 0;

ROS端在運(yùn)行時(shí)可能會(huì)提示串口打開失敗，有兩種原因，一是串口號(hào)不對(duì)，使用dmesg | grep ttyS*列出檢測到的串口號(hào)，逐個(gè)測試；

二是沒有操作權(quán)限，使用sudo chmod 666 /dev/ttyACM0即可解決，也可以使用sudo usermod -aG dialout 用戶名來獲得永久權(quán)限，用戶名可使用whoami查看。