00001 """autogenerated by genpy from object_manipulation_msgs/PickupResult.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 PickupResult(genpy.Message):
00014 _md5sum = "e37faa4bc17adb31e87f0a21276cde44"
00015 _type = "object_manipulation_msgs/PickupResult"
00016 _has_header = False
00017 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00018
00019 # The overall result of the pickup attempt
00020 ManipulationResult manipulation_result
00021
00022 # The performed grasp, if attempt was successful
00023 Grasp grasp
00024
00025 # the complete list of attempted grasp, in the order in which they have been attempted
00026 # the successful one should be the last one in this list
00027 Grasp[] attempted_grasps
00028
00029 # the outcomes of the attempted grasps, in the same order as attempted_grasps
00030 GraspResult[] attempted_grasp_results
00031
00032
00033 ================================================================================
00034 MSG: object_manipulation_msgs/ManipulationResult
00035 # Result codes for manipulation tasks
00036
00037 # task completed as expected
00038 # generally means you can proceed as planned
00039 int32 SUCCESS = 1
00040
00041 # task not possible (e.g. out of reach or obstacles in the way)
00042 # generally means that the world was not disturbed, so you can try another task
00043 int32 UNFEASIBLE = -1
00044
00045 # task was thought possible, but failed due to unexpected events during execution
00046 # it is likely that the world was disturbed, so you are encouraged to refresh
00047 # your sensed world model before proceeding to another task
00048 int32 FAILED = -2
00049
00050 # a lower level error prevented task completion (e.g. joint controller not responding)
00051 # generally requires human attention
00052 int32 ERROR = -3
00053
00054 # means that at some point during execution we ended up in a state that the collision-aware
00055 # arm navigation module will not move out of. The world was likely not disturbed, but you
00056 # probably need a new collision map to move the arm out of the stuck position
00057 int32 ARM_MOVEMENT_PREVENTED = -4
00058
00059 # specific to grasp actions
00060 # the object was grasped successfully, but the lift attempt could not achieve the minimum lift distance requested
00061 # it is likely that the collision environment will see collisions between the hand/object and the support surface
00062 int32 LIFT_FAILED = -5
00063
00064 # specific to place actions
00065 # the object was placed successfully, but the retreat attempt could not achieve the minimum retreat distance requested
00066 # it is likely that the collision environment will see collisions between the hand and the object
00067 int32 RETREAT_FAILED = -6
00068
00069 # indicates that somewhere along the line a human said "wait, stop, this is bad, go back and do something else"
00070 int32 CANCELLED = -7
00071
00072 # the actual value of this error code
00073 int32 value
00074
00075 ================================================================================
00076 MSG: object_manipulation_msgs/Grasp
00077
00078 # The internal posture of the hand for the pre-grasp
00079 # only positions are used
00080 sensor_msgs/JointState pre_grasp_posture
00081
00082 # The internal posture of the hand for the grasp
00083 # positions and efforts are used
00084 sensor_msgs/JointState grasp_posture
00085
00086 # The position of the end-effector for the grasp relative to a reference frame
00087 # (that is always specified elsewhere, not in this message)
00088 geometry_msgs/Pose grasp_pose
00089
00090 # The estimated probability of success for this grasp
00091 float64 success_probability
00092
00093 # Debug flag to indicate that this grasp would be the best in its cluster
00094 bool cluster_rep
00095
00096 # how far the pre-grasp should ideally be away from the grasp
00097 float32 desired_approach_distance
00098
00099 # how much distance between pre-grasp and grasp must actually be feasible
00100 # for the grasp not to be rejected
00101 float32 min_approach_distance
00102
00103 # an optional list of obstacles that we have semantic information about
00104 # and that we expect might move in the course of executing this grasp
00105 # the grasp planner is expected to make sure they move in an OK way; during
00106 # execution, grasp executors will not check for collisions against these objects
00107 GraspableObject[] moved_obstacles
00108
00109 ================================================================================
00110 MSG: sensor_msgs/JointState
00111 # This is a message that holds data to describe the state of a set of torque controlled joints.
00112 #
00113 # The state of each joint (revolute or prismatic) is defined by:
00114 # * the position of the joint (rad or m),
00115 # * the velocity of the joint (rad/s or m/s) and
00116 # * the effort that is applied in the joint (Nm or N).
00117 #
00118 # Each joint is uniquely identified by its name
00119 # The header specifies the time at which the joint states were recorded. All the joint states
00120 # in one message have to be recorded at the same time.
00121 #
00122 # This message consists of a multiple arrays, one for each part of the joint state.
00123 # The goal is to make each of the fields optional. When e.g. your joints have no
00124 # effort associated with them, you can leave the effort array empty.
00125 #
00126 # All arrays in this message should have the same size, or be empty.
00127 # This is the only way to uniquely associate the joint name with the correct
00128 # states.
00129
00130
00131 Header header
00132
00133 string[] name
00134 float64[] position
00135 float64[] velocity
00136 float64[] effort
00137
00138 ================================================================================
00139 MSG: std_msgs/Header
00140 # Standard metadata for higher-level stamped data types.
00141 # This is generally used to communicate timestamped data
00142 # in a particular coordinate frame.
00143 #
00144 # sequence ID: consecutively increasing ID
00145 uint32 seq
00146 #Two-integer timestamp that is expressed as:
00147 # * stamp.secs: seconds (stamp_secs) since epoch
00148 # * stamp.nsecs: nanoseconds since stamp_secs
00149 # time-handling sugar is provided by the client library
00150 time stamp
00151 #Frame this data is associated with
00152 # 0: no frame
00153 # 1: global frame
00154 string frame_id
00155
00156 ================================================================================
00157 MSG: geometry_msgs/Pose
00158 # A representation of pose in free space, composed of postion and orientation.
00159 Point position
00160 Quaternion orientation
00161
00162 ================================================================================
00163 MSG: geometry_msgs/Point
00164 # This contains the position of a point in free space
00165 float64 x
00166 float64 y
00167 float64 z
00168
00169 ================================================================================
00170 MSG: geometry_msgs/Quaternion
00171 # This represents an orientation in free space in quaternion form.
00172
00173 float64 x
00174 float64 y
00175 float64 z
00176 float64 w
00177
00178 ================================================================================
00179 MSG: object_manipulation_msgs/GraspableObject
00180 # an object that the object_manipulator can work on
00181
00182 # a graspable object can be represented in multiple ways. This message
00183 # can contain all of them. Which one is actually used is up to the receiver
00184 # of this message. When adding new representations, one must be careful that
00185 # they have reasonable lightweight defaults indicating that that particular
00186 # representation is not available.
00187
00188 # the tf frame to be used as a reference frame when combining information from
00189 # the different representations below
00190 string reference_frame_id
00191
00192 # potential recognition results from a database of models
00193 # all poses are relative to the object reference pose
00194 household_objects_database_msgs/DatabaseModelPose[] potential_models
00195
00196 # the point cloud itself
00197 sensor_msgs/PointCloud cluster
00198
00199 # a region of a PointCloud2 of interest
00200 object_manipulation_msgs/SceneRegion region
00201
00202 # the name that this object has in the collision environment
00203 string collision_name
00204 ================================================================================
00205 MSG: household_objects_database_msgs/DatabaseModelPose
00206 # Informs that a specific model from the Model Database has been
00207 # identified at a certain location
00208
00209 # the database id of the model
00210 int32 model_id
00211
00212 # the pose that it can be found in
00213 geometry_msgs/PoseStamped pose
00214
00215 # a measure of the confidence level in this detection result
00216 float32 confidence
00217
00218 # the name of the object detector that generated this detection result
00219 string detector_name
00220
00221 ================================================================================
00222 MSG: geometry_msgs/PoseStamped
00223 # A Pose with reference coordinate frame and timestamp
00224 Header header
00225 Pose pose
00226
00227 ================================================================================
00228 MSG: sensor_msgs/PointCloud
00229 # This message holds a collection of 3d points, plus optional additional
00230 # information about each point.
00231
00232 # Time of sensor data acquisition, coordinate frame ID.
00233 Header header
00234
00235 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00236 # in the frame given in the header.
00237 geometry_msgs/Point32[] points
00238
00239 # Each channel should have the same number of elements as points array,
00240 # and the data in each channel should correspond 1:1 with each point.
00241 # Channel names in common practice are listed in ChannelFloat32.msg.
00242 ChannelFloat32[] channels
00243
00244 ================================================================================
00245 MSG: geometry_msgs/Point32
00246 # This contains the position of a point in free space(with 32 bits of precision).
00247 # It is recommeded to use Point wherever possible instead of Point32.
00248 #
00249 # This recommendation is to promote interoperability.
00250 #
00251 # This message is designed to take up less space when sending
00252 # lots of points at once, as in the case of a PointCloud.
00253
00254 float32 x
00255 float32 y
00256 float32 z
00257 ================================================================================
00258 MSG: sensor_msgs/ChannelFloat32
00259 # This message is used by the PointCloud message to hold optional data
00260 # associated with each point in the cloud. The length of the values
00261 # array should be the same as the length of the points array in the
00262 # PointCloud, and each value should be associated with the corresponding
00263 # point.
00264
00265 # Channel names in existing practice include:
00266 # "u", "v" - row and column (respectively) in the left stereo image.
00267 # This is opposite to usual conventions but remains for
00268 # historical reasons. The newer PointCloud2 message has no
00269 # such problem.
00270 # "rgb" - For point clouds produced by color stereo cameras. uint8
00271 # (R,G,B) values packed into the least significant 24 bits,
00272 # in order.
00273 # "intensity" - laser or pixel intensity.
00274 # "distance"
00275
00276 # The channel name should give semantics of the channel (e.g.
00277 # "intensity" instead of "value").
00278 string name
00279
00280 # The values array should be 1-1 with the elements of the associated
00281 # PointCloud.
00282 float32[] values
00283
00284 ================================================================================
00285 MSG: object_manipulation_msgs/SceneRegion
00286 # Point cloud
00287 sensor_msgs/PointCloud2 cloud
00288
00289 # Indices for the region of interest
00290 int32[] mask
00291
00292 # One of the corresponding 2D images, if applicable
00293 sensor_msgs/Image image
00294
00295 # The disparity image, if applicable
00296 sensor_msgs/Image disparity_image
00297
00298 # Camera info for the camera that took the image
00299 sensor_msgs/CameraInfo cam_info
00300
00301 # a 3D region of interest for grasp planning
00302 geometry_msgs/PoseStamped roi_box_pose
00303 geometry_msgs/Vector3 roi_box_dims
00304
00305 ================================================================================
00306 MSG: sensor_msgs/PointCloud2
00307 # This message holds a collection of N-dimensional points, which may
00308 # contain additional information such as normals, intensity, etc. The
00309 # point data is stored as a binary blob, its layout described by the
00310 # contents of the "fields" array.
00311
00312 # The point cloud data may be organized 2d (image-like) or 1d
00313 # (unordered). Point clouds organized as 2d images may be produced by
00314 # camera depth sensors such as stereo or time-of-flight.
00315
00316 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00317 # points).
00318 Header header
00319
00320 # 2D structure of the point cloud. If the cloud is unordered, height is
00321 # 1 and width is the length of the point cloud.
00322 uint32 height
00323 uint32 width
00324
00325 # Describes the channels and their layout in the binary data blob.
00326 PointField[] fields
00327
00328 bool is_bigendian # Is this data bigendian?
00329 uint32 point_step # Length of a point in bytes
00330 uint32 row_step # Length of a row in bytes
00331 uint8[] data # Actual point data, size is (row_step*height)
00332
00333 bool is_dense # True if there are no invalid points
00334
00335 ================================================================================
00336 MSG: sensor_msgs/PointField
00337 # This message holds the description of one point entry in the
00338 # PointCloud2 message format.
00339 uint8 INT8 = 1
00340 uint8 UINT8 = 2
00341 uint8 INT16 = 3
00342 uint8 UINT16 = 4
00343 uint8 INT32 = 5
00344 uint8 UINT32 = 6
00345 uint8 FLOAT32 = 7
00346 uint8 FLOAT64 = 8
00347
00348 string name # Name of field
00349 uint32 offset # Offset from start of point struct
00350 uint8 datatype # Datatype enumeration, see above
00351 uint32 count # How many elements in the field
00352
00353 ================================================================================
00354 MSG: sensor_msgs/Image
00355 # This message contains an uncompressed image
00356 # (0, 0) is at top-left corner of image
00357 #
00358
00359 Header header # Header timestamp should be acquisition time of image
00360 # Header frame_id should be optical frame of camera
00361 # origin of frame should be optical center of cameara
00362 # +x should point to the right in the image
00363 # +y should point down in the image
00364 # +z should point into to plane of the image
00365 # If the frame_id here and the frame_id of the CameraInfo
00366 # message associated with the image conflict
00367 # the behavior is undefined
00368
00369 uint32 height # image height, that is, number of rows
00370 uint32 width # image width, that is, number of columns
00371
00372 # The legal values for encoding are in file src/image_encodings.cpp
00373 # If you want to standardize a new string format, join
00374 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00375
00376 string encoding # Encoding of pixels -- channel meaning, ordering, size
00377 # taken from the list of strings in src/image_encodings.cpp
00378
00379 uint8 is_bigendian # is this data bigendian?
00380 uint32 step # Full row length in bytes
00381 uint8[] data # actual matrix data, size is (step * rows)
00382
00383 ================================================================================
00384 MSG: sensor_msgs/CameraInfo
00385 # This message defines meta information for a camera. It should be in a
00386 # camera namespace on topic "camera_info" and accompanied by up to five
00387 # image topics named:
00388 #
00389 # image_raw - raw data from the camera driver, possibly Bayer encoded
00390 # image - monochrome, distorted
00391 # image_color - color, distorted
00392 # image_rect - monochrome, rectified
00393 # image_rect_color - color, rectified
00394 #
00395 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00396 # for producing the four processed image topics from image_raw and
00397 # camera_info. The meaning of the camera parameters are described in
00398 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00399 #
00400 # The image_geometry package provides a user-friendly interface to
00401 # common operations using this meta information. If you want to, e.g.,
00402 # project a 3d point into image coordinates, we strongly recommend
00403 # using image_geometry.
00404 #
00405 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00406 # zeroed out. In particular, clients may assume that K[0] == 0.0
00407 # indicates an uncalibrated camera.
00408
00409 #######################################################################
00410 # Image acquisition info #
00411 #######################################################################
00412
00413 # Time of image acquisition, camera coordinate frame ID
00414 Header header # Header timestamp should be acquisition time of image
00415 # Header frame_id should be optical frame of camera
00416 # origin of frame should be optical center of camera
00417 # +x should point to the right in the image
00418 # +y should point down in the image
00419 # +z should point into the plane of the image
00420
00421
00422 #######################################################################
00423 # Calibration Parameters #
00424 #######################################################################
00425 # These are fixed during camera calibration. Their values will be the #
00426 # same in all messages until the camera is recalibrated. Note that #
00427 # self-calibrating systems may "recalibrate" frequently. #
00428 # #
00429 # The internal parameters can be used to warp a raw (distorted) image #
00430 # to: #
00431 # 1. An undistorted image (requires D and K) #
00432 # 2. A rectified image (requires D, K, R) #
00433 # The projection matrix P projects 3D points into the rectified image.#
00434 #######################################################################
00435
00436 # The image dimensions with which the camera was calibrated. Normally
00437 # this will be the full camera resolution in pixels.
00438 uint32 height
00439 uint32 width
00440
00441 # The distortion model used. Supported models are listed in
00442 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00443 # simple model of radial and tangential distortion - is sufficent.
00444 string distortion_model
00445
00446 # The distortion parameters, size depending on the distortion model.
00447 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00448 float64[] D
00449
00450 # Intrinsic camera matrix for the raw (distorted) images.
00451 # [fx 0 cx]
00452 # K = [ 0 fy cy]
00453 # [ 0 0 1]
00454 # Projects 3D points in the camera coordinate frame to 2D pixel
00455 # coordinates using the focal lengths (fx, fy) and principal point
00456 # (cx, cy).
00457 float64[9] K # 3x3 row-major matrix
00458
00459 # Rectification matrix (stereo cameras only)
00460 # A rotation matrix aligning the camera coordinate system to the ideal
00461 # stereo image plane so that epipolar lines in both stereo images are
00462 # parallel.
00463 float64[9] R # 3x3 row-major matrix
00464
00465 # Projection/camera matrix
00466 # [fx' 0 cx' Tx]
00467 # P = [ 0 fy' cy' Ty]
00468 # [ 0 0 1 0]
00469 # By convention, this matrix specifies the intrinsic (camera) matrix
00470 # of the processed (rectified) image. That is, the left 3x3 portion
00471 # is the normal camera intrinsic matrix for the rectified image.
00472 # It projects 3D points in the camera coordinate frame to 2D pixel
00473 # coordinates using the focal lengths (fx', fy') and principal point
00474 # (cx', cy') - these may differ from the values in K.
00475 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00476 # also have R = the identity and P[1:3,1:3] = K.
00477 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00478 # position of the optical center of the second camera in the first
00479 # camera's frame. We assume Tz = 0 so both cameras are in the same
00480 # stereo image plane. The first camera always has Tx = Ty = 0. For
00481 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00482 # Tx = -fx' * B, where B is the baseline between the cameras.
00483 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00484 # the rectified image is given by:
00485 # [u v w]' = P * [X Y Z 1]'
00486 # x = u / w
00487 # y = v / w
00488 # This holds for both images of a stereo pair.
00489 float64[12] P # 3x4 row-major matrix
00490
00491
00492 #######################################################################
00493 # Operational Parameters #
00494 #######################################################################
00495 # These define the image region actually captured by the camera #
00496 # driver. Although they affect the geometry of the output image, they #
00497 # may be changed freely without recalibrating the camera. #
00498 #######################################################################
00499
00500 # Binning refers here to any camera setting which combines rectangular
00501 # neighborhoods of pixels into larger "super-pixels." It reduces the
00502 # resolution of the output image to
00503 # (width / binning_x) x (height / binning_y).
00504 # The default values binning_x = binning_y = 0 is considered the same
00505 # as binning_x = binning_y = 1 (no subsampling).
00506 uint32 binning_x
00507 uint32 binning_y
00508
00509 # Region of interest (subwindow of full camera resolution), given in
00510 # full resolution (unbinned) image coordinates. A particular ROI
00511 # always denotes the same window of pixels on the camera sensor,
00512 # regardless of binning settings.
00513 # The default setting of roi (all values 0) is considered the same as
00514 # full resolution (roi.width = width, roi.height = height).
00515 RegionOfInterest roi
00516
00517 ================================================================================
00518 MSG: sensor_msgs/RegionOfInterest
00519 # This message is used to specify a region of interest within an image.
00520 #
00521 # When used to specify the ROI setting of the camera when the image was
00522 # taken, the height and width fields should either match the height and
00523 # width fields for the associated image; or height = width = 0
00524 # indicates that the full resolution image was captured.
00525
00526 uint32 x_offset # Leftmost pixel of the ROI
00527 # (0 if the ROI includes the left edge of the image)
00528 uint32 y_offset # Topmost pixel of the ROI
00529 # (0 if the ROI includes the top edge of the image)
00530 uint32 height # Height of ROI
00531 uint32 width # Width of ROI
00532
00533 # True if a distinct rectified ROI should be calculated from the "raw"
00534 # ROI in this message. Typically this should be False if the full image
00535 # is captured (ROI not used), and True if a subwindow is captured (ROI
00536 # used).
00537 bool do_rectify
00538
00539 ================================================================================
00540 MSG: geometry_msgs/Vector3
00541 # This represents a vector in free space.
00542
00543 float64 x
00544 float64 y
00545 float64 z
00546 ================================================================================
00547 MSG: object_manipulation_msgs/GraspResult
00548 int32 SUCCESS = 1
00549 int32 GRASP_OUT_OF_REACH = 2
00550 int32 GRASP_IN_COLLISION = 3
00551 int32 GRASP_UNFEASIBLE = 4
00552 int32 PREGRASP_OUT_OF_REACH = 5
00553 int32 PREGRASP_IN_COLLISION = 6
00554 int32 PREGRASP_UNFEASIBLE = 7
00555 int32 LIFT_OUT_OF_REACH = 8
00556 int32 LIFT_IN_COLLISION = 9
00557 int32 LIFT_UNFEASIBLE = 10
00558 int32 MOVE_ARM_FAILED = 11
00559 int32 GRASP_FAILED = 12
00560 int32 LIFT_FAILED = 13
00561 int32 RETREAT_FAILED = 14
00562 int32 result_code
00563
00564 # whether the state of the world was disturbed by this attempt. generally, this flag
00565 # shows if another task can be attempted, or a new sensed world model is recommeded
00566 # before proceeding
00567 bool continuation_possible
00568
00569 """
00570 __slots__ = ['manipulation_result','grasp','attempted_grasps','attempted_grasp_results']
00571 _slot_types = ['object_manipulation_msgs/ManipulationResult','object_manipulation_msgs/Grasp','object_manipulation_msgs/Grasp[]','object_manipulation_msgs/GraspResult[]']
00572
00573 def __init__(self, *args, **kwds):
00574 """
00575 Constructor. Any message fields that are implicitly/explicitly
00576 set to None will be assigned a default value. The recommend
00577 use is keyword arguments as this is more robust to future message
00578 changes. You cannot mix in-order arguments and keyword arguments.
00579
00580 The available fields are:
00581 manipulation_result,grasp,attempted_grasps,attempted_grasp_results
00582
00583 :param args: complete set of field values, in .msg order
00584 :param kwds: use keyword arguments corresponding to message field names
00585 to set specific fields.
00586 """
00587 if args or kwds:
00588 super(PickupResult, self).__init__(*args, **kwds)
00589 #message fields cannot be None, assign default values for those that are
00590 if self.manipulation_result is None:
00591 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00592 if self.grasp is None:
00593 self.grasp = object_manipulation_msgs.msg.Grasp()
00594 if self.attempted_grasps is None:
00595 self.attempted_grasps = []
00596 if self.attempted_grasp_results is None:
00597 self.attempted_grasp_results = []
00598 else:
00599 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
00600 self.grasp = object_manipulation_msgs.msg.Grasp()
00601 self.attempted_grasps = []
00602 self.attempted_grasp_results = []
00603
00604 def _get_types(self):
00605 """
00606 internal API method
00607 """
00608 return self._slot_types
00609
00610 def serialize(self, buff):
00611 """
00612 serialize message into buffer
00613 :param buff: buffer, ``StringIO``
00614 """
00615 try:
00616 _x = self
00617 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
00618 _x = self.grasp.pre_grasp_posture.header.frame_id
00619 length = len(_x)
00620 if python3 or type(_x) == unicode:
00621 _x = _x.encode('utf-8')
00622 length = len(_x)
00623 buff.write(struct.pack('<I%ss'%length, length, _x))
00624 length = len(self.grasp.pre_grasp_posture.name)
00625 buff.write(_struct_I.pack(length))
00626 for val1 in self.grasp.pre_grasp_posture.name:
00627 length = len(val1)
00628 if python3 or type(val1) == unicode:
00629 val1 = val1.encode('utf-8')
00630 length = len(val1)
00631 buff.write(struct.pack('<I%ss'%length, length, val1))
00632 length = len(self.grasp.pre_grasp_posture.position)
00633 buff.write(_struct_I.pack(length))
00634 pattern = '<%sd'%length
00635 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.position))
00636 length = len(self.grasp.pre_grasp_posture.velocity)
00637 buff.write(_struct_I.pack(length))
00638 pattern = '<%sd'%length
00639 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.velocity))
00640 length = len(self.grasp.pre_grasp_posture.effort)
00641 buff.write(_struct_I.pack(length))
00642 pattern = '<%sd'%length
00643 buff.write(struct.pack(pattern, *self.grasp.pre_grasp_posture.effort))
00644 _x = self
00645 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
00646 _x = self.grasp.grasp_posture.header.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 length = len(self.grasp.grasp_posture.name)
00653 buff.write(_struct_I.pack(length))
00654 for val1 in self.grasp.grasp_posture.name:
00655 length = len(val1)
00656 if python3 or type(val1) == unicode:
00657 val1 = val1.encode('utf-8')
00658 length = len(val1)
00659 buff.write(struct.pack('<I%ss'%length, length, val1))
00660 length = len(self.grasp.grasp_posture.position)
00661 buff.write(_struct_I.pack(length))
00662 pattern = '<%sd'%length
00663 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.position))
00664 length = len(self.grasp.grasp_posture.velocity)
00665 buff.write(_struct_I.pack(length))
00666 pattern = '<%sd'%length
00667 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.velocity))
00668 length = len(self.grasp.grasp_posture.effort)
00669 buff.write(_struct_I.pack(length))
00670 pattern = '<%sd'%length
00671 buff.write(struct.pack(pattern, *self.grasp.grasp_posture.effort))
00672 _x = self
00673 buff.write(_struct_8dB2f.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance))
00674 length = len(self.grasp.moved_obstacles)
00675 buff.write(_struct_I.pack(length))
00676 for val1 in self.grasp.moved_obstacles:
00677 _x = val1.reference_frame_id
00678 length = len(_x)
00679 if python3 or type(_x) == unicode:
00680 _x = _x.encode('utf-8')
00681 length = len(_x)
00682 buff.write(struct.pack('<I%ss'%length, length, _x))
00683 length = len(val1.potential_models)
00684 buff.write(_struct_I.pack(length))
00685 for val2 in val1.potential_models:
00686 buff.write(_struct_i.pack(val2.model_id))
00687 _v1 = val2.pose
00688 _v2 = _v1.header
00689 buff.write(_struct_I.pack(_v2.seq))
00690 _v3 = _v2.stamp
00691 _x = _v3
00692 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00693 _x = _v2.frame_id
00694 length = len(_x)
00695 if python3 or type(_x) == unicode:
00696 _x = _x.encode('utf-8')
00697 length = len(_x)
00698 buff.write(struct.pack('<I%ss'%length, length, _x))
00699 _v4 = _v1.pose
00700 _v5 = _v4.position
00701 _x = _v5
00702 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00703 _v6 = _v4.orientation
00704 _x = _v6
00705 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00706 buff.write(_struct_f.pack(val2.confidence))
00707 _x = val2.detector_name
00708 length = len(_x)
00709 if python3 or type(_x) == unicode:
00710 _x = _x.encode('utf-8')
00711 length = len(_x)
00712 buff.write(struct.pack('<I%ss'%length, length, _x))
00713 _v7 = val1.cluster
00714 _v8 = _v7.header
00715 buff.write(_struct_I.pack(_v8.seq))
00716 _v9 = _v8.stamp
00717 _x = _v9
00718 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00719 _x = _v8.frame_id
00720 length = len(_x)
00721 if python3 or type(_x) == unicode:
00722 _x = _x.encode('utf-8')
00723 length = len(_x)
00724 buff.write(struct.pack('<I%ss'%length, length, _x))
00725 length = len(_v7.points)
00726 buff.write(_struct_I.pack(length))
00727 for val3 in _v7.points:
00728 _x = val3
00729 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00730 length = len(_v7.channels)
00731 buff.write(_struct_I.pack(length))
00732 for val3 in _v7.channels:
00733 _x = val3.name
00734 length = len(_x)
00735 if python3 or type(_x) == unicode:
00736 _x = _x.encode('utf-8')
00737 length = len(_x)
00738 buff.write(struct.pack('<I%ss'%length, length, _x))
00739 length = len(val3.values)
00740 buff.write(_struct_I.pack(length))
00741 pattern = '<%sf'%length
00742 buff.write(struct.pack(pattern, *val3.values))
00743 _v10 = val1.region
00744 _v11 = _v10.cloud
00745 _v12 = _v11.header
00746 buff.write(_struct_I.pack(_v12.seq))
00747 _v13 = _v12.stamp
00748 _x = _v13
00749 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00750 _x = _v12.frame_id
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 = _v11
00757 buff.write(_struct_2I.pack(_x.height, _x.width))
00758 length = len(_v11.fields)
00759 buff.write(_struct_I.pack(length))
00760 for val4 in _v11.fields:
00761 _x = val4.name
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 = val4
00768 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00769 _x = _v11
00770 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
00771 _x = _v11.data
00772 length = len(_x)
00773 # - if encoded as a list instead, serialize as bytes instead of string
00774 if type(_x) in [list, tuple]:
00775 buff.write(struct.pack('<I%sB'%length, length, *_x))
00776 else:
00777 buff.write(struct.pack('<I%ss'%length, length, _x))
00778 buff.write(_struct_B.pack(_v11.is_dense))
00779 length = len(_v10.mask)
00780 buff.write(_struct_I.pack(length))
00781 pattern = '<%si'%length
00782 buff.write(struct.pack(pattern, *_v10.mask))
00783 _v14 = _v10.image
00784 _v15 = _v14.header
00785 buff.write(_struct_I.pack(_v15.seq))
00786 _v16 = _v15.stamp
00787 _x = _v16
00788 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00789 _x = _v15.frame_id
00790 length = len(_x)
00791 if python3 or type(_x) == unicode:
00792 _x = _x.encode('utf-8')
00793 length = len(_x)
00794 buff.write(struct.pack('<I%ss'%length, length, _x))
00795 _x = _v14
00796 buff.write(_struct_2I.pack(_x.height, _x.width))
00797 _x = _v14.encoding
00798 length = len(_x)
00799 if python3 or type(_x) == unicode:
00800 _x = _x.encode('utf-8')
00801 length = len(_x)
00802 buff.write(struct.pack('<I%ss'%length, length, _x))
00803 _x = _v14
00804 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00805 _x = _v14.data
00806 length = len(_x)
00807 # - if encoded as a list instead, serialize as bytes instead of string
00808 if type(_x) in [list, tuple]:
00809 buff.write(struct.pack('<I%sB'%length, length, *_x))
00810 else:
00811 buff.write(struct.pack('<I%ss'%length, length, _x))
00812 _v17 = _v10.disparity_image
00813 _v18 = _v17.header
00814 buff.write(_struct_I.pack(_v18.seq))
00815 _v19 = _v18.stamp
00816 _x = _v19
00817 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00818 _x = _v18.frame_id
00819 length = len(_x)
00820 if python3 or type(_x) == unicode:
00821 _x = _x.encode('utf-8')
00822 length = len(_x)
00823 buff.write(struct.pack('<I%ss'%length, length, _x))
00824 _x = _v17
00825 buff.write(_struct_2I.pack(_x.height, _x.width))
00826 _x = _v17.encoding
00827 length = len(_x)
00828 if python3 or type(_x) == unicode:
00829 _x = _x.encode('utf-8')
00830 length = len(_x)
00831 buff.write(struct.pack('<I%ss'%length, length, _x))
00832 _x = _v17
00833 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
00834 _x = _v17.data
00835 length = len(_x)
00836 # - if encoded as a list instead, serialize as bytes instead of string
00837 if type(_x) in [list, tuple]:
00838 buff.write(struct.pack('<I%sB'%length, length, *_x))
00839 else:
00840 buff.write(struct.pack('<I%ss'%length, length, _x))
00841 _v20 = _v10.cam_info
00842 _v21 = _v20.header
00843 buff.write(_struct_I.pack(_v21.seq))
00844 _v22 = _v21.stamp
00845 _x = _v22
00846 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00847 _x = _v21.frame_id
00848 length = len(_x)
00849 if python3 or type(_x) == unicode:
00850 _x = _x.encode('utf-8')
00851 length = len(_x)
00852 buff.write(struct.pack('<I%ss'%length, length, _x))
00853 _x = _v20
00854 buff.write(_struct_2I.pack(_x.height, _x.width))
00855 _x = _v20.distortion_model
00856 length = len(_x)
00857 if python3 or type(_x) == unicode:
00858 _x = _x.encode('utf-8')
00859 length = len(_x)
00860 buff.write(struct.pack('<I%ss'%length, length, _x))
00861 length = len(_v20.D)
00862 buff.write(_struct_I.pack(length))
00863 pattern = '<%sd'%length
00864 buff.write(struct.pack(pattern, *_v20.D))
00865 buff.write(_struct_9d.pack(*_v20.K))
00866 buff.write(_struct_9d.pack(*_v20.R))
00867 buff.write(_struct_12d.pack(*_v20.P))
00868 _x = _v20
00869 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
00870 _v23 = _v20.roi
00871 _x = _v23
00872 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
00873 _v24 = _v10.roi_box_pose
00874 _v25 = _v24.header
00875 buff.write(_struct_I.pack(_v25.seq))
00876 _v26 = _v25.stamp
00877 _x = _v26
00878 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00879 _x = _v25.frame_id
00880 length = len(_x)
00881 if python3 or type(_x) == unicode:
00882 _x = _x.encode('utf-8')
00883 length = len(_x)
00884 buff.write(struct.pack('<I%ss'%length, length, _x))
00885 _v27 = _v24.pose
00886 _v28 = _v27.position
00887 _x = _v28
00888 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00889 _v29 = _v27.orientation
00890 _x = _v29
00891 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00892 _v30 = _v10.roi_box_dims
00893 _x = _v30
00894 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00895 _x = val1.collision_name
00896 length = len(_x)
00897 if python3 or type(_x) == unicode:
00898 _x = _x.encode('utf-8')
00899 length = len(_x)
00900 buff.write(struct.pack('<I%ss'%length, length, _x))
00901 length = len(self.attempted_grasps)
00902 buff.write(_struct_I.pack(length))
00903 for val1 in self.attempted_grasps:
00904 _v31 = val1.pre_grasp_posture
00905 _v32 = _v31.header
00906 buff.write(_struct_I.pack(_v32.seq))
00907 _v33 = _v32.stamp
00908 _x = _v33
00909 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00910 _x = _v32.frame_id
00911 length = len(_x)
00912 if python3 or type(_x) == unicode:
00913 _x = _x.encode('utf-8')
00914 length = len(_x)
00915 buff.write(struct.pack('<I%ss'%length, length, _x))
00916 length = len(_v31.name)
00917 buff.write(_struct_I.pack(length))
00918 for val3 in _v31.name:
00919 length = len(val3)
00920 if python3 or type(val3) == unicode:
00921 val3 = val3.encode('utf-8')
00922 length = len(val3)
00923 buff.write(struct.pack('<I%ss'%length, length, val3))
00924 length = len(_v31.position)
00925 buff.write(_struct_I.pack(length))
00926 pattern = '<%sd'%length
00927 buff.write(struct.pack(pattern, *_v31.position))
00928 length = len(_v31.velocity)
00929 buff.write(_struct_I.pack(length))
00930 pattern = '<%sd'%length
00931 buff.write(struct.pack(pattern, *_v31.velocity))
00932 length = len(_v31.effort)
00933 buff.write(_struct_I.pack(length))
00934 pattern = '<%sd'%length
00935 buff.write(struct.pack(pattern, *_v31.effort))
00936 _v34 = val1.grasp_posture
00937 _v35 = _v34.header
00938 buff.write(_struct_I.pack(_v35.seq))
00939 _v36 = _v35.stamp
00940 _x = _v36
00941 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00942 _x = _v35.frame_id
00943 length = len(_x)
00944 if python3 or type(_x) == unicode:
00945 _x = _x.encode('utf-8')
00946 length = len(_x)
00947 buff.write(struct.pack('<I%ss'%length, length, _x))
00948 length = len(_v34.name)
00949 buff.write(_struct_I.pack(length))
00950 for val3 in _v34.name:
00951 length = len(val3)
00952 if python3 or type(val3) == unicode:
00953 val3 = val3.encode('utf-8')
00954 length = len(val3)
00955 buff.write(struct.pack('<I%ss'%length, length, val3))
00956 length = len(_v34.position)
00957 buff.write(_struct_I.pack(length))
00958 pattern = '<%sd'%length
00959 buff.write(struct.pack(pattern, *_v34.position))
00960 length = len(_v34.velocity)
00961 buff.write(_struct_I.pack(length))
00962 pattern = '<%sd'%length
00963 buff.write(struct.pack(pattern, *_v34.velocity))
00964 length = len(_v34.effort)
00965 buff.write(_struct_I.pack(length))
00966 pattern = '<%sd'%length
00967 buff.write(struct.pack(pattern, *_v34.effort))
00968 _v37 = val1.grasp_pose
00969 _v38 = _v37.position
00970 _x = _v38
00971 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00972 _v39 = _v37.orientation
00973 _x = _v39
00974 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00975 _x = val1
00976 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
00977 length = len(val1.moved_obstacles)
00978 buff.write(_struct_I.pack(length))
00979 for val2 in val1.moved_obstacles:
00980 _x = val2.reference_frame_id
00981 length = len(_x)
00982 if python3 or type(_x) == unicode:
00983 _x = _x.encode('utf-8')
00984 length = len(_x)
00985 buff.write(struct.pack('<I%ss'%length, length, _x))
00986 length = len(val2.potential_models)
00987 buff.write(_struct_I.pack(length))
00988 for val3 in val2.potential_models:
00989 buff.write(_struct_i.pack(val3.model_id))
00990 _v40 = val3.pose
00991 _v41 = _v40.header
00992 buff.write(_struct_I.pack(_v41.seq))
00993 _v42 = _v41.stamp
00994 _x = _v42
00995 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00996 _x = _v41.frame_id
00997 length = len(_x)
00998 if python3 or type(_x) == unicode:
00999 _x = _x.encode('utf-8')
01000 length = len(_x)
01001 buff.write(struct.pack('<I%ss'%length, length, _x))
01002 _v43 = _v40.pose
01003 _v44 = _v43.position
01004 _x = _v44
01005 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01006 _v45 = _v43.orientation
01007 _x = _v45
01008 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01009 buff.write(_struct_f.pack(val3.confidence))
01010 _x = val3.detector_name
01011 length = len(_x)
01012 if python3 or type(_x) == unicode:
01013 _x = _x.encode('utf-8')
01014 length = len(_x)
01015 buff.write(struct.pack('<I%ss'%length, length, _x))
01016 _v46 = val2.cluster
01017 _v47 = _v46.header
01018 buff.write(_struct_I.pack(_v47.seq))
01019 _v48 = _v47.stamp
01020 _x = _v48
01021 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01022 _x = _v47.frame_id
01023 length = len(_x)
01024 if python3 or type(_x) == unicode:
01025 _x = _x.encode('utf-8')
01026 length = len(_x)
01027 buff.write(struct.pack('<I%ss'%length, length, _x))
01028 length = len(_v46.points)
01029 buff.write(_struct_I.pack(length))
01030 for val4 in _v46.points:
01031 _x = val4
01032 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01033 length = len(_v46.channels)
01034 buff.write(_struct_I.pack(length))
01035 for val4 in _v46.channels:
01036 _x = val4.name
01037 length = len(_x)
01038 if python3 or type(_x) == unicode:
01039 _x = _x.encode('utf-8')
01040 length = len(_x)
01041 buff.write(struct.pack('<I%ss'%length, length, _x))
01042 length = len(val4.values)
01043 buff.write(_struct_I.pack(length))
01044 pattern = '<%sf'%length
01045 buff.write(struct.pack(pattern, *val4.values))
01046 _v49 = val2.region
01047 _v50 = _v49.cloud
01048 _v51 = _v50.header
01049 buff.write(_struct_I.pack(_v51.seq))
01050 _v52 = _v51.stamp
01051 _x = _v52
01052 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01053 _x = _v51.frame_id
01054 length = len(_x)
01055 if python3 or type(_x) == unicode:
01056 _x = _x.encode('utf-8')
01057 length = len(_x)
01058 buff.write(struct.pack('<I%ss'%length, length, _x))
01059 _x = _v50
01060 buff.write(_struct_2I.pack(_x.height, _x.width))
01061 length = len(_v50.fields)
01062 buff.write(_struct_I.pack(length))
01063 for val5 in _v50.fields:
01064 _x = val5.name
01065 length = len(_x)
01066 if python3 or type(_x) == unicode:
01067 _x = _x.encode('utf-8')
01068 length = len(_x)
01069 buff.write(struct.pack('<I%ss'%length, length, _x))
01070 _x = val5
01071 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01072 _x = _v50
01073 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01074 _x = _v50.data
01075 length = len(_x)
01076 # - if encoded as a list instead, serialize as bytes instead of string
01077 if type(_x) in [list, tuple]:
01078 buff.write(struct.pack('<I%sB'%length, length, *_x))
01079 else:
01080 buff.write(struct.pack('<I%ss'%length, length, _x))
01081 buff.write(_struct_B.pack(_v50.is_dense))
01082 length = len(_v49.mask)
01083 buff.write(_struct_I.pack(length))
01084 pattern = '<%si'%length
01085 buff.write(struct.pack(pattern, *_v49.mask))
01086 _v53 = _v49.image
01087 _v54 = _v53.header
01088 buff.write(_struct_I.pack(_v54.seq))
01089 _v55 = _v54.stamp
01090 _x = _v55
01091 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01092 _x = _v54.frame_id
01093 length = len(_x)
01094 if python3 or type(_x) == unicode:
01095 _x = _x.encode('utf-8')
01096 length = len(_x)
01097 buff.write(struct.pack('<I%ss'%length, length, _x))
01098 _x = _v53
01099 buff.write(_struct_2I.pack(_x.height, _x.width))
01100 _x = _v53.encoding
01101 length = len(_x)
01102 if python3 or type(_x) == unicode:
01103 _x = _x.encode('utf-8')
01104 length = len(_x)
01105 buff.write(struct.pack('<I%ss'%length, length, _x))
01106 _x = _v53
01107 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01108 _x = _v53.data
01109 length = len(_x)
01110 # - if encoded as a list instead, serialize as bytes instead of string
01111 if type(_x) in [list, tuple]:
01112 buff.write(struct.pack('<I%sB'%length, length, *_x))
01113 else:
01114 buff.write(struct.pack('<I%ss'%length, length, _x))
01115 _v56 = _v49.disparity_image
01116 _v57 = _v56.header
01117 buff.write(_struct_I.pack(_v57.seq))
01118 _v58 = _v57.stamp
01119 _x = _v58
01120 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01121 _x = _v57.frame_id
01122 length = len(_x)
01123 if python3 or type(_x) == unicode:
01124 _x = _x.encode('utf-8')
01125 length = len(_x)
01126 buff.write(struct.pack('<I%ss'%length, length, _x))
01127 _x = _v56
01128 buff.write(_struct_2I.pack(_x.height, _x.width))
01129 _x = _v56.encoding
01130 length = len(_x)
01131 if python3 or type(_x) == unicode:
01132 _x = _x.encode('utf-8')
01133 length = len(_x)
01134 buff.write(struct.pack('<I%ss'%length, length, _x))
01135 _x = _v56
01136 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01137 _x = _v56.data
01138 length = len(_x)
01139 # - if encoded as a list instead, serialize as bytes instead of string
01140 if type(_x) in [list, tuple]:
01141 buff.write(struct.pack('<I%sB'%length, length, *_x))
01142 else:
01143 buff.write(struct.pack('<I%ss'%length, length, _x))
01144 _v59 = _v49.cam_info
01145 _v60 = _v59.header
01146 buff.write(_struct_I.pack(_v60.seq))
01147 _v61 = _v60.stamp
01148 _x = _v61
01149 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01150 _x = _v60.frame_id
01151 length = len(_x)
01152 if python3 or type(_x) == unicode:
01153 _x = _x.encode('utf-8')
01154 length = len(_x)
01155 buff.write(struct.pack('<I%ss'%length, length, _x))
01156 _x = _v59
01157 buff.write(_struct_2I.pack(_x.height, _x.width))
01158 _x = _v59.distortion_model
01159 length = len(_x)
01160 if python3 or type(_x) == unicode:
01161 _x = _x.encode('utf-8')
01162 length = len(_x)
01163 buff.write(struct.pack('<I%ss'%length, length, _x))
01164 length = len(_v59.D)
01165 buff.write(_struct_I.pack(length))
01166 pattern = '<%sd'%length
01167 buff.write(struct.pack(pattern, *_v59.D))
01168 buff.write(_struct_9d.pack(*_v59.K))
01169 buff.write(_struct_9d.pack(*_v59.R))
01170 buff.write(_struct_12d.pack(*_v59.P))
01171 _x = _v59
01172 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01173 _v62 = _v59.roi
01174 _x = _v62
01175 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01176 _v63 = _v49.roi_box_pose
01177 _v64 = _v63.header
01178 buff.write(_struct_I.pack(_v64.seq))
01179 _v65 = _v64.stamp
01180 _x = _v65
01181 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01182 _x = _v64.frame_id
01183 length = len(_x)
01184 if python3 or type(_x) == unicode:
01185 _x = _x.encode('utf-8')
01186 length = len(_x)
01187 buff.write(struct.pack('<I%ss'%length, length, _x))
01188 _v66 = _v63.pose
01189 _v67 = _v66.position
01190 _x = _v67
01191 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01192 _v68 = _v66.orientation
01193 _x = _v68
01194 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01195 _v69 = _v49.roi_box_dims
01196 _x = _v69
01197 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01198 _x = val2.collision_name
01199 length = len(_x)
01200 if python3 or type(_x) == unicode:
01201 _x = _x.encode('utf-8')
01202 length = len(_x)
01203 buff.write(struct.pack('<I%ss'%length, length, _x))
01204 length = len(self.attempted_grasp_results)
01205 buff.write(_struct_I.pack(length))
01206 for val1 in self.attempted_grasp_results:
01207 _x = val1
01208 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
01209 except struct.error as se: self._check_types(se)
01210 except TypeError as te: self._check_types(te)
01211
01212 def deserialize(self, str):
01213 """
01214 unpack serialized message in str into this message instance
01215 :param str: byte array of serialized message, ``str``
01216 """
01217 try:
01218 if self.manipulation_result is None:
01219 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
01220 if self.grasp is None:
01221 self.grasp = object_manipulation_msgs.msg.Grasp()
01222 if self.attempted_grasps is None:
01223 self.attempted_grasps = None
01224 if self.attempted_grasp_results is None:
01225 self.attempted_grasp_results = None
01226 end = 0
01227 _x = self
01228 start = end
01229 end += 16
01230 (_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
01231 start = end
01232 end += 4
01233 (length,) = _struct_I.unpack(str[start:end])
01234 start = end
01235 end += length
01236 if python3:
01237 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01238 else:
01239 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01240 start = end
01241 end += 4
01242 (length,) = _struct_I.unpack(str[start:end])
01243 self.grasp.pre_grasp_posture.name = []
01244 for i in range(0, length):
01245 start = end
01246 end += 4
01247 (length,) = _struct_I.unpack(str[start:end])
01248 start = end
01249 end += length
01250 if python3:
01251 val1 = str[start:end].decode('utf-8')
01252 else:
01253 val1 = str[start:end]
01254 self.grasp.pre_grasp_posture.name.append(val1)
01255 start = end
01256 end += 4
01257 (length,) = _struct_I.unpack(str[start:end])
01258 pattern = '<%sd'%length
01259 start = end
01260 end += struct.calcsize(pattern)
01261 self.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01262 start = end
01263 end += 4
01264 (length,) = _struct_I.unpack(str[start:end])
01265 pattern = '<%sd'%length
01266 start = end
01267 end += struct.calcsize(pattern)
01268 self.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01269 start = end
01270 end += 4
01271 (length,) = _struct_I.unpack(str[start:end])
01272 pattern = '<%sd'%length
01273 start = end
01274 end += struct.calcsize(pattern)
01275 self.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01276 _x = self
01277 start = end
01278 end += 12
01279 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01280 start = end
01281 end += 4
01282 (length,) = _struct_I.unpack(str[start:end])
01283 start = end
01284 end += length
01285 if python3:
01286 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01287 else:
01288 self.grasp.grasp_posture.header.frame_id = str[start:end]
01289 start = end
01290 end += 4
01291 (length,) = _struct_I.unpack(str[start:end])
01292 self.grasp.grasp_posture.name = []
01293 for i in range(0, length):
01294 start = end
01295 end += 4
01296 (length,) = _struct_I.unpack(str[start:end])
01297 start = end
01298 end += length
01299 if python3:
01300 val1 = str[start:end].decode('utf-8')
01301 else:
01302 val1 = str[start:end]
01303 self.grasp.grasp_posture.name.append(val1)
01304 start = end
01305 end += 4
01306 (length,) = _struct_I.unpack(str[start:end])
01307 pattern = '<%sd'%length
01308 start = end
01309 end += struct.calcsize(pattern)
01310 self.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
01311 start = end
01312 end += 4
01313 (length,) = _struct_I.unpack(str[start:end])
01314 pattern = '<%sd'%length
01315 start = end
01316 end += struct.calcsize(pattern)
01317 self.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01318 start = end
01319 end += 4
01320 (length,) = _struct_I.unpack(str[start:end])
01321 pattern = '<%sd'%length
01322 start = end
01323 end += struct.calcsize(pattern)
01324 self.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01325 _x = self
01326 start = end
01327 end += 73
01328 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end])
01329 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
01330 start = end
01331 end += 4
01332 (length,) = _struct_I.unpack(str[start:end])
01333 self.grasp.moved_obstacles = []
01334 for i in range(0, length):
01335 val1 = object_manipulation_msgs.msg.GraspableObject()
01336 start = end
01337 end += 4
01338 (length,) = _struct_I.unpack(str[start:end])
01339 start = end
01340 end += length
01341 if python3:
01342 val1.reference_frame_id = str[start:end].decode('utf-8')
01343 else:
01344 val1.reference_frame_id = str[start:end]
01345 start = end
01346 end += 4
01347 (length,) = _struct_I.unpack(str[start:end])
01348 val1.potential_models = []
01349 for i in range(0, length):
01350 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
01351 start = end
01352 end += 4
01353 (val2.model_id,) = _struct_i.unpack(str[start:end])
01354 _v70 = val2.pose
01355 _v71 = _v70.header
01356 start = end
01357 end += 4
01358 (_v71.seq,) = _struct_I.unpack(str[start:end])
01359 _v72 = _v71.stamp
01360 _x = _v72
01361 start = end
01362 end += 8
01363 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01364 start = end
01365 end += 4
01366 (length,) = _struct_I.unpack(str[start:end])
01367 start = end
01368 end += length
01369 if python3:
01370 _v71.frame_id = str[start:end].decode('utf-8')
01371 else:
01372 _v71.frame_id = str[start:end]
01373 _v73 = _v70.pose
01374 _v74 = _v73.position
01375 _x = _v74
01376 start = end
01377 end += 24
01378 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01379 _v75 = _v73.orientation
01380 _x = _v75
01381 start = end
01382 end += 32
01383 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01384 start = end
01385 end += 4
01386 (val2.confidence,) = _struct_f.unpack(str[start:end])
01387 start = end
01388 end += 4
01389 (length,) = _struct_I.unpack(str[start:end])
01390 start = end
01391 end += length
01392 if python3:
01393 val2.detector_name = str[start:end].decode('utf-8')
01394 else:
01395 val2.detector_name = str[start:end]
01396 val1.potential_models.append(val2)
01397 _v76 = val1.cluster
01398 _v77 = _v76.header
01399 start = end
01400 end += 4
01401 (_v77.seq,) = _struct_I.unpack(str[start:end])
01402 _v78 = _v77.stamp
01403 _x = _v78
01404 start = end
01405 end += 8
01406 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01407 start = end
01408 end += 4
01409 (length,) = _struct_I.unpack(str[start:end])
01410 start = end
01411 end += length
01412 if python3:
01413 _v77.frame_id = str[start:end].decode('utf-8')
01414 else:
01415 _v77.frame_id = str[start:end]
01416 start = end
01417 end += 4
01418 (length,) = _struct_I.unpack(str[start:end])
01419 _v76.points = []
01420 for i in range(0, length):
01421 val3 = geometry_msgs.msg.Point32()
01422 _x = val3
01423 start = end
01424 end += 12
01425 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01426 _v76.points.append(val3)
01427 start = end
01428 end += 4
01429 (length,) = _struct_I.unpack(str[start:end])
01430 _v76.channels = []
01431 for i in range(0, length):
01432 val3 = sensor_msgs.msg.ChannelFloat32()
01433 start = end
01434 end += 4
01435 (length,) = _struct_I.unpack(str[start:end])
01436 start = end
01437 end += length
01438 if python3:
01439 val3.name = str[start:end].decode('utf-8')
01440 else:
01441 val3.name = str[start:end]
01442 start = end
01443 end += 4
01444 (length,) = _struct_I.unpack(str[start:end])
01445 pattern = '<%sf'%length
01446 start = end
01447 end += struct.calcsize(pattern)
01448 val3.values = struct.unpack(pattern, str[start:end])
01449 _v76.channels.append(val3)
01450 _v79 = val1.region
01451 _v80 = _v79.cloud
01452 _v81 = _v80.header
01453 start = end
01454 end += 4
01455 (_v81.seq,) = _struct_I.unpack(str[start:end])
01456 _v82 = _v81.stamp
01457 _x = _v82
01458 start = end
01459 end += 8
01460 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01461 start = end
01462 end += 4
01463 (length,) = _struct_I.unpack(str[start:end])
01464 start = end
01465 end += length
01466 if python3:
01467 _v81.frame_id = str[start:end].decode('utf-8')
01468 else:
01469 _v81.frame_id = str[start:end]
01470 _x = _v80
01471 start = end
01472 end += 8
01473 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01474 start = end
01475 end += 4
01476 (length,) = _struct_I.unpack(str[start:end])
01477 _v80.fields = []
01478 for i in range(0, length):
01479 val4 = sensor_msgs.msg.PointField()
01480 start = end
01481 end += 4
01482 (length,) = _struct_I.unpack(str[start:end])
01483 start = end
01484 end += length
01485 if python3:
01486 val4.name = str[start:end].decode('utf-8')
01487 else:
01488 val4.name = str[start:end]
01489 _x = val4
01490 start = end
01491 end += 9
01492 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01493 _v80.fields.append(val4)
01494 _x = _v80
01495 start = end
01496 end += 9
01497 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
01498 _v80.is_bigendian = bool(_v80.is_bigendian)
01499 start = end
01500 end += 4
01501 (length,) = _struct_I.unpack(str[start:end])
01502 start = end
01503 end += length
01504 if python3:
01505 _v80.data = str[start:end].decode('utf-8')
01506 else:
01507 _v80.data = str[start:end]
01508 start = end
01509 end += 1
01510 (_v80.is_dense,) = _struct_B.unpack(str[start:end])
01511 _v80.is_dense = bool(_v80.is_dense)
01512 start = end
01513 end += 4
01514 (length,) = _struct_I.unpack(str[start:end])
01515 pattern = '<%si'%length
01516 start = end
01517 end += struct.calcsize(pattern)
01518 _v79.mask = struct.unpack(pattern, str[start:end])
01519 _v83 = _v79.image
01520 _v84 = _v83.header
01521 start = end
01522 end += 4
01523 (_v84.seq,) = _struct_I.unpack(str[start:end])
01524 _v85 = _v84.stamp
01525 _x = _v85
01526 start = end
01527 end += 8
01528 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01529 start = end
01530 end += 4
01531 (length,) = _struct_I.unpack(str[start:end])
01532 start = end
01533 end += length
01534 if python3:
01535 _v84.frame_id = str[start:end].decode('utf-8')
01536 else:
01537 _v84.frame_id = str[start:end]
01538 _x = _v83
01539 start = end
01540 end += 8
01541 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01542 start = end
01543 end += 4
01544 (length,) = _struct_I.unpack(str[start:end])
01545 start = end
01546 end += length
01547 if python3:
01548 _v83.encoding = str[start:end].decode('utf-8')
01549 else:
01550 _v83.encoding = str[start:end]
01551 _x = _v83
01552 start = end
01553 end += 5
01554 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01555 start = end
01556 end += 4
01557 (length,) = _struct_I.unpack(str[start:end])
01558 start = end
01559 end += length
01560 if python3:
01561 _v83.data = str[start:end].decode('utf-8')
01562 else:
01563 _v83.data = str[start:end]
01564 _v86 = _v79.disparity_image
01565 _v87 = _v86.header
01566 start = end
01567 end += 4
01568 (_v87.seq,) = _struct_I.unpack(str[start:end])
01569 _v88 = _v87.stamp
01570 _x = _v88
01571 start = end
01572 end += 8
01573 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01574 start = end
01575 end += 4
01576 (length,) = _struct_I.unpack(str[start:end])
01577 start = end
01578 end += length
01579 if python3:
01580 _v87.frame_id = str[start:end].decode('utf-8')
01581 else:
01582 _v87.frame_id = str[start:end]
01583 _x = _v86
01584 start = end
01585 end += 8
01586 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01587 start = end
01588 end += 4
01589 (length,) = _struct_I.unpack(str[start:end])
01590 start = end
01591 end += length
01592 if python3:
01593 _v86.encoding = str[start:end].decode('utf-8')
01594 else:
01595 _v86.encoding = str[start:end]
01596 _x = _v86
01597 start = end
01598 end += 5
01599 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
01600 start = end
01601 end += 4
01602 (length,) = _struct_I.unpack(str[start:end])
01603 start = end
01604 end += length
01605 if python3:
01606 _v86.data = str[start:end].decode('utf-8')
01607 else:
01608 _v86.data = str[start:end]
01609 _v89 = _v79.cam_info
01610 _v90 = _v89.header
01611 start = end
01612 end += 4
01613 (_v90.seq,) = _struct_I.unpack(str[start:end])
01614 _v91 = _v90.stamp
01615 _x = _v91
01616 start = end
01617 end += 8
01618 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01619 start = end
01620 end += 4
01621 (length,) = _struct_I.unpack(str[start:end])
01622 start = end
01623 end += length
01624 if python3:
01625 _v90.frame_id = str[start:end].decode('utf-8')
01626 else:
01627 _v90.frame_id = str[start:end]
01628 _x = _v89
01629 start = end
01630 end += 8
01631 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01632 start = end
01633 end += 4
01634 (length,) = _struct_I.unpack(str[start:end])
01635 start = end
01636 end += length
01637 if python3:
01638 _v89.distortion_model = str[start:end].decode('utf-8')
01639 else:
01640 _v89.distortion_model = str[start:end]
01641 start = end
01642 end += 4
01643 (length,) = _struct_I.unpack(str[start:end])
01644 pattern = '<%sd'%length
01645 start = end
01646 end += struct.calcsize(pattern)
01647 _v89.D = struct.unpack(pattern, str[start:end])
01648 start = end
01649 end += 72
01650 _v89.K = _struct_9d.unpack(str[start:end])
01651 start = end
01652 end += 72
01653 _v89.R = _struct_9d.unpack(str[start:end])
01654 start = end
01655 end += 96
01656 _v89.P = _struct_12d.unpack(str[start:end])
01657 _x = _v89
01658 start = end
01659 end += 8
01660 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
01661 _v92 = _v89.roi
01662 _x = _v92
01663 start = end
01664 end += 17
01665 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
01666 _v92.do_rectify = bool(_v92.do_rectify)
01667 _v93 = _v79.roi_box_pose
01668 _v94 = _v93.header
01669 start = end
01670 end += 4
01671 (_v94.seq,) = _struct_I.unpack(str[start:end])
01672 _v95 = _v94.stamp
01673 _x = _v95
01674 start = end
01675 end += 8
01676 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01677 start = end
01678 end += 4
01679 (length,) = _struct_I.unpack(str[start:end])
01680 start = end
01681 end += length
01682 if python3:
01683 _v94.frame_id = str[start:end].decode('utf-8')
01684 else:
01685 _v94.frame_id = str[start:end]
01686 _v96 = _v93.pose
01687 _v97 = _v96.position
01688 _x = _v97
01689 start = end
01690 end += 24
01691 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01692 _v98 = _v96.orientation
01693 _x = _v98
01694 start = end
01695 end += 32
01696 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01697 _v99 = _v79.roi_box_dims
01698 _x = _v99
01699 start = end
01700 end += 24
01701 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01702 start = end
01703 end += 4
01704 (length,) = _struct_I.unpack(str[start:end])
01705 start = end
01706 end += length
01707 if python3:
01708 val1.collision_name = str[start:end].decode('utf-8')
01709 else:
01710 val1.collision_name = str[start:end]
01711 self.grasp.moved_obstacles.append(val1)
01712 start = end
01713 end += 4
01714 (length,) = _struct_I.unpack(str[start:end])
01715 self.attempted_grasps = []
01716 for i in range(0, length):
01717 val1 = object_manipulation_msgs.msg.Grasp()
01718 _v100 = val1.pre_grasp_posture
01719 _v101 = _v100.header
01720 start = end
01721 end += 4
01722 (_v101.seq,) = _struct_I.unpack(str[start:end])
01723 _v102 = _v101.stamp
01724 _x = _v102
01725 start = end
01726 end += 8
01727 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01728 start = end
01729 end += 4
01730 (length,) = _struct_I.unpack(str[start:end])
01731 start = end
01732 end += length
01733 if python3:
01734 _v101.frame_id = str[start:end].decode('utf-8')
01735 else:
01736 _v101.frame_id = str[start:end]
01737 start = end
01738 end += 4
01739 (length,) = _struct_I.unpack(str[start:end])
01740 _v100.name = []
01741 for i in range(0, length):
01742 start = end
01743 end += 4
01744 (length,) = _struct_I.unpack(str[start:end])
01745 start = end
01746 end += length
01747 if python3:
01748 val3 = str[start:end].decode('utf-8')
01749 else:
01750 val3 = str[start:end]
01751 _v100.name.append(val3)
01752 start = end
01753 end += 4
01754 (length,) = _struct_I.unpack(str[start:end])
01755 pattern = '<%sd'%length
01756 start = end
01757 end += struct.calcsize(pattern)
01758 _v100.position = struct.unpack(pattern, str[start:end])
01759 start = end
01760 end += 4
01761 (length,) = _struct_I.unpack(str[start:end])
01762 pattern = '<%sd'%length
01763 start = end
01764 end += struct.calcsize(pattern)
01765 _v100.velocity = struct.unpack(pattern, str[start:end])
01766 start = end
01767 end += 4
01768 (length,) = _struct_I.unpack(str[start:end])
01769 pattern = '<%sd'%length
01770 start = end
01771 end += struct.calcsize(pattern)
01772 _v100.effort = struct.unpack(pattern, str[start:end])
01773 _v103 = val1.grasp_posture
01774 _v104 = _v103.header
01775 start = end
01776 end += 4
01777 (_v104.seq,) = _struct_I.unpack(str[start:end])
01778 _v105 = _v104.stamp
01779 _x = _v105
01780 start = end
01781 end += 8
01782 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01783 start = end
01784 end += 4
01785 (length,) = _struct_I.unpack(str[start:end])
01786 start = end
01787 end += length
01788 if python3:
01789 _v104.frame_id = str[start:end].decode('utf-8')
01790 else:
01791 _v104.frame_id = str[start:end]
01792 start = end
01793 end += 4
01794 (length,) = _struct_I.unpack(str[start:end])
01795 _v103.name = []
01796 for i in range(0, length):
01797 start = end
01798 end += 4
01799 (length,) = _struct_I.unpack(str[start:end])
01800 start = end
01801 end += length
01802 if python3:
01803 val3 = str[start:end].decode('utf-8')
01804 else:
01805 val3 = str[start:end]
01806 _v103.name.append(val3)
01807 start = end
01808 end += 4
01809 (length,) = _struct_I.unpack(str[start:end])
01810 pattern = '<%sd'%length
01811 start = end
01812 end += struct.calcsize(pattern)
01813 _v103.position = struct.unpack(pattern, str[start:end])
01814 start = end
01815 end += 4
01816 (length,) = _struct_I.unpack(str[start:end])
01817 pattern = '<%sd'%length
01818 start = end
01819 end += struct.calcsize(pattern)
01820 _v103.velocity = struct.unpack(pattern, str[start:end])
01821 start = end
01822 end += 4
01823 (length,) = _struct_I.unpack(str[start:end])
01824 pattern = '<%sd'%length
01825 start = end
01826 end += struct.calcsize(pattern)
01827 _v103.effort = struct.unpack(pattern, str[start:end])
01828 _v106 = val1.grasp_pose
01829 _v107 = _v106.position
01830 _x = _v107
01831 start = end
01832 end += 24
01833 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01834 _v108 = _v106.orientation
01835 _x = _v108
01836 start = end
01837 end += 32
01838 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01839 _x = val1
01840 start = end
01841 end += 17
01842 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
01843 val1.cluster_rep = bool(val1.cluster_rep)
01844 start = end
01845 end += 4
01846 (length,) = _struct_I.unpack(str[start:end])
01847 val1.moved_obstacles = []
01848 for i in range(0, length):
01849 val2 = object_manipulation_msgs.msg.GraspableObject()
01850 start = end
01851 end += 4
01852 (length,) = _struct_I.unpack(str[start:end])
01853 start = end
01854 end += length
01855 if python3:
01856 val2.reference_frame_id = str[start:end].decode('utf-8')
01857 else:
01858 val2.reference_frame_id = str[start:end]
01859 start = end
01860 end += 4
01861 (length,) = _struct_I.unpack(str[start:end])
01862 val2.potential_models = []
01863 for i in range(0, length):
01864 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
01865 start = end
01866 end += 4
01867 (val3.model_id,) = _struct_i.unpack(str[start:end])
01868 _v109 = val3.pose
01869 _v110 = _v109.header
01870 start = end
01871 end += 4
01872 (_v110.seq,) = _struct_I.unpack(str[start:end])
01873 _v111 = _v110.stamp
01874 _x = _v111
01875 start = end
01876 end += 8
01877 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01878 start = end
01879 end += 4
01880 (length,) = _struct_I.unpack(str[start:end])
01881 start = end
01882 end += length
01883 if python3:
01884 _v110.frame_id = str[start:end].decode('utf-8')
01885 else:
01886 _v110.frame_id = str[start:end]
01887 _v112 = _v109.pose
01888 _v113 = _v112.position
01889 _x = _v113
01890 start = end
01891 end += 24
01892 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01893 _v114 = _v112.orientation
01894 _x = _v114
01895 start = end
01896 end += 32
01897 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01898 start = end
01899 end += 4
01900 (val3.confidence,) = _struct_f.unpack(str[start:end])
01901 start = end
01902 end += 4
01903 (length,) = _struct_I.unpack(str[start:end])
01904 start = end
01905 end += length
01906 if python3:
01907 val3.detector_name = str[start:end].decode('utf-8')
01908 else:
01909 val3.detector_name = str[start:end]
01910 val2.potential_models.append(val3)
01911 _v115 = val2.cluster
01912 _v116 = _v115.header
01913 start = end
01914 end += 4
01915 (_v116.seq,) = _struct_I.unpack(str[start:end])
01916 _v117 = _v116.stamp
01917 _x = _v117
01918 start = end
01919 end += 8
01920 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01921 start = end
01922 end += 4
01923 (length,) = _struct_I.unpack(str[start:end])
01924 start = end
01925 end += length
01926 if python3:
01927 _v116.frame_id = str[start:end].decode('utf-8')
01928 else:
01929 _v116.frame_id = str[start:end]
01930 start = end
01931 end += 4
01932 (length,) = _struct_I.unpack(str[start:end])
01933 _v115.points = []
01934 for i in range(0, length):
01935 val4 = geometry_msgs.msg.Point32()
01936 _x = val4
01937 start = end
01938 end += 12
01939 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01940 _v115.points.append(val4)
01941 start = end
01942 end += 4
01943 (length,) = _struct_I.unpack(str[start:end])
01944 _v115.channels = []
01945 for i in range(0, length):
01946 val4 = sensor_msgs.msg.ChannelFloat32()
01947 start = end
01948 end += 4
01949 (length,) = _struct_I.unpack(str[start:end])
01950 start = end
01951 end += length
01952 if python3:
01953 val4.name = str[start:end].decode('utf-8')
01954 else:
01955 val4.name = str[start:end]
01956 start = end
01957 end += 4
01958 (length,) = _struct_I.unpack(str[start:end])
01959 pattern = '<%sf'%length
01960 start = end
01961 end += struct.calcsize(pattern)
01962 val4.values = struct.unpack(pattern, str[start:end])
01963 _v115.channels.append(val4)
01964 _v118 = val2.region
01965 _v119 = _v118.cloud
01966 _v120 = _v119.header
01967 start = end
01968 end += 4
01969 (_v120.seq,) = _struct_I.unpack(str[start:end])
01970 _v121 = _v120.stamp
01971 _x = _v121
01972 start = end
01973 end += 8
01974 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01975 start = end
01976 end += 4
01977 (length,) = _struct_I.unpack(str[start:end])
01978 start = end
01979 end += length
01980 if python3:
01981 _v120.frame_id = str[start:end].decode('utf-8')
01982 else:
01983 _v120.frame_id = str[start:end]
01984 _x = _v119
01985 start = end
01986 end += 8
01987 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
01988 start = end
01989 end += 4
01990 (length,) = _struct_I.unpack(str[start:end])
01991 _v119.fields = []
01992 for i in range(0, length):
01993 val5 = sensor_msgs.msg.PointField()
01994 start = end
01995 end += 4
01996 (length,) = _struct_I.unpack(str[start:end])
01997 start = end
01998 end += length
01999 if python3:
02000 val5.name = str[start:end].decode('utf-8')
02001 else:
02002 val5.name = str[start:end]
02003 _x = val5
02004 start = end
02005 end += 9
02006 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02007 _v119.fields.append(val5)
02008 _x = _v119
02009 start = end
02010 end += 9
02011 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
02012 _v119.is_bigendian = bool(_v119.is_bigendian)
02013 start = end
02014 end += 4
02015 (length,) = _struct_I.unpack(str[start:end])
02016 start = end
02017 end += length
02018 if python3:
02019 _v119.data = str[start:end].decode('utf-8')
02020 else:
02021 _v119.data = str[start:end]
02022 start = end
02023 end += 1
02024 (_v119.is_dense,) = _struct_B.unpack(str[start:end])
02025 _v119.is_dense = bool(_v119.is_dense)
02026 start = end
02027 end += 4
02028 (length,) = _struct_I.unpack(str[start:end])
02029 pattern = '<%si'%length
02030 start = end
02031 end += struct.calcsize(pattern)
02032 _v118.mask = struct.unpack(pattern, str[start:end])
02033 _v122 = _v118.image
02034 _v123 = _v122.header
02035 start = end
02036 end += 4
02037 (_v123.seq,) = _struct_I.unpack(str[start:end])
02038 _v124 = _v123.stamp
02039 _x = _v124
02040 start = end
02041 end += 8
02042 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02043 start = end
02044 end += 4
02045 (length,) = _struct_I.unpack(str[start:end])
02046 start = end
02047 end += length
02048 if python3:
02049 _v123.frame_id = str[start:end].decode('utf-8')
02050 else:
02051 _v123.frame_id = str[start:end]
02052 _x = _v122
02053 start = end
02054 end += 8
02055 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02056 start = end
02057 end += 4
02058 (length,) = _struct_I.unpack(str[start:end])
02059 start = end
02060 end += length
02061 if python3:
02062 _v122.encoding = str[start:end].decode('utf-8')
02063 else:
02064 _v122.encoding = str[start:end]
02065 _x = _v122
02066 start = end
02067 end += 5
02068 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02069 start = end
02070 end += 4
02071 (length,) = _struct_I.unpack(str[start:end])
02072 start = end
02073 end += length
02074 if python3:
02075 _v122.data = str[start:end].decode('utf-8')
02076 else:
02077 _v122.data = str[start:end]
02078 _v125 = _v118.disparity_image
02079 _v126 = _v125.header
02080 start = end
02081 end += 4
02082 (_v126.seq,) = _struct_I.unpack(str[start:end])
02083 _v127 = _v126.stamp
02084 _x = _v127
02085 start = end
02086 end += 8
02087 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02088 start = end
02089 end += 4
02090 (length,) = _struct_I.unpack(str[start:end])
02091 start = end
02092 end += length
02093 if python3:
02094 _v126.frame_id = str[start:end].decode('utf-8')
02095 else:
02096 _v126.frame_id = str[start:end]
02097 _x = _v125
02098 start = end
02099 end += 8
02100 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02101 start = end
02102 end += 4
02103 (length,) = _struct_I.unpack(str[start:end])
02104 start = end
02105 end += length
02106 if python3:
02107 _v125.encoding = str[start:end].decode('utf-8')
02108 else:
02109 _v125.encoding = str[start:end]
02110 _x = _v125
02111 start = end
02112 end += 5
02113 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02114 start = end
02115 end += 4
02116 (length,) = _struct_I.unpack(str[start:end])
02117 start = end
02118 end += length
02119 if python3:
02120 _v125.data = str[start:end].decode('utf-8')
02121 else:
02122 _v125.data = str[start:end]
02123 _v128 = _v118.cam_info
02124 _v129 = _v128.header
02125 start = end
02126 end += 4
02127 (_v129.seq,) = _struct_I.unpack(str[start:end])
02128 _v130 = _v129.stamp
02129 _x = _v130
02130 start = end
02131 end += 8
02132 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02133 start = end
02134 end += 4
02135 (length,) = _struct_I.unpack(str[start:end])
02136 start = end
02137 end += length
02138 if python3:
02139 _v129.frame_id = str[start:end].decode('utf-8')
02140 else:
02141 _v129.frame_id = str[start:end]
02142 _x = _v128
02143 start = end
02144 end += 8
02145 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02146 start = end
02147 end += 4
02148 (length,) = _struct_I.unpack(str[start:end])
02149 start = end
02150 end += length
02151 if python3:
02152 _v128.distortion_model = str[start:end].decode('utf-8')
02153 else:
02154 _v128.distortion_model = str[start:end]
02155 start = end
02156 end += 4
02157 (length,) = _struct_I.unpack(str[start:end])
02158 pattern = '<%sd'%length
02159 start = end
02160 end += struct.calcsize(pattern)
02161 _v128.D = struct.unpack(pattern, str[start:end])
02162 start = end
02163 end += 72
02164 _v128.K = _struct_9d.unpack(str[start:end])
02165 start = end
02166 end += 72
02167 _v128.R = _struct_9d.unpack(str[start:end])
02168 start = end
02169 end += 96
02170 _v128.P = _struct_12d.unpack(str[start:end])
02171 _x = _v128
02172 start = end
02173 end += 8
02174 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
02175 _v131 = _v128.roi
02176 _x = _v131
02177 start = end
02178 end += 17
02179 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
02180 _v131.do_rectify = bool(_v131.do_rectify)
02181 _v132 = _v118.roi_box_pose
02182 _v133 = _v132.header
02183 start = end
02184 end += 4
02185 (_v133.seq,) = _struct_I.unpack(str[start:end])
02186 _v134 = _v133.stamp
02187 _x = _v134
02188 start = end
02189 end += 8
02190 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02191 start = end
02192 end += 4
02193 (length,) = _struct_I.unpack(str[start:end])
02194 start = end
02195 end += length
02196 if python3:
02197 _v133.frame_id = str[start:end].decode('utf-8')
02198 else:
02199 _v133.frame_id = str[start:end]
02200 _v135 = _v132.pose
02201 _v136 = _v135.position
02202 _x = _v136
02203 start = end
02204 end += 24
02205 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02206 _v137 = _v135.orientation
02207 _x = _v137
02208 start = end
02209 end += 32
02210 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02211 _v138 = _v118.roi_box_dims
02212 _x = _v138
02213 start = end
02214 end += 24
02215 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02216 start = end
02217 end += 4
02218 (length,) = _struct_I.unpack(str[start:end])
02219 start = end
02220 end += length
02221 if python3:
02222 val2.collision_name = str[start:end].decode('utf-8')
02223 else:
02224 val2.collision_name = str[start:end]
02225 val1.moved_obstacles.append(val2)
02226 self.attempted_grasps.append(val1)
02227 start = end
02228 end += 4
02229 (length,) = _struct_I.unpack(str[start:end])
02230 self.attempted_grasp_results = []
02231 for i in range(0, length):
02232 val1 = object_manipulation_msgs.msg.GraspResult()
02233 _x = val1
02234 start = end
02235 end += 5
02236 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
02237 val1.continuation_possible = bool(val1.continuation_possible)
02238 self.attempted_grasp_results.append(val1)
02239 return self
02240 except struct.error as e:
02241 raise genpy.DeserializationError(e) #most likely buffer underfill
02242
02243
02244 def serialize_numpy(self, buff, numpy):
02245 """
02246 serialize message with numpy array types into buffer
02247 :param buff: buffer, ``StringIO``
02248 :param numpy: numpy python module
02249 """
02250 try:
02251 _x = self
02252 buff.write(_struct_i3I.pack(_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs))
02253 _x = self.grasp.pre_grasp_posture.header.frame_id
02254 length = len(_x)
02255 if python3 or type(_x) == unicode:
02256 _x = _x.encode('utf-8')
02257 length = len(_x)
02258 buff.write(struct.pack('<I%ss'%length, length, _x))
02259 length = len(self.grasp.pre_grasp_posture.name)
02260 buff.write(_struct_I.pack(length))
02261 for val1 in self.grasp.pre_grasp_posture.name:
02262 length = len(val1)
02263 if python3 or type(val1) == unicode:
02264 val1 = val1.encode('utf-8')
02265 length = len(val1)
02266 buff.write(struct.pack('<I%ss'%length, length, val1))
02267 length = len(self.grasp.pre_grasp_posture.position)
02268 buff.write(_struct_I.pack(length))
02269 pattern = '<%sd'%length
02270 buff.write(self.grasp.pre_grasp_posture.position.tostring())
02271 length = len(self.grasp.pre_grasp_posture.velocity)
02272 buff.write(_struct_I.pack(length))
02273 pattern = '<%sd'%length
02274 buff.write(self.grasp.pre_grasp_posture.velocity.tostring())
02275 length = len(self.grasp.pre_grasp_posture.effort)
02276 buff.write(_struct_I.pack(length))
02277 pattern = '<%sd'%length
02278 buff.write(self.grasp.pre_grasp_posture.effort.tostring())
02279 _x = self
02280 buff.write(_struct_3I.pack(_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs))
02281 _x = self.grasp.grasp_posture.header.frame_id
02282 length = len(_x)
02283 if python3 or type(_x) == unicode:
02284 _x = _x.encode('utf-8')
02285 length = len(_x)
02286 buff.write(struct.pack('<I%ss'%length, length, _x))
02287 length = len(self.grasp.grasp_posture.name)
02288 buff.write(_struct_I.pack(length))
02289 for val1 in self.grasp.grasp_posture.name:
02290 length = len(val1)
02291 if python3 or type(val1) == unicode:
02292 val1 = val1.encode('utf-8')
02293 length = len(val1)
02294 buff.write(struct.pack('<I%ss'%length, length, val1))
02295 length = len(self.grasp.grasp_posture.position)
02296 buff.write(_struct_I.pack(length))
02297 pattern = '<%sd'%length
02298 buff.write(self.grasp.grasp_posture.position.tostring())
02299 length = len(self.grasp.grasp_posture.velocity)
02300 buff.write(_struct_I.pack(length))
02301 pattern = '<%sd'%length
02302 buff.write(self.grasp.grasp_posture.velocity.tostring())
02303 length = len(self.grasp.grasp_posture.effort)
02304 buff.write(_struct_I.pack(length))
02305 pattern = '<%sd'%length
02306 buff.write(self.grasp.grasp_posture.effort.tostring())
02307 _x = self
02308 buff.write(_struct_8dB2f.pack(_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance))
02309 length = len(self.grasp.moved_obstacles)
02310 buff.write(_struct_I.pack(length))
02311 for val1 in self.grasp.moved_obstacles:
02312 _x = val1.reference_frame_id
02313 length = len(_x)
02314 if python3 or type(_x) == unicode:
02315 _x = _x.encode('utf-8')
02316 length = len(_x)
02317 buff.write(struct.pack('<I%ss'%length, length, _x))
02318 length = len(val1.potential_models)
02319 buff.write(_struct_I.pack(length))
02320 for val2 in val1.potential_models:
02321 buff.write(_struct_i.pack(val2.model_id))
02322 _v139 = val2.pose
02323 _v140 = _v139.header
02324 buff.write(_struct_I.pack(_v140.seq))
02325 _v141 = _v140.stamp
02326 _x = _v141
02327 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02328 _x = _v140.frame_id
02329 length = len(_x)
02330 if python3 or type(_x) == unicode:
02331 _x = _x.encode('utf-8')
02332 length = len(_x)
02333 buff.write(struct.pack('<I%ss'%length, length, _x))
02334 _v142 = _v139.pose
02335 _v143 = _v142.position
02336 _x = _v143
02337 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02338 _v144 = _v142.orientation
02339 _x = _v144
02340 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02341 buff.write(_struct_f.pack(val2.confidence))
02342 _x = val2.detector_name
02343 length = len(_x)
02344 if python3 or type(_x) == unicode:
02345 _x = _x.encode('utf-8')
02346 length = len(_x)
02347 buff.write(struct.pack('<I%ss'%length, length, _x))
02348 _v145 = val1.cluster
02349 _v146 = _v145.header
02350 buff.write(_struct_I.pack(_v146.seq))
02351 _v147 = _v146.stamp
02352 _x = _v147
02353 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02354 _x = _v146.frame_id
02355 length = len(_x)
02356 if python3 or type(_x) == unicode:
02357 _x = _x.encode('utf-8')
02358 length = len(_x)
02359 buff.write(struct.pack('<I%ss'%length, length, _x))
02360 length = len(_v145.points)
02361 buff.write(_struct_I.pack(length))
02362 for val3 in _v145.points:
02363 _x = val3
02364 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02365 length = len(_v145.channels)
02366 buff.write(_struct_I.pack(length))
02367 for val3 in _v145.channels:
02368 _x = val3.name
02369 length = len(_x)
02370 if python3 or type(_x) == unicode:
02371 _x = _x.encode('utf-8')
02372 length = len(_x)
02373 buff.write(struct.pack('<I%ss'%length, length, _x))
02374 length = len(val3.values)
02375 buff.write(_struct_I.pack(length))
02376 pattern = '<%sf'%length
02377 buff.write(val3.values.tostring())
02378 _v148 = val1.region
02379 _v149 = _v148.cloud
02380 _v150 = _v149.header
02381 buff.write(_struct_I.pack(_v150.seq))
02382 _v151 = _v150.stamp
02383 _x = _v151
02384 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02385 _x = _v150.frame_id
02386 length = len(_x)
02387 if python3 or type(_x) == unicode:
02388 _x = _x.encode('utf-8')
02389 length = len(_x)
02390 buff.write(struct.pack('<I%ss'%length, length, _x))
02391 _x = _v149
02392 buff.write(_struct_2I.pack(_x.height, _x.width))
02393 length = len(_v149.fields)
02394 buff.write(_struct_I.pack(length))
02395 for val4 in _v149.fields:
02396 _x = val4.name
02397 length = len(_x)
02398 if python3 or type(_x) == unicode:
02399 _x = _x.encode('utf-8')
02400 length = len(_x)
02401 buff.write(struct.pack('<I%ss'%length, length, _x))
02402 _x = val4
02403 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
02404 _x = _v149
02405 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
02406 _x = _v149.data
02407 length = len(_x)
02408 # - if encoded as a list instead, serialize as bytes instead of string
02409 if type(_x) in [list, tuple]:
02410 buff.write(struct.pack('<I%sB'%length, length, *_x))
02411 else:
02412 buff.write(struct.pack('<I%ss'%length, length, _x))
02413 buff.write(_struct_B.pack(_v149.is_dense))
02414 length = len(_v148.mask)
02415 buff.write(_struct_I.pack(length))
02416 pattern = '<%si'%length
02417 buff.write(_v148.mask.tostring())
02418 _v152 = _v148.image
02419 _v153 = _v152.header
02420 buff.write(_struct_I.pack(_v153.seq))
02421 _v154 = _v153.stamp
02422 _x = _v154
02423 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02424 _x = _v153.frame_id
02425 length = len(_x)
02426 if python3 or type(_x) == unicode:
02427 _x = _x.encode('utf-8')
02428 length = len(_x)
02429 buff.write(struct.pack('<I%ss'%length, length, _x))
02430 _x = _v152
02431 buff.write(_struct_2I.pack(_x.height, _x.width))
02432 _x = _v152.encoding
02433 length = len(_x)
02434 if python3 or type(_x) == unicode:
02435 _x = _x.encode('utf-8')
02436 length = len(_x)
02437 buff.write(struct.pack('<I%ss'%length, length, _x))
02438 _x = _v152
02439 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02440 _x = _v152.data
02441 length = len(_x)
02442 # - if encoded as a list instead, serialize as bytes instead of string
02443 if type(_x) in [list, tuple]:
02444 buff.write(struct.pack('<I%sB'%length, length, *_x))
02445 else:
02446 buff.write(struct.pack('<I%ss'%length, length, _x))
02447 _v155 = _v148.disparity_image
02448 _v156 = _v155.header
02449 buff.write(_struct_I.pack(_v156.seq))
02450 _v157 = _v156.stamp
02451 _x = _v157
02452 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02453 _x = _v156.frame_id
02454 length = len(_x)
02455 if python3 or type(_x) == unicode:
02456 _x = _x.encode('utf-8')
02457 length = len(_x)
02458 buff.write(struct.pack('<I%ss'%length, length, _x))
02459 _x = _v155
02460 buff.write(_struct_2I.pack(_x.height, _x.width))
02461 _x = _v155.encoding
02462 length = len(_x)
02463 if python3 or type(_x) == unicode:
02464 _x = _x.encode('utf-8')
02465 length = len(_x)
02466 buff.write(struct.pack('<I%ss'%length, length, _x))
02467 _x = _v155
02468 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02469 _x = _v155.data
02470 length = len(_x)
02471 # - if encoded as a list instead, serialize as bytes instead of string
02472 if type(_x) in [list, tuple]:
02473 buff.write(struct.pack('<I%sB'%length, length, *_x))
02474 else:
02475 buff.write(struct.pack('<I%ss'%length, length, _x))
02476 _v158 = _v148.cam_info
02477 _v159 = _v158.header
02478 buff.write(_struct_I.pack(_v159.seq))
02479 _v160 = _v159.stamp
02480 _x = _v160
02481 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02482 _x = _v159.frame_id
02483 length = len(_x)
02484 if python3 or type(_x) == unicode:
02485 _x = _x.encode('utf-8')
02486 length = len(_x)
02487 buff.write(struct.pack('<I%ss'%length, length, _x))
02488 _x = _v158
02489 buff.write(_struct_2I.pack(_x.height, _x.width))
02490 _x = _v158.distortion_model
02491 length = len(_x)
02492 if python3 or type(_x) == unicode:
02493 _x = _x.encode('utf-8')
02494 length = len(_x)
02495 buff.write(struct.pack('<I%ss'%length, length, _x))
02496 length = len(_v158.D)
02497 buff.write(_struct_I.pack(length))
02498 pattern = '<%sd'%length
02499 buff.write(_v158.D.tostring())
02500 buff.write(_v158.K.tostring())
02501 buff.write(_v158.R.tostring())
02502 buff.write(_v158.P.tostring())
02503 _x = _v158
02504 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
02505 _v161 = _v158.roi
02506 _x = _v161
02507 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
02508 _v162 = _v148.roi_box_pose
02509 _v163 = _v162.header
02510 buff.write(_struct_I.pack(_v163.seq))
02511 _v164 = _v163.stamp
02512 _x = _v164
02513 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02514 _x = _v163.frame_id
02515 length = len(_x)
02516 if python3 or type(_x) == unicode:
02517 _x = _x.encode('utf-8')
02518 length = len(_x)
02519 buff.write(struct.pack('<I%ss'%length, length, _x))
02520 _v165 = _v162.pose
02521 _v166 = _v165.position
02522 _x = _v166
02523 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02524 _v167 = _v165.orientation
02525 _x = _v167
02526 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02527 _v168 = _v148.roi_box_dims
02528 _x = _v168
02529 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02530 _x = val1.collision_name
02531 length = len(_x)
02532 if python3 or type(_x) == unicode:
02533 _x = _x.encode('utf-8')
02534 length = len(_x)
02535 buff.write(struct.pack('<I%ss'%length, length, _x))
02536 length = len(self.attempted_grasps)
02537 buff.write(_struct_I.pack(length))
02538 for val1 in self.attempted_grasps:
02539 _v169 = val1.pre_grasp_posture
02540 _v170 = _v169.header
02541 buff.write(_struct_I.pack(_v170.seq))
02542 _v171 = _v170.stamp
02543 _x = _v171
02544 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02545 _x = _v170.frame_id
02546 length = len(_x)
02547 if python3 or type(_x) == unicode:
02548 _x = _x.encode('utf-8')
02549 length = len(_x)
02550 buff.write(struct.pack('<I%ss'%length, length, _x))
02551 length = len(_v169.name)
02552 buff.write(_struct_I.pack(length))
02553 for val3 in _v169.name:
02554 length = len(val3)
02555 if python3 or type(val3) == unicode:
02556 val3 = val3.encode('utf-8')
02557 length = len(val3)
02558 buff.write(struct.pack('<I%ss'%length, length, val3))
02559 length = len(_v169.position)
02560 buff.write(_struct_I.pack(length))
02561 pattern = '<%sd'%length
02562 buff.write(_v169.position.tostring())
02563 length = len(_v169.velocity)
02564 buff.write(_struct_I.pack(length))
02565 pattern = '<%sd'%length
02566 buff.write(_v169.velocity.tostring())
02567 length = len(_v169.effort)
02568 buff.write(_struct_I.pack(length))
02569 pattern = '<%sd'%length
02570 buff.write(_v169.effort.tostring())
02571 _v172 = val1.grasp_posture
02572 _v173 = _v172.header
02573 buff.write(_struct_I.pack(_v173.seq))
02574 _v174 = _v173.stamp
02575 _x = _v174
02576 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02577 _x = _v173.frame_id
02578 length = len(_x)
02579 if python3 or type(_x) == unicode:
02580 _x = _x.encode('utf-8')
02581 length = len(_x)
02582 buff.write(struct.pack('<I%ss'%length, length, _x))
02583 length = len(_v172.name)
02584 buff.write(_struct_I.pack(length))
02585 for val3 in _v172.name:
02586 length = len(val3)
02587 if python3 or type(val3) == unicode:
02588 val3 = val3.encode('utf-8')
02589 length = len(val3)
02590 buff.write(struct.pack('<I%ss'%length, length, val3))
02591 length = len(_v172.position)
02592 buff.write(_struct_I.pack(length))
02593 pattern = '<%sd'%length
02594 buff.write(_v172.position.tostring())
02595 length = len(_v172.velocity)
02596 buff.write(_struct_I.pack(length))
02597 pattern = '<%sd'%length
02598 buff.write(_v172.velocity.tostring())
02599 length = len(_v172.effort)
02600 buff.write(_struct_I.pack(length))
02601 pattern = '<%sd'%length
02602 buff.write(_v172.effort.tostring())
02603 _v175 = val1.grasp_pose
02604 _v176 = _v175.position
02605 _x = _v176
02606 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02607 _v177 = _v175.orientation
02608 _x = _v177
02609 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02610 _x = val1
02611 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
02612 length = len(val1.moved_obstacles)
02613 buff.write(_struct_I.pack(length))
02614 for val2 in val1.moved_obstacles:
02615 _x = val2.reference_frame_id
02616 length = len(_x)
02617 if python3 or type(_x) == unicode:
02618 _x = _x.encode('utf-8')
02619 length = len(_x)
02620 buff.write(struct.pack('<I%ss'%length, length, _x))
02621 length = len(val2.potential_models)
02622 buff.write(_struct_I.pack(length))
02623 for val3 in val2.potential_models:
02624 buff.write(_struct_i.pack(val3.model_id))
02625 _v178 = val3.pose
02626 _v179 = _v178.header
02627 buff.write(_struct_I.pack(_v179.seq))
02628 _v180 = _v179.stamp
02629 _x = _v180
02630 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02631 _x = _v179.frame_id
02632 length = len(_x)
02633 if python3 or type(_x) == unicode:
02634 _x = _x.encode('utf-8')
02635 length = len(_x)
02636 buff.write(struct.pack('<I%ss'%length, length, _x))
02637 _v181 = _v178.pose
02638 _v182 = _v181.position
02639 _x = _v182
02640 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02641 _v183 = _v181.orientation
02642 _x = _v183
02643 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02644 buff.write(_struct_f.pack(val3.confidence))
02645 _x = val3.detector_name
02646 length = len(_x)
02647 if python3 or type(_x) == unicode:
02648 _x = _x.encode('utf-8')
02649 length = len(_x)
02650 buff.write(struct.pack('<I%ss'%length, length, _x))
02651 _v184 = val2.cluster
02652 _v185 = _v184.header
02653 buff.write(_struct_I.pack(_v185.seq))
02654 _v186 = _v185.stamp
02655 _x = _v186
02656 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02657 _x = _v185.frame_id
02658 length = len(_x)
02659 if python3 or type(_x) == unicode:
02660 _x = _x.encode('utf-8')
02661 length = len(_x)
02662 buff.write(struct.pack('<I%ss'%length, length, _x))
02663 length = len(_v184.points)
02664 buff.write(_struct_I.pack(length))
02665 for val4 in _v184.points:
02666 _x = val4
02667 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
02668 length = len(_v184.channels)
02669 buff.write(_struct_I.pack(length))
02670 for val4 in _v184.channels:
02671 _x = val4.name
02672 length = len(_x)
02673 if python3 or type(_x) == unicode:
02674 _x = _x.encode('utf-8')
02675 length = len(_x)
02676 buff.write(struct.pack('<I%ss'%length, length, _x))
02677 length = len(val4.values)
02678 buff.write(_struct_I.pack(length))
02679 pattern = '<%sf'%length
02680 buff.write(val4.values.tostring())
02681 _v187 = val2.region
02682 _v188 = _v187.cloud
02683 _v189 = _v188.header
02684 buff.write(_struct_I.pack(_v189.seq))
02685 _v190 = _v189.stamp
02686 _x = _v190
02687 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02688 _x = _v189.frame_id
02689 length = len(_x)
02690 if python3 or type(_x) == unicode:
02691 _x = _x.encode('utf-8')
02692 length = len(_x)
02693 buff.write(struct.pack('<I%ss'%length, length, _x))
02694 _x = _v188
02695 buff.write(_struct_2I.pack(_x.height, _x.width))
02696 length = len(_v188.fields)
02697 buff.write(_struct_I.pack(length))
02698 for val5 in _v188.fields:
02699 _x = val5.name
02700 length = len(_x)
02701 if python3 or type(_x) == unicode:
02702 _x = _x.encode('utf-8')
02703 length = len(_x)
02704 buff.write(struct.pack('<I%ss'%length, length, _x))
02705 _x = val5
02706 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
02707 _x = _v188
02708 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
02709 _x = _v188.data
02710 length = len(_x)
02711 # - if encoded as a list instead, serialize as bytes instead of string
02712 if type(_x) in [list, tuple]:
02713 buff.write(struct.pack('<I%sB'%length, length, *_x))
02714 else:
02715 buff.write(struct.pack('<I%ss'%length, length, _x))
02716 buff.write(_struct_B.pack(_v188.is_dense))
02717 length = len(_v187.mask)
02718 buff.write(_struct_I.pack(length))
02719 pattern = '<%si'%length
02720 buff.write(_v187.mask.tostring())
02721 _v191 = _v187.image
02722 _v192 = _v191.header
02723 buff.write(_struct_I.pack(_v192.seq))
02724 _v193 = _v192.stamp
02725 _x = _v193
02726 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02727 _x = _v192.frame_id
02728 length = len(_x)
02729 if python3 or type(_x) == unicode:
02730 _x = _x.encode('utf-8')
02731 length = len(_x)
02732 buff.write(struct.pack('<I%ss'%length, length, _x))
02733 _x = _v191
02734 buff.write(_struct_2I.pack(_x.height, _x.width))
02735 _x = _v191.encoding
02736 length = len(_x)
02737 if python3 or type(_x) == unicode:
02738 _x = _x.encode('utf-8')
02739 length = len(_x)
02740 buff.write(struct.pack('<I%ss'%length, length, _x))
02741 _x = _v191
02742 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02743 _x = _v191.data
02744 length = len(_x)
02745 # - if encoded as a list instead, serialize as bytes instead of string
02746 if type(_x) in [list, tuple]:
02747 buff.write(struct.pack('<I%sB'%length, length, *_x))
02748 else:
02749 buff.write(struct.pack('<I%ss'%length, length, _x))
02750 _v194 = _v187.disparity_image
02751 _v195 = _v194.header
02752 buff.write(_struct_I.pack(_v195.seq))
02753 _v196 = _v195.stamp
02754 _x = _v196
02755 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02756 _x = _v195.frame_id
02757 length = len(_x)
02758 if python3 or type(_x) == unicode:
02759 _x = _x.encode('utf-8')
02760 length = len(_x)
02761 buff.write(struct.pack('<I%ss'%length, length, _x))
02762 _x = _v194
02763 buff.write(_struct_2I.pack(_x.height, _x.width))
02764 _x = _v194.encoding
02765 length = len(_x)
02766 if python3 or type(_x) == unicode:
02767 _x = _x.encode('utf-8')
02768 length = len(_x)
02769 buff.write(struct.pack('<I%ss'%length, length, _x))
02770 _x = _v194
02771 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02772 _x = _v194.data
02773 length = len(_x)
02774 # - if encoded as a list instead, serialize as bytes instead of string
02775 if type(_x) in [list, tuple]:
02776 buff.write(struct.pack('<I%sB'%length, length, *_x))
02777 else:
02778 buff.write(struct.pack('<I%ss'%length, length, _x))
02779 _v197 = _v187.cam_info
02780 _v198 = _v197.header
02781 buff.write(_struct_I.pack(_v198.seq))
02782 _v199 = _v198.stamp
02783 _x = _v199
02784 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02785 _x = _v198.frame_id
02786 length = len(_x)
02787 if python3 or type(_x) == unicode:
02788 _x = _x.encode('utf-8')
02789 length = len(_x)
02790 buff.write(struct.pack('<I%ss'%length, length, _x))
02791 _x = _v197
02792 buff.write(_struct_2I.pack(_x.height, _x.width))
02793 _x = _v197.distortion_model
02794 length = len(_x)
02795 if python3 or type(_x) == unicode:
02796 _x = _x.encode('utf-8')
02797 length = len(_x)
02798 buff.write(struct.pack('<I%ss'%length, length, _x))
02799 length = len(_v197.D)
02800 buff.write(_struct_I.pack(length))
02801 pattern = '<%sd'%length
02802 buff.write(_v197.D.tostring())
02803 buff.write(_v197.K.tostring())
02804 buff.write(_v197.R.tostring())
02805 buff.write(_v197.P.tostring())
02806 _x = _v197
02807 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
02808 _v200 = _v197.roi
02809 _x = _v200
02810 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
02811 _v201 = _v187.roi_box_pose
02812 _v202 = _v201.header
02813 buff.write(_struct_I.pack(_v202.seq))
02814 _v203 = _v202.stamp
02815 _x = _v203
02816 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02817 _x = _v202.frame_id
02818 length = len(_x)
02819 if python3 or type(_x) == unicode:
02820 _x = _x.encode('utf-8')
02821 length = len(_x)
02822 buff.write(struct.pack('<I%ss'%length, length, _x))
02823 _v204 = _v201.pose
02824 _v205 = _v204.position
02825 _x = _v205
02826 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02827 _v206 = _v204.orientation
02828 _x = _v206
02829 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02830 _v207 = _v187.roi_box_dims
02831 _x = _v207
02832 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02833 _x = val2.collision_name
02834 length = len(_x)
02835 if python3 or type(_x) == unicode:
02836 _x = _x.encode('utf-8')
02837 length = len(_x)
02838 buff.write(struct.pack('<I%ss'%length, length, _x))
02839 length = len(self.attempted_grasp_results)
02840 buff.write(_struct_I.pack(length))
02841 for val1 in self.attempted_grasp_results:
02842 _x = val1
02843 buff.write(_struct_iB.pack(_x.result_code, _x.continuation_possible))
02844 except struct.error as se: self._check_types(se)
02845 except TypeError as te: self._check_types(te)
02846
02847 def deserialize_numpy(self, str, numpy):
02848 """
02849 unpack serialized message in str into this message instance using numpy for array types
02850 :param str: byte array of serialized message, ``str``
02851 :param numpy: numpy python module
02852 """
02853 try:
02854 if self.manipulation_result is None:
02855 self.manipulation_result = object_manipulation_msgs.msg.ManipulationResult()
02856 if self.grasp is None:
02857 self.grasp = object_manipulation_msgs.msg.Grasp()
02858 if self.attempted_grasps is None:
02859 self.attempted_grasps = None
02860 if self.attempted_grasp_results is None:
02861 self.attempted_grasp_results = None
02862 end = 0
02863 _x = self
02864 start = end
02865 end += 16
02866 (_x.manipulation_result.value, _x.grasp.pre_grasp_posture.header.seq, _x.grasp.pre_grasp_posture.header.stamp.secs, _x.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
02867 start = end
02868 end += 4
02869 (length,) = _struct_I.unpack(str[start:end])
02870 start = end
02871 end += length
02872 if python3:
02873 self.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02874 else:
02875 self.grasp.pre_grasp_posture.header.frame_id = str[start:end]
02876 start = end
02877 end += 4
02878 (length,) = _struct_I.unpack(str[start:end])
02879 self.grasp.pre_grasp_posture.name = []
02880 for i in range(0, length):
02881 start = end
02882 end += 4
02883 (length,) = _struct_I.unpack(str[start:end])
02884 start = end
02885 end += length
02886 if python3:
02887 val1 = str[start:end].decode('utf-8')
02888 else:
02889 val1 = str[start:end]
02890 self.grasp.pre_grasp_posture.name.append(val1)
02891 start = end
02892 end += 4
02893 (length,) = _struct_I.unpack(str[start:end])
02894 pattern = '<%sd'%length
02895 start = end
02896 end += struct.calcsize(pattern)
02897 self.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02898 start = end
02899 end += 4
02900 (length,) = _struct_I.unpack(str[start:end])
02901 pattern = '<%sd'%length
02902 start = end
02903 end += struct.calcsize(pattern)
02904 self.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02905 start = end
02906 end += 4
02907 (length,) = _struct_I.unpack(str[start:end])
02908 pattern = '<%sd'%length
02909 start = end
02910 end += struct.calcsize(pattern)
02911 self.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02912 _x = self
02913 start = end
02914 end += 12
02915 (_x.grasp.grasp_posture.header.seq, _x.grasp.grasp_posture.header.stamp.secs, _x.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02916 start = end
02917 end += 4
02918 (length,) = _struct_I.unpack(str[start:end])
02919 start = end
02920 end += length
02921 if python3:
02922 self.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02923 else:
02924 self.grasp.grasp_posture.header.frame_id = str[start:end]
02925 start = end
02926 end += 4
02927 (length,) = _struct_I.unpack(str[start:end])
02928 self.grasp.grasp_posture.name = []
02929 for i in range(0, length):
02930 start = end
02931 end += 4
02932 (length,) = _struct_I.unpack(str[start:end])
02933 start = end
02934 end += length
02935 if python3:
02936 val1 = str[start:end].decode('utf-8')
02937 else:
02938 val1 = str[start:end]
02939 self.grasp.grasp_posture.name.append(val1)
02940 start = end
02941 end += 4
02942 (length,) = _struct_I.unpack(str[start:end])
02943 pattern = '<%sd'%length
02944 start = end
02945 end += struct.calcsize(pattern)
02946 self.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02947 start = end
02948 end += 4
02949 (length,) = _struct_I.unpack(str[start:end])
02950 pattern = '<%sd'%length
02951 start = end
02952 end += struct.calcsize(pattern)
02953 self.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02954 start = end
02955 end += 4
02956 (length,) = _struct_I.unpack(str[start:end])
02957 pattern = '<%sd'%length
02958 start = end
02959 end += struct.calcsize(pattern)
02960 self.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02961 _x = self
02962 start = end
02963 end += 73
02964 (_x.grasp.grasp_pose.position.x, _x.grasp.grasp_pose.position.y, _x.grasp.grasp_pose.position.z, _x.grasp.grasp_pose.orientation.x, _x.grasp.grasp_pose.orientation.y, _x.grasp.grasp_pose.orientation.z, _x.grasp.grasp_pose.orientation.w, _x.grasp.success_probability, _x.grasp.cluster_rep, _x.grasp.desired_approach_distance, _x.grasp.min_approach_distance,) = _struct_8dB2f.unpack(str[start:end])
02965 self.grasp.cluster_rep = bool(self.grasp.cluster_rep)
02966 start = end
02967 end += 4
02968 (length,) = _struct_I.unpack(str[start:end])
02969 self.grasp.moved_obstacles = []
02970 for i in range(0, length):
02971 val1 = object_manipulation_msgs.msg.GraspableObject()
02972 start = end
02973 end += 4
02974 (length,) = _struct_I.unpack(str[start:end])
02975 start = end
02976 end += length
02977 if python3:
02978 val1.reference_frame_id = str[start:end].decode('utf-8')
02979 else:
02980 val1.reference_frame_id = str[start:end]
02981 start = end
02982 end += 4
02983 (length,) = _struct_I.unpack(str[start:end])
02984 val1.potential_models = []
02985 for i in range(0, length):
02986 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
02987 start = end
02988 end += 4
02989 (val2.model_id,) = _struct_i.unpack(str[start:end])
02990 _v208 = val2.pose
02991 _v209 = _v208.header
02992 start = end
02993 end += 4
02994 (_v209.seq,) = _struct_I.unpack(str[start:end])
02995 _v210 = _v209.stamp
02996 _x = _v210
02997 start = end
02998 end += 8
02999 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03000 start = end
03001 end += 4
03002 (length,) = _struct_I.unpack(str[start:end])
03003 start = end
03004 end += length
03005 if python3:
03006 _v209.frame_id = str[start:end].decode('utf-8')
03007 else:
03008 _v209.frame_id = str[start:end]
03009 _v211 = _v208.pose
03010 _v212 = _v211.position
03011 _x = _v212
03012 start = end
03013 end += 24
03014 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03015 _v213 = _v211.orientation
03016 _x = _v213
03017 start = end
03018 end += 32
03019 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03020 start = end
03021 end += 4
03022 (val2.confidence,) = _struct_f.unpack(str[start:end])
03023 start = end
03024 end += 4
03025 (length,) = _struct_I.unpack(str[start:end])
03026 start = end
03027 end += length
03028 if python3:
03029 val2.detector_name = str[start:end].decode('utf-8')
03030 else:
03031 val2.detector_name = str[start:end]
03032 val1.potential_models.append(val2)
03033 _v214 = val1.cluster
03034 _v215 = _v214.header
03035 start = end
03036 end += 4
03037 (_v215.seq,) = _struct_I.unpack(str[start:end])
03038 _v216 = _v215.stamp
03039 _x = _v216
03040 start = end
03041 end += 8
03042 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03043 start = end
03044 end += 4
03045 (length,) = _struct_I.unpack(str[start:end])
03046 start = end
03047 end += length
03048 if python3:
03049 _v215.frame_id = str[start:end].decode('utf-8')
03050 else:
03051 _v215.frame_id = str[start:end]
03052 start = end
03053 end += 4
03054 (length,) = _struct_I.unpack(str[start:end])
03055 _v214.points = []
03056 for i in range(0, length):
03057 val3 = geometry_msgs.msg.Point32()
03058 _x = val3
03059 start = end
03060 end += 12
03061 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03062 _v214.points.append(val3)
03063 start = end
03064 end += 4
03065 (length,) = _struct_I.unpack(str[start:end])
03066 _v214.channels = []
03067 for i in range(0, length):
03068 val3 = sensor_msgs.msg.ChannelFloat32()
03069 start = end
03070 end += 4
03071 (length,) = _struct_I.unpack(str[start:end])
03072 start = end
03073 end += length
03074 if python3:
03075 val3.name = str[start:end].decode('utf-8')
03076 else:
03077 val3.name = str[start:end]
03078 start = end
03079 end += 4
03080 (length,) = _struct_I.unpack(str[start:end])
03081 pattern = '<%sf'%length
03082 start = end
03083 end += struct.calcsize(pattern)
03084 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
03085 _v214.channels.append(val3)
03086 _v217 = val1.region
03087 _v218 = _v217.cloud
03088 _v219 = _v218.header
03089 start = end
03090 end += 4
03091 (_v219.seq,) = _struct_I.unpack(str[start:end])
03092 _v220 = _v219.stamp
03093 _x = _v220
03094 start = end
03095 end += 8
03096 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03097 start = end
03098 end += 4
03099 (length,) = _struct_I.unpack(str[start:end])
03100 start = end
03101 end += length
03102 if python3:
03103 _v219.frame_id = str[start:end].decode('utf-8')
03104 else:
03105 _v219.frame_id = str[start:end]
03106 _x = _v218
03107 start = end
03108 end += 8
03109 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03110 start = end
03111 end += 4
03112 (length,) = _struct_I.unpack(str[start:end])
03113 _v218.fields = []
03114 for i in range(0, length):
03115 val4 = sensor_msgs.msg.PointField()
03116 start = end
03117 end += 4
03118 (length,) = _struct_I.unpack(str[start:end])
03119 start = end
03120 end += length
03121 if python3:
03122 val4.name = str[start:end].decode('utf-8')
03123 else:
03124 val4.name = str[start:end]
03125 _x = val4
03126 start = end
03127 end += 9
03128 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
03129 _v218.fields.append(val4)
03130 _x = _v218
03131 start = end
03132 end += 9
03133 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
03134 _v218.is_bigendian = bool(_v218.is_bigendian)
03135 start = end
03136 end += 4
03137 (length,) = _struct_I.unpack(str[start:end])
03138 start = end
03139 end += length
03140 if python3:
03141 _v218.data = str[start:end].decode('utf-8')
03142 else:
03143 _v218.data = str[start:end]
03144 start = end
03145 end += 1
03146 (_v218.is_dense,) = _struct_B.unpack(str[start:end])
03147 _v218.is_dense = bool(_v218.is_dense)
03148 start = end
03149 end += 4
03150 (length,) = _struct_I.unpack(str[start:end])
03151 pattern = '<%si'%length
03152 start = end
03153 end += struct.calcsize(pattern)
03154 _v217.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03155 _v221 = _v217.image
03156 _v222 = _v221.header
03157 start = end
03158 end += 4
03159 (_v222.seq,) = _struct_I.unpack(str[start:end])
03160 _v223 = _v222.stamp
03161 _x = _v223
03162 start = end
03163 end += 8
03164 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03165 start = end
03166 end += 4
03167 (length,) = _struct_I.unpack(str[start:end])
03168 start = end
03169 end += length
03170 if python3:
03171 _v222.frame_id = str[start:end].decode('utf-8')
03172 else:
03173 _v222.frame_id = str[start:end]
03174 _x = _v221
03175 start = end
03176 end += 8
03177 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03178 start = end
03179 end += 4
03180 (length,) = _struct_I.unpack(str[start:end])
03181 start = end
03182 end += length
03183 if python3:
03184 _v221.encoding = str[start:end].decode('utf-8')
03185 else:
03186 _v221.encoding = str[start:end]
03187 _x = _v221
03188 start = end
03189 end += 5
03190 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03191 start = end
03192 end += 4
03193 (length,) = _struct_I.unpack(str[start:end])
03194 start = end
03195 end += length
03196 if python3:
03197 _v221.data = str[start:end].decode('utf-8')
03198 else:
03199 _v221.data = str[start:end]
03200 _v224 = _v217.disparity_image
03201 _v225 = _v224.header
03202 start = end
03203 end += 4
03204 (_v225.seq,) = _struct_I.unpack(str[start:end])
03205 _v226 = _v225.stamp
03206 _x = _v226
03207 start = end
03208 end += 8
03209 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03210 start = end
03211 end += 4
03212 (length,) = _struct_I.unpack(str[start:end])
03213 start = end
03214 end += length
03215 if python3:
03216 _v225.frame_id = str[start:end].decode('utf-8')
03217 else:
03218 _v225.frame_id = str[start:end]
03219 _x = _v224
03220 start = end
03221 end += 8
03222 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03223 start = end
03224 end += 4
03225 (length,) = _struct_I.unpack(str[start:end])
03226 start = end
03227 end += length
03228 if python3:
03229 _v224.encoding = str[start:end].decode('utf-8')
03230 else:
03231 _v224.encoding = str[start:end]
03232 _x = _v224
03233 start = end
03234 end += 5
03235 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03236 start = end
03237 end += 4
03238 (length,) = _struct_I.unpack(str[start:end])
03239 start = end
03240 end += length
03241 if python3:
03242 _v224.data = str[start:end].decode('utf-8')
03243 else:
03244 _v224.data = str[start:end]
03245 _v227 = _v217.cam_info
03246 _v228 = _v227.header
03247 start = end
03248 end += 4
03249 (_v228.seq,) = _struct_I.unpack(str[start:end])
03250 _v229 = _v228.stamp
03251 _x = _v229
03252 start = end
03253 end += 8
03254 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03255 start = end
03256 end += 4
03257 (length,) = _struct_I.unpack(str[start:end])
03258 start = end
03259 end += length
03260 if python3:
03261 _v228.frame_id = str[start:end].decode('utf-8')
03262 else:
03263 _v228.frame_id = str[start:end]
03264 _x = _v227
03265 start = end
03266 end += 8
03267 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03268 start = end
03269 end += 4
03270 (length,) = _struct_I.unpack(str[start:end])
03271 start = end
03272 end += length
03273 if python3:
03274 _v227.distortion_model = str[start:end].decode('utf-8')
03275 else:
03276 _v227.distortion_model = str[start:end]
03277 start = end
03278 end += 4
03279 (length,) = _struct_I.unpack(str[start:end])
03280 pattern = '<%sd'%length
03281 start = end
03282 end += struct.calcsize(pattern)
03283 _v227.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03284 start = end
03285 end += 72
03286 _v227.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
03287 start = end
03288 end += 72
03289 _v227.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
03290 start = end
03291 end += 96
03292 _v227.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
03293 _x = _v227
03294 start = end
03295 end += 8
03296 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
03297 _v230 = _v227.roi
03298 _x = _v230
03299 start = end
03300 end += 17
03301 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
03302 _v230.do_rectify = bool(_v230.do_rectify)
03303 _v231 = _v217.roi_box_pose
03304 _v232 = _v231.header
03305 start = end
03306 end += 4
03307 (_v232.seq,) = _struct_I.unpack(str[start:end])
03308 _v233 = _v232.stamp
03309 _x = _v233
03310 start = end
03311 end += 8
03312 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03313 start = end
03314 end += 4
03315 (length,) = _struct_I.unpack(str[start:end])
03316 start = end
03317 end += length
03318 if python3:
03319 _v232.frame_id = str[start:end].decode('utf-8')
03320 else:
03321 _v232.frame_id = str[start:end]
03322 _v234 = _v231.pose
03323 _v235 = _v234.position
03324 _x = _v235
03325 start = end
03326 end += 24
03327 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03328 _v236 = _v234.orientation
03329 _x = _v236
03330 start = end
03331 end += 32
03332 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03333 _v237 = _v217.roi_box_dims
03334 _x = _v237
03335 start = end
03336 end += 24
03337 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03338 start = end
03339 end += 4
03340 (length,) = _struct_I.unpack(str[start:end])
03341 start = end
03342 end += length
03343 if python3:
03344 val1.collision_name = str[start:end].decode('utf-8')
03345 else:
03346 val1.collision_name = str[start:end]
03347 self.grasp.moved_obstacles.append(val1)
03348 start = end
03349 end += 4
03350 (length,) = _struct_I.unpack(str[start:end])
03351 self.attempted_grasps = []
03352 for i in range(0, length):
03353 val1 = object_manipulation_msgs.msg.Grasp()
03354 _v238 = val1.pre_grasp_posture
03355 _v239 = _v238.header
03356 start = end
03357 end += 4
03358 (_v239.seq,) = _struct_I.unpack(str[start:end])
03359 _v240 = _v239.stamp
03360 _x = _v240
03361 start = end
03362 end += 8
03363 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03364 start = end
03365 end += 4
03366 (length,) = _struct_I.unpack(str[start:end])
03367 start = end
03368 end += length
03369 if python3:
03370 _v239.frame_id = str[start:end].decode('utf-8')
03371 else:
03372 _v239.frame_id = str[start:end]
03373 start = end
03374 end += 4
03375 (length,) = _struct_I.unpack(str[start:end])
03376 _v238.name = []
03377 for i in range(0, length):
03378 start = end
03379 end += 4
03380 (length,) = _struct_I.unpack(str[start:end])
03381 start = end
03382 end += length
03383 if python3:
03384 val3 = str[start:end].decode('utf-8')
03385 else:
03386 val3 = str[start:end]
03387 _v238.name.append(val3)
03388 start = end
03389 end += 4
03390 (length,) = _struct_I.unpack(str[start:end])
03391 pattern = '<%sd'%length
03392 start = end
03393 end += struct.calcsize(pattern)
03394 _v238.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03395 start = end
03396 end += 4
03397 (length,) = _struct_I.unpack(str[start:end])
03398 pattern = '<%sd'%length
03399 start = end
03400 end += struct.calcsize(pattern)
03401 _v238.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03402 start = end
03403 end += 4
03404 (length,) = _struct_I.unpack(str[start:end])
03405 pattern = '<%sd'%length
03406 start = end
03407 end += struct.calcsize(pattern)
03408 _v238.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03409 _v241 = val1.grasp_posture
03410 _v242 = _v241.header
03411 start = end
03412 end += 4
03413 (_v242.seq,) = _struct_I.unpack(str[start:end])
03414 _v243 = _v242.stamp
03415 _x = _v243
03416 start = end
03417 end += 8
03418 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03419 start = end
03420 end += 4
03421 (length,) = _struct_I.unpack(str[start:end])
03422 start = end
03423 end += length
03424 if python3:
03425 _v242.frame_id = str[start:end].decode('utf-8')
03426 else:
03427 _v242.frame_id = str[start:end]
03428 start = end
03429 end += 4
03430 (length,) = _struct_I.unpack(str[start:end])
03431 _v241.name = []
03432 for i in range(0, length):
03433 start = end
03434 end += 4
03435 (length,) = _struct_I.unpack(str[start:end])
03436 start = end
03437 end += length
03438 if python3:
03439 val3 = str[start:end].decode('utf-8')
03440 else:
03441 val3 = str[start:end]
03442 _v241.name.append(val3)
03443 start = end
03444 end += 4
03445 (length,) = _struct_I.unpack(str[start:end])
03446 pattern = '<%sd'%length
03447 start = end
03448 end += struct.calcsize(pattern)
03449 _v241.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03450 start = end
03451 end += 4
03452 (length,) = _struct_I.unpack(str[start:end])
03453 pattern = '<%sd'%length
03454 start = end
03455 end += struct.calcsize(pattern)
03456 _v241.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03457 start = end
03458 end += 4
03459 (length,) = _struct_I.unpack(str[start:end])
03460 pattern = '<%sd'%length
03461 start = end
03462 end += struct.calcsize(pattern)
03463 _v241.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03464 _v244 = val1.grasp_pose
03465 _v245 = _v244.position
03466 _x = _v245
03467 start = end
03468 end += 24
03469 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03470 _v246 = _v244.orientation
03471 _x = _v246
03472 start = end
03473 end += 32
03474 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03475 _x = val1
03476 start = end
03477 end += 17
03478 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
03479 val1.cluster_rep = bool(val1.cluster_rep)
03480 start = end
03481 end += 4
03482 (length,) = _struct_I.unpack(str[start:end])
03483 val1.moved_obstacles = []
03484 for i in range(0, length):
03485 val2 = object_manipulation_msgs.msg.GraspableObject()
03486 start = end
03487 end += 4
03488 (length,) = _struct_I.unpack(str[start:end])
03489 start = end
03490 end += length
03491 if python3:
03492 val2.reference_frame_id = str[start:end].decode('utf-8')
03493 else:
03494 val2.reference_frame_id = str[start:end]
03495 start = end
03496 end += 4
03497 (length,) = _struct_I.unpack(str[start:end])
03498 val2.potential_models = []
03499 for i in range(0, length):
03500 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
03501 start = end
03502 end += 4
03503 (val3.model_id,) = _struct_i.unpack(str[start:end])
03504 _v247 = val3.pose
03505 _v248 = _v247.header
03506 start = end
03507 end += 4
03508 (_v248.seq,) = _struct_I.unpack(str[start:end])
03509 _v249 = _v248.stamp
03510 _x = _v249
03511 start = end
03512 end += 8
03513 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03514 start = end
03515 end += 4
03516 (length,) = _struct_I.unpack(str[start:end])
03517 start = end
03518 end += length
03519 if python3:
03520 _v248.frame_id = str[start:end].decode('utf-8')
03521 else:
03522 _v248.frame_id = str[start:end]
03523 _v250 = _v247.pose
03524 _v251 = _v250.position
03525 _x = _v251
03526 start = end
03527 end += 24
03528 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03529 _v252 = _v250.orientation
03530 _x = _v252
03531 start = end
03532 end += 32
03533 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03534 start = end
03535 end += 4
03536 (val3.confidence,) = _struct_f.unpack(str[start:end])
03537 start = end
03538 end += 4
03539 (length,) = _struct_I.unpack(str[start:end])
03540 start = end
03541 end += length
03542 if python3:
03543 val3.detector_name = str[start:end].decode('utf-8')
03544 else:
03545 val3.detector_name = str[start:end]
03546 val2.potential_models.append(val3)
03547 _v253 = val2.cluster
03548 _v254 = _v253.header
03549 start = end
03550 end += 4
03551 (_v254.seq,) = _struct_I.unpack(str[start:end])
03552 _v255 = _v254.stamp
03553 _x = _v255
03554 start = end
03555 end += 8
03556 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03557 start = end
03558 end += 4
03559 (length,) = _struct_I.unpack(str[start:end])
03560 start = end
03561 end += length
03562 if python3:
03563 _v254.frame_id = str[start:end].decode('utf-8')
03564 else:
03565 _v254.frame_id = str[start:end]
03566 start = end
03567 end += 4
03568 (length,) = _struct_I.unpack(str[start:end])
03569 _v253.points = []
03570 for i in range(0, length):
03571 val4 = geometry_msgs.msg.Point32()
03572 _x = val4
03573 start = end
03574 end += 12
03575 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03576 _v253.points.append(val4)
03577 start = end
03578 end += 4
03579 (length,) = _struct_I.unpack(str[start:end])
03580 _v253.channels = []
03581 for i in range(0, length):
03582 val4 = sensor_msgs.msg.ChannelFloat32()
03583 start = end
03584 end += 4
03585 (length,) = _struct_I.unpack(str[start:end])
03586 start = end
03587 end += length
03588 if python3:
03589 val4.name = str[start:end].decode('utf-8')
03590 else:
03591 val4.name = str[start:end]
03592 start = end
03593 end += 4
03594 (length,) = _struct_I.unpack(str[start:end])
03595 pattern = '<%sf'%length
03596 start = end
03597 end += struct.calcsize(pattern)
03598 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
03599 _v253.channels.append(val4)
03600 _v256 = val2.region
03601 _v257 = _v256.cloud
03602 _v258 = _v257.header
03603 start = end
03604 end += 4
03605 (_v258.seq,) = _struct_I.unpack(str[start:end])
03606 _v259 = _v258.stamp
03607 _x = _v259
03608 start = end
03609 end += 8
03610 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03611 start = end
03612 end += 4
03613 (length,) = _struct_I.unpack(str[start:end])
03614 start = end
03615 end += length
03616 if python3:
03617 _v258.frame_id = str[start:end].decode('utf-8')
03618 else:
03619 _v258.frame_id = str[start:end]
03620 _x = _v257
03621 start = end
03622 end += 8
03623 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03624 start = end
03625 end += 4
03626 (length,) = _struct_I.unpack(str[start:end])
03627 _v257.fields = []
03628 for i in range(0, length):
03629 val5 = sensor_msgs.msg.PointField()
03630 start = end
03631 end += 4
03632 (length,) = _struct_I.unpack(str[start:end])
03633 start = end
03634 end += length
03635 if python3:
03636 val5.name = str[start:end].decode('utf-8')
03637 else:
03638 val5.name = str[start:end]
03639 _x = val5
03640 start = end
03641 end += 9
03642 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
03643 _v257.fields.append(val5)
03644 _x = _v257
03645 start = end
03646 end += 9
03647 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
03648 _v257.is_bigendian = bool(_v257.is_bigendian)
03649 start = end
03650 end += 4
03651 (length,) = _struct_I.unpack(str[start:end])
03652 start = end
03653 end += length
03654 if python3:
03655 _v257.data = str[start:end].decode('utf-8')
03656 else:
03657 _v257.data = str[start:end]
03658 start = end
03659 end += 1
03660 (_v257.is_dense,) = _struct_B.unpack(str[start:end])
03661 _v257.is_dense = bool(_v257.is_dense)
03662 start = end
03663 end += 4
03664 (length,) = _struct_I.unpack(str[start:end])
03665 pattern = '<%si'%length
03666 start = end
03667 end += struct.calcsize(pattern)
03668 _v256.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
03669 _v260 = _v256.image
03670 _v261 = _v260.header
03671 start = end
03672 end += 4
03673 (_v261.seq,) = _struct_I.unpack(str[start:end])
03674 _v262 = _v261.stamp
03675 _x = _v262
03676 start = end
03677 end += 8
03678 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03679 start = end
03680 end += 4
03681 (length,) = _struct_I.unpack(str[start:end])
03682 start = end
03683 end += length
03684 if python3:
03685 _v261.frame_id = str[start:end].decode('utf-8')
03686 else:
03687 _v261.frame_id = str[start:end]
03688 _x = _v260
03689 start = end
03690 end += 8
03691 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03692 start = end
03693 end += 4
03694 (length,) = _struct_I.unpack(str[start:end])
03695 start = end
03696 end += length
03697 if python3:
03698 _v260.encoding = str[start:end].decode('utf-8')
03699 else:
03700 _v260.encoding = str[start:end]
03701 _x = _v260
03702 start = end
03703 end += 5
03704 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03705 start = end
03706 end += 4
03707 (length,) = _struct_I.unpack(str[start:end])
03708 start = end
03709 end += length
03710 if python3:
03711 _v260.data = str[start:end].decode('utf-8')
03712 else:
03713 _v260.data = str[start:end]
03714 _v263 = _v256.disparity_image
03715 _v264 = _v263.header
03716 start = end
03717 end += 4
03718 (_v264.seq,) = _struct_I.unpack(str[start:end])
03719 _v265 = _v264.stamp
03720 _x = _v265
03721 start = end
03722 end += 8
03723 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03724 start = end
03725 end += 4
03726 (length,) = _struct_I.unpack(str[start:end])
03727 start = end
03728 end += length
03729 if python3:
03730 _v264.frame_id = str[start:end].decode('utf-8')
03731 else:
03732 _v264.frame_id = str[start:end]
03733 _x = _v263
03734 start = end
03735 end += 8
03736 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03737 start = end
03738 end += 4
03739 (length,) = _struct_I.unpack(str[start:end])
03740 start = end
03741 end += length
03742 if python3:
03743 _v263.encoding = str[start:end].decode('utf-8')
03744 else:
03745 _v263.encoding = str[start:end]
03746 _x = _v263
03747 start = end
03748 end += 5
03749 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03750 start = end
03751 end += 4
03752 (length,) = _struct_I.unpack(str[start:end])
03753 start = end
03754 end += length
03755 if python3:
03756 _v263.data = str[start:end].decode('utf-8')
03757 else:
03758 _v263.data = str[start:end]
03759 _v266 = _v256.cam_info
03760 _v267 = _v266.header
03761 start = end
03762 end += 4
03763 (_v267.seq,) = _struct_I.unpack(str[start:end])
03764 _v268 = _v267.stamp
03765 _x = _v268
03766 start = end
03767 end += 8
03768 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03769 start = end
03770 end += 4
03771 (length,) = _struct_I.unpack(str[start:end])
03772 start = end
03773 end += length
03774 if python3:
03775 _v267.frame_id = str[start:end].decode('utf-8')
03776 else:
03777 _v267.frame_id = str[start:end]
03778 _x = _v266
03779 start = end
03780 end += 8
03781 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03782 start = end
03783 end += 4
03784 (length,) = _struct_I.unpack(str[start:end])
03785 start = end
03786 end += length
03787 if python3:
03788 _v266.distortion_model = str[start:end].decode('utf-8')
03789 else:
03790 _v266.distortion_model = str[start:end]
03791 start = end
03792 end += 4
03793 (length,) = _struct_I.unpack(str[start:end])
03794 pattern = '<%sd'%length
03795 start = end
03796 end += struct.calcsize(pattern)
03797 _v266.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
03798 start = end
03799 end += 72
03800 _v266.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
03801 start = end
03802 end += 72
03803 _v266.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
03804 start = end
03805 end += 96
03806 _v266.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
03807 _x = _v266
03808 start = end
03809 end += 8
03810 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
03811 _v269 = _v266.roi
03812 _x = _v269
03813 start = end
03814 end += 17
03815 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
03816 _v269.do_rectify = bool(_v269.do_rectify)
03817 _v270 = _v256.roi_box_pose
03818 _v271 = _v270.header
03819 start = end
03820 end += 4
03821 (_v271.seq,) = _struct_I.unpack(str[start:end])
03822 _v272 = _v271.stamp
03823 _x = _v272
03824 start = end
03825 end += 8
03826 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03827 start = end
03828 end += 4
03829 (length,) = _struct_I.unpack(str[start:end])
03830 start = end
03831 end += length
03832 if python3:
03833 _v271.frame_id = str[start:end].decode('utf-8')
03834 else:
03835 _v271.frame_id = str[start:end]
03836 _v273 = _v270.pose
03837 _v274 = _v273.position
03838 _x = _v274
03839 start = end
03840 end += 24
03841 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03842 _v275 = _v273.orientation
03843 _x = _v275
03844 start = end
03845 end += 32
03846 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03847 _v276 = _v256.roi_box_dims
03848 _x = _v276
03849 start = end
03850 end += 24
03851 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03852 start = end
03853 end += 4
03854 (length,) = _struct_I.unpack(str[start:end])
03855 start = end
03856 end += length
03857 if python3:
03858 val2.collision_name = str[start:end].decode('utf-8')
03859 else:
03860 val2.collision_name = str[start:end]
03861 val1.moved_obstacles.append(val2)
03862 self.attempted_grasps.append(val1)
03863 start = end
03864 end += 4
03865 (length,) = _struct_I.unpack(str[start:end])
03866 self.attempted_grasp_results = []
03867 for i in range(0, length):
03868 val1 = object_manipulation_msgs.msg.GraspResult()
03869 _x = val1
03870 start = end
03871 end += 5
03872 (_x.result_code, _x.continuation_possible,) = _struct_iB.unpack(str[start:end])
03873 val1.continuation_possible = bool(val1.continuation_possible)
03874 self.attempted_grasp_results.append(val1)
03875 return self
03876 except struct.error as e:
03877 raise genpy.DeserializationError(e) #most likely buffer underfill
03878
03879 _struct_I = genpy.struct_I
03880 _struct_IBI = struct.Struct("<IBI")
03881 _struct_8dB2f = struct.Struct("<8dB2f")
03882 _struct_12d = struct.Struct("<12d")
03883 _struct_f = struct.Struct("<f")
03884 _struct_i = struct.Struct("<i")
03885 _struct_dB2f = struct.Struct("<dB2f")
03886 _struct_3I = struct.Struct("<3I")
03887 _struct_3f = struct.Struct("<3f")
03888 _struct_i3I = struct.Struct("<i3I")
03889 _struct_B = struct.Struct("<B")
03890 _struct_9d = struct.Struct("<9d")
03891 _struct_B2I = struct.Struct("<B2I")
03892 _struct_4d = struct.Struct("<4d")
03893 _struct_iB = struct.Struct("<iB")
03894 _struct_BI = struct.Struct("<BI")
03895 _struct_2I = struct.Struct("<2I")
03896 _struct_4IB = struct.Struct("<4IB")
03897 _struct_3d = struct.Struct("<3d")