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 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!'