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_mechanism_controllers.srv import *
00015
00016 def print_usage(exit_code = 0):
00017 print '''Usage:
00018 control.py <controller> sine <period> <amplitude> <offset>
00019 - Defines a sine sweep for the laser controller
00020 '''
00021 sys.exit(exit_code)
00022
00023 if __name__ == '__main__':
00024 if len(sys.argv) < 6:
00025 print_usage()
00026 if sys.argv[2] == 'sine' :
00027 profile_type = 4
00028 controller = sys.argv[1]
00029 period = float (sys.argv[3])
00030 amplitude = float (sys.argv[4])
00031 offset = float (sys.argv[5])
00032
00033 print 'Sending Command: '
00034 print ' Profile Type: Sine'
00035 print ' Period: %f Seconds' % period
00036 print ' Amplitude: %f Radians' % amplitude
00037 print ' Offset: %f Radians' % offset
00038
00039 rospy.wait_for_service(controller + '/set_profile')
00040 s = rospy.ServiceProxy(controller + '/set_profile', SetProfile)
00041 resp = s.call(SetProfileRequest(0.0, 0.0, 0.0, 0.0, profile_type, period, amplitude, offset))
00042
00043 print 'Command Sent'
00044 print ' Response: %s' % str(resp.time)
00045 else :
00046 print 'Unknown profile type [%s]' % sys.argv[2]
00047