V1.0.0 sentry follows properly

BBoxProcessor.py

@@ -14,7 +14,7 @@ class BBoxProcessor:
         """
         new_boxes = self.process_next(new_boxes)
         closest_box = self.get_box_closest_to_center(new_boxes)
-        if closest_box is None:
+        if closest_box == []:
             return [0, 0]
         print("Closest box: ", closest_box, "All: ", new_boxes)
         return self.get_normalized_box_position(closest_box)
@@ -49,7 +49,7 @@ class BBoxProcessor:
             closest = None
             for new_box in new_boxes:
                 new_distance = self.bbox_distance(box, new_box)
-                if new_distance < min_distance and new_distance < distance:
+                if (new_distance < min_distance and new_distance < distance):
                     distance = new_distance
                     closest = new_box
 
@@ -58,7 +58,7 @@ class BBoxProcessor:
                 and timestamp < self.last_timestamps[index] + time_to_live
             ):
                 new_boxes_list.append(box)
-                new_timestamps_list.append(timestamp)
+                new_timestamps_list.append(self.last_timestamps[index])
 
         self.last_boxes = new_boxes_list
         self.last_timestamps = new_timestamps_list

Communication/ModbusTCP.py

@@ -205,19 +205,19 @@ class ModbusTCP:
 
         if mbap[0] != self.__transaction_id:
             print("Modbus: Transaction ID mismatch"
-                  "\n - Send: {} \n - Response: {}".format(self.__transaction_id, mbap[0]))
+                  "\t - Send: {} \t - Response: {}".format(self.__transaction_id, mbap[0]))
             return True
         elif mbap[1] != self.__protocol_id:
             print("Modbus: Protocol ID mismatch"
-                  "\n - Send: {} \n - Response: {}".format(self.__protocol_id, mbap[1]))
+                  "\t - Send: {} \t - Response: {}".format(self.__protocol_id, mbap[1]))
             return True
         elif mbap[3] != self.__unit_id:
             print("Modbus: Unit ID mismatch"
-                  "\n - Send: {} \n - Response: {}".format(self.__unit_id, mbap[3]))
+                  "\t - Send: {} \t - Response: {}".format(self.__unit_id, mbap[3]))
             return True
         elif mbap[2] != len(response[6:]):
             print("Modbus: Length mismatch"
-                  "\n - Length: {} \n - Remaining: {}".format(mbap[2], len(response[6:])))
+                  "\t - Length: {} \t - Remaining: {}".format(mbap[2], len(response[6:])))
             return True
 
         function_code = struct.unpack(">B", function_code)

Robot/UR/URModbusServer.py

@@ -66,11 +66,10 @@ class URModbusServer:
         packet_signs = self.modbusTCP.read_holding_registers(320, quantity=6)
 
         if (packet is None) or (packet_signs is None):
-            time.sleep(0.01)
+            time.sleep(0.05)
             print(f"Modbus Error #{tries}: retrying")
             return self.get_joint_angles(tries+1)
         else:
-            #print(f"Packet: {packet_signs.hex()}")
             base = self._format_sign(packet[9:11], packet_signs[9:11])
             shoulder = self._format_sign(packet[11:13], packet_signs[11:13])
             elbow = self._format_sign(packet[13:15], packet_signs[13:15])
@@ -86,8 +85,8 @@ class URModbusServer:
 
     def get_joint_speeds(self, tries=0):
         """
-        Connects with the Modbus server to requests the speed of each joint, in radians
+        Connects with the Modbus server to requests the speed of each joint, in radians per second
-        :return: Readable angle values of each joint in radials
+        :return: Readable angle speeds of each joint in radians per second
         """
         if tries>10:
             print("Modbus Error: Failed")
@@ -96,11 +95,10 @@ class URModbusServer:
         packet = self.modbusTCP.read_holding_registers(280, quantity=6)
 
         if (packet is None):
-            time.sleep(0.01)
+            time.sleep(0.05)
             print(f"Modbus Error #{tries}: retrying")
             return self.get_joint_speeds(tries+1)
         else:
-            #print(f"Packet: {packet_signs.hex()}")
             base = self._format(packet[9:11]) / 1000
             shoulder = self._format(packet[11:13]) / 1000
             elbow = self._format(packet[13:15]) / 1000

URSentry.py

@@ -79,7 +79,7 @@ class URSentry:
 
         # print("Base Speed: ", self.robot_speed[0])
 
-        ticks_to_wait = 10
+        ticks_to_wait = 3
         if self.await_stop:
             print("Awaiting stop --------------------")
             joint_speeds = self.robot.get_joint_speeds()
@@ -95,6 +95,7 @@ class URSentry:
             self.await_stop = True
             self.send_to_zero_on_stop = False
             self.forward_position_to_base_angle_degrees(0)
+            return
 
         # Cancel smooth stopping due to no input
         if self.smooth_stop_delayed_call is not None:
@@ -113,8 +114,8 @@ class URSentry:
         vertical_dead_zone_radius = 0.1
 
         # Speeds for the base and neck joints
-        base_max_speed = 1.1
+        base_max_speed = 1.5
-        base_min_speed = 0.2
+        base_min_speed = 0.1
 
         base_acceleration = 1.5
 
@@ -156,7 +157,7 @@ class URSentry:
             )
             self.smooth_stop_delayed_call.start()
 
-        print("Robot speed: ", self.robot_speed)
+        print("Base speed: ", self.robot_speed[0], " Joystick: ", joystick_pos_x)
         self.robot.speedj(self.robot_speed, base_acceleration, 1)
 
 

main.py

@@ -25,8 +25,13 @@ class CameraJoystick(threading.Thread):
 
     def run(self):
         while self.keep_running:
-            bb = b.get_joystick_position_from_new_set_of_bboxes(q.get())
+            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)
 
     def stop(self):
@@ -62,7 +67,7 @@ ur.forward_position()
 while True:
     try:
         ur.move_robot_base(-current_joystick[0])
-        print("Setting robot base velocity to: ", current_joystick[0])
+        #print("Setting robot base velocity to: ", current_joystick[0])
         time.sleep(0.2)
     except KeyboardInterrupt:
         camera_joystick.stop()