先把处理的方法的代码放这里:
解析数据帧的代码:
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.");
}