-
-
Save rossbar/ebb282c3b73c41c1404123de6cea4771 to your computer and use it in GitHub Desktop.
""" | |
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() |
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):'
@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.
Hi I dared to copy your code and made some tweaks providing disclaimer and link to this page. Is it ok this way?
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?
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