00001 """autogenerated by genpy from pr2_grasp_adjust/GraspAdjustResult.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 GraspAdjustResult(genpy.Message):
00014 _md5sum = "4b2c78da8eafa798492827fe57d55f30"
00015 _type = "pr2_grasp_adjust/GraspAdjustResult"
00016 _has_header = False
00017 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00018 #result definition
00019
00020 object_manipulation_msgs/Grasp[] grasps
00021 object_manipulation_msgs/GraspPlanningErrorCode result
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 include/sensor_msgs/image_encodings.h
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 MSG: object_manipulation_msgs/GraspPlanningErrorCode
00497 # Error codes for grasp and place planning
00498
00499 # plan completed as expected
00500 int32 SUCCESS = 0
00501
00502 # tf error encountered while transforming
00503 int32 TF_ERROR = 1
00504
00505 # some other error
00506 int32 OTHER_ERROR = 2
00507
00508 # the actual value of this error code
00509 int32 value
00510 """
00511 __slots__ = ['grasps','result']
00512 _slot_types = ['object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspPlanningErrorCode']
00513
00514 def __init__(self, *args, **kwds):
00515 """
00516 Constructor. Any message fields that are implicitly/explicitly
00517 set to None will be assigned a default value. The recommend
00518 use is keyword arguments as this is more robust to future message
00519 changes. You cannot mix in-order arguments and keyword arguments.
00520
00521 The available fields are:
00522 grasps,result
00523
00524 :param args: complete set of field values, in .msg order
00525 :param kwds: use keyword arguments corresponding to message field names
00526 to set specific fields.
00527 """
00528 if args or kwds:
00529 super(GraspAdjustResult, self).__init__(*args, **kwds)
00530 #message fields cannot be None, assign default values for those that are
00531 if self.grasps is None:
00532 self.grasps = []
00533 if self.result is None:
00534 self.result = object_manipulation_msgs.msg.GraspPlanningErrorCode()
00535 else:
00536 self.grasps = []
00537 self.result = object_manipulation_msgs.msg.GraspPlanningErrorCode()
00538
00539 def _get_types(self):
00540 """
00541 internal API method
00542 """
00543 return self._slot_types
00544
00545 def serialize(self, buff):
00546 """
00547 serialize message into buffer
00548 :param buff: buffer, ``StringIO``
00549 """
00550 try:
00551 length = len(self.grasps)
00552 buff.write(_struct_I.pack(length))
00553 for val1 in self.grasps:
00554 _v1 = val1.pre_grasp_posture
00555 _v2 = _v1.header
00556 buff.write(_struct_I.pack(_v2.seq))
00557 _v3 = _v2.stamp
00558 _x = _v3
00559 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00560 _x = _v2.frame_id
00561 length = len(_x)
00562 if python3 or type(_x) == unicode:
00563 _x = _x.encode('utf-8')
00564 length = len(_x)
00565 buff.write(struct.pack('<I%ss'%length, length, _x))
00566 length = len(_v1.name)
00567 buff.write(_struct_I.pack(length))
00568 for val3 in _v1.name:
00569 length = len(val3)
00570 if python3 or type(val3) == unicode:
00571 val3 = val3.encode('utf-8')
00572 length = len(val3)
00573 buff.write(struct.pack('<I%ss'%length, length, val3))
00574 length = len(_v1.position)
00575 buff.write(_struct_I.pack(length))
00576 pattern = '<%sd'%length
00577 buff.write(struct.pack(pattern, *_v1.position))
00578 length = len(_v1.velocity)
00579 buff.write(_struct_I.pack(length))
00580 pattern = '<%sd'%length
00581 buff.write(struct.pack(pattern, *_v1.velocity))
00582 length = len(_v1.effort)
00583 buff.write(_struct_I.pack(length))
00584 pattern = '<%sd'%length
00585 buff.write(struct.pack(pattern, *_v1.effort))
00586 _v4 = val1.grasp_posture
00587 _v5 = _v4.header
00588 buff.write(_struct_I.pack(_v5.seq))
00589 _v6 = _v5.stamp
00590 _x = _v6
00591 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00592 _x = _v5.frame_id
00593 length = len(_x)
00594 if python3 or type(_x) == unicode:
00595 _x = _x.encode('utf-8')
00596 length = len(_x)
00597 buff.write(struct.pack('<I%ss'%length, length, _x))
00598 length = len(_v4.name)
00599 buff.write(_struct_I.pack(length))
00600 for val3 in _v4.name:
00601 length = len(val3)
00602 if python3 or type(val3) == unicode:
00603 val3 = val3.encode('utf-8')
00604 length = len(val3)
00605 buff.write(struct.pack('<I%ss'%length, length, val3))
00606 length = len(_v4.position)
00607 buff.write(_struct_I.pack(length))
00608 pattern = '<%sd'%length
00609 buff.write(struct.pack(pattern, *_v4.position))
00610 length = len(_v4.velocity)
00611 buff.write(_struct_I.pack(length))
00612 pattern = '<%sd'%length
00613 buff.write(struct.pack(pattern, *_v4.velocity))
00614 length = len(_v4.effort)
00615 buff.write(_struct_I.pack(length))
00616 pattern = '<%sd'%length
00617 buff.write(struct.pack(pattern, *_v4.effort))
00618 _v7 = val1.grasp_pose
00619 _v8 = _v7.position
00620 _x = _v8
00621 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00622 _v9 = _v7.orientation
00623 _x = _v9
00624 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00625 _x = val1
00626 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
00627 length = len(val1.moved_obstacles)
00628 buff.write(_struct_I.pack(length))
00629 for val2 in val1.moved_obstacles:
00630 _x = val2.reference_frame_id
00631 length = len(_x)
00632 if python3 or type(_x) == unicode:
00633 _x = _x.encode('utf-8')
00634 length = len(_x)
00635 buff.write(struct.pack('<I%ss'%length, length, _x))
00636 length = len(val2.potential_models)
00637 buff.write(_struct_I.pack(length))
00638 for val3 in val2.potential_models:
00639 buff.write(_struct_i.pack(val3.model_id))
00640 _v10 = val3.pose
00641 _v11 = _v10.header
00642 buff.write(_struct_I.pack(_v11.seq))
00643 _v12 = _v11.stamp
00644 _x = _v12
00645 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00646 _x = _v11.frame_id
00647 length = len(_x)
00648 if python3 or type(_x) == unicode:
00649 _x = _x.encode('utf-8')
00650 length = len(_x)
00651 buff.write(struct.pack('<I%ss'%length, length, _x))
00652 _v13 = _v10.pose
00653 _v14 = _v13.position
00654 _x = _v14
00655 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00656 _v15 = _v13.orientation
00657 _x = _v15
00658 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00659 buff.write(_struct_f.pack(val3.confidence))
00660 _x = val3.detector_name
00661 length = len(_x)
00662 if python3 or type(_x) == unicode:
00663 _x = _x.encode('utf-8')
00664 length = len(_x)
00665 buff.write(struct.pack('<I%ss'%length, length, _x))
00666 _v16 = val2.cluster
00667 _v17 = _v16.header
00668 buff.write(_struct_I.pack(_v17.seq))
00669 _v18 = _v17.stamp
00670 _x = _v18
00671 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00672 _x = _v17.frame_id
00673 length = len(_x)
00674 if python3 or type(_x) == unicode:
00675 _x = _x.encode('utf-8')
00676 length = len(_x)
00677 buff.write(struct.pack('<I%ss'%length, length, _x))
00678 length = len(_v16.points)
00679 buff.write(_struct_I.pack(length))
00680 for val4 in _v16.points:
00681 _x = val4
00682 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00683 length = len(_v16.channels)
00684 buff.write(_struct_I.pack(length))
00685 for val4 in _v16.channels:
00686 _x = val4.name
00687 length = len(_x)
00688 if python3 or type(_x) == unicode:
00689 _x = _x.encode('utf-8')
00690 length = len(_x)
00691 buff.write(struct.pack('<I%ss'%length, length, _x))
00692 length = len(val4.values)
00693 buff.write(_struct_I.pack(length))
00694 pattern = '<%sf'%length
00695 buff.write(struct.pack(pattern, *val4.values))
00696 _v19 = val2.region
00697 _v20 = _v19.cloud
00698 _v21 = _v20.header
00699 buff.write(_struct_I.pack(_v21.seq))
00700 _v22 = _v21.stamp
00701 _x = _v22
00702 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00703 _x = _v21.frame_id
00704 length = len(_x)
00705 if python3 or type(_x) == unicode:
00706 _x = _x.encode('utf-8')
00707 length = len(_x)
00708 buff.write(struct.pack('<I%ss'%length, length, _x))
00709 _x = _v20
00710 buff.write(_struct_2I.pack(_x.height, _x.width))
00711 length = len(_v20.fields)
00712 buff.write(_struct_I.pack(length))
00713 for val5 in _v20.fields:
00714 _x = val5.name
00715 length = len(_x)
00716 if python3 or type(_x) == unicode:
00717 _x = _x.encode('utf-8')
00718 length = len(_x)
00719 buff.write(struct.pack('<I%ss'%length, length, _x))
00720 _x = val5
00721 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00722 _x = _v20
00723 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
00724 _x = _v20.data
00725 length = len(_x)
00726 # - if encoded as a list instead, serialize as bytes instead of string
00727 if type(_x) in [list, tuple]:
00728 buff.write(struct.pack('<I%sB'%length, length, *_x))
00729 else:
00730 buff.write(struct.pack('<I%ss'%length, length, _x))
00731 buff.write(_struct_B.pack(_v20.is_dense))
00732 length = len(_v19.mask)
00733 buff.write(_struct_I.pack(length))
00734 pattern = '<%si'%length
00735 buff.write(struct.pack(pattern, *_v19.mask))
00736 _v23 = _v19.image
00737 _v24 = _v23.header
00738 buff.write(_struct_I.pack(_v24.seq))
00739 _v25 = _v24.stamp
00740 _x = _v25
00741 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00742 _x = _v24.frame_id
00743 length = len(_x)
00744 if python3 or type(_x) == unicode:
00745 _x = _x.encode('utf-8')
00746 length = len(_x)
00747 buff.write(struct.pack('<I%ss'%length, length, _x))
00748 _x = _v23
00749 buff.write(_struct_2I.pack(_x.height, _x.width))
00750 _x = _v23.encoding
00751 length = len(_x)
00752 if python3 or type(_x) == unicode:
00753 _x = _x.encode('utf-8')
00754 length = len(_x)
00755 buff.write(struct.pack('<I%ss'%length, length, _x))
00756 _x = _v23
00757 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00758 _x = _v23.data
00759 length = len(_x)
00760 # - if encoded as a list instead, serialize as bytes instead of string
00761 if type(_x) in [list, tuple]:
00762 buff.write(struct.pack('<I%sB'%length, length, *_x))
00763 else:
00764 buff.write(struct.pack('<I%ss'%length, length, _x))
00765 _v26 = _v19.disparity_image
00766 _v27 = _v26.header
00767 buff.write(_struct_I.pack(_v27.seq))
00768 _v28 = _v27.stamp
00769 _x = _v28
00770 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00771 _x = _v27.frame_id
00772 length = len(_x)
00773 if python3 or type(_x) == unicode:
00774 _x = _x.encode('utf-8')
00775 length = len(_x)
00776 buff.write(struct.pack('<I%ss'%length, length, _x))
00777 _x = _v26
00778 buff.write(_struct_2I.pack(_x.height, _x.width))
00779 _x = _v26.encoding
00780 length = len(_x)
00781 if python3 or type(_x) == unicode:
00782 _x = _x.encode('utf-8')
00783 length = len(_x)
00784 buff.write(struct.pack('<I%ss'%length, length, _x))
00785 _x = _v26
00786 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00787 _x = _v26.data
00788 length = len(_x)
00789 # - if encoded as a list instead, serialize as bytes instead of string
00790 if type(_x) in [list, tuple]:
00791 buff.write(struct.pack('<I%sB'%length, length, *_x))
00792 else:
00793 buff.write(struct.pack('<I%ss'%length, length, _x))
00794 _v29 = _v19.cam_info
00795 _v30 = _v29.header
00796 buff.write(_struct_I.pack(_v30.seq))
00797 _v31 = _v30.stamp
00798 _x = _v31
00799 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00800 _x = _v30.frame_id
00801 length = len(_x)
00802 if python3 or type(_x) == unicode:
00803 _x = _x.encode('utf-8')
00804 length = len(_x)
00805 buff.write(struct.pack('<I%ss'%length, length, _x))
00806 _x = _v29
00807 buff.write(_struct_2I.pack(_x.height, _x.width))
00808 _x = _v29.distortion_model
00809 length = len(_x)
00810 if python3 or type(_x) == unicode:
00811 _x = _x.encode('utf-8')
00812 length = len(_x)
00813 buff.write(struct.pack('<I%ss'%length, length, _x))
00814 length = len(_v29.D)
00815 buff.write(_struct_I.pack(length))
00816 pattern = '<%sd'%length
00817 buff.write(struct.pack(pattern, *_v29.D))
00818 buff.write(_struct_9d.pack(*_v29.K))
00819 buff.write(_struct_9d.pack(*_v29.R))
00820 buff.write(_struct_12d.pack(*_v29.P))
00821 _x = _v29
00822 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
00823 _v32 = _v29.roi
00824 _x = _v32
00825 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
00826 _v33 = _v19.roi_box_pose
00827 _v34 = _v33.header
00828 buff.write(_struct_I.pack(_v34.seq))
00829 _v35 = _v34.stamp
00830 _x = _v35
00831 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00832 _x = _v34.frame_id
00833 length = len(_x)
00834 if python3 or type(_x) == unicode:
00835 _x = _x.encode('utf-8')
00836 length = len(_x)
00837 buff.write(struct.pack('<I%ss'%length, length, _x))
00838 _v36 = _v33.pose
00839 _v37 = _v36.position
00840 _x = _v37
00841 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00842 _v38 = _v36.orientation
00843 _x = _v38
00844 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00845 _v39 = _v19.roi_box_dims
00846 _x = _v39
00847 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00848 _x = val2.collision_name
00849 length = len(_x)
00850 if python3 or type(_x) == unicode:
00851 _x = _x.encode('utf-8')
00852 length = len(_x)
00853 buff.write(struct.pack('<I%ss'%length, length, _x))
00854 buff.write(_struct_i.pack(self.result.value))
00855 except struct.error as se: self._check_types(se)
00856 except TypeError as te: self._check_types(te)
00857
00858 def deserialize(self, str):
00859 """
00860 unpack serialized message in str into this message instance
00861 :param str: byte array of serialized message, ``str``
00862 """
00863 try:
00864 if self.grasps is None:
00865 self.grasps = None
00866 if self.result is None:
00867 self.result = object_manipulation_msgs.msg.GraspPlanningErrorCode()
00868 end = 0
00869 start = end
00870 end += 4
00871 (length,) = _struct_I.unpack(str[start:end])
00872 self.grasps = []
00873 for i in range(0, length):
00874 val1 = object_manipulation_msgs.msg.Grasp()
00875 _v40 = val1.pre_grasp_posture
00876 _v41 = _v40.header
00877 start = end
00878 end += 4
00879 (_v41.seq,) = _struct_I.unpack(str[start:end])
00880 _v42 = _v41.stamp
00881 _x = _v42
00882 start = end
00883 end += 8
00884 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00885 start = end
00886 end += 4
00887 (length,) = _struct_I.unpack(str[start:end])
00888 start = end
00889 end += length
00890 if python3:
00891 _v41.frame_id = str[start:end].decode('utf-8')
00892 else:
00893 _v41.frame_id = str[start:end]
00894 start = end
00895 end += 4
00896 (length,) = _struct_I.unpack(str[start:end])
00897 _v40.name = []
00898 for i in range(0, length):
00899 start = end
00900 end += 4
00901 (length,) = _struct_I.unpack(str[start:end])
00902 start = end
00903 end += length
00904 if python3:
00905 val3 = str[start:end].decode('utf-8')
00906 else:
00907 val3 = str[start:end]
00908 _v40.name.append(val3)
00909 start = end
00910 end += 4
00911 (length,) = _struct_I.unpack(str[start:end])
00912 pattern = '<%sd'%length
00913 start = end
00914 end += struct.calcsize(pattern)
00915 _v40.position = struct.unpack(pattern, str[start:end])
00916 start = end
00917 end += 4
00918 (length,) = _struct_I.unpack(str[start:end])
00919 pattern = '<%sd'%length
00920 start = end
00921 end += struct.calcsize(pattern)
00922 _v40.velocity = struct.unpack(pattern, str[start:end])
00923 start = end
00924 end += 4
00925 (length,) = _struct_I.unpack(str[start:end])
00926 pattern = '<%sd'%length
00927 start = end
00928 end += struct.calcsize(pattern)
00929 _v40.effort = struct.unpack(pattern, str[start:end])
00930 _v43 = val1.grasp_posture
00931 _v44 = _v43.header
00932 start = end
00933 end += 4
00934 (_v44.seq,) = _struct_I.unpack(str[start:end])
00935 _v45 = _v44.stamp
00936 _x = _v45
00937 start = end
00938 end += 8
00939 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00940 start = end
00941 end += 4
00942 (length,) = _struct_I.unpack(str[start:end])
00943 start = end
00944 end += length
00945 if python3:
00946 _v44.frame_id = str[start:end].decode('utf-8')
00947 else:
00948 _v44.frame_id = str[start:end]
00949 start = end
00950 end += 4
00951 (length,) = _struct_I.unpack(str[start:end])
00952 _v43.name = []
00953 for i in range(0, length):
00954 start = end
00955 end += 4
00956 (length,) = _struct_I.unpack(str[start:end])
00957 start = end
00958 end += length
00959 if python3:
00960 val3 = str[start:end].decode('utf-8')
00961 else:
00962 val3 = str[start:end]
00963 _v43.name.append(val3)
00964 start = end
00965 end += 4
00966 (length,) = _struct_I.unpack(str[start:end])
00967 pattern = '<%sd'%length
00968 start = end
00969 end += struct.calcsize(pattern)
00970 _v43.position = struct.unpack(pattern, str[start:end])
00971 start = end
00972 end += 4
00973 (length,) = _struct_I.unpack(str[start:end])
00974 pattern = '<%sd'%length
00975 start = end
00976 end += struct.calcsize(pattern)
00977 _v43.velocity = struct.unpack(pattern, str[start:end])
00978 start = end
00979 end += 4
00980 (length,) = _struct_I.unpack(str[start:end])
00981 pattern = '<%sd'%length
00982 start = end
00983 end += struct.calcsize(pattern)
00984 _v43.effort = struct.unpack(pattern, str[start:end])
00985 _v46 = val1.grasp_pose
00986 _v47 = _v46.position
00987 _x = _v47
00988 start = end
00989 end += 24
00990 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00991 _v48 = _v46.orientation
00992 _x = _v48
00993 start = end
00994 end += 32
00995 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00996 _x = val1
00997 start = end
00998 end += 17
00999 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
01000 val1.cluster_rep = bool(val1.cluster_rep)
01001 start = end
01002 end += 4
01003 (length,) = _struct_I.unpack(str[start:end])
01004 val1.moved_obstacles = []
01005 for i in range(0, length):
01006 val2 = object_manipulation_msgs.msg.GraspableObject()
01007 start = end
01008 end += 4
01009 (length,) = _struct_I.unpack(str[start:end])
01010 start = end
01011 end += length
01012 if python3:
01013 val2.reference_frame_id = str[start:end].decode('utf-8')
01014 else:
01015 val2.reference_frame_id = str[start:end]
01016 start = end
01017 end += 4
01018 (length,) = _struct_I.unpack(str[start:end])
01019 val2.potential_models = []
01020 for i in range(0, length):
01021 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
01022 start = end
01023 end += 4
01024 (val3.model_id,) = _struct_i.unpack(str[start:end])
01025 _v49 = val3.pose
01026 _v50 = _v49.header
01027 start = end
01028 end += 4
01029 (_v50.seq,) = _struct_I.unpack(str[start:end])
01030 _v51 = _v50.stamp
01031 _x = _v51
01032 start = end
01033 end += 8
01034 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01035 start = end
01036 end += 4
01037 (length,) = _struct_I.unpack(str[start:end])
01038 start = end
01039 end += length
01040 if python3:
01041 _v50.frame_id = str[start:end].decode('utf-8')
01042 else:
01043 _v50.frame_id = str[start:end]
01044 _v52 = _v49.pose
01045 _v53 = _v52.position
01046 _x = _v53
01047 start = end
01048 end += 24
01049 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01050 _v54 = _v52.orientation
01051 _x = _v54
01052 start = end
01053 end += 32
01054 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01055 start = end
01056 end += 4
01057 (val3.confidence,) = _struct_f.unpack(str[start:end])
01058 start = end
01059 end += 4
01060 (length,) = _struct_I.unpack(str[start:end])
01061 start = end
01062 end += length
01063 if python3:
01064 val3.detector_name = str[start:end].decode('utf-8')
01065 else:
01066 val3.detector_name = str[start:end]
01067 val2.potential_models.append(val3)
01068 _v55 = val2.cluster
01069 _v56 = _v55.header
01070 start = end
01071 end += 4
01072 (_v56.seq,) = _struct_I.unpack(str[start:end])
01073 _v57 = _v56.stamp
01074 _x = _v57
01075 start = end
01076 end += 8
01077 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01078 start = end
01079 end += 4
01080 (length,) = _struct_I.unpack(str[start:end])
01081 start = end
01082 end += length
01083 if python3:
01084 _v56.frame_id = str[start:end].decode('utf-8')
01085 else:
01086 _v56.frame_id = str[start:end]
01087 start = end
01088 end += 4
01089 (length,) = _struct_I.unpack(str[start:end])
01090 _v55.points = []
01091 for i in range(0, length):
01092 val4 = geometry_msgs.msg.Point32()
01093 _x = val4
01094 start = end
01095 end += 12
01096 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01097 _v55.points.append(val4)
01098 start = end
01099 end += 4
01100 (length,) = _struct_I.unpack(str[start:end])
01101 _v55.channels = []
01102 for i in range(0, length):
01103 val4 = sensor_msgs.msg.ChannelFloat32()
01104 start = end
01105 end += 4
01106 (length,) = _struct_I.unpack(str[start:end])
01107 start = end
01108 end += length
01109 if python3:
01110 val4.name = str[start:end].decode('utf-8')
01111 else:
01112 val4.name = str[start:end]
01113 start = end
01114 end += 4
01115 (length,) = _struct_I.unpack(str[start:end])
01116 pattern = '<%sf'%length
01117 start = end
01118 end += struct.calcsize(pattern)
01119 val4.values = struct.unpack(pattern, str[start:end])
01120 _v55.channels.append(val4)
01121 _v58 = val2.region
01122 _v59 = _v58.cloud
01123 _v60 = _v59.header
01124 start = end
01125 end += 4
01126 (_v60.seq,) = _struct_I.unpack(str[start:end])
01127 _v61 = _v60.stamp
01128 _x = _v61
01129 start = end
01130 end += 8
01131 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01132 start = end
01133 end += 4
01134 (length,) = _struct_I.unpack(str[start:end])
01135 start = end
01136 end += length
01137 if python3:
01138 _v60.frame_id = str[start:end].decode('utf-8')
01139 else:
01140 _v60.frame_id = str[start:end]
01141 _x = _v59
01142 start = end
01143 end += 8
01144 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01145 start = end
01146 end += 4
01147 (length,) = _struct_I.unpack(str[start:end])
01148 _v59.fields = []
01149 for i in range(0, length):
01150 val5 = sensor_msgs.msg.PointField()
01151 start = end
01152 end += 4
01153 (length,) = _struct_I.unpack(str[start:end])
01154 start = end
01155 end += length
01156 if python3:
01157 val5.name = str[start:end].decode('utf-8')
01158 else:
01159 val5.name = str[start:end]
01160 _x = val5
01161 start = end
01162 end += 9
01163 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01164 _v59.fields.append(val5)
01165 _x = _v59
01166 start = end
01167 end += 9
01168 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
01169 _v59.is_bigendian = bool(_v59.is_bigendian)
01170 start = end
01171 end += 4
01172 (length,) = _struct_I.unpack(str[start:end])
01173 start = end
01174 end += length
01175 if python3:
01176 _v59.data = str[start:end].decode('utf-8')
01177 else:
01178 _v59.data = str[start:end]
01179 start = end
01180 end += 1
01181 (_v59.is_dense,) = _struct_B.unpack(str[start:end])
01182 _v59.is_dense = bool(_v59.is_dense)
01183 start = end
01184 end += 4
01185 (length,) = _struct_I.unpack(str[start:end])
01186 pattern = '<%si'%length
01187 start = end
01188 end += struct.calcsize(pattern)
01189 _v58.mask = struct.unpack(pattern, str[start:end])
01190 _v62 = _v58.image
01191 _v63 = _v62.header
01192 start = end
01193 end += 4
01194 (_v63.seq,) = _struct_I.unpack(str[start:end])
01195 _v64 = _v63.stamp
01196 _x = _v64
01197 start = end
01198 end += 8
01199 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01200 start = end
01201 end += 4
01202 (length,) = _struct_I.unpack(str[start:end])
01203 start = end
01204 end += length
01205 if python3:
01206 _v63.frame_id = str[start:end].decode('utf-8')
01207 else:
01208 _v63.frame_id = str[start:end]
01209 _x = _v62
01210 start = end
01211 end += 8
01212 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01213 start = end
01214 end += 4
01215 (length,) = _struct_I.unpack(str[start:end])
01216 start = end
01217 end += length
01218 if python3:
01219 _v62.encoding = str[start:end].decode('utf-8')
01220 else:
01221 _v62.encoding = str[start:end]
01222 _x = _v62
01223 start = end
01224 end += 5
01225 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01226 start = end
01227 end += 4
01228 (length,) = _struct_I.unpack(str[start:end])
01229 start = end
01230 end += length
01231 if python3:
01232 _v62.data = str[start:end].decode('utf-8')
01233 else:
01234 _v62.data = str[start:end]
01235 _v65 = _v58.disparity_image
01236 _v66 = _v65.header
01237 start = end
01238 end += 4
01239 (_v66.seq,) = _struct_I.unpack(str[start:end])
01240 _v67 = _v66.stamp
01241 _x = _v67
01242 start = end
01243 end += 8
01244 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01245 start = end
01246 end += 4
01247 (length,) = _struct_I.unpack(str[start:end])
01248 start = end
01249 end += length
01250 if python3:
01251 _v66.frame_id = str[start:end].decode('utf-8')
01252 else:
01253 _v66.frame_id = str[start:end]
01254 _x = _v65
01255 start = end
01256 end += 8
01257 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01258 start = end
01259 end += 4
01260 (length,) = _struct_I.unpack(str[start:end])
01261 start = end
01262 end += length
01263 if python3:
01264 _v65.encoding = str[start:end].decode('utf-8')
01265 else:
01266 _v65.encoding = str[start:end]
01267 _x = _v65
01268 start = end
01269 end += 5
01270 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01271 start = end
01272 end += 4
01273 (length,) = _struct_I.unpack(str[start:end])
01274 start = end
01275 end += length
01276 if python3:
01277 _v65.data = str[start:end].decode('utf-8')
01278 else:
01279 _v65.data = str[start:end]
01280 _v68 = _v58.cam_info
01281 _v69 = _v68.header
01282 start = end
01283 end += 4
01284 (_v69.seq,) = _struct_I.unpack(str[start:end])
01285 _v70 = _v69.stamp
01286 _x = _v70
01287 start = end
01288 end += 8
01289 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01290 start = end
01291 end += 4
01292 (length,) = _struct_I.unpack(str[start:end])
01293 start = end
01294 end += length
01295 if python3:
01296 _v69.frame_id = str[start:end].decode('utf-8')
01297 else:
01298 _v69.frame_id = str[start:end]
01299 _x = _v68
01300 start = end
01301 end += 8
01302 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01303 start = end
01304 end += 4
01305 (length,) = _struct_I.unpack(str[start:end])
01306 start = end
01307 end += length
01308 if python3:
01309 _v68.distortion_model = str[start:end].decode('utf-8')
01310 else:
01311 _v68.distortion_model = str[start:end]
01312 start = end
01313 end += 4
01314 (length,) = _struct_I.unpack(str[start:end])
01315 pattern = '<%sd'%length
01316 start = end
01317 end += struct.calcsize(pattern)
01318 _v68.D = struct.unpack(pattern, str[start:end])
01319 start = end
01320 end += 72
01321 _v68.K = _struct_9d.unpack(str[start:end])
01322 start = end
01323 end += 72
01324 _v68.R = _struct_9d.unpack(str[start:end])
01325 start = end
01326 end += 96
01327 _v68.P = _struct_12d.unpack(str[start:end])
01328 _x = _v68
01329 start = end
01330 end += 8
01331 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
01332 _v71 = _v68.roi
01333 _x = _v71
01334 start = end
01335 end += 17
01336 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
01337 _v71.do_rectify = bool(_v71.do_rectify)
01338 _v72 = _v58.roi_box_pose
01339 _v73 = _v72.header
01340 start = end
01341 end += 4
01342 (_v73.seq,) = _struct_I.unpack(str[start:end])
01343 _v74 = _v73.stamp
01344 _x = _v74
01345 start = end
01346 end += 8
01347 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01348 start = end
01349 end += 4
01350 (length,) = _struct_I.unpack(str[start:end])
01351 start = end
01352 end += length
01353 if python3:
01354 _v73.frame_id = str[start:end].decode('utf-8')
01355 else:
01356 _v73.frame_id = str[start:end]
01357 _v75 = _v72.pose
01358 _v76 = _v75.position
01359 _x = _v76
01360 start = end
01361 end += 24
01362 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01363 _v77 = _v75.orientation
01364 _x = _v77
01365 start = end
01366 end += 32
01367 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01368 _v78 = _v58.roi_box_dims
01369 _x = _v78
01370 start = end
01371 end += 24
01372 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01373 start = end
01374 end += 4
01375 (length,) = _struct_I.unpack(str[start:end])
01376 start = end
01377 end += length
01378 if python3:
01379 val2.collision_name = str[start:end].decode('utf-8')
01380 else:
01381 val2.collision_name = str[start:end]
01382 val1.moved_obstacles.append(val2)
01383 self.grasps.append(val1)
01384 start = end
01385 end += 4
01386 (self.result.value,) = _struct_i.unpack(str[start:end])
01387 return self
01388 except struct.error as e:
01389 raise genpy.DeserializationError(e) #most likely buffer underfill
01390
01391
01392 def serialize_numpy(self, buff, numpy):
01393 """
01394 serialize message with numpy array types into buffer
01395 :param buff: buffer, ``StringIO``
01396 :param numpy: numpy python module
01397 """
01398 try:
01399 length = len(self.grasps)
01400 buff.write(_struct_I.pack(length))
01401 for val1 in self.grasps:
01402 _v79 = val1.pre_grasp_posture
01403 _v80 = _v79.header
01404 buff.write(_struct_I.pack(_v80.seq))
01405 _v81 = _v80.stamp
01406 _x = _v81
01407 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01408 _x = _v80.frame_id
01409 length = len(_x)
01410 if python3 or type(_x) == unicode:
01411 _x = _x.encode('utf-8')
01412 length = len(_x)
01413 buff.write(struct.pack('<I%ss'%length, length, _x))
01414 length = len(_v79.name)
01415 buff.write(_struct_I.pack(length))
01416 for val3 in _v79.name:
01417 length = len(val3)
01418 if python3 or type(val3) == unicode:
01419 val3 = val3.encode('utf-8')
01420 length = len(val3)
01421 buff.write(struct.pack('<I%ss'%length, length, val3))
01422 length = len(_v79.position)
01423 buff.write(_struct_I.pack(length))
01424 pattern = '<%sd'%length
01425 buff.write(_v79.position.tostring())
01426 length = len(_v79.velocity)
01427 buff.write(_struct_I.pack(length))
01428 pattern = '<%sd'%length
01429 buff.write(_v79.velocity.tostring())
01430 length = len(_v79.effort)
01431 buff.write(_struct_I.pack(length))
01432 pattern = '<%sd'%length
01433 buff.write(_v79.effort.tostring())
01434 _v82 = val1.grasp_posture
01435 _v83 = _v82.header
01436 buff.write(_struct_I.pack(_v83.seq))
01437 _v84 = _v83.stamp
01438 _x = _v84
01439 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01440 _x = _v83.frame_id
01441 length = len(_x)
01442 if python3 or type(_x) == unicode:
01443 _x = _x.encode('utf-8')
01444 length = len(_x)
01445 buff.write(struct.pack('<I%ss'%length, length, _x))
01446 length = len(_v82.name)
01447 buff.write(_struct_I.pack(length))
01448 for val3 in _v82.name:
01449 length = len(val3)
01450 if python3 or type(val3) == unicode:
01451 val3 = val3.encode('utf-8')
01452 length = len(val3)
01453 buff.write(struct.pack('<I%ss'%length, length, val3))
01454 length = len(_v82.position)
01455 buff.write(_struct_I.pack(length))
01456 pattern = '<%sd'%length
01457 buff.write(_v82.position.tostring())
01458 length = len(_v82.velocity)
01459 buff.write(_struct_I.pack(length))
01460 pattern = '<%sd'%length
01461 buff.write(_v82.velocity.tostring())
01462 length = len(_v82.effort)
01463 buff.write(_struct_I.pack(length))
01464 pattern = '<%sd'%length
01465 buff.write(_v82.effort.tostring())
01466 _v85 = val1.grasp_pose
01467 _v86 = _v85.position
01468 _x = _v86
01469 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01470 _v87 = _v85.orientation
01471 _x = _v87
01472 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01473 _x = val1
01474 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
01475 length = len(val1.moved_obstacles)
01476 buff.write(_struct_I.pack(length))
01477 for val2 in val1.moved_obstacles:
01478 _x = val2.reference_frame_id
01479 length = len(_x)
01480 if python3 or type(_x) == unicode:
01481 _x = _x.encode('utf-8')
01482 length = len(_x)
01483 buff.write(struct.pack('<I%ss'%length, length, _x))
01484 length = len(val2.potential_models)
01485 buff.write(_struct_I.pack(length))
01486 for val3 in val2.potential_models:
01487 buff.write(_struct_i.pack(val3.model_id))
01488 _v88 = val3.pose
01489 _v89 = _v88.header
01490 buff.write(_struct_I.pack(_v89.seq))
01491 _v90 = _v89.stamp
01492 _x = _v90
01493 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01494 _x = _v89.frame_id
01495 length = len(_x)
01496 if python3 or type(_x) == unicode:
01497 _x = _x.encode('utf-8')
01498 length = len(_x)
01499 buff.write(struct.pack('<I%ss'%length, length, _x))
01500 _v91 = _v88.pose
01501 _v92 = _v91.position
01502 _x = _v92
01503 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01504 _v93 = _v91.orientation
01505 _x = _v93
01506 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01507 buff.write(_struct_f.pack(val3.confidence))
01508 _x = val3.detector_name
01509 length = len(_x)
01510 if python3 or type(_x) == unicode:
01511 _x = _x.encode('utf-8')
01512 length = len(_x)
01513 buff.write(struct.pack('<I%ss'%length, length, _x))
01514 _v94 = val2.cluster
01515 _v95 = _v94.header
01516 buff.write(_struct_I.pack(_v95.seq))
01517 _v96 = _v95.stamp
01518 _x = _v96
01519 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01520 _x = _v95.frame_id
01521 length = len(_x)
01522 if python3 or type(_x) == unicode:
01523 _x = _x.encode('utf-8')
01524 length = len(_x)
01525 buff.write(struct.pack('<I%ss'%length, length, _x))
01526 length = len(_v94.points)
01527 buff.write(_struct_I.pack(length))
01528 for val4 in _v94.points:
01529 _x = val4
01530 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01531 length = len(_v94.channels)
01532 buff.write(_struct_I.pack(length))
01533 for val4 in _v94.channels:
01534 _x = val4.name
01535 length = len(_x)
01536 if python3 or type(_x) == unicode:
01537 _x = _x.encode('utf-8')
01538 length = len(_x)
01539 buff.write(struct.pack('<I%ss'%length, length, _x))
01540 length = len(val4.values)
01541 buff.write(_struct_I.pack(length))
01542 pattern = '<%sf'%length
01543 buff.write(val4.values.tostring())
01544 _v97 = val2.region
01545 _v98 = _v97.cloud
01546 _v99 = _v98.header
01547 buff.write(_struct_I.pack(_v99.seq))
01548 _v100 = _v99.stamp
01549 _x = _v100
01550 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01551 _x = _v99.frame_id
01552 length = len(_x)
01553 if python3 or type(_x) == unicode:
01554 _x = _x.encode('utf-8')
01555 length = len(_x)
01556 buff.write(struct.pack('<I%ss'%length, length, _x))
01557 _x = _v98
01558 buff.write(_struct_2I.pack(_x.height, _x.width))
01559 length = len(_v98.fields)
01560 buff.write(_struct_I.pack(length))
01561 for val5 in _v98.fields:
01562 _x = val5.name
01563 length = len(_x)
01564 if python3 or type(_x) == unicode:
01565 _x = _x.encode('utf-8')
01566 length = len(_x)
01567 buff.write(struct.pack('<I%ss'%length, length, _x))
01568 _x = val5
01569 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01570 _x = _v98
01571 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01572 _x = _v98.data
01573 length = len(_x)
01574 # - if encoded as a list instead, serialize as bytes instead of string
01575 if type(_x) in [list, tuple]:
01576 buff.write(struct.pack('<I%sB'%length, length, *_x))
01577 else:
01578 buff.write(struct.pack('<I%ss'%length, length, _x))
01579 buff.write(_struct_B.pack(_v98.is_dense))
01580 length = len(_v97.mask)
01581 buff.write(_struct_I.pack(length))
01582 pattern = '<%si'%length
01583 buff.write(_v97.mask.tostring())
01584 _v101 = _v97.image
01585 _v102 = _v101.header
01586 buff.write(_struct_I.pack(_v102.seq))
01587 _v103 = _v102.stamp
01588 _x = _v103
01589 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01590 _x = _v102.frame_id
01591 length = len(_x)
01592 if python3 or type(_x) == unicode:
01593 _x = _x.encode('utf-8')
01594 length = len(_x)
01595 buff.write(struct.pack('<I%ss'%length, length, _x))
01596 _x = _v101
01597 buff.write(_struct_2I.pack(_x.height, _x.width))
01598 _x = _v101.encoding
01599 length = len(_x)
01600 if python3 or type(_x) == unicode:
01601 _x = _x.encode('utf-8')
01602 length = len(_x)
01603 buff.write(struct.pack('<I%ss'%length, length, _x))
01604 _x = _v101
01605 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01606 _x = _v101.data
01607 length = len(_x)
01608 # - if encoded as a list instead, serialize as bytes instead of string
01609 if type(_x) in [list, tuple]:
01610 buff.write(struct.pack('<I%sB'%length, length, *_x))
01611 else:
01612 buff.write(struct.pack('<I%ss'%length, length, _x))
01613 _v104 = _v97.disparity_image
01614 _v105 = _v104.header
01615 buff.write(_struct_I.pack(_v105.seq))
01616 _v106 = _v105.stamp
01617 _x = _v106
01618 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01619 _x = _v105.frame_id
01620 length = len(_x)
01621 if python3 or type(_x) == unicode:
01622 _x = _x.encode('utf-8')
01623 length = len(_x)
01624 buff.write(struct.pack('<I%ss'%length, length, _x))
01625 _x = _v104
01626 buff.write(_struct_2I.pack(_x.height, _x.width))
01627 _x = _v104.encoding
01628 length = len(_x)
01629 if python3 or type(_x) == unicode:
01630 _x = _x.encode('utf-8')
01631 length = len(_x)
01632 buff.write(struct.pack('<I%ss'%length, length, _x))
01633 _x = _v104
01634 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01635 _x = _v104.data
01636 length = len(_x)
01637 # - if encoded as a list instead, serialize as bytes instead of string
01638 if type(_x) in [list, tuple]:
01639 buff.write(struct.pack('<I%sB'%length, length, *_x))
01640 else:
01641 buff.write(struct.pack('<I%ss'%length, length, _x))
01642 _v107 = _v97.cam_info
01643 _v108 = _v107.header
01644 buff.write(_struct_I.pack(_v108.seq))
01645 _v109 = _v108.stamp
01646 _x = _v109
01647 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01648 _x = _v108.frame_id
01649 length = len(_x)
01650 if python3 or type(_x) == unicode:
01651 _x = _x.encode('utf-8')
01652 length = len(_x)
01653 buff.write(struct.pack('<I%ss'%length, length, _x))
01654 _x = _v107
01655 buff.write(_struct_2I.pack(_x.height, _x.width))
01656 _x = _v107.distortion_model
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 length = len(_v107.D)
01663 buff.write(_struct_I.pack(length))
01664 pattern = '<%sd'%length
01665 buff.write(_v107.D.tostring())
01666 buff.write(_v107.K.tostring())
01667 buff.write(_v107.R.tostring())
01668 buff.write(_v107.P.tostring())
01669 _x = _v107
01670 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01671 _v110 = _v107.roi
01672 _x = _v110
01673 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01674 _v111 = _v97.roi_box_pose
01675 _v112 = _v111.header
01676 buff.write(_struct_I.pack(_v112.seq))
01677 _v113 = _v112.stamp
01678 _x = _v113
01679 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01680 _x = _v112.frame_id
01681 length = len(_x)
01682 if python3 or type(_x) == unicode:
01683 _x = _x.encode('utf-8')
01684 length = len(_x)
01685 buff.write(struct.pack('<I%ss'%length, length, _x))
01686 _v114 = _v111.pose
01687 _v115 = _v114.position
01688 _x = _v115
01689 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01690 _v116 = _v114.orientation
01691 _x = _v116
01692 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01693 _v117 = _v97.roi_box_dims
01694 _x = _v117
01695 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01696 _x = val2.collision_name
01697 length = len(_x)
01698 if python3 or type(_x) == unicode:
01699 _x = _x.encode('utf-8')
01700 length = len(_x)
01701 buff.write(struct.pack('<I%ss'%length, length, _x))
01702 buff.write(_struct_i.pack(self.result.value))
01703 except struct.error as se: self._check_types(se)
01704 except TypeError as te: self._check_types(te)
01705
01706 def deserialize_numpy(self, str, numpy):
01707 """
01708 unpack serialized message in str into this message instance using numpy for array types
01709 :param str: byte array of serialized message, ``str``
01710 :param numpy: numpy python module
01711 """
01712 try:
01713 if self.grasps is None:
01714 self.grasps = None
01715 if self.result is None:
01716 self.result = object_manipulation_msgs.msg.GraspPlanningErrorCode()
01717 end = 0
01718 start = end
01719 end += 4
01720 (length,) = _struct_I.unpack(str[start:end])
01721 self.grasps = []
01722 for i in range(0, length):
01723 val1 = object_manipulation_msgs.msg.Grasp()
01724 _v118 = val1.pre_grasp_posture
01725 _v119 = _v118.header
01726 start = end
01727 end += 4
01728 (_v119.seq,) = _struct_I.unpack(str[start:end])
01729 _v120 = _v119.stamp
01730 _x = _v120
01731 start = end
01732 end += 8
01733 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01734 start = end
01735 end += 4
01736 (length,) = _struct_I.unpack(str[start:end])
01737 start = end
01738 end += length
01739 if python3:
01740 _v119.frame_id = str[start:end].decode('utf-8')
01741 else:
01742 _v119.frame_id = str[start:end]
01743 start = end
01744 end += 4
01745 (length,) = _struct_I.unpack(str[start:end])
01746 _v118.name = []
01747 for i in range(0, length):
01748 start = end
01749 end += 4
01750 (length,) = _struct_I.unpack(str[start:end])
01751 start = end
01752 end += length
01753 if python3:
01754 val3 = str[start:end].decode('utf-8')
01755 else:
01756 val3 = str[start:end]
01757 _v118.name.append(val3)
01758 start = end
01759 end += 4
01760 (length,) = _struct_I.unpack(str[start:end])
01761 pattern = '<%sd'%length
01762 start = end
01763 end += struct.calcsize(pattern)
01764 _v118.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01765 start = end
01766 end += 4
01767 (length,) = _struct_I.unpack(str[start:end])
01768 pattern = '<%sd'%length
01769 start = end
01770 end += struct.calcsize(pattern)
01771 _v118.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01772 start = end
01773 end += 4
01774 (length,) = _struct_I.unpack(str[start:end])
01775 pattern = '<%sd'%length
01776 start = end
01777 end += struct.calcsize(pattern)
01778 _v118.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01779 _v121 = val1.grasp_posture
01780 _v122 = _v121.header
01781 start = end
01782 end += 4
01783 (_v122.seq,) = _struct_I.unpack(str[start:end])
01784 _v123 = _v122.stamp
01785 _x = _v123
01786 start = end
01787 end += 8
01788 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01789 start = end
01790 end += 4
01791 (length,) = _struct_I.unpack(str[start:end])
01792 start = end
01793 end += length
01794 if python3:
01795 _v122.frame_id = str[start:end].decode('utf-8')
01796 else:
01797 _v122.frame_id = str[start:end]
01798 start = end
01799 end += 4
01800 (length,) = _struct_I.unpack(str[start:end])
01801 _v121.name = []
01802 for i in range(0, length):
01803 start = end
01804 end += 4
01805 (length,) = _struct_I.unpack(str[start:end])
01806 start = end
01807 end += length
01808 if python3:
01809 val3 = str[start:end].decode('utf-8')
01810 else:
01811 val3 = str[start:end]
01812 _v121.name.append(val3)
01813 start = end
01814 end += 4
01815 (length,) = _struct_I.unpack(str[start:end])
01816 pattern = '<%sd'%length
01817 start = end
01818 end += struct.calcsize(pattern)
01819 _v121.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01820 start = end
01821 end += 4
01822 (length,) = _struct_I.unpack(str[start:end])
01823 pattern = '<%sd'%length
01824 start = end
01825 end += struct.calcsize(pattern)
01826 _v121.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01827 start = end
01828 end += 4
01829 (length,) = _struct_I.unpack(str[start:end])
01830 pattern = '<%sd'%length
01831 start = end
01832 end += struct.calcsize(pattern)
01833 _v121.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
01834 _v124 = val1.grasp_pose
01835 _v125 = _v124.position
01836 _x = _v125
01837 start = end
01838 end += 24
01839 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01840 _v126 = _v124.orientation
01841 _x = _v126
01842 start = end
01843 end += 32
01844 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01845 _x = val1
01846 start = end
01847 end += 17
01848 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
01849 val1.cluster_rep = bool(val1.cluster_rep)
01850 start = end
01851 end += 4
01852 (length,) = _struct_I.unpack(str[start:end])
01853 val1.moved_obstacles = []
01854 for i in range(0, length):
01855 val2 = object_manipulation_msgs.msg.GraspableObject()
01856 start = end
01857 end += 4
01858 (length,) = _struct_I.unpack(str[start:end])
01859 start = end
01860 end += length
01861 if python3:
01862 val2.reference_frame_id = str[start:end].decode('utf-8')
01863 else:
01864 val2.reference_frame_id = str[start:end]
01865 start = end
01866 end += 4
01867 (length,) = _struct_I.unpack(str[start:end])
01868 val2.potential_models = []
01869 for i in range(0, length):
01870 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
01871 start = end
01872 end += 4
01873 (val3.model_id,) = _struct_i.unpack(str[start:end])
01874 _v127 = val3.pose
01875 _v128 = _v127.header
01876 start = end
01877 end += 4
01878 (_v128.seq,) = _struct_I.unpack(str[start:end])
01879 _v129 = _v128.stamp
01880 _x = _v129
01881 start = end
01882 end += 8
01883 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01884 start = end
01885 end += 4
01886 (length,) = _struct_I.unpack(str[start:end])
01887 start = end
01888 end += length
01889 if python3:
01890 _v128.frame_id = str[start:end].decode('utf-8')
01891 else:
01892 _v128.frame_id = str[start:end]
01893 _v130 = _v127.pose
01894 _v131 = _v130.position
01895 _x = _v131
01896 start = end
01897 end += 24
01898 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01899 _v132 = _v130.orientation
01900 _x = _v132
01901 start = end
01902 end += 32
01903 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01904 start = end
01905 end += 4
01906 (val3.confidence,) = _struct_f.unpack(str[start:end])
01907 start = end
01908 end += 4
01909 (length,) = _struct_I.unpack(str[start:end])
01910 start = end
01911 end += length
01912 if python3:
01913 val3.detector_name = str[start:end].decode('utf-8')
01914 else:
01915 val3.detector_name = str[start:end]
01916 val2.potential_models.append(val3)
01917 _v133 = val2.cluster
01918 _v134 = _v133.header
01919 start = end
01920 end += 4
01921 (_v134.seq,) = _struct_I.unpack(str[start:end])
01922 _v135 = _v134.stamp
01923 _x = _v135
01924 start = end
01925 end += 8
01926 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01927 start = end
01928 end += 4
01929 (length,) = _struct_I.unpack(str[start:end])
01930 start = end
01931 end += length
01932 if python3:
01933 _v134.frame_id = str[start:end].decode('utf-8')
01934 else:
01935 _v134.frame_id = str[start:end]
01936 start = end
01937 end += 4
01938 (length,) = _struct_I.unpack(str[start:end])
01939 _v133.points = []
01940 for i in range(0, length):
01941 val4 = geometry_msgs.msg.Point32()
01942 _x = val4
01943 start = end
01944 end += 12
01945 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01946 _v133.points.append(val4)
01947 start = end
01948 end += 4
01949 (length,) = _struct_I.unpack(str[start:end])
01950 _v133.channels = []
01951 for i in range(0, length):
01952 val4 = sensor_msgs.msg.ChannelFloat32()
01953 start = end
01954 end += 4
01955 (length,) = _struct_I.unpack(str[start:end])
01956 start = end
01957 end += length
01958 if python3:
01959 val4.name = str[start:end].decode('utf-8')
01960 else:
01961 val4.name = str[start:end]
01962 start = end
01963 end += 4
01964 (length,) = _struct_I.unpack(str[start:end])
01965 pattern = '<%sf'%length
01966 start = end
01967 end += struct.calcsize(pattern)
01968 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
01969 _v133.channels.append(val4)
01970 _v136 = val2.region
01971 _v137 = _v136.cloud
01972 _v138 = _v137.header
01973 start = end
01974 end += 4
01975 (_v138.seq,) = _struct_I.unpack(str[start:end])
01976 _v139 = _v138.stamp
01977 _x = _v139
01978 start = end
01979 end += 8
01980 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01981 start = end
01982 end += 4
01983 (length,) = _struct_I.unpack(str[start:end])
01984 start = end
01985 end += length
01986 if python3:
01987 _v138.frame_id = str[start:end].decode('utf-8')
01988 else:
01989 _v138.frame_id = str[start:end]
01990 _x = _v137
01991 start = end
01992 end += 8
01993 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01994 start = end
01995 end += 4
01996 (length,) = _struct_I.unpack(str[start:end])
01997 _v137.fields = []
01998 for i in range(0, length):
01999 val5 = sensor_msgs.msg.PointField()
02000 start = end
02001 end += 4
02002 (length,) = _struct_I.unpack(str[start:end])
02003 start = end
02004 end += length
02005 if python3:
02006 val5.name = str[start:end].decode('utf-8')
02007 else:
02008 val5.name = str[start:end]
02009 _x = val5
02010 start = end
02011 end += 9
02012 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02013 _v137.fields.append(val5)
02014 _x = _v137
02015 start = end
02016 end += 9
02017 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
02018 _v137.is_bigendian = bool(_v137.is_bigendian)
02019 start = end
02020 end += 4
02021 (length,) = _struct_I.unpack(str[start:end])
02022 start = end
02023 end += length
02024 if python3:
02025 _v137.data = str[start:end].decode('utf-8')
02026 else:
02027 _v137.data = str[start:end]
02028 start = end
02029 end += 1
02030 (_v137.is_dense,) = _struct_B.unpack(str[start:end])
02031 _v137.is_dense = bool(_v137.is_dense)
02032 start = end
02033 end += 4
02034 (length,) = _struct_I.unpack(str[start:end])
02035 pattern = '<%si'%length
02036 start = end
02037 end += struct.calcsize(pattern)
02038 _v136.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02039 _v140 = _v136.image
02040 _v141 = _v140.header
02041 start = end
02042 end += 4
02043 (_v141.seq,) = _struct_I.unpack(str[start:end])
02044 _v142 = _v141.stamp
02045 _x = _v142
02046 start = end
02047 end += 8
02048 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02049 start = end
02050 end += 4
02051 (length,) = _struct_I.unpack(str[start:end])
02052 start = end
02053 end += length
02054 if python3:
02055 _v141.frame_id = str[start:end].decode('utf-8')
02056 else:
02057 _v141.frame_id = str[start:end]
02058 _x = _v140
02059 start = end
02060 end += 8
02061 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02062 start = end
02063 end += 4
02064 (length,) = _struct_I.unpack(str[start:end])
02065 start = end
02066 end += length
02067 if python3:
02068 _v140.encoding = str[start:end].decode('utf-8')
02069 else:
02070 _v140.encoding = str[start:end]
02071 _x = _v140
02072 start = end
02073 end += 5
02074 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02075 start = end
02076 end += 4
02077 (length,) = _struct_I.unpack(str[start:end])
02078 start = end
02079 end += length
02080 if python3:
02081 _v140.data = str[start:end].decode('utf-8')
02082 else:
02083 _v140.data = str[start:end]
02084 _v143 = _v136.disparity_image
02085 _v144 = _v143.header
02086 start = end
02087 end += 4
02088 (_v144.seq,) = _struct_I.unpack(str[start:end])
02089 _v145 = _v144.stamp
02090 _x = _v145
02091 start = end
02092 end += 8
02093 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02094 start = end
02095 end += 4
02096 (length,) = _struct_I.unpack(str[start:end])
02097 start = end
02098 end += length
02099 if python3:
02100 _v144.frame_id = str[start:end].decode('utf-8')
02101 else:
02102 _v144.frame_id = str[start:end]
02103 _x = _v143
02104 start = end
02105 end += 8
02106 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02107 start = end
02108 end += 4
02109 (length,) = _struct_I.unpack(str[start:end])
02110 start = end
02111 end += length
02112 if python3:
02113 _v143.encoding = str[start:end].decode('utf-8')
02114 else:
02115 _v143.encoding = str[start:end]
02116 _x = _v143
02117 start = end
02118 end += 5
02119 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02120 start = end
02121 end += 4
02122 (length,) = _struct_I.unpack(str[start:end])
02123 start = end
02124 end += length
02125 if python3:
02126 _v143.data = str[start:end].decode('utf-8')
02127 else:
02128 _v143.data = str[start:end]
02129 _v146 = _v136.cam_info
02130 _v147 = _v146.header
02131 start = end
02132 end += 4
02133 (_v147.seq,) = _struct_I.unpack(str[start:end])
02134 _v148 = _v147.stamp
02135 _x = _v148
02136 start = end
02137 end += 8
02138 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02139 start = end
02140 end += 4
02141 (length,) = _struct_I.unpack(str[start:end])
02142 start = end
02143 end += length
02144 if python3:
02145 _v147.frame_id = str[start:end].decode('utf-8')
02146 else:
02147 _v147.frame_id = str[start:end]
02148 _x = _v146
02149 start = end
02150 end += 8
02151 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02152 start = end
02153 end += 4
02154 (length,) = _struct_I.unpack(str[start:end])
02155 start = end
02156 end += length
02157 if python3:
02158 _v146.distortion_model = str[start:end].decode('utf-8')
02159 else:
02160 _v146.distortion_model = str[start:end]
02161 start = end
02162 end += 4
02163 (length,) = _struct_I.unpack(str[start:end])
02164 pattern = '<%sd'%length
02165 start = end
02166 end += struct.calcsize(pattern)
02167 _v146.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02168 start = end
02169 end += 72
02170 _v146.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02171 start = end
02172 end += 72
02173 _v146.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02174 start = end
02175 end += 96
02176 _v146.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
02177 _x = _v146
02178 start = end
02179 end += 8
02180 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
02181 _v149 = _v146.roi
02182 _x = _v149
02183 start = end
02184 end += 17
02185 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
02186 _v149.do_rectify = bool(_v149.do_rectify)
02187 _v150 = _v136.roi_box_pose
02188 _v151 = _v150.header
02189 start = end
02190 end += 4
02191 (_v151.seq,) = _struct_I.unpack(str[start:end])
02192 _v152 = _v151.stamp
02193 _x = _v152
02194 start = end
02195 end += 8
02196 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02197 start = end
02198 end += 4
02199 (length,) = _struct_I.unpack(str[start:end])
02200 start = end
02201 end += length
02202 if python3:
02203 _v151.frame_id = str[start:end].decode('utf-8')
02204 else:
02205 _v151.frame_id = str[start:end]
02206 _v153 = _v150.pose
02207 _v154 = _v153.position
02208 _x = _v154
02209 start = end
02210 end += 24
02211 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02212 _v155 = _v153.orientation
02213 _x = _v155
02214 start = end
02215 end += 32
02216 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02217 _v156 = _v136.roi_box_dims
02218 _x = _v156
02219 start = end
02220 end += 24
02221 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02222 start = end
02223 end += 4
02224 (length,) = _struct_I.unpack(str[start:end])
02225 start = end
02226 end += length
02227 if python3:
02228 val2.collision_name = str[start:end].decode('utf-8')
02229 else:
02230 val2.collision_name = str[start:end]
02231 val1.moved_obstacles.append(val2)
02232 self.grasps.append(val1)
02233 start = end
02234 end += 4
02235 (self.result.value,) = _struct_i.unpack(str[start:end])
02236 return self
02237 except struct.error as e:
02238 raise genpy.DeserializationError(e) #most likely buffer underfill
02239
02240 _struct_I = genpy.struct_I
02241 _struct_IBI = struct.Struct("<IBI")
02242 _struct_B = struct.Struct("<B")
02243 _struct_12d = struct.Struct("<12d")
02244 _struct_f = struct.Struct("<f")
02245 _struct_dB2f = struct.Struct("<dB2f")
02246 _struct_BI = struct.Struct("<BI")
02247 _struct_3f = struct.Struct("<3f")
02248 _struct_i = struct.Struct("<i")
02249 _struct_9d = struct.Struct("<9d")
02250 _struct_B2I = struct.Struct("<B2I")
02251 _struct_4d = struct.Struct("<4d")
02252 _struct_2I = struct.Struct("<2I")
02253 _struct_4IB = struct.Struct("<4IB")
02254 _struct_3d = struct.Struct("<3d")