00001 """autogenerated by genpy from pr2_interactive_object_detection/GraspableObjectList.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 arm_navigation_msgs.msg
00008 import object_manipulation_msgs.msg
00009 import geometry_msgs.msg
00010 import sensor_msgs.msg
00011 import household_objects_database_msgs.msg
00012 import std_msgs.msg
00013
00014 class GraspableObjectList(genpy.Message):
00015 _md5sum = "3504435d89178a4a3df53086cd29599c"
00016 _type = "pr2_interactive_object_detection/GraspableObjectList"
00017 _has_header = False
00018 _full_text = """object_manipulation_msgs/GraspableObject[] graspable_objects
00019
00020 #Information required for visualization
00021
00022 sensor_msgs/Image image
00023 sensor_msgs/CameraInfo camera_info
00024
00025 #Holds a single mesh for each recognized graspable object, an empty mesh otherwise
00026 arm_navigation_msgs/Shape[] meshes
00027
00028 #pose to transform the frame of the clusters/object poses into camera coordinates
00029 geometry_msgs/Pose reference_to_camera
00030
00031 ================================================================================
00032 MSG: object_manipulation_msgs/GraspableObject
00033 # an object that the object_manipulator can work on
00034
00035 # a graspable object can be represented in multiple ways. This message
00036 # can contain all of them. Which one is actually used is up to the receiver
00037 # of this message. When adding new representations, one must be careful that
00038 # they have reasonable lightweight defaults indicating that that particular
00039 # representation is not available.
00040
00041 # the tf frame to be used as a reference frame when combining information from
00042 # the different representations below
00043 string reference_frame_id
00044
00045 # potential recognition results from a database of models
00046 # all poses are relative to the object reference pose
00047 household_objects_database_msgs/DatabaseModelPose[] potential_models
00048
00049 # the point cloud itself
00050 sensor_msgs/PointCloud cluster
00051
00052 # a region of a PointCloud2 of interest
00053 object_manipulation_msgs/SceneRegion region
00054
00055 # the name that this object has in the collision environment
00056 string collision_name
00057 ================================================================================
00058 MSG: household_objects_database_msgs/DatabaseModelPose
00059 # Informs that a specific model from the Model Database has been
00060 # identified at a certain location
00061
00062 # the database id of the model
00063 int32 model_id
00064
00065 # the pose that it can be found in
00066 geometry_msgs/PoseStamped pose
00067
00068 # a measure of the confidence level in this detection result
00069 float32 confidence
00070
00071 # the name of the object detector that generated this detection result
00072 string detector_name
00073
00074 ================================================================================
00075 MSG: geometry_msgs/PoseStamped
00076 # A Pose with reference coordinate frame and timestamp
00077 Header header
00078 Pose pose
00079
00080 ================================================================================
00081 MSG: std_msgs/Header
00082 # Standard metadata for higher-level stamped data types.
00083 # This is generally used to communicate timestamped data
00084 # in a particular coordinate frame.
00085 #
00086 # sequence ID: consecutively increasing ID
00087 uint32 seq
00088 #Two-integer timestamp that is expressed as:
00089 # * stamp.secs: seconds (stamp_secs) since epoch
00090 # * stamp.nsecs: nanoseconds since stamp_secs
00091 # time-handling sugar is provided by the client library
00092 time stamp
00093 #Frame this data is associated with
00094 # 0: no frame
00095 # 1: global frame
00096 string frame_id
00097
00098 ================================================================================
00099 MSG: geometry_msgs/Pose
00100 # A representation of pose in free space, composed of postion and orientation.
00101 Point position
00102 Quaternion orientation
00103
00104 ================================================================================
00105 MSG: geometry_msgs/Point
00106 # This contains the position of a point in free space
00107 float64 x
00108 float64 y
00109 float64 z
00110
00111 ================================================================================
00112 MSG: geometry_msgs/Quaternion
00113 # This represents an orientation in free space in quaternion form.
00114
00115 float64 x
00116 float64 y
00117 float64 z
00118 float64 w
00119
00120 ================================================================================
00121 MSG: sensor_msgs/PointCloud
00122 # This message holds a collection of 3d points, plus optional additional
00123 # information about each point.
00124
00125 # Time of sensor data acquisition, coordinate frame ID.
00126 Header header
00127
00128 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00129 # in the frame given in the header.
00130 geometry_msgs/Point32[] points
00131
00132 # Each channel should have the same number of elements as points array,
00133 # and the data in each channel should correspond 1:1 with each point.
00134 # Channel names in common practice are listed in ChannelFloat32.msg.
00135 ChannelFloat32[] channels
00136
00137 ================================================================================
00138 MSG: geometry_msgs/Point32
00139 # This contains the position of a point in free space(with 32 bits of precision).
00140 # It is recommeded to use Point wherever possible instead of Point32.
00141 #
00142 # This recommendation is to promote interoperability.
00143 #
00144 # This message is designed to take up less space when sending
00145 # lots of points at once, as in the case of a PointCloud.
00146
00147 float32 x
00148 float32 y
00149 float32 z
00150 ================================================================================
00151 MSG: sensor_msgs/ChannelFloat32
00152 # This message is used by the PointCloud message to hold optional data
00153 # associated with each point in the cloud. The length of the values
00154 # array should be the same as the length of the points array in the
00155 # PointCloud, and each value should be associated with the corresponding
00156 # point.
00157
00158 # Channel names in existing practice include:
00159 # "u", "v" - row and column (respectively) in the left stereo image.
00160 # This is opposite to usual conventions but remains for
00161 # historical reasons. The newer PointCloud2 message has no
00162 # such problem.
00163 # "rgb" - For point clouds produced by color stereo cameras. uint8
00164 # (R,G,B) values packed into the least significant 24 bits,
00165 # in order.
00166 # "intensity" - laser or pixel intensity.
00167 # "distance"
00168
00169 # The channel name should give semantics of the channel (e.g.
00170 # "intensity" instead of "value").
00171 string name
00172
00173 # The values array should be 1-1 with the elements of the associated
00174 # PointCloud.
00175 float32[] values
00176
00177 ================================================================================
00178 MSG: object_manipulation_msgs/SceneRegion
00179 # Point cloud
00180 sensor_msgs/PointCloud2 cloud
00181
00182 # Indices for the region of interest
00183 int32[] mask
00184
00185 # One of the corresponding 2D images, if applicable
00186 sensor_msgs/Image image
00187
00188 # The disparity image, if applicable
00189 sensor_msgs/Image disparity_image
00190
00191 # Camera info for the camera that took the image
00192 sensor_msgs/CameraInfo cam_info
00193
00194 # a 3D region of interest for grasp planning
00195 geometry_msgs/PoseStamped roi_box_pose
00196 geometry_msgs/Vector3 roi_box_dims
00197
00198 ================================================================================
00199 MSG: sensor_msgs/PointCloud2
00200 # This message holds a collection of N-dimensional points, which may
00201 # contain additional information such as normals, intensity, etc. The
00202 # point data is stored as a binary blob, its layout described by the
00203 # contents of the "fields" array.
00204
00205 # The point cloud data may be organized 2d (image-like) or 1d
00206 # (unordered). Point clouds organized as 2d images may be produced by
00207 # camera depth sensors such as stereo or time-of-flight.
00208
00209 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00210 # points).
00211 Header header
00212
00213 # 2D structure of the point cloud. If the cloud is unordered, height is
00214 # 1 and width is the length of the point cloud.
00215 uint32 height
00216 uint32 width
00217
00218 # Describes the channels and their layout in the binary data blob.
00219 PointField[] fields
00220
00221 bool is_bigendian # Is this data bigendian?
00222 uint32 point_step # Length of a point in bytes
00223 uint32 row_step # Length of a row in bytes
00224 uint8[] data # Actual point data, size is (row_step*height)
00225
00226 bool is_dense # True if there are no invalid points
00227
00228 ================================================================================
00229 MSG: sensor_msgs/PointField
00230 # This message holds the description of one point entry in the
00231 # PointCloud2 message format.
00232 uint8 INT8 = 1
00233 uint8 UINT8 = 2
00234 uint8 INT16 = 3
00235 uint8 UINT16 = 4
00236 uint8 INT32 = 5
00237 uint8 UINT32 = 6
00238 uint8 FLOAT32 = 7
00239 uint8 FLOAT64 = 8
00240
00241 string name # Name of field
00242 uint32 offset # Offset from start of point struct
00243 uint8 datatype # Datatype enumeration, see above
00244 uint32 count # How many elements in the field
00245
00246 ================================================================================
00247 MSG: sensor_msgs/Image
00248 # This message contains an uncompressed image
00249 # (0, 0) is at top-left corner of image
00250 #
00251
00252 Header header # Header timestamp should be acquisition time of image
00253 # Header frame_id should be optical frame of camera
00254 # origin of frame should be optical center of cameara
00255 # +x should point to the right in the image
00256 # +y should point down in the image
00257 # +z should point into to plane of the image
00258 # If the frame_id here and the frame_id of the CameraInfo
00259 # message associated with the image conflict
00260 # the behavior is undefined
00261
00262 uint32 height # image height, that is, number of rows
00263 uint32 width # image width, that is, number of columns
00264
00265 # The legal values for encoding are in file src/image_encodings.cpp
00266 # If you want to standardize a new string format, join
00267 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00268
00269 string encoding # Encoding of pixels -- channel meaning, ordering, size
00270 # taken from the list of strings in src/image_encodings.cpp
00271
00272 uint8 is_bigendian # is this data bigendian?
00273 uint32 step # Full row length in bytes
00274 uint8[] data # actual matrix data, size is (step * rows)
00275
00276 ================================================================================
00277 MSG: sensor_msgs/CameraInfo
00278 # This message defines meta information for a camera. It should be in a
00279 # camera namespace on topic "camera_info" and accompanied by up to five
00280 # image topics named:
00281 #
00282 # image_raw - raw data from the camera driver, possibly Bayer encoded
00283 # image - monochrome, distorted
00284 # image_color - color, distorted
00285 # image_rect - monochrome, rectified
00286 # image_rect_color - color, rectified
00287 #
00288 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00289 # for producing the four processed image topics from image_raw and
00290 # camera_info. The meaning of the camera parameters are described in
00291 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00292 #
00293 # The image_geometry package provides a user-friendly interface to
00294 # common operations using this meta information. If you want to, e.g.,
00295 # project a 3d point into image coordinates, we strongly recommend
00296 # using image_geometry.
00297 #
00298 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00299 # zeroed out. In particular, clients may assume that K[0] == 0.0
00300 # indicates an uncalibrated camera.
00301
00302 #######################################################################
00303 # Image acquisition info #
00304 #######################################################################
00305
00306 # Time of image acquisition, camera coordinate frame ID
00307 Header header # Header timestamp should be acquisition time of image
00308 # Header frame_id should be optical frame of camera
00309 # origin of frame should be optical center of camera
00310 # +x should point to the right in the image
00311 # +y should point down in the image
00312 # +z should point into the plane of the image
00313
00314
00315 #######################################################################
00316 # Calibration Parameters #
00317 #######################################################################
00318 # These are fixed during camera calibration. Their values will be the #
00319 # same in all messages until the camera is recalibrated. Note that #
00320 # self-calibrating systems may "recalibrate" frequently. #
00321 # #
00322 # The internal parameters can be used to warp a raw (distorted) image #
00323 # to: #
00324 # 1. An undistorted image (requires D and K) #
00325 # 2. A rectified image (requires D, K, R) #
00326 # The projection matrix P projects 3D points into the rectified image.#
00327 #######################################################################
00328
00329 # The image dimensions with which the camera was calibrated. Normally
00330 # this will be the full camera resolution in pixels.
00331 uint32 height
00332 uint32 width
00333
00334 # The distortion model used. Supported models are listed in
00335 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00336 # simple model of radial and tangential distortion - is sufficent.
00337 string distortion_model
00338
00339 # The distortion parameters, size depending on the distortion model.
00340 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00341 float64[] D
00342
00343 # Intrinsic camera matrix for the raw (distorted) images.
00344 # [fx 0 cx]
00345 # K = [ 0 fy cy]
00346 # [ 0 0 1]
00347 # Projects 3D points in the camera coordinate frame to 2D pixel
00348 # coordinates using the focal lengths (fx, fy) and principal point
00349 # (cx, cy).
00350 float64[9] K # 3x3 row-major matrix
00351
00352 # Rectification matrix (stereo cameras only)
00353 # A rotation matrix aligning the camera coordinate system to the ideal
00354 # stereo image plane so that epipolar lines in both stereo images are
00355 # parallel.
00356 float64[9] R # 3x3 row-major matrix
00357
00358 # Projection/camera matrix
00359 # [fx' 0 cx' Tx]
00360 # P = [ 0 fy' cy' Ty]
00361 # [ 0 0 1 0]
00362 # By convention, this matrix specifies the intrinsic (camera) matrix
00363 # of the processed (rectified) image. That is, the left 3x3 portion
00364 # is the normal camera intrinsic matrix for the rectified image.
00365 # It projects 3D points in the camera coordinate frame to 2D pixel
00366 # coordinates using the focal lengths (fx', fy') and principal point
00367 # (cx', cy') - these may differ from the values in K.
00368 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00369 # also have R = the identity and P[1:3,1:3] = K.
00370 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00371 # position of the optical center of the second camera in the first
00372 # camera's frame. We assume Tz = 0 so both cameras are in the same
00373 # stereo image plane. The first camera always has Tx = Ty = 0. For
00374 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00375 # Tx = -fx' * B, where B is the baseline between the cameras.
00376 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00377 # the rectified image is given by:
00378 # [u v w]' = P * [X Y Z 1]'
00379 # x = u / w
00380 # y = v / w
00381 # This holds for both images of a stereo pair.
00382 float64[12] P # 3x4 row-major matrix
00383
00384
00385 #######################################################################
00386 # Operational Parameters #
00387 #######################################################################
00388 # These define the image region actually captured by the camera #
00389 # driver. Although they affect the geometry of the output image, they #
00390 # may be changed freely without recalibrating the camera. #
00391 #######################################################################
00392
00393 # Binning refers here to any camera setting which combines rectangular
00394 # neighborhoods of pixels into larger "super-pixels." It reduces the
00395 # resolution of the output image to
00396 # (width / binning_x) x (height / binning_y).
00397 # The default values binning_x = binning_y = 0 is considered the same
00398 # as binning_x = binning_y = 1 (no subsampling).
00399 uint32 binning_x
00400 uint32 binning_y
00401
00402 # Region of interest (subwindow of full camera resolution), given in
00403 # full resolution (unbinned) image coordinates. A particular ROI
00404 # always denotes the same window of pixels on the camera sensor,
00405 # regardless of binning settings.
00406 # The default setting of roi (all values 0) is considered the same as
00407 # full resolution (roi.width = width, roi.height = height).
00408 RegionOfInterest roi
00409
00410 ================================================================================
00411 MSG: sensor_msgs/RegionOfInterest
00412 # This message is used to specify a region of interest within an image.
00413 #
00414 # When used to specify the ROI setting of the camera when the image was
00415 # taken, the height and width fields should either match the height and
00416 # width fields for the associated image; or height = width = 0
00417 # indicates that the full resolution image was captured.
00418
00419 uint32 x_offset # Leftmost pixel of the ROI
00420 # (0 if the ROI includes the left edge of the image)
00421 uint32 y_offset # Topmost pixel of the ROI
00422 # (0 if the ROI includes the top edge of the image)
00423 uint32 height # Height of ROI
00424 uint32 width # Width of ROI
00425
00426 # True if a distinct rectified ROI should be calculated from the "raw"
00427 # ROI in this message. Typically this should be False if the full image
00428 # is captured (ROI not used), and True if a subwindow is captured (ROI
00429 # used).
00430 bool do_rectify
00431
00432 ================================================================================
00433 MSG: geometry_msgs/Vector3
00434 # This represents a vector in free space.
00435
00436 float64 x
00437 float64 y
00438 float64 z
00439 ================================================================================
00440 MSG: arm_navigation_msgs/Shape
00441 byte SPHERE=0
00442 byte BOX=1
00443 byte CYLINDER=2
00444 byte MESH=3
00445
00446 byte type
00447
00448
00449 #### define sphere, box, cylinder ####
00450 # the origin of each shape is considered at the shape's center
00451
00452 # for sphere
00453 # radius := dimensions[0]
00454
00455 # for cylinder
00456 # radius := dimensions[0]
00457 # length := dimensions[1]
00458 # the length is along the Z axis
00459
00460 # for box
00461 # size_x := dimensions[0]
00462 # size_y := dimensions[1]
00463 # size_z := dimensions[2]
00464 float64[] dimensions
00465
00466
00467 #### define mesh ####
00468
00469 # list of triangles; triangle k is defined by tre vertices located
00470 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00471 int32[] triangles
00472 geometry_msgs/Point[] vertices
00473
00474 """
00475 __slots__ = ['graspable_objects','image','camera_info','meshes','reference_to_camera']
00476 _slot_types = ['object_manipulation_msgs/GraspableObject[]','sensor_msgs/Image','sensor_msgs/CameraInfo','arm_navigation_msgs/Shape[]','geometry_msgs/Pose']
00477
00478 def __init__(self, *args, **kwds):
00479 """
00480 Constructor. Any message fields that are implicitly/explicitly
00481 set to None will be assigned a default value. The recommend
00482 use is keyword arguments as this is more robust to future message
00483 changes. You cannot mix in-order arguments and keyword arguments.
00484
00485 The available fields are:
00486 graspable_objects,image,camera_info,meshes,reference_to_camera
00487
00488 :param args: complete set of field values, in .msg order
00489 :param kwds: use keyword arguments corresponding to message field names
00490 to set specific fields.
00491 """
00492 if args or kwds:
00493 super(GraspableObjectList, self).__init__(*args, **kwds)
00494 #message fields cannot be None, assign default values for those that are
00495 if self.graspable_objects is None:
00496 self.graspable_objects = []
00497 if self.image is None:
00498 self.image = sensor_msgs.msg.Image()
00499 if self.camera_info is None:
00500 self.camera_info = sensor_msgs.msg.CameraInfo()
00501 if self.meshes is None:
00502 self.meshes = []
00503 if self.reference_to_camera is None:
00504 self.reference_to_camera = geometry_msgs.msg.Pose()
00505 else:
00506 self.graspable_objects = []
00507 self.image = sensor_msgs.msg.Image()
00508 self.camera_info = sensor_msgs.msg.CameraInfo()
00509 self.meshes = []
00510 self.reference_to_camera = geometry_msgs.msg.Pose()
00511
00512 def _get_types(self):
00513 """
00514 internal API method
00515 """
00516 return self._slot_types
00517
00518 def serialize(self, buff):
00519 """
00520 serialize message into buffer
00521 :param buff: buffer, ``StringIO``
00522 """
00523 try:
00524 length = len(self.graspable_objects)
00525 buff.write(_struct_I.pack(length))
00526 for val1 in self.graspable_objects:
00527 _x = val1.reference_frame_id
00528 length = len(_x)
00529 if python3 or type(_x) == unicode:
00530 _x = _x.encode('utf-8')
00531 length = len(_x)
00532 buff.write(struct.pack('<I%ss'%length, length, _x))
00533 length = len(val1.potential_models)
00534 buff.write(_struct_I.pack(length))
00535 for val2 in val1.potential_models:
00536 buff.write(_struct_i.pack(val2.model_id))
00537 _v1 = val2.pose
00538 _v2 = _v1.header
00539 buff.write(_struct_I.pack(_v2.seq))
00540 _v3 = _v2.stamp
00541 _x = _v3
00542 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00543 _x = _v2.frame_id
00544 length = len(_x)
00545 if python3 or type(_x) == unicode:
00546 _x = _x.encode('utf-8')
00547 length = len(_x)
00548 buff.write(struct.pack('<I%ss'%length, length, _x))
00549 _v4 = _v1.pose
00550 _v5 = _v4.position
00551 _x = _v5
00552 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00553 _v6 = _v4.orientation
00554 _x = _v6
00555 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00556 buff.write(_struct_f.pack(val2.confidence))
00557 _x = val2.detector_name
00558 length = len(_x)
00559 if python3 or type(_x) == unicode:
00560 _x = _x.encode('utf-8')
00561 length = len(_x)
00562 buff.write(struct.pack('<I%ss'%length, length, _x))
00563 _v7 = val1.cluster
00564 _v8 = _v7.header
00565 buff.write(_struct_I.pack(_v8.seq))
00566 _v9 = _v8.stamp
00567 _x = _v9
00568 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00569 _x = _v8.frame_id
00570 length = len(_x)
00571 if python3 or type(_x) == unicode:
00572 _x = _x.encode('utf-8')
00573 length = len(_x)
00574 buff.write(struct.pack('<I%ss'%length, length, _x))
00575 length = len(_v7.points)
00576 buff.write(_struct_I.pack(length))
00577 for val3 in _v7.points:
00578 _x = val3
00579 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00580 length = len(_v7.channels)
00581 buff.write(_struct_I.pack(length))
00582 for val3 in _v7.channels:
00583 _x = val3.name
00584 length = len(_x)
00585 if python3 or type(_x) == unicode:
00586 _x = _x.encode('utf-8')
00587 length = len(_x)
00588 buff.write(struct.pack('<I%ss'%length, length, _x))
00589 length = len(val3.values)
00590 buff.write(_struct_I.pack(length))
00591 pattern = '<%sf'%length
00592 buff.write(struct.pack(pattern, *val3.values))
00593 _v10 = val1.region
00594 _v11 = _v10.cloud
00595 _v12 = _v11.header
00596 buff.write(_struct_I.pack(_v12.seq))
00597 _v13 = _v12.stamp
00598 _x = _v13
00599 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00600 _x = _v12.frame_id
00601 length = len(_x)
00602 if python3 or type(_x) == unicode:
00603 _x = _x.encode('utf-8')
00604 length = len(_x)
00605 buff.write(struct.pack('<I%ss'%length, length, _x))
00606 _x = _v11
00607 buff.write(_struct_2I.pack(_x.height, _x.width))
00608 length = len(_v11.fields)
00609 buff.write(_struct_I.pack(length))
00610 for val4 in _v11.fields:
00611 _x = val4.name
00612 length = len(_x)
00613 if python3 or type(_x) == unicode:
00614 _x = _x.encode('utf-8')
00615 length = len(_x)
00616 buff.write(struct.pack('<I%ss'%length, length, _x))
00617 _x = val4
00618 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00619 _x = _v11
00620 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
00621 _x = _v11.data
00622 length = len(_x)
00623 # - if encoded as a list instead, serialize as bytes instead of string
00624 if type(_x) in [list, tuple]:
00625 buff.write(struct.pack('<I%sB'%length, length, *_x))
00626 else:
00627 buff.write(struct.pack('<I%ss'%length, length, _x))
00628 buff.write(_struct_B.pack(_v11.is_dense))
00629 length = len(_v10.mask)
00630 buff.write(_struct_I.pack(length))
00631 pattern = '<%si'%length
00632 buff.write(struct.pack(pattern, *_v10.mask))
00633 _v14 = _v10.image
00634 _v15 = _v14.header
00635 buff.write(_struct_I.pack(_v15.seq))
00636 _v16 = _v15.stamp
00637 _x = _v16
00638 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00639 _x = _v15.frame_id
00640 length = len(_x)
00641 if python3 or type(_x) == unicode:
00642 _x = _x.encode('utf-8')
00643 length = len(_x)
00644 buff.write(struct.pack('<I%ss'%length, length, _x))
00645 _x = _v14
00646 buff.write(_struct_2I.pack(_x.height, _x.width))
00647 _x = _v14.encoding
00648 length = len(_x)
00649 if python3 or type(_x) == unicode:
00650 _x = _x.encode('utf-8')
00651 length = len(_x)
00652 buff.write(struct.pack('<I%ss'%length, length, _x))
00653 _x = _v14
00654 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00655 _x = _v14.data
00656 length = len(_x)
00657 # - if encoded as a list instead, serialize as bytes instead of string
00658 if type(_x) in [list, tuple]:
00659 buff.write(struct.pack('<I%sB'%length, length, *_x))
00660 else:
00661 buff.write(struct.pack('<I%ss'%length, length, _x))
00662 _v17 = _v10.disparity_image
00663 _v18 = _v17.header
00664 buff.write(_struct_I.pack(_v18.seq))
00665 _v19 = _v18.stamp
00666 _x = _v19
00667 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00668 _x = _v18.frame_id
00669 length = len(_x)
00670 if python3 or type(_x) == unicode:
00671 _x = _x.encode('utf-8')
00672 length = len(_x)
00673 buff.write(struct.pack('<I%ss'%length, length, _x))
00674 _x = _v17
00675 buff.write(_struct_2I.pack(_x.height, _x.width))
00676 _x = _v17.encoding
00677 length = len(_x)
00678 if python3 or type(_x) == unicode:
00679 _x = _x.encode('utf-8')
00680 length = len(_x)
00681 buff.write(struct.pack('<I%ss'%length, length, _x))
00682 _x = _v17
00683 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00684 _x = _v17.data
00685 length = len(_x)
00686 # - if encoded as a list instead, serialize as bytes instead of string
00687 if type(_x) in [list, tuple]:
00688 buff.write(struct.pack('<I%sB'%length, length, *_x))
00689 else:
00690 buff.write(struct.pack('<I%ss'%length, length, _x))
00691 _v20 = _v10.cam_info
00692 _v21 = _v20.header
00693 buff.write(_struct_I.pack(_v21.seq))
00694 _v22 = _v21.stamp
00695 _x = _v22
00696 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00697 _x = _v21.frame_id
00698 length = len(_x)
00699 if python3 or type(_x) == unicode:
00700 _x = _x.encode('utf-8')
00701 length = len(_x)
00702 buff.write(struct.pack('<I%ss'%length, length, _x))
00703 _x = _v20
00704 buff.write(_struct_2I.pack(_x.height, _x.width))
00705 _x = _v20.distortion_model
00706 length = len(_x)
00707 if python3 or type(_x) == unicode:
00708 _x = _x.encode('utf-8')
00709 length = len(_x)
00710 buff.write(struct.pack('<I%ss'%length, length, _x))
00711 length = len(_v20.D)
00712 buff.write(_struct_I.pack(length))
00713 pattern = '<%sd'%length
00714 buff.write(struct.pack(pattern, *_v20.D))
00715 buff.write(_struct_9d.pack(*_v20.K))
00716 buff.write(_struct_9d.pack(*_v20.R))
00717 buff.write(_struct_12d.pack(*_v20.P))
00718 _x = _v20
00719 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
00720 _v23 = _v20.roi
00721 _x = _v23
00722 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
00723 _v24 = _v10.roi_box_pose
00724 _v25 = _v24.header
00725 buff.write(_struct_I.pack(_v25.seq))
00726 _v26 = _v25.stamp
00727 _x = _v26
00728 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00729 _x = _v25.frame_id
00730 length = len(_x)
00731 if python3 or type(_x) == unicode:
00732 _x = _x.encode('utf-8')
00733 length = len(_x)
00734 buff.write(struct.pack('<I%ss'%length, length, _x))
00735 _v27 = _v24.pose
00736 _v28 = _v27.position
00737 _x = _v28
00738 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00739 _v29 = _v27.orientation
00740 _x = _v29
00741 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00742 _v30 = _v10.roi_box_dims
00743 _x = _v30
00744 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00745 _x = val1.collision_name
00746 length = len(_x)
00747 if python3 or type(_x) == unicode:
00748 _x = _x.encode('utf-8')
00749 length = len(_x)
00750 buff.write(struct.pack('<I%ss'%length, length, _x))
00751 _x = self
00752 buff.write(_struct_3I.pack(_x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs))
00753 _x = self.image.header.frame_id
00754 length = len(_x)
00755 if python3 or type(_x) == unicode:
00756 _x = _x.encode('utf-8')
00757 length = len(_x)
00758 buff.write(struct.pack('<I%ss'%length, length, _x))
00759 _x = self
00760 buff.write(_struct_2I.pack(_x.image.height, _x.image.width))
00761 _x = self.image.encoding
00762 length = len(_x)
00763 if python3 or type(_x) == unicode:
00764 _x = _x.encode('utf-8')
00765 length = len(_x)
00766 buff.write(struct.pack('<I%ss'%length, length, _x))
00767 _x = self
00768 buff.write(_struct_BI.pack(_x.image.is_bigendian, _x.image.step))
00769 _x = self.image.data
00770 length = len(_x)
00771 # - if encoded as a list instead, serialize as bytes instead of string
00772 if type(_x) in [list, tuple]:
00773 buff.write(struct.pack('<I%sB'%length, length, *_x))
00774 else:
00775 buff.write(struct.pack('<I%ss'%length, length, _x))
00776 _x = self
00777 buff.write(_struct_3I.pack(_x.camera_info.header.seq, _x.camera_info.header.stamp.secs, _x.camera_info.header.stamp.nsecs))
00778 _x = self.camera_info.header.frame_id
00779 length = len(_x)
00780 if python3 or type(_x) == unicode:
00781 _x = _x.encode('utf-8')
00782 length = len(_x)
00783 buff.write(struct.pack('<I%ss'%length, length, _x))
00784 _x = self
00785 buff.write(_struct_2I.pack(_x.camera_info.height, _x.camera_info.width))
00786 _x = self.camera_info.distortion_model
00787 length = len(_x)
00788 if python3 or type(_x) == unicode:
00789 _x = _x.encode('utf-8')
00790 length = len(_x)
00791 buff.write(struct.pack('<I%ss'%length, length, _x))
00792 length = len(self.camera_info.D)
00793 buff.write(_struct_I.pack(length))
00794 pattern = '<%sd'%length
00795 buff.write(struct.pack(pattern, *self.camera_info.D))
00796 buff.write(_struct_9d.pack(*self.camera_info.K))
00797 buff.write(_struct_9d.pack(*self.camera_info.R))
00798 buff.write(_struct_12d.pack(*self.camera_info.P))
00799 _x = self
00800 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))
00801 length = len(self.meshes)
00802 buff.write(_struct_I.pack(length))
00803 for val1 in self.meshes:
00804 buff.write(_struct_b.pack(val1.type))
00805 length = len(val1.dimensions)
00806 buff.write(_struct_I.pack(length))
00807 pattern = '<%sd'%length
00808 buff.write(struct.pack(pattern, *val1.dimensions))
00809 length = len(val1.triangles)
00810 buff.write(_struct_I.pack(length))
00811 pattern = '<%si'%length
00812 buff.write(struct.pack(pattern, *val1.triangles))
00813 length = len(val1.vertices)
00814 buff.write(_struct_I.pack(length))
00815 for val2 in val1.vertices:
00816 _x = val2
00817 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00818 _x = self
00819 buff.write(_struct_7d.pack(_x.reference_to_camera.position.x, _x.reference_to_camera.position.y, _x.reference_to_camera.position.z, _x.reference_to_camera.orientation.x, _x.reference_to_camera.orientation.y, _x.reference_to_camera.orientation.z, _x.reference_to_camera.orientation.w))
00820 except struct.error as se: self._check_types(se)
00821 except TypeError as te: self._check_types(te)
00822
00823 def deserialize(self, str):
00824 """
00825 unpack serialized message in str into this message instance
00826 :param str: byte array of serialized message, ``str``
00827 """
00828 try:
00829 if self.graspable_objects is None:
00830 self.graspable_objects = None
00831 if self.image is None:
00832 self.image = sensor_msgs.msg.Image()
00833 if self.camera_info is None:
00834 self.camera_info = sensor_msgs.msg.CameraInfo()
00835 if self.meshes is None:
00836 self.meshes = None
00837 if self.reference_to_camera is None:
00838 self.reference_to_camera = geometry_msgs.msg.Pose()
00839 end = 0
00840 start = end
00841 end += 4
00842 (length,) = _struct_I.unpack(str[start:end])
00843 self.graspable_objects = []
00844 for i in range(0, length):
00845 val1 = object_manipulation_msgs.msg.GraspableObject()
00846 start = end
00847 end += 4
00848 (length,) = _struct_I.unpack(str[start:end])
00849 start = end
00850 end += length
00851 if python3:
00852 val1.reference_frame_id = str[start:end].decode('utf-8')
00853 else:
00854 val1.reference_frame_id = str[start:end]
00855 start = end
00856 end += 4
00857 (length,) = _struct_I.unpack(str[start:end])
00858 val1.potential_models = []
00859 for i in range(0, length):
00860 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
00861 start = end
00862 end += 4
00863 (val2.model_id,) = _struct_i.unpack(str[start:end])
00864 _v31 = val2.pose
00865 _v32 = _v31.header
00866 start = end
00867 end += 4
00868 (_v32.seq,) = _struct_I.unpack(str[start:end])
00869 _v33 = _v32.stamp
00870 _x = _v33
00871 start = end
00872 end += 8
00873 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00874 start = end
00875 end += 4
00876 (length,) = _struct_I.unpack(str[start:end])
00877 start = end
00878 end += length
00879 if python3:
00880 _v32.frame_id = str[start:end].decode('utf-8')
00881 else:
00882 _v32.frame_id = str[start:end]
00883 _v34 = _v31.pose
00884 _v35 = _v34.position
00885 _x = _v35
00886 start = end
00887 end += 24
00888 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00889 _v36 = _v34.orientation
00890 _x = _v36
00891 start = end
00892 end += 32
00893 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00894 start = end
00895 end += 4
00896 (val2.confidence,) = _struct_f.unpack(str[start:end])
00897 start = end
00898 end += 4
00899 (length,) = _struct_I.unpack(str[start:end])
00900 start = end
00901 end += length
00902 if python3:
00903 val2.detector_name = str[start:end].decode('utf-8')
00904 else:
00905 val2.detector_name = str[start:end]
00906 val1.potential_models.append(val2)
00907 _v37 = val1.cluster
00908 _v38 = _v37.header
00909 start = end
00910 end += 4
00911 (_v38.seq,) = _struct_I.unpack(str[start:end])
00912 _v39 = _v38.stamp
00913 _x = _v39
00914 start = end
00915 end += 8
00916 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00917 start = end
00918 end += 4
00919 (length,) = _struct_I.unpack(str[start:end])
00920 start = end
00921 end += length
00922 if python3:
00923 _v38.frame_id = str[start:end].decode('utf-8')
00924 else:
00925 _v38.frame_id = str[start:end]
00926 start = end
00927 end += 4
00928 (length,) = _struct_I.unpack(str[start:end])
00929 _v37.points = []
00930 for i in range(0, length):
00931 val3 = geometry_msgs.msg.Point32()
00932 _x = val3
00933 start = end
00934 end += 12
00935 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
00936 _v37.points.append(val3)
00937 start = end
00938 end += 4
00939 (length,) = _struct_I.unpack(str[start:end])
00940 _v37.channels = []
00941 for i in range(0, length):
00942 val3 = sensor_msgs.msg.ChannelFloat32()
00943 start = end
00944 end += 4
00945 (length,) = _struct_I.unpack(str[start:end])
00946 start = end
00947 end += length
00948 if python3:
00949 val3.name = str[start:end].decode('utf-8')
00950 else:
00951 val3.name = str[start:end]
00952 start = end
00953 end += 4
00954 (length,) = _struct_I.unpack(str[start:end])
00955 pattern = '<%sf'%length
00956 start = end
00957 end += struct.calcsize(pattern)
00958 val3.values = struct.unpack(pattern, str[start:end])
00959 _v37.channels.append(val3)
00960 _v40 = val1.region
00961 _v41 = _v40.cloud
00962 _v42 = _v41.header
00963 start = end
00964 end += 4
00965 (_v42.seq,) = _struct_I.unpack(str[start:end])
00966 _v43 = _v42.stamp
00967 _x = _v43
00968 start = end
00969 end += 8
00970 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00971 start = end
00972 end += 4
00973 (length,) = _struct_I.unpack(str[start:end])
00974 start = end
00975 end += length
00976 if python3:
00977 _v42.frame_id = str[start:end].decode('utf-8')
00978 else:
00979 _v42.frame_id = str[start:end]
00980 _x = _v41
00981 start = end
00982 end += 8
00983 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
00984 start = end
00985 end += 4
00986 (length,) = _struct_I.unpack(str[start:end])
00987 _v41.fields = []
00988 for i in range(0, length):
00989 val4 = sensor_msgs.msg.PointField()
00990 start = end
00991 end += 4
00992 (length,) = _struct_I.unpack(str[start:end])
00993 start = end
00994 end += length
00995 if python3:
00996 val4.name = str[start:end].decode('utf-8')
00997 else:
00998 val4.name = str[start:end]
00999 _x = val4
01000 start = end
01001 end += 9
01002 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01003 _v41.fields.append(val4)
01004 _x = _v41
01005 start = end
01006 end += 9
01007 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
01008 _v41.is_bigendian = bool(_v41.is_bigendian)
01009 start = end
01010 end += 4
01011 (length,) = _struct_I.unpack(str[start:end])
01012 start = end
01013 end += length
01014 if python3:
01015 _v41.data = str[start:end].decode('utf-8')
01016 else:
01017 _v41.data = str[start:end]
01018 start = end
01019 end += 1
01020 (_v41.is_dense,) = _struct_B.unpack(str[start:end])
01021 _v41.is_dense = bool(_v41.is_dense)
01022 start = end
01023 end += 4
01024 (length,) = _struct_I.unpack(str[start:end])
01025 pattern = '<%si'%length
01026 start = end
01027 end += struct.calcsize(pattern)
01028 _v40.mask = struct.unpack(pattern, str[start:end])
01029 _v44 = _v40.image
01030 _v45 = _v44.header
01031 start = end
01032 end += 4
01033 (_v45.seq,) = _struct_I.unpack(str[start:end])
01034 _v46 = _v45.stamp
01035 _x = _v46
01036 start = end
01037 end += 8
01038 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01039 start = end
01040 end += 4
01041 (length,) = _struct_I.unpack(str[start:end])
01042 start = end
01043 end += length
01044 if python3:
01045 _v45.frame_id = str[start:end].decode('utf-8')
01046 else:
01047 _v45.frame_id = str[start:end]
01048 _x = _v44
01049 start = end
01050 end += 8
01051 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01052 start = end
01053 end += 4
01054 (length,) = _struct_I.unpack(str[start:end])
01055 start = end
01056 end += length
01057 if python3:
01058 _v44.encoding = str[start:end].decode('utf-8')
01059 else:
01060 _v44.encoding = str[start:end]
01061 _x = _v44
01062 start = end
01063 end += 5
01064 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01065 start = end
01066 end += 4
01067 (length,) = _struct_I.unpack(str[start:end])
01068 start = end
01069 end += length
01070 if python3:
01071 _v44.data = str[start:end].decode('utf-8')
01072 else:
01073 _v44.data = str[start:end]
01074 _v47 = _v40.disparity_image
01075 _v48 = _v47.header
01076 start = end
01077 end += 4
01078 (_v48.seq,) = _struct_I.unpack(str[start:end])
01079 _v49 = _v48.stamp
01080 _x = _v49
01081 start = end
01082 end += 8
01083 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01084 start = end
01085 end += 4
01086 (length,) = _struct_I.unpack(str[start:end])
01087 start = end
01088 end += length
01089 if python3:
01090 _v48.frame_id = str[start:end].decode('utf-8')
01091 else:
01092 _v48.frame_id = str[start:end]
01093 _x = _v47
01094 start = end
01095 end += 8
01096 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01097 start = end
01098 end += 4
01099 (length,) = _struct_I.unpack(str[start:end])
01100 start = end
01101 end += length
01102 if python3:
01103 _v47.encoding = str[start:end].decode('utf-8')
01104 else:
01105 _v47.encoding = str[start:end]
01106 _x = _v47
01107 start = end
01108 end += 5
01109 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01110 start = end
01111 end += 4
01112 (length,) = _struct_I.unpack(str[start:end])
01113 start = end
01114 end += length
01115 if python3:
01116 _v47.data = str[start:end].decode('utf-8')
01117 else:
01118 _v47.data = str[start:end]
01119 _v50 = _v40.cam_info
01120 _v51 = _v50.header
01121 start = end
01122 end += 4
01123 (_v51.seq,) = _struct_I.unpack(str[start:end])
01124 _v52 = _v51.stamp
01125 _x = _v52
01126 start = end
01127 end += 8
01128 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01129 start = end
01130 end += 4
01131 (length,) = _struct_I.unpack(str[start:end])
01132 start = end
01133 end += length
01134 if python3:
01135 _v51.frame_id = str[start:end].decode('utf-8')
01136 else:
01137 _v51.frame_id = str[start:end]
01138 _x = _v50
01139 start = end
01140 end += 8
01141 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01142 start = end
01143 end += 4
01144 (length,) = _struct_I.unpack(str[start:end])
01145 start = end
01146 end += length
01147 if python3:
01148 _v50.distortion_model = str[start:end].decode('utf-8')
01149 else:
01150 _v50.distortion_model = str[start:end]
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 pattern = '<%sd'%length
01155 start = end
01156 end += struct.calcsize(pattern)
01157 _v50.D = struct.unpack(pattern, str[start:end])
01158 start = end
01159 end += 72
01160 _v50.K = _struct_9d.unpack(str[start:end])
01161 start = end
01162 end += 72
01163 _v50.R = _struct_9d.unpack(str[start:end])
01164 start = end
01165 end += 96
01166 _v50.P = _struct_12d.unpack(str[start:end])
01167 _x = _v50
01168 start = end
01169 end += 8
01170 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
01171 _v53 = _v50.roi
01172 _x = _v53
01173 start = end
01174 end += 17
01175 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
01176 _v53.do_rectify = bool(_v53.do_rectify)
01177 _v54 = _v40.roi_box_pose
01178 _v55 = _v54.header
01179 start = end
01180 end += 4
01181 (_v55.seq,) = _struct_I.unpack(str[start:end])
01182 _v56 = _v55.stamp
01183 _x = _v56
01184 start = end
01185 end += 8
01186 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01187 start = end
01188 end += 4
01189 (length,) = _struct_I.unpack(str[start:end])
01190 start = end
01191 end += length
01192 if python3:
01193 _v55.frame_id = str[start:end].decode('utf-8')
01194 else:
01195 _v55.frame_id = str[start:end]
01196 _v57 = _v54.pose
01197 _v58 = _v57.position
01198 _x = _v58
01199 start = end
01200 end += 24
01201 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01202 _v59 = _v57.orientation
01203 _x = _v59
01204 start = end
01205 end += 32
01206 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01207 _v60 = _v40.roi_box_dims
01208 _x = _v60
01209 start = end
01210 end += 24
01211 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01212 start = end
01213 end += 4
01214 (length,) = _struct_I.unpack(str[start:end])
01215 start = end
01216 end += length
01217 if python3:
01218 val1.collision_name = str[start:end].decode('utf-8')
01219 else:
01220 val1.collision_name = str[start:end]
01221 self.graspable_objects.append(val1)
01222 _x = self
01223 start = end
01224 end += 12
01225 (_x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01226 start = end
01227 end += 4
01228 (length,) = _struct_I.unpack(str[start:end])
01229 start = end
01230 end += length
01231 if python3:
01232 self.image.header.frame_id = str[start:end].decode('utf-8')
01233 else:
01234 self.image.header.frame_id = str[start:end]
01235 _x = self
01236 start = end
01237 end += 8
01238 (_x.image.height, _x.image.width,) = _struct_2I.unpack(str[start:end])
01239 start = end
01240 end += 4
01241 (length,) = _struct_I.unpack(str[start:end])
01242 start = end
01243 end += length
01244 if python3:
01245 self.image.encoding = str[start:end].decode('utf-8')
01246 else:
01247 self.image.encoding = str[start:end]
01248 _x = self
01249 start = end
01250 end += 5
01251 (_x.image.is_bigendian, _x.image.step,) = _struct_BI.unpack(str[start:end])
01252 start = end
01253 end += 4
01254 (length,) = _struct_I.unpack(str[start:end])
01255 start = end
01256 end += length
01257 if python3:
01258 self.image.data = str[start:end].decode('utf-8')
01259 else:
01260 self.image.data = str[start:end]
01261 _x = self
01262 start = end
01263 end += 12
01264 (_x.camera_info.header.seq, _x.camera_info.header.stamp.secs, _x.camera_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01265 start = end
01266 end += 4
01267 (length,) = _struct_I.unpack(str[start:end])
01268 start = end
01269 end += length
01270 if python3:
01271 self.camera_info.header.frame_id = str[start:end].decode('utf-8')
01272 else:
01273 self.camera_info.header.frame_id = str[start:end]
01274 _x = self
01275 start = end
01276 end += 8
01277 (_x.camera_info.height, _x.camera_info.width,) = _struct_2I.unpack(str[start:end])
01278 start = end
01279 end += 4
01280 (length,) = _struct_I.unpack(str[start:end])
01281 start = end
01282 end += length
01283 if python3:
01284 self.camera_info.distortion_model = str[start:end].decode('utf-8')
01285 else:
01286 self.camera_info.distortion_model = str[start:end]
01287 start = end
01288 end += 4
01289 (length,) = _struct_I.unpack(str[start:end])
01290 pattern = '<%sd'%length
01291 start = end
01292 end += struct.calcsize(pattern)
01293 self.camera_info.D = struct.unpack(pattern, str[start:end])
01294 start = end
01295 end += 72
01296 self.camera_info.K = _struct_9d.unpack(str[start:end])
01297 start = end
01298 end += 72
01299 self.camera_info.R = _struct_9d.unpack(str[start:end])
01300 start = end
01301 end += 96
01302 self.camera_info.P = _struct_12d.unpack(str[start:end])
01303 _x = self
01304 start = end
01305 end += 25
01306 (_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])
01307 self.camera_info.roi.do_rectify = bool(self.camera_info.roi.do_rectify)
01308 start = end
01309 end += 4
01310 (length,) = _struct_I.unpack(str[start:end])
01311 self.meshes = []
01312 for i in range(0, length):
01313 val1 = arm_navigation_msgs.msg.Shape()
01314 start = end
01315 end += 1
01316 (val1.type,) = _struct_b.unpack(str[start:end])
01317 start = end
01318 end += 4
01319 (length,) = _struct_I.unpack(str[start:end])
01320 pattern = '<%sd'%length
01321 start = end
01322 end += struct.calcsize(pattern)
01323 val1.dimensions = struct.unpack(pattern, str[start:end])
01324 start = end
01325 end += 4
01326 (length,) = _struct_I.unpack(str[start:end])
01327 pattern = '<%si'%length
01328 start = end
01329 end += struct.calcsize(pattern)
01330 val1.triangles = struct.unpack(pattern, str[start:end])
01331 start = end
01332 end += 4
01333 (length,) = _struct_I.unpack(str[start:end])
01334 val1.vertices = []
01335 for i in range(0, length):
01336 val2 = geometry_msgs.msg.Point()
01337 _x = val2
01338 start = end
01339 end += 24
01340 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01341 val1.vertices.append(val2)
01342 self.meshes.append(val1)
01343 _x = self
01344 start = end
01345 end += 56
01346 (_x.reference_to_camera.position.x, _x.reference_to_camera.position.y, _x.reference_to_camera.position.z, _x.reference_to_camera.orientation.x, _x.reference_to_camera.orientation.y, _x.reference_to_camera.orientation.z, _x.reference_to_camera.orientation.w,) = _struct_7d.unpack(str[start:end])
01347 return self
01348 except struct.error as e:
01349 raise genpy.DeserializationError(e) #most likely buffer underfill
01350
01351
01352 def serialize_numpy(self, buff, numpy):
01353 """
01354 serialize message with numpy array types into buffer
01355 :param buff: buffer, ``StringIO``
01356 :param numpy: numpy python module
01357 """
01358 try:
01359 length = len(self.graspable_objects)
01360 buff.write(_struct_I.pack(length))
01361 for val1 in self.graspable_objects:
01362 _x = val1.reference_frame_id
01363 length = len(_x)
01364 if python3 or type(_x) == unicode:
01365 _x = _x.encode('utf-8')
01366 length = len(_x)
01367 buff.write(struct.pack('<I%ss'%length, length, _x))
01368 length = len(val1.potential_models)
01369 buff.write(_struct_I.pack(length))
01370 for val2 in val1.potential_models:
01371 buff.write(_struct_i.pack(val2.model_id))
01372 _v61 = val2.pose
01373 _v62 = _v61.header
01374 buff.write(_struct_I.pack(_v62.seq))
01375 _v63 = _v62.stamp
01376 _x = _v63
01377 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01378 _x = _v62.frame_id
01379 length = len(_x)
01380 if python3 or type(_x) == unicode:
01381 _x = _x.encode('utf-8')
01382 length = len(_x)
01383 buff.write(struct.pack('<I%ss'%length, length, _x))
01384 _v64 = _v61.pose
01385 _v65 = _v64.position
01386 _x = _v65
01387 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01388 _v66 = _v64.orientation
01389 _x = _v66
01390 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01391 buff.write(_struct_f.pack(val2.confidence))
01392 _x = val2.detector_name
01393 length = len(_x)
01394 if python3 or type(_x) == unicode:
01395 _x = _x.encode('utf-8')
01396 length = len(_x)
01397 buff.write(struct.pack('<I%ss'%length, length, _x))
01398 _v67 = val1.cluster
01399 _v68 = _v67.header
01400 buff.write(_struct_I.pack(_v68.seq))
01401 _v69 = _v68.stamp
01402 _x = _v69
01403 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01404 _x = _v68.frame_id
01405 length = len(_x)
01406 if python3 or type(_x) == unicode:
01407 _x = _x.encode('utf-8')
01408 length = len(_x)
01409 buff.write(struct.pack('<I%ss'%length, length, _x))
01410 length = len(_v67.points)
01411 buff.write(_struct_I.pack(length))
01412 for val3 in _v67.points:
01413 _x = val3
01414 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01415 length = len(_v67.channels)
01416 buff.write(_struct_I.pack(length))
01417 for val3 in _v67.channels:
01418 _x = val3.name
01419 length = len(_x)
01420 if python3 or type(_x) == unicode:
01421 _x = _x.encode('utf-8')
01422 length = len(_x)
01423 buff.write(struct.pack('<I%ss'%length, length, _x))
01424 length = len(val3.values)
01425 buff.write(_struct_I.pack(length))
01426 pattern = '<%sf'%length
01427 buff.write(val3.values.tostring())
01428 _v70 = val1.region
01429 _v71 = _v70.cloud
01430 _v72 = _v71.header
01431 buff.write(_struct_I.pack(_v72.seq))
01432 _v73 = _v72.stamp
01433 _x = _v73
01434 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01435 _x = _v72.frame_id
01436 length = len(_x)
01437 if python3 or type(_x) == unicode:
01438 _x = _x.encode('utf-8')
01439 length = len(_x)
01440 buff.write(struct.pack('<I%ss'%length, length, _x))
01441 _x = _v71
01442 buff.write(_struct_2I.pack(_x.height, _x.width))
01443 length = len(_v71.fields)
01444 buff.write(_struct_I.pack(length))
01445 for val4 in _v71.fields:
01446 _x = val4.name
01447 length = len(_x)
01448 if python3 or type(_x) == unicode:
01449 _x = _x.encode('utf-8')
01450 length = len(_x)
01451 buff.write(struct.pack('<I%ss'%length, length, _x))
01452 _x = val4
01453 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01454 _x = _v71
01455 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01456 _x = _v71.data
01457 length = len(_x)
01458 # - if encoded as a list instead, serialize as bytes instead of string
01459 if type(_x) in [list, tuple]:
01460 buff.write(struct.pack('<I%sB'%length, length, *_x))
01461 else:
01462 buff.write(struct.pack('<I%ss'%length, length, _x))
01463 buff.write(_struct_B.pack(_v71.is_dense))
01464 length = len(_v70.mask)
01465 buff.write(_struct_I.pack(length))
01466 pattern = '<%si'%length
01467 buff.write(_v70.mask.tostring())
01468 _v74 = _v70.image
01469 _v75 = _v74.header
01470 buff.write(_struct_I.pack(_v75.seq))
01471 _v76 = _v75.stamp
01472 _x = _v76
01473 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01474 _x = _v75.frame_id
01475 length = len(_x)
01476 if python3 or type(_x) == unicode:
01477 _x = _x.encode('utf-8')
01478 length = len(_x)
01479 buff.write(struct.pack('<I%ss'%length, length, _x))
01480 _x = _v74
01481 buff.write(_struct_2I.pack(_x.height, _x.width))
01482 _x = _v74.encoding
01483 length = len(_x)
01484 if python3 or type(_x) == unicode:
01485 _x = _x.encode('utf-8')
01486 length = len(_x)
01487 buff.write(struct.pack('<I%ss'%length, length, _x))
01488 _x = _v74
01489 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01490 _x = _v74.data
01491 length = len(_x)
01492 # - if encoded as a list instead, serialize as bytes instead of string
01493 if type(_x) in [list, tuple]:
01494 buff.write(struct.pack('<I%sB'%length, length, *_x))
01495 else:
01496 buff.write(struct.pack('<I%ss'%length, length, _x))
01497 _v77 = _v70.disparity_image
01498 _v78 = _v77.header
01499 buff.write(_struct_I.pack(_v78.seq))
01500 _v79 = _v78.stamp
01501 _x = _v79
01502 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01503 _x = _v78.frame_id
01504 length = len(_x)
01505 if python3 or type(_x) == unicode:
01506 _x = _x.encode('utf-8')
01507 length = len(_x)
01508 buff.write(struct.pack('<I%ss'%length, length, _x))
01509 _x = _v77
01510 buff.write(_struct_2I.pack(_x.height, _x.width))
01511 _x = _v77.encoding
01512 length = len(_x)
01513 if python3 or type(_x) == unicode:
01514 _x = _x.encode('utf-8')
01515 length = len(_x)
01516 buff.write(struct.pack('<I%ss'%length, length, _x))
01517 _x = _v77
01518 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01519 _x = _v77.data
01520 length = len(_x)
01521 # - if encoded as a list instead, serialize as bytes instead of string
01522 if type(_x) in [list, tuple]:
01523 buff.write(struct.pack('<I%sB'%length, length, *_x))
01524 else:
01525 buff.write(struct.pack('<I%ss'%length, length, _x))
01526 _v80 = _v70.cam_info
01527 _v81 = _v80.header
01528 buff.write(_struct_I.pack(_v81.seq))
01529 _v82 = _v81.stamp
01530 _x = _v82
01531 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01532 _x = _v81.frame_id
01533 length = len(_x)
01534 if python3 or type(_x) == unicode:
01535 _x = _x.encode('utf-8')
01536 length = len(_x)
01537 buff.write(struct.pack('<I%ss'%length, length, _x))
01538 _x = _v80
01539 buff.write(_struct_2I.pack(_x.height, _x.width))
01540 _x = _v80.distortion_model
01541 length = len(_x)
01542 if python3 or type(_x) == unicode:
01543 _x = _x.encode('utf-8')
01544 length = len(_x)
01545 buff.write(struct.pack('<I%ss'%length, length, _x))
01546 length = len(_v80.D)
01547 buff.write(_struct_I.pack(length))
01548 pattern = '<%sd'%length
01549 buff.write(_v80.D.tostring())
01550 buff.write(_v80.K.tostring())
01551 buff.write(_v80.R.tostring())
01552 buff.write(_v80.P.tostring())
01553 _x = _v80
01554 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01555 _v83 = _v80.roi
01556 _x = _v83
01557 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01558 _v84 = _v70.roi_box_pose
01559 _v85 = _v84.header
01560 buff.write(_struct_I.pack(_v85.seq))
01561 _v86 = _v85.stamp
01562 _x = _v86
01563 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01564 _x = _v85.frame_id
01565 length = len(_x)
01566 if python3 or type(_x) == unicode:
01567 _x = _x.encode('utf-8')
01568 length = len(_x)
01569 buff.write(struct.pack('<I%ss'%length, length, _x))
01570 _v87 = _v84.pose
01571 _v88 = _v87.position
01572 _x = _v88
01573 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01574 _v89 = _v87.orientation
01575 _x = _v89
01576 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01577 _v90 = _v70.roi_box_dims
01578 _x = _v90
01579 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01580 _x = val1.collision_name
01581 length = len(_x)
01582 if python3 or type(_x) == unicode:
01583 _x = _x.encode('utf-8')
01584 length = len(_x)
01585 buff.write(struct.pack('<I%ss'%length, length, _x))
01586 _x = self
01587 buff.write(_struct_3I.pack(_x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs))
01588 _x = self.image.header.frame_id
01589 length = len(_x)
01590 if python3 or type(_x) == unicode:
01591 _x = _x.encode('utf-8')
01592 length = len(_x)
01593 buff.write(struct.pack('<I%ss'%length, length, _x))
01594 _x = self
01595 buff.write(_struct_2I.pack(_x.image.height, _x.image.width))
01596 _x = self.image.encoding
01597 length = len(_x)
01598 if python3 or type(_x) == unicode:
01599 _x = _x.encode('utf-8')
01600 length = len(_x)
01601 buff.write(struct.pack('<I%ss'%length, length, _x))
01602 _x = self
01603 buff.write(_struct_BI.pack(_x.image.is_bigendian, _x.image.step))
01604 _x = self.image.data
01605 length = len(_x)
01606 # - if encoded as a list instead, serialize as bytes instead of string
01607 if type(_x) in [list, tuple]:
01608 buff.write(struct.pack('<I%sB'%length, length, *_x))
01609 else:
01610 buff.write(struct.pack('<I%ss'%length, length, _x))
01611 _x = self
01612 buff.write(_struct_3I.pack(_x.camera_info.header.seq, _x.camera_info.header.stamp.secs, _x.camera_info.header.stamp.nsecs))
01613 _x = self.camera_info.header.frame_id
01614 length = len(_x)
01615 if python3 or type(_x) == unicode:
01616 _x = _x.encode('utf-8')
01617 length = len(_x)
01618 buff.write(struct.pack('<I%ss'%length, length, _x))
01619 _x = self
01620 buff.write(_struct_2I.pack(_x.camera_info.height, _x.camera_info.width))
01621 _x = self.camera_info.distortion_model
01622 length = len(_x)
01623 if python3 or type(_x) == unicode:
01624 _x = _x.encode('utf-8')
01625 length = len(_x)
01626 buff.write(struct.pack('<I%ss'%length, length, _x))
01627 length = len(self.camera_info.D)
01628 buff.write(_struct_I.pack(length))
01629 pattern = '<%sd'%length
01630 buff.write(self.camera_info.D.tostring())
01631 buff.write(self.camera_info.K.tostring())
01632 buff.write(self.camera_info.R.tostring())
01633 buff.write(self.camera_info.P.tostring())
01634 _x = self
01635 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))
01636 length = len(self.meshes)
01637 buff.write(_struct_I.pack(length))
01638 for val1 in self.meshes:
01639 buff.write(_struct_b.pack(val1.type))
01640 length = len(val1.dimensions)
01641 buff.write(_struct_I.pack(length))
01642 pattern = '<%sd'%length
01643 buff.write(val1.dimensions.tostring())
01644 length = len(val1.triangles)
01645 buff.write(_struct_I.pack(length))
01646 pattern = '<%si'%length
01647 buff.write(val1.triangles.tostring())
01648 length = len(val1.vertices)
01649 buff.write(_struct_I.pack(length))
01650 for val2 in val1.vertices:
01651 _x = val2
01652 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01653 _x = self
01654 buff.write(_struct_7d.pack(_x.reference_to_camera.position.x, _x.reference_to_camera.position.y, _x.reference_to_camera.position.z, _x.reference_to_camera.orientation.x, _x.reference_to_camera.orientation.y, _x.reference_to_camera.orientation.z, _x.reference_to_camera.orientation.w))
01655 except struct.error as se: self._check_types(se)
01656 except TypeError as te: self._check_types(te)
01657
01658 def deserialize_numpy(self, str, numpy):
01659 """
01660 unpack serialized message in str into this message instance using numpy for array types
01661 :param str: byte array of serialized message, ``str``
01662 :param numpy: numpy python module
01663 """
01664 try:
01665 if self.graspable_objects is None:
01666 self.graspable_objects = None
01667 if self.image is None:
01668 self.image = sensor_msgs.msg.Image()
01669 if self.camera_info is None:
01670 self.camera_info = sensor_msgs.msg.CameraInfo()
01671 if self.meshes is None:
01672 self.meshes = None
01673 if self.reference_to_camera is None:
01674 self.reference_to_camera = geometry_msgs.msg.Pose()
01675 end = 0
01676 start = end
01677 end += 4
01678 (length,) = _struct_I.unpack(str[start:end])
01679 self.graspable_objects = []
01680 for i in range(0, length):
01681 val1 = object_manipulation_msgs.msg.GraspableObject()
01682 start = end
01683 end += 4
01684 (length,) = _struct_I.unpack(str[start:end])
01685 start = end
01686 end += length
01687 if python3:
01688 val1.reference_frame_id = str[start:end].decode('utf-8')
01689 else:
01690 val1.reference_frame_id = str[start:end]
01691 start = end
01692 end += 4
01693 (length,) = _struct_I.unpack(str[start:end])
01694 val1.potential_models = []
01695 for i in range(0, length):
01696 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
01697 start = end
01698 end += 4
01699 (val2.model_id,) = _struct_i.unpack(str[start:end])
01700 _v91 = val2.pose
01701 _v92 = _v91.header
01702 start = end
01703 end += 4
01704 (_v92.seq,) = _struct_I.unpack(str[start:end])
01705 _v93 = _v92.stamp
01706 _x = _v93
01707 start = end
01708 end += 8
01709 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01710 start = end
01711 end += 4
01712 (length,) = _struct_I.unpack(str[start:end])
01713 start = end
01714 end += length
01715 if python3:
01716 _v92.frame_id = str[start:end].decode('utf-8')
01717 else:
01718 _v92.frame_id = str[start:end]
01719 _v94 = _v91.pose
01720 _v95 = _v94.position
01721 _x = _v95
01722 start = end
01723 end += 24
01724 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01725 _v96 = _v94.orientation
01726 _x = _v96
01727 start = end
01728 end += 32
01729 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01730 start = end
01731 end += 4
01732 (val2.confidence,) = _struct_f.unpack(str[start:end])
01733 start = end
01734 end += 4
01735 (length,) = _struct_I.unpack(str[start:end])
01736 start = end
01737 end += length
01738 if python3:
01739 val2.detector_name = str[start:end].decode('utf-8')
01740 else:
01741 val2.detector_name = str[start:end]
01742 val1.potential_models.append(val2)
01743 _v97 = val1.cluster
01744 _v98 = _v97.header
01745 start = end
01746 end += 4
01747 (_v98.seq,) = _struct_I.unpack(str[start:end])
01748 _v99 = _v98.stamp
01749 _x = _v99
01750 start = end
01751 end += 8
01752 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01753 start = end
01754 end += 4
01755 (length,) = _struct_I.unpack(str[start:end])
01756 start = end
01757 end += length
01758 if python3:
01759 _v98.frame_id = str[start:end].decode('utf-8')
01760 else:
01761 _v98.frame_id = str[start:end]
01762 start = end
01763 end += 4
01764 (length,) = _struct_I.unpack(str[start:end])
01765 _v97.points = []
01766 for i in range(0, length):
01767 val3 = geometry_msgs.msg.Point32()
01768 _x = val3
01769 start = end
01770 end += 12
01771 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01772 _v97.points.append(val3)
01773 start = end
01774 end += 4
01775 (length,) = _struct_I.unpack(str[start:end])
01776 _v97.channels = []
01777 for i in range(0, length):
01778 val3 = sensor_msgs.msg.ChannelFloat32()
01779 start = end
01780 end += 4
01781 (length,) = _struct_I.unpack(str[start:end])
01782 start = end
01783 end += length
01784 if python3:
01785 val3.name = str[start:end].decode('utf-8')
01786 else:
01787 val3.name = str[start:end]
01788 start = end
01789 end += 4
01790 (length,) = _struct_I.unpack(str[start:end])
01791 pattern = '<%sf'%length
01792 start = end
01793 end += struct.calcsize(pattern)
01794 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01795 _v97.channels.append(val3)
01796 _v100 = val1.region
01797 _v101 = _v100.cloud
01798 _v102 = _v101.header
01799 start = end
01800 end += 4
01801 (_v102.seq,) = _struct_I.unpack(str[start:end])
01802 _v103 = _v102.stamp
01803 _x = _v103
01804 start = end
01805 end += 8
01806 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01807 start = end
01808 end += 4
01809 (length,) = _struct_I.unpack(str[start:end])
01810 start = end
01811 end += length
01812 if python3:
01813 _v102.frame_id = str[start:end].decode('utf-8')
01814 else:
01815 _v102.frame_id = str[start:end]
01816 _x = _v101
01817 start = end
01818 end += 8
01819 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01820 start = end
01821 end += 4
01822 (length,) = _struct_I.unpack(str[start:end])
01823 _v101.fields = []
01824 for i in range(0, length):
01825 val4 = sensor_msgs.msg.PointField()
01826 start = end
01827 end += 4
01828 (length,) = _struct_I.unpack(str[start:end])
01829 start = end
01830 end += length
01831 if python3:
01832 val4.name = str[start:end].decode('utf-8')
01833 else:
01834 val4.name = str[start:end]
01835 _x = val4
01836 start = end
01837 end += 9
01838 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01839 _v101.fields.append(val4)
01840 _x = _v101
01841 start = end
01842 end += 9
01843 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
01844 _v101.is_bigendian = bool(_v101.is_bigendian)
01845 start = end
01846 end += 4
01847 (length,) = _struct_I.unpack(str[start:end])
01848 start = end
01849 end += length
01850 if python3:
01851 _v101.data = str[start:end].decode('utf-8')
01852 else:
01853 _v101.data = str[start:end]
01854 start = end
01855 end += 1
01856 (_v101.is_dense,) = _struct_B.unpack(str[start:end])
01857 _v101.is_dense = bool(_v101.is_dense)
01858 start = end
01859 end += 4
01860 (length,) = _struct_I.unpack(str[start:end])
01861 pattern = '<%si'%length
01862 start = end
01863 end += struct.calcsize(pattern)
01864 _v100.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
01865 _v104 = _v100.image
01866 _v105 = _v104.header
01867 start = end
01868 end += 4
01869 (_v105.seq,) = _struct_I.unpack(str[start:end])
01870 _v106 = _v105.stamp
01871 _x = _v106
01872 start = end
01873 end += 8
01874 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01875 start = end
01876 end += 4
01877 (length,) = _struct_I.unpack(str[start:end])
01878 start = end
01879 end += length
01880 if python3:
01881 _v105.frame_id = str[start:end].decode('utf-8')
01882 else:
01883 _v105.frame_id = str[start:end]
01884 _x = _v104
01885 start = end
01886 end += 8
01887 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01888 start = end
01889 end += 4
01890 (length,) = _struct_I.unpack(str[start:end])
01891 start = end
01892 end += length
01893 if python3:
01894 _v104.encoding = str[start:end].decode('utf-8')
01895 else:
01896 _v104.encoding = str[start:end]
01897 _x = _v104
01898 start = end
01899 end += 5
01900 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01901 start = end
01902 end += 4
01903 (length,) = _struct_I.unpack(str[start:end])
01904 start = end
01905 end += length
01906 if python3:
01907 _v104.data = str[start:end].decode('utf-8')
01908 else:
01909 _v104.data = str[start:end]
01910 _v107 = _v100.disparity_image
01911 _v108 = _v107.header
01912 start = end
01913 end += 4
01914 (_v108.seq,) = _struct_I.unpack(str[start:end])
01915 _v109 = _v108.stamp
01916 _x = _v109
01917 start = end
01918 end += 8
01919 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01920 start = end
01921 end += 4
01922 (length,) = _struct_I.unpack(str[start:end])
01923 start = end
01924 end += length
01925 if python3:
01926 _v108.frame_id = str[start:end].decode('utf-8')
01927 else:
01928 _v108.frame_id = str[start:end]
01929 _x = _v107
01930 start = end
01931 end += 8
01932 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01933 start = end
01934 end += 4
01935 (length,) = _struct_I.unpack(str[start:end])
01936 start = end
01937 end += length
01938 if python3:
01939 _v107.encoding = str[start:end].decode('utf-8')
01940 else:
01941 _v107.encoding = str[start:end]
01942 _x = _v107
01943 start = end
01944 end += 5
01945 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01946 start = end
01947 end += 4
01948 (length,) = _struct_I.unpack(str[start:end])
01949 start = end
01950 end += length
01951 if python3:
01952 _v107.data = str[start:end].decode('utf-8')
01953 else:
01954 _v107.data = str[start:end]
01955 _v110 = _v100.cam_info
01956 _v111 = _v110.header
01957 start = end
01958 end += 4
01959 (_v111.seq,) = _struct_I.unpack(str[start:end])
01960 _v112 = _v111.stamp
01961 _x = _v112
01962 start = end
01963 end += 8
01964 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01965 start = end
01966 end += 4
01967 (length,) = _struct_I.unpack(str[start:end])
01968 start = end
01969 end += length
01970 if python3:
01971 _v111.frame_id = str[start:end].decode('utf-8')
01972 else:
01973 _v111.frame_id = str[start:end]
01974 _x = _v110
01975 start = end
01976 end += 8
01977 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01978 start = end
01979 end += 4
01980 (length,) = _struct_I.unpack(str[start:end])
01981 start = end
01982 end += length
01983 if python3:
01984 _v110.distortion_model = str[start:end].decode('utf-8')
01985 else:
01986 _v110.distortion_model = str[start:end]
01987 start = end
01988 end += 4
01989 (length,) = _struct_I.unpack(str[start:end])
01990 pattern = '<%sd'%length
01991 start = end
01992 end += struct.calcsize(pattern)
01993 _v110.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01994 start = end
01995 end += 72
01996 _v110.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
01997 start = end
01998 end += 72
01999 _v110.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02000 start = end
02001 end += 96
02002 _v110.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
02003 _x = _v110
02004 start = end
02005 end += 8
02006 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
02007 _v113 = _v110.roi
02008 _x = _v113
02009 start = end
02010 end += 17
02011 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
02012 _v113.do_rectify = bool(_v113.do_rectify)
02013 _v114 = _v100.roi_box_pose
02014 _v115 = _v114.header
02015 start = end
02016 end += 4
02017 (_v115.seq,) = _struct_I.unpack(str[start:end])
02018 _v116 = _v115.stamp
02019 _x = _v116
02020 start = end
02021 end += 8
02022 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02023 start = end
02024 end += 4
02025 (length,) = _struct_I.unpack(str[start:end])
02026 start = end
02027 end += length
02028 if python3:
02029 _v115.frame_id = str[start:end].decode('utf-8')
02030 else:
02031 _v115.frame_id = str[start:end]
02032 _v117 = _v114.pose
02033 _v118 = _v117.position
02034 _x = _v118
02035 start = end
02036 end += 24
02037 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02038 _v119 = _v117.orientation
02039 _x = _v119
02040 start = end
02041 end += 32
02042 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02043 _v120 = _v100.roi_box_dims
02044 _x = _v120
02045 start = end
02046 end += 24
02047 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02048 start = end
02049 end += 4
02050 (length,) = _struct_I.unpack(str[start:end])
02051 start = end
02052 end += length
02053 if python3:
02054 val1.collision_name = str[start:end].decode('utf-8')
02055 else:
02056 val1.collision_name = str[start:end]
02057 self.graspable_objects.append(val1)
02058 _x = self
02059 start = end
02060 end += 12
02061 (_x.image.header.seq, _x.image.header.stamp.secs, _x.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02062 start = end
02063 end += 4
02064 (length,) = _struct_I.unpack(str[start:end])
02065 start = end
02066 end += length
02067 if python3:
02068 self.image.header.frame_id = str[start:end].decode('utf-8')
02069 else:
02070 self.image.header.frame_id = str[start:end]
02071 _x = self
02072 start = end
02073 end += 8
02074 (_x.image.height, _x.image.width,) = _struct_2I.unpack(str[start:end])
02075 start = end
02076 end += 4
02077 (length,) = _struct_I.unpack(str[start:end])
02078 start = end
02079 end += length
02080 if python3:
02081 self.image.encoding = str[start:end].decode('utf-8')
02082 else:
02083 self.image.encoding = str[start:end]
02084 _x = self
02085 start = end
02086 end += 5
02087 (_x.image.is_bigendian, _x.image.step,) = _struct_BI.unpack(str[start:end])
02088 start = end
02089 end += 4
02090 (length,) = _struct_I.unpack(str[start:end])
02091 start = end
02092 end += length
02093 if python3:
02094 self.image.data = str[start:end].decode('utf-8')
02095 else:
02096 self.image.data = str[start:end]
02097 _x = self
02098 start = end
02099 end += 12
02100 (_x.camera_info.header.seq, _x.camera_info.header.stamp.secs, _x.camera_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02101 start = end
02102 end += 4
02103 (length,) = _struct_I.unpack(str[start:end])
02104 start = end
02105 end += length
02106 if python3:
02107 self.camera_info.header.frame_id = str[start:end].decode('utf-8')
02108 else:
02109 self.camera_info.header.frame_id = str[start:end]
02110 _x = self
02111 start = end
02112 end += 8
02113 (_x.camera_info.height, _x.camera_info.width,) = _struct_2I.unpack(str[start:end])
02114 start = end
02115 end += 4
02116 (length,) = _struct_I.unpack(str[start:end])
02117 start = end
02118 end += length
02119 if python3:
02120 self.camera_info.distortion_model = str[start:end].decode('utf-8')
02121 else:
02122 self.camera_info.distortion_model = str[start:end]
02123 start = end
02124 end += 4
02125 (length,) = _struct_I.unpack(str[start:end])
02126 pattern = '<%sd'%length
02127 start = end
02128 end += struct.calcsize(pattern)
02129 self.camera_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02130 start = end
02131 end += 72
02132 self.camera_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02133 start = end
02134 end += 72
02135 self.camera_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02136 start = end
02137 end += 96
02138 self.camera_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
02139 _x = self
02140 start = end
02141 end += 25
02142 (_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])
02143 self.camera_info.roi.do_rectify = bool(self.camera_info.roi.do_rectify)
02144 start = end
02145 end += 4
02146 (length,) = _struct_I.unpack(str[start:end])
02147 self.meshes = []
02148 for i in range(0, length):
02149 val1 = arm_navigation_msgs.msg.Shape()
02150 start = end
02151 end += 1
02152 (val1.type,) = _struct_b.unpack(str[start:end])
02153 start = end
02154 end += 4
02155 (length,) = _struct_I.unpack(str[start:end])
02156 pattern = '<%sd'%length
02157 start = end
02158 end += struct.calcsize(pattern)
02159 val1.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02160 start = end
02161 end += 4
02162 (length,) = _struct_I.unpack(str[start:end])
02163 pattern = '<%si'%length
02164 start = end
02165 end += struct.calcsize(pattern)
02166 val1.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02167 start = end
02168 end += 4
02169 (length,) = _struct_I.unpack(str[start:end])
02170 val1.vertices = []
02171 for i in range(0, length):
02172 val2 = geometry_msgs.msg.Point()
02173 _x = val2
02174 start = end
02175 end += 24
02176 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02177 val1.vertices.append(val2)
02178 self.meshes.append(val1)
02179 _x = self
02180 start = end
02181 end += 56
02182 (_x.reference_to_camera.position.x, _x.reference_to_camera.position.y, _x.reference_to_camera.position.z, _x.reference_to_camera.orientation.x, _x.reference_to_camera.orientation.y, _x.reference_to_camera.orientation.z, _x.reference_to_camera.orientation.w,) = _struct_7d.unpack(str[start:end])
02183 return self
02184 except struct.error as e:
02185 raise genpy.DeserializationError(e) #most likely buffer underfill
02186
02187 _struct_I = genpy.struct_I
02188 _struct_6IB = struct.Struct("<6IB")
02189 _struct_IBI = struct.Struct("<IBI")
02190 _struct_B = struct.Struct("<B")
02191 _struct_12d = struct.Struct("<12d")
02192 _struct_f = struct.Struct("<f")
02193 _struct_i = struct.Struct("<i")
02194 _struct_BI = struct.Struct("<BI")
02195 _struct_3f = struct.Struct("<3f")
02196 _struct_3I = struct.Struct("<3I")
02197 _struct_b = struct.Struct("<b")
02198 _struct_9d = struct.Struct("<9d")
02199 _struct_B2I = struct.Struct("<B2I")
02200 _struct_4d = struct.Struct("<4d")
02201 _struct_7d = struct.Struct("<7d")
02202 _struct_2I = struct.Struct("<2I")
02203 _struct_4IB = struct.Struct("<4IB")
02204 _struct_3d = struct.Struct("<3d")