$search
00001 """autogenerated by genmsg_py from CameraMeasurement.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import geometry_msgs.msg 00006 import calibration_msgs.msg 00007 import sensor_msgs.msg 00008 import std_msgs.msg 00009 00010 class CameraMeasurement(roslib.message.Message): 00011 _md5sum = "82fd8e2ebaf7cfafd8e2a52b210c7523" 00012 _type = "calibration_msgs/CameraMeasurement" 00013 _has_header = True #flag to mark the presence of a Header object 00014 _full_text = """Header header 00015 string camera_id 00016 ImagePoint[] image_points 00017 sensor_msgs/CameraInfo cam_info 00018 00019 # True -> The extra debugging fields are populated 00020 bool verbose 00021 00022 # Extra, partially processed data. Only needed for debugging 00023 sensor_msgs/Image image 00024 sensor_msgs/Image image_rect 00025 calibration_msgs/CalibrationPattern features 00026 00027 ================================================================================ 00028 MSG: std_msgs/Header 00029 # Standard metadata for higher-level stamped data types. 00030 # This is generally used to communicate timestamped data 00031 # in a particular coordinate frame. 00032 # 00033 # sequence ID: consecutively increasing ID 00034 uint32 seq 00035 #Two-integer timestamp that is expressed as: 00036 # * stamp.secs: seconds (stamp_secs) since epoch 00037 # * stamp.nsecs: nanoseconds since stamp_secs 00038 # time-handling sugar is provided by the client library 00039 time stamp 00040 #Frame this data is associated with 00041 # 0: no frame 00042 # 1: global frame 00043 string frame_id 00044 00045 ================================================================================ 00046 MSG: calibration_msgs/ImagePoint 00047 float32 x 00048 float32 y 00049 00050 ================================================================================ 00051 MSG: sensor_msgs/CameraInfo 00052 # This message defines meta information for a camera. It should be in a 00053 # camera namespace on topic "camera_info" and accompanied by up to five 00054 # image topics named: 00055 # 00056 # image_raw - raw data from the camera driver, possibly Bayer encoded 00057 # image - monochrome, distorted 00058 # image_color - color, distorted 00059 # image_rect - monochrome, rectified 00060 # image_rect_color - color, rectified 00061 # 00062 # The image_pipeline contains packages (image_proc, stereo_image_proc) 00063 # for producing the four processed image topics from image_raw and 00064 # camera_info. The meaning of the camera parameters are described in 00065 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo. 00066 # 00067 # The image_geometry package provides a user-friendly interface to 00068 # common operations using this meta information. If you want to, e.g., 00069 # project a 3d point into image coordinates, we strongly recommend 00070 # using image_geometry. 00071 # 00072 # If the camera is uncalibrated, the matrices D, K, R, P should be left 00073 # zeroed out. In particular, clients may assume that K[0] == 0.0 00074 # indicates an uncalibrated camera. 00075 00076 ####################################################################### 00077 # Image acquisition info # 00078 ####################################################################### 00079 00080 # Time of image acquisition, camera coordinate frame ID 00081 Header header # Header timestamp should be acquisition time of image 00082 # Header frame_id should be optical frame of camera 00083 # origin of frame should be optical center of camera 00084 # +x should point to the right in the image 00085 # +y should point down in the image 00086 # +z should point into the plane of the image 00087 00088 00089 ####################################################################### 00090 # Calibration Parameters # 00091 ####################################################################### 00092 # These are fixed during camera calibration. Their values will be the # 00093 # same in all messages until the camera is recalibrated. Note that # 00094 # self-calibrating systems may "recalibrate" frequently. # 00095 # # 00096 # The internal parameters can be used to warp a raw (distorted) image # 00097 # to: # 00098 # 1. An undistorted image (requires D and K) # 00099 # 2. A rectified image (requires D, K, R) # 00100 # The projection matrix P projects 3D points into the rectified image.# 00101 ####################################################################### 00102 00103 # The image dimensions with which the camera was calibrated. Normally 00104 # this will be the full camera resolution in pixels. 00105 uint32 height 00106 uint32 width 00107 00108 # The distortion model used. Supported models are listed in 00109 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a 00110 # simple model of radial and tangential distortion - is sufficent. 00111 string distortion_model 00112 00113 # The distortion parameters, size depending on the distortion model. 00114 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3). 00115 float64[] D 00116 00117 # Intrinsic camera matrix for the raw (distorted) images. 00118 # [fx 0 cx] 00119 # K = [ 0 fy cy] 00120 # [ 0 0 1] 00121 # Projects 3D points in the camera coordinate frame to 2D pixel 00122 # coordinates using the focal lengths (fx, fy) and principal point 00123 # (cx, cy). 00124 float64[9] K # 3x3 row-major matrix 00125 00126 # Rectification matrix (stereo cameras only) 00127 # A rotation matrix aligning the camera coordinate system to the ideal 00128 # stereo image plane so that epipolar lines in both stereo images are 00129 # parallel. 00130 float64[9] R # 3x3 row-major matrix 00131 00132 # Projection/camera matrix 00133 # [fx' 0 cx' Tx] 00134 # P = [ 0 fy' cy' Ty] 00135 # [ 0 0 1 0] 00136 # By convention, this matrix specifies the intrinsic (camera) matrix 00137 # of the processed (rectified) image. That is, the left 3x3 portion 00138 # is the normal camera intrinsic matrix for the rectified image. 00139 # It projects 3D points in the camera coordinate frame to 2D pixel 00140 # coordinates using the focal lengths (fx', fy') and principal point 00141 # (cx', cy') - these may differ from the values in K. 00142 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will 00143 # also have R = the identity and P[1:3,1:3] = K. 00144 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the 00145 # position of the optical center of the second camera in the first 00146 # camera's frame. We assume Tz = 0 so both cameras are in the same 00147 # stereo image plane. The first camera always has Tx = Ty = 0. For 00148 # the right (second) camera of a horizontal stereo pair, Ty = 0 and 00149 # Tx = -fx' * B, where B is the baseline between the cameras. 00150 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto 00151 # the rectified image is given by: 00152 # [u v w]' = P * [X Y Z 1]' 00153 # x = u / w 00154 # y = v / w 00155 # This holds for both images of a stereo pair. 00156 float64[12] P # 3x4 row-major matrix 00157 00158 00159 ####################################################################### 00160 # Operational Parameters # 00161 ####################################################################### 00162 # These define the image region actually captured by the camera # 00163 # driver. Although they affect the geometry of the output image, they # 00164 # may be changed freely without recalibrating the camera. # 00165 ####################################################################### 00166 00167 # Binning refers here to any camera setting which combines rectangular 00168 # neighborhoods of pixels into larger "super-pixels." It reduces the 00169 # resolution of the output image to 00170 # (width / binning_x) x (height / binning_y). 00171 # The default values binning_x = binning_y = 0 is considered the same 00172 # as binning_x = binning_y = 1 (no subsampling). 00173 uint32 binning_x 00174 uint32 binning_y 00175 00176 # Region of interest (subwindow of full camera resolution), given in 00177 # full resolution (unbinned) image coordinates. A particular ROI 00178 # always denotes the same window of pixels on the camera sensor, 00179 # regardless of binning settings. 00180 # The default setting of roi (all values 0) is considered the same as 00181 # full resolution (roi.width = width, roi.height = height). 00182 RegionOfInterest roi 00183 00184 ================================================================================ 00185 MSG: sensor_msgs/RegionOfInterest 00186 # This message is used to specify a region of interest within an image. 00187 # 00188 # When used to specify the ROI setting of the camera when the image was 00189 # taken, the height and width fields should either match the height and 00190 # width fields for the associated image; or height = width = 0 00191 # indicates that the full resolution image was captured. 00192 00193 uint32 x_offset # Leftmost pixel of the ROI 00194 # (0 if the ROI includes the left edge of the image) 00195 uint32 y_offset # Topmost pixel of the ROI 00196 # (0 if the ROI includes the top edge of the image) 00197 uint32 height # Height of ROI 00198 uint32 width # Width of ROI 00199 00200 # True if a distinct rectified ROI should be calculated from the "raw" 00201 # ROI in this message. Typically this should be False if the full image 00202 # is captured (ROI not used), and True if a subwindow is captured (ROI 00203 # used). 00204 bool do_rectify 00205 00206 ================================================================================ 00207 MSG: sensor_msgs/Image 00208 # This message contains an uncompressed image 00209 # (0, 0) is at top-left corner of image 00210 # 00211 00212 Header header # Header timestamp should be acquisition time of image 00213 # Header frame_id should be optical frame of camera 00214 # origin of frame should be optical center of cameara 00215 # +x should point to the right in the image 00216 # +y should point down in the image 00217 # +z should point into to plane of the image 00218 # If the frame_id here and the frame_id of the CameraInfo 00219 # message associated with the image conflict 00220 # the behavior is undefined 00221 00222 uint32 height # image height, that is, number of rows 00223 uint32 width # image width, that is, number of columns 00224 00225 # The legal values for encoding are in file src/image_encodings.cpp 00226 # If you want to standardize a new string format, join 00227 # ros-users@lists.sourceforge.net and send an email proposing a new encoding. 00228 00229 string encoding # Encoding of pixels -- channel meaning, ordering, size 00230 # taken from the list of strings in src/image_encodings.cpp 00231 00232 uint8 is_bigendian # is this data bigendian? 00233 uint32 step # Full row length in bytes 00234 uint8[] data # actual matrix data, size is (step * rows) 00235 00236 ================================================================================ 00237 MSG: calibration_msgs/CalibrationPattern 00238 Header header 00239 geometry_msgs/Point32[] object_points 00240 ImagePoint[] image_points 00241 uint8 success 00242 00243 ================================================================================ 00244 MSG: geometry_msgs/Point32 00245 # This contains the position of a point in free space(with 32 bits of precision). 00246 # It is recommeded to use Point wherever possible instead of Point32. 00247 # 00248 # This recommendation is to promote interoperability. 00249 # 00250 # This message is designed to take up less space when sending 00251 # lots of points at once, as in the case of a PointCloud. 00252 00253 float32 x 00254 float32 y 00255 float32 z 00256 """ 00257 __slots__ = ['header','camera_id','image_points','cam_info','verbose','image','image_rect','features'] 00258 _slot_types = ['Header','string','calibration_msgs/ImagePoint[]','sensor_msgs/CameraInfo','bool','sensor_msgs/Image','sensor_msgs/Image','calibration_msgs/CalibrationPattern'] 00259 00260 def __init__(self, *args, **kwds): 00261 """ 00262 Constructor. Any message fields that are implicitly/explicitly 00263 set to None will be assigned a default value. The recommend 00264 use is keyword arguments as this is more robust to future message 00265 changes. You cannot mix in-order arguments and keyword arguments. 00266 00267 The available fields are: 00268 header,camera_id,image_points,cam_info,verbose,image,image_rect,features 00269 00270 @param args: complete set of field values, in .msg order 00271 @param kwds: use keyword arguments corresponding to message field names 00272 to set specific fields. 00273 """ 00274 if args or kwds: 00275 super(CameraMeasurement, self).__init__(*args, **kwds) 00276 #message fields cannot be None, assign default values for those that are 00277 if self.header is None: 00278 self.header = std_msgs.msg._Header.Header() 00279 if self.camera_id is None: 00280 self.camera_id = '' 00281 if self.image_points is None: 00282 self.image_points = [] 00283 if self.cam_info is None: 00284 self.cam_info = sensor_msgs.msg.CameraInfo() 00285 if self.verbose is None: 00286 self.verbose = False 00287 if self.image is None: 00288 self.image = sensor_msgs.msg.Image() 00289 if self.image_rect is None: 00290 self.image_rect = sensor_msgs.msg.Image() 00291 if self.features is None: 00292 self.features = calibration_msgs.msg.CalibrationPattern() 00293 else: 00294 self.header = std_msgs.msg._Header.Header() 00295 self.camera_id = '' 00296 self.image_points = [] 00297 self.cam_info = sensor_msgs.msg.CameraInfo() 00298 self.verbose = False 00299 self.image = sensor_msgs.msg.Image() 00300 self.image_rect = sensor_msgs.msg.Image() 00301 self.features = calibration_msgs.msg.CalibrationPattern() 00302 00303 def _get_types(self): 00304 """ 00305 internal API method 00306 """ 00307 return self._slot_types 00308 00309 def serialize(self, buff): 00310 """ 00311 serialize message into buffer 00312 @param buff: buffer 00313 @type buff: StringIO 00314 """ 00315 try: 00316 _x = self 00317 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00318 _x = self.header.frame_id 00319 length = len(_x) 00320 buff.write(struct.pack('<I%ss'%length, length, _x)) 00321 _x = self.camera_id 00322 length = len(_x) 00323 buff.write(struct.pack('<I%ss'%length, length, _x)) 00324 length = len(self.image_points) 00325 buff.write(_struct_I.pack(length)) 00326 for val1 in self.image_points: 00327 _x = val1 00328 buff.write(_struct_2f.pack(_x.x, _x.y)) 00329 _x = self 00330 buff.write(_struct_3I.pack(_x.cam_info.header.seq, _x.cam_info.header.stamp.secs, _x.cam_info.header.stamp.nsecs)) 00331 _x = self.cam_info.header.frame_id 00332 length = len(_x) 00333 buff.write(struct.pack('<I%ss'%length, length, _x)) 00334 _x = self 00335 buff.write(_struct_2I.pack(_x.cam_info.height, _x.cam_info.width)) 00336 _x = self.cam_info.distortion_model 00337 length = len(_x) 00338 buff.write(struct.pack('<I%ss'%length, length, _x)) 00339 length = len(self.cam_info.D) 00340 buff.write(_struct_I.pack(length)) 00341 pattern = '<%sd'%length 00342 buff.write(struct.pack(pattern, *self.cam_info.D)) 00343 buff.write(_struct_9d.pack(*self.cam_info.K)) 00344 buff.write(_struct_9d.pack(*self.cam_info.R)) 00345 buff.write(_struct_12d.pack(*self.cam_info.P)) 00346 _x = self 00347 buff.write(_struct_6I2B3I.pack(_x.cam_info.binning_x, _x.cam_info.binning_y, _x.cam_info.roi.x_offset, _x.cam_info.roi.y_offset, _x.cam_info.roi.height, _x.cam_info.roi.width, _x.cam_info.roi.do_rectify, _x.verbose, _x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs)) 00348 _x = self.image.header.frame_id 00349 length = len(_x) 00350 buff.write(struct.pack('<I%ss'%length, length, _x)) 00351 _x = self 00352 buff.write(_struct_2I.pack(_x.image.height, _x.image.width)) 00353 _x = self.image.encoding 00354 length = len(_x) 00355 buff.write(struct.pack('<I%ss'%length, length, _x)) 00356 _x = self 00357 buff.write(_struct_BI.pack(_x.image.is_bigendian, _x.image.step)) 00358 _x = self.image.data 00359 length = len(_x) 00360 # - if encoded as a list instead, serialize as bytes instead of string 00361 if type(_x) in [list, tuple]: 00362 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00363 else: 00364 buff.write(struct.pack('<I%ss'%length, length, _x)) 00365 _x = self 00366 buff.write(_struct_3I.pack(_x.image_rect.header.seq, _x.image_rect.header.stamp.secs, _x.image_rect.header.stamp.nsecs)) 00367 _x = self.image_rect.header.frame_id 00368 length = len(_x) 00369 buff.write(struct.pack('<I%ss'%length, length, _x)) 00370 _x = self 00371 buff.write(_struct_2I.pack(_x.image_rect.height, _x.image_rect.width)) 00372 _x = self.image_rect.encoding 00373 length = len(_x) 00374 buff.write(struct.pack('<I%ss'%length, length, _x)) 00375 _x = self 00376 buff.write(_struct_BI.pack(_x.image_rect.is_bigendian, _x.image_rect.step)) 00377 _x = self.image_rect.data 00378 length = len(_x) 00379 # - if encoded as a list instead, serialize as bytes instead of string 00380 if type(_x) in [list, tuple]: 00381 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00382 else: 00383 buff.write(struct.pack('<I%ss'%length, length, _x)) 00384 _x = self 00385 buff.write(_struct_3I.pack(_x.features.header.seq, _x.features.header.stamp.secs, _x.features.header.stamp.nsecs)) 00386 _x = self.features.header.frame_id 00387 length = len(_x) 00388 buff.write(struct.pack('<I%ss'%length, length, _x)) 00389 length = len(self.features.object_points) 00390 buff.write(_struct_I.pack(length)) 00391 for val1 in self.features.object_points: 00392 _x = val1 00393 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00394 length = len(self.features.image_points) 00395 buff.write(_struct_I.pack(length)) 00396 for val1 in self.features.image_points: 00397 _x = val1 00398 buff.write(_struct_2f.pack(_x.x, _x.y)) 00399 buff.write(_struct_B.pack(self.features.success)) 00400 except struct.error as se: self._check_types(se) 00401 except TypeError as te: self._check_types(te) 00402 00403 def deserialize(self, str): 00404 """ 00405 unpack serialized message in str into this message instance 00406 @param str: byte array of serialized message 00407 @type str: str 00408 """ 00409 try: 00410 if self.header is None: 00411 self.header = std_msgs.msg._Header.Header() 00412 if self.cam_info is None: 00413 self.cam_info = sensor_msgs.msg.CameraInfo() 00414 if self.image is None: 00415 self.image = sensor_msgs.msg.Image() 00416 if self.image_rect is None: 00417 self.image_rect = sensor_msgs.msg.Image() 00418 if self.features is None: 00419 self.features = calibration_msgs.msg.CalibrationPattern() 00420 end = 0 00421 _x = self 00422 start = end 00423 end += 12 00424 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00425 start = end 00426 end += 4 00427 (length,) = _struct_I.unpack(str[start:end]) 00428 start = end 00429 end += length 00430 self.header.frame_id = str[start:end] 00431 start = end 00432 end += 4 00433 (length,) = _struct_I.unpack(str[start:end]) 00434 start = end 00435 end += length 00436 self.camera_id = str[start:end] 00437 start = end 00438 end += 4 00439 (length,) = _struct_I.unpack(str[start:end]) 00440 self.image_points = [] 00441 for i in range(0, length): 00442 val1 = calibration_msgs.msg.ImagePoint() 00443 _x = val1 00444 start = end 00445 end += 8 00446 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 00447 self.image_points.append(val1) 00448 _x = self 00449 start = end 00450 end += 12 00451 (_x.cam_info.header.seq, _x.cam_info.header.stamp.secs, _x.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00452 start = end 00453 end += 4 00454 (length,) = _struct_I.unpack(str[start:end]) 00455 start = end 00456 end += length 00457 self.cam_info.header.frame_id = str[start:end] 00458 _x = self 00459 start = end 00460 end += 8 00461 (_x.cam_info.height, _x.cam_info.width,) = _struct_2I.unpack(str[start:end]) 00462 start = end 00463 end += 4 00464 (length,) = _struct_I.unpack(str[start:end]) 00465 start = end 00466 end += length 00467 self.cam_info.distortion_model = str[start:end] 00468 start = end 00469 end += 4 00470 (length,) = _struct_I.unpack(str[start:end]) 00471 pattern = '<%sd'%length 00472 start = end 00473 end += struct.calcsize(pattern) 00474 self.cam_info.D = struct.unpack(pattern, str[start:end]) 00475 start = end 00476 end += 72 00477 self.cam_info.K = _struct_9d.unpack(str[start:end]) 00478 start = end 00479 end += 72 00480 self.cam_info.R = _struct_9d.unpack(str[start:end]) 00481 start = end 00482 end += 96 00483 self.cam_info.P = _struct_12d.unpack(str[start:end]) 00484 _x = self 00485 start = end 00486 end += 38 00487 (_x.cam_info.binning_x, _x.cam_info.binning_y, _x.cam_info.roi.x_offset, _x.cam_info.roi.y_offset, _x.cam_info.roi.height, _x.cam_info.roi.width, _x.cam_info.roi.do_rectify, _x.verbose, _x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs,) = _struct_6I2B3I.unpack(str[start:end]) 00488 self.cam_info.roi.do_rectify = bool(self.cam_info.roi.do_rectify) 00489 self.verbose = bool(self.verbose) 00490 start = end 00491 end += 4 00492 (length,) = _struct_I.unpack(str[start:end]) 00493 start = end 00494 end += length 00495 self.image.header.frame_id = str[start:end] 00496 _x = self 00497 start = end 00498 end += 8 00499 (_x.image.height, _x.image.width,) = _struct_2I.unpack(str[start:end]) 00500 start = end 00501 end += 4 00502 (length,) = _struct_I.unpack(str[start:end]) 00503 start = end 00504 end += length 00505 self.image.encoding = str[start:end] 00506 _x = self 00507 start = end 00508 end += 5 00509 (_x.image.is_bigendian, _x.image.step,) = _struct_BI.unpack(str[start:end]) 00510 start = end 00511 end += 4 00512 (length,) = _struct_I.unpack(str[start:end]) 00513 start = end 00514 end += length 00515 self.image.data = str[start:end] 00516 _x = self 00517 start = end 00518 end += 12 00519 (_x.image_rect.header.seq, _x.image_rect.header.stamp.secs, _x.image_rect.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00520 start = end 00521 end += 4 00522 (length,) = _struct_I.unpack(str[start:end]) 00523 start = end 00524 end += length 00525 self.image_rect.header.frame_id = str[start:end] 00526 _x = self 00527 start = end 00528 end += 8 00529 (_x.image_rect.height, _x.image_rect.width,) = _struct_2I.unpack(str[start:end]) 00530 start = end 00531 end += 4 00532 (length,) = _struct_I.unpack(str[start:end]) 00533 start = end 00534 end += length 00535 self.image_rect.encoding = str[start:end] 00536 _x = self 00537 start = end 00538 end += 5 00539 (_x.image_rect.is_bigendian, _x.image_rect.step,) = _struct_BI.unpack(str[start:end]) 00540 start = end 00541 end += 4 00542 (length,) = _struct_I.unpack(str[start:end]) 00543 start = end 00544 end += length 00545 self.image_rect.data = str[start:end] 00546 _x = self 00547 start = end 00548 end += 12 00549 (_x.features.header.seq, _x.features.header.stamp.secs, _x.features.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00550 start = end 00551 end += 4 00552 (length,) = _struct_I.unpack(str[start:end]) 00553 start = end 00554 end += length 00555 self.features.header.frame_id = str[start:end] 00556 start = end 00557 end += 4 00558 (length,) = _struct_I.unpack(str[start:end]) 00559 self.features.object_points = [] 00560 for i in range(0, length): 00561 val1 = geometry_msgs.msg.Point32() 00562 _x = val1 00563 start = end 00564 end += 12 00565 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00566 self.features.object_points.append(val1) 00567 start = end 00568 end += 4 00569 (length,) = _struct_I.unpack(str[start:end]) 00570 self.features.image_points = [] 00571 for i in range(0, length): 00572 val1 = calibration_msgs.msg.ImagePoint() 00573 _x = val1 00574 start = end 00575 end += 8 00576 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 00577 self.features.image_points.append(val1) 00578 start = end 00579 end += 1 00580 (self.features.success,) = _struct_B.unpack(str[start:end]) 00581 return self 00582 except struct.error as e: 00583 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00584 00585 00586 def serialize_numpy(self, buff, numpy): 00587 """ 00588 serialize message with numpy array types into buffer 00589 @param buff: buffer 00590 @type buff: StringIO 00591 @param numpy: numpy python module 00592 @type numpy module 00593 """ 00594 try: 00595 _x = self 00596 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00597 _x = self.header.frame_id 00598 length = len(_x) 00599 buff.write(struct.pack('<I%ss'%length, length, _x)) 00600 _x = self.camera_id 00601 length = len(_x) 00602 buff.write(struct.pack('<I%ss'%length, length, _x)) 00603 length = len(self.image_points) 00604 buff.write(_struct_I.pack(length)) 00605 for val1 in self.image_points: 00606 _x = val1 00607 buff.write(_struct_2f.pack(_x.x, _x.y)) 00608 _x = self 00609 buff.write(_struct_3I.pack(_x.cam_info.header.seq, _x.cam_info.header.stamp.secs, _x.cam_info.header.stamp.nsecs)) 00610 _x = self.cam_info.header.frame_id 00611 length = len(_x) 00612 buff.write(struct.pack('<I%ss'%length, length, _x)) 00613 _x = self 00614 buff.write(_struct_2I.pack(_x.cam_info.height, _x.cam_info.width)) 00615 _x = self.cam_info.distortion_model 00616 length = len(_x) 00617 buff.write(struct.pack('<I%ss'%length, length, _x)) 00618 length = len(self.cam_info.D) 00619 buff.write(_struct_I.pack(length)) 00620 pattern = '<%sd'%length 00621 buff.write(self.cam_info.D.tostring()) 00622 buff.write(self.cam_info.K.tostring()) 00623 buff.write(self.cam_info.R.tostring()) 00624 buff.write(self.cam_info.P.tostring()) 00625 _x = self 00626 buff.write(_struct_6I2B3I.pack(_x.cam_info.binning_x, _x.cam_info.binning_y, _x.cam_info.roi.x_offset, _x.cam_info.roi.y_offset, _x.cam_info.roi.height, _x.cam_info.roi.width, _x.cam_info.roi.do_rectify, _x.verbose, _x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs)) 00627 _x = self.image.header.frame_id 00628 length = len(_x) 00629 buff.write(struct.pack('<I%ss'%length, length, _x)) 00630 _x = self 00631 buff.write(_struct_2I.pack(_x.image.height, _x.image.width)) 00632 _x = self.image.encoding 00633 length = len(_x) 00634 buff.write(struct.pack('<I%ss'%length, length, _x)) 00635 _x = self 00636 buff.write(_struct_BI.pack(_x.image.is_bigendian, _x.image.step)) 00637 _x = self.image.data 00638 length = len(_x) 00639 # - if encoded as a list instead, serialize as bytes instead of string 00640 if type(_x) in [list, tuple]: 00641 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00642 else: 00643 buff.write(struct.pack('<I%ss'%length, length, _x)) 00644 _x = self 00645 buff.write(_struct_3I.pack(_x.image_rect.header.seq, _x.image_rect.header.stamp.secs, _x.image_rect.header.stamp.nsecs)) 00646 _x = self.image_rect.header.frame_id 00647 length = len(_x) 00648 buff.write(struct.pack('<I%ss'%length, length, _x)) 00649 _x = self 00650 buff.write(_struct_2I.pack(_x.image_rect.height, _x.image_rect.width)) 00651 _x = self.image_rect.encoding 00652 length = len(_x) 00653 buff.write(struct.pack('<I%ss'%length, length, _x)) 00654 _x = self 00655 buff.write(_struct_BI.pack(_x.image_rect.is_bigendian, _x.image_rect.step)) 00656 _x = self.image_rect.data 00657 length = len(_x) 00658 # - if encoded as a list instead, serialize as bytes instead of string 00659 if type(_x) in [list, tuple]: 00660 buff.write(struct.pack('<I%sB'%length, length, *_x)) 00661 else: 00662 buff.write(struct.pack('<I%ss'%length, length, _x)) 00663 _x = self 00664 buff.write(_struct_3I.pack(_x.features.header.seq, _x.features.header.stamp.secs, _x.features.header.stamp.nsecs)) 00665 _x = self.features.header.frame_id 00666 length = len(_x) 00667 buff.write(struct.pack('<I%ss'%length, length, _x)) 00668 length = len(self.features.object_points) 00669 buff.write(_struct_I.pack(length)) 00670 for val1 in self.features.object_points: 00671 _x = val1 00672 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00673 length = len(self.features.image_points) 00674 buff.write(_struct_I.pack(length)) 00675 for val1 in self.features.image_points: 00676 _x = val1 00677 buff.write(_struct_2f.pack(_x.x, _x.y)) 00678 buff.write(_struct_B.pack(self.features.success)) 00679 except struct.error as se: self._check_types(se) 00680 except TypeError as te: self._check_types(te) 00681 00682 def deserialize_numpy(self, str, numpy): 00683 """ 00684 unpack serialized message in str into this message instance using numpy for array types 00685 @param str: byte array of serialized message 00686 @type str: str 00687 @param numpy: numpy python module 00688 @type numpy: module 00689 """ 00690 try: 00691 if self.header is None: 00692 self.header = std_msgs.msg._Header.Header() 00693 if self.cam_info is None: 00694 self.cam_info = sensor_msgs.msg.CameraInfo() 00695 if self.image is None: 00696 self.image = sensor_msgs.msg.Image() 00697 if self.image_rect is None: 00698 self.image_rect = sensor_msgs.msg.Image() 00699 if self.features is None: 00700 self.features = calibration_msgs.msg.CalibrationPattern() 00701 end = 0 00702 _x = self 00703 start = end 00704 end += 12 00705 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00706 start = end 00707 end += 4 00708 (length,) = _struct_I.unpack(str[start:end]) 00709 start = end 00710 end += length 00711 self.header.frame_id = str[start:end] 00712 start = end 00713 end += 4 00714 (length,) = _struct_I.unpack(str[start:end]) 00715 start = end 00716 end += length 00717 self.camera_id = str[start:end] 00718 start = end 00719 end += 4 00720 (length,) = _struct_I.unpack(str[start:end]) 00721 self.image_points = [] 00722 for i in range(0, length): 00723 val1 = calibration_msgs.msg.ImagePoint() 00724 _x = val1 00725 start = end 00726 end += 8 00727 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 00728 self.image_points.append(val1) 00729 _x = self 00730 start = end 00731 end += 12 00732 (_x.cam_info.header.seq, _x.cam_info.header.stamp.secs, _x.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00733 start = end 00734 end += 4 00735 (length,) = _struct_I.unpack(str[start:end]) 00736 start = end 00737 end += length 00738 self.cam_info.header.frame_id = str[start:end] 00739 _x = self 00740 start = end 00741 end += 8 00742 (_x.cam_info.height, _x.cam_info.width,) = _struct_2I.unpack(str[start:end]) 00743 start = end 00744 end += 4 00745 (length,) = _struct_I.unpack(str[start:end]) 00746 start = end 00747 end += length 00748 self.cam_info.distortion_model = str[start:end] 00749 start = end 00750 end += 4 00751 (length,) = _struct_I.unpack(str[start:end]) 00752 pattern = '<%sd'%length 00753 start = end 00754 end += struct.calcsize(pattern) 00755 self.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length) 00756 start = end 00757 end += 72 00758 self.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 00759 start = end 00760 end += 72 00761 self.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9) 00762 start = end 00763 end += 96 00764 self.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12) 00765 _x = self 00766 start = end 00767 end += 38 00768 (_x.cam_info.binning_x, _x.cam_info.binning_y, _x.cam_info.roi.x_offset, _x.cam_info.roi.y_offset, _x.cam_info.roi.height, _x.cam_info.roi.width, _x.cam_info.roi.do_rectify, _x.verbose, _x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs,) = _struct_6I2B3I.unpack(str[start:end]) 00769 self.cam_info.roi.do_rectify = bool(self.cam_info.roi.do_rectify) 00770 self.verbose = bool(self.verbose) 00771 start = end 00772 end += 4 00773 (length,) = _struct_I.unpack(str[start:end]) 00774 start = end 00775 end += length 00776 self.image.header.frame_id = str[start:end] 00777 _x = self 00778 start = end 00779 end += 8 00780 (_x.image.height, _x.image.width,) = _struct_2I.unpack(str[start:end]) 00781 start = end 00782 end += 4 00783 (length,) = _struct_I.unpack(str[start:end]) 00784 start = end 00785 end += length 00786 self.image.encoding = str[start:end] 00787 _x = self 00788 start = end 00789 end += 5 00790 (_x.image.is_bigendian, _x.image.step,) = _struct_BI.unpack(str[start:end]) 00791 start = end 00792 end += 4 00793 (length,) = _struct_I.unpack(str[start:end]) 00794 start = end 00795 end += length 00796 self.image.data = str[start:end] 00797 _x = self 00798 start = end 00799 end += 12 00800 (_x.image_rect.header.seq, _x.image_rect.header.stamp.secs, _x.image_rect.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00801 start = end 00802 end += 4 00803 (length,) = _struct_I.unpack(str[start:end]) 00804 start = end 00805 end += length 00806 self.image_rect.header.frame_id = str[start:end] 00807 _x = self 00808 start = end 00809 end += 8 00810 (_x.image_rect.height, _x.image_rect.width,) = _struct_2I.unpack(str[start:end]) 00811 start = end 00812 end += 4 00813 (length,) = _struct_I.unpack(str[start:end]) 00814 start = end 00815 end += length 00816 self.image_rect.encoding = str[start:end] 00817 _x = self 00818 start = end 00819 end += 5 00820 (_x.image_rect.is_bigendian, _x.image_rect.step,) = _struct_BI.unpack(str[start:end]) 00821 start = end 00822 end += 4 00823 (length,) = _struct_I.unpack(str[start:end]) 00824 start = end 00825 end += length 00826 self.image_rect.data = str[start:end] 00827 _x = self 00828 start = end 00829 end += 12 00830 (_x.features.header.seq, _x.features.header.stamp.secs, _x.features.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00831 start = end 00832 end += 4 00833 (length,) = _struct_I.unpack(str[start:end]) 00834 start = end 00835 end += length 00836 self.features.header.frame_id = str[start:end] 00837 start = end 00838 end += 4 00839 (length,) = _struct_I.unpack(str[start:end]) 00840 self.features.object_points = [] 00841 for i in range(0, length): 00842 val1 = geometry_msgs.msg.Point32() 00843 _x = val1 00844 start = end 00845 end += 12 00846 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00847 self.features.object_points.append(val1) 00848 start = end 00849 end += 4 00850 (length,) = _struct_I.unpack(str[start:end]) 00851 self.features.image_points = [] 00852 for i in range(0, length): 00853 val1 = calibration_msgs.msg.ImagePoint() 00854 _x = val1 00855 start = end 00856 end += 8 00857 (_x.x, _x.y,) = _struct_2f.unpack(str[start:end]) 00858 self.features.image_points.append(val1) 00859 start = end 00860 end += 1 00861 (self.features.success,) = _struct_B.unpack(str[start:end]) 00862 return self 00863 except struct.error as e: 00864 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00865 00866 _struct_I = roslib.message.struct_I 00867 _struct_B = struct.Struct("<B") 00868 _struct_12d = struct.Struct("<12d") 00869 _struct_6I2B3I = struct.Struct("<6I2B3I") 00870 _struct_9d = struct.Struct("<9d") 00871 _struct_BI = struct.Struct("<BI") 00872 _struct_3f = struct.Struct("<3f") 00873 _struct_3I = struct.Struct("<3I") 00874 _struct_2f = struct.Struct("<2f") 00875 _struct_2I = struct.Struct("<2I")