camera_info workaround for pointgrey camera driver ros indigo

subpub.yaml

#!/usr/bin/env python

# Import required Python code.
import cv2
import roslib
import rospy
import yaml
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import CameraInfo
from sensor_msgs.msg import Image
from std_msgs.msg import String

# Node example class.
class syncImageInfo():
	
	# Must have __init__(self) function for a class, similar to a C++ class constructor.
	def __init__(self):
		# subscribers and publishers
		self.bridge = CvBridge()
		self.left_image_sub = rospy.Subscriber("/stereo/left/image_raw",Image,self.left_image_callback)
		self.left_image_pub = rospy.Publisher("/hast/left/image_raw",Image, queue_size=10)
		self.left_info_pub  = rospy.Publisher("/hast/left/camera_info", CameraInfo, queue_size=10)

		self.right_image_sub = rospy.Subscriber("/stereo/right/image_raw",Image,self.right_image_callback)
		self.right_image_pub = rospy.Publisher("/hast/right/image_raw",Image, queue_size=10)
		self.right_info_pub  = rospy.Publisher("/hast/right/camera_info", CameraInfo, queue_size=10)

		# Get the ~private namespace parameters from command line or launch file.
		left_id = rospy.get_param('~left_id')
		left_yaml = rospy.get_param('~left_yaml')
		right_id = rospy.get_param('~right_id')
		right_yaml = rospy.get_param('~right_yaml')

		with open(left_yaml, "r") as left_handle:
			left_yaml_data = yaml.load(left_handle)
		# Parse
		self.left_info_msg = CameraInfo()
		self.left_info_msg.width = left_yaml_data["image_width"]
		self.left_info_msg.height = left_yaml_data["image_height"]
		self.left_info_msg.K = left_yaml_data["camera_matrix"]["data"]
		self.left_info_msg.D = left_yaml_data["distortion_coefficients"]["data"]
		self.left_info_msg.R = left_yaml_data["rectification_matrix"]["data"]
		self.left_info_msg.P = left_yaml_data["projection_matrix"]["data"]
		self.left_info_msg.distortion_model = left_yaml_data["distortion_model"]
		self.left_info_msg.header.frame_id = left_id

		with open(right_yaml, "r") as right_handle:
			right_yaml_data = yaml.load(right_handle)
		# Parse
		self.right_info_msg = CameraInfo()
		self.right_info_msg.width = right_yaml_data["image_width"]
		self.right_info_msg.height = right_yaml_data["image_height"]
		self.right_info_msg.K = right_yaml_data["camera_matrix"]["data"]
		self.right_info_msg.D = right_yaml_data["distortion_coefficients"]["data"]
		self.right_info_msg.R = right_yaml_data["rectification_matrix"]["data"]
		self.right_info_msg.P = right_yaml_data["projection_matrix"]["data"]
		self.right_info_msg.distortion_model = right_yaml_data["distortion_model"]
		self.right_info_msg.header.frame_id = right_id

		while not rospy.is_shutdown():
			rospy.spin()

	def left_image_callback(self,data):
		try:
			cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
		except CvBridgeError as e:
			print(e)

		try:
			# self.left_info_msg.header.stamp = rospy.Time.now() 
			self.left_image_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
			self.left_image_msg.header.stamp = data.header.stamp
			self.left_info_msg.header.stamp = data.header.stamp
			self.left_image_pub.publish(self.left_image_msg)
			self.left_info_pub.publish(self.left_info_msg)
		except CvBridgeError as e:
			print(e)

	def right_image_callback(self,data):
		try:
			cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
		except CvBridgeError as e:
			print(e)

		try:
			self.right_image_msg = self.bridge.cv2_to_imgmsg(cv_image, "bgr8")
			self.right_image_msg.header.stamp = data.header.stamp
			self.right_info_msg.header.stamp = data.header.stamp
			self.right_image_pub.publish(self.right_image_msg)
			self.right_info_pub.publish(self.right_info_msg)
		except CvBridgeError as e:
			print(e)



if __name__ == '__main__':
    # Initialize the node and name it.
    rospy.init_node('syncImageInfo')
    # Go to class functions that do all the heavy lifting. Do error checking.
    try:
        sII = syncImageInfo()
    except rospy.ROSInterruptException: pass


#~~~~~~~~~~~~~~~~~~~~~~
#Launch using :
<!--  set static tfs  -->
<node pkg="tf" type="static_transform_publisher" name="Base2StereoCenter" args="0.055000 0.000000 0.225000 -1.57079632679 0 -1.22173047640 /tb_base /stereo_center 100"/> 
<node pkg="tf" type="static_transform_publisher" name="stereo_left" args="0.171489170 0 0 0 0 0 /stereo_center /stereo_left 100"/> 
<node pkg="tf" type="static_transform_publisher" name="stereo_right" args="-0.171489170 0 0 0 0 0 /stereo_center /stereo_right 100"/> 

<!--  launch rebroadcaster  -->
<node pkg="hast" type="subpub.py" name="hast_subpub">
	<param name="/left_id" value="/stereo_left" />
	<param name="/right_id" value="/stereo_right" />
	<param name="/left_yaml" value="$(arg left_camera_yaml)" />
	<param name="/right_yaml" value="$(arg right_camera_yaml)" />
</node>