先把处理的方法的代码放这里:
解析数据帧的代码:
bool CSocket::findData(byte* buff, int& len) { for (int i = 0; i <= len - 4; i++) { int dataLen; if(buff[i] == 0xAA && buff[i + 1] == 0xBB) { short slen; slen = (short)(byte(buff[i+3])); slen |=(short)((short)(byte(buff[i+2])) << 8); dataLen = slen; if(slen < 0) return false; if(i+4 +dataLen + 4 <= len) { if(buff[i + 4 + dataLen] == 0x0DD && buff[i + 4 + dataLen + 1] == 0xEE && buff[i + 4 + dataLen + 2] == 0xDD && buff[i + 4 + dataLen + 3] == 0xEE) { MsgType type; memcpy(&type, buff + i + 4 , sizeof(type)); if(type == ID_LASER) {} else if(type == ID_PATH) {} else if(type == ID_POSE) {} len = len - i - 4 - dataLen - 4; if( len > 0) { memcpy(swapBuff, buff + i + 4 + dataLen + 4, len); memcpy(buff, swapBuff, len); } return true; } else { len = len - i -4; if(len > 0) { memcpy(swapBuff, buff + i + 4, len); memcpy(buff, swapBuff, len); return false; } } }//if else if(i != 0) { len = len - i; memcpy(swapBuff, buff + i, len); memcpy(buff, swapBuff, len); return false; } }//if }//for return false; }//end function
接收数据帧的代码:
void CSocket::receiveData() { ROS_INFO("Receive Thread Start..."); int len =0; int revDateCount = 0; while(bRunning) { if(!connected) { ROS_INFO("Restarting connection..."); ErroFunc(_T("连接中断,尝试重新连接...")); if(serverSocket > 0) { closesocket(serverSocket); serverSocket = -1; } if(clientSocket > 0) { closesocket(clientSocket); clientSocket = -1; } bool re; if(bServer) re = startServer(); else re = connectServer(); if(!re) continue; revDateCount = 0; MsgFunc(_T("连接到机器人!")); } int re = selectSocket(clientSocket, 500); if(re < 0) { ROS_ERROR("Receive Thread Select Error!"); connected = false; continue; } else if(re == 0) continue; else { len = recv(clientSocket, (char*)recvBuffer, MAX_BUFFER_LEN, 0); if(len > 0) { if(revDateCount + len < MAX_BUFFER_LEN) { memcpy((void*)(buff + revDateCount), (void*)recvBuffer, len); revDateCount += len; } else { memcpy((void*)buff, (void*)recvBuffer, len); revDateCount = len; } while(findData(buff, revDateCount)) { ROS_INFO("Receive data!"); } } else { ROS_ERROR("Receive Data Error!"); connected = false; goto __reconnect__; } } __reconnect__:; } ROS_INFO("Receive Thread Stop."); }
时间: 2024-10-11 23:39:43