send_periodic_cmd_srv.py
Go to the documentation of this file.
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 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     #rospy.init_node('periodic_cmd_commander', anonymous=True)
00051     #sleep(1)
00052     #command_publisher.publish( cmd )
00053 
00054     #sleep(1)
00055 
00056     print 'Command sent!'
00057     print '  Response: %f' % resp.start_time.to_sec()


pr2_mechanism_controllers
Author(s): Sachin Chita, John Hsu, Melonee Wise
autogenerated on Thu Aug 27 2015 14:22:52