some cleanup

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])

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

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

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 <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
 
 
 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})

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

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):