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