From 56941f7b000f36ea2ed4c77bcfe69409775d70a3 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Mon, 26 Mar 2012 20:10:22 -0700
Subject: [PATCH] Following waypoints. Reporting when reached.
---
ur5_driver/driver.py | 29 ++++++++++++++++++++++-------
ur5_driver/prog | 31 +++++++++++++++++++++----------
2 files changed, 43 insertions(+), 17 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index 1104f0e..c82e32c 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -1,6 +1,7 @@
#!/usr/bin/env python
import roslib; roslib.load_manifest('ur5_driver')
import time, sys, threading
+import datetime
import socket
import struct
import SocketServer
@@ -15,6 +16,7 @@ MSG_OUT = 1
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MSG_MOVEJ = 4
+MSG_WAYPOINT_FINISHED = 5
MULT_jointstate = 1000.0
MULT_time = 1000000.0
MULT_blend = 1000.0
@@ -34,6 +36,9 @@ dump_state = open('dump_state', 'wb')
class EOF(Exception): pass
+def log(s):
+ print "[%s] %s" % (datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), s)
+
def setConnectedRobot(r):
global connected_robot, connected_robot_lock
with connected_robot_lock:
@@ -81,7 +86,8 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
if len(buf) > 2000:
raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
s, buf = buf[:i], buf[i+1:]
- print "Out: %s" % s
+ log("Out: %s" % s)
+
elif mtype == MSG_JOINT_STATES:
while len(buf) < 3*(6*4):
buf = buf + self.recv_more()
@@ -99,6 +105,12 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
elif mtype == MSG_QUIT:
print "Quitting"
raise EOF()
+ elif mtype == MSG_WAYPOINT_FINISHED:
+ while len(buf) < 4:
+ buf = buf + self.recv_more()
+ waypoint_id = struct.unpack_from("!i", buf, 0)[0]
+ buf = buf[4:]
+ log("Waypoint finished: %i" % waypoint_id)
else:
raise Exception("Unknown message type: %i" % mtype)
@@ -111,9 +123,9 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
with self.socket_lock:
self.request.send(struct.pack("!i", MSG_QUIT))
- def send_movej(self, q, a=3, v=0.75, t=0, r=0):
+ def send_movej(self, waypoint_id, q, a=3, v=0.75, t=0, r=0):
assert(len(q) == 6)
- buf = ''.join([struct.pack("!i", MSG_MOVEJ),
+ buf = ''.join([struct.pack("!ii", MSG_MOVEJ, waypoint_id),
struct.pack("!iiiiii", *[MULT_jointstate * qq for qq in q]),
struct.pack("!ii", MULT_jointstate * a, MULT_jointstate * v),
struct.pack("!ii", MULT_time * t, MULT_blend * r)])
@@ -150,9 +162,10 @@ def main():
r = getConnectedRobot(wait=True)
print "Robot connected"
- r.send_movej(Q1)
- time.sleep(2)
- r.send_movej(Q2)
+ log("movej Q1")
+ r.send_movej(1, Q1, t=2.0)
+ log("movej Q2")
+ r.send_movej(2, Q2, t=1.0)
time.sleep(3)
print "Sending quit"
@@ -160,7 +173,8 @@ def main():
# Waits for the threads to finish
joinAll([thread_commander])
-
+
+ '''
print "Dump"
print "="*70
o = ""
@@ -169,6 +183,7 @@ def main():
if not r: break
o = o + r
print o
+ '''
if __name__ == '__main__': main()
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 57f43ef..9f5cff9 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -4,18 +4,25 @@ def driverProg():
MSG_QUIT = 2
MSG_JOINT_STATES = 3
MSG_MOVEJ = 4
+ MSG_WAYPOINT_FINISHED = 5
MULT_jointstate = 1000
MULT_time = 1000000.0
MULT_blend = 1000.0
def send_out(msg):
enter_critical
- socket_send_int(1)
+ socket_send_int(MSG_OUT)
socket_send_string(msg)
socket_send_string("~")
exit_critical
end
-
+
+ def send_waypoint_finished(waypoint_id):
+ enter_critical
+ socket_send_int(MSG_WAYPOINT_FINISHED)
+ socket_send_int(waypoint_id)
+ exit_critical
+ end
thread statePublisherThread():
def send_joint_state():
@@ -79,25 +86,29 @@ def driverProg():
break
elif mtype == MSG_MOVEJ:
send_out("Received movej")
- params_mult = socket_read_binary_integer(6+4)
+ params_mult = socket_read_binary_integer(1+6+4)
if params_mult[0] == 0:
send_out("Received no parameters for movej message")
end
# Unpacks the parameters
- q = [params_mult[1] / MULT_jointstate,
- params_mult[2] / MULT_jointstate,
+ waypoint_id = params_mult[1]
+ q = [params_mult[2] / MULT_jointstate,
params_mult[3] / MULT_jointstate,
params_mult[4] / MULT_jointstate,
params_mult[5] / MULT_jointstate,
- params_mult[6] / MULT_jointstate]
- a = params_mult[7] / MULT_jointstate
- v = params_mult[8] / MULT_jointstate
- t = params_mult[9] / MULT_time
- r = params_mult[10] / MULT_blend
+ params_mult[6] / MULT_jointstate,
+ params_mult[7] / MULT_jointstate]
+ a = params_mult[8] / MULT_jointstate
+ v = params_mult[9] / MULT_jointstate
+ t = params_mult[10] / MULT_time
+ r = params_mult[11] / MULT_blend
# Sends the command
+ send_out("movej started")
movej(q, a, v, t, r)
+ send_waypoint_finished(waypoint_id)
+ send_out("movej finished")
else:
send_out("Received unknown message type")
end
--
GitLab