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 import os.path
00037
00038 from geometry_msgs.msg import Twist, Pose, PoseStamped, TwistStamped
00039 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00040 from pr2_controllers_msgs.msg import *
00041
00042 verbose = False
00043 duration = 3.0
00044
00045 class pr2_pose_loader:
00046 def __init__(self, verbose = False):
00047 """ @package pr2_pose_saver : load
00048 init: defines goal state variables and opens publishers and clients
00049 """
00050 self.verbose = verbose
00051
00052
00053
00054 self._base_pub = rospy.Publisher('base_controller/command', Twist)
00055
00056
00057 self._l_gripper_pub = rospy.Publisher('l_gripper_controller/command', Pr2GripperCommand)
00058 self._r_gripper_pub = rospy.Publisher('r_gripper_controller/command', Pr2GripperCommand)
00059
00060
00061 self._torso_pub = rospy.Publisher('/torso_controller/command', JointTrajectory)
00062
00063
00064 self._head_pub = rospy.Publisher('/head_traj_controller/command', JointTrajectory)
00065
00066
00067 self._r_arm_pub = rospy.Publisher("r_arm_controller/command", JointTrajectory, latch=True)
00068 self._l_arm_pub = rospy.Publisher("l_arm_controller/command", JointTrajectory, latch=True)
00069
00070
00071 self.publishers = {'base':self._base_pub,
00072 'l_gripper':self._l_gripper_pub,
00073 'r_gripper':self._r_gripper_pub,
00074 'torso':self._torso_pub,
00075 'head':self._head_pub,
00076 'r_arm':self._r_arm_pub,
00077 'l_arm':self._l_arm_pub,
00078 'r_gripper':self._r_gripper_pub,
00079 'l_gripper':self._l_gripper_pub
00080 }
00081 rospy.sleep(0.5)
00082
00083 def set_arm_state(self, jvals, arm):
00084 """ Sets goal for indicated arm (r_arm/l_arm) using provided joint values"""
00085
00086 command = JointTrajectory()
00087 command.joint_names = ['%s_shoulder_pan_joint' % arm[0],
00088 '%s_shoulder_lift_joint' % arm[0],
00089 '%s_upper_arm_roll_joint' % arm[0],
00090 '%s_elbow_flex_joint' % arm[0],
00091 '%s_forearm_roll_joint' % arm[0],
00092 '%s_wrist_flex_joint' % arm[0],
00093 '%s_wrist_roll_joint' % arm[0]]
00094 command.points.append(JointTrajectoryPoint(
00095 positions=jvals,
00096 velocities = [0.0] * (len(command.joint_names)),
00097 accelerations = [],
00098 time_from_start = rospy.Duration(duration)))
00099 command.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
00100
00101 try:
00102 self.publishers[arm].publish(command)
00103 if self.verbose:
00104 print "published [%s] to %s_controller/command topic" % (jvals, arm)
00105 except:
00106 print "failed to publish arm positions!"
00107
00108 def set_gripper_state(self, jval, gripper):
00109 """ Sets goal for indicated gripper (r_gripper/l_gripper) using provided joint angle"""
00110
00111 goal = Pr2GripperCommand()
00112 goal.max_effort = -1
00113 goal.position = jval
00114 try:
00115 self.publishers[gripper].publish(goal)
00116 if self.verbose:
00117 print "published [%s] to %s_controller/command topic" % (jval, gripper)
00118 except:
00119 print "failed to publish gripper positions!"
00120
00121 def set_head_state(self, jvals):
00122 """ Sets goal for head using provided joint values"""
00123
00124 head_goal = JointTrajectory()
00125 head_goal.joint_names.append('head_pan_joint')
00126 head_goal.joint_names.append('head_tilt_joint')
00127 head_goal.points.append(JointTrajectoryPoint())
00128 head_goal.points[0].time_from_start = rospy.Duration(0.25)
00129
00130 for i in range(len(head_goal.joint_names)):
00131 head_goal.points[0].positions.append(jvals[i])
00132 head_goal.points[0].velocities.append(1)
00133 head_goal.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
00134 try:
00135
00136 self.publishers["head"].publish(head_goal)
00137 if self.verbose:
00138 print "published [%s] to head_traj_controller/command topic" % jvals
00139 except:
00140 print "failed to publish head position!"
00141
00142 def set_torso_state(self, jval):
00143 """ Sets goal for torso using provided value"""
00144
00145 torso_goal = JointTrajectory()
00146 torso_goal.joint_names.append('torso_lift_joint')
00147 torso_goal.points.append(JointTrajectoryPoint())
00148 torso_goal.points[0].time_from_start = rospy.Duration(0.25)
00149 torso_goal.points[0].velocities.append(0)
00150 torso_goal.points[0].positions.append(jval)
00151 torso_goal.header.stamp = rospy.get_rostime() + rospy.Duration(0.01)
00152 try:
00153
00154 self.publishers["torso"].publish(torso_goal)
00155 if self.verbose:
00156 print "published [%s] to torso_controller/command topic" % jval
00157 except:
00158 print "failed to publish torso position!"
00159
00160 def parse_bookmark_file(self, bfile):
00161 f=open(bfile,'r')
00162
00163 for l in f.readlines():
00164 if l.find("arm") != -1:
00165 jvals_str = l.split(":")[1]
00166 jvals = map(lambda x: float(x),jvals_str.strip("\n").split(","))
00167 arm = l.split(":")[0]
00168 self.set_arm_state(jvals, arm)
00169 if l.find("gripper") != -1:
00170 jval_str = l.split(":")[1]
00171 jval = map(lambda x: float(x),jval_str.strip("\n").split(","))[0]
00172 gripper = l.split(":")[0]
00173 self.set_gripper_state(jval, gripper)
00174 if l.find("head") != -1:
00175 jvals_str = l.split(":")[1]
00176 jvals = map(lambda x: float(x),jvals_str.strip("\n").split(","))
00177 self.set_head_state(jvals)
00178 if l.find("torso") != -1:
00179 jvals_str = l.split(":")[1]
00180 jval = map(lambda x: float(x),jvals_str.strip("\n").split(","))[0]
00181 self.set_torso_state(jval)
00182
00183 f.close()
00184
00185 if __name__ == "__main__":
00186 from optparse import OptionParser
00187 parser = OptionParser()
00188 parser.add_option("-v", "--verbose", action="store_true", dest="verbose", default=False,
00189 help="verbose mode")
00190 parser.add_option("-a", "--all", action="store_true", dest="load_all",
00191 help="load all", default=True)
00192 parser.add_option("-r", "--right_arm", action = "store_true", dest="load_right_arm",
00193 help="Load pose of right arm", default=True)
00194 parser.add_option("-l", "--left_arm", action = "store_true", dest="load_left_arm",
00195 help="Load pose of left arm", default=True)
00196 parser.add_option("-R", "--right-gripper", action = "store_true", dest="load_right_gripper",
00197 help="Save pose of right gripper", default=True)
00198 parser.add_option("-L", "--left-gripper", action = "store_true", dest="load_left_gripper",
00199 help="Save pose of right gripper", default=True)
00200 parser.add_option("-H", "--head", action = "store_true", dest="load_head",
00201 help="Load pose of head", default=True)
00202 parser.add_option("-t", "--torso", action = "store_true", dest="load_torso",
00203 help="Load pose of torso", default=True)
00204 parser.add_option("-T", "--tilting-aser", action = "store_true", dest="load_laser",
00205 help="Load pose of tilting laser", default=True)
00206
00207 parser.add_option("-s", "--seconds", action="store", type="float", dest="duration",
00208 help="Input number of seconds to take to achieve goal")
00209 parser.add_option("-b", "--completion-topic", action="store", type="string", dest="completion_topic",
00210 help="Topic on which to broadcast message upon completion (not necessarily success)")
00211 parser.add_option("-w", "--wait", action="store", type="float", dest="wait",
00212 help="Time in seconds to wait before broadcasting completion message")
00213
00214 (options, args) = parser.parse_args()
00215 verbose = options.verbose
00216 if options.duration:
00217 duration = options.duration
00218
00219 if len(args) == 0:
00220 print "Error: no bookmark file provided"
00221 print "See load.py --help for instructions"
00222 sys.exit()
00223 else:
00224 bookmark_file = args[0]
00225
00226 if not os.path.exists(bookmark_file):
00227 print "Error: could not locate provided bookmark file"
00228 sys.exit()
00229
00230 rospy.init_node('bookmarker_publisher', anonymous=True)
00231
00232 loader = pr2_pose_loader(options.verbose)
00233 loader.parse_bookmark_file(bookmark_file)
00234
00235 if options.completion_topic:
00236 if options.wait:
00237 rospy.loginfo('Messages sent. Waiting %s seconds', options.wait)
00238 rospy.sleep(options.wait)
00239 msg = rospy.Header()
00240 msg.stamp = rospy.Time.now()
00241 pub = rospy.Publisher(options.completion_topic, rospy.Header)
00242 rospy.loginfo('Done; broadcasting success on %s', options.completion_topic)
00243 while not rospy.is_shutdown():
00244 pub.publish(msg)
00245 rospy.sleep(1)
00246
00247
00248
