$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_msgs.msg import PeriodicCmd 00015 from time import sleep 00016 00017 def print_usage(exit_code = 0): 00018 print '''Usage: 00019 send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset] 00020 - [profile] - Possible options are linear or linear_blended 00021 - [period] - Time for one entire cycle to execute (in seconds) 00022 - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller) 00023 - [offset] - Constant cmd to add to profile (offset=0 results in profile centered around 0) 00024 ''' 00025 sys.exit(exit_code) 00026 00027 if __name__ == '__main__': 00028 rospy.init_node('periodic_cmd_commander', sys.argv, anonymous=True) 00029 00030 if len(sys.argv) != 6: 00031 print_usage() 00032 00033 cmd = PeriodicCmd() 00034 controller = sys.argv[1] 00035 cmd.header = rospy.Header(None, None, None) 00036 cmd.profile = sys.argv[2] 00037 cmd.period = float (sys.argv[3]) 00038 cmd.amplitude = float (sys.argv[4]) 00039 cmd.offset = float (sys.argv[5]) 00040 00041 print 'Sending Command to %s: ' % controller 00042 print ' Profile Type: %s' % cmd.profile 00043 print ' Period: %f Seconds' % cmd.period 00044 print ' Amplitude: %f Radians' % cmd.amplitude 00045 print ' Offset: %f Radians' % cmd.offset 00046 00047 command_publisher = rospy.Publisher(controller + '/set_periodic_cmd', PeriodicCmd) 00048 sleep(1) 00049 command_publisher.publish( cmd ) 00050 00051 sleep(1) 00052 00053 print 'Command sent!'