head_wobbler.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 import argparse
00031 import random
00032 
00033 import rospy
00034 
00035 import baxter_interface
00036 
00037 from baxter_interface import CHECK_VERSION
00038 
00039 
00040 class Wobbler(object):
00041 
00042     def __init__(self):
00043         """
00044         'Wobbles' the head
00045         """
00046         self._done = False
00047         self._head = baxter_interface.Head()
00048 
00049         # verify robot is enabled
00050         print("Getting robot state... ")
00051         self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
00052         self._init_state = self._rs.state().enabled
00053         print("Enabling robot... ")
00054         self._rs.enable()
00055         print("Running. Ctrl-c to quit")
00056 
00057     def clean_shutdown(self):
00058         """
00059         Exits example cleanly by moving head to neutral position and
00060         maintaining start state
00061         """
00062         print("\nExiting example...")
00063         if self._done:
00064             self.set_neutral()
00065         if not self._init_state and self._rs.state().enabled:
00066             print("Disabling robot...")
00067             self._rs.disable()
00068 
00069     def set_neutral(self):
00070         """
00071         Sets the head back into a neutral pose
00072         """
00073         self._head.set_pan(0.0)
00074 
00075     def wobble(self):
00076         self.set_neutral()
00077         """
00078         Performs the wobbling
00079         """
00080         self._head.command_nod()
00081         command_rate = rospy.Rate(1)
00082         control_rate = rospy.Rate(100)
00083         start = rospy.get_time()
00084         while not rospy.is_shutdown() and (rospy.get_time() - start < 10.0):
00085             angle = random.uniform(-1.5, 1.5)
00086             while (not rospy.is_shutdown() and
00087                    not (abs(self._head.pan() - angle) <=
00088                        baxter_interface.HEAD_PAN_ANGLE_TOLERANCE)):
00089                 self._head.set_pan(angle, speed=30, timeout=0)
00090                 control_rate.sleep()
00091             command_rate.sleep()
00092 
00093         self._done = True
00094         rospy.signal_shutdown("Example finished.")
00095 
00096 
00097 def main():
00098     """RSDK Head Example: Wobbler
00099 
00100     Nods the head and pans side-to-side towards random angles.
00101     Demonstrates the use of the baxter_interface.Head class.
00102     """
00103     arg_fmt = argparse.RawDescriptionHelpFormatter
00104     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00105                                      description=main.__doc__)
00106     parser.parse_args(rospy.myargv()[1:])
00107 
00108     print("Initializing node... ")
00109     rospy.init_node("rsdk_head_wobbler")
00110 
00111     wobbler = Wobbler()
00112     rospy.on_shutdown(wobbler.clean_shutdown)
00113     print("Wobbling... ")
00114     wobbler.wobble()
00115     print("Done.")
00116 
00117 if __name__ == '__main__':
00118     main()


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