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