Go to the documentation of this file.00001 import roslib; roslib.load_manifest('pr2_precise_trajectory')
00002 from pr2_precise_trajectory import *
00003 import rospy
00004 from sensor_msgs.msg import JointState
00005 from tf.transformations import euler_from_quaternion
00006 import tf
00007
00008 class JointWatcher:
00009 def __init__(self, name_map):
00010 self.name_map = name_map
00011 self.key_map = {}
00012 for key, names in name_map.iteritems():
00013 for name in names:
00014 self.key_map[name] = key
00015
00016 self.state = {}
00017 self.data = []
00018 self.start_time = None
00019 self.done = False
00020
00021 self.tf = None
00022 self.frame = None
00023
00024 self.joint_sub = rospy.Subscriber('/joint_states', JointState, self.joint_cb)
00025
00026 def add_tf(self, tf, frame='/map'):
00027 self.tf = tf
00028 self.frame = frame
00029 self.key_map[BASE] = ['x', 'y', 'theta']
00030
00031 def joint_cb(self, msg):
00032 self.state = {}
00033 for key, names in self.name_map.iteritems():
00034 pos = []
00035 for name in names:
00036 i = msg.name.index(name)
00037 pos.append( msg.position[i] )
00038 self.state[key] = pos
00039
00040 if BASE in self.key_map and self.tf:
00041 attempts = 0
00042 while attempts < 10:
00043 try:
00044 (trans,rot) = self.tf.lookupTransform(self.frame, '/base_footprint', rospy.Time(0))
00045 break
00046 except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
00047 attempts += 1
00048 continue
00049 if attempts < 10:
00050 euler = euler_from_quaternion(rot)
00051 self.state[BASE] = [trans[0], trans[1], euler[2]]
00052 else:
00053 rospy.logerr("TF ERROR!")
00054
00055 self.state[TIME] = msg.header.stamp
00056
00057 if self.start_time is not None and not self.done:
00058 self.data.append(self.state)
00059
00060 def get_positions(self, key):
00061 return self.state[key]
00062
00063 def get_state(self):
00064 return self.state
00065
00066 def record(self):
00067 self.start_time = rospy.Time.now()
00068 self.data = []
00069 self.done = False
00070
00071 def stop(self, delay=0.0):
00072 self.done = True
00073 last = None
00074 for move in self.data:
00075 if last is None:
00076 last = move[TIME]
00077 move[TIME] = delay+ 0.0
00078 else:
00079 temp = move[TIME]
00080 move[TIME] = (temp - last).to_sec()
00081 last = temp
00082 return self.data
00083