digital_io_blink.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 import argparse
00031 
00032 import rospy
00033 
00034 import baxter_interface.digital_io as DIO
00035 
00036 
00037 def test_interface(io_component='left_itb_light_outer'):
00038     """Blinks a Digital Output on then off."""
00039     rospy.loginfo("Blinking Digital Output: %s", io_component)
00040     b = DIO.DigitalIO(io_component)
00041 
00042     print "Initial state: ", b.state
00043 
00044     # turn on light
00045     b.set_output(True)
00046     rospy.sleep(1)
00047     print "New state: ", b.state
00048 
00049     # reset output
00050     b.set_output(False)
00051     rospy.sleep(1)
00052     print "Final state:", b.state
00053 
00054 
00055 def main():
00056     """RSDK Digital IO Example: Blink
00057 
00058     Turns the output of a DigitalIO component on then off again
00059     while printing the state at each step. Simple demonstration
00060     of using the baxter_interface.DigitalIO class.
00061 
00062     Run this example with default arguments and watch the light
00063     on the left arm Navigator blink on and off while the console
00064     echos the state. Use the component_id argument or ROS Parameter
00065     to change the DigitalIO component used.
00066     """
00067     epilog = """
00068 ROS Parameters:
00069   ~component_id        - name of DigitalIO component to use
00070 
00071 Baxter DigitalIO
00072     Note that 'DigitalIO' components are only those that use
00073     the custom ROS Messages baxter_core_msgs/DigitalIOState
00074     and baxter_core_msgs/DigitalOutputCommand.
00075 
00076     Component names can be found on the Wiki, or by echoing
00077     the names field of the digital_io_states topic:
00078       $ rostopic echo -n 1 /robot/digital_io_states/names
00079     """
00080     arg_fmt = argparse.RawDescriptionHelpFormatter
00081     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00082                                      description=main.__doc__,
00083                                      epilog=epilog)
00084     parser.add_argument(
00085         '-c', '--component', dest='component_id',
00086         default='left_itb_light_outer',
00087         help=('name of Digital IO component to use'
00088               ' (default: left_itb_light_outer)')
00089     )
00090     args = parser.parse_args(rospy.myargv()[1:])
00091 
00092     rospy.init_node('rsdk_digital_io_blink', anonymous=True)
00093     io_component = rospy.get_param('~component_id', args.component_id)
00094     test_interface(io_component)
00095 
00096 if __name__ == '__main__':
00097     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Thu Aug 27 2015 12:31:14