script/devices/RobotTCP.py

@@ -40,6 +40,7 @@ class RobotTCP(TcpDevice, Stoppable):
          self.joint_motors = []
          self.tool = None
          self.default_desc = None
+         self.tool_open = None
          #self.tool_trsf = [0.0] * 6
          self.frame = FRAME_DEFAULT
          self.polling_interval = 0.01
@@ -52,7 +53,7 @@ class RobotTCP(TcpDevice, Stoppable):
 
     def doInitialize(self):           
         super(RobotTCP, self).doInitialize()
-        self.reset = True      
+        self.reset = True        
 
     def set_tool(self, tool):
         self.tool = tool
@@ -60,7 +61,8 @@ class RobotTCP(TcpDevice, Stoppable):
         self.evaluate("tcp_curtool=" + tool)         
         if self.cartesian_motors_enabled:            
             self.update()
-            self.set_motors_enabled(True)            
+            self.set_motors_enabled(True)       
+        self.is_tool_open()     
     
     def get_tool(self):    
         return self.tool
@@ -395,18 +397,36 @@ class RobotTCP(TcpDevice, Stoppable):
         #self.waitState(State.Ready, -1)
             
 
-    #Tool
-    #This function can timeout since it synchronizes move. 
-    #Better check state before otherwise it can freeze the communication
+    #Tool - synchronized as can freeze communication
+    """
+    def open_tool(self, tool=None, timeout=3000):
+        #This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
+        self.waitState(State.Ready, -1)
+        if tool is None: tool = self.tool
+        return self.evaluate("open(" + tool + " )", timeout=timeout)
+
+    def close_tool(self, tool=None, timeout=3000):
+        #This function can timeout since it synchronizes move. Checking state before otherwise it can freeze the communication.
+        self.waitState(State.Ready, -1)
+        if tool is None: tool = self.tool
+        return self.evaluate("close(" + tool + " )", timeout=timeout)
+    """
+    #Tool - Not synchronized calls: atention to open/close only when state is Ready
     def open_tool(self, tool=None):
         if tool is None: tool = self.tool
-        return self.evaluate("open(" + tool + " )", timeout=3000)
+        self.evaluate(tool + ".gripper=true")
+        self.tool_open = True
 
-    #This function can timeout since it synchronizes move. Better check state before
-    #Better check state before otherwise it can freeze the communication
     def close_tool(self, tool=None):
         if tool is None: tool = self.tool
-        return self.evaluate("close(" + tool + " )", timeout=3000)
+        self.evaluate(tool + ".gripper=false")
+        self.tool_open = False
+
+    def is_tool_open(self, tool=None):
+        if tool is None: tool = self.tool
+        self.tool_open =  robot.eval_bool(tool + ".gripper")        
+        return self.tool_open
+
 
     #Arm position
     def herej(self):
@@ -521,6 +541,7 @@ class RobotTCP(TcpDevice, Stoppable):
             self.speed = int(sts[3])
             self.empty = sts[4] == "1"
             self.settled = sts[5] == "1"
+            #TODO: add tool open
             if cur_task is not None:                                    
                 if int(sts[6]) < 0:         
                     log("Task "+ str(cur_task) + " finished with code: " + str(sts[7]), False)
@@ -547,7 +568,8 @@ class RobotTCP(TcpDevice, Stoppable):
                            "settled": self.settled,
                            "task": cur_task,
                            "mode": self.working_mode,
-                           "status": self.status
+                           "status": self.status,
+                           "open": self.tool_open
                            }, None)
             if self.cartesian_motors_enabled:
                 for m in self.cartesian_motors: