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 LaserTrajCmd
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]
00021 '''
00022 sys.exit(exit_code)
00023
00024 if __name__ == '__main__':
00025 if len(sys.argv) < 2:
00026 print_usage()
00027
00028 cmd = LaserTrajCmd()
00029 controller = sys.argv[1]
00030 cmd.header = rospy.Header(None, None, None)
00031 cmd.profile = "blended_linear"
00032
00033 d = .025
00034
00035
00036 cmd.position = [1.2, -.7, 1.2]
00037 cmd.time_from_start = [0.0, 1.8, 2.025]
00038 cmd.time_from_start = [rospy.Duration.from_sec(x) for x in cmd.time_from_start]
00039
00040 cmd.max_velocity = 5
00041 cmd.max_acceleration = 5
00042
00043 print 'Sending Command to %s: ' % controller
00044 print ' Profile Type: %s' % cmd.profile
00045 print ' Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position])
00046 print ' Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])
00047 print ' MaxVelocity: %f' % cmd.max_velocity
00048 print ' MaxAcceleration: %f' % cmd.max_acceleration
00049
00050 rospy.wait_for_service(controller + '/set_traj_cmd')
00051 s = rospy.ServiceProxy(controller + '/set_traj_cmd', SetLaserTrajCmd)
00052 resp = s.call(SetLaserTrajCmdRequest(cmd))
00053
00054 print 'Command sent!'
00055 print ' Resposne: %f' % resp.start_time.to_seconds()