Skip to content

Instantly share code, notes, and snippets.

Embed
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 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()
@benjaminabruzzo
Copy link

benjaminabruzzo commented Jul 25, 2017

Thanks for this post! Very helpful.

For others: I modified this file to use a ros parameter for the yaml filename (instead of a command line argument) and then made a launch file for it:
node_py
launch

@agneevguin
Copy link

agneevguin commented Nov 22, 2017

@benjaminabruzzo Dead link for your files.

@malharjajoo
Copy link

malharjajoo commented Feb 27, 2018

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

@mrmouss
Copy link

mrmouss commented Oct 4, 2018

Hello,

I just faced this issue of pointgrey_camera_driver package not publishing camera_info topic. It seems that it does publish the topic given that at least one node subcribes to the image_raw topic.

@TillBeemelmanns
Copy link

TillBeemelmanns commented Feb 8, 2019

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

@lixz123007
Copy link

lixz123007 commented Jul 23, 2019

can you give a example of calibration.yaml file?
with my calibration.yaml,it will return:
/home/harold/tools/srtab/src/stereo_image_proc/scripts/camera_info.py: 行 11:
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: not authorized rospy' @ error/constitute.c/WriteImage/1028. import: not authorized yaml' @ error/constitute.c/WriteImage/1028.
from: can't read /var/mail/sensor_msgs.msg
/home/harold/tools/srtab/src/stereo_image_proc/scripts/camera_info.py: 行 16: 未预期的符号 (' 附近有语法错误 /home/harold/tools/srtab/src/stereo_image_proc/scripts/camera_info.py: 行 16: def yaml_to_CameraInfo(yaml_fname):'

@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

chatom98 commented Sep 16, 2021

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

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