gripper_action_client.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2014, Rethink Robotics
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 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. 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 # 3. Neither the name of the Rethink Robotics 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 """
00031 Baxter RSDK Gripper Action Client Example
00032 """
00033 import sys
00034 import argparse
00035 
00036 import rospy
00037 
00038 import actionlib
00039 
00040 from control_msgs.msg import (
00041     GripperCommandAction,
00042     GripperCommandGoal,
00043 )
00044 
00045 import baxter_interface
00046 
00047 from baxter_interface import CHECK_VERSION
00048 
00049 
00050 class GripperClient(object):
00051     def __init__(self, gripper):
00052         ns = 'robot/end_effector/' + gripper + '_gripper/'
00053         self._client = actionlib.SimpleActionClient(
00054             ns + "gripper_action",
00055             GripperCommandAction,
00056         )
00057         self._goal = GripperCommandGoal()
00058 
00059         # Wait 10 Seconds for the gripper action server to start or exit
00060         if not self._client.wait_for_server(rospy.Duration(10.0)):
00061             rospy.logerr("Exiting - %s Gripper Action Server Not Found" %
00062                          (gripper.capitalize(),))
00063             rospy.signal_shutdown("Action Server not found")
00064             sys.exit(1)
00065         self.clear()
00066 
00067     def command(self, position, effort):
00068         self._goal.command.position = position
00069         self._goal.command.max_effort = effort
00070         self._client.send_goal(self._goal)
00071 
00072     def stop(self):
00073         self._client.cancel_goal()
00074 
00075     def wait(self, timeout=5.0):
00076         self._client.wait_for_result(timeout=rospy.Duration(timeout))
00077         return self._client.get_result()
00078 
00079     def clear(self):
00080         self._goal = GripperCommandGoal()
00081 
00082 
00083 def main():
00084     """RSDK Gripper Example: Action Client
00085 
00086     Demonstrates creating a client of the Gripper Action Server,
00087     which enables sending commands of standard action type
00088     control_msgs/GripperCommand.
00089 
00090     The example will command the grippers to a number of positions
00091     while specifying moving force or vacuum sensor threshold. Be sure
00092     to start Baxter's gripper_action_server before running this example.
00093     """
00094     arg_fmt = argparse.RawDescriptionHelpFormatter
00095     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00096                                      description=main.__doc__)
00097     parser.add_argument(
00098         '-g', '--gripper', dest='gripper', required=True,
00099         choices=['left', 'right'],
00100         help='which gripper to send action commands'
00101     )
00102     args = parser.parse_args(rospy.myargv()[1:])
00103     gripper = args.gripper
00104 
00105     print("Initializing node... ")
00106     rospy.init_node("rsdk_gripper_action_client_%s" % (gripper,))
00107     print("Getting robot state... ")
00108     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00109     print("Enabling robot... ")
00110     rs.enable()
00111     print("Running. Ctrl-c to quit")
00112 
00113     gc = GripperClient(gripper)
00114     gc.command(position=0.0, effort=50.0)
00115     gc.wait()
00116     gc.command(position=100.0, effort=50.0)
00117     gc.wait()
00118     gc.command(position=25.0, effort=40.0)
00119     gc.wait()
00120     gc.command(position=75.0, effort=20.0)
00121     gc.wait()
00122     gc.command(position=0.0, effort=30.0)
00123     gc.wait()
00124     gc.command(position=100.0, effort=40.0)
00125     print gc.wait()
00126     print "Exiting - Gripper Action Test Example Complete"
00127 
00128 if __name__ == "__main__":
00129     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Fri Oct 3 2014 16:37:39