Go to the documentation of this file.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
00032
00033
00034 PKG = "life_test"
00035
00036 import roslib; roslib.load_manifest(PKG)
00037
00038 import sys
00039 import os
00040 import string
00041
00042 import rospy
00043 from std_msgs import *
00044
00045 from pr2_msgs.msg import LaserTrajCmd
00046 from pr2_msgs.srv import *
00047 from time import sleep
00048
00049 def print_usage(exit_code = 0):
00050 print '''Usage:
00051 laser_traj_cmd.py [controller]
00052 '''
00053 sys.exit(exit_code)
00054
00055 if __name__ == '__main__':
00056 if len(sys.argv) < 2:
00057 print_usage()
00058
00059 rospy.init_node('laser_tilt_commander', anonymous=True)
00060
00061 cmd = LaserTrajCmd()
00062 controller = sys.argv[1]
00063 cmd.header.stamp = rospy.get_rostime()
00064 cmd.profile = "linear"
00065
00066 d = .025
00067
00068
00069 cmd.position = [1.5, -.7, 1.5]
00070 cmd.time_from_start = [0.0, 0.5, 1.0]
00071 cmd.time_from_start = [rospy.Time.from_sec(x) for x in cmd.time_from_start]
00072 cmd.max_velocity = 5
00073 cmd.max_acceleration = 5
00074
00075 print 'Sending Command to %s: ' % controller
00076 print ' Profile Type: %s' % cmd.profile
00077 print ' Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position])
00078 print ' Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])
00079 print ' MaxRate: %f' % cmd.max_velocity
00080 print ' MaxAccel: %f' % cmd.max_acceleration
00081
00082 rospy.wait_for_service(controller + '/set_traj_cmd')
00083 s = rospy.ServiceProxy(controller + '/set_traj_cmd', SetLaserTrajCmd)
00084 resp = s.call(SetLaserTrajCmdRequest(cmd))
00085
00086 print 'Command sent!'
00087 print ' Response: %f' % resp.start_time.to_sec()
00088
00089 rospy.spin()
00090
00091
00092
00093
00094