prints

.gitignore

@@ -1,2 +1,3 @@
 .env
 __pycache__/
+venv*

BBoxProcessor.py

@@ -16,6 +16,7 @@ class BBoxProcessor:
         closest_box = self.get_box_closest_to_center(new_boxes)
         if closest_box is None:
             return [0, 0]
+        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:
@@ -82,4 +83,4 @@ class BBoxProcessor:
         """
         Returns the normalized position of a bounding box, shringking its range from 0 to 1000 to 0 to 1
         """
-        return [(box[0] + box[2] / 2) / 500.0 - 1, (box[1] + box[3] / 2) / 500 - 1]
+        return [round((box[0] + box[2] / 2) / 500.0 - 1, 3), round((box[1] + box[3] / 2) / 500 - 1, 3)]

URSentry.py

@@ -81,6 +81,7 @@ class URSentry:
 
         ticks_to_wait = 10
         if self.await_stop:
+            print("Awaiting stop --------------------")
             joint_speeds = self.robot.get_joint_speeds()
             if all([speed == 0 for speed in joint_speeds]):
                 self.await_stop_ticks += 1
@@ -90,6 +91,7 @@ class URSentry:
             return
 
         if self.send_to_zero_on_stop:
+            print("Sending to zero --------------------")
             self.await_stop = True
             self.send_to_zero_on_stop = False
             self.forward_position_to_base_angle_degrees(0)
@@ -111,8 +113,8 @@ class URSentry:
         vertical_dead_zone_radius = 0.1
 
         # Speeds for the base and neck joints
-        base_max_speed = 0.8
-        base_min_speed = 0.3
+        base_max_speed = 1.1
+        base_min_speed = 0.2
 
         base_acceleration = 1.5
 
@@ -154,7 +156,7 @@ class URSentry:
             )
             self.smooth_stop_delayed_call.start()
 
-        # print("Robot speed: ", self.robot_speed)
+        print("Robot speed: ", self.robot_speed)
         self.robot.speedj(self.robot_speed, base_acceleration, 1)
 
 

main.py

@@ -26,7 +26,7 @@ class CameraJoystick(threading.Thread):
     def run(self):
         while self.keep_running:
             bb = b.get_joystick_position_from_new_set_of_bboxes(q.get())
-            print(bb)
+            #print(bb)
             self.joystick_queue.put(bb)
 
     def stop(self):