46 lines
1.0 KiB
Python
46 lines
1.0 KiB
Python
import socket
|
|
import time
|
|
from oscpy import *
|
|
|
|
|
|
SEND_IP = "172.18.0.2"
|
|
SEND_PORT = 8000
|
|
RECV_IP = "172.18.0.1"
|
|
RECV_PORT = 7000
|
|
|
|
pub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
sub = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
|
|
msg = Message("/joint_positions", [-1]*6).encode()
|
|
n = pub.sendto(msg, (SEND_IP, SEND_PORT))
|
|
if n > 0:
|
|
print(f"INFO: Sent {n} bytes")
|
|
|
|
time.sleep(10)
|
|
|
|
msg = Message("/tcp_coordinates", [-1]*6).encode()
|
|
n = pub.sendto(msg, (SEND_IP, SEND_PORT))
|
|
if n > 0:
|
|
print(f"INFO: Sent {n} bytes")
|
|
|
|
time.sleep(10)
|
|
|
|
sub.bind((RECV_IP, RECV_PORT))
|
|
joint_names = []
|
|
found = False
|
|
while not found:
|
|
data, addr = sub.recvfrom(5*1024)
|
|
if "#bundle" in data[:8].decode():
|
|
b = Bundle().decode(data)
|
|
for msg in b.msgs:
|
|
if "/joint_state/name" in msg.address:
|
|
joint_names = msg.args
|
|
found = True
|
|
|
|
if joint_names:
|
|
msg = Message(f"/joint_position/{joint_names[2]}", [ 0 ])
|
|
n = pub.sendto(msg.encode(), (SEND_IP, SEND_PORT))
|
|
if n > 0:
|
|
print(f"INFO: Sent {n} bytes")
|
|
|