Telos4/RoboRally
1
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity

some cleanup

Browse Source
This commit is contained in:
Simon Pirkelmann 2020-11-17 21:23:22 +01:00
parent 35b6c3bdfd
commit edc3e8d290
6 changed files with 26 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
remote_control/aruco_estimator.py
View File

@ -10,6 +10,7 @@ from queue import Queue
import aruco import aruco
class ArucoEstimator: class ArucoEstimator:
corner_marker_ids = { corner_marker_ids = {
'a': 0, 'a': 0,
@ -69,10 +70,6 @@ class ArucoEstimator:
# create detector and get parameters # create detector and get parameters
self.detector = aruco.MarkerDetector() self.detector = aruco.MarkerDetector()
# self.detector.setDictionary('ARUCO_MIP_36h12')
#self.detector.setDictionary('ARUCO_MIP_16h3')
# self.detector.setDictionary('ARUCO')
#self.detector.setDetectionMode(aruco.DM_NORMAL, 0.05)
self.detector.setDetectionMode(aruco.DM_VIDEO_FAST, 0.05) self.detector.setDetectionMode(aruco.DM_VIDEO_FAST, 0.05)
self.detector_params = self.detector.getParameters() self.detector_params = self.detector.getParameters()
@ -114,8 +111,6 @@ class ArucoEstimator:
target_x = x1 + alpha * (x3 - x1) target_x = x1 + alpha * (x3 - x1)
target_y = y1 + beta * (y3 - y1) target_y = y1 + beta * (y3 - y1)
target = np.array([target_x, target_y]) target = np.array([target_x, target_y])
#print(f"target = ({target_x},{target_y})")
else: else:
print("not all markers have been detected!") print("not all markers have been detected!")
target = np.array([px, -py]) target = np.array([px, -py])

9
remote_control/controller.py
View File

@ -1,6 +1,7 @@
import threading import threading
import time import time
class ControllerBase: class ControllerBase:
def __init__(self, control_rate=0.1): def __init__(self, control_rate=0.1):
self.robot = None self.robot = None
@ -29,16 +30,18 @@ class ControllerBase:
if self.robot is not None: if self.robot is not None:
return self.robot.get_measurement() return self.robot.get_measurement()
else: else:
raise Exception("error: controller cannot get measurement!\n there is no robot attached to the controller!") raise Exception("error: controller cannot get measurement!\n"
" there is no robot attached to the controller!")
def compute_control(self, state): def compute_control(self, state):
return (0.0, 0.0) return 0.0, 0.0
def apply_control(self, control): def apply_control(self, control):
if self.robot is not None: if self.robot is not None:
self.robot.send_cmd(u1=control[0], u2=control[1]) self.robot.send_cmd(u1=control[0], u2=control[1])
else: else:
raise Exception("error: controller cannot apply control!\n there is no robot attached to the controller!") raise Exception("error: controller cannot apply control!\n"
" there is no robot attached to the controller!")
def attach_robot(self, robot): def attach_robot(self, robot):
self.robot = robot self.robot = robot

2
remote_control/event_listener.py
View File

@ -22,7 +22,7 @@ class EventListener:
self.event_socket.settimeout(0.1) self.event_socket.settimeout(0.1)
# check if we receive data from the event server # check if we receive data from the event server
response = self.event_socket.recv(1024) response = self.event_socket.recv(1024)
if not 'error' in str(response): if 'error' not in str(response):
print("... connected! -> start listening for events") print("... connected! -> start listening for events")
self.event_socket.settimeout(None) self.event_socket.settimeout(None)
# if so we start the event thread # if so we start the event thread

14
remote_control/measurement_server.py
View File

@ -8,12 +8,12 @@ from aruco_estimator import ArucoEstimator
class MeasurementHandler(socketserver.BaseRequestHandler): class MeasurementHandler(socketserver.BaseRequestHandler):
def handle(self) -> None: def handle(self) -> None:
self.data = self.request.recv(1024).strip() data = self.request.recv(1024).strip()
cur_thread = threading.current_thread() cur_thread = threading.current_thread()
print(f"current thread {cur_thread}") print(f"current thread {cur_thread}")
if 'events' in self.data.decode(): if 'events' in data.decode():
self.request.sendall('subscribed to events\n'.encode()) self.request.sendall('subscribed to events\n'.encode())
# send input events # send input events
while True: while True:
@ -26,24 +26,27 @@ class MeasurementHandler(socketserver.BaseRequestHandler):
else: else:
# send robot position # send robot position
try: try:
marker_id = int(self.data) marker_id = int(data)
except ValueError: except ValueError:
marker_id = None marker_id = None
if marker_id is not None: if marker_id is not None:
if marker_id in self.server.estimator.robot_marker_estimates: if marker_id in self.server.estimator.robot_marker_estimates:
while True: while True:
self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id]) + '\n').encode()) self.request.sendall((json.dumps(self.server.estimator.robot_marker_estimates[marker_id])
+ '\n').encode())
time.sleep(1.0 / self.server.max_measurements_per_second) time.sleep(1.0 / self.server.max_measurements_per_second)
else: else:
self.request.sendall("error: unknown robot marker id\n".encode()) self.request.sendall("error: unknown robot marker id\n".encode())
else: else:
self.request.sendall("error: data not understood. expected <robot marker id (int)> or 'events'\n".encode()) self.request.sendall("error: data not understood. "
"expected <robot marker id (int)> or 'events'\n".encode())
return return
class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer): class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
allow_reuse_address = True allow_reuse_address = True
def __init__(self, server_address, RequestHandlerClass, estimator, max_measurements_per_second=30): def __init__(self, server_address, RequestHandlerClass, estimator, max_measurements_per_second=30):
super().__init__(server_address, RequestHandlerClass) super().__init__(server_address, RequestHandlerClass)
self.estimator = estimator self.estimator = estimator
@ -52,6 +55,7 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer):
def handle_error(self, request, client_address): def handle_error(self, request, client_address):
print("an error occurred -> terminating connection") print("an error occurred -> terminating connection")
if __name__ == "__main__": if __name__ == "__main__":
aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14]) aruco_estimator = ArucoEstimator(use_realsense=True, robot_marker_ids=[11, 12, 13, 14])
estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True}) estimator_thread = threading.Thread(target=aruco_estimator.run_tracking, kwargs={'invert_grayscale': True})

