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