Source Code for Module baxter_interface.head

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