#!/usr/bin/env python
import collections
import rospy
from sensor_msgs.msg import Joy

queue = collections.deque()
pub = None


def callback(msg):
    delay = rospy.get_param('/delay_seconds', 0.0)
    if delay <= 0:
        pub.publish(msg)
        return
    queue.append((rospy.Time.now().to_sec(), msg))


def spin():
    rate = rospy.Rate(50)
    while not rospy.is_shutdown():
        delay = rospy.get_param('/delay_seconds', 0.0)
        now = rospy.Time.now().to_sec()
        if delay <= 0:
            while queue:
                pub.publish(queue.popleft()[1])
        else:
            while queue and (now - queue[0][0]) >= delay:
                pub.publish(queue.popleft()[1])
        rate.sleep()


if __name__ == '__main__':
    rospy.init_node('delay_node')
    rospy.loginfo('delay_node gestartet')
    pub = rospy.Publisher('/joy_delayed', Joy, queue_size=50)
    rospy.Subscriber('/joy', Joy, callback, queue_size=50)
    spin()