$search
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 #flag to mark the presence of a Header object 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 #message fields cannot be None, assign default values for those that are 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 # - if encoded as a list instead, serialize as bytes instead of string 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 # - if encoded as a list instead, serialize as bytes instead of string 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 # - if encoded as a list instead, serialize as bytes instead of string 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) #most likely buffer underfill 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 # - if encoded as a list instead, serialize as bytes instead of string 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 # - if encoded as a list instead, serialize as bytes instead of string 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 # - if encoded as a list instead, serialize as bytes instead of string 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) #most likely buffer underfill 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")