$search
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 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 #cmd.pos = [1.0, .26, -.26, -.7, -.7, -.26, .26, 1.0, 1.0] 00033 d = .025 00034 #cmd.time = [0.0, 0.4, 1.0, 1.1, 1.1+d, 1.2+d, 1.8+d, 2.2+d, 2.2+2*d] 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()