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