full_arm_holder.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2009, Willow Garage, Inc.
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the name of the Willow Garage nor the names of its
00019 #    contributors may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 ##\author Kevin Watts
00036 ##\brief Raises torso and moves arms of PR2 for counterbalance check
00037 
00038 PKG='pr2_counterbalance_check'
00039 import roslib
00040 roslib.load_manifest(PKG)
00041 import rospy, sys, time
00042 import subprocess
00043 from optparse import OptionParser
00044 from std_msgs.msg import Bool, Float64
00045 
00046 import actionlib
00047 from pr2_controllers_msgs.msg import *
00048 
00049 
00050 class SendMessageOnSubscribe(rospy.SubscribeListener):
00051     def __init__(self, msg):
00052         self.msg = msg
00053         print "Waiting for subscriber..."
00054 
00055     def peer_subscribe(self, topic_name, topic_publish, peer_publish):
00056         peer_publish(self.msg)
00057         time.sleep(0.1)
00058 
00059 
00060 def set_controller(controller, command):
00061     pub = rospy.Publisher(controller + '/command', Float64,
00062                               SendMessageOnSubscribe(Float64(command)))
00063 
00064 def hold_arm(side, pan_angle):
00065     set_controller("%s_wrist_roll_position_controller" % side, float(0.0))
00066     set_controller("%s_wrist_flex_position_controller" % side, float(0.0))
00067     set_controller("%s_forearm_roll_position_controller" % side, float(0.0))
00068     set_controller("%s_elbow_flex_position_controller" % side, float(-0.5))
00069     set_controller("%s_upper_arm_roll_position_controller" % side, float(0.0))
00070     set_controller("%s_shoulder_lift_position_controller" % side, float(0.0))
00071     set_controller("%s_shoulder_pan_position_controller" % side, float(pan_angle))
00072 
00073 def move_torso_up():
00074     client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction)
00075     
00076     goal = SingleJointPositionGoal()
00077     goal.position = 0.25
00078     goal.min_duration = rospy.Duration(2.0)
00079     goal.max_velocity = 1.0
00080 
00081     rospy.loginfo("Moving torso up")
00082     client.send_goal(goal)
00083     client.wait_for_result(rospy.Duration.from_sec(5))
00084     
00085 
00086 
00087 def main():
00088     parser = OptionParser()
00089     parser.add_option('--wait-for', action="store", dest="wait_for_topic", default=None)
00090 
00091     options, args = parser.parse_args(rospy.myargv())
00092 
00093     rospy.init_node('arm_test_holder')
00094     pub_held = rospy.Publisher('arms_held', Bool, latch=True)
00095 
00096 
00097     try:
00098         if options.wait_for_topic:
00099             global result
00100             result = None
00101             def wait_for_topic_cb(msg):
00102                 global result
00103                 result = msg
00104 
00105             rospy.Subscriber(options.wait_for_topic, Bool, wait_for_topic_cb)
00106             started_waiting = rospy.get_time()
00107             
00108             # We might not have receieved any time messages yet
00109             warned_about_not_hearing_anything = False
00110             while not result and not rospy.is_shutdown():
00111                 time.sleep(0.01)
00112                 if not warned_about_not_hearing_anything:
00113                     if rospy.get_time() - started_waiting > 10.0:
00114                         warned_about_not_hearing_anything = True
00115                         rospy.logwarn("Full arm holder hasn't heard anything from its \"wait for\" topic (%s)" % \
00116                                          options.wait_for_topic)
00117             if result:
00118                 while not result.data and not rospy.is_shutdown():
00119                     time.sleep(0.01)
00120 
00121         if not rospy.is_shutdown():
00122             rospy.loginfo('Raising torso')
00123             move_torso_up()
00124             
00125         if not rospy.is_shutdown():
00126             rospy.loginfo('Holding arms')
00127             # Hold both arms in place
00128             hold_arm('r', -1.2)
00129             hold_arm('l', 1.2)
00130             time.sleep(1.5)
00131         
00132         if not rospy.is_shutdown():
00133             pub_held.publish(True)
00134 
00135         rospy.spin()
00136     except KeyboardInterrupt:
00137         pass
00138     except Exception, e:
00139         import traceback
00140         traceback.print_exc()
00141 
00142 if __name__ == '__main__':
00143     main()


pr2_counterbalance_check
Author(s): Kevin Watts
autogenerated on Tue Apr 22 2014 19:40:12