driver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('ur5_driver')
00003 import time, sys, threading, math
00004 import copy
00005 import datetime
00006 import socket, select
00007 import struct
00008 import traceback, code
00009 import optparse
00010 import SocketServer
00011 from BeautifulSoup import BeautifulSoup
00012 
00013 import rospy
00014 import actionlib
00015 from sensor_msgs.msg import JointState
00016 from control_msgs.msg import FollowJointTrajectoryAction
00017 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00018 
00019 from deserialize import RobotState, RobotMode
00020 
00021 prevent_programming = False
00022 
00023 # Joint offsets, pulled from calibration information stored in the URDF
00024 #
00025 # { "joint_name" : offset }
00026 #
00027 # q_actual = q_from_driver + offset
00028 joint_offsets = {}
00029 
00030 PORT=30002
00031 REVERSE_PORT = 50001
00032 
00033 MSG_OUT = 1
00034 MSG_QUIT = 2
00035 MSG_JOINT_STATES = 3
00036 MSG_MOVEJ = 4
00037 MSG_WAYPOINT_FINISHED = 5
00038 MSG_STOPJ = 6
00039 MSG_SERVOJ = 7
00040 MULT_jointstate = 10000.0
00041 MULT_time = 1000000.0
00042 MULT_blend = 1000.0
00043 
00044 JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
00045                'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
00046 
00047 Q1 = [2.2,0,-1.57,0,0,0]
00048 Q2 = [1.5,0,-1.57,0,0,0]
00049 Q3 = [1.5,-0.2,-1.57,0,0,0]
00050   
00051 
00052 connected_robot = None
00053 connected_robot_lock = threading.Lock()
00054 connected_robot_cond = threading.Condition(connected_robot_lock)
00055 pub_joint_states = rospy.Publisher('joint_states', JointState)
00056 #dump_state = open('dump_state', 'wb')
00057 
00058 class EOF(Exception): pass
00059 
00060 def dumpstacks():
00061     id2name = dict([(th.ident, th.name) for th in threading.enumerate()])
00062     code = []
00063     for threadId, stack in sys._current_frames().items():
00064         code.append("\n# Thread: %s(%d)" % (id2name.get(threadId,""), threadId))
00065         for filename, lineno, name, line in traceback.extract_stack(stack):
00066             code.append('File: "%s", line %d, in %s' % (filename, lineno, name))
00067             if line:
00068                 code.append("  %s" % (line.strip()))
00069     print "\n".join(code)
00070 
00071 def log(s):
00072     print "[%s] %s" % (datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f'), s)
00073 
00074 
00075 RESET_PROGRAM = '''def resetProg():
00076   sleep(0.0)
00077 end
00078 '''
00079 #RESET_PROGRAM = ''
00080     
00081 class UR5Connection(object):
00082     TIMEOUT = 1.0
00083     
00084     DISCONNECTED = 0
00085     CONNECTED = 1
00086     READY_TO_PROGRAM = 2
00087     EXECUTING = 3
00088     
00089     def __init__(self, hostname, port, program):
00090         self.__thread = None
00091         self.__sock = None
00092         self.robot_state = self.DISCONNECTED
00093         self.hostname = hostname
00094         self.port = port
00095         self.program = program
00096         self.last_state = None
00097 
00098     def connect(self):
00099         if self.__sock:
00100             self.disconnect()
00101         self.__buf = ""
00102         self.robot_state = self.CONNECTED
00103         self.__sock = socket.create_connection((self.hostname, self.port))
00104         self.__keep_running = True
00105         self.__thread = threading.Thread(name="UR5Connection", target=self.__run)
00106         self.__thread.daemon = True
00107         self.__thread.start()
00108 
00109     def send_program(self):
00110         global prevent_programming
00111         if prevent_programming:
00112             rospy.loginfo("Programming is currently prevented")
00113             return
00114         assert self.robot_state in [self.READY_TO_PROGRAM, self.EXECUTING]
00115         rospy.loginfo("Programming the robot at %s" % self.hostname)
00116         self.__sock.sendall(self.program)
00117         self.robot_state = self.EXECUTING
00118 
00119     def send_reset_program(self):
00120         self.__sock.sendall(RESET_PROGRAM)
00121         self.robot_state = self.READY_TO_PROGRAM
00122         
00123     def disconnect(self):
00124         if self.__thread:
00125             self.__keep_running = False
00126             self.__thread.join()
00127             self.__thread = None
00128         if self.__sock:
00129             self.__sock.close()
00130             self.__sock = None
00131         self.last_state = None
00132         self.robot_state = self.DISCONNECTED
00133 
00134     def ready_to_program(self):
00135         return self.robot_state in [self.READY_TO_PROGRAM, self.EXECUTING]
00136 
00137     def __trigger_disconnected(self):
00138         log("Robot disconnected")
00139         self.robot_state = self.DISCONNECTED
00140     def __trigger_ready_to_program(self):
00141         rospy.loginfo("Robot ready to program")
00142     def __trigger_halted(self):
00143         log("Halted")
00144 
00145     def __on_packet(self, buf):
00146         state = RobotState.unpack(buf)
00147         self.last_state = state
00148         #import deserialize; deserialize.pstate(self.last_state)
00149 
00150         #log("Packet.  Mode=%s" % state.robot_mode_data.robot_mode)
00151 
00152         if not state.robot_mode_data.real_robot_enabled:
00153             rospy.logfatal("Real robot is no longer enabled.  Driver is fuxored")
00154             time.sleep(2)
00155             sys.exit(1)
00156 
00157         # If the urscript program is not executing, then the driver
00158         # needs to publish joint states using information from the
00159         # robot state packet.
00160         if self.robot_state != self.EXECUTING:
00161             msg = JointState()
00162             msg.header.stamp = rospy.get_rostime()
00163             msg.header.frame_id = "From binary state data"
00164             msg.name = joint_names
00165             msg.position = [0.0] * 6
00166             for i, jd in enumerate(state.joint_data):
00167                 msg.position[i] = jd.q_actual + joint_offsets.get(joint_names[i], 0.0)
00168             msg.velocity = [jd.qd_actual for jd in state.joint_data]
00169             msg.effort = [0]*6
00170             pub_joint_states.publish(msg)
00171             self.last_joint_states = msg
00172 
00173         # Updates the state machine that determines whether we can program the robot.
00174         can_execute = (state.robot_mode_data.robot_mode in [RobotMode.READY, RobotMode.RUNNING])
00175         if self.robot_state == self.CONNECTED:
00176             if can_execute:
00177                 self.__trigger_ready_to_program()
00178                 self.robot_state = self.READY_TO_PROGRAM
00179         elif self.robot_state == self.READY_TO_PROGRAM:
00180             if not can_execute:
00181                 self.robot_state = self.CONNECTED
00182         elif self.robot_state == self.EXECUTING:
00183             if not can_execute:
00184                 self.__trigger_halted()
00185                 self.robot_state = self.CONNECTED
00186 
00187     def __run(self):
00188         while self.__keep_running:
00189             r, _, _ = select.select([self.__sock], [], [], self.TIMEOUT)
00190             if r:
00191                 more = self.__sock.recv(4096)
00192                 if more:
00193                     self.__buf = self.__buf + more
00194 
00195                     # Attempts to extract a packet
00196                     packet_length, ptype = struct.unpack_from("!IB", self.__buf)
00197                     if len(self.__buf) >= packet_length:
00198                         packet, self.__buf = self.__buf[:packet_length], self.__buf[packet_length:]
00199                         self.__on_packet(packet)
00200                 else:
00201                     self.__trigger_disconnected()
00202                     self.__keep_running = False
00203                     
00204             else:
00205                 self.__trigger_disconnected()
00206                 self.__keep_running = False
00207 
00208 
00209 def setConnectedRobot(r):
00210     global connected_robot, connected_robot_lock
00211     with connected_robot_lock:
00212         connected_robot = r
00213         connected_robot_cond.notify()
00214 
00215 def getConnectedRobot(wait=False, timeout=-1):
00216     started = time.time()
00217     with connected_robot_lock:
00218         if wait:
00219             while not connected_robot:
00220                 if timeout >= 0 and time.time() > started + timeout:
00221                     break
00222                 connected_robot_cond.wait(0.2)
00223         return connected_robot
00224 
00225 # Receives messages from the robot over the socket
00226 class CommanderTCPHandler(SocketServer.BaseRequestHandler):
00227 
00228     def recv_more(self):
00229         while True:
00230             r, _, _ = select.select([self.request], [], [], 0.2)
00231             if r:
00232                 more = self.request.recv(4096)
00233                 if not more:
00234                     raise EOF("EOF on recv")
00235                 return more
00236             else:
00237                 now = rospy.get_rostime()
00238                 if self.last_joint_states and \
00239                         self.last_joint_states.header.stamp < now - rospy.Duration(1.0):
00240                     rospy.logerr("Stopped hearing from robot (last heard %.3f sec ago).  Disconnected" % \
00241                                      (now - self.last_joint_states.header.stamp).to_sec())
00242                     raise EOF()
00243 
00244     def handle(self):
00245         self.socket_lock = threading.Lock()
00246         self.last_joint_states = None
00247         setConnectedRobot(self)
00248         print "Handling a request"
00249         try:
00250             buf = self.recv_more()
00251             if not buf: return
00252 
00253             while True:
00254                 #print "Buf:", [ord(b) for b in buf]
00255 
00256                 # Unpacks the message type
00257                 mtype = struct.unpack_from("!i", buf, 0)[0]
00258                 buf = buf[4:]
00259                 #print "Message type:", mtype
00260 
00261                 if mtype == MSG_OUT:
00262                     # Unpacks string message, terminated by tilde
00263                     i = buf.find("~")
00264                     while i < 0:
00265                         buf = buf + self.recv_more()
00266                         i = buf.find("~")
00267                         if len(buf) > 2000:
00268                             raise Exception("Probably forgot to terminate a string: %s..." % buf[:150])
00269                     s, buf = buf[:i], buf[i+1:]
00270                     log("Out: %s" % s)
00271 
00272                 elif mtype == MSG_JOINT_STATES:
00273                     while len(buf) < 3*(6*4):
00274                         buf = buf + self.recv_more()
00275                     state_mult = struct.unpack_from("!%ii" % (3*6), buf, 0)
00276                     buf = buf[3*6*4:]
00277                     state = [s / MULT_jointstate for s in state_mult]
00278 
00279                     msg = JointState()
00280                     msg.header.stamp = rospy.get_rostime()
00281                     msg.name = joint_names
00282                     msg.position = [0.0] * 6
00283                     for i, q_meas in enumerate(state[:6]):
00284                         msg.position[i] = q_meas + joint_offsets.get(joint_names[i], 0.0)
00285                     msg.velocity = state[6:12]
00286                     msg.effort = state[12:18]
00287                     self.last_joint_states = msg
00288                     pub_joint_states.publish(msg)
00289                 elif mtype == MSG_QUIT:
00290                     print "Quitting"
00291                     raise EOF("Received quit")
00292                 elif mtype == MSG_WAYPOINT_FINISHED:
00293                     while len(buf) < 4:
00294                         buf = buf + self.recv_more()
00295                     waypoint_id = struct.unpack_from("!i", buf, 0)[0]
00296                     buf = buf[4:]
00297                     print "Waypoint finished (not handled)"
00298                 else:
00299                     raise Exception("Unknown message type: %i" % mtype)
00300 
00301                 if not buf:
00302                     buf = buf + self.recv_more()
00303         except EOF, ex:
00304             print "Connection closed (command):", ex
00305             setConnectedRobot(None)
00306 
00307     def send_quit(self):
00308         with self.socket_lock:
00309             self.request.send(struct.pack("!i", MSG_QUIT))
00310             
00311     def send_servoj(self, waypoint_id, q_actual, t):
00312         assert(len(q_actual) == 6)
00313         q_robot = [0.0] * 6
00314         for i, q in enumerate(q_actual):
00315             q_robot[i] = q - joint_offsets.get(joint_names[i], 0.0)
00316         params = [MSG_SERVOJ, waypoint_id] + \
00317                  [MULT_jointstate * qq for qq in q_robot] + \
00318                  [MULT_time * t]
00319         buf = struct.pack("!%ii" % len(params), *params)
00320         with self.socket_lock:
00321             self.request.send(buf)
00322         
00323 
00324     def send_stopj(self):
00325         with self.socket_lock:
00326             self.request.send(struct.pack("!i", MSG_STOPJ))
00327 
00328     def set_waypoint_finished_cb(self, cb):
00329         self.waypoint_finished_cb = cb
00330 
00331     # Returns the last JointState message sent out
00332     def get_joint_states(self):
00333         return self.last_joint_states
00334     
00335 
00336 class TCPServer(SocketServer.TCPServer):
00337     allow_reuse_address = True  # Allows the program to restart gracefully on crash
00338     timeout = 5
00339 
00340 
00341 # Waits until all threads have completed.  Allows KeyboardInterrupt to occur
00342 def joinAll(threads):
00343     while any(t.isAlive() for t in threads):
00344         for t in threads:
00345             t.join(0.2)
00346 
00347 # Returns the duration between moving from point (index-1) to point
00348 # index in the given JointTrajectory
00349 def get_segment_duration(traj, index):
00350     if index == 0:
00351         return traj.points[0].time_from_start.to_sec()
00352     return (traj.points[index].time_from_start - traj.points[index-1].time_from_start).to_sec()
00353 
00354 # Reorders the JointTrajectory traj according to the order in
00355 # joint_names.  Destructive.
00356 def reorder_traj_joints(traj, joint_names):
00357     order = [traj.joint_names.index(j) for j in joint_names]
00358 
00359     new_points = []
00360     for p in traj.points:
00361         new_points.append(JointTrajectoryPoint(
00362             positions = [p.positions[i] for i in order],
00363             velocities = [p.velocities[i] for i in order] if p.velocities else [],
00364             accelerations = [p.accelerations[i] for i in order] if p.accelerations else [],
00365             time_from_start = p.time_from_start))
00366     traj.joint_names = joint_names
00367     traj.points = new_points
00368 
00369 def interp_cubic(p0, p1, t_abs):
00370     T = (p1.time_from_start - p0.time_from_start).to_sec()
00371     t = t_abs - p0.time_from_start.to_sec()
00372     q = [0] * 6
00373     qdot = [0] * 6
00374     qddot = [0] * 6
00375     for i in range(len(p0.positions)):
00376         a = p0.positions[i]
00377         b = p0.velocities[i]
00378         c = (-3*p0.positions[i] + 3*p1.positions[i] - 2*T*p0.velocities[i] - T*p1.velocities[i]) / T**2
00379         d = (2*p0.positions[i] - 2*p1.positions[i] + T*p0.velocities[i] + T*p1.velocities[i]) / T**3
00380 
00381         q[i] = a + b*t + c*t**2 + d*t**3
00382         qdot[i] = b + 2*c*t + 3*d*t**2
00383         qddot[i] = 2*c + 6*d*t
00384     return JointTrajectoryPoint(q, qdot, qddot, rospy.Duration(t_abs))
00385 
00386 # Returns (q, qdot, qddot) for sampling the JointTrajectory at time t.
00387 # The time t is the time since the trajectory was started.
00388 def sample_traj(traj, t):
00389     # First point
00390     if t <= 0.0:
00391         return copy.deepcopy(traj.points[0])
00392     # Last point
00393     if t >= traj.points[-1].time_from_start.to_sec():
00394         return copy.deepcopy(traj.points[-1])
00395     
00396     # Finds the (middle) segment containing t
00397     i = 0
00398     while traj.points[i+1].time_from_start.to_sec() < t:
00399         i += 1
00400     return interp_cubic(traj.points[i], traj.points[i+1], t)
00401 
00402 def traj_is_finite(traj):
00403     for pt in traj.points:
00404         for p in pt.positions:
00405             if math.isinf(p) or math.isnan(p):
00406                 return False
00407         for v in pt.velocities:
00408             if math.isinf(v) or math.isnan(v):
00409                 return False
00410     return True
00411         
00412 def has_limited_velocities(traj):
00413     for p in traj.points:
00414         for v in p.velocities:
00415             if math.fabs(v) > max_velocity:
00416                 return False
00417     return True
00418 
00419 def has_velocities(traj):
00420     for p in traj.points:
00421         if len(p.velocities) != len(p.positions):
00422             return False
00423     return True
00424 
00425 def within_tolerance(a_vec, b_vec, tol_vec):
00426     for a, b, tol in zip(a_vec, b_vec, tol_vec):
00427         if abs(a - b) > tol:
00428             return False
00429     return True
00430 
00431 class UR5TrajectoryFollower(object):
00432     RATE = 0.02
00433     def __init__(self, robot, goal_time_tolerance=None):
00434         self.goal_time_tolerance = goal_time_tolerance or rospy.Duration(0.0)
00435         self.joint_goal_tolerances = [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
00436         self.following_lock = threading.Lock()
00437         self.T0 = time.time()
00438         self.robot = robot
00439         self.server = actionlib.ActionServer("follow_joint_trajectory",
00440                                              FollowJointTrajectoryAction,
00441                                              self.on_goal, self.on_cancel, auto_start=False)
00442 
00443         self.goal_handle = None
00444         self.traj = None
00445         self.traj_t0 = 0.0
00446         self.first_waypoint_id = 10
00447         self.tracking_i = 0
00448         self.pending_i = 0
00449 
00450         self.update_timer = rospy.Timer(rospy.Duration(self.RATE), self._update)
00451 
00452     def set_robot(self, robot):
00453         # Cancels any goals in progress
00454         if self.goal_handle:
00455             self.goal_handle.set_canceled()
00456             self.goal_handle = None
00457         self.traj = None
00458         self.robot = robot
00459         if self.robot:
00460             self.init_traj_from_robot()
00461 
00462     # Sets the trajectory to remain stationary at the current position
00463     # of the robot.
00464     def init_traj_from_robot(self):
00465         if not self.robot: raise Exception("No robot connected")
00466         # Busy wait (avoids another mutex)
00467         state = self.robot.get_joint_states()
00468         while not state:
00469             time.sleep(0.1)
00470             state = self.robot.get_joint_states()
00471         self.traj_t0 = time.time()
00472         self.traj = JointTrajectory()
00473         self.traj.joint_names = joint_names
00474         self.traj.points = [JointTrajectoryPoint(
00475             positions = state.position,
00476             velocities = [0] * 6,
00477             accelerations = [0] * 6,
00478             time_from_start = rospy.Duration(0.0))]
00479 
00480     def start(self):
00481         self.init_traj_from_robot()
00482         self.server.start()
00483         print "The action server for this driver has been started"
00484 
00485     def on_goal(self, goal_handle):
00486         log("on_goal")
00487 
00488         # Checks that the robot is connected
00489         if not self.robot:
00490             rospy.logerr("Received a goal, but the robot is not connected")
00491             goal_handle.set_rejected()
00492             return
00493 
00494         # Checks if the joints are just incorrect
00495         if set(goal_handle.get_goal().trajectory.joint_names) != set(joint_names):
00496             rospy.logerr("Received a goal with incorrect joint names: (%s)" % \
00497                          ', '.join(goal_handle.get_goal().trajectory.joint_names))
00498             goal_handle.set_rejected()
00499             return
00500 
00501         if not traj_is_finite(goal_handle.get_goal().trajectory):
00502             rospy.logerr("Received a goal with infinites or NaNs")
00503             goal_handle.set_rejected(text="Received a goal with infinites or NaNs")
00504             return
00505         
00506         # Checks that the trajectory has velocities
00507         if not has_velocities(goal_handle.get_goal().trajectory):
00508             rospy.logerr("Received a goal without velocities")
00509             goal_handle.set_rejected(text="Received a goal without velocities")
00510             return
00511 
00512         # Checks that the velocities are withing the specified limits
00513         if not has_limited_velocities(goal_handle.get_goal().trajectory):
00514             message = "Received a goal with velocities that are higher than %f" % max_velocity
00515             rospy.logerr(message)
00516             goal_handle.set_rejected(text=message)
00517             return
00518 
00519         # Orders the joints of the trajectory according to joint_names
00520         reorder_traj_joints(goal_handle.get_goal().trajectory, joint_names)
00521                 
00522         with self.following_lock:
00523             if self.goal_handle:
00524                 # Cancels the existing goal
00525                 self.goal_handle.set_canceled()
00526                 self.first_waypoint_id += len(self.goal_handle.get_goal().trajectory.points)
00527                 self.goal_handle = None
00528 
00529             # Inserts the current setpoint at the head of the trajectory
00530             now = time.time()
00531             point0 = sample_traj(self.traj, now)
00532             point0.time_from_start = rospy.Duration(0.0)
00533             goal_handle.get_goal().trajectory.points.insert(0, point0)
00534             self.traj_t0 = now
00535 
00536             # Replaces the goal
00537             self.goal_handle = goal_handle
00538             self.traj = goal_handle.get_goal().trajectory
00539             self.goal_handle.set_accepted()
00540 
00541     def on_cancel(self, goal_handle):
00542         log("on_cancel")
00543         if goal_handle == self.goal_handle:
00544             with self.following_lock:
00545                 # Uses the next little bit of trajectory to slow to a stop
00546                 STOP_DURATION = 0.5
00547                 now = time.time()
00548                 point0 = sample_traj(self.traj, now - self.traj_t0)
00549                 point0.time_from_start = rospy.Duration(0.0)
00550                 point1 = sample_traj(self.traj, now - self.traj_t0 + STOP_DURATION)
00551                 point1.velocities = [0] * 6
00552                 point1.accelerations = [0] * 6
00553                 point1.time_from_start = rospy.Duration(STOP_DURATION)
00554                 self.traj_t0 = now
00555                 self.traj = JointTrajectory()
00556                 self.traj.joint_names = joint_names
00557                 self.traj.points = [point0, point1]
00558                 
00559                 self.goal_handle.set_canceled()
00560                 self.goal_handle = None
00561         else:
00562             goal_handle.set_canceled()
00563 
00564     last_now = time.time()
00565     def _update(self, event):
00566         if self.robot and self.traj:
00567             now = time.time()
00568             if (now - self.traj_t0) <= self.traj.points[-1].time_from_start.to_sec():
00569                 setpoint = sample_traj(self.traj, now - self.traj_t0)
00570                 try:
00571                     self.robot.send_servoj(999, setpoint.positions, 4 * self.RATE)
00572                 except socket.error:
00573                     pass
00574             else:  # Off the end
00575                 if self.goal_handle:
00576                     last_point = self.traj.points[-1]
00577                     state = self.robot.get_joint_states()
00578                     position_in_tol = within_tolerance(state.position, last_point.positions, self.joint_goal_tolerances)
00579                     velocity_in_tol = within_tolerance(state.velocity, last_point.velocities, [0.005]*6)
00580                     if position_in_tol and velocity_in_tol:
00581                         # The arm reached the goal (and isn't moving).  Succeeding
00582                         self.goal_handle.set_succeeded()
00583                         self.goal_handle = None
00584                     elif now - (self.traj_t0 + last_point.time_from_start.to_sec()) > self.goal_time_tolerance.to_sec():
00585                         # Took too long to reach the goal.  Aborting
00586                         rospy.logwarn("Took too long to reach the goal.\nDesired: %s\nactual: %s\nvelocity: %s" % \
00587                                           (last_point.positions, state.position, state.velocity))
00588                         self.goal_handle.set_aborted(text="Took too long to reach the goal")
00589                         self.goal_handle = None
00590 
00591 # joint_names: list of joints
00592 #
00593 # returns: { "joint_name" : joint_offset }
00594 def load_joint_offsets(joint_names):
00595     robot_description = rospy.get_param("robot_description")
00596     soup = BeautifulSoup(robot_description)
00597     
00598     result = {}
00599     for joint in joint_names:
00600         try:
00601             joint_elt = soup.find('joint', attrs={'name': joint})
00602             calibration_offset = float(joint_elt.calibration_offset["value"])
00603             result[joint] = calibration_offset
00604         except Exception, ex:
00605             rospy.logwarn("No calibration offset for joint \"%s\"" % joint)
00606     return result
00607 
00608 def main():
00609     rospy.init_node('ur5_driver', disable_signals=True)
00610     if rospy.get_param("use_sim_time", False):
00611         rospy.logfatal("use_sim_time is set!!!")
00612         sys.exit(1)
00613     global prevent_programming
00614     prevent_programming = rospy.get_param("prevent_programming", False)
00615     prefix = rospy.get_param("~prefix", "")
00616     print "Setting prefix to %s" % prefix
00617     global joint_names
00618     joint_names = [prefix + name for name in JOINT_NAMES]
00619 
00620     # Parses command line arguments
00621     parser = optparse.OptionParser(usage="usage: %prog robot_hostname")
00622     (options, args) = parser.parse_args(rospy.myargv()[1:])
00623     if len(args) != 1:
00624         parser.error("You must specify the robot hostname")
00625     robot_hostname = args[0]
00626 
00627     # Reads the calibrated joint offsets from the URDF
00628     global joint_offsets
00629     joint_offsets = load_joint_offsets(joint_names)
00630     rospy.logerr("Loaded calibration offsets: %s" % joint_offsets)
00631 
00632     # Reads the maximum velocity
00633     global max_velocity
00634     max_velocity = rospy.get_param("~max_velocity", 0.5)
00635 
00636     # Sets up the server for the robot to connect to
00637     server = TCPServer(("", 50001), CommanderTCPHandler)
00638     thread_commander = threading.Thread(name="CommanderHandler", target=server.serve_forever)
00639     thread_commander.daemon = True
00640     thread_commander.start()
00641 
00642     with open(roslib.packages.get_pkg_dir('ur5_driver') + '/prog') as fin:
00643         program = fin.read() % {"driver_hostname": socket.getfqdn()}
00644     connection = UR5Connection(robot_hostname, PORT, program)
00645     connection.connect()
00646     connection.send_reset_program()
00647     
00648     action_server = None
00649     try:
00650         while not rospy.is_shutdown():
00651             # Checks for disconnect
00652             if getConnectedRobot(wait=False):
00653                 time.sleep(0.2)
00654                 prevent_programming = rospy.get_param("prevent_programming", False)
00655                 if prevent_programming:
00656                     print "Programming now prevented"
00657                     connection.send_reset_program()
00658             else:
00659                 print "Disconnected.  Reconnecting"
00660                 if action_server:
00661                     action_server.set_robot(None)
00662 
00663                 rospy.loginfo("Programming the robot")
00664                 while True:
00665                     # Sends the program to the robot
00666                     while not connection.ready_to_program():
00667                         print "Waiting to program"
00668                         time.sleep(1.0)
00669                     prevent_programming = rospy.get_param("prevent_programming", False)
00670                     connection.send_program()
00671 
00672                     r = getConnectedRobot(wait=True, timeout=1.0)
00673                     if r:
00674                         break
00675                 rospy.loginfo("Robot connected")
00676 
00677                 if action_server:
00678                     action_server.set_robot(r)
00679                 else:
00680                     action_server = UR5TrajectoryFollower(r, rospy.Duration(1.0))
00681                     action_server.start()
00682 
00683     except KeyboardInterrupt:
00684         try:
00685             r = getConnectedRobot(wait=False)
00686             rospy.signal_shutdown("KeyboardInterrupt")
00687             if r: r.send_quit()
00688         except:
00689             pass
00690         raise
00691 
00692 if __name__ == '__main__': main()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ur5_driver
Author(s): Stuart Glaser
autogenerated on Mon Aug 19 2013 12:33:19