robot_enable.py 文件源码

def reset(self):
        """
        Reset all joints.  Trigger JRCP hardware to reset all faults.  Disable
        the robot.
        """
        error_not_stopped = """\
Robot is not in a Error State. Cannot perform Reset.
"""
        error_estop = """\
E-Stop is ASSERTED. Disengage E-Stop and then reset the robot.
"""
        error_nonfatal = """Non-fatal Robot Error on reset.
Robot reset cleared stopped state and robot can be enabled, but a non-fatal
error persists. Check diagnostics or rethink.log for more info.
"""
        error_env = """Failed to reset robot.
Please verify that the ROS_IP or ROS_HOSTNAME environment variables are set
and resolvable. For more information please visit:
http://sdk.rethinkrobotics.com/intera/SDK_Shell
"""


        is_reset = lambda: (self._state.stopped == False and
                            self._state.error == False and
                            self._state.estop_button == 0 and
                            self._state.estop_source == 0)
        pub = rospy.Publisher('robot/set_super_reset', Empty, queue_size=10)

        if (not self._state.stopped):
            rospy.logfatal(error_not_stopped)
            raise IOError(errno.EREMOTEIO, "Failed to Reset due to lack of Error State.")

        if (self._state.stopped and
              self._state.estop_button == AssemblyState.ESTOP_BUTTON_PRESSED):
            rospy.logfatal(error_estop)
            raise IOError(errno.EREMOTEIO, "Failed to Reset: E-Stop Engaged")

        rospy.loginfo("Resetting robot...")
        try:
            intera_dataflow.wait_for(
                test=is_reset,
                timeout=5.0,
                timeout_msg=error_env,
                body=pub.publish
            )
        except OSError, e:
            if e.errno == errno.ETIMEDOUT:
                if self._state.error == True and self._state.stopped == False:
                    rospy.logwarn(error_nonfatal)
                    return False
            raise