00001
00002
00003 import roslib
00004 roslib.load_manifest('cob_camera_sensors')
00005 import rospy
00006 from sensor_msgs.msg import *
00007 from sensor_msgs.srv import *
00008
00009 rospy.init_node('set_camera_info')
00010 rospy.wait_for_service('camera/set_camera_info')
00011 set_camera_info = rospy.ServiceProxy('camera/set_camera_info', SetCameraInfo)
00012 req = SetCameraInfoRequest()
00013 req.camera_info.header.stamp = rospy.Time.now()
00014 req.camera_info.height = rospy.get_param('~image_height')
00015 req.camera_info.width = rospy.get_param('~image_width')
00016 req.camera_info.distortion_model = "plumb_bob"
00017 req.camera_info.D = rospy.get_param('~distortion_coefficients/data')
00018 req.camera_info.K = rospy.get_param('~camera_matrix/data')
00019 req.camera_info.R = rospy.get_param('~rectification_matrix/data')
00020 req.camera_info.P = rospy.get_param('~projection_matrix/data')
00021 print req.camera_info
00022 try:
00023 resp1 = set_camera_info(req)
00024 except rospy.ServiceException, e:
00025 print "Service did not process request: %s"%str(e)
