converter.py
Go to the documentation of this file.
00001 import roslib; roslib.load_manifest('pr2_precise_trajectory')
00002 from pr2_precise_trajectory import *
00003 from pr2_precise_trajectory.arm_controller import get_arm_joint_names
00004 from pr2_precise_trajectory.head_controller import HEAD_JOINTS
00005 from pr2_precise_trajectory.msg import *
00006 from geometry_msgs.msg import Pose
00007 from tf.transformations import quaternion_from_euler
00008 from sensor_msgs.msg import JointState
00009 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00010 import pickle
00011 import yaml
00012 import rospy
00013 
00014 def load_trajectory(filename):
00015     if ".traj" in filename:
00016         trajectory = pickle.load(open(filename, 'r'))
00017         return trajectory_to_simple( trajectory )
00018     elif '.yaml' in filename:
00019         return yaml.load( open(filename, 'r'))
00020     else:
00021         print "Unknown file type"
00022         return None
00023 
00024 def save_trajectory(trajectory, filename, width=1000):
00025     f = open(filename, 'w')
00026     f.write(yaml.dump(trajectory, width=width))
00027     f.close()
00028 
00029 
00030 def simple_to_message_single(angles, duration, key):
00031     movements = {key: angles, TIME: duration}
00032     return simple_to_message([movements], key)
00033 
00034 def simple_to_message(movements, key):
00035     trajectory = JointTrajectory()
00036     if key==HEAD:
00037         trajectory.joint_names = HEAD_JOINTS
00038     else:
00039         trajectory.joint_names = get_arm_joint_names(key)
00040     trajectory.header.stamp = rospy.Time.now()
00041     t=0
00042     for move in precise_subset(movements, key):
00043         pt = JointTrajectoryPoint()
00044         pt.positions = move[key]
00045         t+= get_time(move)
00046         pt.time_from_start = rospy.Duration(t)
00047         #pt.velocities = [0.0]*7
00048         trajectory.points.append(pt)
00049     return trajectory
00050 
00051 def simple_to_move_sequence(movements, frame="/map", now=None, delay=0.0):
00052     nav_goal = MoveSequenceGoal()
00053     nav_goal.header.frame_id = frame
00054     for move in precise_subset(movements, BASE):
00055         t = get_time(move)
00056         pose = move[BASE]
00057         nav_goal.times.append(t-delay)
00058         p = Pose()
00059         p.position.x = pose[0]
00060         p.position.y = pose[1]
00061         q = quaternion_from_euler(0, 0, pose[2])
00062         p.orientation.x = q[0]
00063         p.orientation.y = q[1]
00064         p.orientation.z = q[2]
00065         p.orientation.w = q[3]
00066         nav_goal.poses.append(p)
00067     if now is None:
00068         now = rospy.Time.now()
00069     nav_goal.header.stamp = now + rospy.Duration(delay)
00070     return nav_goal
00071 
00072 def simple_to_gripper_sequence(movements, hand, now=None):
00073     goal = GripperSequenceGoal()
00074     for move in precise_subset(movements, hand):
00075         position = move[hand]
00076         t = get_time(move)
00077         goal.times.append(t)
00078         goal.positions += position
00079     if now is None:
00080         now = rospy.Time.now()
00081     goal.header.stamp = now
00082     return goal
00083 
00084 
00085 def trajectory_to_simple(trajectory, fill_missing_with_zeros=True):
00086     indexes = {}
00087     for arm in [LEFT, RIGHT]:
00088         idx = []
00089         found = 0
00090         missing = 0
00091         for name in get_arm_joint_names(arm):
00092             if name not in trajectory.joint_names:
00093                 idx.append(None)
00094                 missing += 1
00095             else:
00096                 idx.append(trajectory.joint_names.index(name))
00097                 found += 1
00098             
00099         if found>0 and (fill_missing_with_zeros or missing == 0):
00100             indexes[arm] = idx
00101 
00102     if len(indexes)==0:
00103         print "ERROR: Neither arm defined"
00104         return []
00105 
00106     arr = []
00107     last_time = 0.0
00108     for point in trajectory.points:
00109         m = {}
00110         for arm, idx in indexes.iteritems():
00111             m[arm] = []
00112             for index in idx: 
00113                 if index==None:
00114                     m[arm].append(0.0)
00115                 else:
00116                     m[arm].append( point.positions[index] ) 
00117             #TODO Velocity
00118         time = point.time_from_start.to_sec()
00119         m[TIME] = time - last_time
00120         last_time = time
00121         arr.append(m)
00122     return arr
00123     
00124 def simple_to_joint_states(movements, start_time=None):
00125     arr = []
00126     if start_time is None:
00127         start_time = rospy.Time.now()
00128     for move in movements:
00129         start_time += rospy.Duration( get_time(move) )
00130         state = JointState()
00131         state.header.stamp = start_time
00132         for arm in [LEFT, RIGHT]:
00133             if arm not in move:
00134                 continue
00135             state.name += get_arm_joint_names(arm)
00136             state.position += move[arm]
00137         arr.append(state)
00138     return arr
00139 
00140 def tprint(movements):
00141     for move in movements:
00142         print "%0.4f"% get_time(move) , 
00143         PRESET = [LEFT, RIGHT, HEAD, BODY]
00144         for key in PRESET:
00145             if key in move:
00146                 j = ["%.3f"%x for x in move[key] ]
00147                 print "%s: [%s]"%(key,",".join(j)),
00148         for x,a in move.iteritems():
00149             if x not in PRESET + [TIME]:
00150                 print "%s: %s"%(x,str(a)),
00151         print 


pr2_precise_trajectory
Author(s): David V. Lu!!
autogenerated on Fri Aug 28 2015 12:12:32