laser_traj_cmd.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright (c) 2009, Willow Garage, Inc.
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #     * Redistributions of source code must retain the above copyright
00010 #       notice, this list of conditions and the following disclaimer.
00011 #     * Redistributions in binary form must reproduce the above copyright
00012 #       notice, this list of conditions and the following disclaimer in the
00013 #       documentation and/or other materials provided with the distribution.
00014 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00015 #       contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 ##\author Kevin Watts
00031 ##\brief Modified from pr2_mechanism_controllers/send_laser_traj_cmd_ms2 by Vijay Pradeep
00032 
00033 
00034 PKG = "life_test"
00035 
00036 import roslib; roslib.load_manifest(PKG) 
00037 
00038 import sys
00039 import os
00040 import string
00041 
00042 import rospy
00043 from std_msgs import *
00044 
00045 from pr2_msgs.msg import LaserTrajCmd
00046 from pr2_msgs.srv import *
00047 from time import sleep
00048 
00049 def print_usage(exit_code = 0):
00050     print '''Usage:
00051     laser_traj_cmd.py [controller]
00052 '''
00053     sys.exit(exit_code)
00054 
00055 if __name__ == '__main__':
00056     if len(sys.argv) < 2:
00057         print_usage()
00058 
00059     rospy.init_node('laser_tilt_commander', anonymous=True)
00060 
00061     cmd = LaserTrajCmd()
00062     controller   =    sys.argv[1]
00063     cmd.header.stamp = rospy.get_rostime()
00064     cmd.profile  = "linear"
00065     #cmd.pos      = [1.0, .26, -.26, -.7,   -.7,   -.26,   .26,   1.0, 1.0]
00066     d = .025
00067     #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]
00068     
00069     cmd.position = [1.5,  -.7, 1.5]
00070     cmd.time_from_start = [0.0, 0.5, 1.0]
00071     cmd.time_from_start = [rospy.Time.from_sec(x) for x in cmd.time_from_start]
00072     cmd.max_velocity = 5
00073     cmd.max_acceleration =  5
00074 
00075     print 'Sending Command to %s: ' % controller
00076     print '  Profile Type: %s' % cmd.profile
00077     print '  Pos: %s ' % ','.join(['%.3f' % x for x in cmd.position])
00078     print '  Time: %s' % ','.join(['%.3f' % x.to_sec() for x in cmd.time_from_start])
00079     print '  MaxRate: %f' % cmd.max_velocity
00080     print '  MaxAccel: %f' % cmd.max_acceleration
00081 
00082     rospy.wait_for_service(controller + '/set_traj_cmd')                                        
00083     s = rospy.ServiceProxy(controller + '/set_traj_cmd', SetLaserTrajCmd)
00084     resp = s.call(SetLaserTrajCmdRequest(cmd))
00085 
00086     print 'Command sent!'
00087     print '  Response: %f' % resp.start_time.to_sec()
00088 
00089     rospy.spin()
00090 
00091 
00092     
00093 
00094 


life_test
Author(s): Kevin Watts
autogenerated on Sat Dec 28 2013 17:56:37