From 91493bba221c23a0b5331b86593f6336cdebec3f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tom=C3=A1s=20L?= <37879596+Tomz295@users.noreply.github.com>
Date: Tue, 23 Jul 2024 12:33:20 -0400
Subject: [PATCH] Fixed delay error on queue

---
 BBoxProcessor.py           |  9 +++++----
 Robot/UR/URModbusServer.py | 11 ++++++-----
 URSentry.py                |  4 ++--
 main.py                    | 12 +++++++-----
 4 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/BBoxProcessor.py b/BBoxProcessor.py
index c161477..235c3ac 100644
--- a/BBoxProcessor.py
+++ b/BBoxProcessor.py
@@ -8,7 +8,7 @@ class BBoxProcessor:
         self.last_boxes = []
         self.last_timestamps = []
 
-    def get_joystick_position_from_new_set_of_bboxes(self, new_boxes: list) -> list:
+    def get_joystick_position_from_new_set_of_bboxes(self, new_boxes: list) -> list | None:
         """
         Given a set of bounding boxes, returns the joystick position that would move the robot to the closest one.
         """
@@ -16,7 +16,7 @@ class BBoxProcessor:
         closest_box = self.get_box_closest_to_center(new_boxes)
         if closest_box == []:
             return None
-        print("Closest box: ", closest_box, "All: ", new_boxes)
+        #print("Closest box: ", closest_box, "All: ", new_boxes)
         return self.get_normalized_box_position(closest_box)
 
     def bbox_distance(self, box1: list, box2: list) -> float:
@@ -42,7 +42,7 @@ class BBoxProcessor:
         for new_box in new_boxes:
             new_boxes_list.append(new_box)
             new_timestamps_list.append(timestamp)
-
+        # print("New boxes: ", new_boxes_list, end="")
         min_distance = dist_threshold
         for index, box in enumerate(self.last_boxes):
             distance = 9999
@@ -57,9 +57,10 @@ class BBoxProcessor:
                 closest is None
                 and timestamp < self.last_timestamps[index] + time_to_live
             ):
+                # print(" | Survivor: ", box, end="")
                 new_boxes_list.append(box)
                 new_timestamps_list.append(self.last_timestamps[index])
-
+        # print("")
         self.last_boxes = new_boxes_list
         self.last_timestamps = new_timestamps_list
         return new_boxes_list
diff --git a/Robot/UR/URModbusServer.py b/Robot/UR/URModbusServer.py
index 7a35ac9..5464daf 100644
--- a/Robot/UR/URModbusServer.py
+++ b/Robot/UR/URModbusServer.py
@@ -42,7 +42,7 @@ class URModbusServer:
 
         if packet is None:
             time.sleep(0.5)
-            print("Modbus Error: retrying")
+            print("[TCP] Modbus Error: retrying")
             return self.get_tcp_position()
         else:
             x = self._format(packet[9:11]) / 10
@@ -59,15 +59,16 @@ class URModbusServer:
         :return: Readable angle values of each joint in radials
         """
         if tries>10:
-            print("Modbus Error: Failed")
+            print("[Angles] Modbus Error: Failed")
             return 0, 0, 0, 0, 0, 0
 
         packet = self.modbusTCP.read_holding_registers(270, quantity=6)
+        time.sleep(0.001)
         packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6)
 
         if (packet is None) or (packet_signs is None):
             time.sleep(0.01)
-            print(f"Modbus Error #{tries}: retrying")
+            print(f"[Angles] Modbus Error #{tries}: retrying")
             return self.get_joint_angles(tries+1)
         else:
             base = self._format_sign(packet[9:11], packet_signs[9:11])
@@ -89,14 +90,14 @@ class URModbusServer:
         :return: Readable angle speeds of each joint in radians per second
         """
         if tries>10:
-            print("Modbus Error: Failed")
+            print("[Speeds] Modbus Error: Failed")
             return 0, 0, 0, 0, 0, 0
 
         packet = self.modbusTCP.read_holding_registers(280, quantity=6)
 
         if (packet is None):
             time.sleep(0.01)
-            print(f"Modbus Error #{tries}: retrying")
+            print(f"[Speeds] Modbus Error #{tries}: retrying")
             return self.get_joint_speeds(tries+1)
         else:
             base = self._format(packet[9:11]) / 1000
diff --git a/URSentry.py b/URSentry.py
index 6279176..65ee6c8 100644
--- a/URSentry.py
+++ b/URSentry.py
@@ -73,7 +73,7 @@ class URSentry:
         """
         Control the robot based on a joystick input.
         """
-
+        print("Joystick pos: ", joystick_pos)
         # Check for flags that would block control due to things happening
 
         # Initialize robot if it hasn't already
@@ -189,7 +189,7 @@ class URSentry:
 
         # Speeds for the base and neck joints
         base_max_speed = 1.5
-        base_min_speed = 0.1
+        base_min_speed = 0.0
 
         base_acceleration = 1.5
 
diff --git a/main.py b/main.py
index a303463..db940a7 100644
--- a/main.py
+++ b/main.py
@@ -24,15 +24,17 @@ class CameraJoystick(threading.Thread):
         self.keep_running = True
 
     def run(self):
+        global current_joystick
         while self.keep_running:
+            time.sleep(0.0001)
             q_val = []
             try:
                 q_val = q.get_nowait()
             except queue.Empty:
                 pass
             #print(bb)
-            bb = b.get_joystick_position_from_new_set_of_bboxes(q_val)
-            self.joystick_queue.put(bb)
+            current_joystick = b.get_joystick_position_from_new_set_of_bboxes(q_val)
+            #self.joystick_queue.put(bb)
 
     def stop(self):
         self.keep_running = False
@@ -58,8 +60,8 @@ class JoystickScheduler(threading.Thread):
 
 camera_joystick = CameraJoystick(joystick_queue)
 camera_joystick.start()
-joystick_scheduler = JoystickScheduler(joystick_queue)
-joystick_scheduler.start()
+# joystick_scheduler = JoystickScheduler(joystick_queue)
+# joystick_scheduler.start()
 
 ur.initialize_pose()
 
@@ -70,5 +72,5 @@ while True:
         time.sleep(0.2)
     except KeyboardInterrupt:
         camera_joystick.stop()
-        joystick_scheduler.stop()
+        # joystick_scheduler.stop()
         break