From edc3e8d29094499d3bb8b420ae8a7df0d74b21a8 Mon Sep 17 00:00:00 2001 From: Simon Pirkelmann Date: Tue, 17 Nov 2020 21:23:22 +0100 Subject: [PATCH] some cleanup --- remote_control/aruco_estimator.py | 9 ++------- remote_control/controller.py | 11 +++++++---- remote_control/event_listener.py | 2 +- remote_control/measurement_server.py | 14 +++++++++----- remote_control/pid_controller.py | 6 ++---- remote_control/robot.py | 14 +++++--------- 6 files changed, 26 insertions(+), 30 deletions(-) diff --git a/remote_control/aruco_estimator.py b/remote_control/aruco_estimator.py index fc0fee9..b72f59a 100644 --- a/remote_control/aruco_estimator.py +++ b/remote_control/aruco_estimator.py @@ -10,6 +10,7 @@ from queue import Queue import aruco + class ArucoEstimator: corner_marker_ids = { 'a': 0, @@ -43,7 +44,7 @@ class ArucoEstimator: # Configure depth and color streams self.pipeline = rs.pipeline() config = rs.config() - #config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30) + # config.enable_stream(rs.stream.color, 1920, 1080, rs.format.bgr8, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) # Start streaming @@ -69,10 +70,6 @@ class ArucoEstimator: # create detector and get parameters 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_params = self.detector.getParameters() @@ -114,8 +111,6 @@ class ArucoEstimator: target_x = x1 + alpha * (x3 - x1) target_y = y1 + beta * (y3 - y1) target = np.array([target_x, target_y]) - - #print(f"target = ({target_x},{target_y})") else: print("not all markers have been detected!") target = np.array([px, -py]) diff --git a/remote_control/controller.py b/remote_control/controller.py index 80baee6..bd6a128 100644 --- a/remote_control/controller.py +++ b/remote_control/controller.py @@ -1,6 +1,7 @@ import threading import time + class ControllerBase: def __init__(self, control_rate=0.1): self.robot = None @@ -20,7 +21,7 @@ class ControllerBase: self.apply_control(control) time.sleep(self.control_rate) - self.apply_control((0.0,0.0)) # stop robot + self.apply_control((0.0, 0.0)) # stop robot def set_target_position(self, target_pos): self.target_pos = target_pos @@ -29,16 +30,18 @@ class ControllerBase: if self.robot is not None: return self.robot.get_measurement() 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): - return (0.0, 0.0) + return 0.0, 0.0 def apply_control(self, control): if self.robot is not None: self.robot.send_cmd(u1=control[0], u2=control[1]) 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): self.robot = robot diff --git a/remote_control/event_listener.py b/remote_control/event_listener.py index 48db580..d423fc4 100644 --- a/remote_control/event_listener.py +++ b/remote_control/event_listener.py @@ -22,7 +22,7 @@ class EventListener: self.event_socket.settimeout(0.1) # check if we receive data from the event server response = self.event_socket.recv(1024) - if not 'error' in str(response): + if 'error' not in str(response): print("... connected! -> start listening for events") self.event_socket.settimeout(None) # if so we start the event thread diff --git a/remote_control/measurement_server.py b/remote_control/measurement_server.py index a4fdd91..d9942b9 100644 --- a/remote_control/measurement_server.py +++ b/remote_control/measurement_server.py @@ -8,12 +8,12 @@ from aruco_estimator import ArucoEstimator class MeasurementHandler(socketserver.BaseRequestHandler): def handle(self) -> None: - self.data = self.request.recv(1024).strip() + data = self.request.recv(1024).strip() cur_thread = threading.current_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()) # send input events while True: @@ -26,24 +26,27 @@ class MeasurementHandler(socketserver.BaseRequestHandler): else: # send robot position try: - marker_id = int(self.data) + marker_id = int(data) except ValueError: marker_id = None if marker_id is not None: if marker_id in self.server.estimator.robot_marker_estimates: 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) else: self.request.sendall("error: unknown robot marker id\n".encode()) else: - self.request.sendall("error: data not understood. expected or 'events'\n".encode()) + self.request.sendall("error: data not understood. " + "expected or 'events'\n".encode()) return class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer): allow_reuse_address = True + def __init__(self, server_address, RequestHandlerClass, estimator, max_measurements_per_second=30): super().__init__(server_address, RequestHandlerClass) self.estimator = estimator @@ -52,6 +55,7 @@ class MeasurementServer(socketserver.ThreadingMixIn, socketserver.TCPServer): def handle_error(self, request, client_address): print("an error occurred -> terminating connection") + if __name__ == "__main__": 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}) diff --git a/remote_control/pid_controller.py b/remote_control/pid_controller.py index 0a63f3f..dc46e9a 100644 --- a/remote_control/pid_controller.py +++ b/remote_control/pid_controller.py @@ -4,6 +4,7 @@ import time from controller import ControllerBase + class PIDController(ControllerBase): def __init__(self): super().__init__() @@ -47,7 +48,6 @@ class PIDController(ControllerBase): else: 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': # compute angle such that robot faces to target point target_angle = self.target_pos[2] @@ -56,7 +56,6 @@ class PIDController(ControllerBase): e_angle = angles_unwrapped[0] - angles_unwrapped[1] # angle difference 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 d = self.D_angle * (e_angle - self.e_angle_old)/dt @@ -113,6 +112,5 @@ class PIDController(ControllerBase): else: u1 = 0.0 u2 = 0.0 - #print(f"u = ({u1}, {u2})") self.t = time.time() # save time when the most recent control was applied - return (u1, u2) + return u1, u2 diff --git a/remote_control/robot.py b/remote_control/robot.py index 811116b..f6c41b4 100644 --- a/remote_control/robot.py +++ b/remote_control/robot.py @@ -25,10 +25,10 @@ class Robot: 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) - 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 - def connect(self): # connect to robot try: @@ -49,7 +49,7 @@ class Robot: 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): + if 'error' not in str(response): print("... connected! -> start listening for measurements") self.measurement_socket.settimeout(None) # if so we start the measurement thread @@ -89,17 +89,13 @@ class Robot: self.receiving = False 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): - 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): connection_state = '' if self.connected else 'not' 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 def __repr__(self):