Add brand and images, fileserver for images, start to add arm queue

ur5_control.py

@@ -10,7 +10,7 @@ import yaml
 import sys
 from util import fprint
 from pyModbusTCP.client import ModbusClient
-
+from multiprocessing import Queue
 import subprocess
 from util import win32
 
@@ -20,6 +20,7 @@ class Rob():
     robot = None
     #offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
     #
+    pos_updates = Queue()
     def __init__(self, config):
         self.config = config
         armc = config["arm"]
@@ -91,7 +92,6 @@ def connect(robot):
 
 def init_arm(robot):
     robot = connect(robot)
-    
     # init urx
     
     #rob.set_payload(2, (0, 0, 0.1))
@@ -510,7 +510,7 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
         goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
     else:
         goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
-    
+    robot.pos_updates.put(1)
     if pick_up:
         open_gripper()
 
@@ -530,7 +530,7 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
     new_pos[2] = 0.2
     rob.movel(new_pos, vel=2, acc=1)
     was_flipped = is_flipped(robot)
-
+    robot.pos_updates.put(2)
     # goto_holder_index(robot, 25, z=0.2)
 def pick_up_holder(robot, holder_index, verbose=False):
     holder_routine(robot, holder_index, True, verbose=verbose)
@@ -613,6 +613,7 @@ def drop_off_tray(robot, slot=0):
 
 
 def return_routine(robot, slot, holder_index=None, verbose=False):
+    # OLD UNUSED
     robot = connect(robot)
     rob = robot.robot
 
@@ -697,6 +698,12 @@ def close_gripper():
     c.close()
     #
 
+def get_position_thread(robot):
+    robot = connect(robot)
+    rob = robot.robot
+    vals = rob.getl()
+    robot.pos_updates.put(tuple(vals))
+
 if __name__ == "__main__":
     
     #init("192.168.1.145")