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