Skip to content

Instantly share code, notes, and snippets.

@rossbar
Last active July 30, 2023 12:15
Show Gist options
  • Star 24 You must be signed in to star a gist
  • Fork 2 You must be signed in to fork a gist
  • Save rossbar/ebb282c3b73c41c1404123de6cea4771 to your computer and use it in GitHub Desktop.
Save rossbar/ebb282c3b73c41c1404123de6cea4771 to your computer and use it in GitHub Desktop.
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 cameracalibrator.py in the camera_calibration pkg),
convert it to a valid sensor_msgs/CameraInfo message, and publish it on a
topic.
The yaml parsing is courtesy ROS-user Stephan:
http://answers.ros.org/question/33929/camera-calibration-parser-in-python/
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 cameracalibrator.py) into a
sensor_msgs/CameraInfo msg.
Parameters
----------
yaml_fname : str
Path to yaml file containing camera calibration data
Returns
-------
camera_info_msg : sensor_msgs.msg.CameraInfo
A sensor_msgs.msg.CameraInfo message containing the camera calibration
data
"""
# 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():
publisher.publish(camera_info_msg)
rate.sleep()
@rossbar
Copy link
Author

rossbar commented Jul 25, 2019

@lixz123007 I haven't used this code in some time, so I can't vouch for how well/whether it works. Here's a link to a yaml file of the format that I had been using this with: https://www.dropbox.com/s/ybi9ubf9kw6djp0/camera_calib_sample.yaml?dl=0

I suspect that all of this has been obsoleted by newer versions of ROS and/or the camera calibration package.

Hope that helps.

@chatom98
Copy link

Hi I dared to copy your code and made some tweaks providing disclaimer and link to this page. Is it ok this way?

@Darken23
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 = rospy.Time.now().Is 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