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