Go to the documentation of this file.00001 import roslib; roslib.load_manifest('hai_sandbox')
00002 import rospy
00003 import actionlib
00004 import pr2_controllers_msgs.msg as pr2m
00005 import trajectory_msgs.msg as tm
00006 import sensor_msgs.msg as sm
00007 import cv
00008
00009 class Arm:
00010 def __init__(self, name):
00011 self.joint_names = rospy.get_param('/%s/joints' % name)
00012 self.client = actionlib.SimpleActionClient('/%s/joint_trajectory_action' % name, pr2m.JointTrajectoryAction)
00013 rospy.loginfo('waiting for server')
00014 self.client.wait_for_server()
00015 self.recorded = []
00016
00017 class JointTrajRecordReplay:
00018
00019 def __init__(self):
00020 self.left_arm = Arm('l_arm_controller')
00021 self.right_arm = Arm('r_arm_controller')
00022 self.names_index = None
00023 rospy.Subscriber("joint_states", sm.JointState, self.joint_state_cb)
00024 cv.NamedWindow('keyboard_input', 1)
00025 self.exit = False
00026
00027 def rarm_goal(self, g):
00028 self.right_arm.client.send_goal(g)
00029 self.right_arm.client.wait_for_result()
00030 return self.right_arm.client.get_result()
00031
00032 def get_joint_states(self, msg):
00033 if self.names_index == None:
00034 self.names_index = {}
00035 for i, n in enumerate(msg.name):
00036 self.names_index[n] = i
00037 positions = [[msg.position[self.names_index[n]] for n in names_list] for names_list in [self.right_arm.joint_names, self.left_arm.joint_names]]
00038 rpos = positions[0]
00039 lpos = positions[1]
00040 return lpos, rpos
00041
00042 def construct_points(self, posl, tstep):
00043 points = [tm.JointTrajectoryPoint() for i in range(len(posl))]
00044 for i in range(len(posl)):
00045 points[i].positions = posl[i]
00046 points[i].velocities = [0 for j in range(7)]
00047 for i in range(len(posl)):
00048 points[i].time_from_start = rospy.Duration(i*tstep)
00049 return points
00050
00051 def joint_state_cb(self, msg):
00052 k = chr(cv.WaitKey(1) & 0xff)
00053 if k == 'r':
00054 lpos, rpos = self.get_joint_states(msg)
00055 self.left_arm.recorded.append(lpos)
00056 self.right_arm.recorded.append(rpos)
00057 rospy.loginfo('Recorded \nr: %s \nl: %s' % (str(rpos), str(lpos)))
00058
00059 elif k == chr(27):
00060 self.exit = True
00061
00062 elif k == 'p':
00063
00064 lpos, rpos = self.get_joint_states(msg)
00065 rospy.loginfo('playing back')
00066 tstep = 2.0
00067 l = list(self.right_arm.recorded)
00068 l.append(self.right_arm.recorded[0])
00069 l.insert(0, rpos)
00070 points = self.construct_points(l, tstep)
00071
00072 g = pr2m.JointTrajectoryGoal()
00073 g.trajectory.joint_names = self.right_arm.joint_names
00074 g.trajectory.points = points
00075
00076 g.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0)
00077 self.right_arm.client.send_goal(g)
00078
00079
00080 if __name__ == '__main__':
00081 try:
00082 rospy.init_node('traj_client')
00083 jtr = JointTrajRecordReplay()
00084 r = rospy.Rate(10)
00085 while not rospy.is_shutdown():
00086 r.sleep()
00087 if jtr.exit:
00088 rospy.loginfo('exiting')
00089 break
00090 except rospy.ROSInterruptException:
00091 print 'prog interrupted'
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133