Interruptable Rate for ROS

interruptablerate.py

class InterruptableRate(rospy.Rate):
    def sleep(self):
        """
        This implementation is cloned from rospy.sleep

        Attempt sleep at the specified rate. sleep() takes into
        account the time elapsed since the last successful
        sleep().

        @raise ROSInterruptException: if ROS shutdown occurs before
        sleep completes
        @raise ROSTimeMovedBackwardsException: if ROS time is set
        backwards
        """
        curr_time = rospy.rostime.get_rostime()
        try:
            # changed this to a self-call
            self.interruptable_sleep(self._remaining(curr_time))
        except rospy.exceptions.ROSTimeMovedBackwardsException:
            if not self._reset:
                raise
            self.last_time = rospy.rostime.get_rostime()
            return
        self.last_time = self.last_time + self.sleep_dur

        # detect time jumping forwards, as well as loops that are
        # inherently too slow
        if curr_time - self.last_time > self.sleep_dur * 2:
            self.last_time = curr_time

    def interruptable_sleep(self, duration):
        """
        This implementation is cloned from rospy.sleep

        sleep for the specified duration in ROS time. If duration
        is negative, sleep immediately returns.

        @param duration: seconds (or rospy.Duration) to sleep
        @type  duration: float or Duration
        @raise ROSInterruptException: if ROS shutdown occurs before sleep
        completes
        @raise ROSTimeMovedBackwardsException: if ROS time is set backwards
        @raise EarlyTerminateRequested: the "interrupt" of the rate has been
        called
        """
        if rospy.rostime.is_wallclock():
            if isinstance(duration, genpy.Duration):
                duration = duration.to_sec()
                if duration < 0:
                    return
                else:
                    rospy.rostime.wallsleep(duration)
        else:
            initial_rostime = rospy.rostime.get_rostime()
            if not isinstance(duration, genpy.Duration):
                duration = genpy.Duration.from_sec(duration)
            rostime_cond = rospy.rostime.get_rostime_cond()

            # #3123
            if initial_rostime == genpy.Time(0):
                # break loop if time is initialized or node is shutdown
                while initial_rostime == genpy.Time(0) and \
                      not rospy.core.is_shutdown():
                    with rostime_cond:
                        rostime_cond.wait(0.3)
                    initial_rostime = rospy.rostime.get_rostime()

            sleep_t = initial_rostime + duration

            # Change
            self.is_interrupted = False

            # break loop if sleep_t is reached, time moves backwards, or
            # node is shutdown or if interrupt is called
            while rospy.rostime.get_rostime() < sleep_t and \
                  rospy.rostime.get_rostime() >= initial_rostime and \
                  not rospy.core.is_shutdown() and \
                  not self.is_interrupted: # change
                with rostime_cond:
                    rostime_cond.wait(0.5)

            # change
            self.is_interrupted = False

            if rospy.rostime.get_rostime() < initial_rostime:
                time_jump = (initial_rostime - rospy.rostime.get_rostime()).to_sec()
                raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
            if rospy.core.is_shutdown():
                raise rospy.exceptions.ROSInterruptException("ROS shutdown request")

    def interrupt(self):
        self.is_interrupted = True

        # early terminate the rostime_wait_cond for sleep
        rostime_cond = rospy.rostime.get_rostime_cond()
        with rostime_cond:
            rostime_cond.notifyAll() # make sure our sleep receives the interrupt!