RoboRally/remote_control/robot.py

150 lines
6.0 KiB
Python
Raw Normal View History

import socket
import threading
import json
2021-08-25 20:53:09 +00:00
import numpy as np
import time
import math
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)
2020-11-17 20:23:22 +00:00
# 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)
2020-11-17 20:23:22 +00:00
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
2020-10-24 18:01:16 +00:00
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):
2020-11-17 20:23:22 +00:00
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()
2020-11-17 20:23:22 +00:00
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)
2021-08-25 20:53:09 +00:00
def move_to_pos(self, target_pos, blocking=False):
if self.controller is not None:
2021-08-25 20:53:09 +00:00
if not blocking:
self.controller.set_target_position(target_pos)
else: # only return after the robot has reached the target
self.stop_control()
self.controller.set_target_position(target_pos)
self.start_control()
close_to_target = False
while not close_to_target:
current_pos = np.array([self.x, self.y, self.angle])
v = target_pos - current_pos
angles_unwrapped = np.unwrap([current_pos[2], target_pos[2]]) # unwrap angle to avoid jump in data
e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
e_pos = np.linalg.norm(v[0:2])
print(f"e_pos = {e_pos}, e_ang = {e_angle}")
close_to_target = e_pos < 0.05 and abs(e_angle) < 0.1
time.sleep(0.1)
print("target reached!")
self.stop_control()
else:
2021-08-25 20:53:09 +00:00
raise Exception("Error: Cannot move to position: there is not controller attached to the robot!")