forked from Telos4/RoboRally
36 lines
974 B
Python
36 lines
974 B
Python
|
import socket
|
||
|
|
import sys
|
||
|
|
|
||
|
|
|
||
|
|
class Robot:
|
||
|
|
def __init__(self, marker_id, ip):
|
||
|
|
self.id = marker_id
|
||
|
|
self.pos = None
|
||
|
|
self.euler = None
|
||
|
|
|
||
|
|
self.ip = ip
|
||
|
|
self.socket = socket.socket()
|
||
|
|
|
||
|
|
# currently active control
|
||
|
|
self.u1 = 0.0
|
||
|
|
self.u2 = 0.0
|
||
|
|
|
||
|
|
def connect(self):
|
||
|
|
# connect to robot
|
||
|
|
try:
|
||
|
|
print("connecting to robot {} with ip {} ...".format(self.id, self.ip))
|
||
|
|
self.socket.connect((self.ip, 1234)) # connect to robot
|
||
|
|
print("connected!")
|
||
|
|
except socket.error:
|
||
|
|
print("could not connect to robot {} with ip {}".format(self.id, self.ip))
|
||
|
|
sys.exit(1)
|
||
|
|
|
||
|
|
def send_cmd(self, u1=0.0, u2=0.0):
|
||
|
|
if self.socket:
|
||
|
|
try:
|
||
|
|
self.socket.send(f'({u1},{u2})\n'.encode())
|
||
|
|
except BrokenPipeError:
|
||
|
|
# print(f"error: connection to robot {self.id} with ip {self.ip} lost")
|
||
|
|
pass
|
||
|
|
|