Skip to content
Snippets Groups Projects
Commit a5b09f68 authored by Stuart Glaser's avatar Stuart Glaser
Browse files

Driver successfully reconnects after emergency stop

parent 58389590
Branches
Tags
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 104 additions and 139 deletions
ur5_driver/driver.py
+ 104
− 139
View file @ a5b09f68
...@@ -3,8 +3,9 @@ import roslib; roslib.load_manifest('ur5_driver') ...@@ -3,8 +3,9 @@ import roslib; roslib.load_manifest('ur5_driver')
import time, sys, threading, math import time, sys, threading, math
import copy import copy
import datetime import datetime
import socket import socket, select
import struct import struct
import traceback, code
import SocketServer import SocketServer
import rospy import rospy
...@@ -44,6 +45,17 @@ dump_state = open('dump_state', 'wb') ...@@ -44,6 +45,17 @@ dump_state = open('dump_state', 'wb')
class EOF(Exception): pass class EOF(Exception): pass
def dumpstacks():
id2name = dict([(th.ident, th.name) for th in threading.enumerate()])
code = []
for threadId, stack in sys._current_frames().items():
code.append("\n# Thread: %s(%d)" % (id2name.get(threadId,""), threadId))
for filename, lineno, name, line in traceback.extract_stack(stack):
code.append('File: "%s", line %d, in %s' % (filename, lineno, name))
if line:
code.append(" %s" % (line.strip()))
print "\n".join(code)
def log(s): def log(s):
print "[%s] %s" % (datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), s) print "[%s] %s" % (datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), s)
...@@ -58,23 +70,32 @@ def getConnectedRobot(wait=False): ...@@ -58,23 +70,32 @@ def getConnectedRobot(wait=False):
if wait: if wait:
while not connected_robot: while not connected_robot:
connected_robot_cond.wait(0.5) connected_robot_cond.wait(0.5)
#print "Threads:"
#dumpstacks()
return connected_robot return connected_robot
# Receives messages from the robot over the socket # Receives messages from the robot over the socket
class CommanderTCPHandler(SocketServer.BaseRequestHandler): class CommanderTCPHandler(SocketServer.BaseRequestHandler):
def recv_more(self): def recv_more(self):
more = self.request.recv(4096) while True:
if not more: r, _, _ = select.select([self.request], [], [], 0.2)
raise EOF() if r:
return more more = self.request.recv(4096)
if not more:
raise EOF()
return more
else:
if self.last_joint_states and \
self.last_joint_states.header.stamp.to_sec() < time.time() + 1.0:
rospy.logerr("Stopped hearing from robot. Disconnected")
raise EOF()
def handle(self): def handle(self):
self.socket_lock = threading.Lock() self.socket_lock = threading.Lock()
self.waypoint_finished_cb = None
self.last_joint_states = None self.last_joint_states = None
setConnectedRobot(self) setConnectedRobot(self)
#print "Handling a request" print "Handling a request"
try: try:
buf = self.recv_more() buf = self.recv_more()
if not buf: return if not buf: return
...@@ -121,8 +142,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler): ...@@ -121,8 +142,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = buf + self.recv_more() buf = buf + self.recv_more()
waypoint_id = struct.unpack_from("!i", buf, 0)[0] waypoint_id = struct.unpack_from("!i", buf, 0)[0]
buf = buf[4:] buf = buf[4:]
if self.waypoint_finished_cb: print "Waypoint finished (not handled)"
self.waypoint_finished_cb(waypoint_id)
else: else:
raise Exception("Unknown message type: %i" % mtype) raise Exception("Unknown message type: %i" % mtype)
...@@ -130,6 +150,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler): ...@@ -130,6 +150,7 @@ class CommanderTCPHandler(SocketServer.BaseRequestHandler):
buf = buf + self.recv_more() buf = buf + self.recv_more()
except EOF: except EOF:
print "Connection closed (command)" print "Connection closed (command)"
setConnectedRobot(None)
def send_quit(self): def send_quit(self):
with self.socket_lock: with self.socket_lock:
...@@ -232,26 +253,6 @@ def sample_traj(traj, t): ...@@ -232,26 +253,6 @@ def sample_traj(traj, t):
i += 1 i += 1
return interp_cubic(traj.points[i], traj.points[i+1], t) return interp_cubic(traj.points[i], traj.points[i+1], t)
MAX_BLEND = 0.1
# Determines a blend for the given point that doesn't overlap with its neighbors.
def compute_blend(traj, index):
# The first and last points have nothing to blend with.
if index == 0 or index == len(traj.points) - 1:
return 0.0
return 0.01
# The blend can take up to half the distance to the previous or
# next point.
min_diff = MAX_BLEND
q = traj.points[index].positions
qbefore = traj.points[index-1].positions
qafter = traj.points[index+1].positions
for j in range(len(traj.joint_names)):
min_diff = min(min_diff, abs(q[j] - qbefore[j]) / 2.0)
min_diff = min(min_diff, abs(q[j] - qafter[j]) / 2.0)
return min_diff
class UR5TrajectoryFollower(object): class UR5TrajectoryFollower(object):
RATE = 0.02 RATE = 0.02
def __init__(self, robot): def __init__(self, robot):
...@@ -261,7 +262,6 @@ class UR5TrajectoryFollower(object): ...@@ -261,7 +262,6 @@ class UR5TrajectoryFollower(object):
self.server = actionlib.ActionServer("follow_joint_trajectory", self.server = actionlib.ActionServer("follow_joint_trajectory",
FollowJointTrajectoryAction, FollowJointTrajectoryAction,
self.on_goal, self.on_cancel, auto_start=False) self.on_goal, self.on_cancel, auto_start=False)
self.robot.set_waypoint_finished_cb(self.on_waypoint_finished)
self.goal_handle = None self.goal_handle = None
self.traj = None self.traj = None
...@@ -272,7 +272,20 @@ class UR5TrajectoryFollower(object): ...@@ -272,7 +272,20 @@ class UR5TrajectoryFollower(object):
self.update_timer = rospy.Timer(rospy.Duration(self.RATE), self._update) self.update_timer = rospy.Timer(rospy.Duration(self.RATE), self._update)
def set_robot(self, robot):
# Cancels any goals in progress
if self.goal_handle:
self.goal_handle.set_canceled()
self.goal_handle = None
self.traj = None
self.robot = robot
if self.robot:
self.init_traj_from_robot()
# Sets the trajectory to remain stationary at the current position
# of the robot.
def init_traj_from_robot(self): def init_traj_from_robot(self):
if not self.robot: raise Exception("No robot connected")
# Busy wait (avoids another mutex) # Busy wait (avoids another mutex)
state = self.robot.get_joint_states() state = self.robot.get_joint_states()
while not state: while not state:
...@@ -294,6 +307,12 @@ class UR5TrajectoryFollower(object): ...@@ -294,6 +307,12 @@ class UR5TrajectoryFollower(object):
def on_goal(self, goal_handle): def on_goal(self, goal_handle):
log("on_goal") log("on_goal")
# Checks that the robot is connected
if not self.robot:
rospy.logerr("Received a goal, but the robot is not connected")
goal_handle.set_rejected()
return
# Checks if the joints are just incorrect # Checks if the joints are just incorrect
if set(goal_handle.get_goal().trajectory.joint_names) != set(JOINT_NAMES): if set(goal_handle.get_goal().trajectory.joint_names) != set(JOINT_NAMES):
rospy.logerr("Received a goal with incorrect joint names: (%s)" % \ rospy.logerr("Received a goal with incorrect joint names: (%s)" % \
...@@ -323,14 +342,11 @@ class UR5TrajectoryFollower(object): ...@@ -323,14 +342,11 @@ class UR5TrajectoryFollower(object):
self.traj = goal_handle.get_goal().trajectory self.traj = goal_handle.get_goal().trajectory
self.goal_handle.set_accepted() self.goal_handle.set_accepted()
print "New trajectory:"
print self.traj
def on_cancel(self, goal_handle): def on_cancel(self, goal_handle):
log("on_cancel") log("on_cancel")
if goal_handle == self.goal_handle: if goal_handle == self.goal_handle:
with self.following_lock: with self.following_lock:
# Uses the next 100ms of trajectory to slow to a stop # Uses the next little bit of trajectory to slow to a stop
STOP_DURATION = 0.5 STOP_DURATION = 0.5
now = time.time() now = time.time()
point0 = sample_traj(self.traj, now - self.traj_t0) point0 = sample_traj(self.traj, now - self.traj_t0)
...@@ -350,130 +366,79 @@ class UR5TrajectoryFollower(object): ...@@ -350,130 +366,79 @@ class UR5TrajectoryFollower(object):
goal_handle.set_canceled() goal_handle.set_canceled()
def _update(self, event): def _update(self, event):
#t = time.time() - self.T0 if self.robot and self.traj:
#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)
if self.traj:
now = time.time() now = time.time()
if (now - self.traj_t0) <= self.traj.points[-1].time_from_start.to_sec(): if (now - self.traj_t0) <= self.traj.points[-1].time_from_start.to_sec():
setpoint = sample_traj(self.traj, now - self.traj_t0) setpoint = sample_traj(self.traj, now - self.traj_t0)
self.robot.send_servoj(999, setpoint.positions, 2 * self.RATE) try:
self.robot.send_servoj(999, setpoint.positions, 2 * self.RATE)
except socket.error:
pass
else: # Off the end else: # Off the end
if self.goal_handle: if self.goal_handle:
self.goal_handle.set_succeeded() self.goal_handle.set_succeeded()
self.goal_handle = None self.goal_handle = None
# The URScript program sends back waypoint_finished messages,
# which trigger this callback.
def on_waypoint_finished(self, waypoint_id):
return
with self.following_lock:
log("Waypoint finished: %i" % waypoint_id)
if not self.goal_handle:
return
if waypoint_id < self.first_waypoint_id:
return
index = waypoint_id - self.first_waypoint_id
if index != self.tracking_i:
rospy.logerr("Completed waypoint %i (id=%i), but tracking %i (id=%i)" % \
(index, waypoint_id, self.tracking_i,
self.first_waypoint_id + self.tracking_i))
# TODO: Probably need to fail here
traj = self.goal_handle.get_goal().trajectory
traj_len = len(traj.points)
# Checks if we've completed the trajectory
if index == traj_len - 1:
self.goal_handle.set_succeeded()
self.first_waypoint_id += traj_len
self.goal_handle = None
# Moves onto the next segment
self.tracking_i += 1
if self.pending_i + 1 < traj_len:
self.pending_i += 1
self.robot.send_movej(self.first_waypoint_id + self.pending_i,
traj.points[self.pending_i].positions,
t=get_segment_duration(traj, self.pending_i),
r=compute_blend(traj, self.pending_i))
print "New blend radius:", compute_blend(traj, self.pending_i)
sock = None sock = None
def main(): def main():
global sock global sock
rospy.init_node('ur5_driver', disable_signals=True) rospy.init_node('ur5_driver', disable_signals=True)
if rospy.get_param("use_sim_time", False):
rospy.logfatal("use_sim_time is set!!!")
sys.exit(1)
# Sends the program to the robot # Sets up the server for the robot to connect to
sock = socket.create_connection((HOSTNAME, PORT))
with open('prog') as fin:
program = fin.read()
sock.sendall(program % {"driver_hostname": socket.getfqdn()})
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) server = TCPServer(("", 50001), CommanderTCPHandler)
thread_commander = threading.Thread(name="CommanderHandler", target=server.serve_forever)
thread_commander = threading.Thread(name="CommanderHandler", target=server.handle_request)
thread_commander.daemon = True thread_commander.daemon = True
thread_commander.start() thread_commander.start()
r = getConnectedRobot(wait=True) action_server = None
print "Robot connected"
action_server = UR5TrajectoryFollower(r)
action_server.start()
try: try:
while True:
# Checks for disconnect
if getConnectedRobot(wait=False):
time.sleep(0.2)
else:
print "Disconnected. Reconnecting"
if action_server:
action_server.set_robot(None)
# 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)
rospy.loginfo("Robot connected")
if action_server:
action_server.set_robot(r)
else:
action_server = UR5TrajectoryFollower(r)
action_server.start()
#r.send_servoj(1, Q1, 1.0)
#time.sleep(0.5)
#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)
#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)
#r.send_quit()
# Waits for the threads to finish
joinAll([thread_commander])
except KeyboardInterrupt: except KeyboardInterrupt:
r.send_quit() try:
rospy.signal_shutdown("KeyboardInterrupt") rospy.signal_shutdown("KeyboardInterrupt")
if r: r.send_quit()
except:
pass
raise raise
if __name__ == '__main__': main() if __name__ == '__main__': main()
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment