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