Created
November 1, 2017 19:06
-
-
Save amogh112/908968bd228ed4a1809822a5fbe857f9 to your computer and use it in GitHub Desktop.
Change frame_id and timestamp of topics
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#!/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() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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)