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 geometry_msgs.msg
00007 import std_msgs.msg
00008 import geometric_shapes_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
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 geometric_shapes_msgs/Shape mesh
00239 geometry_msgs/PoseStamped pose
00240
00241 ================================================================================
00242 MSG: geometric_shapes_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
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
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, se: self._check_types(se)
00426 except TypeError, 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 xrange(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 xrange(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 xrange(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, e:
00587 raise roslib.message.DeserializationError(e)
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
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, se: self._check_types(se)
00676 except TypeError, 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 xrange(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 xrange(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 xrange(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, e:
00839 raise roslib.message.DeserializationError(e)
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")