Go to the documentation of this file.00001
00002
00003 """
00004 simple_arm_server_test.py - tests the simple_arm_server.py program
00005 Copyright (c) 2011 Michael Ferguson. All right reserved.
00006
00007 Redistribution and use in source and binary forms, with or without
00008 modification, are permitted provided that the following conditions are met:
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 copyright holders nor the names of its
00015 contributors may be used to endorse or promote products derived
00016 from this software without specific prior written permission.
00017
00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00019 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00020 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
00022 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00023 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00024 OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00025 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00026 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00027 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 """
00029
00030 usage_= "usage: simple_arm_server_test.py x y z wrist_pitch [wrist_roll=0.0 wrist_yaw=0.0 frame_id='base_link' duration=5.0]"
00031
00032 import roslib; roslib.load_manifest('simple_arm_server')
00033 import rospy, actionlib
00034 import sys
00035
00036 import tf
00037 from tf.transformations import euler_from_quaternion, quaternion_from_euler
00038 from simple_arm_server.msg import *
00039
00040 if __name__ == '__main__':
00041 if len(sys.argv) > 4:
00042 rospy.init_node('simple_arm_server_test')
00043
00044 client = actionlib.SimpleActionClient('move_arm', MoveArmAction)
00045 client.wait_for_server()
00046
00047 goal = MoveArmGoal()
00048 goal.header.frame_id = "base_link"
00049 if len(sys.argv) > 7:
00050 goal.header.frame_id = sys.argv[7]
00051
00052 action = ArmAction()
00053 action.type = ArmAction.MOVE_ARM
00054 action.goal.position.x = float(sys.argv[1])
00055 action.goal.position.y = float(sys.argv[2])
00056 action.goal.position.z = float(sys.argv[3])
00057
00058 roll = 0.0
00059 if len(sys.argv) > 5:
00060 roll = float(sys.argv[5])
00061 yaw = 0.0
00062 if len(sys.argv) > 6:
00063 yaw = float(sys.argv[6])
00064 q = quaternion_from_euler(roll, float(sys.argv[4]), yaw, 'sxyz')
00065 action.goal.orientation.x = q[0]
00066 action.goal.orientation.y = q[1]
00067 action.goal.orientation.z = q[2]
00068 action.goal.orientation.w = q[3]
00069
00070 if len(sys.argv) > 8:
00071 action.move_time = rospy.Duration(float(sys.argv[8]))
00072
00073 try:
00074 goal.motions.append(action)
00075 client.send_goal(goal)
00076 client.wait_for_result()
00077 r = client.get_result()
00078 print r
00079 except rospy.ServiceException, e:
00080 print "Service did not process request: %s"%str(e)
00081 else:
00082 print usage_
00083