Source Code for Module baxter_interface.head

  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  from std_msgs.msg import ( 
 31      Bool 
 32  ) 
 33   
 34  import baxter_dataflow 
 35   
 36  from baxter_core_msgs.msg import ( 
 37     HeadPanCommand, 
 38     HeadState, 
 39  ) 
 40  from baxter_interface import settings 
 41   
 42   
 43 -class Head(object): 
 44      """ 
 45      Interface class for the head on the Baxter Robot. 
 46   
 47      Used to control the head pan angle and to enable/disable the head nod 
 48      action. 
 49      """ 
 50 -    def __init__(self): 
 51          """ 
 52          Constructor. 
 53          """ 
 54          self._state = dict() 
 55   
 56          self._pub_pan = rospy.Publisher( 
 57              '/robot/head/command_head_pan', 
 58              HeadPanCommand) 
 59   
 60          self._pub_nod = rospy.Publisher( 
 61              '/robot/head/command_head_nod', 
 62              Bool) 
 63   
 64          state_topic = '/robot/head/head_state' 
 65          self._sub_state = rospy.Subscriber( 
 66              state_topic, 
 67              HeadState, 
 68              self._on_head_state) 
 69   
 70          baxter_dataflow.wait_for( 
 71              lambda: len(self._state) != 0, 
 72              timeout=5.0, 
 73              timeout_msg=("Failed to get current head state from %s" % 
 74                           (state_topic,)), 
 75          ) 
 76   
 77 -    def _on_head_state(self, msg): 
 78          self._state['pan'] = msg.pan 
 79          self._state['panning'] = msg.isPanning 
 80          self._state['nodding'] = msg.isNodding 
 81   
 82 -    def pan(self): 
 83          """ 
 84          Get the current pan angle of the head. 
 85   
 86          @rtype: float 
 87          @return: current angle in radians 
 88          """ 
 89          return self._state['pan'] 
 90   
 91 -    def nodding(self): 
 92          """ 
 93          Check if the head is currently nodding. 
 94   
 95          @rtype: bool 
 96          @return: True if the head is currently nodding, False otherwise. 
 97          """ 
 98          return self._state['nodding'] 
 99   
100 -    def panning(self): 
101          """ 
102          Check if the head is currently panning. 
103   
104          @rtype: bool 
105          @return: True if the head is currently panning, False otherwise. 
106          """ 
107          return self._state['panning'] 
108   
109 -    def set_pan(self, angle, speed=100, timeout=10.0): 
110          """ 
111          Pan at the given speed to the desired angle. 
112   
113          @type angle: float 
114          @param angle: Desired pan angle in radians. 
115          @type speed: int 
116          @param speed: Desired speed to pan at, range is 0-100 [100] 
117          @type timeout: float 
118          @param timeout: Seconds to wait for the head to pan to the 
119                          specified angle. If 0, just command once and 
120                          return. [10] 
121          """ 
122          msg = HeadPanCommand(angle, speed) 
123          self._pub_pan.publish(msg) 
124   
125          if not timeout == 0: 
126              baxter_dataflow.wait_for( 
127                  lambda: (abs(self.pan() - angle) <= 
128                           settings.HEAD_PAN_ANGLE_TOLERANCE), 
129                  timeout=timeout, 
130                  rate=100, 
131                  timeout_msg="Failed to move head to pan command %f" % angle, 
132                  body=lambda: self._pub_pan.publish(msg) 
133                  ) 
134   
135 -    def command_nod(self, timeout=5.0): 
136          """ 
137          Command the head to nod once. 
138   
139          @type timeout: float 
140          @param timeout: Seconds to wait for the head to nod. 
141                          If 0, just command once and return. [0] 
142          """ 
143          self._pub_nod.publish(True) 
144   
145          if not timeout == 0: 
146              # Wait for nod to initiate 
147              baxter_dataflow.wait_for( 
148                  test=self.nodding, 
149                  timeout=timeout, 
150                  rate=100, 
151                  timeout_msg="Failed to initiate head nod command", 
152                  body=lambda: self._pub_nod.publish(True) 
153              ) 
154   
155              # Wait for nod to complete 
156              baxter_dataflow.wait_for( 
157                  test=lambda: not self.nodding(), 
158                  timeout=timeout, 
159                  rate=100, 
160                  timeout_msg="Failed to complete head nod command", 
161                  body=lambda: self._pub_nod.publish(False) 
162              ) 
163