Go to the documentation of this file.00001
00002
00003 PKG = "pr2_mechanism_controllers"
00004
00005 import roslib; roslib.load_manifest(PKG)
00006
00007 import sys
00008 import os
00009 import string
00010
00011 import rospy
00012 from std_msgs import *
00013
00014 from pr2_msgs.msg import PeriodicCmd
00015 from pr2_msgs.srv import *
00016 from time import sleep
00017
00018 def print_usage(exit_code = 0):
00019 print '''Usage:
00020 send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset]
00021 - [profile] - Possible options are linear or linear_blended
00022 - [period] - Time for one entire cycle to execute (in seconds)
00023 - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller)
00024 - [offset] - Constant cmd to add to profile (offset=0 results in profile centered around 0)
00025 '''
00026 sys.exit(exit_code)
00027
00028 if __name__ == '__main__':
00029 if len(sys.argv) < 6:
00030 print_usage()
00031
00032 cmd = PeriodicCmd()
00033 controller = sys.argv[1]
00034 cmd.header = rospy.Header(None, None, None)
00035 cmd.profile = sys.argv[2]
00036 cmd.period = float (sys.argv[3])
00037 cmd.amplitude = float (sys.argv[4])
00038 cmd.offset = float (sys.argv[5])
00039
00040 print 'Sending Command to %s: ' % controller
00041 print ' Profile Type: %s' % cmd.profile
00042 print ' Period: %f Seconds' % cmd.period
00043 print ' Amplitude: %f Radians' % cmd.amplitude
00044 print ' Offset: %f Radians' % cmd.offset
00045
00046 rospy.wait_for_service(controller + '/set_periodic_cmd')
00047 s = rospy.ServiceProxy(controller + '/set_periodic_cmd', SetPeriodicCmd)
00048 resp = s.call(SetPeriodicCmdRequest(cmd))
00049
00050
00051
00052
00053
00054
00055
00056 print 'Command sent!'
00057 print ' Response: %f' % resp.start_time.to_sec()