RoboRally/remote_control/robot.py

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)