From 02923dbbdbbcbcce4673140b04737bdac344b800 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Mon, 26 Mar 2012 16:48:11 -0700
Subject: [PATCH] Tracks the connected robot. Supports sending a quit command
---
ur5_driver/driver.py | 39 ++++++++++++++++++++++++++++++++++-----
1 file changed, 34 insertions(+), 5 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 8e070c5..c7fdf33 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -19,14 +19,30 @@ MULT_jointstate = 1000.0
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
+connected_robot = None
+connected_robot_lock = threading.Lock()
+connected_robot_cond = threading.Condition(connected_robot_lock)
pub_joint_states = rospy.Publisher('joint_states', JointState)
+dump_state = open('dump_state', 'wb')
class EOF(Exception): pass
-dump_state = open('dump_state', 'wb')
+def setConnectedRobot(r):
+ global connected_robot, connected_robot_lock
+ with connected_robot_lock:
+ connected_robot = r
+ connected_robot_cond.notify()
+
+def getConnectedRobot(wait=False):
+ with connected_robot_lock:
+ if wait:
+ while not connected_robot:
+ connected_robot_cond.wait()
+ return connected_robot
# Receives messages from the robot over the socket
class CommanderTCPHandler(SocketServer.BaseRequestHandler):
+
def recv_more(self):
more = self.request.recv(4096)
if not more:
@@ -34,7 +50,9 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
return more
def handle(self):
- print "Handling a request"
+ self.socket_lock = threading.Lock()
+ setConnectedRobot(self)
+ #print "Handling a request"
try:
buf = self.recv_more()
if not buf: return
@@ -73,7 +91,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
pub_joint_states.publish(msg)
elif mtype == MSG_QUIT:
print "Quitting"
- sys.exit(0)
+ raise EOF()
else:
raise Exception("Unknown message type: %i" % mtype)
@@ -81,10 +99,14 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = buf + self.recv_more()
except EOF:
print "Connection closed (command)"
+
+ def send_quit(self):
+ with self.socket_lock:
+ self.request.send(struct.pack("!i", MSG_QUIT))
class TCPServer(SocketServer.TCPServer):
- allow_reuse_address = True
+ allow_reuse_address = True # Allows the program to restart gracefully on crash
timeout = 5
@@ -92,7 +114,7 @@ class TCPServer(SocketServer.TCPServer):
def joinAll(threads):
while any(t.isAlive() for t in threads):
for t in threads:
- t.join(0.1)
+ t.join(0.2)
sock = None
def main():
@@ -110,6 +132,13 @@ def main():
thread_commander.daemon = True
thread_commander.start()
+ r = getConnectedRobot(wait=True)
+ print "Robot connected"
+
+ time.sleep(1)
+ print "Sending quit"
+ r.send_quit()
+
# Waits for the threads to finish
joinAll([thread_commander])
--
GitLab