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

「这是我参与2022首次更文挑战的第5天，活动详情查看：2022首次更文挑战」。

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

queue 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();
    }
} 

}