RoboRally/remote_control/robot.py
2020-11-17 21:23:22 +01:00

131 lines
5.0 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.port = 1234
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)
# mark thread as daemon -> it terminates automatically when program shuts down
self.measurement_thread.daemon = True
self.receiving = False
def connect(self):
# connect to robot
try:
print(f"connecting to robot {self.ip} at {self.ip}:{self.port} ...")
self.socket.connect((self.ip, self.port)) # connect to robot
print("connected!")
self.connected = True
except socket.error:
print(f"error: could not connect to robot {self.id} at {self.ip}:{self.port}")
# 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 'error' not in str(response):
print("... connected! -> start listening for measurements")
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 and self.connected:
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
else:
print(f"error: robot {self.id} is not connected to {self.ip}")
def receive_measurements(self):
self.receiving = True
while self.receiving:
received = str(self.measurement_socket.recv(1024), "utf-8")
if len(received) > 0:
last_received = received.split('\n')[-2]
measurement = json.loads(last_received)
self.t_last_measurement = measurement['t']
self.x = measurement['x']
self.y = measurement['y']
self.angle = measurement['angle']
else:
self.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
def __str__(self):
connection_state = '' if self.connected else 'not'
state = self.get_measurement()
last_measurement = f'last measurement = {state}' if None not in state else 'no measurements available'
return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement
def __repr__(self):
return str(self)
class ControlledRobot(Robot):
def __init__(self, marker_id, ip):
super().__init__(marker_id, ip)
self.controller = None
def start_control(self):
if self.controller is not None:
self.controller.start()
else:
raise Exception("Error: Cannot start control: there is not controller attached to the robot!")
def stop_control(self):
if self.controller is not None:
self.controller.stop()
else:
raise Exception("Error: Cannot stop control: there is not controller attached to the robot!")
def attach_controller(self, controller):
self.controller = controller
self.controller.attach_robot(self)
def move_to_pos(self, target_pos):
if self.controller is not None:
self.controller.set_target_position(target_pos)
else:
raise Exception("Error: Cannot move to position: there is not controller attached to the robot!")