anymsg delay_timestamp.py

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()