From b4ba7bf47aa8fd7393fa8ca0ea9b44523c84df42 Mon Sep 17 00:00:00 2001
From: Stuart Glaser <sglaser@willowgarage.com>
Date: Wed, 28 Mar 2012 16:16:45 -0700
Subject: [PATCH] Servoing from the update loop of the driver
---
ur5_driver/driver.py | 58 ++++++++++++++++++++++++++------------------
ur5_driver/prog | 4 +--
2 files changed, 36 insertions(+), 26 deletions(-)
diff --git a/ur5_driver/driver.py b/ur5_driver/driver.py
index ed5f234..0e9d798 100755
--- a/ur5_driver/driver.py
+++ b/ur5_driver/driver.py
@@ -215,7 +215,10 @@ def compute_blend(traj, index):
return min_diff
class UR5TrajectoryFollower(object):
+ RATE = 0.01
def __init__(self, robot):
+ self.following_lock = threading.Lock()
+ self.T0 = time.time()
self.robot = robot
self.server = actionlib.ActionServer("follow_joint_trajectory",
FollowJointTrajectoryAction,
@@ -228,7 +231,7 @@ class UR5TrajectoryFollower(object):
self.pending_i = 0
self.goal_start_time = rospy.Time(0)
- self.following_lock = threading.Lock()
+ self.update_timer = rospy.Timer(rospy.Duration(self.RATE), self._update)
def start(self):
self.server.start()
@@ -280,6 +283,14 @@ class UR5TrajectoryFollower(object):
else:
goal_handle.set_canceled()
+ def _update(self, event):
+ t = time.time() - self.T0
+ q = Q2[:]
+ q[0] = Q2[0] + 0.2 * math.sin(0.25 * t*(2*math.pi))
+ q[1] = Q2[1] - 0.2 + 0.2 * math.cos(0.25 * t*(2*math.pi))
+ self.robot.send_servoj(999, q, 0.016)
+
+
# The URScript program sends back waypoint_finished messages,
# which trigger this callback.
def on_waypoint_finished(self, waypoint_id):
@@ -327,14 +338,15 @@ def main():
with open('prog') as fin:
sock.sendall(fin.read())
- print "Dump"
- print "="*70
- o = ""
- while len(o) < 4096:
- r = sock.recv(1024)
- if not r: break
- o = o + r
- print o
+ if False:
+ print "Dump"
+ print "="*70
+ o = ""
+ while len(o) < 4096:
+ r = sock.recv(1024)
+ if not r: break
+ o = o + r
+ print o
server = TCPServer(("", 50001), CommanderTCPHandler)
@@ -355,20 +367,20 @@ def main():
#r.send_servoj(2, Q2, 1.0)
#time.sleep(0.2)
#r.send_servoj(3, Q3, 1.0)
- r.send_servoj(4, Q2, 1.0)
- time.sleep(0.5)
-
- t0 = time.time()
- waypoint_id = 100
- while True:
- t = time.time() - t0
- q = Q2[:]
- q[0] = Q2[0] + 0.2 * math.sin(0.25 * t*(2*math.pi))
- q[1] = Q2[1] - 0.2 + 0.2 * math.cos(0.25 * t*(2*math.pi))
- r.send_servoj(waypoint_id, q, 0.016)
- waypoint_id += 1
- #print "Servo:", t, q[0], q[1]
- time.sleep(0.008)
+ #r.send_servoj(4, Q2, 1.0)
+ #time.sleep(0.5)
+
+ #t0 = time.time()
+ #waypoint_id = 100
+ #while True:
+ # t = time.time() - t0
+ # q = Q2[:]
+ # q[0] = Q2[0] + 0.2 * math.sin(0.25 * t*(2*math.pi))
+ # q[1] = Q2[1] - 0.2 + 0.2 * math.cos(0.25 * t*(2*math.pi))
+ # r.send_servoj(waypoint_id, q, 0.016)
+ # waypoint_id += 1
+ # #print "Servo:", t, q[0], q[1]
+ # time.sleep(0.008)
#log("movej Q1")
diff --git a/ur5_driver/prog b/ur5_driver/prog
index 61c516c..2a0fe98 100644
--- a/ur5_driver/prog
+++ b/ur5_driver/prog
@@ -199,12 +199,10 @@ def driverProg():
send_out("Received unknown message type")
end
end
- #movej([2.2,0,-1.57,0,0,0],2)
- #movej([1.5,0,-1.57,0,0,0],2)
end
#sleep(1)
kill thread_state
- socket_send_int(2)
+ socket_send_int(MSG_QUIT)
end
driverProg()
--
GitLab