Subscriber callback does not change value of timestamp
I am trying to use the "/image_raw" topic's header.stamp.sec to change the timestamp for the topic that is being published on my camera publisher node.
import rospy
import argparse
import yaml
import os
from sensor_msgs.msg import CameraInfo, Image
from std_msgs.msg import String
def callback(msg):
global camera_info_msg
timestamp = msg.header.stamp
camera_info_msg.header.stamp = timestamp
if __name__ == "__main__":
# Get filename from command input
arg_parser = argparse.ArgumentParser()
arg_parser.add_argument("filename")
args = arg_parser.parse_args()
filename = args.filename
# Get the file name minus path and .yaml
topicname = os.path.split(filename)[1]
topicname = os.path.splitext(topicname)[0]
topicname = topicname + "/camera_info"
# Load calibration paramaters from file
with open(filename, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse the data into a message
camera_info_msg = CameraInfo()
camera_info_msg.width = calib_data["image_width"]
camera_info_msg.height = calib_data["image_height"]
camera_info_msg.K = calib_data["camera_matrix"]["data"]
camera_info_msg.D = calib_data["distortion_coefficients"]["data"]
camera_info_msg.R = calib_data["rectification_matrix"]["data"]
camera_info_msg.P = calib_data["projection_matrix"]["data"]
camera_info_msg.distortion_model = calib_data["distortion_model"]
# Initialize the camera info publisher node
rospy.init_node("camera_info_publisher", anonymous=True)
# Begin subscribing to the image_raw topic
subscriber = rospy.Subscriber("/B09D0D0123BEF7h/image_raw",Image, callback)
# Begin publishing to the /camera_info topic
publisher = rospy.Publisher(topicname, CameraInfo, queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
publisher.publish(camera_info_msg)
rate.sleep()
But when I run the command:
rostopic echo /camera*/camera_info
It still has a timestamp of zero. My callback function for the subcscriber should be updating the header.stamp field to the msg.header.stamp, which is the Image.Header from the /imageraw topic. I can see that the /imageraw topic is being successfully sent using the rostopic echo command. How can I change this to fix the problem?
Asked by mbfg on 2019-07-29 18:05:35 UTC
Comments