Source Code for Module baxter_interface.navigator

  1  # Copyright (c) 2013-2014, 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   
 30  import baxter_dataflow 
 31   
 32  from baxter_core_msgs.msg import ( 
 33      ITBState, 
 34  ) 
 35  from baxter_interface import ( 
 36      digital_io, 
 37  ) 
 38   
 39   
 40 -class Navigator(object): 
 41      """ 
 42      Interface class for a Navigator on the Baxter robot. 
 43   
 44      Inputs: 
 45          Button 0     - press wheel 
 46          Button 1     - above wheel 
 47          Button 2     - below wheel 
 48          Scroll wheel - 0-255 
 49   
 50      Outputs: 
 51          Inner LED 
 52          Outer LED 
 53   
 54      Signals: 
 55          button0_changed - True/False 
 56          button1_changed - True/False 
 57          button2_changed - True/False 
 58          wheel_changed   - New wheel value 
 59   
 60      Valid identifiers: 
 61          left, right, torso_left, torso_right 
 62      """ 
 63   
 64      __LOCATIONS = ('left', 'right', 'torso_left', 'torso_right') 
 65   
 66 -    def __init__(self, location): 
 67          """ 
 68          Constructor. 
 69   
 70          @type location: str 
 71          @param location: body location (prefix) of Navigator to control. 
 72   
 73          Valid locations are in L{Navigator.__LOCATIONS}:: 
 74            left, right, torso_left, torso_right 
 75          """ 
 76          if not location in self.__LOCATIONS: 
 77              raise AttributeError("Invalid Navigator name '%s'" % (location,)) 
 78          self._id = location 
 79          self._state = None 
 80          self.button0_changed = baxter_dataflow.Signal() 
 81          self.button1_changed = baxter_dataflow.Signal() 
 82          self.button2_changed = baxter_dataflow.Signal() 
 83          self.wheel_changed = baxter_dataflow.Signal() 
 84   
 85          nav_state_topic = 'robot/itb/%s_itb/state' % (self._id,) 
 86          self._state_sub = rospy.Subscriber( 
 87              nav_state_topic, 
 88              ITBState, 
 89              self._on_state) 
 90   
 91          self._inner_led = digital_io.DigitalIO( 
 92              '%s_itb_light_inner' % (self._id,)) 
 93   
 94          self._outer_led = digital_io.DigitalIO( 
 95              '%s_itb_light_outer' % (self._id,)) 
 96   
 97          init_err_msg = ("Navigator init failed to get current state from %s" % 
 98                          (nav_state_topic,)) 
 99          baxter_dataflow.wait_for(lambda: self._state != None, 
100                                   timeout_msg=init_err_msg) 
101   
102      @property 
103 -    def wheel(self): 
104          """ 
105          Current state of the wheel 
106          """ 
107          return self._state.wheel 
108   
109      @property 
110 -    def button0(self): 
111          """ 
112          Current state of button 0 
113          """ 
114          return self._state.buttons[0] 
115   
116      @property 
117 -    def button1(self): 
118          """ 
119          Current state of button 1 
120          """ 
121          return self._state.buttons[1] 
122   
123      @property 
124 -    def button2(self): 
125          """ 
126          Current state of button 2 
127          """ 
128          return self._state.buttons[2] 
129   
130      @property 
131 -    def inner_led(self): 
132          """ 
133          Current state of the inner LED 
134          """ 
135          return self._state.innerLight 
136   
137      @inner_led.setter 
138 -    def inner_led(self, enable): 
139          """ 
140          Control the inner LED. 
141   
142          @type enable: bool 
143          @param enable: True to enable the light, False otherwise 
144          """ 
145          self._inner_led.set_output(enable) 
146   
147      @property 
148 -    def outer_led(self): 
149          """ 
150          Current state of the outer LED. 
151          """ 
152          return self._state.outerLight 
153   
154      @outer_led.setter 
155 -    def outer_led(self, enable): 
156          """ 
157          Control the outer LED. 
158   
159          @type enable: bool 
160          @param enable: True to enable the light, False otherwise 
161          """ 
162          self._outer_led.set_output(enable) 
163   
164 -    def _on_state(self, msg): 
165          if not self._state: 
166              self._state = msg 
167   
168          if self._state == msg: 
169              return 
170   
171          old_state = self._state 
172          self._state = msg 
173   
174          buttons = [self.button0_changed, 
175                     self.button1_changed, 
176                     self.button2_changed 
177                     ] 
178          for i, signal in enumerate(buttons): 
179              if old_state.buttons[i] != msg.buttons[i]: 
180                  signal(msg.buttons[i]) 
181   
182          if old_state.wheel != msg.wheel: 
183              diff = msg.wheel - old_state.wheel 
184              if abs(diff % 256) < 127: 
185                  self.wheel_changed(diff % 256) 
186              else: 
187                  self.wheel_changed(diff % (-256)) 
188