Skip to content

Instantly share code, notes, and snippets.

@amogh112
Created November 1, 2017 19:06
Show Gist options
  • Save amogh112/908968bd228ed4a1809822a5fbe857f9 to your computer and use it in GitHub Desktop.
Save amogh112/908968bd228ed4a1809822a5fbe857f9 to your computer and use it in GitHub Desktop.
Change frame_id and timestamp of topics
#!/usr/bin/env python
import rospy
import sys
from std_msgs.msg import String, Header
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image
class publishTF(object):
def __init__(self):
rospy.loginfo("Initialising subscriber and publisher")
self.left_image_raw_sub=rospy.Subscriber('/zed/left/image_raw',Image, self.callback_left_image_raw, queue_size=1)
self.right_image_raw_sub=rospy.Subscriber('/zed/right/image_raw',Image, self.callback_right_image_raw, queue_size=1)
self.left_info_sub=rospy.Subscriber('/zed/left/camera_info',CameraInfo, self.callback_left_info, queue_size=1)
self.right_info_sub=rospy.Subscriber('/zed/right/camera_info',CameraInfo, self.callback_right_info, queue_size=1)
# self.lastMsg1=None
# self.lastMsg2=None
# self.lastMsg3=None
# self.lastMsg4=None
# self.i=rospy.Publisher('/imu_new', Imu, queue_size=1)
self.left_image_raw_pub=rospy.Publisher('/zed/left/image_raw_currentStamp',Image, queue_size=1)
self.right_image_raw_pub=rospy.Publisher('/zed/right/image_raw_currentStamp',Image, queue_size=1)
self.left_info_pub=rospy.Publisher('/zed/left/camera_info_currentStamp',CameraInfo, queue_size=1)
self.right_info_pub=rospy.Publisher('/zed/right/camera_info_currentStamp',CameraInfo, queue_size=1)
rospy.sleep(2)
rospy.loginfo("initialised")
def callback_left_image_raw(self, data):
# self.left_image_raw_pub=data
data.header.stamp=rospy.Time.now()
data.header.frame_id='zed_frame'
self.left_image_raw_pub.publish(data)
# imumsg.header.stamp = rospy.Time.now()
def callback_right_image_raw(self, data):
# self.right_image_raw_pub=data
data.header.stamp=rospy.Time.now()
data.header.frame_id='zed_frame'
self.right_image_raw_pub.publish(data)
def callback_left_info(self, data):
# self.left_info_raw_pub=data
data.header.frame_id='zed_frame'
data.header.stamp=rospy.Time.now()
self.left_info_pub.publish(data)
def callback_right_info(self, data):
# self.right_info_raw_pub=data
data.header.frame_id='zed_frame'
data.header.stamp=rospy.Time.now()
self.right_info_pub.publish(data)
# def do_work(self):
# self.splitStrings= str(self.lastMsg).split(",")
# imumsg= Imu()
# imumsg.header.stamp = rospy.Time.now()
# imumsg.header.frame_id='imu'
# imumsg.orientation.x=float(self.splitStrings[1][2:])
# imumsg.orientation.y=float(self.splitStrings[2][2:])
# imumsg.orientation.z=float(self.splitStrings[3][2:])
# imumsg.orientation.w=float(self.splitStrings[4][2:])
# rospy.loginfo(self.splitStrings[1])
# self.imu_pub.publish(imumsg)
# def run(self):
# r=rospy.Rate(1)
# while not rospy.is_shutdown():
# # self.do_work()
# r.sleep()
if __name__=='__main__':
rospy.init_node('publish_tf')
obj=publishTF()
while not rospy.is_shutdown():
rospy.spin()
@amogh112
Copy link
Author

amogh112 commented Nov 1, 2017

Subscribes to four topics (see topics in Subscriber nodes)
Publishes to four topics (see topics in Publisher nodes)
Changes time stamp to node and frame_id to 'zed_frame' each time a callback occurs (ie message arrives on that topic)

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment