add extrusion control using enable pins through UR IO

src/main1.py

@@ -11,6 +11,7 @@ from trajectory_msgs.msg import (
     JointTrajectoryPoint,
     JointTrajectory
 )
+from ur_msgs.srv import SetIO
 
 ## Our libraries
 from utils import load_plan, JTraj2ROS, speed_multiplier, calibration_correction
@@ -23,12 +24,26 @@ robot_num = 0 # 0 or 1
 robot_desc_param = "/r{}/robot_description".format(robot_num+1)
 
 
+class ExtrusionControl:
+    def __init__(self, namespace='/r1'):
+        self.service_name = "{}/ur_hardware_interface/set_io".format(namespace)
+    def on(self):
+        #turn extruder on
+        rospy.wait_for_service(self.service_name)
+        set_io = rospy.ServiceProxy(self.service_name, SetIO)
+        set_io(1,0,0)
+    def off(self):
+        # turn extruder off
+        rospy.wait_for_service(self.service_name)
+        set_io = rospy.ServiceProxy(self.service_name, SetIO)
+        set_io(1,0,1)
+
+
 def execute_plan(plan,
         controller,
-        contours=None,
+        extrude=False,
         confirm=False,
         skip_first=1,
-        contour_speed=0.2,
         move_speed=1,
         n_trajs=100,
         retry_time=0.1,
@@ -47,7 +62,12 @@ def execute_plan(plan,
     for t in plan.trajs[arm]:
         calibration_correction(t, robot_desc_param)
 
+    # create etruder control
+    if extrude:
+        extruder = ExtrusionControl("/r{}".format(arm+1))
+
     # execute trajectories in plan
+    sleep_for = 0
     for i, t in enumerate(plan.trajs[arm]):
         print("---")
 
@@ -61,29 +81,28 @@ def execute_plan(plan,
 
         if confirm:
             a = raw_input('Press enter to continue')
-            if a== 'n':
+            if a == 'n':
                 break
+        else:
+            time.sleep(sleep_for)
+            # turn extrusion off if it was on
+            if extrude:
+                extruder.off()
 
         print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
 
         # Extruding with trajectory
         retries = 0
-        if t.contour is not None and contours is not None:
-            traj2send = calibration_correction(speed_multiplier(t, 1/contour_speed), robot_desc_param)
-            while not controller.exec_ctraj(
-                    traj2send,
-                    contour=contours[t.contour],
-                    wait=False) and retries < max_traj_retries:
-                retries += 1
-                time.sleep(retry_time)
-
-        # Movement only
-        else:
-            traj2send = calibration_correction(speed_multiplier(t, 1/move_speed), robot_desc_param)
-            while not controller.exec_ctraj(traj2send, wait=False) \
-                    and retries < max_traj_retries:
-                retries += 1
-                time.sleep(retry_time)
+        traj2send = calibration_correction(speed_multiplier(t, 1/move_speed), robot_desc_param)
+        while not controller.exec_ctraj(traj2send, wait=False) \
+                and retries < max_traj_retries:
+            retries += 1
+            time.sleep(retry_time)
+
+        sleep_for = traj2send.time[-1] - retry_time
+
+        if extrude and t.contour is not None: # extrude until end of contour
+            extruder.on()
 
         if retries == max_traj_retries:
             print("\nFailed after {} retries, quitting".format(max_traj_retries))
@@ -91,21 +110,16 @@ def execute_plan(plan,
     return
 
 
-if __name__ == '__main__':
-    calibration_test = load_plan('r1_sq')
-    #plan2, flexirex_contours = load_plan('flexirex', contours=True)
+def gohome():
+    con.group.go(plan.trajs[0][1].positions[0])
 
-    # Printing
-    #con = Controller()
-    #execute_plan(plan2, con, contours=flexirex_contours, confirm=True, n_trajs=10)
+
+if __name__ == '__main__':
+    plan = load_plan('knight')
 
     # Test calibration
     con = Controller(robot=1, disable_extruder=True) # r1 when both arms running
     #con = Controller(robot=2, disable_extruder=True) # r2 when both arms running
     #con = Controller(disable_extruder=True)         # any when one arm  running
-    execute_plan(calibration_test, con, confirm=False,arm=robot_num)
 
-    # go to origin and stay (to check calibration)
-    #plan05 = copy.deepcopy(plan1)
-    #plan05.trajs[0].pop()
-    #execute_plan(plan05, con)
+    execute_plan(plan, con, confirm=False, arm=robot_num, extrude=True)