Skip to content

Instantly share code, notes, and snippets.

What would you like to do?
ROS CameraInfo Publisher
pointgrey_camera_driver (at least the version installed with apt-get) doesn't
properly handle camera info in indigo.
This node is a work-around that will read in a camera calibration .yaml
file (as created by the in the camera_calibration pkg),
convert it to a valid sensor_msgs/CameraInfo message, and publish it on a
The yaml parsing is courtesy ROS-user Stephan:
This file just extends that parser into a rosnode.
import rospy
import yaml
from sensor_msgs.msg import CameraInfo
def yaml_to_CameraInfo(yaml_fname):
Parse a yaml file containing camera calibration data (as produced by
rosrun camera_calibration into a
sensor_msgs/CameraInfo msg.
yaml_fname : str
Path to yaml file containing camera calibration data
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
# Load data from file
with open(yaml_fname, "r") as file_handle:
calib_data = yaml.load(file_handle)
# Parse
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"]
return camera_info_msg
if __name__ == "__main__":
# Get fname from command line (cmd line input required)
import argparse
arg_parser = argparse.ArgumentParser()
arg_parser.add_argument("filename", help="Path to yaml file containing " +\
"camera calibration data")
args = arg_parser.parse_args()
filename = args.filename
# Parse yaml file
camera_info_msg = yaml_to_CameraInfo(filename)
# Initialize publisher node
rospy.init_node("camera_info_publisher", anonymous=True)
publisher = rospy.Publisher("camera_info", CameraInfo, queue_size=10)
rate = rospy.Rate(10)
# Run publisher
while not rospy.is_shutdown():
Copy link

How did you get the Header for Camera Info ? I dont see the timestamp and the frame id anywhere.

You cloud extract the current time stamp from the camera message and insert into the camera info header. And further insert the corresponding frame_id.

camera_info_msg.header.frame_id = frame_id camera_info_msg.header.stamp = time_stamp

camera_info_msg.header.stamp = this the correct command for the time stamp?

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