00001 """autogenerated by genpy from object_manipulation_msgs/GraspableObject.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 sensor_msgs.msg
00008 import std_msgs.msg
00009 import geometry_msgs.msg
00010 import object_manipulation_msgs.msg
00011 import household_objects_database_msgs.msg
00012
00013 class GraspableObject(genpy.Message):
00014 _md5sum = "fb38ca9beebc9a80929b7a0397d84cf3"
00015 _type = "object_manipulation_msgs/GraspableObject"
00016 _has_header = False
00017 _full_text = """# an object that the object_manipulator can work on
00018
00019 # a graspable object can be represented in multiple ways. This message
00020 # can contain all of them. Which one is actually used is up to the receiver
00021 # of this message. When adding new representations, one must be careful that
00022 # they have reasonable lightweight defaults indicating that that particular
00023 # representation is not available.
00024
00025 # the tf frame to be used as a reference frame when combining information from
00026 # the different representations below
00027 string reference_frame_id
00028
00029 # potential recognition results from a database of models
00030 # all poses are relative to the object reference pose
00031 household_objects_database_msgs/DatabaseModelPose[] potential_models
00032
00033 # the point cloud itself
00034 sensor_msgs/PointCloud cluster
00035
00036 # a region of a PointCloud2 of interest
00037 object_manipulation_msgs/SceneRegion region
00038
00039 # the name that this object has in the collision environment
00040 string collision_name
00041 ================================================================================
00042 MSG: household_objects_database_msgs/DatabaseModelPose
00043 # Informs that a specific model from the Model Database has been
00044 # identified at a certain location
00045
00046 # the database id of the model
00047 int32 model_id
00048
00049 # the pose that it can be found in
00050 geometry_msgs/PoseStamped pose
00051
00052 # a measure of the confidence level in this detection result
00053 float32 confidence
00054
00055 # the name of the object detector that generated this detection result
00056 string detector_name
00057
00058 ================================================================================
00059 MSG: geometry_msgs/PoseStamped
00060 # A Pose with reference coordinate frame and timestamp
00061 Header header
00062 Pose pose
00063
00064 ================================================================================
00065 MSG: std_msgs/Header
00066 # Standard metadata for higher-level stamped data types.
00067 # This is generally used to communicate timestamped data
00068 # in a particular coordinate frame.
00069 #
00070 # sequence ID: consecutively increasing ID
00071 uint32 seq
00072 #Two-integer timestamp that is expressed as:
00073 # * stamp.secs: seconds (stamp_secs) since epoch
00074 # * stamp.nsecs: nanoseconds since stamp_secs
00075 # time-handling sugar is provided by the client library
00076 time stamp
00077 #Frame this data is associated with
00078 # 0: no frame
00079 # 1: global frame
00080 string frame_id
00081
00082 ================================================================================
00083 MSG: geometry_msgs/Pose
00084 # A representation of pose in free space, composed of postion and orientation.
00085 Point position
00086 Quaternion orientation
00087
00088 ================================================================================
00089 MSG: geometry_msgs/Point
00090 # This contains the position of a point in free space
00091 float64 x
00092 float64 y
00093 float64 z
00094
00095 ================================================================================
00096 MSG: geometry_msgs/Quaternion
00097 # This represents an orientation in free space in quaternion form.
00098
00099 float64 x
00100 float64 y
00101 float64 z
00102 float64 w
00103
00104 ================================================================================
00105 MSG: sensor_msgs/PointCloud
00106 # This message holds a collection of 3d points, plus optional additional
00107 # information about each point.
00108
00109 # Time of sensor data acquisition, coordinate frame ID.
00110 Header header
00111
00112 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00113 # in the frame given in the header.
00114 geometry_msgs/Point32[] points
00115
00116 # Each channel should have the same number of elements as points array,
00117 # and the data in each channel should correspond 1:1 with each point.
00118 # Channel names in common practice are listed in ChannelFloat32.msg.
00119 ChannelFloat32[] channels
00120
00121 ================================================================================
00122 MSG: geometry_msgs/Point32
00123 # This contains the position of a point in free space(with 32 bits of precision).
00124 # It is recommeded to use Point wherever possible instead of Point32.
00125 #
00126 # This recommendation is to promote interoperability.
00127 #
00128 # This message is designed to take up less space when sending
00129 # lots of points at once, as in the case of a PointCloud.
00130
00131 float32 x
00132 float32 y
00133 float32 z
00134 ================================================================================
00135 MSG: sensor_msgs/ChannelFloat32
00136 # This message is used by the PointCloud message to hold optional data
00137 # associated with each point in the cloud. The length of the values
00138 # array should be the same as the length of the points array in the
00139 # PointCloud, and each value should be associated with the corresponding
00140 # point.
00141
00142 # Channel names in existing practice include:
00143 # "u", "v" - row and column (respectively) in the left stereo image.
00144 # This is opposite to usual conventions but remains for
00145 # historical reasons. The newer PointCloud2 message has no
00146 # such problem.
00147 # "rgb" - For point clouds produced by color stereo cameras. uint8
00148 # (R,G,B) values packed into the least significant 24 bits,
00149 # in order.
00150 # "intensity" - laser or pixel intensity.
00151 # "distance"
00152
00153 # The channel name should give semantics of the channel (e.g.
00154 # "intensity" instead of "value").
00155 string name
00156
00157 # The values array should be 1-1 with the elements of the associated
00158 # PointCloud.
00159 float32[] values
00160
00161 ================================================================================
00162 MSG: object_manipulation_msgs/SceneRegion
00163 # Point cloud
00164 sensor_msgs/PointCloud2 cloud
00165
00166 # Indices for the region of interest
00167 int32[] mask
00168
00169 # One of the corresponding 2D images, if applicable
00170 sensor_msgs/Image image
00171
00172 # The disparity image, if applicable
00173 sensor_msgs/Image disparity_image
00174
00175 # Camera info for the camera that took the image
00176 sensor_msgs/CameraInfo cam_info
00177
00178 # a 3D region of interest for grasp planning
00179 geometry_msgs/PoseStamped roi_box_pose
00180 geometry_msgs/Vector3 roi_box_dims
00181
00182 ================================================================================
00183 MSG: sensor_msgs/PointCloud2
00184 # This message holds a collection of N-dimensional points, which may
00185 # contain additional information such as normals, intensity, etc. The
00186 # point data is stored as a binary blob, its layout described by the
00187 # contents of the "fields" array.
00188
00189 # The point cloud data may be organized 2d (image-like) or 1d
00190 # (unordered). Point clouds organized as 2d images may be produced by
00191 # camera depth sensors such as stereo or time-of-flight.
00192
00193 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00194 # points).
00195 Header header
00196
00197 # 2D structure of the point cloud. If the cloud is unordered, height is
00198 # 1 and width is the length of the point cloud.
00199 uint32 height
00200 uint32 width
00201
00202 # Describes the channels and their layout in the binary data blob.
00203 PointField[] fields
00204
00205 bool is_bigendian # Is this data bigendian?
00206 uint32 point_step # Length of a point in bytes
00207 uint32 row_step # Length of a row in bytes
00208 uint8[] data # Actual point data, size is (row_step*height)
00209
00210 bool is_dense # True if there are no invalid points
00211
00212 ================================================================================
00213 MSG: sensor_msgs/PointField
00214 # This message holds the description of one point entry in the
00215 # PointCloud2 message format.
00216 uint8 INT8 = 1
00217 uint8 UINT8 = 2
00218 uint8 INT16 = 3
00219 uint8 UINT16 = 4
00220 uint8 INT32 = 5
00221 uint8 UINT32 = 6
00222 uint8 FLOAT32 = 7
00223 uint8 FLOAT64 = 8
00224
00225 string name # Name of field
00226 uint32 offset # Offset from start of point struct
00227 uint8 datatype # Datatype enumeration, see above
00228 uint32 count # How many elements in the field
00229
00230 ================================================================================
00231 MSG: sensor_msgs/Image
00232 # This message contains an uncompressed image
00233 # (0, 0) is at top-left corner of image
00234 #
00235
00236 Header header # Header timestamp should be acquisition time of image
00237 # Header frame_id should be optical frame of camera
00238 # origin of frame should be optical center of cameara
00239 # +x should point to the right in the image
00240 # +y should point down in the image
00241 # +z should point into to plane of the image
00242 # If the frame_id here and the frame_id of the CameraInfo
00243 # message associated with the image conflict
00244 # the behavior is undefined
00245
00246 uint32 height # image height, that is, number of rows
00247 uint32 width # image width, that is, number of columns
00248
00249 # The legal values for encoding are in file src/image_encodings.cpp
00250 # If you want to standardize a new string format, join
00251 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00252
00253 string encoding # Encoding of pixels -- channel meaning, ordering, size
00254 # taken from the list of strings in src/image_encodings.cpp
00255
00256 uint8 is_bigendian # is this data bigendian?
00257 uint32 step # Full row length in bytes
00258 uint8[] data # actual matrix data, size is (step * rows)
00259
00260 ================================================================================
00261 MSG: sensor_msgs/CameraInfo
00262 # This message defines meta information for a camera. It should be in a
00263 # camera namespace on topic "camera_info" and accompanied by up to five
00264 # image topics named:
00265 #
00266 # image_raw - raw data from the camera driver, possibly Bayer encoded
00267 # image - monochrome, distorted
00268 # image_color - color, distorted
00269 # image_rect - monochrome, rectified
00270 # image_rect_color - color, rectified
00271 #
00272 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00273 # for producing the four processed image topics from image_raw and
00274 # camera_info. The meaning of the camera parameters are described in
00275 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00276 #
00277 # The image_geometry package provides a user-friendly interface to
00278 # common operations using this meta information. If you want to, e.g.,
00279 # project a 3d point into image coordinates, we strongly recommend
00280 # using image_geometry.
00281 #
00282 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00283 # zeroed out. In particular, clients may assume that K[0] == 0.0
00284 # indicates an uncalibrated camera.
00285
00286 #######################################################################
00287 # Image acquisition info #
00288 #######################################################################
00289
00290 # Time of image acquisition, camera coordinate frame ID
00291 Header header # Header timestamp should be acquisition time of image
00292 # Header frame_id should be optical frame of camera
00293 # origin of frame should be optical center of camera
00294 # +x should point to the right in the image
00295 # +y should point down in the image
00296 # +z should point into the plane of the image
00297
00298
00299 #######################################################################
00300 # Calibration Parameters #
00301 #######################################################################
00302 # These are fixed during camera calibration. Their values will be the #
00303 # same in all messages until the camera is recalibrated. Note that #
00304 # self-calibrating systems may "recalibrate" frequently. #
00305 # #
00306 # The internal parameters can be used to warp a raw (distorted) image #
00307 # to: #
00308 # 1. An undistorted image (requires D and K) #
00309 # 2. A rectified image (requires D, K, R) #
00310 # The projection matrix P projects 3D points into the rectified image.#
00311 #######################################################################
00312
00313 # The image dimensions with which the camera was calibrated. Normally
00314 # this will be the full camera resolution in pixels.
00315 uint32 height
00316 uint32 width
00317
00318 # The distortion model used. Supported models are listed in
00319 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00320 # simple model of radial and tangential distortion - is sufficent.
00321 string distortion_model
00322
00323 # The distortion parameters, size depending on the distortion model.
00324 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00325 float64[] D
00326
00327 # Intrinsic camera matrix for the raw (distorted) images.
00328 # [fx 0 cx]
00329 # K = [ 0 fy cy]
00330 # [ 0 0 1]
00331 # Projects 3D points in the camera coordinate frame to 2D pixel
00332 # coordinates using the focal lengths (fx, fy) and principal point
00333 # (cx, cy).
00334 float64[9] K # 3x3 row-major matrix
00335
00336 # Rectification matrix (stereo cameras only)
00337 # A rotation matrix aligning the camera coordinate system to the ideal
00338 # stereo image plane so that epipolar lines in both stereo images are
00339 # parallel.
00340 float64[9] R # 3x3 row-major matrix
00341
00342 # Projection/camera matrix
00343 # [fx' 0 cx' Tx]
00344 # P = [ 0 fy' cy' Ty]
00345 # [ 0 0 1 0]
00346 # By convention, this matrix specifies the intrinsic (camera) matrix
00347 # of the processed (rectified) image. That is, the left 3x3 portion
00348 # is the normal camera intrinsic matrix for the rectified image.
00349 # It projects 3D points in the camera coordinate frame to 2D pixel
00350 # coordinates using the focal lengths (fx', fy') and principal point
00351 # (cx', cy') - these may differ from the values in K.
00352 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00353 # also have R = the identity and P[1:3,1:3] = K.
00354 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00355 # position of the optical center of the second camera in the first
00356 # camera's frame. We assume Tz = 0 so both cameras are in the same
00357 # stereo image plane. The first camera always has Tx = Ty = 0. For
00358 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00359 # Tx = -fx' * B, where B is the baseline between the cameras.
00360 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00361 # the rectified image is given by:
00362 # [u v w]' = P * [X Y Z 1]'
00363 # x = u / w
00364 # y = v / w
00365 # This holds for both images of a stereo pair.
00366 float64[12] P # 3x4 row-major matrix
00367
00368
00369 #######################################################################
00370 # Operational Parameters #
00371 #######################################################################
00372 # These define the image region actually captured by the camera #
00373 # driver. Although they affect the geometry of the output image, they #
00374 # may be changed freely without recalibrating the camera. #
00375 #######################################################################
00376
00377 # Binning refers here to any camera setting which combines rectangular
00378 # neighborhoods of pixels into larger "super-pixels." It reduces the
00379 # resolution of the output image to
00380 # (width / binning_x) x (height / binning_y).
00381 # The default values binning_x = binning_y = 0 is considered the same
00382 # as binning_x = binning_y = 1 (no subsampling).
00383 uint32 binning_x
00384 uint32 binning_y
00385
00386 # Region of interest (subwindow of full camera resolution), given in
00387 # full resolution (unbinned) image coordinates. A particular ROI
00388 # always denotes the same window of pixels on the camera sensor,
00389 # regardless of binning settings.
00390 # The default setting of roi (all values 0) is considered the same as
00391 # full resolution (roi.width = width, roi.height = height).
00392 RegionOfInterest roi
00393
00394 ================================================================================
00395 MSG: sensor_msgs/RegionOfInterest
00396 # This message is used to specify a region of interest within an image.
00397 #
00398 # When used to specify the ROI setting of the camera when the image was
00399 # taken, the height and width fields should either match the height and
00400 # width fields for the associated image; or height = width = 0
00401 # indicates that the full resolution image was captured.
00402
00403 uint32 x_offset # Leftmost pixel of the ROI
00404 # (0 if the ROI includes the left edge of the image)
00405 uint32 y_offset # Topmost pixel of the ROI
00406 # (0 if the ROI includes the top edge of the image)
00407 uint32 height # Height of ROI
00408 uint32 width # Width of ROI
00409
00410 # True if a distinct rectified ROI should be calculated from the "raw"
00411 # ROI in this message. Typically this should be False if the full image
00412 # is captured (ROI not used), and True if a subwindow is captured (ROI
00413 # used).
00414 bool do_rectify
00415
00416 ================================================================================
00417 MSG: geometry_msgs/Vector3
00418 # This represents a vector in free space.
00419
00420 float64 x
00421 float64 y
00422 float64 z
00423 """
00424 __slots__ = ['reference_frame_id','potential_models','cluster','region','collision_name']
00425 _slot_types = ['string','household_objects_database_msgs/DatabaseModelPose[]','sensor_msgs/PointCloud','object_manipulation_msgs/SceneRegion','string']
00426
00427 def __init__(self, *args, **kwds):
00428 """
00429 Constructor. Any message fields that are implicitly/explicitly
00430 set to None will be assigned a default value. The recommend
00431 use is keyword arguments as this is more robust to future message
00432 changes. You cannot mix in-order arguments and keyword arguments.
00433
00434 The available fields are:
00435 reference_frame_id,potential_models,cluster,region,collision_name
00436
00437 :param args: complete set of field values, in .msg order
00438 :param kwds: use keyword arguments corresponding to message field names
00439 to set specific fields.
00440 """
00441 if args or kwds:
00442 super(GraspableObject, self).__init__(*args, **kwds)
00443 #message fields cannot be None, assign default values for those that are
00444 if self.reference_frame_id is None:
00445 self.reference_frame_id = ''
00446 if self.potential_models is None:
00447 self.potential_models = []
00448 if self.cluster is None:
00449 self.cluster = sensor_msgs.msg.PointCloud()
00450 if self.region is None:
00451 self.region = object_manipulation_msgs.msg.SceneRegion()
00452 if self.collision_name is None:
00453 self.collision_name = ''
00454 else:
00455 self.reference_frame_id = ''
00456 self.potential_models = []
00457 self.cluster = sensor_msgs.msg.PointCloud()
00458 self.region = object_manipulation_msgs.msg.SceneRegion()
00459 self.collision_name = ''
00460
00461 def _get_types(self):
00462 """
00463 internal API method
00464 """
00465 return self._slot_types
00466
00467 def serialize(self, buff):
00468 """
00469 serialize message into buffer
00470 :param buff: buffer, ``StringIO``
00471 """
00472 try:
00473 _x = self.reference_frame_id
00474 length = len(_x)
00475 if python3 or type(_x) == unicode:
00476 _x = _x.encode('utf-8')
00477 length = len(_x)
00478 buff.write(struct.pack('<I%ss'%length, length, _x))
00479 length = len(self.potential_models)
00480 buff.write(_struct_I.pack(length))
00481 for val1 in self.potential_models:
00482 buff.write(_struct_i.pack(val1.model_id))
00483 _v1 = val1.pose
00484 _v2 = _v1.header
00485 buff.write(_struct_I.pack(_v2.seq))
00486 _v3 = _v2.stamp
00487 _x = _v3
00488 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00489 _x = _v2.frame_id
00490 length = len(_x)
00491 if python3 or type(_x) == unicode:
00492 _x = _x.encode('utf-8')
00493 length = len(_x)
00494 buff.write(struct.pack('<I%ss'%length, length, _x))
00495 _v4 = _v1.pose
00496 _v5 = _v4.position
00497 _x = _v5
00498 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00499 _v6 = _v4.orientation
00500 _x = _v6
00501 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00502 buff.write(_struct_f.pack(val1.confidence))
00503 _x = val1.detector_name
00504 length = len(_x)
00505 if python3 or type(_x) == unicode:
00506 _x = _x.encode('utf-8')
00507 length = len(_x)
00508 buff.write(struct.pack('<I%ss'%length, length, _x))
00509 _x = self
00510 buff.write(_struct_3I.pack(_x.cluster.header.seq, _x.cluster.header.stamp.secs, _x.cluster.header.stamp.nsecs))
00511 _x = self.cluster.header.frame_id
00512 length = len(_x)
00513 if python3 or type(_x) == unicode:
00514 _x = _x.encode('utf-8')
00515 length = len(_x)
00516 buff.write(struct.pack('<I%ss'%length, length, _x))
00517 length = len(self.cluster.points)
00518 buff.write(_struct_I.pack(length))
00519 for val1 in self.cluster.points:
00520 _x = val1
00521 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00522 length = len(self.cluster.channels)
00523 buff.write(_struct_I.pack(length))
00524 for val1 in self.cluster.channels:
00525 _x = val1.name
00526 length = len(_x)
00527 if python3 or type(_x) == unicode:
00528 _x = _x.encode('utf-8')
00529 length = len(_x)
00530 buff.write(struct.pack('<I%ss'%length, length, _x))
00531 length = len(val1.values)
00532 buff.write(_struct_I.pack(length))
00533 pattern = '<%sf'%length
00534 buff.write(struct.pack(pattern, *val1.values))
00535 _x = self
00536 buff.write(_struct_3I.pack(_x.region.cloud.header.seq, _x.region.cloud.header.stamp.secs, _x.region.cloud.header.stamp.nsecs))
00537 _x = self.region.cloud.header.frame_id
00538 length = len(_x)
00539 if python3 or type(_x) == unicode:
00540 _x = _x.encode('utf-8')
00541 length = len(_x)
00542 buff.write(struct.pack('<I%ss'%length, length, _x))
00543 _x = self
00544 buff.write(_struct_2I.pack(_x.region.cloud.height, _x.region.cloud.width))
00545 length = len(self.region.cloud.fields)
00546 buff.write(_struct_I.pack(length))
00547 for val1 in self.region.cloud.fields:
00548 _x = val1.name
00549 length = len(_x)
00550 if python3 or type(_x) == unicode:
00551 _x = _x.encode('utf-8')
00552 length = len(_x)
00553 buff.write(struct.pack('<I%ss'%length, length, _x))
00554 _x = val1
00555 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00556 _x = self
00557 buff.write(_struct_B2I.pack(_x.region.cloud.is_bigendian, _x.region.cloud.point_step, _x.region.cloud.row_step))
00558 _x = self.region.cloud.data
00559 length = len(_x)
00560 # - if encoded as a list instead, serialize as bytes instead of string
00561 if type(_x) in [list, tuple]:
00562 buff.write(struct.pack('<I%sB'%length, length, *_x))
00563 else:
00564 buff.write(struct.pack('<I%ss'%length, length, _x))
00565 buff.write(_struct_B.pack(self.region.cloud.is_dense))
00566 length = len(self.region.mask)
00567 buff.write(_struct_I.pack(length))
00568 pattern = '<%si'%length
00569 buff.write(struct.pack(pattern, *self.region.mask))
00570 _x = self
00571 buff.write(_struct_3I.pack(_x.region.image.header.seq, _x.region.image.header.stamp.secs, _x.region.image.header.stamp.nsecs))
00572 _x = self.region.image.header.frame_id
00573 length = len(_x)
00574 if python3 or type(_x) == unicode:
00575 _x = _x.encode('utf-8')
00576 length = len(_x)
00577 buff.write(struct.pack('<I%ss'%length, length, _x))
00578 _x = self
00579 buff.write(_struct_2I.pack(_x.region.image.height, _x.region.image.width))
00580 _x = self.region.image.encoding
00581 length = len(_x)
00582 if python3 or type(_x) == unicode:
00583 _x = _x.encode('utf-8')
00584 length = len(_x)
00585 buff.write(struct.pack('<I%ss'%length, length, _x))
00586 _x = self
00587 buff.write(_struct_BI.pack(_x.region.image.is_bigendian, _x.region.image.step))
00588 _x = self.region.image.data
00589 length = len(_x)
00590 # - if encoded as a list instead, serialize as bytes instead of string
00591 if type(_x) in [list, tuple]:
00592 buff.write(struct.pack('<I%sB'%length, length, *_x))
00593 else:
00594 buff.write(struct.pack('<I%ss'%length, length, _x))
00595 _x = self
00596 buff.write(_struct_3I.pack(_x.region.disparity_image.header.seq, _x.region.disparity_image.header.stamp.secs, _x.region.disparity_image.header.stamp.nsecs))
00597 _x = self.region.disparity_image.header.frame_id
00598 length = len(_x)
00599 if python3 or type(_x) == unicode:
00600 _x = _x.encode('utf-8')
00601 length = len(_x)
00602 buff.write(struct.pack('<I%ss'%length, length, _x))
00603 _x = self
00604 buff.write(_struct_2I.pack(_x.region.disparity_image.height, _x.region.disparity_image.width))
00605 _x = self.region.disparity_image.encoding
00606 length = len(_x)
00607 if python3 or type(_x) == unicode:
00608 _x = _x.encode('utf-8')
00609 length = len(_x)
00610 buff.write(struct.pack('<I%ss'%length, length, _x))
00611 _x = self
00612 buff.write(_struct_BI.pack(_x.region.disparity_image.is_bigendian, _x.region.disparity_image.step))
00613 _x = self.region.disparity_image.data
00614 length = len(_x)
00615 # - if encoded as a list instead, serialize as bytes instead of string
00616 if type(_x) in [list, tuple]:
00617 buff.write(struct.pack('<I%sB'%length, length, *_x))
00618 else:
00619 buff.write(struct.pack('<I%ss'%length, length, _x))
00620 _x = self
00621 buff.write(_struct_3I.pack(_x.region.cam_info.header.seq, _x.region.cam_info.header.stamp.secs, _x.region.cam_info.header.stamp.nsecs))
00622 _x = self.region.cam_info.header.frame_id
00623 length = len(_x)
00624 if python3 or type(_x) == unicode:
00625 _x = _x.encode('utf-8')
00626 length = len(_x)
00627 buff.write(struct.pack('<I%ss'%length, length, _x))
00628 _x = self
00629 buff.write(_struct_2I.pack(_x.region.cam_info.height, _x.region.cam_info.width))
00630 _x = self.region.cam_info.distortion_model
00631 length = len(_x)
00632 if python3 or type(_x) == unicode:
00633 _x = _x.encode('utf-8')
00634 length = len(_x)
00635 buff.write(struct.pack('<I%ss'%length, length, _x))
00636 length = len(self.region.cam_info.D)
00637 buff.write(_struct_I.pack(length))
00638 pattern = '<%sd'%length
00639 buff.write(struct.pack(pattern, *self.region.cam_info.D))
00640 buff.write(_struct_9d.pack(*self.region.cam_info.K))
00641 buff.write(_struct_9d.pack(*self.region.cam_info.R))
00642 buff.write(_struct_12d.pack(*self.region.cam_info.P))
00643 _x = self
00644 buff.write(_struct_6IB3I.pack(_x.region.cam_info.binning_x, _x.region.cam_info.binning_y, _x.region.cam_info.roi.x_offset, _x.region.cam_info.roi.y_offset, _x.region.cam_info.roi.height, _x.region.cam_info.roi.width, _x.region.cam_info.roi.do_rectify, _x.region.roi_box_pose.header.seq, _x.region.roi_box_pose.header.stamp.secs, _x.region.roi_box_pose.header.stamp.nsecs))
00645 _x = self.region.roi_box_pose.header.frame_id
00646 length = len(_x)
00647 if python3 or type(_x) == unicode:
00648 _x = _x.encode('utf-8')
00649 length = len(_x)
00650 buff.write(struct.pack('<I%ss'%length, length, _x))
00651 _x = self
00652 buff.write(_struct_10d.pack(_x.region.roi_box_pose.pose.position.x, _x.region.roi_box_pose.pose.position.y, _x.region.roi_box_pose.pose.position.z, _x.region.roi_box_pose.pose.orientation.x, _x.region.roi_box_pose.pose.orientation.y, _x.region.roi_box_pose.pose.orientation.z, _x.region.roi_box_pose.pose.orientation.w, _x.region.roi_box_dims.x, _x.region.roi_box_dims.y, _x.region.roi_box_dims.z))
00653 _x = self.collision_name
00654 length = len(_x)
00655 if python3 or type(_x) == unicode:
00656 _x = _x.encode('utf-8')
00657 length = len(_x)
00658 buff.write(struct.pack('<I%ss'%length, length, _x))
00659 except struct.error as se: self._check_types(se)
00660 except TypeError as te: self._check_types(te)
00661
00662 def deserialize(self, str):
00663 """
00664 unpack serialized message in str into this message instance
00665 :param str: byte array of serialized message, ``str``
00666 """
00667 try:
00668 if self.potential_models is None:
00669 self.potential_models = None
00670 if self.cluster is None:
00671 self.cluster = sensor_msgs.msg.PointCloud()
00672 if self.region is None:
00673 self.region = object_manipulation_msgs.msg.SceneRegion()
00674 end = 0
00675 start = end
00676 end += 4
00677 (length,) = _struct_I.unpack(str[start:end])
00678 start = end
00679 end += length
00680 if python3:
00681 self.reference_frame_id = str[start:end].decode('utf-8')
00682 else:
00683 self.reference_frame_id = str[start:end]
00684 start = end
00685 end += 4
00686 (length,) = _struct_I.unpack(str[start:end])
00687 self.potential_models = []
00688 for i in range(0, length):
00689 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
00690 start = end
00691 end += 4
00692 (val1.model_id,) = _struct_i.unpack(str[start:end])
00693 _v7 = val1.pose
00694 _v8 = _v7.header
00695 start = end
00696 end += 4
00697 (_v8.seq,) = _struct_I.unpack(str[start:end])
00698 _v9 = _v8.stamp
00699 _x = _v9
00700 start = end
00701 end += 8
00702 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00703 start = end
00704 end += 4
00705 (length,) = _struct_I.unpack(str[start:end])
00706 start = end
00707 end += length
00708 if python3:
00709 _v8.frame_id = str[start:end].decode('utf-8')
00710 else:
00711 _v8.frame_id = str[start:end]
00712 _v10 = _v7.pose
00713 _v11 = _v10.position
00714 _x = _v11
00715 start = end
00716 end += 24
00717 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00718 _v12 = _v10.orientation
00719 _x = _v12
00720 start = end
00721 end += 32
00722 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00723 start = end
00724 end += 4
00725 (val1.confidence,) = _struct_f.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 if python3:
00732 val1.detector_name = str[start:end].decode('utf-8')
00733 else:
00734 val1.detector_name = str[start:end]
00735 self.potential_models.append(val1)
00736 _x = self
00737 start = end
00738 end += 12
00739 (_x.cluster.header.seq, _x.cluster.header.stamp.secs, _x.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00740 start = end
00741 end += 4
00742 (length,) = _struct_I.unpack(str[start:end])
00743 start = end
00744 end += length
00745 if python3:
00746 self.cluster.header.frame_id = str[start:end].decode('utf-8')
00747 else:
00748 self.cluster.header.frame_id = str[start:end]
00749 start = end
00750 end += 4
00751 (length,) = _struct_I.unpack(str[start:end])
00752 self.cluster.points = []
00753 for i in range(0, length):
00754 val1 = geometry_msgs.msg.Point32()
00755 _x = val1
00756 start = end
00757 end += 12
00758 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00759 self.cluster.points.append(val1)
00760 start = end
00761 end += 4
00762 (length,) = _struct_I.unpack(str[start:end])
00763 self.cluster.channels = []
00764 for i in range(0, length):
00765 val1 = sensor_msgs.msg.ChannelFloat32()
00766 start = end
00767 end += 4
00768 (length,) = _struct_I.unpack(str[start:end])
00769 start = end
00770 end += length
00771 if python3:
00772 val1.name = str[start:end].decode('utf-8')
00773 else:
00774 val1.name = str[start:end]
00775 start = end
00776 end += 4
00777 (length,) = _struct_I.unpack(str[start:end])
00778 pattern = '<%sf'%length
00779 start = end
00780 end += struct.calcsize(pattern)
00781 val1.values = struct.unpack(pattern, str[start:end])
00782 self.cluster.channels.append(val1)
00783 _x = self
00784 start = end
00785 end += 12
00786 (_x.region.cloud.header.seq, _x.region.cloud.header.stamp.secs, _x.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00787 start = end
00788 end += 4
00789 (length,) = _struct_I.unpack(str[start:end])
00790 start = end
00791 end += length
00792 if python3:
00793 self.region.cloud.header.frame_id = str[start:end].decode('utf-8')
00794 else:
00795 self.region.cloud.header.frame_id = str[start:end]
00796 _x = self
00797 start = end
00798 end += 8
00799 (_x.region.cloud.height, _x.region.cloud.width,) = _struct_2I.unpack(str[start:end])
00800 start = end
00801 end += 4
00802 (length,) = _struct_I.unpack(str[start:end])
00803 self.region.cloud.fields = []
00804 for i in range(0, length):
00805 val1 = sensor_msgs.msg.PointField()
00806 start = end
00807 end += 4
00808 (length,) = _struct_I.unpack(str[start:end])
00809 start = end
00810 end += length
00811 if python3:
00812 val1.name = str[start:end].decode('utf-8')
00813 else:
00814 val1.name = str[start:end]
00815 _x = val1
00816 start = end
00817 end += 9
00818 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
00819 self.region.cloud.fields.append(val1)
00820 _x = self
00821 start = end
00822 end += 9
00823 (_x.region.cloud.is_bigendian, _x.region.cloud.point_step, _x.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
00824 self.region.cloud.is_bigendian = bool(self.region.cloud.is_bigendian)
00825 start = end
00826 end += 4
00827 (length,) = _struct_I.unpack(str[start:end])
00828 start = end
00829 end += length
00830 if python3:
00831 self.region.cloud.data = str[start:end].decode('utf-8')
00832 else:
00833 self.region.cloud.data = str[start:end]
00834 start = end
00835 end += 1
00836 (self.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
00837 self.region.cloud.is_dense = bool(self.region.cloud.is_dense)
00838 start = end
00839 end += 4
00840 (length,) = _struct_I.unpack(str[start:end])
00841 pattern = '<%si'%length
00842 start = end
00843 end += struct.calcsize(pattern)
00844 self.region.mask = struct.unpack(pattern, str[start:end])
00845 _x = self
00846 start = end
00847 end += 12
00848 (_x.region.image.header.seq, _x.region.image.header.stamp.secs, _x.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00849 start = end
00850 end += 4
00851 (length,) = _struct_I.unpack(str[start:end])
00852 start = end
00853 end += length
00854 if python3:
00855 self.region.image.header.frame_id = str[start:end].decode('utf-8')
00856 else:
00857 self.region.image.header.frame_id = str[start:end]
00858 _x = self
00859 start = end
00860 end += 8
00861 (_x.region.image.height, _x.region.image.width,) = _struct_2I.unpack(str[start:end])
00862 start = end
00863 end += 4
00864 (length,) = _struct_I.unpack(str[start:end])
00865 start = end
00866 end += length
00867 if python3:
00868 self.region.image.encoding = str[start:end].decode('utf-8')
00869 else:
00870 self.region.image.encoding = str[start:end]
00871 _x = self
00872 start = end
00873 end += 5
00874 (_x.region.image.is_bigendian, _x.region.image.step,) = _struct_BI.unpack(str[start:end])
00875 start = end
00876 end += 4
00877 (length,) = _struct_I.unpack(str[start:end])
00878 start = end
00879 end += length
00880 if python3:
00881 self.region.image.data = str[start:end].decode('utf-8')
00882 else:
00883 self.region.image.data = str[start:end]
00884 _x = self
00885 start = end
00886 end += 12
00887 (_x.region.disparity_image.header.seq, _x.region.disparity_image.header.stamp.secs, _x.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00888 start = end
00889 end += 4
00890 (length,) = _struct_I.unpack(str[start:end])
00891 start = end
00892 end += length
00893 if python3:
00894 self.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
00895 else:
00896 self.region.disparity_image.header.frame_id = str[start:end]
00897 _x = self
00898 start = end
00899 end += 8
00900 (_x.region.disparity_image.height, _x.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
00901 start = end
00902 end += 4
00903 (length,) = _struct_I.unpack(str[start:end])
00904 start = end
00905 end += length
00906 if python3:
00907 self.region.disparity_image.encoding = str[start:end].decode('utf-8')
00908 else:
00909 self.region.disparity_image.encoding = str[start:end]
00910 _x = self
00911 start = end
00912 end += 5
00913 (_x.region.disparity_image.is_bigendian, _x.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
00914 start = end
00915 end += 4
00916 (length,) = _struct_I.unpack(str[start:end])
00917 start = end
00918 end += length
00919 if python3:
00920 self.region.disparity_image.data = str[start:end].decode('utf-8')
00921 else:
00922 self.region.disparity_image.data = str[start:end]
00923 _x = self
00924 start = end
00925 end += 12
00926 (_x.region.cam_info.header.seq, _x.region.cam_info.header.stamp.secs, _x.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00927 start = end
00928 end += 4
00929 (length,) = _struct_I.unpack(str[start:end])
00930 start = end
00931 end += length
00932 if python3:
00933 self.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
00934 else:
00935 self.region.cam_info.header.frame_id = str[start:end]
00936 _x = self
00937 start = end
00938 end += 8
00939 (_x.region.cam_info.height, _x.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
00940 start = end
00941 end += 4
00942 (length,) = _struct_I.unpack(str[start:end])
00943 start = end
00944 end += length
00945 if python3:
00946 self.region.cam_info.distortion_model = str[start:end].decode('utf-8')
00947 else:
00948 self.region.cam_info.distortion_model = str[start:end]
00949 start = end
00950 end += 4
00951 (length,) = _struct_I.unpack(str[start:end])
00952 pattern = '<%sd'%length
00953 start = end
00954 end += struct.calcsize(pattern)
00955 self.region.cam_info.D = struct.unpack(pattern, str[start:end])
00956 start = end
00957 end += 72
00958 self.region.cam_info.K = _struct_9d.unpack(str[start:end])
00959 start = end
00960 end += 72
00961 self.region.cam_info.R = _struct_9d.unpack(str[start:end])
00962 start = end
00963 end += 96
00964 self.region.cam_info.P = _struct_12d.unpack(str[start:end])
00965 _x = self
00966 start = end
00967 end += 37
00968 (_x.region.cam_info.binning_x, _x.region.cam_info.binning_y, _x.region.cam_info.roi.x_offset, _x.region.cam_info.roi.y_offset, _x.region.cam_info.roi.height, _x.region.cam_info.roi.width, _x.region.cam_info.roi.do_rectify, _x.region.roi_box_pose.header.seq, _x.region.roi_box_pose.header.stamp.secs, _x.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
00969 self.region.cam_info.roi.do_rectify = bool(self.region.cam_info.roi.do_rectify)
00970 start = end
00971 end += 4
00972 (length,) = _struct_I.unpack(str[start:end])
00973 start = end
00974 end += length
00975 if python3:
00976 self.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
00977 else:
00978 self.region.roi_box_pose.header.frame_id = str[start:end]
00979 _x = self
00980 start = end
00981 end += 80
00982 (_x.region.roi_box_pose.pose.position.x, _x.region.roi_box_pose.pose.position.y, _x.region.roi_box_pose.pose.position.z, _x.region.roi_box_pose.pose.orientation.x, _x.region.roi_box_pose.pose.orientation.y, _x.region.roi_box_pose.pose.orientation.z, _x.region.roi_box_pose.pose.orientation.w, _x.region.roi_box_dims.x, _x.region.roi_box_dims.y, _x.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
00983 start = end
00984 end += 4
00985 (length,) = _struct_I.unpack(str[start:end])
00986 start = end
00987 end += length
00988 if python3:
00989 self.collision_name = str[start:end].decode('utf-8')
00990 else:
00991 self.collision_name = str[start:end]
00992 return self
00993 except struct.error as e:
00994 raise genpy.DeserializationError(e) #most likely buffer underfill
00995
00996
00997 def serialize_numpy(self, buff, numpy):
00998 """
00999 serialize message with numpy array types into buffer
01000 :param buff: buffer, ``StringIO``
01001 :param numpy: numpy python module
01002 """
01003 try:
01004 _x = self.reference_frame_id
01005 length = len(_x)
01006 if python3 or type(_x) == unicode:
01007 _x = _x.encode('utf-8')
01008 length = len(_x)
01009 buff.write(struct.pack('<I%ss'%length, length, _x))
01010 length = len(self.potential_models)
01011 buff.write(_struct_I.pack(length))
01012 for val1 in self.potential_models:
01013 buff.write(_struct_i.pack(val1.model_id))
01014 _v13 = val1.pose
01015 _v14 = _v13.header
01016 buff.write(_struct_I.pack(_v14.seq))
01017 _v15 = _v14.stamp
01018 _x = _v15
01019 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01020 _x = _v14.frame_id
01021 length = len(_x)
01022 if python3 or type(_x) == unicode:
01023 _x = _x.encode('utf-8')
01024 length = len(_x)
01025 buff.write(struct.pack('<I%ss'%length, length, _x))
01026 _v16 = _v13.pose
01027 _v17 = _v16.position
01028 _x = _v17
01029 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01030 _v18 = _v16.orientation
01031 _x = _v18
01032 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01033 buff.write(_struct_f.pack(val1.confidence))
01034 _x = val1.detector_name
01035 length = len(_x)
01036 if python3 or type(_x) == unicode:
01037 _x = _x.encode('utf-8')
01038 length = len(_x)
01039 buff.write(struct.pack('<I%ss'%length, length, _x))
01040 _x = self
01041 buff.write(_struct_3I.pack(_x.cluster.header.seq, _x.cluster.header.stamp.secs, _x.cluster.header.stamp.nsecs))
01042 _x = self.cluster.header.frame_id
01043 length = len(_x)
01044 if python3 or type(_x) == unicode:
01045 _x = _x.encode('utf-8')
01046 length = len(_x)
01047 buff.write(struct.pack('<I%ss'%length, length, _x))
01048 length = len(self.cluster.points)
01049 buff.write(_struct_I.pack(length))
01050 for val1 in self.cluster.points:
01051 _x = val1
01052 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01053 length = len(self.cluster.channels)
01054 buff.write(_struct_I.pack(length))
01055 for val1 in self.cluster.channels:
01056 _x = val1.name
01057 length = len(_x)
01058 if python3 or type(_x) == unicode:
01059 _x = _x.encode('utf-8')
01060 length = len(_x)
01061 buff.write(struct.pack('<I%ss'%length, length, _x))
01062 length = len(val1.values)
01063 buff.write(_struct_I.pack(length))
01064 pattern = '<%sf'%length
01065 buff.write(val1.values.tostring())
01066 _x = self
01067 buff.write(_struct_3I.pack(_x.region.cloud.header.seq, _x.region.cloud.header.stamp.secs, _x.region.cloud.header.stamp.nsecs))
01068 _x = self.region.cloud.header.frame_id
01069 length = len(_x)
01070 if python3 or type(_x) == unicode:
01071 _x = _x.encode('utf-8')
01072 length = len(_x)
01073 buff.write(struct.pack('<I%ss'%length, length, _x))
01074 _x = self
01075 buff.write(_struct_2I.pack(_x.region.cloud.height, _x.region.cloud.width))
01076 length = len(self.region.cloud.fields)
01077 buff.write(_struct_I.pack(length))
01078 for val1 in self.region.cloud.fields:
01079 _x = val1.name
01080 length = len(_x)
01081 if python3 or type(_x) == unicode:
01082 _x = _x.encode('utf-8')
01083 length = len(_x)
01084 buff.write(struct.pack('<I%ss'%length, length, _x))
01085 _x = val1
01086 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01087 _x = self
01088 buff.write(_struct_B2I.pack(_x.region.cloud.is_bigendian, _x.region.cloud.point_step, _x.region.cloud.row_step))
01089 _x = self.region.cloud.data
01090 length = len(_x)
01091 # - if encoded as a list instead, serialize as bytes instead of string
01092 if type(_x) in [list, tuple]:
01093 buff.write(struct.pack('<I%sB'%length, length, *_x))
01094 else:
01095 buff.write(struct.pack('<I%ss'%length, length, _x))
01096 buff.write(_struct_B.pack(self.region.cloud.is_dense))
01097 length = len(self.region.mask)
01098 buff.write(_struct_I.pack(length))
01099 pattern = '<%si'%length
01100 buff.write(self.region.mask.tostring())
01101 _x = self
01102 buff.write(_struct_3I.pack(_x.region.image.header.seq, _x.region.image.header.stamp.secs, _x.region.image.header.stamp.nsecs))
01103 _x = self.region.image.header.frame_id
01104 length = len(_x)
01105 if python3 or type(_x) == unicode:
01106 _x = _x.encode('utf-8')
01107 length = len(_x)
01108 buff.write(struct.pack('<I%ss'%length, length, _x))
01109 _x = self
01110 buff.write(_struct_2I.pack(_x.region.image.height, _x.region.image.width))
01111 _x = self.region.image.encoding
01112 length = len(_x)
01113 if python3 or type(_x) == unicode:
01114 _x = _x.encode('utf-8')
01115 length = len(_x)
01116 buff.write(struct.pack('<I%ss'%length, length, _x))
01117 _x = self
01118 buff.write(_struct_BI.pack(_x.region.image.is_bigendian, _x.region.image.step))
01119 _x = self.region.image.data
01120 length = len(_x)
01121 # - if encoded as a list instead, serialize as bytes instead of string
01122 if type(_x) in [list, tuple]:
01123 buff.write(struct.pack('<I%sB'%length, length, *_x))
01124 else:
01125 buff.write(struct.pack('<I%ss'%length, length, _x))
01126 _x = self
01127 buff.write(_struct_3I.pack(_x.region.disparity_image.header.seq, _x.region.disparity_image.header.stamp.secs, _x.region.disparity_image.header.stamp.nsecs))
01128 _x = self.region.disparity_image.header.frame_id
01129 length = len(_x)
01130 if python3 or type(_x) == unicode:
01131 _x = _x.encode('utf-8')
01132 length = len(_x)
01133 buff.write(struct.pack('<I%ss'%length, length, _x))
01134 _x = self
01135 buff.write(_struct_2I.pack(_x.region.disparity_image.height, _x.region.disparity_image.width))
01136 _x = self.region.disparity_image.encoding
01137 length = len(_x)
01138 if python3 or type(_x) == unicode:
01139 _x = _x.encode('utf-8')
01140 length = len(_x)
01141 buff.write(struct.pack('<I%ss'%length, length, _x))
01142 _x = self
01143 buff.write(_struct_BI.pack(_x.region.disparity_image.is_bigendian, _x.region.disparity_image.step))
01144 _x = self.region.disparity_image.data
01145 length = len(_x)
01146 # - if encoded as a list instead, serialize as bytes instead of string
01147 if type(_x) in [list, tuple]:
01148 buff.write(struct.pack('<I%sB'%length, length, *_x))
01149 else:
01150 buff.write(struct.pack('<I%ss'%length, length, _x))
01151 _x = self
01152 buff.write(_struct_3I.pack(_x.region.cam_info.header.seq, _x.region.cam_info.header.stamp.secs, _x.region.cam_info.header.stamp.nsecs))
01153 _x = self.region.cam_info.header.frame_id
01154 length = len(_x)
01155 if python3 or type(_x) == unicode:
01156 _x = _x.encode('utf-8')
01157 length = len(_x)
01158 buff.write(struct.pack('<I%ss'%length, length, _x))
01159 _x = self
01160 buff.write(_struct_2I.pack(_x.region.cam_info.height, _x.region.cam_info.width))
01161 _x = self.region.cam_info.distortion_model
01162 length = len(_x)
01163 if python3 or type(_x) == unicode:
01164 _x = _x.encode('utf-8')
01165 length = len(_x)
01166 buff.write(struct.pack('<I%ss'%length, length, _x))
01167 length = len(self.region.cam_info.D)
01168 buff.write(_struct_I.pack(length))
01169 pattern = '<%sd'%length
01170 buff.write(self.region.cam_info.D.tostring())
01171 buff.write(self.region.cam_info.K.tostring())
01172 buff.write(self.region.cam_info.R.tostring())
01173 buff.write(self.region.cam_info.P.tostring())
01174 _x = self
01175 buff.write(_struct_6IB3I.pack(_x.region.cam_info.binning_x, _x.region.cam_info.binning_y, _x.region.cam_info.roi.x_offset, _x.region.cam_info.roi.y_offset, _x.region.cam_info.roi.height, _x.region.cam_info.roi.width, _x.region.cam_info.roi.do_rectify, _x.region.roi_box_pose.header.seq, _x.region.roi_box_pose.header.stamp.secs, _x.region.roi_box_pose.header.stamp.nsecs))
01176 _x = self.region.roi_box_pose.header.frame_id
01177 length = len(_x)
01178 if python3 or type(_x) == unicode:
01179 _x = _x.encode('utf-8')
01180 length = len(_x)
01181 buff.write(struct.pack('<I%ss'%length, length, _x))
01182 _x = self
01183 buff.write(_struct_10d.pack(_x.region.roi_box_pose.pose.position.x, _x.region.roi_box_pose.pose.position.y, _x.region.roi_box_pose.pose.position.z, _x.region.roi_box_pose.pose.orientation.x, _x.region.roi_box_pose.pose.orientation.y, _x.region.roi_box_pose.pose.orientation.z, _x.region.roi_box_pose.pose.orientation.w, _x.region.roi_box_dims.x, _x.region.roi_box_dims.y, _x.region.roi_box_dims.z))
01184 _x = self.collision_name
01185 length = len(_x)
01186 if python3 or type(_x) == unicode:
01187 _x = _x.encode('utf-8')
01188 length = len(_x)
01189 buff.write(struct.pack('<I%ss'%length, length, _x))
01190 except struct.error as se: self._check_types(se)
01191 except TypeError as te: self._check_types(te)
01192
01193 def deserialize_numpy(self, str, numpy):
01194 """
01195 unpack serialized message in str into this message instance using numpy for array types
01196 :param str: byte array of serialized message, ``str``
01197 :param numpy: numpy python module
01198 """
01199 try:
01200 if self.potential_models is None:
01201 self.potential_models = None
01202 if self.cluster is None:
01203 self.cluster = sensor_msgs.msg.PointCloud()
01204 if self.region is None:
01205 self.region = object_manipulation_msgs.msg.SceneRegion()
01206 end = 0
01207 start = end
01208 end += 4
01209 (length,) = _struct_I.unpack(str[start:end])
01210 start = end
01211 end += length
01212 if python3:
01213 self.reference_frame_id = str[start:end].decode('utf-8')
01214 else:
01215 self.reference_frame_id = str[start:end]
01216 start = end
01217 end += 4
01218 (length,) = _struct_I.unpack(str[start:end])
01219 self.potential_models = []
01220 for i in range(0, length):
01221 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01222 start = end
01223 end += 4
01224 (val1.model_id,) = _struct_i.unpack(str[start:end])
01225 _v19 = val1.pose
01226 _v20 = _v19.header
01227 start = end
01228 end += 4
01229 (_v20.seq,) = _struct_I.unpack(str[start:end])
01230 _v21 = _v20.stamp
01231 _x = _v21
01232 start = end
01233 end += 8
01234 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01235 start = end
01236 end += 4
01237 (length,) = _struct_I.unpack(str[start:end])
01238 start = end
01239 end += length
01240 if python3:
01241 _v20.frame_id = str[start:end].decode('utf-8')
01242 else:
01243 _v20.frame_id = str[start:end]
01244 _v22 = _v19.pose
01245 _v23 = _v22.position
01246 _x = _v23
01247 start = end
01248 end += 24
01249 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01250 _v24 = _v22.orientation
01251 _x = _v24
01252 start = end
01253 end += 32
01254 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01255 start = end
01256 end += 4
01257 (val1.confidence,) = _struct_f.unpack(str[start:end])
01258 start = end
01259 end += 4
01260 (length,) = _struct_I.unpack(str[start:end])
01261 start = end
01262 end += length
01263 if python3:
01264 val1.detector_name = str[start:end].decode('utf-8')
01265 else:
01266 val1.detector_name = str[start:end]
01267 self.potential_models.append(val1)
01268 _x = self
01269 start = end
01270 end += 12
01271 (_x.cluster.header.seq, _x.cluster.header.stamp.secs, _x.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01272 start = end
01273 end += 4
01274 (length,) = _struct_I.unpack(str[start:end])
01275 start = end
01276 end += length
01277 if python3:
01278 self.cluster.header.frame_id = str[start:end].decode('utf-8')
01279 else:
01280 self.cluster.header.frame_id = str[start:end]
01281 start = end
01282 end += 4
01283 (length,) = _struct_I.unpack(str[start:end])
01284 self.cluster.points = []
01285 for i in range(0, length):
01286 val1 = geometry_msgs.msg.Point32()
01287 _x = val1
01288 start = end
01289 end += 12
01290 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01291 self.cluster.points.append(val1)
01292 start = end
01293 end += 4
01294 (length,) = _struct_I.unpack(str[start:end])
01295 self.cluster.channels = []
01296 for i in range(0, length):
01297 val1 = sensor_msgs.msg.ChannelFloat32()
01298 start = end
01299 end += 4
01300 (length,) = _struct_I.unpack(str[start:end])
01301 start = end
01302 end += length
01303 if python3:
01304 val1.name = str[start:end].decode('utf-8')
01305 else:
01306 val1.name = str[start:end]
01307 start = end
01308 end += 4
01309 (length,) = _struct_I.unpack(str[start:end])
01310 pattern = '<%sf'%length
01311 start = end
01312 end += struct.calcsize(pattern)
01313 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01314 self.cluster.channels.append(val1)
01315 _x = self
01316 start = end
01317 end += 12
01318 (_x.region.cloud.header.seq, _x.region.cloud.header.stamp.secs, _x.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01319 start = end
01320 end += 4
01321 (length,) = _struct_I.unpack(str[start:end])
01322 start = end
01323 end += length
01324 if python3:
01325 self.region.cloud.header.frame_id = str[start:end].decode('utf-8')
01326 else:
01327 self.region.cloud.header.frame_id = str[start:end]
01328 _x = self
01329 start = end
01330 end += 8
01331 (_x.region.cloud.height, _x.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01332 start = end
01333 end += 4
01334 (length,) = _struct_I.unpack(str[start:end])
01335 self.region.cloud.fields = []
01336 for i in range(0, length):
01337 val1 = sensor_msgs.msg.PointField()
01338 start = end
01339 end += 4
01340 (length,) = _struct_I.unpack(str[start:end])
01341 start = end
01342 end += length
01343 if python3:
01344 val1.name = str[start:end].decode('utf-8')
01345 else:
01346 val1.name = str[start:end]
01347 _x = val1
01348 start = end
01349 end += 9
01350 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01351 self.region.cloud.fields.append(val1)
01352 _x = self
01353 start = end
01354 end += 9
01355 (_x.region.cloud.is_bigendian, _x.region.cloud.point_step, _x.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01356 self.region.cloud.is_bigendian = bool(self.region.cloud.is_bigendian)
01357 start = end
01358 end += 4
01359 (length,) = _struct_I.unpack(str[start:end])
01360 start = end
01361 end += length
01362 if python3:
01363 self.region.cloud.data = str[start:end].decode('utf-8')
01364 else:
01365 self.region.cloud.data = str[start:end]
01366 start = end
01367 end += 1
01368 (self.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01369 self.region.cloud.is_dense = bool(self.region.cloud.is_dense)
01370 start = end
01371 end += 4
01372 (length,) = _struct_I.unpack(str[start:end])
01373 pattern = '<%si'%length
01374 start = end
01375 end += struct.calcsize(pattern)
01376 self.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01377 _x = self
01378 start = end
01379 end += 12
01380 (_x.region.image.header.seq, _x.region.image.header.stamp.secs, _x.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01381 start = end
01382 end += 4
01383 (length,) = _struct_I.unpack(str[start:end])
01384 start = end
01385 end += length
01386 if python3:
01387 self.region.image.header.frame_id = str[start:end].decode('utf-8')
01388 else:
01389 self.region.image.header.frame_id = str[start:end]
01390 _x = self
01391 start = end
01392 end += 8
01393 (_x.region.image.height, _x.region.image.width,) = _struct_2I.unpack(str[start:end])
01394 start = end
01395 end += 4
01396 (length,) = _struct_I.unpack(str[start:end])
01397 start = end
01398 end += length
01399 if python3:
01400 self.region.image.encoding = str[start:end].decode('utf-8')
01401 else:
01402 self.region.image.encoding = str[start:end]
01403 _x = self
01404 start = end
01405 end += 5
01406 (_x.region.image.is_bigendian, _x.region.image.step,) = _struct_BI.unpack(str[start:end])
01407 start = end
01408 end += 4
01409 (length,) = _struct_I.unpack(str[start:end])
01410 start = end
01411 end += length
01412 if python3:
01413 self.region.image.data = str[start:end].decode('utf-8')
01414 else:
01415 self.region.image.data = str[start:end]
01416 _x = self
01417 start = end
01418 end += 12
01419 (_x.region.disparity_image.header.seq, _x.region.disparity_image.header.stamp.secs, _x.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01420 start = end
01421 end += 4
01422 (length,) = _struct_I.unpack(str[start:end])
01423 start = end
01424 end += length
01425 if python3:
01426 self.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
01427 else:
01428 self.region.disparity_image.header.frame_id = str[start:end]
01429 _x = self
01430 start = end
01431 end += 8
01432 (_x.region.disparity_image.height, _x.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01433 start = end
01434 end += 4
01435 (length,) = _struct_I.unpack(str[start:end])
01436 start = end
01437 end += length
01438 if python3:
01439 self.region.disparity_image.encoding = str[start:end].decode('utf-8')
01440 else:
01441 self.region.disparity_image.encoding = str[start:end]
01442 _x = self
01443 start = end
01444 end += 5
01445 (_x.region.disparity_image.is_bigendian, _x.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01446 start = end
01447 end += 4
01448 (length,) = _struct_I.unpack(str[start:end])
01449 start = end
01450 end += length
01451 if python3:
01452 self.region.disparity_image.data = str[start:end].decode('utf-8')
01453 else:
01454 self.region.disparity_image.data = str[start:end]
01455 _x = self
01456 start = end
01457 end += 12
01458 (_x.region.cam_info.header.seq, _x.region.cam_info.header.stamp.secs, _x.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01459 start = end
01460 end += 4
01461 (length,) = _struct_I.unpack(str[start:end])
01462 start = end
01463 end += length
01464 if python3:
01465 self.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
01466 else:
01467 self.region.cam_info.header.frame_id = str[start:end]
01468 _x = self
01469 start = end
01470 end += 8
01471 (_x.region.cam_info.height, _x.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01472 start = end
01473 end += 4
01474 (length,) = _struct_I.unpack(str[start:end])
01475 start = end
01476 end += length
01477 if python3:
01478 self.region.cam_info.distortion_model = str[start:end].decode('utf-8')
01479 else:
01480 self.region.cam_info.distortion_model = str[start:end]
01481 start = end
01482 end += 4
01483 (length,) = _struct_I.unpack(str[start:end])
01484 pattern = '<%sd'%length
01485 start = end
01486 end += struct.calcsize(pattern)
01487 self.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01488 start = end
01489 end += 72
01490 self.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01491 start = end
01492 end += 72
01493 self.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01494 start = end
01495 end += 96
01496 self.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
01497 _x = self
01498 start = end
01499 end += 37
01500 (_x.region.cam_info.binning_x, _x.region.cam_info.binning_y, _x.region.cam_info.roi.x_offset, _x.region.cam_info.roi.y_offset, _x.region.cam_info.roi.height, _x.region.cam_info.roi.width, _x.region.cam_info.roi.do_rectify, _x.region.roi_box_pose.header.seq, _x.region.roi_box_pose.header.stamp.secs, _x.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01501 self.region.cam_info.roi.do_rectify = bool(self.region.cam_info.roi.do_rectify)
01502 start = end
01503 end += 4
01504 (length,) = _struct_I.unpack(str[start:end])
01505 start = end
01506 end += length
01507 if python3:
01508 self.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
01509 else:
01510 self.region.roi_box_pose.header.frame_id = str[start:end]
01511 _x = self
01512 start = end
01513 end += 80
01514 (_x.region.roi_box_pose.pose.position.x, _x.region.roi_box_pose.pose.position.y, _x.region.roi_box_pose.pose.position.z, _x.region.roi_box_pose.pose.orientation.x, _x.region.roi_box_pose.pose.orientation.y, _x.region.roi_box_pose.pose.orientation.z, _x.region.roi_box_pose.pose.orientation.w, _x.region.roi_box_dims.x, _x.region.roi_box_dims.y, _x.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
01515 start = end
01516 end += 4
01517 (length,) = _struct_I.unpack(str[start:end])
01518 start = end
01519 end += length
01520 if python3:
01521 self.collision_name = str[start:end].decode('utf-8')
01522 else:
01523 self.collision_name = str[start:end]
01524 return self
01525 except struct.error as e:
01526 raise genpy.DeserializationError(e) #most likely buffer underfill
01527
01528 _struct_I = genpy.struct_I
01529 _struct_IBI = struct.Struct("<IBI")
01530 _struct_6IB3I = struct.Struct("<6IB3I")
01531 _struct_B = struct.Struct("<B")
01532 _struct_12d = struct.Struct("<12d")
01533 _struct_f = struct.Struct("<f")
01534 _struct_i = struct.Struct("<i")
01535 _struct_BI = struct.Struct("<BI")
01536 _struct_10d = struct.Struct("<10d")
01537 _struct_3f = struct.Struct("<3f")
01538 _struct_3I = struct.Struct("<3I")
01539 _struct_9d = struct.Struct("<9d")
01540 _struct_B2I = struct.Struct("<B2I")
01541 _struct_4d = struct.Struct("<4d")
01542 _struct_2I = struct.Struct("<2I")
01543 _struct_3d = struct.Struct("<3d")