tuck_arms_test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 
00035 """
00036 usage: tuck_arms.py [-l ACTION] [-r ACTION]
00037 
00038 Options:
00039   -l or --left   Action for left arm
00040   -r or --right  Action for right arm
00041 
00042 Actions:
00043   t or tuck
00044   u or untuck
00045 
00046 NB: If action unspecified for an arm, defaults to tuck
00047 
00048 """
00049 
00050 
00051 import getopt
00052 
00053 import rospy
00054 
00055 import actionlib
00056 
00057 from pr2_common_action_msgs.msg import TuckArmsAction, TuckArmsGoal
00058 
00059 def main():
00060     action_name = 'tuck_arms'
00061 
00062     # check for command line arguments, and send goal to action server if required
00063     myargs = rospy.myargv()[1:]
00064     if len(myargs):
00065         goal = TuckArmsGoal()
00066         goal.tuck_left = True
00067         goal.tuck_right = True
00068         opts, args = getopt.getopt(myargs, 'hql:r:', ['quit','left','right'])
00069         for arm, action in opts:
00070             if arm in ('-l', '--left'):
00071                 if action in ('tuck', 't'):
00072                     goal.tuck_left = True
00073                 elif action in ('untuck', 'u'):
00074                     goal.tuck_left = False
00075                 else:
00076                     rospy.logerr('Invalid action for right arm: %s'%action)
00077                     rospy.signal_shutdown("ERROR")
00078 
00079             if arm in ('-r', '--right'):
00080                 if action in ('tuck', 't'):
00081                     goal.tuck_right = True
00082                 elif action in ('untuck', 'u'):
00083                     goal.tuck_right = False
00084                 else:
00085                     rospy.logerr('Invalid action for left arm: %s'%action)
00086                     rospy.signal_shutdown("ERROR")
00087 
00088             if arm in ('--help', '-h'):
00089                 print __doc__ % vars()
00090     
00091     tuck_arm_client = actionlib.SimpleActionClient(action_name, TuckArmsAction)
00092     rospy.logdebug('Waiting for action server to start')
00093     tuck_arm_client.wait_for_server(rospy.Duration(10.0))
00094     rospy.logdebug('Sending goal to action server')
00095     tuck_arm_client.send_goal_and_wait(goal, rospy.Duration(30.0), rospy.Duration(5.0))
00096 
00097 if __name__ == "__main__":
00098     rospy.init_node('tuck_amrs_test')
00099     main()


pr2_tuck_arms_action
Author(s): Wim Meeussen
autogenerated on Thu Jun 6 2019 17:52:32