Loading... >> 本内容的通信包括两部分 >> >> 1. TCP：用于标志架位姿信息的传输 >> 2. UDP：用于刀头位姿信息的实时显示 >> > >  > > --- > > ## TCP: NDI -> Master -> Robot > > #### NDI端 > > ```c++ > > ``` > > #### Master端 > > ```python > # coding=utf-8 > > """ > 这个脚本是用于给med提供坐标数据 > 用于机器人的移动 > """ > > import socket > import struct > > def main(): > tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) > > addr = "172.31.1.147" > port = 30001 > > tcp_socket.connect((addr, port)) > > x = 1.0 > y = 2.0 > z = 3.0 > rx = 45.0 > ry = 90.0 > rz = 180.0 > > data = struct.pack(">dddddd", x, y, z, rx, ry, rz) # >dddddd表示大端字节序，java默认就是这个，所以给java传数据就用>dddddd > tcp_socket.send(data) > > tcp_socket.close() > > if __name__ == "__main__": > main() > ``` > > #### Robot端 > > ```java > package network; > > import java.io.DataInputStream; > import java.io.IOException; > import java.io.InputStream; > import java.net.ServerSocket; > import java.net.Socket; > > import javax.inject.Inject; > > import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; > import com.kuka.task.ITaskLogger; > > public class TCPServer extends RoboticsAPIApplication{ > > @Inject > private ITaskLogger logger; > > private ServerSocket ss; > private Socket s; > static final int port = 30001; > > @Override > public void initialize() { > > try { > ss = new ServerSocket(port); > s = ss.accept(); > } catch (Exception e) { > e.printStackTrace(); > } > > } > > @Override > public void run() { > > try { > > DataInputStream dataInput = new DataInputStream(s.getInputStream()); > > double x = dataInput.readDouble(); > double y = dataInput.readDouble(); > double z = dataInput.readDouble(); > double rx = dataInput.readDouble(); > double ry = dataInput.readDouble(); > double rz = dataInput.readDouble(); > > logger.info("x = " + x + "y = " + y + "z = " + z + "rx = " + rx + "ry = " + ry + "rz = " + rz); > s.shutdownInput(); > ss.close(); > > // InputStream in = s.getInputStream(); > // byte[] bs = new byte[1024]; > // int len = -1; > // while((len = in.read(bs)) != -1) { > // logger.info(new String(bs, 0, len)); > // } > // s.shutdownInput(); > // s.getOutputStream().write("Med14 get it!".getBytes()); > // s.shutdownOutput(); > // ss.close(); > } catch (Exception e) { > e.printStackTrace(); > } > > > } > > } > > ``` > > --- > > ## UDP: Robot -> Master -> Software > > #### Robot端 > > ```java > package background; > > import java.net.DatagramPacket; > import java.net.DatagramSocket; > import java.net.InetSocketAddress; > import java.net.SocketException; > import java.nio.ByteBuffer; > import java.util.concurrent.TimeUnit; > > import javax.inject.Inject; > import javax.inject.Named; > > import com.kuka.roboticsAPI.applicationModel.tasks.CycleBehavior; > import com.kuka.roboticsAPI.applicationModel.tasks.RoboticsAPICyclicBackgroundTask; > import com.kuka.roboticsAPI.deviceModel.LBR; > > import com.kuka.roboticsAPI.geometricModel.Tool; > > import com.kuka.roboticsAPI.geometricModel.math.Transformation; > > public class SendCoordinate extends RoboticsAPICyclicBackgroundTask { > > @Inject > private LBR robot; > @Inject > @Named("Gripper") > private Tool gripper; > > private Transformation tcp; > private DatagramSocket ds; > private DatagramPacket dp; > int sendPort = 30001; > > double x, y, z, rx, ry, rz; > > @Override > public void initialize() { > > initializeCyclic(0, 10, TimeUnit.MILLISECONDS, > CycleBehavior.BestEffort); > gripper.attachTo(robot.getFlange()); > > try { > ds = new DatagramSocket(); > } catch (SocketException e) { > e.printStackTrace(); > } > > } > > @Override > public void runCyclic() { > > tcp = gripper.getFrame("/TCP_OS").transformationFromWorld(); > x = tcp.getX(); > y = tcp.getY(); > z = tcp.getZ(); > rx = tcp.getGammaRad(); > ry = tcp.getBetaRad(); > rz = tcp.getAlphaRad(); > > > ByteBuffer buffer = ByteBuffer.allocate(48); > buffer.putDouble(x); > buffer.putDouble(y); > buffer.putDouble(z); > buffer.putDouble(rx); > buffer.putDouble(ry); > buffer.putDouble(rz); > byte[] bs = buffer.array(); > > try { > dp = new DatagramPacket(bs, bs.length, new InetSocketAddress("172.31.1.110", 8090)); > ds.send(dp); > } catch (SocketException e) { > e.printStackTrace(); > } catch (Exception e) { > e.printStackTrace(); > } > > } > > } > > ``` > > #### Master端 > > ```python > # coding=utf-8 > > """ > 这个脚本用于接收med提供的尖端位姿 > 并将位姿信息传送给上位机cpp用于实时显示 > """ > > import socket > import struct > import math > > def bytes_to_float(b): > return struct.unpack('!6d', b) > > > def send_to_cpp(x, y, z, rx, ry, rz): > return 0 > > def main(): > # 准备接受方udp > udp_socket_receive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) > recv_addr = ('', 8090) > udp_socket_receive.bind(recv_addr) > > # 准备发送方udp > udp_socket_send = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) > dest_addr = ('localhost', 8091) > > while True: > # 接受后再发送 > recv_data = udp_socket_receive.recvfrom(1024) > udp_socket_send.sendto(recv_data[0], dest_addr) > > x = bytes_to_float(recv_data[0])[0] > y = bytes_to_float(recv_data[0])[1] > z = bytes_to_float(recv_data[0])[2] > rx = math.degrees(bytes_to_float(recv_data[0])[3]) > ry = math.degrees(bytes_to_float(recv_data[0])[4]) > rz = math.degrees(bytes_to_float(recv_data[0])[5]) > > print(recv_data[1],":", > " x = ", round(x, 2), > " y = ", round(y, 2), > " z = ", round(z, 2), > " rx = ", round(rx, 2), > " ry = ", round(ry, 2), > " rz = ", round(rz, 2)) > > udp_socket_receive.close() > > > if __name__ == "__main__": > main() > ``` > > #### Software端 > > ```c++ > #define _USE_MATH_DEFINES > #include <iostream> > #include <iomanip> // For std::setprecision and std::fixed > #include <winsock2.h> > #include <ws2tcpip.h> > #include <cmath> > > #include <cmath> > > #pragma comment(lib, "Ws2_32.lib") > > // Function to convert network double to host double > double ntohd(double net_double) { > uint64_t host_int = _byteswap_uint64(*reinterpret_cast<uint64_t*>(&net_double)); > return *reinterpret_cast<double*>(&host_int); > } > > int main() { > WSADATA wsaData; > SOCKET recvSocket; > sockaddr_in recvAddr, senderAddr; > int iResult, senderAddrSize = sizeof(senderAddr); > double recvbuf[6]; > int recvbuflen = sizeof(recvbuf); > > iResult = WSAStartup(MAKEWORD(2, 2), &wsaData); > if (iResult != 0) { > std::cerr << "WSAStartup failed: " << iResult << std::endl; > return 1; > } > > recvSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); > if (recvSocket == INVALID_SOCKET) { > std::cerr << "Error at socket(): " << WSAGetLastError() << std::endl; > WSACleanup(); > return 1; > } > > recvAddr.sin_family = AF_INET; > recvAddr.sin_port = htons(8091); > recvAddr.sin_addr.s_addr = htonl(INADDR_ANY); > > iResult = bind(recvSocket, (SOCKADDR*)&recvAddr, sizeof(recvAddr)); > if (iResult == SOCKET_ERROR) { > std::cerr << "Bind failed with error: " << WSAGetLastError() << std::endl; > closesocket(recvSocket); > WSACleanup(); > return 1; > } > > std::cout << "Waiting for data..." << std::endl; > > // Loop indefinitely to keep receiving data > while (true) { > iResult = recvfrom(recvSocket, (char*)recvbuf, recvbuflen, 0, (SOCKADDR*)&senderAddr, &senderAddrSize); > if (iResult == SOCKET_ERROR) { > std::cerr << "recvfrom failed with error: " << WSAGetLastError() << std::endl; > break; > } > else { > //std::cout << "Bytes received: " << iResult << std::endl; > // Convert each received double from network to host byte order > for (int i = 0; i < 6; i++) { > recvbuf[i] = ntohd(recvbuf[i]); > } > // Convert rx, ry, rz from radians to degrees > recvbuf[3] *= 180.0 / M_PI; > recvbuf[4] *= 180.0 / M_PI; > recvbuf[5] *= 180.0 / M_PI; > > // Print the converted values with two decimal places > std::cout << "Received data: "; > std::cout << "x = " << std::fixed << std::setprecision(2) << recvbuf[0] << ", "; > std::cout << "y = " << std::fixed << std::setprecision(2) << recvbuf[1] << ", "; > std::cout << "z = " << std::fixed << std::setprecision(2) << recvbuf[2] << ", "; > std::cout << "rx = " << std::fixed << std::setprecision(2) << recvbuf[3] << ", "; > std::cout << "ry = " << std::fixed << std::setprecision(2) << recvbuf[4] << " , "; > std::cout << "rz = " << std::fixed << std::setprecision(2) << recvbuf[5] << ""; > std::cout << std::endl; > > } > } > > closesocket(recvSocket); > WSACleanup(); > return 0; > } > > ``` ``` 最后修改：2024 年 05 月 06 日 © 允许规范转载 赞 0 如果觉得我的文章对你有用，请随意赞赏
