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