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