Source Code for Module baxter_interface.robot_enable

  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  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                                queue_size=10) 
 99   
100          baxter_dataflow.wait_for( 
101              test=lambda: self._state.enabled == status, 
102              timeout=2.0 if status else 5.0, 
103              timeout_msg=("Failed to %sable robot" % 
104                           ('en' if status else 'dis',)), 
105              body=lambda: pub.publish(status), 
106          ) 
107          rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled')) 
108   
109 -    def state(self): 
110          """ 
111          Returns the last known robot state. 
112          """ 
113          return self._state 
114   
115 -    def enable(self): 
116          """ 
117          Enable all joints 
118          """ 
119          if self._state.stopped: 
120              rospy.loginfo("Robot Stopped: Attempting Reset...") 
121              self.reset() 
122          self._toggle_enabled(True) 
123   
124 -    def disable(self): 
125          """ 
126          Disable all joints 
127          """ 
128          self._toggle_enabled(False) 
129   
130 -    def reset(self): 
131          """ 
132          Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable 
133          the robot. 
134          """ 
135          error_estop = """\ 
136  E-Stop is ASSERTED. Disengage E-Stop and then reset the robot. 
137  """ 
138          error_nonfatal = """Non-fatal Robot Error on reset. 
139  Robot reset cleared stopped state and robot can be enabled, but a non-fatal 
140  error persists. Check diagnostics or rethink.log for more info. 
141  """ 
142          error_env = """Failed to reset robot. 
143  Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set 
144  and resolvable. For more information please visit: 
145  http://sdk.rethinkrobotics.com/wiki/RSDK_Shell#Initialize 
146  """ 
147          is_reset = lambda: (self._state.enabled == False and 
148                              self._state.stopped == False and 
149                              self._state.error == False and 
150                              self._state.estop_button == 0 and 
151                              self._state.estop_source == 0) 
152          pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10) 
153   
154          if (self._state.stopped and 
155              self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED): 
156              rospy.logfatal(error_estop) 
157              raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged") 
158   
159          rospy.loginfo("Resetting robot...") 
160          try: 
161              baxter_dataflow.wait_for( 
162                  test=is_reset, 
163                  timeout=3.0, 
164                  timeout_msg=error_env, 
165                  body=pub.publish 
166              ) 
167          except OSError, e: 
168              if e.errno == errno.ETIMEDOUT: 
169                  if self._state.error == True and self._state.stopped == False: 
170                      rospy.logwarn(error_nonfatal) 
171                      return False 
172              raise 
173   
174 -    def stop(self): 
175          """ 
176          Simulate an e-stop button being pressed.  Robot must be reset to clear 
177          the stopped state. 
178          """ 
179          pub = rospy.Publisher('robot/set_super_stop', Empty, queue_size=10) 
180          baxter_dataflow.wait_for( 
181              test=lambda: self._state.stopped == True, 
182              timeout=3.0, 
183              timeout_msg="Failed to stop the robot", 
184              body=pub.publish, 
185          ) 
186   
187 -    def version_check(self): 
188          """ 
189          Verifies the version of the software running on the robot is 
190          compatible with this local version of the Baxter RSDK. 
191   
192          Currently uses the variables in baxter_interface.settings and 
193          can be overridden for all default examples by setting CHECK_VERSION 
194          to False. 
195          """ 
196          param_name = "rethink/software_version" 
197          sdk_version = settings.SDK_VERSION 
198   
199          # get local lock for rosparam threading bug 
200          with self.__class__.param_lock: 
201              robot_version = rospy.get_param(param_name, None) 
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 first 3 digits of robot version tag 
210              pattern = ("^([0-9]+)\.([0-9]+)\.([0-9]+)") 
211              match = re.search(pattern, robot_version) 
212              if not match: 
213                  rospy.logwarn("RobotEnable: Invalid robot version: %s", 
214                                robot_version) 
215                  return False 
216              robot_version = match.string[match.start(1):match.end(3)] 
217              if robot_version not in settings.VERSIONS_SDK2ROBOT[sdk_version]: 
218                  errstr_version = """RobotEnable: Software Version Mismatch. 
219  Robot Software version (%s) does not match local SDK version (%s). Please 
220  Update your Robot Software. \ 
221  See: http://sdk.rethinkrobotics.com/wiki/Software_Update""" 
222                  rospy.logerr(errstr_version, robot_version, sdk_version) 
223                  return False 
224          return True 
225