Source Code for Module baxter_interface.robot_enable

  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 errno 
 29  import re 
 30  import sys 
 31   
 32  from threading import Lock 
 33   
 34  import rospy 
 35   
 36  from std_msgs.msg import ( 
 37      Bool, 
 38      Empty, 
 39  ) 
 40   
 41  import baxter_dataflow 
 42   
 43  from baxter_core_msgs.msg import ( 
 44      AssemblyState, 
 45  ) 
 46  from baxter_interface import settings 
 47   
 48   
 49 -class RobotEnable(object): 
 50      """ 
 51      Class RobotEnable - simple control/status wrapper around robot state 
 52   
 53      enable()  - enable all joints 
 54      disable() - disable all joints 
 55      reset()   - reset all joints, reset all jrcp faults, disable the robot 
 56      stop()    - stop the robot, similar to hitting the e-stop button 
 57      """ 
 58   
 59      param_lock = Lock() 
 60   
 61 -    def __init__(self, versioned=False): 
 62          """ 
 63          Version checking capable constructor. 
 64   
 65          @type versioned: bool 
 66          @param versioned: True to check robot software version 
 67          compatibility on initialization. False (default) to ignore. 
 68   
 69          The compatibility of robot versions to SDK (baxter_interface) 
 70          versions is defined in the L{baxter_interface.VERSIONS_SDK2ROBOT}. 
 71   
 72          By default, the class does not check, but all examples do. The 
 73          example behavior can be overridden by changing the value of 
 74          L{baxter_interface.CHECK_VERSION} to False. 
 75          """ 
 76          self._state = None 
 77          state_topic = 'robot/state' 
 78          self._state_sub = rospy.Subscriber(state_topic, 
 79                                             AssemblyState, 
 80                                             self._state_callback 
 81                                             ) 
 82          if versioned and not self.version_check(): 
 83              sys.exit(1) 
 84   
 85          baxter_dataflow.wait_for( 
 86              lambda: not self._state is None, 
 87              timeout=2.0, 
 88              timeout_msg=("Failed to get robot state on %s" % 
 89              (state_topic,)), 
 90          ) 
 91   
 92 -    def _state_callback(self, msg): 
 93          self._state = msg 
 94   
 95 -    def _toggle_enabled(self, status): 
 96   
 97          pub = rospy.Publisher('robot/set_super_enable', Bool) 
 98   
 99          baxter_dataflow.wait_for( 
100              test=lambda: self._state.enabled == status, 
101              timeout=2.0 if status else 5.0, 
102              timeout_msg=("Failed to %sable robot" % 
103                           ('en' if status else 'dis',)), 
104              body=lambda: pub.publish(status), 
105          ) 
106          rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled')) 
107   
108 -    def state(self): 
109          """ 
110          Returns the last known robot state. 
111          """ 
112          return self._state 
113   
114 -    def enable(self): 
115          """ 
116          Enable all joints 
117          """ 
118          if self._state.stopped: 
119              rospy.loginfo("Robot Stopped: Attempting Reset...") 
120              self.reset() 
121          self._toggle_enabled(True) 
122   
123 -    def disable(self): 
124          """ 
125          Disable all joints 
126          """ 
127          self._toggle_enabled(False) 
128   
129 -    def reset(self): 
130          """ 
131          Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable 
132          the robot. 
133          """ 
134          error_estop = """\ 
135  E-Stop is ASSERTED. Disengage E-Stop and then reset the robot. 
136  """ 
137          error_nonfatal = """Non-fatal Robot Error on reset. 
138  Robot reset cleared stopped state and robot can be enabled, but a non-fatal 
139  error persists. Check diagnostics or rethink.log for more info. 
140  """ 
141          error_env = """Failed to reset robot. 
142  Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set 
143  and resolvable. For more information please visit: 
144  http://sdk.rethinkrobotics.com/wiki/RSDK_Shell#Initialize 
145  """ 
146          is_reset = lambda: (self._state.enabled == False and 
147                              self._state.stopped == False and 
148                              self._state.error == False and 
149                              self._state.estop_button == 0 and 
150                              self._state.estop_source == 0) 
151          pub = rospy.Publisher('robot/set_super_reset', Empty) 
152   
153          if (self._state.stopped and 
154              self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED): 
155              rospy.logfatal(error_estop) 
156              raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged") 
157   
158          rospy.loginfo("Resetting robot...") 
159          try: 
160              baxter_dataflow.wait_for( 
161                  test=is_reset, 
162                  timeout=3.0, 
163                  timeout_msg=error_env, 
164                  body=pub.publish 
165              ) 
166          except OSError, e: 
167              if e.errno == errno.ETIMEDOUT: 
168                  if self._state.error == True and self._state.stopped == False: 
169                      rospy.logwarn(error_nonfatal) 
170                      return False 
171              raise 
172   
173 -    def stop(self): 
174          """ 
175          Simulate an e-stop button being pressed.  Robot must be reset to clear 
176          the stopped state. 
177          """ 
178          pub = rospy.Publisher('robot/set_super_stop', Empty) 
179          baxter_dataflow.wait_for( 
180              test=lambda: self._state.stopped == True, 
181              timeout=3.0, 
182              timeout_msg="Failed to stop the robot", 
183              body=pub.publish, 
184          ) 
185   
186 -    def version_check(self): 
187          """ 
188          Verifies the version of the software running on the robot is 
189          compatible with this local version of the Baxter RSDK. 
190   
191          Currently uses the variables in baxter_interface.settings and 
192          can be overridden for all default examples by setting CHECK_VERSION 
193          to False. 
194          """ 
195          param_name = "rethink/software_version" 
196          sdk_version = settings.SDK_VERSION 
197   
198          # get local lock for rosparam threading bug 
199          with self.__class__.param_lock: 
200              robot_version = rospy.get_param(param_name, None) 
201   
202          if not robot_version: 
203              rospy.logwarn("RobotEnable: Failed to retrieve robot version " 
204                            "from rosparam: %s\n" 
205                            "Verify robot state and connectivity " 
206                            "(i.e. ROS_MASTER_URI)", param_name) 
207              return False 
208          else: 
209              # parse out tags 
210              pattern = ("([0-9]+)\.([0-9]+)\.([0-9]+)" 
211                         "($|_(alpha|beta|pre|rc|p)([0-9]+))") 
212              match = re.search(pattern, robot_version) 
213              if not match: 
214                  rospy.logwarn("RobotEnable: Invalid robot version: %s", 
215                                robot_version) 
216                  return False 
217              robot_version = match.string[match.start(1):match.end(3)] 
218              if robot_version not in settings.VERSIONS_SDK2ROBOT[sdk_version]: 
219                  errstr_version = """RobotEnable: Software Version Mismatch. 
220  Robot Software version (%s) does not match local SDK version (%s). Please 
221  Update your Robot Software. \ 
222  See: http://sdk.rethinkrobotics.com/wiki/Software_Update""" 
223                  rospy.logerr(errstr_version, robot_version, sdk_version) 
224                  return False 
225          return True 
226