6
remote_control/pid_controller.py
View File

@ -4,6 +4,7 @@ import time
from controller import ControllerBase from controller import ControllerBase
class PIDController(ControllerBase): class PIDController(ControllerBase):
def __init__(self): def __init__(self):
super().__init__() super().__init__()
@ -47,7 +48,6 @@ class PIDController(ControllerBase):
else: else:
dt = time.time() - self.t dt = time.time() - self.t
#print(f"x_pred = {x_pred}\ntarget = {target_pos}\nerror = {np.linalg.norm(target_pos - x_pred)}\n")
if self.mode == 'angle': if self.mode == 'angle':
# compute angle such that robot faces to target point # compute angle such that robot faces to target point
target_angle = self.target_pos[2] target_angle = self.target_pos[2]
@ -56,7 +56,6 @@ class PIDController(ControllerBase):
e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference
p = self.P_angle * e_angle p = self.P_angle * e_angle
# i += self.I * dt * e # right Riemann sum
self.i += self.I_angle * dt * (e_angle + self.e_angle_old)/2.0 # trapezoidal rule self.i += self.I_angle * dt * (e_angle + self.e_angle_old)/2.0 # trapezoidal rule
d = self.D_angle * (e_angle - self.e_angle_old)/dt d = self.D_angle * (e_angle - self.e_angle_old)/dt
@ -113,6 +112,5 @@ class PIDController(ControllerBase):
else: else:
u1 = 0.0 u1 = 0.0
u2 = 0.0 u2 = 0.0
#print(f"u = ({u1}, {u2})")
self.t = time.time() # save time when the most recent control was applied self.t = time.time() # save time when the most recent control was applied
return (u1, u2) return u1, u2

14
remote_control/robot.py
View File

@ -25,10 +25,10 @@ class Robot:
self.measurement_server = measurement_server self.measurement_server = measurement_server
self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # TCP socket self.measurement_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # TCP socket
self.measurement_thread = threading.Thread(target=self.receive_measurements) self.measurement_thread = threading.Thread(target=self.receive_measurements)
self.measurement_thread.daemon = True # mark thread as daemon -> it terminates automatically when program shuts down # mark thread as daemon -> it terminates automatically when program shuts down
self.measurement_thread.daemon = True
self.receiving = False self.receiving = False
def connect(self): def connect(self):
# connect to robot # connect to robot
try: try:
@ -49,7 +49,7 @@ class Robot:
self.measurement_socket.settimeout(0.1) self.measurement_socket.settimeout(0.1)
# check if we receive data from the measurement server # check if we receive data from the measurement server
response = self.measurement_socket.recv(1024) response = self.measurement_socket.recv(1024)
if not 'error' in str(response): if 'error' not in str(response):
print("... connected! -> start listening for measurements") print("... connected! -> start listening for measurements")
self.measurement_socket.settimeout(None) self.measurement_socket.settimeout(None)
# if so we start the measurement thread # if so we start the measurement thread
@ -89,17 +89,13 @@ class Robot:
self.receiving = False self.receiving = False
print(f"measurement server stopped sending data for robot {self.id}") print(f"measurement server stopped sending data for robot {self.id}")
def stop_measurement_thread(self):
self.receiving = False
self.measurement_thread.join()
def get_measurement(self): def get_measurement(self):
return (self.t_last_measurement, self.x, self.y, self.angle) return self.t_last_measurement, self.x, self.y, self.angle
def __str__(self): def __str__(self):
connection_state = '' if self.connected else 'not' connection_state = '' if self.connected else 'not'
state = self.get_measurement() state = self.get_measurement()
last_measurement = f'last measurement = {state}' if not None in state else 'no measurements available' 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 return f"Robot {self.id}: ip = {self.ip}:{self.port} ({connection_state} connected) " + last_measurement
def __repr__(self): def __repr__(self):