jsk_baxter_startup: camera_info_fixer.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from sensor_msgs.msg import CameraInfo
00005 
00006 class CameraInfoFixer:
00007     def __init__(self):
00008         topic_name = rospy.resolve_name('camera_info')
00009         self.sub = rospy.Subscriber(topic_name, CameraInfo, self.callback)
00010         self.pub = rospy.Publisher(topic_name + '_fixed', CameraInfo, queue_size=1)
00011 
00012     def callback(self, m):
00013         # https://github.com/ros-perception/vision_opencv/blob/8216fb5df7eb262601f12ac4b0c9415477717514/image_geometry/src/pinhole_camera_model.cpp#L149
00014         K = list(m.K)
00015         P = list(m.P)
00016         K[0*3+2] -= m.roi.x_offset
00017         K[1*3+2] -= m.roi.y_offset
00018         P[0*4+2] -= m.roi.x_offset/2
00019         P[1*4+2] -= m.roi.y_offset/2
00020         m.K = tuple(K)
00021         m.P = tuple(P)
00022 
00023         m.roi.x_offset = 0
00024         m.roi.y_offset = 0
00025         m.roi.height = m.height
00026         m.roi.width = m.width
00027 
00028         self.pub.publish(m)
00029 
00030 if __name__ == '__main__':
00031     rospy.init_node('camera_info_fixer', anonymous=True)
00032     app = CameraInfoFixer()
00033     rospy.spin()