#!/usr/bin/env python                                                                                                                                                                                             

import roslib; roslib.load_manifest('rospy')

import rospy, threading, time

class TimerEvent(object):

    def __init__(self, last_expected, last_real, current_expected, current_real, last_duration):

        self.last_expected    = last_expected

        self.last_real        = last_real

        self.current_expected = current_expected

        self.current_real     = current_real

        self.last_duration    = last_duration

class Timer(threading.Thread):

    def __init__(self, duration, callback, oneshot=False):

        threading.Thread.__init__(self)

        self._duration = duration

        self._callback = callback

        self._oneshot  = oneshot

        self.setDaemon(True)

        self.start()

    def run(self):

        r = rospy.Rate(1.0 / self._duration.to_sec())

        current_expected = rospy.Time.now()

        last_expected, last_real, last_duration = None, None, None

        while not rospy.is_shutdown():

            r.sleep()

            current_real = rospy.Time.now()

            start = time.time()

            self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration))

            if self._oneshot:

                break

            last_duration = time.time() - start

            last_expected, last_real = current_expected, current_real

            current_expected += self._duration

if __name__ == '__main__':

    import time

    rospy.init_node('test')

    def callback(event):

        print 'last_expected:        ', event.last_expected

        print 'last_real:            ', event.last_real

        print 'current_expected:     ', event.current_expected

        print 'current_real:         ', event.current_real

        print 'current_error:        ', (event.current_real - event.current_expected).to_sec()

        print 'profile.last_duration:', event.last_duration

        if event.last_real:

            print 'last_error:           ', (event.last_real - event.last_expected).to_sec(), 'secs'

        print

    Timer(rospy.Duration(2), callback)

    time.sleep(5)