send_laser_traj_cmd_ms2.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 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()


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