38 lines
952 B
Python
38 lines
952 B
Python
#!/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()
|