$search
00001 #!/usr/bin/env python 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 # Implies a sine sweep 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