airhand_command.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2013, Tokyo Opensource Robotics Kyokai Association
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 Tokyo Opensource Robotics Kyokai Association. nor the
00019 #    names of its contributors may be used to endorse or promote products
00020 #    derived 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: Isaac Isao Saito
00036 
00037 import time
00038 import threading
00039 
00040 import rospy
00041 
00042 from abs_hand_command import AbsractHandCommand
00043 
00044 
00045 class AirhandReleaseThread(threading.Thread):
00046     '''
00047     With airhand, to release by blowing air needs to be stopped after certain
00048     amount of time. Usually a few seconds, not too short for the air compressor
00049     to have ample time for a reaction, is a good idea. This thread is used
00050     in order to do so without letting the computer program halt.
00051     '''
00052     def __init__(self, command, sleeptime):
00053         '''
00054         @type command: AirhandCommand
00055         '''
00056         threading.Thread.__init__(self)
00057         self._command = command
00058         self._sleeptime = sleeptime
00059 
00060     def run(self):
00061         time.sleep(self._sleeptime)
00062         self._command.execute(self._command.AIRHAND_KEEP)
00063 
00064 
00065 class AirhandCommand(AbsractHandCommand):
00066     '''dio_writer
00067     Following Command design pattern, this class represents command for
00068     an Airhand of NEXTAGE OPEN.
00069 
00070     As of 2/1/2014, it's only implemented for a right arm (since there's no
00071     testing environment for left arm).
00072     '''
00073     # TODO: Unittest is needed!!
00074 
00075     # For air hands
00076     AIRHAND_DRAWIN = 'drawin'
00077     AIRHAND_KEEP = 'keep'
00078     AIRHAND_RELEASE = 'release'
00079 
00080     ## Might not be necessary. Maybe use only where you have to specify
00081     ## dangerous situation.AIRHAND_KEEP
00082     AIRHAND_DANGER = 'danger'
00083 
00084     def __init__(self, hands, hand, dio_pins):
00085         '''
00086         @see nextage_ros_bridge.command.abs_hand_command.AbsractHandCommand
00087         @type hands: nextage_ros_bridge.base_hands.BaseHands
00088         @type hand: str
00089         @param hand: Side of hand. Variables that are defined in
00090         nextage_ros_bridge.base_hands.BaseHands can be used { HAND_L, HAND_R }.
00091         '''
00092         super(AirhandCommand, self).__init__(hands, hand, dio_pins)
00093         self._SLEEP_POST_RELEASE = 3.0
00094 
00095     def _assign_dio_names(self, dio_pins):
00096         '''
00097         @see abs_hand_command.AbsractHandCommand._assign_dio_names
00098         '''
00099         #DIO reassignment for the class-specific purpose
00100         self._DIO_SUCTION_L_1 = dio_pins[0]
00101         self._DIO_SUCTION_L_2 = dio_pins[1]
00102         self._DIO_SUCTION_R_1 = dio_pins[2]
00103         self._DIO_SUCTION_R_2 = dio_pins[3]
00104 
00105     def execute(self, operation):
00106         '''
00107         @see abs_hand_command.AbsractHandCommand.execute
00108         '''
00109         dout = []
00110         mask = []  # Will be filled depending on the side of the hand.
00111         mask_l = [self._DIO_SUCTION_L_1, self._DIO_SUCTION_L_2]
00112         mask_r = [self._DIO_SUCTION_R_1, self._DIO_SUCTION_R_2]
00113 
00114         # Set masking value per hand.
00115         if self._hands.HAND_L == self._hand:
00116             mask = mask_l
00117         elif self._hands.HAND_R == self._hand:
00118             mask = mask_r
00119         else:
00120             raise RuntimeError('Make sure _hands object, _hand value are set.')
00121 
00122         if self.AIRHAND_DRAWIN == operation:
00123             if self._hands.HAND_L == self._hand:
00124                 dout = [self._DIO_SUCTION_L_1]
00125             elif self._hands.HAND_R == self._hand:
00126                 dout = [self._DIO_SUCTION_R_1]
00127         elif self.AIRHAND_KEEP == operation:
00128             if self._hands.HAND_L == self._hand:
00129                 pass  # Do nothing since off for both pins.
00130             elif self._hands.HAND_R == self._hand:
00131                 pass  # Do nothing since off for both pins.
00132         elif self.AIRHAND_RELEASE == operation:
00133             if self._hands.HAND_L == self._hand:
00134                 dout = [self._DIO_SUCTION_L_2]
00135             elif self._hands.HAND_R == self._hand:
00136                 dout = [self._DIO_SUCTION_R_2]
00137 
00138             # Create a thread to do KEEP action after the specified amount
00139             # of time without stopping the program.
00140             thread = AirhandReleaseThread(self, self._SLEEP_POST_RELEASE)
00141             thread.start()
00142         else:
00143             # TODO: Might want to thrown exception?
00144             rospy.logwarn('No gripper specified. Do nothing.')
00145             return
00146         return self._hands._dio_writer(dout, mask)


nextage_ros_bridge
Author(s): Isaac Isao Saito , Akio Ochiai
autogenerated on Fri Aug 28 2015 12:55:59