Subscriber callback does not change value of timestamp

asked 2019-07-29 18:05:35 -0500

mbfg gravatar image

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 /image_raw topic. I can see that the /image_raw topic is being successfully sent using the rostopic echo command. How can I change this to fix the problem?

edit retag flag offensive close merge delete