anymsg delay_timestamp.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
from roslib.message import get_message_class
import rospy
import rosgraph
from rospy.msg import AnyMsg
class DelayTimestamp(object):
def __init__(self):
rospy.init_node('delay_timestamp')
self._master = rosgraph.Master(rospy.get_name())
self._delay = rospy.get_param('~delay')
self._pub = None
self._sub = rospy.Subscriber('~input', AnyMsg, self._cb)
def _cb(self, anymsg):
topic_types = self._master.getTopicTypes()
msg_name = [ty for tp, ty in topic_types if tp == self._sub.name][0]
msg_class = get_message_class(msg_name)
# created delayed msg
msg = msg_class().deserialize(anymsg._buff)
msg.header.stamp += rospy.Duration(self._delay)
# construct publisher
if self._pub is None:
self._pub = rospy.Publisher('~output', msg_class, queue_size=10)
return
while msg.header.stamp > rospy.Time.now():
rospy.sleep(0.001)
self._pub.publish(msg)
if __name__ == '__main__':
DelayTimestamp()
rospy.spin()