head_action_client.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2015, 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 Head 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     SingleJointPositionAction,
00042     SingleJointPositionGoal,
00043 )
00044 
00045 import baxter_interface
00046 
00047 from baxter_interface import CHECK_VERSION
00048 
00049 class HeadClient(object):
00050     def __init__(self):
00051         ns = 'robot/head/head_action'
00052         self._client = actionlib.SimpleActionClient(
00053             ns,
00054             SingleJointPositionAction
00055         )
00056         self._goal = SingleJointPositionGoal()
00057 
00058         # Wait 10 Seconds for the head action server to start or exit
00059         if not self._client.wait_for_server(rospy.Duration(10.0)):
00060             rospy.logerr("Exiting - Head Action Server Not Found")
00061             rospy.signal_shutdown("Action Server not found")
00062             sys.exit(1)
00063         self.clear()
00064 
00065     def command(self, position, velocity):
00066         self._goal.position = position
00067         self._goal.max_velocity = velocity
00068         self._client.send_goal(self._goal)
00069 
00070     def stop(self):
00071         self._client.cancel_goal()
00072 
00073     def wait(self, timeout=5.0):
00074         self._client.wait_for_result(timeout=rospy.Duration(timeout))
00075         return self._client.get_result()
00076 
00077     def clear(self):
00078         self._goal = SingleJointPositionGoal()
00079 
00080 def main():
00081     """RSDK Head Example: Action Client
00082 
00083     Demonstrates creating a client of the Head Action Server,
00084     which enables sending commands of standard action type
00085     control_msgs/SingleJointPosition.
00086 
00087     The example will command the head to a position.
00088     Be sure to start Baxter's head_action_server before running this example.
00089     """
00090     arg_fmt = argparse.RawDescriptionHelpFormatter
00091     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00092                                      description=main.__doc__)
00093     parser.parse_args(rospy.myargv()[1:])
00094 
00095     print("Initializing node... ")
00096     rospy.init_node("rsdk_head_action_client")
00097     print("Getting robot state... ")
00098     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00099     print("Enabling robot... ")
00100     rs.enable()
00101     print("Running. Ctrl-c to quit")
00102 
00103     hc = HeadClient()
00104     hc.command(position=0.0, velocity=1.0)
00105     hc.wait()
00106     hc.command(position=1.57, velocity=0.1)
00107     hc.wait()
00108     hc.command(position=0.0, velocity=0.8)
00109     hc.wait()
00110     hc.command(position=-1.0, velocity=0.4)
00111     hc.wait()
00112     hc.command(position=0.0, velocity=0.6)
00113     print hc.wait()
00114     print "Exiting - Head Action Test Example Complete"
00115 
00116 if __name__ == "__main__":
00117     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Sat Jun 8 2019 20:16:27