Source Code for Module intera_interface.cuff

  1  # Copyright (c) 2013-2016, Rethink Robotics 
  2  # All rights reserved. 
  3  # 
  4  # Redistribution and use in source and binary forms, with or without 
  5  # modification, are permitted provided that the following conditions are met: 
  6  # 
  7  # 1. Redistributions of source code must retain the above copyright notice, 
  8  #    this list of conditions and the following disclaimer. 
  9  # 2. Redistributions in binary form must reproduce the above copyright 
 10  #    notice, this list of conditions and the following disclaimer in the 
 11  #    documentation and/or other materials provided with the distribution. 
 12  # 3. Neither the name of the Rethink Robotics nor the names of its 
 13  #    contributors may be used to endorse or promote products derived from 
 14  #    this software without specific prior written permission. 
 15  # 
 16  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 17  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 18  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 19  # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 20  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 21  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 22  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 23  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 24  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 25  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 26  # POSSIBILITY OF SUCH DAMAGE. 
 27   
 28  import rospy 
 29  import intera_dataflow 
 30  from intera_io import IODeviceInterface 
 31  from intera_core_msgs.msg import IODeviceConfiguration 
 32  from robot_params import RobotParams 
 33   
 34   
 35 -class Cuff(object): 
 36      """ 
 37      Interface class for cuff on the Intera robots. 
 38      """ 
 39   
 40 -    def __init__(self, limb="right"): 
 41          """ 
 42          Constructor. 
 43   
 44          @type limb: str 
 45          @param limb: limb side to interface 
 46          """ 
 47          params = RobotParams() 
 48          limb_names = params.get_limb_names() 
 49          if limb not in limb_names: 
 50              rospy.logerr("Cannot detect Cuff's limb {0} on this robot." 
 51                           " Valid limbs are {1}. Exiting Cuff.init().".format( 
 52                                                              limb, limb_names)) 
 53              return 
 54          self.limb = limb 
 55          self.device = None 
 56          self.name = "cuff" 
 57          self.cuff_config_sub = rospy.Subscriber('/io/robot/cuff/config', IODeviceConfiguration, self._config_callback) 
 58          # Wait for the cuff status to be true 
 59          intera_dataflow.wait_for( 
 60              lambda: not self.device is None, timeout=5.0, 
 61              timeout_msg=("Failed find cuff on limb '{0}'.".format(limb)) 
 62              ) 
 63          self._cuff_io = IODeviceInterface("robot", self.name) 
 64   
 65 -    def _config_callback(self, msg): 
 66          """ 
 67          config topic callback 
 68          """ 
 69          if msg.device != []: 
 70              if str(msg.device.name) == self.name: 
 71                  self.device = msg.device 
 72   
 73 -    def lower_button(self): 
 74          """ 
 75          Returns a boolean describing whether the lower button on cuff is pressed. 
 76   
 77          @rtype: bool 
 78          @return: a variable representing button state: (True: pressed, False: unpressed) 
 79          """ 
 80          return bool(self._cuff_io.get_signal_value('_'.join([self.limb, "button_lower"]))) 
 81   
 82 -    def upper_button(self): 
 83          """ 
 84          Returns a boolean describing whether the upper button on cuff is pressed. 
 85          (True: pressed, False: unpressed) 
 86          @rtype: bool 
 87          @return:  a variable representing button state: (True: pressed, False: unpressed) 
 88          """ 
 89          return bool(self._cuff_io.get_signal_value('_'.join([self.limb, "button_upper"]))) 
 90   
 91 -    def cuff_button(self): 
 92          """ 
 93          Returns a boolean describing whether the cuff button on cuff is pressed. 
 94          (True: pressed, False: unpressed) 
 95          @rtype: bool 
 96          @return:  a variable representing cuff button state: (True: pressed, False: unpressed) 
 97          """ 
 98          return bool(self._cuff_io.get_signal_value('_'.join([self.limb, "cuff"]))) 
 99   
100 -    def register_callback(self, callback_function, signal_name, poll_rate=10): 
101          """ 
102          Registers a supplied callback to a change in state of supplied 
103          signal_name's value. Spawns a thread that will call the callback with 
104          the updated value. 
105   
106          @type callback_function: function 
107          @param callback_function: function handle for callback function 
108          @type signal_name: str 
109          @param signal_name: the name of the signal to poll for value change 
110          @type poll_rate: int 
111          @param poll_rate: the rate at which to poll for a value change (in a separate 
112                  thread) 
113   
114          @rtype: str 
115          @return: callback_id retuned if the callback was registered, and an 
116                   empty string if the requested signal_name does not exist in the 
117                   Navigator 
118          """ 
119          return self._cuff_io.register_callback( 
120                                   callback_function=callback_function, 
121                                   signal_name=signal_name, 
122                                   poll_rate=poll_rate) 
123   
124 -    def deregister_callback(self, callback_id): 
125          """ 
126          Deregisters a callback based on the supplied callback_id. 
127   
128          @type callback_id: str 
129          @param callback_id: the callback_id string to deregister 
130   
131          @rtype: bool 
132          @return: returns bool True if the callback was successfully 
133                   deregistered, and False otherwise. 
134          """ 
135          return self._cuff_io.deregister_callback(callback_id) 
136