intera_interface.robot

37 """ 38 Class RobotEnable - simple control/status wrapper around robot state 39 40 enable() - enable all joints 41 disable() - disable all joints 42 reset() - reset all joints, reset all jrcp faults, disable the robot 43 stop() - stop the robot, similar to hitting the e-stop button 44 """ 45 46 param_lock = Lock() 47

48 - def __init__(self, versioned=False):

49 """ 50 Version checking capable constructor. 51 52 @type versioned: bool 53 @param versioned: True to check robot software version 54 compatibility on initialization. False (default) to ignore. 55 56 The compatibility of robot versions to SDK (intera_interface) 57 versions is defined in the L{intera_interface.VERSIONS_SDK2ROBOT}. 58 59 By default, the class does not check, but all examples do. The 60 example behavior can be overridden by changing the value of 61 L{intera_interface.CHECK_VERSION} to False. 62 """ 63 self._state = None 64 state_topic = 'robot/state' 65 self._state_sub = rospy.Subscriber(state_topic, 66 AssemblyState, 67 self._state_callback 68 ) 69 if versioned and not self.version_check(): 70 sys.exit(1) 71 72 intera_dataflow.wait_for( 73 lambda: not self._state is None, 74 timeout=5.0, 75 timeout_msg=("Failed to get robot state on %s" % 76 (state_topic,)), 77 )

78

79 - def _state_callback(self, msg):

80 self._state = msg

81

82 - def _toggle_enabled(self, status):

83 84 pub = rospy.Publisher('robot/set_super_enable', Bool, 85 queue_size=10) 86 87 intera_dataflow.wait_for( 88 test=lambda: self._state.enabled == status, 89 timeout=5.0, 90 timeout_msg=("Failed to %sable robot" % 91 ('en' if status else 'dis',)), 92 body=lambda: pub.publish(status), 93 ) 94 rospy.loginfo("Robot %s", ('Enabled' if status else 'Disabled'))

95

96 - def state(self):

97 """ 98 Returns the last known robot state. 99 100 @rtype: intera_core_msgs/AssemblyState 101 @return: Returns the last received AssemblyState message 102 """ 103 return self._state

104

105 - def enable(self):

106 """ 107 Enable all joints 108 """ 109 if self._state.stopped: 110 rospy.loginfo("Robot Stopped: Attempting Reset...") 111 self.reset() 112 self._toggle_enabled(True)

113

114 - def disable(self):

115 """ 116 Disable all joints 117 """ 118 self._toggle_enabled(False)

119

120 - def reset(self):

121 """ 122 Reset all joints. Trigger JRCP hardware to reset all faults. Disable 123 the robot. 124 """ 125 error_not_stopped = """\ 126 Robot is not in a Error State. Cannot perform Reset. 127 """ 128 error_estop = """\ 129 E-Stop is ASSERTED. Disengage E-Stop and then reset the robot. 130 """ 131 error_nonfatal = """Non-fatal Robot Error on reset. 132 Robot reset cleared stopped state and robot can be enabled, but a non-fatal 133 error persists. Check diagnostics or rethink.log for more info. 134 """ 135 error_env = """Failed to reset robot. 136 Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set 137 and resolvable. For more information please visit: 138 http://sdk.rethinkrobotics.com/intera/SDK_Shell 139 """ 140 141 142 is_reset = lambda: (self._state.stopped == False and 143 self._state.error == False and 144 self._state.estop_button == 0 and 145 self._state.estop_source == 0) 146 pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10) 147 148 if (not self._state.stopped): 149 rospy.logfatal(error_not_stopped) 150 raise IOError(errno.EREMOTEIO, "Failed to Reset due to lack of Error State.") 151 152 if (self._state.stopped and 153 self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED): 154 rospy.logfatal(error_estop) 155 raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged") 156 157 rospy.loginfo("Resetting robot...") 158 try: 159 intera_dataflow.wait_for( 160 test=is_reset, 161 timeout=5.0, 162 timeout_msg=error_env, 163 body=pub.publish 164 ) 165 except OSError, e: 166 if e.errno == errno.ETIMEDOUT: 167 if self._state.error == True and self._state.stopped == False: 168 rospy.logwarn(error_nonfatal) 169 return False 170 raise

171

172 - def stop(self):

173 """ 174 Simulate an e-stop button being pressed. Robot must be reset to clear 175 the stopped state. 176 """ 177 pub = rospy.Publisher('robot/set_super_stop', Empty, queue_size=10) 178 intera_dataflow.wait_for( 179 test=lambda: self._state.stopped == True, 180 timeout=5.0, 181 timeout_msg="Failed to stop the robot", 182 body=pub.publish, 183 )

184

185 - def version_check(self):

186 """ 187 Verifies the version of the software running on the robot is 188 compatible with this local version of the Intera SDK. 189 190 Currently uses the variables in intera_interface.settings and 191 can be overridden for all default examples by setting CHECK_VERSION 192 to False. 193 194 @rtype: bool 195 @return: Returns True if SDK version is compatible with robot Version, False otherwise 196 """ 197 param_name = "/manifest/robot_software/version/HLR_VERSION_STRING" 198 sdk_version = settings.SDK_VERSION 199 200 # get local lock for rosparam threading bug 201 with self.__class__.param_lock: 202 robot_version = rospy.get_param(param_name, None) 203 if not robot_version: 204 rospy.logwarn("RobotEnable: Failed to retrieve robot version " 205 "from rosparam: %s\n" 206 "Verify robot state and connectivity " 207 "(i.e. ROS_MASTER_URI)", param_name) 208 return False 209 else: 210 # parse out first 3 digits of robot version tag 211 pattern = ("^([0-9]+)\.([0-9]+)\.([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/intera/Software_Update""" 223 rospy.logerr(errstr_version, robot_version, sdk_version) 224 return False 225 return True

Source Code for Module intera_interface.robot_enable