$search
00001 """autogenerated by genmsg_py from CameraInfo.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import sensor_msgs.msg 00006 import std_msgs.msg 00007 00008 class CameraInfo(roslib.message.Message): 00009 _md5sum = "c9a58c1b0b154e0e6da7578cb991d214" 00010 _type = "sensor_msgs/CameraInfo" 00011 _has_header = True #flag to mark the presence of a Header object 00012 _full_text = """# This message defines meta information for a camera. It should be in a 00013 # camera namespace on topic "camera_info" and accompanied by up to five 00014 # image topics named: 00015 # 00016 # image_raw - raw data from the camera driver, possibly Bayer encoded 00017 # image - monochrome, distorted 00018 # image_color - color, distorted 00019 # image_rect - monochrome, rectified 00020 # image_rect_color - color, rectified 00021 # 00022 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00023 # for producing the four processed image topics from image_raw and 00024 # camera_info. The meaning of the camera parameters are described in 00025 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00026 # 00027 # The image_geometry package provides a user-friendly interface to 00028 # common operations using this meta information. If you want to, e.g., 00029 # project a 3d point into image coordinates, we strongly recommend 00030 # using image_geometry. 00031 # 00032 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00033 # zeroed out. In particular, clients may assume that K[0] == 0.0 00034 # indicates an uncalibrated camera. 00035 00036 ####################################################################### 00037 # Image acquisition info # 00038 ####################################################################### 00039 00040 # Time of image acquisition, camera coordinate frame ID 00041 Header header # Header timestamp should be acquisition time of image 00042 # Header frame_id should be optical frame of camera 00043 # origin of frame should be optical center of camera 00044 # +x should point to the right in the image 00045 # +y should point down in the image 00046 # +z should point into the plane of the image 00047 00048 00049 ####################################################################### 00050 # Calibration Parameters # 00051 ####################################################################### 00052 # These are fixed during camera calibration. Their values will be the # 00053 # same in all messages until the camera is recalibrated. Note that # 00054 # self-calibrating systems may "recalibrate" frequently. # 00055 # # 00056 # The internal parameters can be used to warp a raw (distorted) image # 00057 # to: # 00058 # 1. An undistorted image (requires D and K) # 00059 # 2. A rectified image (requires D, K, R) # 00060 # The projection matrix P projects 3D points into the rectified image.# 00061 ####################################################################### 00062 00063 # The image dimensions with which the camera was calibrated. Normally 00064 # this will be the full camera resolution in pixels. 00065 uint32 height 00066 uint32 width 00067 00068 # The distortion model used. Supported models are listed in 00069 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00070 # simple model of radial and tangential distortion - is sufficent. 00071 string distortion_model 00072 00073 # The distortion parameters, size depending on the distortion model. 00074 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00075 float64[] D 00076 00077 # Intrinsic camera matrix for the raw (distorted) images. 00078 # [fx 0 cx] 00079 # K = [ 0 fy cy] 00080 # [ 0 0 1] 00081 # Projects 3D points in the camera coordinate frame to 2D pixel 00082 # coordinates using the focal lengths (fx, fy) and principal point 00083 # (cx, cy). 00084 float64[9] K # 3x3 row-major matrix 00085 00086 # Rectification matrix (stereo cameras only) 00087 # A rotation matrix aligning the camera coordinate system to the ideal 00088 # stereo image plane so that epipolar lines in both stereo images are 00089 # parallel. 00090 float64[9] R # 3x3 row-major matrix 00091 00092 # Projection/camera matrix 00093 # [fx' 0 cx' Tx] 00094 # P = [ 0 fy' cy' Ty] 00095 # [ 0 0 1 0] 00096 # By convention, this matrix specifies the intrinsic (camera) matrix 00097 # of the processed (rectified) image. That is, the left 3x3 portion 00098 # is the normal camera intrinsic matrix for the rectified image. 00099 # It projects 3D points in the camera coordinate frame to 2D pixel 00100 # coordinates using the focal lengths (fx', fy') and principal point 00101 # (cx', cy') - these may differ from the values in K. 00102 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00103 # also have R = the identity and P[1:3,1:3] = K. 00104 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00105 # position of the optical center of the second camera in the first 00106 # camera's frame. We assume Tz = 0 so both cameras are in the same 00107 # stereo image plane. The first camera always has Tx = Ty = 0. For 00108 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00109 # Tx = -fx' * B, where B is the baseline between the cameras. 00110 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00111 # the rectified image is given by: 00112 # [u v w]' = P * [X Y Z 1]' 00113 # x = u / w 00114 # y = v / w 00115 # This holds for both images of a stereo pair. 00116 float64[12] P # 3x4 row-major matrix 00117 00118 00119 ####################################################################### 00120 # Operational Parameters # 00121 ####################################################################### 00122 # These define the image region actually captured by the camera # 00123 # driver. Although they affect the geometry of the output image, they # 00124 # may be changed freely without recalibrating the camera. # 00125 ####################################################################### 00126 00127 # Binning refers here to any camera setting which combines rectangular 00128 # neighborhoods of pixels into larger "super-pixels." It reduces the 00129 # resolution of the output image to 00130 # (width / binning_x) x (height / binning_y). 00131 # The default values binning_x = binning_y = 0 is considered the same 00132 # as binning_x = binning_y = 1 (no subsampling). 00133 uint32 binning_x 00134 uint32 binning_y 00135 00136 # Region of interest (subwindow of full camera resolution), given in 00137 # full resolution (unbinned) image coordinates. A particular ROI 00138 # always denotes the same window of pixels on the camera sensor, 00139 # regardless of binning settings. 00140 # The default setting of roi (all values 0) is considered the same as 00141 # full resolution (roi.width = width, roi.height = height). 00142 RegionOfInterest roi 00143 00144 ================================================================================ 00145 MSG: std_msgs/Header 00146 # Standard metadata for higher-level stamped data types. 00147 # This is generally used to communicate timestamped data 00148 # in a particular coordinate frame. 00149 # 00150 # sequence ID: consecutively increasing ID 00151 uint32 seq 00152 #Two-integer timestamp that is expressed as: 00153 # * stamp.secs: seconds (stamp_secs) since epoch 00154 # * stamp.nsecs: nanoseconds since stamp_secs 00155 # time-handling sugar is provided by the client library 00156 time stamp 00157 #Frame this data is associated with 00158 # 0: no frame 00159 # 1: global frame 00160 string frame_id 00161 00162 ================================================================================ 00163 MSG: sensor_msgs/RegionOfInterest 00164 # This message is used to specify a region of interest within an image. 00165 # 00166 # When used to specify the ROI setting of the camera when the image was 00167 # taken, the height and width fields should either match the height and 00168 # width fields for the associated image; or height = width = 0 00169 # indicates that the full resolution image was captured. 00170 00171 uint32 x_offset # Leftmost pixel of the ROI 00172 # (0 if the ROI includes the left edge of the image) 00173 uint32 y_offset # Topmost pixel of the ROI 00174 # (0 if the ROI includes the top edge of the image) 00175 uint32 height # Height of ROI 00176 uint32 width # Width of ROI 00177 00178 # True if a distinct rectified ROI should be calculated from the "raw" 00179 # ROI in this message. Typically this should be False if the full image 00180 # is captured (ROI not used), and True if a subwindow is captured (ROI 00181 # used). 00182 bool do_rectify 00183 00184 """ 00185 __slots__ = ['header','height','width','distortion_model','D','K','R','P','binning_x','binning_y','roi'] 00186 _slot_types = ['Header','uint32','uint32','string','float64[]','float64[9]','float64[9]','float64[12]','uint32','uint32','sensor_msgs/RegionOfInterest'] 00187 00188 def __init__(self, *args, **kwds): 00189 """ 00190 Constructor. Any message fields that are implicitly/explicitly 00191 set to None will be assigned a default value. The recommend 00192 use is keyword arguments as this is more robust to future message 00193 changes. You cannot mix in-order arguments and keyword arguments. 00194 00195 The available fields are: 00196 header,height,width,distortion_model,D,K,R,P,binning_x,binning_y,roi 00197 00198 @param args: complete set of field values, in .msg order 00199 @param kwds: use keyword arguments corresponding to message field names 00200 to set specific fields. 00201 """ 00202 if args or kwds: 00203 super(CameraInfo, self).__init__(*args, **kwds) 00204 #message fields cannot be None, assign default values for those that are 00205 if self.header is None: 00206 self.header = std_msgs.msg._Header.Header() 00207 if self.height is None: 00208 self.height = 0 00209 if self.width is None: 00210 self.width = 0 00211 if self.distortion_model is None: 00212 self.distortion_model = '' 00213 if self.D is None: 00214 self.D = [] 00215 if self.K is None: 00216 self.K = [0.,0.,0.,0.,0.,0.,0.,0.,0.] 00217 if self.R is None: 00218 self.R = [0.,0.,0.,0.,0.,0.,0.,0.,0.] 00219 if self.P is None: 00220 self.P = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] 00221 if self.binning_x is None: 00222 self.binning_x = 0 00223 if self.binning_y is None: 00224 self.binning_y = 0 00225 if self.roi is None: 00226 self.roi = sensor_msgs.msg.RegionOfInterest() 00227 else: 00228 self.header = std_msgs.msg._Header.Header() 00229 self.height = 0 00230 self.width = 0 00231 self.distortion_model = '' 00232 self.D = [] 00233 self.K = [0.,0.,0.,0.,0.,0.,0.,0.,0.] 00234 self.R = [0.,0.,0.,0.,0.,0.,0.,0.,0.] 00235 self.P = [0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.,0.] 00236 self.binning_x = 0 00237 self.binning_y = 0 00238 self.roi = sensor_msgs.msg.RegionOfInterest() 00239 00240 def _get_types(self): 00241 """ 00242 internal API method 00243 """ 00244 return self._slot_types 00245 00246 def serialize(self, buff): 00247 """ 00248 serialize message into buffer 00249 @param buff: buffer 00250 @type buff: StringIO 00251 """ 00252 try: 00253 _x = self 00254 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00255 _x = self.header.frame_id 00256 length = len(_x) 00257 buff.write(struct.pack('<I%ss'%length, length, _x)) 00258 _x = self 00259 buff.write(_struct_2I.pack(_x.height, _x.width)) 00260 _x = self.distortion_model 00261 length = len(_x) 00262 buff.write(struct.pack('<I%ss'%length, length, _x)) 00263 length = len(self.D) 00264 buff.write(_struct_I.pack(length)) 00265 pattern = '<%sd'%length 00266 buff.write(struct.pack(pattern, *self.D)) 00267 buff.write(_struct_9d.pack(*self.K)) 00268 buff.write(_struct_9d.pack(*self.R)) 00269 buff.write(_struct_12d.pack(*self.P)) 00270 _x = self 00271 buff.write(_struct_6IB.pack(_x.binning_x, _x.binning_y, _x.roi.x_offset, _x.roi.y_offset, _x.roi.height, _x.roi.width, _x.roi.do_rectify)) 00272 except struct.error as se: self._check_types(se) 00273 except TypeError as te: self._check_types(te) 00274 00275 def deserialize(self, str): 00276 """ 00277 unpack serialized message in str into this message instance 00278 @param str: byte array of serialized message 00279 @type str: str 00280 """ 00281 try: 00282 if self.header is None: 00283 self.header = std_msgs.msg._Header.Header() 00284 if self.roi is None: 00285 self.roi = sensor_msgs.msg.RegionOfInterest() 00286 end = 0 00287 _x = self 00288 start = end 00289 end += 12 00290 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00291 start = end 00292 end += 4 00293 (length,) = _struct_I.unpack(str[start:end]) 00294 start = end 00295 end += length 00296 self.header.frame_id = str[start:end] 00297 _x = self 00298 start = end 00299 end += 8 00300 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 00301 start = end 00302 end += 4 00303 (length,) = _struct_I.unpack(str[start:end]) 00304 start = end 00305 end += length 00306 self.distortion_model = str[start:end] 00307 start = end 00308 end += 4 00309 (length,) = _struct_I.unpack(str[start:end]) 00310 pattern = '<%sd'%length 00311 start = end 00312 end += struct.calcsize(pattern) 00313 self.D = struct.unpack(pattern, str[start:end]) 00314 start = end 00315 end += 72 00316 self.K = _struct_9d.unpack(str[start:end]) 00317 start = end 00318 end += 72 00319 self.R = _struct_9d.unpack(str[start:end]) 00320 start = end 00321 end += 96 00322 self.P = _struct_12d.unpack(str[start:end]) 00323 _x = self 00324 start = end 00325 end += 25 00326 (_x.binning_x, _x.binning_y, _x.roi.x_offset, _x.roi.y_offset, _x.roi.height, _x.roi.width, _x.roi.do_rectify,) = _struct_6IB.unpack(str[start:end]) 00327 self.roi.do_rectify = bool(self.roi.do_rectify) 00328 return self 00329 except struct.error as e: 00330 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00331 00332 00333 def serialize_numpy(self, buff, numpy): 00334 """ 00335 serialize message with numpy array types into buffer 00336 @param buff: buffer 00337 @type buff: StringIO 00338 @param numpy: numpy python module 00339 @type numpy module 00340 """ 00341 try: 00342 _x = self 00343 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00344 _x = self.header.frame_id 00345 length = len(_x) 00346 buff.write(struct.pack('<I%ss'%length, length, _x)) 00347 _x = self 00348 buff.write(_struct_2I.pack(_x.height, _x.width)) 00349 _x = self.distortion_model 00350 length = len(_x) 00351 buff.write(struct.pack('<I%ss'%length, length, _x)) 00352 length = len(self.D) 00353 buff.write(_struct_I.pack(length)) 00354 pattern = '<%sd'%length 00355 buff.write(self.D.tostring()) 00356 buff.write(self.K.tostring()) 00357 buff.write(self.R.tostring()) 00358 buff.write(self.P.tostring()) 00359 _x = self 00360 buff.write(_struct_6IB.pack(_x.binning_x, _x.binning_y, _x.roi.x_offset, _x.roi.y_offset, _x.roi.height, _x.roi.width, _x.roi.do_rectify)) 00361 except struct.error as se: self._check_types(se) 00362 except TypeError as te: self._check_types(te) 00363 00364 def deserialize_numpy(self, str, numpy): 00365 """ 00366 unpack serialized message in str into this message instance using numpy for array types 00367 @param str: byte array of serialized message 00368 @type str: str 00369 @param numpy: numpy python module 00370 @type numpy: module 00371 """ 00372 try: 00373 if self.header is None: 00374 self.header = std_msgs.msg._Header.Header() 00375 if self.roi is None: 00376 self.roi = sensor_msgs.msg.RegionOfInterest() 00377 end = 0 00378 _x = self 00379 start = end 00380 end += 12 00381 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00382 start = end 00383 end += 4 00384 (length,) = _struct_I.unpack(str[start:end]) 00385 start = end 00386 end += length 00387 self.header.frame_id = str[start:end] 00388 _x = self 00389 start = end 00390 end += 8 00391 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end]) 00392 start = end 00393 end += 4 00394 (length,) = _struct_I.unpack(str[start:end]) 00395 start = end 00396 end += length 00397 self.distortion_model = str[start:end] 00398 start = end 00399 end += 4 00400 (length,) = _struct_I.unpack(str[start:end]) 00401 pattern = '<%sd'%length 00402 start = end 00403 end += struct.calcsize(pattern) 00404 self.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00405 start = end 00406 end += 72 00407 self.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 00408 start = end 00409 end += 72 00410 self.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 00411 start = end 00412 end += 96 00413 self.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 00414 _x = self 00415 start = end 00416 end += 25 00417 (_x.binning_x, _x.binning_y, _x.roi.x_offset, _x.roi.y_offset, _x.roi.height, _x.roi.width, _x.roi.do_rectify,) = _struct_6IB.unpack(str[start:end]) 00418 self.roi.do_rectify = bool(self.roi.do_rectify) 00419 return self 00420 except struct.error as e: 00421 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00422 00423 _struct_I = roslib.message.struct_I 00424 _struct_6IB = struct.Struct("<6IB") 00425 _struct_3I = struct.Struct("<3I") 00426 _struct_12d = struct.Struct("<12d") 00427 _struct_2I = struct.Struct("<2I") 00428 _struct_9d = struct.Struct("<9d")