Windows Sockets 网络编程及UR机器人通信数据解析代码

1 前言

之前写了几篇关于UR机器人网络控制的文章：《UR机器人返回信息格式解析》、《UR机器人通信端口和协议》，有不少读者问关于编程实现方面的问题，因此，这里上传有关的代码，供同行参考。

2 包含内容

我这里是用VS2015编译环境，用C/C++语言实现的。
实际上没有高深的技术，涉及到两个内容：

1. Windows Sockets网络编程；
2. UR机器人返回数据内容解析，包括数据字节的转换。

3 程序实现

3.1 Windows Sockets网络编程

1) Windows Sockets初始化

在程序的开始，进行Windows Sockets初始化。WSAStartup必须是应用程序或DLL调用的第一个Windows Sockets函数。它允许应用程序或DLL指明Windows Sockets API的版本号及获得特定Windows Sockets实现的细节。应用程序或DLL只能在一次成功的WSAStartup()调用之后才能调用进一步的Windows Sockets API函数。

	WSADATA wsaData;
	if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
	{
		printf("Failed to load Winsock");
		return;
	}

对应的，在程序结束前，调用以下语句

	WSACleanup();

2) 建立和断开与服务器的连接

通过IP和端口与服务器建立Socket通信通道，对于UR机器人，缺省的IP和端口为192.168.0.77:30003。
首先建立连接：

	char* pIp = "192.168.0.77";
	int nPort = 30003;
	SOCKADDR_IN addrSrv;
	addrSrv.sin_family = AF_INET;
	addrSrv.sin_port = htons(nPort);
	//	addrSrv.sin_addr.S_un.S_addr = inet_addr(pIp);
	inet_pton(AF_INET, pIp, &addrSrv.sin_addr);

	//创建套接字
	m_sockData = socket(AF_INET, SOCK_STREAM, 0);
	if (INVALID_SOCKET == m_sockData) {
		m_nError = WSAGetLastError();
		return;
	}

	//向服务器发出连接请求
	if (connect(m_sockData, (struct  sockaddr*)&addrSrv, sizeof(addrSrv)) == INVALID_SOCKET)
	{
		m_nError = WSAGetLastError();
		CString msg;
		msg.Format(_T("Connect Error=%d"), m_nError);
		AfxMessageBox(msg);
		return;
	}
	//启动接收线程
	m_bThread=true;
	AfxBeginThread(Thread_RecvData, this, THREAD_PRIORITY_IDLE);

对应的，最后程序退出或不再使用此通道时，断开连接：

	if (m_sockData != INVALID_SOCKET)
	{
		shutdown(m_sockData, 0);
		closesocket(m_sockData);
		m_sockData = INVALID_SOCKET;
	}

3) 数据接收

网络Sockets数据是异步传送的，且使用的接收函数recv()是阻塞式的，即在没有接收事件发生（正常接收到数据、发生错误、接收超时、……）前，该函数是不返回的，所以我把数据接收放在一个专门的接收线程里。

UINT CURCommDlg::Thread_RecvData(PVOID pParam)
{
	CURCommDlg* pDlg = (CURCommDlg*)pParam;
	int nLen = 1024;
	char* recvBuf = new char[nLen];
	memset(recvBuf, 0, nLen);
	while (pDlg->m_bThread)
	{
		//	//接收数据
		int ret = recv(pDlg->m_sockData, recvBuf, nLen, 0);
		if (ret>0)
		{
			pDlg->OnRecvData(recvBuf, ret);
		}
		else
		{
			//recv error
			pDlg->m_nError = WSAGetLastError();
			closesocket(pDlg->m_sockData);
			break;
		}
	}
	delete[]recvBuf;
	pDlg->m_bThread = false;
	return 1;
}

3.2 数据内容解析

以上网络接收到的数据，发送到OnRecvData函数中进行数据解析。
这里按照UR机器人的30003端口返回数据格式进行解析，具体数据格式见《UR机器人返回信息格式解析》。

void CURCommDlg::OnRecvData(char* pData, int nLen)
{
	DWORD dwPackLen;
	dwPackLen = GetDword((PBYTE)pData);
	double data1[54];
	int n;
	for (n = 0; n < 54; n++)
	{
		data1[n] = GetDouble((PBYTE)(pData + 12 + n * 8));
	}
	double data2[30];
	for (n = 0; n < 30; n++)
	{
		data2[n] = GetDouble((PBYTE)(pData + 444 + n * 8));
	}
	for (n = 0; n < 6; n++)
		m_dCurPos[n] = data2[n];
}

其中，由于UR返回数据为Big-Endian，而计算机中的数据为Little-Endian，必须进行数据字节转换，所以编了以下两个函数完成，实际上以下的GetDword函数和htonl()函数功能一样。

double GetDouble(PBYTE pData)
{
	double t;
	PBYTE p = (PBYTE)&t;
	int i;
	for (i = 0; i < 8; i++)
	{
		p[i] = pData[7 - i];
	}
	return t;
}

DWORD GetDword(PBYTE pData)
{
	DWORD t;
	PBYTE p = (PBYTE)&t;
	int i;
	for (i = 0; i < 4; i++)
	{
		p[i] = pData[3 - i];
	}
	return t;
}

解析出来的数据就可以用于程序的其他用途了，例如以上解析函数中的m_dCurPos[6]就是机器人的当前实时位姿的六个数据。
以上代码的完整工程可查看下载页面