ros与底盘串口通讯缓存数据及解析C++

在调试底盘ros包的过程里，出现tf树更新不及时问题，以为是串口堵塞，没有及时处理底盘发送的数据，因此创建了一个串口缓冲区，用的是队列

queue <unsigned char> bufferq;

其中主要注意的是新来的数据写入queue，已处理的数据在queue中pop出

数据的获取

/********************************************************
函数功能：从下位机读取数据
入口参数：无
出口参数：bool
********************************************************/
bool readSpeed()
{
    //获取串口数据
    unsigned char gedtnum = ser.available();
    if(ser.available())
    {
        // ROS_INFO_STREAM("Read serial port");
        ser.read(r_buffer,ser.available());     
    }
    for(int i = 0;i<gedtnum;i++)
    {
        bufferq.push(r_buffer[i]);
        // ROS_INFO("%x\n",r_buffer[i]);
    }
    //ROS_INFO("\n");
    //当堵塞数据过多时候，清空队列
    if(bufferq.size()>1000)
    {
        // while (!bufferq.empty()) bufferq.pop();
        // bufferq.empty();
    }
    return true;
}

数据的解析处理

unsigned char databuf[50];
void CommandProcess(const ros::TimerEvent&)
{
    //队列不为空，则处理
    if(bufferq.size()>8)
    {
        // ROS_INFO("%d\n",bufferq.size());
        // 打印出未处理的数据个数
        unsigned char datat;
        datat = bufferq.front();
        if (datat == 0x5a)
        {
            // 取出的数据及时清空
            bufferq.pop();
            unsigned char  length;
            length = bufferq.front();
            bufferq.pop();
 
            if (length > 1) 
            {
                if ((length-2) <= bufferq.size())
                {
                    bufferq.pop();
 
                    for (int i=0;i< (length-3);i++)
                    {
                        databuf[i]=bufferq.front();
                        bufferq.pop();
                        // ROS_INFO("buf,%x",databuf[i]);
                    }
                    // ROS_INFO("\n");
 
                    if(databuf[0] == 0x04)
                    {
                        chassisvalue.vx =    databuf[1]*256;
                        chassisvalue.vx +=   databuf[2];
                        chassisvalue.vy =    databuf[3]*256;
                        chassisvalue.vy +=   databuf[4];
                        chassisvalue.vth =  databuf[5]*256;
                        chassisvalue.vth += databuf[6];
                        
                        if(chassisvalue.vx!=0)
                        {
                            ROS_INFO("vx:%d\n",chassisvalue.vx);
                            ROS_INFO("vth:%d\n",chassisvalue.vth);
                        }
                                                
                    }
                    //imu                   
                    else if(databuf[0] == 0x06)
                    {
                        chassisvalue.th_imu=  databuf[5]*256;
                        chassisvalue.th_imu += databuf[6];
                        // ROS_INFO("test06:%d\n",vx);
                        // ROS_INFO("vth:%d\n",vth);
                    }
                    // //08 是电流电压                
                    else if(databuf[0] == 0x12)
                    {
                        chassisvalue.vx =    databuf[1]*256;
                        chassisvalue.vx +=   databuf[2];
                        
                        chassisvalue.vy =    databuf[3]*256;
                        chassisvalue.vy +=   databuf[4];
                        
                        chassisvalue.th_imu =    databuf[5]*256;
                        chassisvalue.th_imu +=   databuf[6];
                        
                        chassisvalue.vth =  databuf[7]*256;
                        chassisvalue.vth += databuf[8];
                        if(chassisvalue.th_imu||chassisvalue.vth||chassisvalue.vx||chassisvalue.vy)
                        {                       
                            //  ROS_INFO("121212\n");
                                //              outFile1<<"chassisvaluevx"<<chassisvalue.vx
                                // <<"chassisvaluevy"<<chassisvalue.vy
                                // <<"chassisvalueth"<<chassisvalue.th_imu<<','
                                // <<"chassisvaluevth"<<chassisvalue.vth<<endl;
 
                        }
                        if(chassisvalue.vx!=0)
                        {
                            ROS_INFO("vx:%d\n",chassisvalue.vx);
                            ROS_INFO("vth:%d\n",chassisvalue.vth);
                        }
            
                        // ROS_INFO("vx:%d\n",vx);
                        // ROS_INFO("vth:%d\n",vth);
                    }
                    
    
                    else if (databuf[0] == 0x14)
                    {
                        
                        imuvalue.gyro1 = int32_t(((databuf[4]&0xff)<<24)|((databuf[5]&0xff)<<16)|((databuf[6]&0xff)<<8)|(databuf[7]&0xff));
                        imuvalue.gyro2 = int32_t(((databuf[8]&0xff)<<24)|((databuf[9]&0xff)<<16)|((databuf[10]&0xff)<<8)|(databuf[11]&0xff));
                        imuvalue.gyro3 = int32_t(((databuf[12]&0xff)<<24)|((databuf[13]&0xff)<<16)|((databuf[14]&0xff)<<8)|(databuf[15]&0xff));
                        
                        imuvalue.acc1 = int32_t(((databuf[16]&0xff)<<24)|((databuf[17]&0xff)<<16)|((databuf[18]&0xff)<<8)|(databuf[19]&0xff));
                        imuvalue.acc2 = int32_t(((databuf[20]&0xff)<<24)|((databuf[21]&0xff)<<16)|((databuf[22]&0xff)<<8)|(databuf[23]&0xff));
                        imuvalue.acc3 = int32_t(((databuf[24]&0xff)<<24)|((databuf[25]&0xff)<<16)|((databuf[26]&0xff)<<8)|(databuf[27]&0xff));
                        
                        imuvalue.qw = int16_t((databuf[28]&0xff)<<8|databuf[29]);
                        imuvalue.qx = int16_t((databuf[30]&0xff)<<8|databuf[31]);
                        imuvalue.qy= int16_t((databuf[32]&0xff)<<8|databuf[33]);
                        imuvalue.qz = int16_t((databuf[34]&0xff)<<8|databuf[35]);                       
                    }
                }           
            }
            //databuf中存储了数据流
         }
        else
        {
         bufferq.pop();
        }
    } 
}