send_periodic_cmd_srv.py

Go to the documentation of this file.

#!/usr/bin/env python
 
PKG = "pr2_mechanism_controllers"
 
import roslib; roslib.load_manifest(PKG) 
 
import sys
import os
import string
 
import rospy
from std_msgs import *
 
from pr2_msgs.msg import PeriodicCmd
from pr2_msgs.srv import *
from time import sleep
 
def print_usage(exit_code = 0):
    print('''Usage:
    send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset]
       - [profile]   - Possible options are linear or linear_blended
       - [period]    - Time for one entire cycle to execute (in seconds)
       - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller)
       - [offset]    - Constant cmd to add to profile (offset=0 results in profile centered around 0)
''')
    sys.exit(exit_code)
 
if __name__ == '__main__':
    if len(sys.argv) < 6:
        print_usage()
 
    cmd = PeriodicCmd()
    controller =    sys.argv[1]
    cmd.header =    rospy.Header(None, None, None)
    cmd.profile =   sys.argv[2] 
    cmd.period =    float (sys.argv[3])
    cmd.amplitude = float (sys.argv[4])
    cmd.offset =    float (sys.argv[5])
 
    print('Sending Command to %s: ' % controller)
    print('  Profile Type: %s' % cmd.profile)
    print('  Period:       %f Seconds' % cmd.period)
    print('  Amplitude:    %f Radians' % cmd.amplitude)
    print('  Offset:       %f Radians' % cmd.offset)
 
    rospy.wait_for_service(controller + '/set_periodic_cmd')                                        
    s = rospy.ServiceProxy(controller + '/set_periodic_cmd', SetPeriodicCmd)
    resp = s.call(SetPeriodicCmdRequest(cmd))        
 
    #rospy.init_node('periodic_cmd_commander', anonymous=True)
    #sleep(1)
    #command_publisher.publish( cmd )
 
    #sleep(1)
 
    print('Command sent!')
    print('  Response: %f' % resp.start_time.to_sec())