1 前言
之前写了几篇关于UR机器人网络控制的文章:《UR机器人返回信息格式解析》、《UR机器人通信端口和协议》,有不少读者问关于编程实现方面的问题,因此,这里上传有关的代码,供同行参考。
2 包含内容
我这里是用VS2015编译环境,用C/C++语言实现的。
实际上没有高深的技术,涉及到两个内容:
- Windows Sockets网络编程;
- 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]就是机器人的当前实时位姿的六个数据。
以上代码的完整工程可查看下载页面