joint_watcher.py
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 


pr2_precise_trajectory
Author(s): David V. Lu!!
autogenerated on Sat Jun 8 2019 20:59:18