From 6dddef8978fdfd53e8f1205f2fca5739f1b42611 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Thu, 29 Mar 2012 16:07:21 -0700
Subject: [PATCH] Can now recover from a security stop also
---
ur5_driver/driver.py | 50 +++++++++++++++++++++++---------------------
1 file changed, 26 insertions(+), 24 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 655f620..030dc6d 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -65,13 +65,14 @@ def setConnectedRobot(r):
connected_robot = r
connected_robot_cond.notify()
-def getConnectedRobot(wait=False):
+def getConnectedRobot(wait=False, timeout=-1):
+ started = time.time()
with connected_robot_lock:
if wait:
while not connected_robot:
- connected_robot_cond.wait(0.5)
- #print "Threads:"
- #dumpstacks()
+ if timeout >= 0 and time.time() > started + timeout:
+ break
+ connected_robot_cond.wait(0.2)
return connected_robot
# Receives messages from the robot over the socket
@@ -404,27 +405,28 @@ def main():
if action_server:
action_server.set_robot(None)
-
+ rospy.loginfo("Programming the robot")
+ while True:
# Sends the program to the robot
- sock = socket.create_connection((HOSTNAME, PORT))
- with open('prog') as fin:
- program = fin.read()
- sock.sendall(program % {"driver_hostname": socket.getfqdn()})
-
- # Useful for determining if the robot failed to compile the program
- if False:
- print "Dump"
- print "="*70
- o = ""
- while len(o) < 4096:
- r = sock.recv(1024)
- if not r: break
- o = o + r
- print o
-
- rospy.loginfo("Waiting for the robot to connect back to the driver")
- print "Commander thread:", thread_commander.is_alive()
- r = getConnectedRobot(wait=True)
+ sock = socket.create_connection((HOSTNAME, PORT))
+ with open('prog') as fin:
+ program = fin.read()
+ sock.sendall(program % {"driver_hostname": socket.getfqdn()})
+
+ # Useful for determining if the robot failed to compile the program
+ if False:
+ print "Dump"
+ print "="*70
+ o = ""
+ while len(o) < 4096:
+ r = sock.recv(1024)
+ if not r: break
+ o = o + r
+ print o
+
+ r = getConnectedRobot(wait=True, timeout=1.0)
+ if r:
+ break
rospy.loginfo("Robot connected")
if action_server:
--
GitLab