Source Code for Module intera_interface.robot_enable

  1  # Copyright (c) 2013-2016, 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 intera_dataflow 
 42   
 43  from intera_core_msgs.msg import ( 
 44      AssemblyState, 
 45  ) 
 46  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 (intera_interface) 
 70          versions is defined in the L{intera_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{intera_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          intera_dataflow.wait_for( 
 86              lambda: not self._state is None, 
 87              timeout=5.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          intera_dataflow.wait_for( 
101              test=lambda: self._state.enabled == status, 
102              timeout=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          @rtype: intera_core_msgs/AssemblyState 
114          @return: Returns the last received AssemblyState message 
115          """ 
116          return self._state 
117   
118 -    def enable(self): 
119          """ 
120          Enable all joints 
121          """ 
122          if self._state.stopped: 
123              rospy.loginfo("Robot Stopped: Attempting Reset...") 
124              self.reset() 
125          self._toggle_enabled(True) 
126   
127 -    def disable(self): 
128          """ 
129          Disable all joints 
130          """ 
131          self._toggle_enabled(False) 
132   
133 -    def reset(self): 
134          """ 
135          Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable 
136          the robot. 
137          """ 
138          error_estop = """\ 
139  E-Stop is ASSERTED. Disengage E-Stop and then reset the robot. 
140  """ 
141          error_nonfatal = """Non-fatal Robot Error on reset. 
142  Robot reset cleared stopped state and robot can be enabled, but a non-fatal 
143  error persists. Check diagnostics or rethink.log for more info. 
144  """ 
145          error_env = """Failed to reset robot. 
146  Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set 
147  and resolvable. For more information please visit: 
148  http://sdk.rethinkrobotics.com/intera/SDK_Shell 
149  """ 
150          is_reset = lambda: (self._state.enabled == False and 
151                              self._state.stopped == False and 
152                              self._state.error == False and 
153                              self._state.estop_button == 0 and 
154                              self._state.estop_source == 0) 
155          pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10) 
156   
157          if (self._state.stopped and 
158              self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED): 
159              rospy.logfatal(error_estop) 
160              raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged") 
161   
162          rospy.loginfo("Resetting robot...") 
163          try: 
164              intera_dataflow.wait_for( 
165                  test=is_reset, 
166                  timeout=5.0, 
167                  timeout_msg=error_env, 
168                  body=pub.publish 
169              ) 
170          except OSError, e: 
171              if e.errno == errno.ETIMEDOUT: 
172                  if self._state.error == True and self._state.stopped == False: 
173                      rospy.logwarn(error_nonfatal) 
174                      return False 
175              raise 
176   
177 -    def stop(self): 
178          """ 
179          Simulate an e-stop button being pressed.  Robot must be reset to clear 
180          the stopped state. 
181          """ 
182          pub = rospy.Publisher('robot/set_super_stop', Empty, queue_size=10) 
183          intera_dataflow.wait_for( 
184              test=lambda: self._state.stopped == True, 
185              timeout=5.0, 
186              timeout_msg="Failed to stop the robot", 
187              body=pub.publish, 
188          ) 
189   
190 -    def version_check(self): 
191          """ 
192          Verifies the version of the software running on the robot is 
193          compatible with this local version of the Intera SDK. 
194   
195          Currently uses the variables in intera_interface.settings and 
196          can be overridden for all default examples by setting CHECK_VERSION 
197          to False. 
198   
199          @rtype: bool 
200          @return: Returns True if SDK version is compatible with robot Version, False otherwise 
201          """ 
202          param_name = "/manifest/robot_software/version/HLR_VERSION_STRING" 
203          sdk_version = settings.SDK_VERSION 
204   
205          # get local lock for rosparam threading bug 
206          with self.__class__.param_lock: 
207              robot_version = rospy.get_param(param_name, None) 
208          if not robot_version: 
209              rospy.logwarn("RobotEnable: Failed to retrieve robot version " 
210                            "from rosparam: %s\n" 
211                            "Verify robot state and connectivity " 
212                            "(i.e. ROS_MASTER_URI)", param_name) 
213              return False 
214          else: 
215              # parse out first 3 digits of robot version tag 
216              pattern = ("^([0-9]+)\.([0-9]+)\.([0-9]+)") 
217              match = re.search(pattern, robot_version) 
218              if not match: 
219                  rospy.logwarn("RobotEnable: Invalid robot version: %s", 
220                                robot_version) 
221                  return False 
222              robot_version = match.string[match.start(1):match.end(3)] 
223              if robot_version not in settings.VERSIONS_SDK2ROBOT[sdk_version]: 
224                  errstr_version = """RobotEnable: Software Version Mismatch. 
225  Robot Software version (%s) does not match local SDK version (%s). Please 
226  Update your Robot Software. \ 
227  See: http://sdk.rethinkrobotics.com/intera/Software_Update""" 
228                  rospy.logerr(errstr_version, robot_version, sdk_version) 
229                  return False 
230          return True 
231