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
00071 cmd.time_from_start = [0.0, 4.0, 8.0]
00072 cmd.time_from_start = [rospy.Time.from_sec(x) for x in cmd.time_from_start]
00073 cmd.max_velocity = 5
00074 cmd.max_acceleration = 5
00075
00076 print 'Sending Command to %s: ' % controller
00077 print ' Profile Type: %s' % cmd.profile
00078 print ' Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position])
00079 print ' Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])
00080 print ' MaxRate: %f' % cmd.max_velocity
00081 print ' MaxAccel: %f' % cmd.max_acceleration
00082
00083 rospy.wait_for_service(controller + '/set_traj_cmd')
00084 s = rospy.ServiceProxy(controller + '/set_traj_cmd', SetLaserTrajCmd)
00085 resp = s.call(SetLaserTrajCmdRequest(cmd))
00086
00087 print 'Command sent!'
00088 print ' Response: %f' % resp.start_time.to_sec()
00089
00090 rospy.spin()
00091
00092
00093
00094
00095