89 lines
3.2 KiB
Python
89 lines
3.2 KiB
Python
import socket
|
|
import threading
|
|
import json
|
|
|
|
|
|
class Robot:
|
|
def __init__(self, marker_id, ip, measurement_server=('127.0.0.1', 42424)):
|
|
self.id = marker_id
|
|
|
|
self.t_last_measurement = None
|
|
self.x = None
|
|
self.y = None
|
|
self.angle = None
|
|
|
|
self.ip = ip
|
|
self.socket = socket.socket()
|
|
|
|
# currently active control
|
|
self.u1 = 0.0
|
|
self.u2 = 0.0
|
|
|
|
self.connected = False
|
|
|
|
self.measurement_server = measurement_server
|
|
self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # TCP socket
|
|
self.measurement_thread = threading.Thread(target=self.receive_measurements)
|
|
|
|
|
|
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!")
|
|
self.connected = True
|
|
except socket.error:
|
|
print("error: could not connect to robot {} with ip {}".format(self.id, self.ip))
|
|
|
|
# connect to measurement server
|
|
print(f"connecting to measurement server on {self.measurement_server} ...")
|
|
|
|
try:
|
|
self.measurement_socket.connect(self.measurement_server)
|
|
self.measurement_socket.sendall(f"{self.id}\n".encode())
|
|
|
|
self.measurement_socket.settimeout(0.1)
|
|
# check if we receive data from the measurement server
|
|
response = self.measurement_socket.recv(1024)
|
|
if not 'error' in str(response):
|
|
print("... connected! -> start listening for events")
|
|
self.measurement_socket.settimeout(None)
|
|
# if so we start the measurement thread
|
|
self.measurement_thread.start()
|
|
else:
|
|
print(f"error: cannot communicate with the measurement server.\n The response was: {response}")
|
|
except socket.timeout:
|
|
print(f"error: the measurement server did not respond with data.")
|
|
except ConnectionRefusedError:
|
|
print(f"error: could not connect to measurement server at {self.measurement_server}.")
|
|
|
|
|
|
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
|
|
except ConnectionResetError:
|
|
print(f"error: connection to robot {self.id} with ip {self.ip} lost")
|
|
pass
|
|
|
|
def receive_measurements(self):
|
|
receiving = True
|
|
while receiving:
|
|
received = str(self.measurement_socket.recv(1024), "utf-8")
|
|
if len(received) > 0:
|
|
measurement = json.loads(received)
|
|
self.t_last_measurement = measurement['t']
|
|
self.x = measurement['x']
|
|
self.y = measurement['y']
|
|
self.angle = measurement['angle']
|
|
else:
|
|
receiving = False
|
|
print(f"measurement server stopped sending data for robot {self.id}")
|
|
|
|
def get_measurement(self):
|
|
return (self.t_last_measurement, self.x, self.y, self.angle)
|