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() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment