control_laser.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_mechanism_controllers.srv import *
 
def print_usage(exit_code = 0):
    print('''Usage:
    control.py <controller> sine <period> <amplitude> <offset>
       - Defines a sine sweep for the laser controller
''')
    sys.exit(exit_code)
 
if __name__ == '__main__':
    if len(sys.argv) < 6:
        print_usage()
    if sys.argv[2] == 'sine' :
        profile_type = 4            # Implies a sine sweep
        controller = sys.argv[1]
        period =     float (sys.argv[3])
        amplitude =  float (sys.argv[4])
        offset =     float (sys.argv[5])
 
        print('Sending Command: ')
        print('  Profile Type: Sine')
        print('  Period:       %f Seconds' % period)
        print('  Amplitude:    %f Radians' % amplitude)
        print('  Offset:       %f Radians' % offset)
 
        rospy.wait_for_service(controller + '/set_profile')
        s = rospy.ServiceProxy(controller + '/set_profile', SetProfile)
        resp = s.call(SetProfileRequest(0.0, 0.0, 0.0, 0.0, profile_type, period, amplitude, offset))
        
        print('Command Sent')
        print('  Response: %s' % str(resp.time))
    else :
        print('Unknown profile type [%s]' % sys.argv[2])