00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 PKG="pr2_pose_saver"
00032 import roslib; roslib.load_manifest(PKG)
00033 import rospy
00034 import time
00035 import sys
00036
00037 from geometry_msgs.msg import Twist, Pose, PoseStamped, TwistStamped
00038 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00039 from pr2_controllers_msgs.msg import *
00040
00041
00042 def save_arm_pose_to_file(file_handle, arm):
00043 """ Reads pose of indicated arm and saves to textfile"""
00044 arm_pose = rospy.wait_for_message("%s_arm_controller/state" % arm, JointTrajectoryControllerState)
00045 jpos = arm_pose.actual.positions
00046 f.write("%s_arm:" % arm)
00047 f.write(jpos.__str__().strip("()"))
00048 f.write("\n")
00049
00050 def save_head_pose_to_file(file_handle):
00051 head_pose = rospy.wait_for_message("/head_traj_controller/state", JointTrajectoryControllerState)
00052 jpos = head_pose.actual.positions
00053 f.write("head:")
00054 f.write(jpos.__str__().strip("()"))
00055 f.write("\n")
00056
00057 def save_torso_position_to_file(file_handle):
00058 torso_pose = rospy.wait_for_message("/torso_controller/state", JointTrajectoryControllerState)
00059 jpos = torso_pose.actual.positions[0]
00060 f.write("torso:")
00061 f.write(jpos.__str__().strip("()"))
00062 f.write("\n")
00063
00064 def save_gripper_angle_to_file(file_handle, gripper):
00065 gripper_state = rospy.wait_for_message("/%s_gripper_controller/state" % gripper, JointControllerState)
00066 jpos = gripper_state.process_value
00067 f.write("%s_gripper: %f\n" % (gripper, jpos))
00068
00069 def save_tilting_laser_angle_to_file(file_handle):
00070 pass
00071
00072 def save_base_position_to_file(file_handle):
00073 pass
00074
00075 if __name__ == "__main__":
00076 from optparse import OptionParser
00077 usage = "usage: %prog [options] [filename]"
00078 parser = OptionParser(usage=usage)
00079 parser.add_option("-a", "--all", action="store_true", dest="save_all",
00080 help="save all", default=True)
00081 parser.add_option("-r", "--right_arm", action = "store_true", dest="save_right_arm",
00082 help="Save pose of right arm", default=True)
00083 parser.add_option("-l", "--left_arm", action = "store_true", dest="save_left_arm",
00084 help="Save pose of left arm", default=True)
00085 parser.add_option("-R", "--right-gripper", action = "store_true", dest="save_right_gripper",
00086 help="Save pose of right gripper", default=True)
00087 parser.add_option("-L", "--left-gripper", action = "store_true", dest="save_left_gripper",
00088 help="Save pose of right gripper", default=True)
00089 parser.add_option("-H", "--head", action = "store_true", dest="save_head",
00090 help="Save pose of head", default=True)
00091 parser.add_option("-t", "--torso", action = "store_true", dest="save_torso",
00092 help="Save pose of torso", default=True)
00093 parser.add_option("-T", "--tilting-laser", action = "store_true", dest="save_laser",
00094 help="Save pose of tilting laser", default=True)
00095 (options, args) = parser.parse_args()
00096
00097 rospy.init_node('bookmarker_listener', anonymous=True)
00098
00099 filename = ""
00100 if len(args) == 1:
00101 if args[0].find(".pps") > 0:
00102 filename = args[0]
00103 else:
00104 filename = args[0] + ".pps"
00105 else:
00106 datestr="%s_%s_%s-%s:%s:%s" % (time.localtime().tm_mon,time.localtime().tm_mday,time.localtime().tm_year,
00107 time.localtime().tm_hour, time.localtime().tm_min,time.localtime().tm_sec)
00108 filename = "%s.pps" % datestr
00109
00110 f=open(filename, "w")
00111
00112
00113 if options.save_right_arm or options.save_all:
00114 save_arm_pose_to_file(f, 'r')
00115 if options.save_left_arm or options.save_all:
00116 save_arm_pose_to_file(f, 'l')
00117 if options.save_right_gripper or options.save_all:
00118 save_gripper_angle_to_file(f, 'r')
00119 if options.save_left_gripper or options.save_all:
00120 save_gripper_angle_to_file(f, 'l')
00121 if options.save_head or options.save_all:
00122 save_head_pose_to_file(f)
00123 if options.save_torso or options.save_all:
00124 save_torso_position_to_file(f)
00125
00126 f.close()
