00001 """autogenerated by genpy from object_manipulation_msgs/GraspPlanningAction.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 std_msgs.msg
00012 import genpy
00013 import household_objects_database_msgs.msg
00014
00015 class GraspPlanningAction(genpy.Message):
00016 _md5sum = "633fe6f10f37f6a935a5aa9ae19f25c5"
00017 _type = "object_manipulation_msgs/GraspPlanningAction"
00018 _has_header = False
00019 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00020
00021 GraspPlanningActionGoal action_goal
00022 GraspPlanningActionResult action_result
00023 GraspPlanningActionFeedback action_feedback
00024
00025 ================================================================================
00026 MSG: object_manipulation_msgs/GraspPlanningActionGoal
00027 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00028
00029 Header header
00030 actionlib_msgs/GoalID goal_id
00031 GraspPlanningGoal goal
00032
00033 ================================================================================
00034 MSG: std_msgs/Header
00035 # Standard metadata for higher-level stamped data types.
00036 # This is generally used to communicate timestamped data
00037 # in a particular coordinate frame.
00038 #
00039 # sequence ID: consecutively increasing ID
00040 uint32 seq
00041 #Two-integer timestamp that is expressed as:
00042 # * stamp.secs: seconds (stamp_secs) since epoch
00043 # * stamp.nsecs: nanoseconds since stamp_secs
00044 # time-handling sugar is provided by the client library
00045 time stamp
00046 #Frame this data is associated with
00047 # 0: no frame
00048 # 1: global frame
00049 string frame_id
00050
00051 ================================================================================
00052 MSG: actionlib_msgs/GoalID
00053 # The stamp should store the time at which this goal was requested.
00054 # It is used by an action server when it tries to preempt all
00055 # goals that were requested before a certain time
00056 time stamp
00057
00058 # The id provides a way to associate feedback and
00059 # result message with specific goal requests. The id
00060 # specified must be unique.
00061 string id
00062
00063
00064 ================================================================================
00065 MSG: object_manipulation_msgs/GraspPlanningGoal
00066 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00067 # Requests that grasp planning be performed on the object to be grasped
00068 # returns a list of grasps to be tested and executed
00069
00070 # the arm being used
00071 string arm_name
00072
00073 # the object to be grasped
00074 GraspableObject target
00075
00076 # the name that the target object has in the collision environment
00077 # can be left empty if no name is available
00078 string collision_object_name
00079
00080 # the name that the support surface (e.g. table) has in the collision map
00081 # can be left empty if no name is available
00082 string collision_support_surface_name
00083
00084 # an optional list of grasps to be evaluated by the planner
00085 Grasp[] grasps_to_evaluate
00086
00087 # an optional list of obstacles that we have semantic information about
00088 # and that can be moved in the course of grasping
00089 GraspableObject[] movable_obstacles
00090
00091
00092 ================================================================================
00093 MSG: object_manipulation_msgs/GraspableObject
00094 # an object that the object_manipulator can work on
00095
00096 # a graspable object can be represented in multiple ways. This message
00097 # can contain all of them. Which one is actually used is up to the receiver
00098 # of this message. When adding new representations, one must be careful that
00099 # they have reasonable lightweight defaults indicating that that particular
00100 # representation is not available.
00101
00102 # the tf frame to be used as a reference frame when combining information from
00103 # the different representations below
00104 string reference_frame_id
00105
00106 # potential recognition results from a database of models
00107 # all poses are relative to the object reference pose
00108 household_objects_database_msgs/DatabaseModelPose[] potential_models
00109
00110 # the point cloud itself
00111 sensor_msgs/PointCloud cluster
00112
00113 # a region of a PointCloud2 of interest
00114 object_manipulation_msgs/SceneRegion region
00115
00116 # the name that this object has in the collision environment
00117 string collision_name
00118 ================================================================================
00119 MSG: household_objects_database_msgs/DatabaseModelPose
00120 # Informs that a specific model from the Model Database has been
00121 # identified at a certain location
00122
00123 # the database id of the model
00124 int32 model_id
00125
00126 # the pose that it can be found in
00127 geometry_msgs/PoseStamped pose
00128
00129 # a measure of the confidence level in this detection result
00130 float32 confidence
00131
00132 # the name of the object detector that generated this detection result
00133 string detector_name
00134
00135 ================================================================================
00136 MSG: geometry_msgs/PoseStamped
00137 # A Pose with reference coordinate frame and timestamp
00138 Header header
00139 Pose pose
00140
00141 ================================================================================
00142 MSG: geometry_msgs/Pose
00143 # A representation of pose in free space, composed of postion and orientation.
00144 Point position
00145 Quaternion orientation
00146
00147 ================================================================================
00148 MSG: geometry_msgs/Point
00149 # This contains the position of a point in free space
00150 float64 x
00151 float64 y
00152 float64 z
00153
00154 ================================================================================
00155 MSG: geometry_msgs/Quaternion
00156 # This represents an orientation in free space in quaternion form.
00157
00158 float64 x
00159 float64 y
00160 float64 z
00161 float64 w
00162
00163 ================================================================================
00164 MSG: sensor_msgs/PointCloud
00165 # This message holds a collection of 3d points, plus optional additional
00166 # information about each point.
00167
00168 # Time of sensor data acquisition, coordinate frame ID.
00169 Header header
00170
00171 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00172 # in the frame given in the header.
00173 geometry_msgs/Point32[] points
00174
00175 # Each channel should have the same number of elements as points array,
00176 # and the data in each channel should correspond 1:1 with each point.
00177 # Channel names in common practice are listed in ChannelFloat32.msg.
00178 ChannelFloat32[] channels
00179
00180 ================================================================================
00181 MSG: geometry_msgs/Point32
00182 # This contains the position of a point in free space(with 32 bits of precision).
00183 # It is recommeded to use Point wherever possible instead of Point32.
00184 #
00185 # This recommendation is to promote interoperability.
00186 #
00187 # This message is designed to take up less space when sending
00188 # lots of points at once, as in the case of a PointCloud.
00189
00190 float32 x
00191 float32 y
00192 float32 z
00193 ================================================================================
00194 MSG: sensor_msgs/ChannelFloat32
00195 # This message is used by the PointCloud message to hold optional data
00196 # associated with each point in the cloud. The length of the values
00197 # array should be the same as the length of the points array in the
00198 # PointCloud, and each value should be associated with the corresponding
00199 # point.
00200
00201 # Channel names in existing practice include:
00202 # "u", "v" - row and column (respectively) in the left stereo image.
00203 # This is opposite to usual conventions but remains for
00204 # historical reasons. The newer PointCloud2 message has no
00205 # such problem.
00206 # "rgb" - For point clouds produced by color stereo cameras. uint8
00207 # (R,G,B) values packed into the least significant 24 bits,
00208 # in order.
00209 # "intensity" - laser or pixel intensity.
00210 # "distance"
00211
00212 # The channel name should give semantics of the channel (e.g.
00213 # "intensity" instead of "value").
00214 string name
00215
00216 # The values array should be 1-1 with the elements of the associated
00217 # PointCloud.
00218 float32[] values
00219
00220 ================================================================================
00221 MSG: object_manipulation_msgs/SceneRegion
00222 # Point cloud
00223 sensor_msgs/PointCloud2 cloud
00224
00225 # Indices for the region of interest
00226 int32[] mask
00227
00228 # One of the corresponding 2D images, if applicable
00229 sensor_msgs/Image image
00230
00231 # The disparity image, if applicable
00232 sensor_msgs/Image disparity_image
00233
00234 # Camera info for the camera that took the image
00235 sensor_msgs/CameraInfo cam_info
00236
00237 # a 3D region of interest for grasp planning
00238 geometry_msgs/PoseStamped roi_box_pose
00239 geometry_msgs/Vector3 roi_box_dims
00240
00241 ================================================================================
00242 MSG: sensor_msgs/PointCloud2
00243 # This message holds a collection of N-dimensional points, which may
00244 # contain additional information such as normals, intensity, etc. The
00245 # point data is stored as a binary blob, its layout described by the
00246 # contents of the "fields" array.
00247
00248 # The point cloud data may be organized 2d (image-like) or 1d
00249 # (unordered). Point clouds organized as 2d images may be produced by
00250 # camera depth sensors such as stereo or time-of-flight.
00251
00252 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00253 # points).
00254 Header header
00255
00256 # 2D structure of the point cloud. If the cloud is unordered, height is
00257 # 1 and width is the length of the point cloud.
00258 uint32 height
00259 uint32 width
00260
00261 # Describes the channels and their layout in the binary data blob.
00262 PointField[] fields
00263
00264 bool is_bigendian # Is this data bigendian?
00265 uint32 point_step # Length of a point in bytes
00266 uint32 row_step # Length of a row in bytes
00267 uint8[] data # Actual point data, size is (row_step*height)
00268
00269 bool is_dense # True if there are no invalid points
00270
00271 ================================================================================
00272 MSG: sensor_msgs/PointField
00273 # This message holds the description of one point entry in the
00274 # PointCloud2 message format.
00275 uint8 INT8 = 1
00276 uint8 UINT8 = 2
00277 uint8 INT16 = 3
00278 uint8 UINT16 = 4
00279 uint8 INT32 = 5
00280 uint8 UINT32 = 6
00281 uint8 FLOAT32 = 7
00282 uint8 FLOAT64 = 8
00283
00284 string name # Name of field
00285 uint32 offset # Offset from start of point struct
00286 uint8 datatype # Datatype enumeration, see above
00287 uint32 count # How many elements in the field
00288
00289 ================================================================================
00290 MSG: sensor_msgs/Image
00291 # This message contains an uncompressed image
00292 # (0, 0) is at top-left corner of image
00293 #
00294
00295 Header header # Header timestamp should be acquisition time of image
00296 # Header frame_id should be optical frame of camera
00297 # origin of frame should be optical center of cameara
00298 # +x should point to the right in the image
00299 # +y should point down in the image
00300 # +z should point into to plane of the image
00301 # If the frame_id here and the frame_id of the CameraInfo
00302 # message associated with the image conflict
00303 # the behavior is undefined
00304
00305 uint32 height # image height, that is, number of rows
00306 uint32 width # image width, that is, number of columns
00307
00308 # The legal values for encoding are in file src/image_encodings.cpp
00309 # If you want to standardize a new string format, join
00310 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00311
00312 string encoding # Encoding of pixels -- channel meaning, ordering, size
00313 # taken from the list of strings in src/image_encodings.cpp
00314
00315 uint8 is_bigendian # is this data bigendian?
00316 uint32 step # Full row length in bytes
00317 uint8[] data # actual matrix data, size is (step * rows)
00318
00319 ================================================================================
00320 MSG: sensor_msgs/CameraInfo
00321 # This message defines meta information for a camera. It should be in a
00322 # camera namespace on topic "camera_info" and accompanied by up to five
00323 # image topics named:
00324 #
00325 # image_raw - raw data from the camera driver, possibly Bayer encoded
00326 # image - monochrome, distorted
00327 # image_color - color, distorted
00328 # image_rect - monochrome, rectified
00329 # image_rect_color - color, rectified
00330 #
00331 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00332 # for producing the four processed image topics from image_raw and
00333 # camera_info. The meaning of the camera parameters are described in
00334 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00335 #
00336 # The image_geometry package provides a user-friendly interface to
00337 # common operations using this meta information. If you want to, e.g.,
00338 # project a 3d point into image coordinates, we strongly recommend
00339 # using image_geometry.
00340 #
00341 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00342 # zeroed out. In particular, clients may assume that K[0] == 0.0
00343 # indicates an uncalibrated camera.
00344
00345 #######################################################################
00346 # Image acquisition info #
00347 #######################################################################
00348
00349 # Time of image acquisition, camera coordinate frame ID
00350 Header header # Header timestamp should be acquisition time of image
00351 # Header frame_id should be optical frame of camera
00352 # origin of frame should be optical center of camera
00353 # +x should point to the right in the image
00354 # +y should point down in the image
00355 # +z should point into the plane of the image
00356
00357
00358 #######################################################################
00359 # Calibration Parameters #
00360 #######################################################################
00361 # These are fixed during camera calibration. Their values will be the #
00362 # same in all messages until the camera is recalibrated. Note that #
00363 # self-calibrating systems may "recalibrate" frequently. #
00364 # #
00365 # The internal parameters can be used to warp a raw (distorted) image #
00366 # to: #
00367 # 1. An undistorted image (requires D and K) #
00368 # 2. A rectified image (requires D, K, R) #
00369 # The projection matrix P projects 3D points into the rectified image.#
00370 #######################################################################
00371
00372 # The image dimensions with which the camera was calibrated. Normally
00373 # this will be the full camera resolution in pixels.
00374 uint32 height
00375 uint32 width
00376
00377 # The distortion model used. Supported models are listed in
00378 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00379 # simple model of radial and tangential distortion - is sufficent.
00380 string distortion_model
00381
00382 # The distortion parameters, size depending on the distortion model.
00383 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00384 float64[] D
00385
00386 # Intrinsic camera matrix for the raw (distorted) images.
00387 # [fx 0 cx]
00388 # K = [ 0 fy cy]
00389 # [ 0 0 1]
00390 # Projects 3D points in the camera coordinate frame to 2D pixel
00391 # coordinates using the focal lengths (fx, fy) and principal point
00392 # (cx, cy).
00393 float64[9] K # 3x3 row-major matrix
00394
00395 # Rectification matrix (stereo cameras only)
00396 # A rotation matrix aligning the camera coordinate system to the ideal
00397 # stereo image plane so that epipolar lines in both stereo images are
00398 # parallel.
00399 float64[9] R # 3x3 row-major matrix
00400
00401 # Projection/camera matrix
00402 # [fx' 0 cx' Tx]
00403 # P = [ 0 fy' cy' Ty]
00404 # [ 0 0 1 0]
00405 # By convention, this matrix specifies the intrinsic (camera) matrix
00406 # of the processed (rectified) image. That is, the left 3x3 portion
00407 # is the normal camera intrinsic matrix for the rectified image.
00408 # It projects 3D points in the camera coordinate frame to 2D pixel
00409 # coordinates using the focal lengths (fx', fy') and principal point
00410 # (cx', cy') - these may differ from the values in K.
00411 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00412 # also have R = the identity and P[1:3,1:3] = K.
00413 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00414 # position of the optical center of the second camera in the first
00415 # camera's frame. We assume Tz = 0 so both cameras are in the same
00416 # stereo image plane. The first camera always has Tx = Ty = 0. For
00417 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00418 # Tx = -fx' * B, where B is the baseline between the cameras.
00419 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00420 # the rectified image is given by:
00421 # [u v w]' = P * [X Y Z 1]'
00422 # x = u / w
00423 # y = v / w
00424 # This holds for both images of a stereo pair.
00425 float64[12] P # 3x4 row-major matrix
00426
00427
00428 #######################################################################
00429 # Operational Parameters #
00430 #######################################################################
00431 # These define the image region actually captured by the camera #
00432 # driver. Although they affect the geometry of the output image, they #
00433 # may be changed freely without recalibrating the camera. #
00434 #######################################################################
00435
00436 # Binning refers here to any camera setting which combines rectangular
00437 # neighborhoods of pixels into larger "super-pixels." It reduces the
00438 # resolution of the output image to
00439 # (width / binning_x) x (height / binning_y).
00440 # The default values binning_x = binning_y = 0 is considered the same
00441 # as binning_x = binning_y = 1 (no subsampling).
00442 uint32 binning_x
00443 uint32 binning_y
00444
00445 # Region of interest (subwindow of full camera resolution), given in
00446 # full resolution (unbinned) image coordinates. A particular ROI
00447 # always denotes the same window of pixels on the camera sensor,
00448 # regardless of binning settings.
00449 # The default setting of roi (all values 0) is considered the same as
00450 # full resolution (roi.width = width, roi.height = height).
00451 RegionOfInterest roi
00452
00453 ================================================================================
00454 MSG: sensor_msgs/RegionOfInterest
00455 # This message is used to specify a region of interest within an image.
00456 #
00457 # When used to specify the ROI setting of the camera when the image was
00458 # taken, the height and width fields should either match the height and
00459 # width fields for the associated image; or height = width = 0
00460 # indicates that the full resolution image was captured.
00461
00462 uint32 x_offset # Leftmost pixel of the ROI
00463 # (0 if the ROI includes the left edge of the image)
00464 uint32 y_offset # Topmost pixel of the ROI
00465 # (0 if the ROI includes the top edge of the image)
00466 uint32 height # Height of ROI
00467 uint32 width # Width of ROI
00468
00469 # True if a distinct rectified ROI should be calculated from the "raw"
00470 # ROI in this message. Typically this should be False if the full image
00471 # is captured (ROI not used), and True if a subwindow is captured (ROI
00472 # used).
00473 bool do_rectify
00474
00475 ================================================================================
00476 MSG: geometry_msgs/Vector3
00477 # This represents a vector in free space.
00478
00479 float64 x
00480 float64 y
00481 float64 z
00482 ================================================================================
00483 MSG: object_manipulation_msgs/Grasp
00484
00485 # The internal posture of the hand for the pre-grasp
00486 # only positions are used
00487 sensor_msgs/JointState pre_grasp_posture
00488
00489 # The internal posture of the hand for the grasp
00490 # positions and efforts are used
00491 sensor_msgs/JointState grasp_posture
00492
00493 # The position of the end-effector for the grasp relative to a reference frame
00494 # (that is always specified elsewhere, not in this message)
00495 geometry_msgs/Pose grasp_pose
00496
00497 # The estimated probability of success for this grasp
00498 float64 success_probability
00499
00500 # Debug flag to indicate that this grasp would be the best in its cluster
00501 bool cluster_rep
00502
00503 # how far the pre-grasp should ideally be away from the grasp
00504 float32 desired_approach_distance
00505
00506 # how much distance between pre-grasp and grasp must actually be feasible
00507 # for the grasp not to be rejected
00508 float32 min_approach_distance
00509
00510 # an optional list of obstacles that we have semantic information about
00511 # and that we expect might move in the course of executing this grasp
00512 # the grasp planner is expected to make sure they move in an OK way; during
00513 # execution, grasp executors will not check for collisions against these objects
00514 GraspableObject[] moved_obstacles
00515
00516 ================================================================================
00517 MSG: sensor_msgs/JointState
00518 # This is a message that holds data to describe the state of a set of torque controlled joints.
00519 #
00520 # The state of each joint (revolute or prismatic) is defined by:
00521 # * the position of the joint (rad or m),
00522 # * the velocity of the joint (rad/s or m/s) and
00523 # * the effort that is applied in the joint (Nm or N).
00524 #
00525 # Each joint is uniquely identified by its name
00526 # The header specifies the time at which the joint states were recorded. All the joint states
00527 # in one message have to be recorded at the same time.
00528 #
00529 # This message consists of a multiple arrays, one for each part of the joint state.
00530 # The goal is to make each of the fields optional. When e.g. your joints have no
00531 # effort associated with them, you can leave the effort array empty.
00532 #
00533 # All arrays in this message should have the same size, or be empty.
00534 # This is the only way to uniquely associate the joint name with the correct
00535 # states.
00536
00537
00538 Header header
00539
00540 string[] name
00541 float64[] position
00542 float64[] velocity
00543 float64[] effort
00544
00545 ================================================================================
00546 MSG: object_manipulation_msgs/GraspPlanningActionResult
00547 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00548
00549 Header header
00550 actionlib_msgs/GoalStatus status
00551 GraspPlanningResult result
00552
00553 ================================================================================
00554 MSG: actionlib_msgs/GoalStatus
00555 GoalID goal_id
00556 uint8 status
00557 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00558 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00559 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00560 # and has since completed its execution (Terminal State)
00561 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00562 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00563 # to some failure (Terminal State)
00564 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00565 # because the goal was unattainable or invalid (Terminal State)
00566 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00567 # and has not yet completed execution
00568 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00569 # but the action server has not yet confirmed that the goal is canceled
00570 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00571 # and was successfully cancelled (Terminal State)
00572 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00573 # sent over the wire by an action server
00574
00575 #Allow for the user to associate a string with GoalStatus for debugging
00576 string text
00577
00578
00579 ================================================================================
00580 MSG: object_manipulation_msgs/GraspPlanningResult
00581 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00582
00583 # the list of planned grasps
00584 Grasp[] grasps
00585
00586 # whether an error occurred
00587 GraspPlanningErrorCode error_code
00588
00589
00590 ================================================================================
00591 MSG: object_manipulation_msgs/GraspPlanningErrorCode
00592 # Error codes for grasp and place planning
00593
00594 # plan completed as expected
00595 int32 SUCCESS = 0
00596
00597 # tf error encountered while transforming
00598 int32 TF_ERROR = 1
00599
00600 # some other error
00601 int32 OTHER_ERROR = 2
00602
00603 # the actual value of this error code
00604 int32 value
00605 ================================================================================
00606 MSG: object_manipulation_msgs/GraspPlanningActionFeedback
00607 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00608
00609 Header header
00610 actionlib_msgs/GoalStatus status
00611 GraspPlanningFeedback feedback
00612
00613 ================================================================================
00614 MSG: object_manipulation_msgs/GraspPlanningFeedback
00615 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00616
00617 # grasps planned so far
00618 Grasp[] grasps
00619
00620
00621
00622 """
00623 __slots__ = ['action_goal','action_result','action_feedback']
00624 _slot_types = ['object_manipulation_msgs/GraspPlanningActionGoal','object_manipulation_msgs/GraspPlanningActionResult','object_manipulation_msgs/GraspPlanningActionFeedback']
00625
00626 def __init__(self, *args, **kwds):
00627 """
00628 Constructor. Any message fields that are implicitly/explicitly
00629 set to None will be assigned a default value. The recommend
00630 use is keyword arguments as this is more robust to future message
00631 changes. You cannot mix in-order arguments and keyword arguments.
00632
00633 The available fields are:
00634 action_goal,action_result,action_feedback
00635
00636 :param args: complete set of field values, in .msg order
00637 :param kwds: use keyword arguments corresponding to message field names
00638 to set specific fields.
00639 """
00640 if args or kwds:
00641 super(GraspPlanningAction, self).__init__(*args, **kwds)
00642 #message fields cannot be None, assign default values for those that are
00643 if self.action_goal is None:
00644 self.action_goal = object_manipulation_msgs.msg.GraspPlanningActionGoal()
00645 if self.action_result is None:
00646 self.action_result = object_manipulation_msgs.msg.GraspPlanningActionResult()
00647 if self.action_feedback is None:
00648 self.action_feedback = object_manipulation_msgs.msg.GraspPlanningActionFeedback()
00649 else:
00650 self.action_goal = object_manipulation_msgs.msg.GraspPlanningActionGoal()
00651 self.action_result = object_manipulation_msgs.msg.GraspPlanningActionResult()
00652 self.action_feedback = object_manipulation_msgs.msg.GraspPlanningActionFeedback()
00653
00654 def _get_types(self):
00655 """
00656 internal API method
00657 """
00658 return self._slot_types
00659
00660 def serialize(self, buff):
00661 """
00662 serialize message into buffer
00663 :param buff: buffer, ``StringIO``
00664 """
00665 try:
00666 _x = self
00667 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00668 _x = self.action_goal.header.frame_id
00669 length = len(_x)
00670 if python3 or type(_x) == unicode:
00671 _x = _x.encode('utf-8')
00672 length = len(_x)
00673 buff.write(struct.pack('<I%ss'%length, length, _x))
00674 _x = self
00675 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00676 _x = self.action_goal.goal_id.id
00677 length = len(_x)
00678 if python3 or type(_x) == unicode:
00679 _x = _x.encode('utf-8')
00680 length = len(_x)
00681 buff.write(struct.pack('<I%ss'%length, length, _x))
00682 _x = self.action_goal.goal.arm_name
00683 length = len(_x)
00684 if python3 or type(_x) == unicode:
00685 _x = _x.encode('utf-8')
00686 length = len(_x)
00687 buff.write(struct.pack('<I%ss'%length, length, _x))
00688 _x = self.action_goal.goal.target.reference_frame_id
00689 length = len(_x)
00690 if python3 or type(_x) == unicode:
00691 _x = _x.encode('utf-8')
00692 length = len(_x)
00693 buff.write(struct.pack('<I%ss'%length, length, _x))
00694 length = len(self.action_goal.goal.target.potential_models)
00695 buff.write(_struct_I.pack(length))
00696 for val1 in self.action_goal.goal.target.potential_models:
00697 buff.write(_struct_i.pack(val1.model_id))
00698 _v1 = val1.pose
00699 _v2 = _v1.header
00700 buff.write(_struct_I.pack(_v2.seq))
00701 _v3 = _v2.stamp
00702 _x = _v3
00703 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00704 _x = _v2.frame_id
00705 length = len(_x)
00706 if python3 or type(_x) == unicode:
00707 _x = _x.encode('utf-8')
00708 length = len(_x)
00709 buff.write(struct.pack('<I%ss'%length, length, _x))
00710 _v4 = _v1.pose
00711 _v5 = _v4.position
00712 _x = _v5
00713 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00714 _v6 = _v4.orientation
00715 _x = _v6
00716 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00717 buff.write(_struct_f.pack(val1.confidence))
00718 _x = val1.detector_name
00719 length = len(_x)
00720 if python3 or type(_x) == unicode:
00721 _x = _x.encode('utf-8')
00722 length = len(_x)
00723 buff.write(struct.pack('<I%ss'%length, length, _x))
00724 _x = self
00725 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs))
00726 _x = self.action_goal.goal.target.cluster.header.frame_id
00727 length = len(_x)
00728 if python3 or type(_x) == unicode:
00729 _x = _x.encode('utf-8')
00730 length = len(_x)
00731 buff.write(struct.pack('<I%ss'%length, length, _x))
00732 length = len(self.action_goal.goal.target.cluster.points)
00733 buff.write(_struct_I.pack(length))
00734 for val1 in self.action_goal.goal.target.cluster.points:
00735 _x = val1
00736 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00737 length = len(self.action_goal.goal.target.cluster.channels)
00738 buff.write(_struct_I.pack(length))
00739 for val1 in self.action_goal.goal.target.cluster.channels:
00740 _x = val1.name
00741 length = len(_x)
00742 if python3 or type(_x) == unicode:
00743 _x = _x.encode('utf-8')
00744 length = len(_x)
00745 buff.write(struct.pack('<I%ss'%length, length, _x))
00746 length = len(val1.values)
00747 buff.write(_struct_I.pack(length))
00748 pattern = '<%sf'%length
00749 buff.write(struct.pack(pattern, *val1.values))
00750 _x = self
00751 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs))
00752 _x = self.action_goal.goal.target.region.cloud.header.frame_id
00753 length = len(_x)
00754 if python3 or type(_x) == unicode:
00755 _x = _x.encode('utf-8')
00756 length = len(_x)
00757 buff.write(struct.pack('<I%ss'%length, length, _x))
00758 _x = self
00759 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width))
00760 length = len(self.action_goal.goal.target.region.cloud.fields)
00761 buff.write(_struct_I.pack(length))
00762 for val1 in self.action_goal.goal.target.region.cloud.fields:
00763 _x = val1.name
00764 length = len(_x)
00765 if python3 or type(_x) == unicode:
00766 _x = _x.encode('utf-8')
00767 length = len(_x)
00768 buff.write(struct.pack('<I%ss'%length, length, _x))
00769 _x = val1
00770 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00771 _x = self
00772 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step))
00773 _x = self.action_goal.goal.target.region.cloud.data
00774 length = len(_x)
00775 # - if encoded as a list instead, serialize as bytes instead of string
00776 if type(_x) in [list, tuple]:
00777 buff.write(struct.pack('<I%sB'%length, length, *_x))
00778 else:
00779 buff.write(struct.pack('<I%ss'%length, length, _x))
00780 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense))
00781 length = len(self.action_goal.goal.target.region.mask)
00782 buff.write(_struct_I.pack(length))
00783 pattern = '<%si'%length
00784 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.mask))
00785 _x = self
00786 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs))
00787 _x = self.action_goal.goal.target.region.image.header.frame_id
00788 length = len(_x)
00789 if python3 or type(_x) == unicode:
00790 _x = _x.encode('utf-8')
00791 length = len(_x)
00792 buff.write(struct.pack('<I%ss'%length, length, _x))
00793 _x = self
00794 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width))
00795 _x = self.action_goal.goal.target.region.image.encoding
00796 length = len(_x)
00797 if python3 or type(_x) == unicode:
00798 _x = _x.encode('utf-8')
00799 length = len(_x)
00800 buff.write(struct.pack('<I%ss'%length, length, _x))
00801 _x = self
00802 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step))
00803 _x = self.action_goal.goal.target.region.image.data
00804 length = len(_x)
00805 # - if encoded as a list instead, serialize as bytes instead of string
00806 if type(_x) in [list, tuple]:
00807 buff.write(struct.pack('<I%sB'%length, length, *_x))
00808 else:
00809 buff.write(struct.pack('<I%ss'%length, length, _x))
00810 _x = self
00811 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs))
00812 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id
00813 length = len(_x)
00814 if python3 or type(_x) == unicode:
00815 _x = _x.encode('utf-8')
00816 length = len(_x)
00817 buff.write(struct.pack('<I%ss'%length, length, _x))
00818 _x = self
00819 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width))
00820 _x = self.action_goal.goal.target.region.disparity_image.encoding
00821 length = len(_x)
00822 if python3 or type(_x) == unicode:
00823 _x = _x.encode('utf-8')
00824 length = len(_x)
00825 buff.write(struct.pack('<I%ss'%length, length, _x))
00826 _x = self
00827 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step))
00828 _x = self.action_goal.goal.target.region.disparity_image.data
00829 length = len(_x)
00830 # - if encoded as a list instead, serialize as bytes instead of string
00831 if type(_x) in [list, tuple]:
00832 buff.write(struct.pack('<I%sB'%length, length, *_x))
00833 else:
00834 buff.write(struct.pack('<I%ss'%length, length, _x))
00835 _x = self
00836 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs))
00837 _x = self.action_goal.goal.target.region.cam_info.header.frame_id
00838 length = len(_x)
00839 if python3 or type(_x) == unicode:
00840 _x = _x.encode('utf-8')
00841 length = len(_x)
00842 buff.write(struct.pack('<I%ss'%length, length, _x))
00843 _x = self
00844 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width))
00845 _x = self.action_goal.goal.target.region.cam_info.distortion_model
00846 length = len(_x)
00847 if python3 or type(_x) == unicode:
00848 _x = _x.encode('utf-8')
00849 length = len(_x)
00850 buff.write(struct.pack('<I%ss'%length, length, _x))
00851 length = len(self.action_goal.goal.target.region.cam_info.D)
00852 buff.write(_struct_I.pack(length))
00853 pattern = '<%sd'%length
00854 buff.write(struct.pack(pattern, *self.action_goal.goal.target.region.cam_info.D))
00855 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.K))
00856 buff.write(_struct_9d.pack(*self.action_goal.goal.target.region.cam_info.R))
00857 buff.write(_struct_12d.pack(*self.action_goal.goal.target.region.cam_info.P))
00858 _x = self
00859 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs))
00860 _x = self.action_goal.goal.target.region.roi_box_pose.header.frame_id
00861 length = len(_x)
00862 if python3 or type(_x) == unicode:
00863 _x = _x.encode('utf-8')
00864 length = len(_x)
00865 buff.write(struct.pack('<I%ss'%length, length, _x))
00866 _x = self
00867 buff.write(_struct_10d.pack(_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z))
00868 _x = self.action_goal.goal.target.collision_name
00869 length = len(_x)
00870 if python3 or type(_x) == unicode:
00871 _x = _x.encode('utf-8')
00872 length = len(_x)
00873 buff.write(struct.pack('<I%ss'%length, length, _x))
00874 _x = self.action_goal.goal.collision_object_name
00875 length = len(_x)
00876 if python3 or type(_x) == unicode:
00877 _x = _x.encode('utf-8')
00878 length = len(_x)
00879 buff.write(struct.pack('<I%ss'%length, length, _x))
00880 _x = self.action_goal.goal.collision_support_surface_name
00881 length = len(_x)
00882 if python3 or type(_x) == unicode:
00883 _x = _x.encode('utf-8')
00884 length = len(_x)
00885 buff.write(struct.pack('<I%ss'%length, length, _x))
00886 length = len(self.action_goal.goal.grasps_to_evaluate)
00887 buff.write(_struct_I.pack(length))
00888 for val1 in self.action_goal.goal.grasps_to_evaluate:
00889 _v7 = val1.pre_grasp_posture
00890 _v8 = _v7.header
00891 buff.write(_struct_I.pack(_v8.seq))
00892 _v9 = _v8.stamp
00893 _x = _v9
00894 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00895 _x = _v8.frame_id
00896 length = len(_x)
00897 if python3 or type(_x) == unicode:
00898 _x = _x.encode('utf-8')
00899 length = len(_x)
00900 buff.write(struct.pack('<I%ss'%length, length, _x))
00901 length = len(_v7.name)
00902 buff.write(_struct_I.pack(length))
00903 for val3 in _v7.name:
00904 length = len(val3)
00905 if python3 or type(val3) == unicode:
00906 val3 = val3.encode('utf-8')
00907 length = len(val3)
00908 buff.write(struct.pack('<I%ss'%length, length, val3))
00909 length = len(_v7.position)
00910 buff.write(_struct_I.pack(length))
00911 pattern = '<%sd'%length
00912 buff.write(struct.pack(pattern, *_v7.position))
00913 length = len(_v7.velocity)
00914 buff.write(_struct_I.pack(length))
00915 pattern = '<%sd'%length
00916 buff.write(struct.pack(pattern, *_v7.velocity))
00917 length = len(_v7.effort)
00918 buff.write(_struct_I.pack(length))
00919 pattern = '<%sd'%length
00920 buff.write(struct.pack(pattern, *_v7.effort))
00921 _v10 = val1.grasp_posture
00922 _v11 = _v10.header
00923 buff.write(_struct_I.pack(_v11.seq))
00924 _v12 = _v11.stamp
00925 _x = _v12
00926 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00927 _x = _v11.frame_id
00928 length = len(_x)
00929 if python3 or type(_x) == unicode:
00930 _x = _x.encode('utf-8')
00931 length = len(_x)
00932 buff.write(struct.pack('<I%ss'%length, length, _x))
00933 length = len(_v10.name)
00934 buff.write(_struct_I.pack(length))
00935 for val3 in _v10.name:
00936 length = len(val3)
00937 if python3 or type(val3) == unicode:
00938 val3 = val3.encode('utf-8')
00939 length = len(val3)
00940 buff.write(struct.pack('<I%ss'%length, length, val3))
00941 length = len(_v10.position)
00942 buff.write(_struct_I.pack(length))
00943 pattern = '<%sd'%length
00944 buff.write(struct.pack(pattern, *_v10.position))
00945 length = len(_v10.velocity)
00946 buff.write(_struct_I.pack(length))
00947 pattern = '<%sd'%length
00948 buff.write(struct.pack(pattern, *_v10.velocity))
00949 length = len(_v10.effort)
00950 buff.write(_struct_I.pack(length))
00951 pattern = '<%sd'%length
00952 buff.write(struct.pack(pattern, *_v10.effort))
00953 _v13 = val1.grasp_pose
00954 _v14 = _v13.position
00955 _x = _v14
00956 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00957 _v15 = _v13.orientation
00958 _x = _v15
00959 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00960 _x = val1
00961 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
00962 length = len(val1.moved_obstacles)
00963 buff.write(_struct_I.pack(length))
00964 for val2 in val1.moved_obstacles:
00965 _x = val2.reference_frame_id
00966 length = len(_x)
00967 if python3 or type(_x) == unicode:
00968 _x = _x.encode('utf-8')
00969 length = len(_x)
00970 buff.write(struct.pack('<I%ss'%length, length, _x))
00971 length = len(val2.potential_models)
00972 buff.write(_struct_I.pack(length))
00973 for val3 in val2.potential_models:
00974 buff.write(_struct_i.pack(val3.model_id))
00975 _v16 = val3.pose
00976 _v17 = _v16.header
00977 buff.write(_struct_I.pack(_v17.seq))
00978 _v18 = _v17.stamp
00979 _x = _v18
00980 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00981 _x = _v17.frame_id
00982 length = len(_x)
00983 if python3 or type(_x) == unicode:
00984 _x = _x.encode('utf-8')
00985 length = len(_x)
00986 buff.write(struct.pack('<I%ss'%length, length, _x))
00987 _v19 = _v16.pose
00988 _v20 = _v19.position
00989 _x = _v20
00990 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00991 _v21 = _v19.orientation
00992 _x = _v21
00993 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00994 buff.write(_struct_f.pack(val3.confidence))
00995 _x = val3.detector_name
00996 length = len(_x)
00997 if python3 or type(_x) == unicode:
00998 _x = _x.encode('utf-8')
00999 length = len(_x)
01000 buff.write(struct.pack('<I%ss'%length, length, _x))
01001 _v22 = val2.cluster
01002 _v23 = _v22.header
01003 buff.write(_struct_I.pack(_v23.seq))
01004 _v24 = _v23.stamp
01005 _x = _v24
01006 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01007 _x = _v23.frame_id
01008 length = len(_x)
01009 if python3 or type(_x) == unicode:
01010 _x = _x.encode('utf-8')
01011 length = len(_x)
01012 buff.write(struct.pack('<I%ss'%length, length, _x))
01013 length = len(_v22.points)
01014 buff.write(_struct_I.pack(length))
01015 for val4 in _v22.points:
01016 _x = val4
01017 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01018 length = len(_v22.channels)
01019 buff.write(_struct_I.pack(length))
01020 for val4 in _v22.channels:
01021 _x = val4.name
01022 length = len(_x)
01023 if python3 or type(_x) == unicode:
01024 _x = _x.encode('utf-8')
01025 length = len(_x)
01026 buff.write(struct.pack('<I%ss'%length, length, _x))
01027 length = len(val4.values)
01028 buff.write(_struct_I.pack(length))
01029 pattern = '<%sf'%length
01030 buff.write(struct.pack(pattern, *val4.values))
01031 _v25 = val2.region
01032 _v26 = _v25.cloud
01033 _v27 = _v26.header
01034 buff.write(_struct_I.pack(_v27.seq))
01035 _v28 = _v27.stamp
01036 _x = _v28
01037 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01038 _x = _v27.frame_id
01039 length = len(_x)
01040 if python3 or type(_x) == unicode:
01041 _x = _x.encode('utf-8')
01042 length = len(_x)
01043 buff.write(struct.pack('<I%ss'%length, length, _x))
01044 _x = _v26
01045 buff.write(_struct_2I.pack(_x.height, _x.width))
01046 length = len(_v26.fields)
01047 buff.write(_struct_I.pack(length))
01048 for val5 in _v26.fields:
01049 _x = val5.name
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 _x = val5
01056 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01057 _x = _v26
01058 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01059 _x = _v26.data
01060 length = len(_x)
01061 # - if encoded as a list instead, serialize as bytes instead of string
01062 if type(_x) in [list, tuple]:
01063 buff.write(struct.pack('<I%sB'%length, length, *_x))
01064 else:
01065 buff.write(struct.pack('<I%ss'%length, length, _x))
01066 buff.write(_struct_B.pack(_v26.is_dense))
01067 length = len(_v25.mask)
01068 buff.write(_struct_I.pack(length))
01069 pattern = '<%si'%length
01070 buff.write(struct.pack(pattern, *_v25.mask))
01071 _v29 = _v25.image
01072 _v30 = _v29.header
01073 buff.write(_struct_I.pack(_v30.seq))
01074 _v31 = _v30.stamp
01075 _x = _v31
01076 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01077 _x = _v30.frame_id
01078 length = len(_x)
01079 if python3 or type(_x) == unicode:
01080 _x = _x.encode('utf-8')
01081 length = len(_x)
01082 buff.write(struct.pack('<I%ss'%length, length, _x))
01083 _x = _v29
01084 buff.write(_struct_2I.pack(_x.height, _x.width))
01085 _x = _v29.encoding
01086 length = len(_x)
01087 if python3 or type(_x) == unicode:
01088 _x = _x.encode('utf-8')
01089 length = len(_x)
01090 buff.write(struct.pack('<I%ss'%length, length, _x))
01091 _x = _v29
01092 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01093 _x = _v29.data
01094 length = len(_x)
01095 # - if encoded as a list instead, serialize as bytes instead of string
01096 if type(_x) in [list, tuple]:
01097 buff.write(struct.pack('<I%sB'%length, length, *_x))
01098 else:
01099 buff.write(struct.pack('<I%ss'%length, length, _x))
01100 _v32 = _v25.disparity_image
01101 _v33 = _v32.header
01102 buff.write(_struct_I.pack(_v33.seq))
01103 _v34 = _v33.stamp
01104 _x = _v34
01105 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01106 _x = _v33.frame_id
01107 length = len(_x)
01108 if python3 or type(_x) == unicode:
01109 _x = _x.encode('utf-8')
01110 length = len(_x)
01111 buff.write(struct.pack('<I%ss'%length, length, _x))
01112 _x = _v32
01113 buff.write(_struct_2I.pack(_x.height, _x.width))
01114 _x = _v32.encoding
01115 length = len(_x)
01116 if python3 or type(_x) == unicode:
01117 _x = _x.encode('utf-8')
01118 length = len(_x)
01119 buff.write(struct.pack('<I%ss'%length, length, _x))
01120 _x = _v32
01121 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01122 _x = _v32.data
01123 length = len(_x)
01124 # - if encoded as a list instead, serialize as bytes instead of string
01125 if type(_x) in [list, tuple]:
01126 buff.write(struct.pack('<I%sB'%length, length, *_x))
01127 else:
01128 buff.write(struct.pack('<I%ss'%length, length, _x))
01129 _v35 = _v25.cam_info
01130 _v36 = _v35.header
01131 buff.write(_struct_I.pack(_v36.seq))
01132 _v37 = _v36.stamp
01133 _x = _v37
01134 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01135 _x = _v36.frame_id
01136 length = len(_x)
01137 if python3 or type(_x) == unicode:
01138 _x = _x.encode('utf-8')
01139 length = len(_x)
01140 buff.write(struct.pack('<I%ss'%length, length, _x))
01141 _x = _v35
01142 buff.write(_struct_2I.pack(_x.height, _x.width))
01143 _x = _v35.distortion_model
01144 length = len(_x)
01145 if python3 or type(_x) == unicode:
01146 _x = _x.encode('utf-8')
01147 length = len(_x)
01148 buff.write(struct.pack('<I%ss'%length, length, _x))
01149 length = len(_v35.D)
01150 buff.write(_struct_I.pack(length))
01151 pattern = '<%sd'%length
01152 buff.write(struct.pack(pattern, *_v35.D))
01153 buff.write(_struct_9d.pack(*_v35.K))
01154 buff.write(_struct_9d.pack(*_v35.R))
01155 buff.write(_struct_12d.pack(*_v35.P))
01156 _x = _v35
01157 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01158 _v38 = _v35.roi
01159 _x = _v38
01160 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01161 _v39 = _v25.roi_box_pose
01162 _v40 = _v39.header
01163 buff.write(_struct_I.pack(_v40.seq))
01164 _v41 = _v40.stamp
01165 _x = _v41
01166 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01167 _x = _v40.frame_id
01168 length = len(_x)
01169 if python3 or type(_x) == unicode:
01170 _x = _x.encode('utf-8')
01171 length = len(_x)
01172 buff.write(struct.pack('<I%ss'%length, length, _x))
01173 _v42 = _v39.pose
01174 _v43 = _v42.position
01175 _x = _v43
01176 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01177 _v44 = _v42.orientation
01178 _x = _v44
01179 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01180 _v45 = _v25.roi_box_dims
01181 _x = _v45
01182 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01183 _x = val2.collision_name
01184 length = len(_x)
01185 if python3 or type(_x) == unicode:
01186 _x = _x.encode('utf-8')
01187 length = len(_x)
01188 buff.write(struct.pack('<I%ss'%length, length, _x))
01189 length = len(self.action_goal.goal.movable_obstacles)
01190 buff.write(_struct_I.pack(length))
01191 for val1 in self.action_goal.goal.movable_obstacles:
01192 _x = val1.reference_frame_id
01193 length = len(_x)
01194 if python3 or type(_x) == unicode:
01195 _x = _x.encode('utf-8')
01196 length = len(_x)
01197 buff.write(struct.pack('<I%ss'%length, length, _x))
01198 length = len(val1.potential_models)
01199 buff.write(_struct_I.pack(length))
01200 for val2 in val1.potential_models:
01201 buff.write(_struct_i.pack(val2.model_id))
01202 _v46 = val2.pose
01203 _v47 = _v46.header
01204 buff.write(_struct_I.pack(_v47.seq))
01205 _v48 = _v47.stamp
01206 _x = _v48
01207 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01208 _x = _v47.frame_id
01209 length = len(_x)
01210 if python3 or type(_x) == unicode:
01211 _x = _x.encode('utf-8')
01212 length = len(_x)
01213 buff.write(struct.pack('<I%ss'%length, length, _x))
01214 _v49 = _v46.pose
01215 _v50 = _v49.position
01216 _x = _v50
01217 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01218 _v51 = _v49.orientation
01219 _x = _v51
01220 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01221 buff.write(_struct_f.pack(val2.confidence))
01222 _x = val2.detector_name
01223 length = len(_x)
01224 if python3 or type(_x) == unicode:
01225 _x = _x.encode('utf-8')
01226 length = len(_x)
01227 buff.write(struct.pack('<I%ss'%length, length, _x))
01228 _v52 = val1.cluster
01229 _v53 = _v52.header
01230 buff.write(_struct_I.pack(_v53.seq))
01231 _v54 = _v53.stamp
01232 _x = _v54
01233 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01234 _x = _v53.frame_id
01235 length = len(_x)
01236 if python3 or type(_x) == unicode:
01237 _x = _x.encode('utf-8')
01238 length = len(_x)
01239 buff.write(struct.pack('<I%ss'%length, length, _x))
01240 length = len(_v52.points)
01241 buff.write(_struct_I.pack(length))
01242 for val3 in _v52.points:
01243 _x = val3
01244 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01245 length = len(_v52.channels)
01246 buff.write(_struct_I.pack(length))
01247 for val3 in _v52.channels:
01248 _x = val3.name
01249 length = len(_x)
01250 if python3 or type(_x) == unicode:
01251 _x = _x.encode('utf-8')
01252 length = len(_x)
01253 buff.write(struct.pack('<I%ss'%length, length, _x))
01254 length = len(val3.values)
01255 buff.write(_struct_I.pack(length))
01256 pattern = '<%sf'%length
01257 buff.write(struct.pack(pattern, *val3.values))
01258 _v55 = val1.region
01259 _v56 = _v55.cloud
01260 _v57 = _v56.header
01261 buff.write(_struct_I.pack(_v57.seq))
01262 _v58 = _v57.stamp
01263 _x = _v58
01264 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01265 _x = _v57.frame_id
01266 length = len(_x)
01267 if python3 or type(_x) == unicode:
01268 _x = _x.encode('utf-8')
01269 length = len(_x)
01270 buff.write(struct.pack('<I%ss'%length, length, _x))
01271 _x = _v56
01272 buff.write(_struct_2I.pack(_x.height, _x.width))
01273 length = len(_v56.fields)
01274 buff.write(_struct_I.pack(length))
01275 for val4 in _v56.fields:
01276 _x = val4.name
01277 length = len(_x)
01278 if python3 or type(_x) == unicode:
01279 _x = _x.encode('utf-8')
01280 length = len(_x)
01281 buff.write(struct.pack('<I%ss'%length, length, _x))
01282 _x = val4
01283 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01284 _x = _v56
01285 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01286 _x = _v56.data
01287 length = len(_x)
01288 # - if encoded as a list instead, serialize as bytes instead of string
01289 if type(_x) in [list, tuple]:
01290 buff.write(struct.pack('<I%sB'%length, length, *_x))
01291 else:
01292 buff.write(struct.pack('<I%ss'%length, length, _x))
01293 buff.write(_struct_B.pack(_v56.is_dense))
01294 length = len(_v55.mask)
01295 buff.write(_struct_I.pack(length))
01296 pattern = '<%si'%length
01297 buff.write(struct.pack(pattern, *_v55.mask))
01298 _v59 = _v55.image
01299 _v60 = _v59.header
01300 buff.write(_struct_I.pack(_v60.seq))
01301 _v61 = _v60.stamp
01302 _x = _v61
01303 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01304 _x = _v60.frame_id
01305 length = len(_x)
01306 if python3 or type(_x) == unicode:
01307 _x = _x.encode('utf-8')
01308 length = len(_x)
01309 buff.write(struct.pack('<I%ss'%length, length, _x))
01310 _x = _v59
01311 buff.write(_struct_2I.pack(_x.height, _x.width))
01312 _x = _v59.encoding
01313 length = len(_x)
01314 if python3 or type(_x) == unicode:
01315 _x = _x.encode('utf-8')
01316 length = len(_x)
01317 buff.write(struct.pack('<I%ss'%length, length, _x))
01318 _x = _v59
01319 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01320 _x = _v59.data
01321 length = len(_x)
01322 # - if encoded as a list instead, serialize as bytes instead of string
01323 if type(_x) in [list, tuple]:
01324 buff.write(struct.pack('<I%sB'%length, length, *_x))
01325 else:
01326 buff.write(struct.pack('<I%ss'%length, length, _x))
01327 _v62 = _v55.disparity_image
01328 _v63 = _v62.header
01329 buff.write(_struct_I.pack(_v63.seq))
01330 _v64 = _v63.stamp
01331 _x = _v64
01332 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01333 _x = _v63.frame_id
01334 length = len(_x)
01335 if python3 or type(_x) == unicode:
01336 _x = _x.encode('utf-8')
01337 length = len(_x)
01338 buff.write(struct.pack('<I%ss'%length, length, _x))
01339 _x = _v62
01340 buff.write(_struct_2I.pack(_x.height, _x.width))
01341 _x = _v62.encoding
01342 length = len(_x)
01343 if python3 or type(_x) == unicode:
01344 _x = _x.encode('utf-8')
01345 length = len(_x)
01346 buff.write(struct.pack('<I%ss'%length, length, _x))
01347 _x = _v62
01348 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01349 _x = _v62.data
01350 length = len(_x)
01351 # - if encoded as a list instead, serialize as bytes instead of string
01352 if type(_x) in [list, tuple]:
01353 buff.write(struct.pack('<I%sB'%length, length, *_x))
01354 else:
01355 buff.write(struct.pack('<I%ss'%length, length, _x))
01356 _v65 = _v55.cam_info
01357 _v66 = _v65.header
01358 buff.write(_struct_I.pack(_v66.seq))
01359 _v67 = _v66.stamp
01360 _x = _v67
01361 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01362 _x = _v66.frame_id
01363 length = len(_x)
01364 if python3 or type(_x) == unicode:
01365 _x = _x.encode('utf-8')
01366 length = len(_x)
01367 buff.write(struct.pack('<I%ss'%length, length, _x))
01368 _x = _v65
01369 buff.write(_struct_2I.pack(_x.height, _x.width))
01370 _x = _v65.distortion_model
01371 length = len(_x)
01372 if python3 or type(_x) == unicode:
01373 _x = _x.encode('utf-8')
01374 length = len(_x)
01375 buff.write(struct.pack('<I%ss'%length, length, _x))
01376 length = len(_v65.D)
01377 buff.write(_struct_I.pack(length))
01378 pattern = '<%sd'%length
01379 buff.write(struct.pack(pattern, *_v65.D))
01380 buff.write(_struct_9d.pack(*_v65.K))
01381 buff.write(_struct_9d.pack(*_v65.R))
01382 buff.write(_struct_12d.pack(*_v65.P))
01383 _x = _v65
01384 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01385 _v68 = _v65.roi
01386 _x = _v68
01387 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01388 _v69 = _v55.roi_box_pose
01389 _v70 = _v69.header
01390 buff.write(_struct_I.pack(_v70.seq))
01391 _v71 = _v70.stamp
01392 _x = _v71
01393 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01394 _x = _v70.frame_id
01395 length = len(_x)
01396 if python3 or type(_x) == unicode:
01397 _x = _x.encode('utf-8')
01398 length = len(_x)
01399 buff.write(struct.pack('<I%ss'%length, length, _x))
01400 _v72 = _v69.pose
01401 _v73 = _v72.position
01402 _x = _v73
01403 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01404 _v74 = _v72.orientation
01405 _x = _v74
01406 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01407 _v75 = _v55.roi_box_dims
01408 _x = _v75
01409 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01410 _x = val1.collision_name
01411 length = len(_x)
01412 if python3 or type(_x) == unicode:
01413 _x = _x.encode('utf-8')
01414 length = len(_x)
01415 buff.write(struct.pack('<I%ss'%length, length, _x))
01416 _x = self
01417 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
01418 _x = self.action_result.header.frame_id
01419 length = len(_x)
01420 if python3 or type(_x) == unicode:
01421 _x = _x.encode('utf-8')
01422 length = len(_x)
01423 buff.write(struct.pack('<I%ss'%length, length, _x))
01424 _x = self
01425 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
01426 _x = self.action_result.status.goal_id.id
01427 length = len(_x)
01428 if python3 or type(_x) == unicode:
01429 _x = _x.encode('utf-8')
01430 length = len(_x)
01431 buff.write(struct.pack('<I%ss'%length, length, _x))
01432 buff.write(_struct_B.pack(self.action_result.status.status))
01433 _x = self.action_result.status.text
01434 length = len(_x)
01435 if python3 or type(_x) == unicode:
01436 _x = _x.encode('utf-8')
01437 length = len(_x)
01438 buff.write(struct.pack('<I%ss'%length, length, _x))
01439 length = len(self.action_result.result.grasps)
01440 buff.write(_struct_I.pack(length))
01441 for val1 in self.action_result.result.grasps:
01442 _v76 = val1.pre_grasp_posture
01443 _v77 = _v76.header
01444 buff.write(_struct_I.pack(_v77.seq))
01445 _v78 = _v77.stamp
01446 _x = _v78
01447 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01448 _x = _v77.frame_id
01449 length = len(_x)
01450 if python3 or type(_x) == unicode:
01451 _x = _x.encode('utf-8')
01452 length = len(_x)
01453 buff.write(struct.pack('<I%ss'%length, length, _x))
01454 length = len(_v76.name)
01455 buff.write(_struct_I.pack(length))
01456 for val3 in _v76.name:
01457 length = len(val3)
01458 if python3 or type(val3) == unicode:
01459 val3 = val3.encode('utf-8')
01460 length = len(val3)
01461 buff.write(struct.pack('<I%ss'%length, length, val3))
01462 length = len(_v76.position)
01463 buff.write(_struct_I.pack(length))
01464 pattern = '<%sd'%length
01465 buff.write(struct.pack(pattern, *_v76.position))
01466 length = len(_v76.velocity)
01467 buff.write(_struct_I.pack(length))
01468 pattern = '<%sd'%length
01469 buff.write(struct.pack(pattern, *_v76.velocity))
01470 length = len(_v76.effort)
01471 buff.write(_struct_I.pack(length))
01472 pattern = '<%sd'%length
01473 buff.write(struct.pack(pattern, *_v76.effort))
01474 _v79 = val1.grasp_posture
01475 _v80 = _v79.header
01476 buff.write(_struct_I.pack(_v80.seq))
01477 _v81 = _v80.stamp
01478 _x = _v81
01479 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01480 _x = _v80.frame_id
01481 length = len(_x)
01482 if python3 or type(_x) == unicode:
01483 _x = _x.encode('utf-8')
01484 length = len(_x)
01485 buff.write(struct.pack('<I%ss'%length, length, _x))
01486 length = len(_v79.name)
01487 buff.write(_struct_I.pack(length))
01488 for val3 in _v79.name:
01489 length = len(val3)
01490 if python3 or type(val3) == unicode:
01491 val3 = val3.encode('utf-8')
01492 length = len(val3)
01493 buff.write(struct.pack('<I%ss'%length, length, val3))
01494 length = len(_v79.position)
01495 buff.write(_struct_I.pack(length))
01496 pattern = '<%sd'%length
01497 buff.write(struct.pack(pattern, *_v79.position))
01498 length = len(_v79.velocity)
01499 buff.write(_struct_I.pack(length))
01500 pattern = '<%sd'%length
01501 buff.write(struct.pack(pattern, *_v79.velocity))
01502 length = len(_v79.effort)
01503 buff.write(_struct_I.pack(length))
01504 pattern = '<%sd'%length
01505 buff.write(struct.pack(pattern, *_v79.effort))
01506 _v82 = val1.grasp_pose
01507 _v83 = _v82.position
01508 _x = _v83
01509 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01510 _v84 = _v82.orientation
01511 _x = _v84
01512 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01513 _x = val1
01514 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
01515 length = len(val1.moved_obstacles)
01516 buff.write(_struct_I.pack(length))
01517 for val2 in val1.moved_obstacles:
01518 _x = val2.reference_frame_id
01519 length = len(_x)
01520 if python3 or type(_x) == unicode:
01521 _x = _x.encode('utf-8')
01522 length = len(_x)
01523 buff.write(struct.pack('<I%ss'%length, length, _x))
01524 length = len(val2.potential_models)
01525 buff.write(_struct_I.pack(length))
01526 for val3 in val2.potential_models:
01527 buff.write(_struct_i.pack(val3.model_id))
01528 _v85 = val3.pose
01529 _v86 = _v85.header
01530 buff.write(_struct_I.pack(_v86.seq))
01531 _v87 = _v86.stamp
01532 _x = _v87
01533 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01534 _x = _v86.frame_id
01535 length = len(_x)
01536 if python3 or type(_x) == unicode:
01537 _x = _x.encode('utf-8')
01538 length = len(_x)
01539 buff.write(struct.pack('<I%ss'%length, length, _x))
01540 _v88 = _v85.pose
01541 _v89 = _v88.position
01542 _x = _v89
01543 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01544 _v90 = _v88.orientation
01545 _x = _v90
01546 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01547 buff.write(_struct_f.pack(val3.confidence))
01548 _x = val3.detector_name
01549 length = len(_x)
01550 if python3 or type(_x) == unicode:
01551 _x = _x.encode('utf-8')
01552 length = len(_x)
01553 buff.write(struct.pack('<I%ss'%length, length, _x))
01554 _v91 = val2.cluster
01555 _v92 = _v91.header
01556 buff.write(_struct_I.pack(_v92.seq))
01557 _v93 = _v92.stamp
01558 _x = _v93
01559 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01560 _x = _v92.frame_id
01561 length = len(_x)
01562 if python3 or type(_x) == unicode:
01563 _x = _x.encode('utf-8')
01564 length = len(_x)
01565 buff.write(struct.pack('<I%ss'%length, length, _x))
01566 length = len(_v91.points)
01567 buff.write(_struct_I.pack(length))
01568 for val4 in _v91.points:
01569 _x = val4
01570 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01571 length = len(_v91.channels)
01572 buff.write(_struct_I.pack(length))
01573 for val4 in _v91.channels:
01574 _x = val4.name
01575 length = len(_x)
01576 if python3 or type(_x) == unicode:
01577 _x = _x.encode('utf-8')
01578 length = len(_x)
01579 buff.write(struct.pack('<I%ss'%length, length, _x))
01580 length = len(val4.values)
01581 buff.write(_struct_I.pack(length))
01582 pattern = '<%sf'%length
01583 buff.write(struct.pack(pattern, *val4.values))
01584 _v94 = val2.region
01585 _v95 = _v94.cloud
01586 _v96 = _v95.header
01587 buff.write(_struct_I.pack(_v96.seq))
01588 _v97 = _v96.stamp
01589 _x = _v97
01590 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01591 _x = _v96.frame_id
01592 length = len(_x)
01593 if python3 or type(_x) == unicode:
01594 _x = _x.encode('utf-8')
01595 length = len(_x)
01596 buff.write(struct.pack('<I%ss'%length, length, _x))
01597 _x = _v95
01598 buff.write(_struct_2I.pack(_x.height, _x.width))
01599 length = len(_v95.fields)
01600 buff.write(_struct_I.pack(length))
01601 for val5 in _v95.fields:
01602 _x = val5.name
01603 length = len(_x)
01604 if python3 or type(_x) == unicode:
01605 _x = _x.encode('utf-8')
01606 length = len(_x)
01607 buff.write(struct.pack('<I%ss'%length, length, _x))
01608 _x = val5
01609 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01610 _x = _v95
01611 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01612 _x = _v95.data
01613 length = len(_x)
01614 # - if encoded as a list instead, serialize as bytes instead of string
01615 if type(_x) in [list, tuple]:
01616 buff.write(struct.pack('<I%sB'%length, length, *_x))
01617 else:
01618 buff.write(struct.pack('<I%ss'%length, length, _x))
01619 buff.write(_struct_B.pack(_v95.is_dense))
01620 length = len(_v94.mask)
01621 buff.write(_struct_I.pack(length))
01622 pattern = '<%si'%length
01623 buff.write(struct.pack(pattern, *_v94.mask))
01624 _v98 = _v94.image
01625 _v99 = _v98.header
01626 buff.write(_struct_I.pack(_v99.seq))
01627 _v100 = _v99.stamp
01628 _x = _v100
01629 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01630 _x = _v99.frame_id
01631 length = len(_x)
01632 if python3 or type(_x) == unicode:
01633 _x = _x.encode('utf-8')
01634 length = len(_x)
01635 buff.write(struct.pack('<I%ss'%length, length, _x))
01636 _x = _v98
01637 buff.write(_struct_2I.pack(_x.height, _x.width))
01638 _x = _v98.encoding
01639 length = len(_x)
01640 if python3 or type(_x) == unicode:
01641 _x = _x.encode('utf-8')
01642 length = len(_x)
01643 buff.write(struct.pack('<I%ss'%length, length, _x))
01644 _x = _v98
01645 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01646 _x = _v98.data
01647 length = len(_x)
01648 # - if encoded as a list instead, serialize as bytes instead of string
01649 if type(_x) in [list, tuple]:
01650 buff.write(struct.pack('<I%sB'%length, length, *_x))
01651 else:
01652 buff.write(struct.pack('<I%ss'%length, length, _x))
01653 _v101 = _v94.disparity_image
01654 _v102 = _v101.header
01655 buff.write(_struct_I.pack(_v102.seq))
01656 _v103 = _v102.stamp
01657 _x = _v103
01658 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01659 _x = _v102.frame_id
01660 length = len(_x)
01661 if python3 or type(_x) == unicode:
01662 _x = _x.encode('utf-8')
01663 length = len(_x)
01664 buff.write(struct.pack('<I%ss'%length, length, _x))
01665 _x = _v101
01666 buff.write(_struct_2I.pack(_x.height, _x.width))
01667 _x = _v101.encoding
01668 length = len(_x)
01669 if python3 or type(_x) == unicode:
01670 _x = _x.encode('utf-8')
01671 length = len(_x)
01672 buff.write(struct.pack('<I%ss'%length, length, _x))
01673 _x = _v101
01674 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01675 _x = _v101.data
01676 length = len(_x)
01677 # - if encoded as a list instead, serialize as bytes instead of string
01678 if type(_x) in [list, tuple]:
01679 buff.write(struct.pack('<I%sB'%length, length, *_x))
01680 else:
01681 buff.write(struct.pack('<I%ss'%length, length, _x))
01682 _v104 = _v94.cam_info
01683 _v105 = _v104.header
01684 buff.write(_struct_I.pack(_v105.seq))
01685 _v106 = _v105.stamp
01686 _x = _v106
01687 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01688 _x = _v105.frame_id
01689 length = len(_x)
01690 if python3 or type(_x) == unicode:
01691 _x = _x.encode('utf-8')
01692 length = len(_x)
01693 buff.write(struct.pack('<I%ss'%length, length, _x))
01694 _x = _v104
01695 buff.write(_struct_2I.pack(_x.height, _x.width))
01696 _x = _v104.distortion_model
01697 length = len(_x)
01698 if python3 or type(_x) == unicode:
01699 _x = _x.encode('utf-8')
01700 length = len(_x)
01701 buff.write(struct.pack('<I%ss'%length, length, _x))
01702 length = len(_v104.D)
01703 buff.write(_struct_I.pack(length))
01704 pattern = '<%sd'%length
01705 buff.write(struct.pack(pattern, *_v104.D))
01706 buff.write(_struct_9d.pack(*_v104.K))
01707 buff.write(_struct_9d.pack(*_v104.R))
01708 buff.write(_struct_12d.pack(*_v104.P))
01709 _x = _v104
01710 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01711 _v107 = _v104.roi
01712 _x = _v107
01713 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01714 _v108 = _v94.roi_box_pose
01715 _v109 = _v108.header
01716 buff.write(_struct_I.pack(_v109.seq))
01717 _v110 = _v109.stamp
01718 _x = _v110
01719 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01720 _x = _v109.frame_id
01721 length = len(_x)
01722 if python3 or type(_x) == unicode:
01723 _x = _x.encode('utf-8')
01724 length = len(_x)
01725 buff.write(struct.pack('<I%ss'%length, length, _x))
01726 _v111 = _v108.pose
01727 _v112 = _v111.position
01728 _x = _v112
01729 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01730 _v113 = _v111.orientation
01731 _x = _v113
01732 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01733 _v114 = _v94.roi_box_dims
01734 _x = _v114
01735 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01736 _x = val2.collision_name
01737 length = len(_x)
01738 if python3 or type(_x) == unicode:
01739 _x = _x.encode('utf-8')
01740 length = len(_x)
01741 buff.write(struct.pack('<I%ss'%length, length, _x))
01742 _x = self
01743 buff.write(_struct_i3I.pack(_x.action_result.result.error_code.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
01744 _x = self.action_feedback.header.frame_id
01745 length = len(_x)
01746 if python3 or type(_x) == unicode:
01747 _x = _x.encode('utf-8')
01748 length = len(_x)
01749 buff.write(struct.pack('<I%ss'%length, length, _x))
01750 _x = self
01751 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
01752 _x = self.action_feedback.status.goal_id.id
01753 length = len(_x)
01754 if python3 or type(_x) == unicode:
01755 _x = _x.encode('utf-8')
01756 length = len(_x)
01757 buff.write(struct.pack('<I%ss'%length, length, _x))
01758 buff.write(_struct_B.pack(self.action_feedback.status.status))
01759 _x = self.action_feedback.status.text
01760 length = len(_x)
01761 if python3 or type(_x) == unicode:
01762 _x = _x.encode('utf-8')
01763 length = len(_x)
01764 buff.write(struct.pack('<I%ss'%length, length, _x))
01765 length = len(self.action_feedback.feedback.grasps)
01766 buff.write(_struct_I.pack(length))
01767 for val1 in self.action_feedback.feedback.grasps:
01768 _v115 = val1.pre_grasp_posture
01769 _v116 = _v115.header
01770 buff.write(_struct_I.pack(_v116.seq))
01771 _v117 = _v116.stamp
01772 _x = _v117
01773 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01774 _x = _v116.frame_id
01775 length = len(_x)
01776 if python3 or type(_x) == unicode:
01777 _x = _x.encode('utf-8')
01778 length = len(_x)
01779 buff.write(struct.pack('<I%ss'%length, length, _x))
01780 length = len(_v115.name)
01781 buff.write(_struct_I.pack(length))
01782 for val3 in _v115.name:
01783 length = len(val3)
01784 if python3 or type(val3) == unicode:
01785 val3 = val3.encode('utf-8')
01786 length = len(val3)
01787 buff.write(struct.pack('<I%ss'%length, length, val3))
01788 length = len(_v115.position)
01789 buff.write(_struct_I.pack(length))
01790 pattern = '<%sd'%length
01791 buff.write(struct.pack(pattern, *_v115.position))
01792 length = len(_v115.velocity)
01793 buff.write(_struct_I.pack(length))
01794 pattern = '<%sd'%length
01795 buff.write(struct.pack(pattern, *_v115.velocity))
01796 length = len(_v115.effort)
01797 buff.write(_struct_I.pack(length))
01798 pattern = '<%sd'%length
01799 buff.write(struct.pack(pattern, *_v115.effort))
01800 _v118 = val1.grasp_posture
01801 _v119 = _v118.header
01802 buff.write(_struct_I.pack(_v119.seq))
01803 _v120 = _v119.stamp
01804 _x = _v120
01805 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01806 _x = _v119.frame_id
01807 length = len(_x)
01808 if python3 or type(_x) == unicode:
01809 _x = _x.encode('utf-8')
01810 length = len(_x)
01811 buff.write(struct.pack('<I%ss'%length, length, _x))
01812 length = len(_v118.name)
01813 buff.write(_struct_I.pack(length))
01814 for val3 in _v118.name:
01815 length = len(val3)
01816 if python3 or type(val3) == unicode:
01817 val3 = val3.encode('utf-8')
01818 length = len(val3)
01819 buff.write(struct.pack('<I%ss'%length, length, val3))
01820 length = len(_v118.position)
01821 buff.write(_struct_I.pack(length))
01822 pattern = '<%sd'%length
01823 buff.write(struct.pack(pattern, *_v118.position))
01824 length = len(_v118.velocity)
01825 buff.write(_struct_I.pack(length))
01826 pattern = '<%sd'%length
01827 buff.write(struct.pack(pattern, *_v118.velocity))
01828 length = len(_v118.effort)
01829 buff.write(_struct_I.pack(length))
01830 pattern = '<%sd'%length
01831 buff.write(struct.pack(pattern, *_v118.effort))
01832 _v121 = val1.grasp_pose
01833 _v122 = _v121.position
01834 _x = _v122
01835 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01836 _v123 = _v121.orientation
01837 _x = _v123
01838 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01839 _x = val1
01840 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
01841 length = len(val1.moved_obstacles)
01842 buff.write(_struct_I.pack(length))
01843 for val2 in val1.moved_obstacles:
01844 _x = val2.reference_frame_id
01845 length = len(_x)
01846 if python3 or type(_x) == unicode:
01847 _x = _x.encode('utf-8')
01848 length = len(_x)
01849 buff.write(struct.pack('<I%ss'%length, length, _x))
01850 length = len(val2.potential_models)
01851 buff.write(_struct_I.pack(length))
01852 for val3 in val2.potential_models:
01853 buff.write(_struct_i.pack(val3.model_id))
01854 _v124 = val3.pose
01855 _v125 = _v124.header
01856 buff.write(_struct_I.pack(_v125.seq))
01857 _v126 = _v125.stamp
01858 _x = _v126
01859 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01860 _x = _v125.frame_id
01861 length = len(_x)
01862 if python3 or type(_x) == unicode:
01863 _x = _x.encode('utf-8')
01864 length = len(_x)
01865 buff.write(struct.pack('<I%ss'%length, length, _x))
01866 _v127 = _v124.pose
01867 _v128 = _v127.position
01868 _x = _v128
01869 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01870 _v129 = _v127.orientation
01871 _x = _v129
01872 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01873 buff.write(_struct_f.pack(val3.confidence))
01874 _x = val3.detector_name
01875 length = len(_x)
01876 if python3 or type(_x) == unicode:
01877 _x = _x.encode('utf-8')
01878 length = len(_x)
01879 buff.write(struct.pack('<I%ss'%length, length, _x))
01880 _v130 = val2.cluster
01881 _v131 = _v130.header
01882 buff.write(_struct_I.pack(_v131.seq))
01883 _v132 = _v131.stamp
01884 _x = _v132
01885 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01886 _x = _v131.frame_id
01887 length = len(_x)
01888 if python3 or type(_x) == unicode:
01889 _x = _x.encode('utf-8')
01890 length = len(_x)
01891 buff.write(struct.pack('<I%ss'%length, length, _x))
01892 length = len(_v130.points)
01893 buff.write(_struct_I.pack(length))
01894 for val4 in _v130.points:
01895 _x = val4
01896 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01897 length = len(_v130.channels)
01898 buff.write(_struct_I.pack(length))
01899 for val4 in _v130.channels:
01900 _x = val4.name
01901 length = len(_x)
01902 if python3 or type(_x) == unicode:
01903 _x = _x.encode('utf-8')
01904 length = len(_x)
01905 buff.write(struct.pack('<I%ss'%length, length, _x))
01906 length = len(val4.values)
01907 buff.write(_struct_I.pack(length))
01908 pattern = '<%sf'%length
01909 buff.write(struct.pack(pattern, *val4.values))
01910 _v133 = val2.region
01911 _v134 = _v133.cloud
01912 _v135 = _v134.header
01913 buff.write(_struct_I.pack(_v135.seq))
01914 _v136 = _v135.stamp
01915 _x = _v136
01916 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01917 _x = _v135.frame_id
01918 length = len(_x)
01919 if python3 or type(_x) == unicode:
01920 _x = _x.encode('utf-8')
01921 length = len(_x)
01922 buff.write(struct.pack('<I%ss'%length, length, _x))
01923 _x = _v134
01924 buff.write(_struct_2I.pack(_x.height, _x.width))
01925 length = len(_v134.fields)
01926 buff.write(_struct_I.pack(length))
01927 for val5 in _v134.fields:
01928 _x = val5.name
01929 length = len(_x)
01930 if python3 or type(_x) == unicode:
01931 _x = _x.encode('utf-8')
01932 length = len(_x)
01933 buff.write(struct.pack('<I%ss'%length, length, _x))
01934 _x = val5
01935 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01936 _x = _v134
01937 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01938 _x = _v134.data
01939 length = len(_x)
01940 # - if encoded as a list instead, serialize as bytes instead of string
01941 if type(_x) in [list, tuple]:
01942 buff.write(struct.pack('<I%sB'%length, length, *_x))
01943 else:
01944 buff.write(struct.pack('<I%ss'%length, length, _x))
01945 buff.write(_struct_B.pack(_v134.is_dense))
01946 length = len(_v133.mask)
01947 buff.write(_struct_I.pack(length))
01948 pattern = '<%si'%length
01949 buff.write(struct.pack(pattern, *_v133.mask))
01950 _v137 = _v133.image
01951 _v138 = _v137.header
01952 buff.write(_struct_I.pack(_v138.seq))
01953 _v139 = _v138.stamp
01954 _x = _v139
01955 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01956 _x = _v138.frame_id
01957 length = len(_x)
01958 if python3 or type(_x) == unicode:
01959 _x = _x.encode('utf-8')
01960 length = len(_x)
01961 buff.write(struct.pack('<I%ss'%length, length, _x))
01962 _x = _v137
01963 buff.write(_struct_2I.pack(_x.height, _x.width))
01964 _x = _v137.encoding
01965 length = len(_x)
01966 if python3 or type(_x) == unicode:
01967 _x = _x.encode('utf-8')
01968 length = len(_x)
01969 buff.write(struct.pack('<I%ss'%length, length, _x))
01970 _x = _v137
01971 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01972 _x = _v137.data
01973 length = len(_x)
01974 # - if encoded as a list instead, serialize as bytes instead of string
01975 if type(_x) in [list, tuple]:
01976 buff.write(struct.pack('<I%sB'%length, length, *_x))
01977 else:
01978 buff.write(struct.pack('<I%ss'%length, length, _x))
01979 _v140 = _v133.disparity_image
01980 _v141 = _v140.header
01981 buff.write(_struct_I.pack(_v141.seq))
01982 _v142 = _v141.stamp
01983 _x = _v142
01984 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01985 _x = _v141.frame_id
01986 length = len(_x)
01987 if python3 or type(_x) == unicode:
01988 _x = _x.encode('utf-8')
01989 length = len(_x)
01990 buff.write(struct.pack('<I%ss'%length, length, _x))
01991 _x = _v140
01992 buff.write(_struct_2I.pack(_x.height, _x.width))
01993 _x = _v140.encoding
01994 length = len(_x)
01995 if python3 or type(_x) == unicode:
01996 _x = _x.encode('utf-8')
01997 length = len(_x)
01998 buff.write(struct.pack('<I%ss'%length, length, _x))
01999 _x = _v140
02000 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
02001 _x = _v140.data
02002 length = len(_x)
02003 # - if encoded as a list instead, serialize as bytes instead of string
02004 if type(_x) in [list, tuple]:
02005 buff.write(struct.pack('<I%sB'%length, length, *_x))
02006 else:
02007 buff.write(struct.pack('<I%ss'%length, length, _x))
02008 _v143 = _v133.cam_info
02009 _v144 = _v143.header
02010 buff.write(_struct_I.pack(_v144.seq))
02011 _v145 = _v144.stamp
02012 _x = _v145
02013 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02014 _x = _v144.frame_id
02015 length = len(_x)
02016 if python3 or type(_x) == unicode:
02017 _x = _x.encode('utf-8')
02018 length = len(_x)
02019 buff.write(struct.pack('<I%ss'%length, length, _x))
02020 _x = _v143
02021 buff.write(_struct_2I.pack(_x.height, _x.width))
02022 _x = _v143.distortion_model
02023 length = len(_x)
02024 if python3 or type(_x) == unicode:
02025 _x = _x.encode('utf-8')
02026 length = len(_x)
02027 buff.write(struct.pack('<I%ss'%length, length, _x))
02028 length = len(_v143.D)
02029 buff.write(_struct_I.pack(length))
02030 pattern = '<%sd'%length
02031 buff.write(struct.pack(pattern, *_v143.D))
02032 buff.write(_struct_9d.pack(*_v143.K))
02033 buff.write(_struct_9d.pack(*_v143.R))
02034 buff.write(_struct_12d.pack(*_v143.P))
02035 _x = _v143
02036 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
02037 _v146 = _v143.roi
02038 _x = _v146
02039 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
02040 _v147 = _v133.roi_box_pose
02041 _v148 = _v147.header
02042 buff.write(_struct_I.pack(_v148.seq))
02043 _v149 = _v148.stamp
02044 _x = _v149
02045 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
02046 _x = _v148.frame_id
02047 length = len(_x)
02048 if python3 or type(_x) == unicode:
02049 _x = _x.encode('utf-8')
02050 length = len(_x)
02051 buff.write(struct.pack('<I%ss'%length, length, _x))
02052 _v150 = _v147.pose
02053 _v151 = _v150.position
02054 _x = _v151
02055 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02056 _v152 = _v150.orientation
02057 _x = _v152
02058 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
02059 _v153 = _v133.roi_box_dims
02060 _x = _v153
02061 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
02062 _x = val2.collision_name
02063 length = len(_x)
02064 if python3 or type(_x) == unicode:
02065 _x = _x.encode('utf-8')
02066 length = len(_x)
02067 buff.write(struct.pack('<I%ss'%length, length, _x))
02068 except struct.error as se: self._check_types(se)
02069 except TypeError as te: self._check_types(te)
02070
02071 def deserialize(self, str):
02072 """
02073 unpack serialized message in str into this message instance
02074 :param str: byte array of serialized message, ``str``
02075 """
02076 try:
02077 if self.action_goal is None:
02078 self.action_goal = object_manipulation_msgs.msg.GraspPlanningActionGoal()
02079 if self.action_result is None:
02080 self.action_result = object_manipulation_msgs.msg.GraspPlanningActionResult()
02081 if self.action_feedback is None:
02082 self.action_feedback = object_manipulation_msgs.msg.GraspPlanningActionFeedback()
02083 end = 0
02084 _x = self
02085 start = end
02086 end += 12
02087 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02088 start = end
02089 end += 4
02090 (length,) = _struct_I.unpack(str[start:end])
02091 start = end
02092 end += length
02093 if python3:
02094 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
02095 else:
02096 self.action_goal.header.frame_id = str[start:end]
02097 _x = self
02098 start = end
02099 end += 8
02100 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02101 start = end
02102 end += 4
02103 (length,) = _struct_I.unpack(str[start:end])
02104 start = end
02105 end += length
02106 if python3:
02107 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
02108 else:
02109 self.action_goal.goal_id.id = str[start:end]
02110 start = end
02111 end += 4
02112 (length,) = _struct_I.unpack(str[start:end])
02113 start = end
02114 end += length
02115 if python3:
02116 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
02117 else:
02118 self.action_goal.goal.arm_name = str[start:end]
02119 start = end
02120 end += 4
02121 (length,) = _struct_I.unpack(str[start:end])
02122 start = end
02123 end += length
02124 if python3:
02125 self.action_goal.goal.target.reference_frame_id = str[start:end].decode('utf-8')
02126 else:
02127 self.action_goal.goal.target.reference_frame_id = str[start:end]
02128 start = end
02129 end += 4
02130 (length,) = _struct_I.unpack(str[start:end])
02131 self.action_goal.goal.target.potential_models = []
02132 for i in range(0, length):
02133 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
02134 start = end
02135 end += 4
02136 (val1.model_id,) = _struct_i.unpack(str[start:end])
02137 _v154 = val1.pose
02138 _v155 = _v154.header
02139 start = end
02140 end += 4
02141 (_v155.seq,) = _struct_I.unpack(str[start:end])
02142 _v156 = _v155.stamp
02143 _x = _v156
02144 start = end
02145 end += 8
02146 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02147 start = end
02148 end += 4
02149 (length,) = _struct_I.unpack(str[start:end])
02150 start = end
02151 end += length
02152 if python3:
02153 _v155.frame_id = str[start:end].decode('utf-8')
02154 else:
02155 _v155.frame_id = str[start:end]
02156 _v157 = _v154.pose
02157 _v158 = _v157.position
02158 _x = _v158
02159 start = end
02160 end += 24
02161 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02162 _v159 = _v157.orientation
02163 _x = _v159
02164 start = end
02165 end += 32
02166 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02167 start = end
02168 end += 4
02169 (val1.confidence,) = _struct_f.unpack(str[start:end])
02170 start = end
02171 end += 4
02172 (length,) = _struct_I.unpack(str[start:end])
02173 start = end
02174 end += length
02175 if python3:
02176 val1.detector_name = str[start:end].decode('utf-8')
02177 else:
02178 val1.detector_name = str[start:end]
02179 self.action_goal.goal.target.potential_models.append(val1)
02180 _x = self
02181 start = end
02182 end += 12
02183 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02184 start = end
02185 end += 4
02186 (length,) = _struct_I.unpack(str[start:end])
02187 start = end
02188 end += length
02189 if python3:
02190 self.action_goal.goal.target.cluster.header.frame_id = str[start:end].decode('utf-8')
02191 else:
02192 self.action_goal.goal.target.cluster.header.frame_id = str[start:end]
02193 start = end
02194 end += 4
02195 (length,) = _struct_I.unpack(str[start:end])
02196 self.action_goal.goal.target.cluster.points = []
02197 for i in range(0, length):
02198 val1 = geometry_msgs.msg.Point32()
02199 _x = val1
02200 start = end
02201 end += 12
02202 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02203 self.action_goal.goal.target.cluster.points.append(val1)
02204 start = end
02205 end += 4
02206 (length,) = _struct_I.unpack(str[start:end])
02207 self.action_goal.goal.target.cluster.channels = []
02208 for i in range(0, length):
02209 val1 = sensor_msgs.msg.ChannelFloat32()
02210 start = end
02211 end += 4
02212 (length,) = _struct_I.unpack(str[start:end])
02213 start = end
02214 end += length
02215 if python3:
02216 val1.name = str[start:end].decode('utf-8')
02217 else:
02218 val1.name = str[start:end]
02219 start = end
02220 end += 4
02221 (length,) = _struct_I.unpack(str[start:end])
02222 pattern = '<%sf'%length
02223 start = end
02224 end += struct.calcsize(pattern)
02225 val1.values = struct.unpack(pattern, str[start:end])
02226 self.action_goal.goal.target.cluster.channels.append(val1)
02227 _x = self
02228 start = end
02229 end += 12
02230 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02231 start = end
02232 end += 4
02233 (length,) = _struct_I.unpack(str[start:end])
02234 start = end
02235 end += length
02236 if python3:
02237 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end].decode('utf-8')
02238 else:
02239 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end]
02240 _x = self
02241 start = end
02242 end += 8
02243 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
02244 start = end
02245 end += 4
02246 (length,) = _struct_I.unpack(str[start:end])
02247 self.action_goal.goal.target.region.cloud.fields = []
02248 for i in range(0, length):
02249 val1 = sensor_msgs.msg.PointField()
02250 start = end
02251 end += 4
02252 (length,) = _struct_I.unpack(str[start:end])
02253 start = end
02254 end += length
02255 if python3:
02256 val1.name = str[start:end].decode('utf-8')
02257 else:
02258 val1.name = str[start:end]
02259 _x = val1
02260 start = end
02261 end += 9
02262 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02263 self.action_goal.goal.target.region.cloud.fields.append(val1)
02264 _x = self
02265 start = end
02266 end += 9
02267 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
02268 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian)
02269 start = end
02270 end += 4
02271 (length,) = _struct_I.unpack(str[start:end])
02272 start = end
02273 end += length
02274 if python3:
02275 self.action_goal.goal.target.region.cloud.data = str[start:end].decode('utf-8')
02276 else:
02277 self.action_goal.goal.target.region.cloud.data = str[start:end]
02278 start = end
02279 end += 1
02280 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
02281 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense)
02282 start = end
02283 end += 4
02284 (length,) = _struct_I.unpack(str[start:end])
02285 pattern = '<%si'%length
02286 start = end
02287 end += struct.calcsize(pattern)
02288 self.action_goal.goal.target.region.mask = struct.unpack(pattern, str[start:end])
02289 _x = self
02290 start = end
02291 end += 12
02292 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02293 start = end
02294 end += 4
02295 (length,) = _struct_I.unpack(str[start:end])
02296 start = end
02297 end += length
02298 if python3:
02299 self.action_goal.goal.target.region.image.header.frame_id = str[start:end].decode('utf-8')
02300 else:
02301 self.action_goal.goal.target.region.image.header.frame_id = str[start:end]
02302 _x = self
02303 start = end
02304 end += 8
02305 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
02306 start = end
02307 end += 4
02308 (length,) = _struct_I.unpack(str[start:end])
02309 start = end
02310 end += length
02311 if python3:
02312 self.action_goal.goal.target.region.image.encoding = str[start:end].decode('utf-8')
02313 else:
02314 self.action_goal.goal.target.region.image.encoding = str[start:end]
02315 _x = self
02316 start = end
02317 end += 5
02318 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
02319 start = end
02320 end += 4
02321 (length,) = _struct_I.unpack(str[start:end])
02322 start = end
02323 end += length
02324 if python3:
02325 self.action_goal.goal.target.region.image.data = str[start:end].decode('utf-8')
02326 else:
02327 self.action_goal.goal.target.region.image.data = str[start:end]
02328 _x = self
02329 start = end
02330 end += 12
02331 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02332 start = end
02333 end += 4
02334 (length,) = _struct_I.unpack(str[start:end])
02335 start = end
02336 end += length
02337 if python3:
02338 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
02339 else:
02340 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end]
02341 _x = self
02342 start = end
02343 end += 8
02344 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
02345 start = end
02346 end += 4
02347 (length,) = _struct_I.unpack(str[start:end])
02348 start = end
02349 end += length
02350 if python3:
02351 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end].decode('utf-8')
02352 else:
02353 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end]
02354 _x = self
02355 start = end
02356 end += 5
02357 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
02358 start = end
02359 end += 4
02360 (length,) = _struct_I.unpack(str[start:end])
02361 start = end
02362 end += length
02363 if python3:
02364 self.action_goal.goal.target.region.disparity_image.data = str[start:end].decode('utf-8')
02365 else:
02366 self.action_goal.goal.target.region.disparity_image.data = str[start:end]
02367 _x = self
02368 start = end
02369 end += 12
02370 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02371 start = end
02372 end += 4
02373 (length,) = _struct_I.unpack(str[start:end])
02374 start = end
02375 end += length
02376 if python3:
02377 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
02378 else:
02379 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end]
02380 _x = self
02381 start = end
02382 end += 8
02383 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
02384 start = end
02385 end += 4
02386 (length,) = _struct_I.unpack(str[start:end])
02387 start = end
02388 end += length
02389 if python3:
02390 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end].decode('utf-8')
02391 else:
02392 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end]
02393 start = end
02394 end += 4
02395 (length,) = _struct_I.unpack(str[start:end])
02396 pattern = '<%sd'%length
02397 start = end
02398 end += struct.calcsize(pattern)
02399 self.action_goal.goal.target.region.cam_info.D = struct.unpack(pattern, str[start:end])
02400 start = end
02401 end += 72
02402 self.action_goal.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end])
02403 start = end
02404 end += 72
02405 self.action_goal.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end])
02406 start = end
02407 end += 96
02408 self.action_goal.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end])
02409 _x = self
02410 start = end
02411 end += 37
02412 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
02413 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify)
02414 start = end
02415 end += 4
02416 (length,) = _struct_I.unpack(str[start:end])
02417 start = end
02418 end += length
02419 if python3:
02420 self.action_goal.goal.target.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
02421 else:
02422 self.action_goal.goal.target.region.roi_box_pose.header.frame_id = str[start:end]
02423 _x = self
02424 start = end
02425 end += 80
02426 (_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
02427 start = end
02428 end += 4
02429 (length,) = _struct_I.unpack(str[start:end])
02430 start = end
02431 end += length
02432 if python3:
02433 self.action_goal.goal.target.collision_name = str[start:end].decode('utf-8')
02434 else:
02435 self.action_goal.goal.target.collision_name = str[start:end]
02436 start = end
02437 end += 4
02438 (length,) = _struct_I.unpack(str[start:end])
02439 start = end
02440 end += length
02441 if python3:
02442 self.action_goal.goal.collision_object_name = str[start:end].decode('utf-8')
02443 else:
02444 self.action_goal.goal.collision_object_name = str[start:end]
02445 start = end
02446 end += 4
02447 (length,) = _struct_I.unpack(str[start:end])
02448 start = end
02449 end += length
02450 if python3:
02451 self.action_goal.goal.collision_support_surface_name = str[start:end].decode('utf-8')
02452 else:
02453 self.action_goal.goal.collision_support_surface_name = str[start:end]
02454 start = end
02455 end += 4
02456 (length,) = _struct_I.unpack(str[start:end])
02457 self.action_goal.goal.grasps_to_evaluate = []
02458 for i in range(0, length):
02459 val1 = object_manipulation_msgs.msg.Grasp()
02460 _v160 = val1.pre_grasp_posture
02461 _v161 = _v160.header
02462 start = end
02463 end += 4
02464 (_v161.seq,) = _struct_I.unpack(str[start:end])
02465 _v162 = _v161.stamp
02466 _x = _v162
02467 start = end
02468 end += 8
02469 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02470 start = end
02471 end += 4
02472 (length,) = _struct_I.unpack(str[start:end])
02473 start = end
02474 end += length
02475 if python3:
02476 _v161.frame_id = str[start:end].decode('utf-8')
02477 else:
02478 _v161.frame_id = str[start:end]
02479 start = end
02480 end += 4
02481 (length,) = _struct_I.unpack(str[start:end])
02482 _v160.name = []
02483 for i in range(0, length):
02484 start = end
02485 end += 4
02486 (length,) = _struct_I.unpack(str[start:end])
02487 start = end
02488 end += length
02489 if python3:
02490 val3 = str[start:end].decode('utf-8')
02491 else:
02492 val3 = str[start:end]
02493 _v160.name.append(val3)
02494 start = end
02495 end += 4
02496 (length,) = _struct_I.unpack(str[start:end])
02497 pattern = '<%sd'%length
02498 start = end
02499 end += struct.calcsize(pattern)
02500 _v160.position = struct.unpack(pattern, str[start:end])
02501 start = end
02502 end += 4
02503 (length,) = _struct_I.unpack(str[start:end])
02504 pattern = '<%sd'%length
02505 start = end
02506 end += struct.calcsize(pattern)
02507 _v160.velocity = struct.unpack(pattern, str[start:end])
02508 start = end
02509 end += 4
02510 (length,) = _struct_I.unpack(str[start:end])
02511 pattern = '<%sd'%length
02512 start = end
02513 end += struct.calcsize(pattern)
02514 _v160.effort = struct.unpack(pattern, str[start:end])
02515 _v163 = val1.grasp_posture
02516 _v164 = _v163.header
02517 start = end
02518 end += 4
02519 (_v164.seq,) = _struct_I.unpack(str[start:end])
02520 _v165 = _v164.stamp
02521 _x = _v165
02522 start = end
02523 end += 8
02524 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02525 start = end
02526 end += 4
02527 (length,) = _struct_I.unpack(str[start:end])
02528 start = end
02529 end += length
02530 if python3:
02531 _v164.frame_id = str[start:end].decode('utf-8')
02532 else:
02533 _v164.frame_id = str[start:end]
02534 start = end
02535 end += 4
02536 (length,) = _struct_I.unpack(str[start:end])
02537 _v163.name = []
02538 for i in range(0, length):
02539 start = end
02540 end += 4
02541 (length,) = _struct_I.unpack(str[start:end])
02542 start = end
02543 end += length
02544 if python3:
02545 val3 = str[start:end].decode('utf-8')
02546 else:
02547 val3 = str[start:end]
02548 _v163.name.append(val3)
02549 start = end
02550 end += 4
02551 (length,) = _struct_I.unpack(str[start:end])
02552 pattern = '<%sd'%length
02553 start = end
02554 end += struct.calcsize(pattern)
02555 _v163.position = struct.unpack(pattern, str[start:end])
02556 start = end
02557 end += 4
02558 (length,) = _struct_I.unpack(str[start:end])
02559 pattern = '<%sd'%length
02560 start = end
02561 end += struct.calcsize(pattern)
02562 _v163.velocity = struct.unpack(pattern, str[start:end])
02563 start = end
02564 end += 4
02565 (length,) = _struct_I.unpack(str[start:end])
02566 pattern = '<%sd'%length
02567 start = end
02568 end += struct.calcsize(pattern)
02569 _v163.effort = struct.unpack(pattern, str[start:end])
02570 _v166 = val1.grasp_pose
02571 _v167 = _v166.position
02572 _x = _v167
02573 start = end
02574 end += 24
02575 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02576 _v168 = _v166.orientation
02577 _x = _v168
02578 start = end
02579 end += 32
02580 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02581 _x = val1
02582 start = end
02583 end += 17
02584 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
02585 val1.cluster_rep = bool(val1.cluster_rep)
02586 start = end
02587 end += 4
02588 (length,) = _struct_I.unpack(str[start:end])
02589 val1.moved_obstacles = []
02590 for i in range(0, length):
02591 val2 = object_manipulation_msgs.msg.GraspableObject()
02592 start = end
02593 end += 4
02594 (length,) = _struct_I.unpack(str[start:end])
02595 start = end
02596 end += length
02597 if python3:
02598 val2.reference_frame_id = str[start:end].decode('utf-8')
02599 else:
02600 val2.reference_frame_id = str[start:end]
02601 start = end
02602 end += 4
02603 (length,) = _struct_I.unpack(str[start:end])
02604 val2.potential_models = []
02605 for i in range(0, length):
02606 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
02607 start = end
02608 end += 4
02609 (val3.model_id,) = _struct_i.unpack(str[start:end])
02610 _v169 = val3.pose
02611 _v170 = _v169.header
02612 start = end
02613 end += 4
02614 (_v170.seq,) = _struct_I.unpack(str[start:end])
02615 _v171 = _v170.stamp
02616 _x = _v171
02617 start = end
02618 end += 8
02619 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02620 start = end
02621 end += 4
02622 (length,) = _struct_I.unpack(str[start:end])
02623 start = end
02624 end += length
02625 if python3:
02626 _v170.frame_id = str[start:end].decode('utf-8')
02627 else:
02628 _v170.frame_id = str[start:end]
02629 _v172 = _v169.pose
02630 _v173 = _v172.position
02631 _x = _v173
02632 start = end
02633 end += 24
02634 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02635 _v174 = _v172.orientation
02636 _x = _v174
02637 start = end
02638 end += 32
02639 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02640 start = end
02641 end += 4
02642 (val3.confidence,) = _struct_f.unpack(str[start:end])
02643 start = end
02644 end += 4
02645 (length,) = _struct_I.unpack(str[start:end])
02646 start = end
02647 end += length
02648 if python3:
02649 val3.detector_name = str[start:end].decode('utf-8')
02650 else:
02651 val3.detector_name = str[start:end]
02652 val2.potential_models.append(val3)
02653 _v175 = val2.cluster
02654 _v176 = _v175.header
02655 start = end
02656 end += 4
02657 (_v176.seq,) = _struct_I.unpack(str[start:end])
02658 _v177 = _v176.stamp
02659 _x = _v177
02660 start = end
02661 end += 8
02662 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02663 start = end
02664 end += 4
02665 (length,) = _struct_I.unpack(str[start:end])
02666 start = end
02667 end += length
02668 if python3:
02669 _v176.frame_id = str[start:end].decode('utf-8')
02670 else:
02671 _v176.frame_id = str[start:end]
02672 start = end
02673 end += 4
02674 (length,) = _struct_I.unpack(str[start:end])
02675 _v175.points = []
02676 for i in range(0, length):
02677 val4 = geometry_msgs.msg.Point32()
02678 _x = val4
02679 start = end
02680 end += 12
02681 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02682 _v175.points.append(val4)
02683 start = end
02684 end += 4
02685 (length,) = _struct_I.unpack(str[start:end])
02686 _v175.channels = []
02687 for i in range(0, length):
02688 val4 = sensor_msgs.msg.ChannelFloat32()
02689 start = end
02690 end += 4
02691 (length,) = _struct_I.unpack(str[start:end])
02692 start = end
02693 end += length
02694 if python3:
02695 val4.name = str[start:end].decode('utf-8')
02696 else:
02697 val4.name = str[start:end]
02698 start = end
02699 end += 4
02700 (length,) = _struct_I.unpack(str[start:end])
02701 pattern = '<%sf'%length
02702 start = end
02703 end += struct.calcsize(pattern)
02704 val4.values = struct.unpack(pattern, str[start:end])
02705 _v175.channels.append(val4)
02706 _v178 = val2.region
02707 _v179 = _v178.cloud
02708 _v180 = _v179.header
02709 start = end
02710 end += 4
02711 (_v180.seq,) = _struct_I.unpack(str[start:end])
02712 _v181 = _v180.stamp
02713 _x = _v181
02714 start = end
02715 end += 8
02716 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02717 start = end
02718 end += 4
02719 (length,) = _struct_I.unpack(str[start:end])
02720 start = end
02721 end += length
02722 if python3:
02723 _v180.frame_id = str[start:end].decode('utf-8')
02724 else:
02725 _v180.frame_id = str[start:end]
02726 _x = _v179
02727 start = end
02728 end += 8
02729 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02730 start = end
02731 end += 4
02732 (length,) = _struct_I.unpack(str[start:end])
02733 _v179.fields = []
02734 for i in range(0, length):
02735 val5 = sensor_msgs.msg.PointField()
02736 start = end
02737 end += 4
02738 (length,) = _struct_I.unpack(str[start:end])
02739 start = end
02740 end += length
02741 if python3:
02742 val5.name = str[start:end].decode('utf-8')
02743 else:
02744 val5.name = str[start:end]
02745 _x = val5
02746 start = end
02747 end += 9
02748 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02749 _v179.fields.append(val5)
02750 _x = _v179
02751 start = end
02752 end += 9
02753 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
02754 _v179.is_bigendian = bool(_v179.is_bigendian)
02755 start = end
02756 end += 4
02757 (length,) = _struct_I.unpack(str[start:end])
02758 start = end
02759 end += length
02760 if python3:
02761 _v179.data = str[start:end].decode('utf-8')
02762 else:
02763 _v179.data = str[start:end]
02764 start = end
02765 end += 1
02766 (_v179.is_dense,) = _struct_B.unpack(str[start:end])
02767 _v179.is_dense = bool(_v179.is_dense)
02768 start = end
02769 end += 4
02770 (length,) = _struct_I.unpack(str[start:end])
02771 pattern = '<%si'%length
02772 start = end
02773 end += struct.calcsize(pattern)
02774 _v178.mask = struct.unpack(pattern, str[start:end])
02775 _v182 = _v178.image
02776 _v183 = _v182.header
02777 start = end
02778 end += 4
02779 (_v183.seq,) = _struct_I.unpack(str[start:end])
02780 _v184 = _v183.stamp
02781 _x = _v184
02782 start = end
02783 end += 8
02784 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02785 start = end
02786 end += 4
02787 (length,) = _struct_I.unpack(str[start:end])
02788 start = end
02789 end += length
02790 if python3:
02791 _v183.frame_id = str[start:end].decode('utf-8')
02792 else:
02793 _v183.frame_id = str[start:end]
02794 _x = _v182
02795 start = end
02796 end += 8
02797 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02798 start = end
02799 end += 4
02800 (length,) = _struct_I.unpack(str[start:end])
02801 start = end
02802 end += length
02803 if python3:
02804 _v182.encoding = str[start:end].decode('utf-8')
02805 else:
02806 _v182.encoding = str[start:end]
02807 _x = _v182
02808 start = end
02809 end += 5
02810 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02811 start = end
02812 end += 4
02813 (length,) = _struct_I.unpack(str[start:end])
02814 start = end
02815 end += length
02816 if python3:
02817 _v182.data = str[start:end].decode('utf-8')
02818 else:
02819 _v182.data = str[start:end]
02820 _v185 = _v178.disparity_image
02821 _v186 = _v185.header
02822 start = end
02823 end += 4
02824 (_v186.seq,) = _struct_I.unpack(str[start:end])
02825 _v187 = _v186.stamp
02826 _x = _v187
02827 start = end
02828 end += 8
02829 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02830 start = end
02831 end += 4
02832 (length,) = _struct_I.unpack(str[start:end])
02833 start = end
02834 end += length
02835 if python3:
02836 _v186.frame_id = str[start:end].decode('utf-8')
02837 else:
02838 _v186.frame_id = str[start:end]
02839 _x = _v185
02840 start = end
02841 end += 8
02842 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02843 start = end
02844 end += 4
02845 (length,) = _struct_I.unpack(str[start:end])
02846 start = end
02847 end += length
02848 if python3:
02849 _v185.encoding = str[start:end].decode('utf-8')
02850 else:
02851 _v185.encoding = str[start:end]
02852 _x = _v185
02853 start = end
02854 end += 5
02855 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02856 start = end
02857 end += 4
02858 (length,) = _struct_I.unpack(str[start:end])
02859 start = end
02860 end += length
02861 if python3:
02862 _v185.data = str[start:end].decode('utf-8')
02863 else:
02864 _v185.data = str[start:end]
02865 _v188 = _v178.cam_info
02866 _v189 = _v188.header
02867 start = end
02868 end += 4
02869 (_v189.seq,) = _struct_I.unpack(str[start:end])
02870 _v190 = _v189.stamp
02871 _x = _v190
02872 start = end
02873 end += 8
02874 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02875 start = end
02876 end += 4
02877 (length,) = _struct_I.unpack(str[start:end])
02878 start = end
02879 end += length
02880 if python3:
02881 _v189.frame_id = str[start:end].decode('utf-8')
02882 else:
02883 _v189.frame_id = str[start:end]
02884 _x = _v188
02885 start = end
02886 end += 8
02887 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02888 start = end
02889 end += 4
02890 (length,) = _struct_I.unpack(str[start:end])
02891 start = end
02892 end += length
02893 if python3:
02894 _v188.distortion_model = str[start:end].decode('utf-8')
02895 else:
02896 _v188.distortion_model = str[start:end]
02897 start = end
02898 end += 4
02899 (length,) = _struct_I.unpack(str[start:end])
02900 pattern = '<%sd'%length
02901 start = end
02902 end += struct.calcsize(pattern)
02903 _v188.D = struct.unpack(pattern, str[start:end])
02904 start = end
02905 end += 72
02906 _v188.K = _struct_9d.unpack(str[start:end])
02907 start = end
02908 end += 72
02909 _v188.R = _struct_9d.unpack(str[start:end])
02910 start = end
02911 end += 96
02912 _v188.P = _struct_12d.unpack(str[start:end])
02913 _x = _v188
02914 start = end
02915 end += 8
02916 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
02917 _v191 = _v188.roi
02918 _x = _v191
02919 start = end
02920 end += 17
02921 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
02922 _v191.do_rectify = bool(_v191.do_rectify)
02923 _v192 = _v178.roi_box_pose
02924 _v193 = _v192.header
02925 start = end
02926 end += 4
02927 (_v193.seq,) = _struct_I.unpack(str[start:end])
02928 _v194 = _v193.stamp
02929 _x = _v194
02930 start = end
02931 end += 8
02932 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02933 start = end
02934 end += 4
02935 (length,) = _struct_I.unpack(str[start:end])
02936 start = end
02937 end += length
02938 if python3:
02939 _v193.frame_id = str[start:end].decode('utf-8')
02940 else:
02941 _v193.frame_id = str[start:end]
02942 _v195 = _v192.pose
02943 _v196 = _v195.position
02944 _x = _v196
02945 start = end
02946 end += 24
02947 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02948 _v197 = _v195.orientation
02949 _x = _v197
02950 start = end
02951 end += 32
02952 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02953 _v198 = _v178.roi_box_dims
02954 _x = _v198
02955 start = end
02956 end += 24
02957 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02958 start = end
02959 end += 4
02960 (length,) = _struct_I.unpack(str[start:end])
02961 start = end
02962 end += length
02963 if python3:
02964 val2.collision_name = str[start:end].decode('utf-8')
02965 else:
02966 val2.collision_name = str[start:end]
02967 val1.moved_obstacles.append(val2)
02968 self.action_goal.goal.grasps_to_evaluate.append(val1)
02969 start = end
02970 end += 4
02971 (length,) = _struct_I.unpack(str[start:end])
02972 self.action_goal.goal.movable_obstacles = []
02973 for i in range(0, length):
02974 val1 = object_manipulation_msgs.msg.GraspableObject()
02975 start = end
02976 end += 4
02977 (length,) = _struct_I.unpack(str[start:end])
02978 start = end
02979 end += length
02980 if python3:
02981 val1.reference_frame_id = str[start:end].decode('utf-8')
02982 else:
02983 val1.reference_frame_id = str[start:end]
02984 start = end
02985 end += 4
02986 (length,) = _struct_I.unpack(str[start:end])
02987 val1.potential_models = []
02988 for i in range(0, length):
02989 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
02990 start = end
02991 end += 4
02992 (val2.model_id,) = _struct_i.unpack(str[start:end])
02993 _v199 = val2.pose
02994 _v200 = _v199.header
02995 start = end
02996 end += 4
02997 (_v200.seq,) = _struct_I.unpack(str[start:end])
02998 _v201 = _v200.stamp
02999 _x = _v201
03000 start = end
03001 end += 8
03002 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03003 start = end
03004 end += 4
03005 (length,) = _struct_I.unpack(str[start:end])
03006 start = end
03007 end += length
03008 if python3:
03009 _v200.frame_id = str[start:end].decode('utf-8')
03010 else:
03011 _v200.frame_id = str[start:end]
03012 _v202 = _v199.pose
03013 _v203 = _v202.position
03014 _x = _v203
03015 start = end
03016 end += 24
03017 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03018 _v204 = _v202.orientation
03019 _x = _v204
03020 start = end
03021 end += 32
03022 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03023 start = end
03024 end += 4
03025 (val2.confidence,) = _struct_f.unpack(str[start:end])
03026 start = end
03027 end += 4
03028 (length,) = _struct_I.unpack(str[start:end])
03029 start = end
03030 end += length
03031 if python3:
03032 val2.detector_name = str[start:end].decode('utf-8')
03033 else:
03034 val2.detector_name = str[start:end]
03035 val1.potential_models.append(val2)
03036 _v205 = val1.cluster
03037 _v206 = _v205.header
03038 start = end
03039 end += 4
03040 (_v206.seq,) = _struct_I.unpack(str[start:end])
03041 _v207 = _v206.stamp
03042 _x = _v207
03043 start = end
03044 end += 8
03045 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03046 start = end
03047 end += 4
03048 (length,) = _struct_I.unpack(str[start:end])
03049 start = end
03050 end += length
03051 if python3:
03052 _v206.frame_id = str[start:end].decode('utf-8')
03053 else:
03054 _v206.frame_id = str[start:end]
03055 start = end
03056 end += 4
03057 (length,) = _struct_I.unpack(str[start:end])
03058 _v205.points = []
03059 for i in range(0, length):
03060 val3 = geometry_msgs.msg.Point32()
03061 _x = val3
03062 start = end
03063 end += 12
03064 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03065 _v205.points.append(val3)
03066 start = end
03067 end += 4
03068 (length,) = _struct_I.unpack(str[start:end])
03069 _v205.channels = []
03070 for i in range(0, length):
03071 val3 = sensor_msgs.msg.ChannelFloat32()
03072 start = end
03073 end += 4
03074 (length,) = _struct_I.unpack(str[start:end])
03075 start = end
03076 end += length
03077 if python3:
03078 val3.name = str[start:end].decode('utf-8')
03079 else:
03080 val3.name = str[start:end]
03081 start = end
03082 end += 4
03083 (length,) = _struct_I.unpack(str[start:end])
03084 pattern = '<%sf'%length
03085 start = end
03086 end += struct.calcsize(pattern)
03087 val3.values = struct.unpack(pattern, str[start:end])
03088 _v205.channels.append(val3)
03089 _v208 = val1.region
03090 _v209 = _v208.cloud
03091 _v210 = _v209.header
03092 start = end
03093 end += 4
03094 (_v210.seq,) = _struct_I.unpack(str[start:end])
03095 _v211 = _v210.stamp
03096 _x = _v211
03097 start = end
03098 end += 8
03099 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03100 start = end
03101 end += 4
03102 (length,) = _struct_I.unpack(str[start:end])
03103 start = end
03104 end += length
03105 if python3:
03106 _v210.frame_id = str[start:end].decode('utf-8')
03107 else:
03108 _v210.frame_id = str[start:end]
03109 _x = _v209
03110 start = end
03111 end += 8
03112 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03113 start = end
03114 end += 4
03115 (length,) = _struct_I.unpack(str[start:end])
03116 _v209.fields = []
03117 for i in range(0, length):
03118 val4 = sensor_msgs.msg.PointField()
03119 start = end
03120 end += 4
03121 (length,) = _struct_I.unpack(str[start:end])
03122 start = end
03123 end += length
03124 if python3:
03125 val4.name = str[start:end].decode('utf-8')
03126 else:
03127 val4.name = str[start:end]
03128 _x = val4
03129 start = end
03130 end += 9
03131 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
03132 _v209.fields.append(val4)
03133 _x = _v209
03134 start = end
03135 end += 9
03136 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
03137 _v209.is_bigendian = bool(_v209.is_bigendian)
03138 start = end
03139 end += 4
03140 (length,) = _struct_I.unpack(str[start:end])
03141 start = end
03142 end += length
03143 if python3:
03144 _v209.data = str[start:end].decode('utf-8')
03145 else:
03146 _v209.data = str[start:end]
03147 start = end
03148 end += 1
03149 (_v209.is_dense,) = _struct_B.unpack(str[start:end])
03150 _v209.is_dense = bool(_v209.is_dense)
03151 start = end
03152 end += 4
03153 (length,) = _struct_I.unpack(str[start:end])
03154 pattern = '<%si'%length
03155 start = end
03156 end += struct.calcsize(pattern)
03157 _v208.mask = struct.unpack(pattern, str[start:end])
03158 _v212 = _v208.image
03159 _v213 = _v212.header
03160 start = end
03161 end += 4
03162 (_v213.seq,) = _struct_I.unpack(str[start:end])
03163 _v214 = _v213.stamp
03164 _x = _v214
03165 start = end
03166 end += 8
03167 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03168 start = end
03169 end += 4
03170 (length,) = _struct_I.unpack(str[start:end])
03171 start = end
03172 end += length
03173 if python3:
03174 _v213.frame_id = str[start:end].decode('utf-8')
03175 else:
03176 _v213.frame_id = str[start:end]
03177 _x = _v212
03178 start = end
03179 end += 8
03180 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03181 start = end
03182 end += 4
03183 (length,) = _struct_I.unpack(str[start:end])
03184 start = end
03185 end += length
03186 if python3:
03187 _v212.encoding = str[start:end].decode('utf-8')
03188 else:
03189 _v212.encoding = str[start:end]
03190 _x = _v212
03191 start = end
03192 end += 5
03193 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03194 start = end
03195 end += 4
03196 (length,) = _struct_I.unpack(str[start:end])
03197 start = end
03198 end += length
03199 if python3:
03200 _v212.data = str[start:end].decode('utf-8')
03201 else:
03202 _v212.data = str[start:end]
03203 _v215 = _v208.disparity_image
03204 _v216 = _v215.header
03205 start = end
03206 end += 4
03207 (_v216.seq,) = _struct_I.unpack(str[start:end])
03208 _v217 = _v216.stamp
03209 _x = _v217
03210 start = end
03211 end += 8
03212 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03213 start = end
03214 end += 4
03215 (length,) = _struct_I.unpack(str[start:end])
03216 start = end
03217 end += length
03218 if python3:
03219 _v216.frame_id = str[start:end].decode('utf-8')
03220 else:
03221 _v216.frame_id = str[start:end]
03222 _x = _v215
03223 start = end
03224 end += 8
03225 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03226 start = end
03227 end += 4
03228 (length,) = _struct_I.unpack(str[start:end])
03229 start = end
03230 end += length
03231 if python3:
03232 _v215.encoding = str[start:end].decode('utf-8')
03233 else:
03234 _v215.encoding = str[start:end]
03235 _x = _v215
03236 start = end
03237 end += 5
03238 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03239 start = end
03240 end += 4
03241 (length,) = _struct_I.unpack(str[start:end])
03242 start = end
03243 end += length
03244 if python3:
03245 _v215.data = str[start:end].decode('utf-8')
03246 else:
03247 _v215.data = str[start:end]
03248 _v218 = _v208.cam_info
03249 _v219 = _v218.header
03250 start = end
03251 end += 4
03252 (_v219.seq,) = _struct_I.unpack(str[start:end])
03253 _v220 = _v219.stamp
03254 _x = _v220
03255 start = end
03256 end += 8
03257 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03258 start = end
03259 end += 4
03260 (length,) = _struct_I.unpack(str[start:end])
03261 start = end
03262 end += length
03263 if python3:
03264 _v219.frame_id = str[start:end].decode('utf-8')
03265 else:
03266 _v219.frame_id = str[start:end]
03267 _x = _v218
03268 start = end
03269 end += 8
03270 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03271 start = end
03272 end += 4
03273 (length,) = _struct_I.unpack(str[start:end])
03274 start = end
03275 end += length
03276 if python3:
03277 _v218.distortion_model = str[start:end].decode('utf-8')
03278 else:
03279 _v218.distortion_model = str[start:end]
03280 start = end
03281 end += 4
03282 (length,) = _struct_I.unpack(str[start:end])
03283 pattern = '<%sd'%length
03284 start = end
03285 end += struct.calcsize(pattern)
03286 _v218.D = struct.unpack(pattern, str[start:end])
03287 start = end
03288 end += 72
03289 _v218.K = _struct_9d.unpack(str[start:end])
03290 start = end
03291 end += 72
03292 _v218.R = _struct_9d.unpack(str[start:end])
03293 start = end
03294 end += 96
03295 _v218.P = _struct_12d.unpack(str[start:end])
03296 _x = _v218
03297 start = end
03298 end += 8
03299 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
03300 _v221 = _v218.roi
03301 _x = _v221
03302 start = end
03303 end += 17
03304 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
03305 _v221.do_rectify = bool(_v221.do_rectify)
03306 _v222 = _v208.roi_box_pose
03307 _v223 = _v222.header
03308 start = end
03309 end += 4
03310 (_v223.seq,) = _struct_I.unpack(str[start:end])
03311 _v224 = _v223.stamp
03312 _x = _v224
03313 start = end
03314 end += 8
03315 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03316 start = end
03317 end += 4
03318 (length,) = _struct_I.unpack(str[start:end])
03319 start = end
03320 end += length
03321 if python3:
03322 _v223.frame_id = str[start:end].decode('utf-8')
03323 else:
03324 _v223.frame_id = str[start:end]
03325 _v225 = _v222.pose
03326 _v226 = _v225.position
03327 _x = _v226
03328 start = end
03329 end += 24
03330 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03331 _v227 = _v225.orientation
03332 _x = _v227
03333 start = end
03334 end += 32
03335 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03336 _v228 = _v208.roi_box_dims
03337 _x = _v228
03338 start = end
03339 end += 24
03340 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03341 start = end
03342 end += 4
03343 (length,) = _struct_I.unpack(str[start:end])
03344 start = end
03345 end += length
03346 if python3:
03347 val1.collision_name = str[start:end].decode('utf-8')
03348 else:
03349 val1.collision_name = str[start:end]
03350 self.action_goal.goal.movable_obstacles.append(val1)
03351 _x = self
03352 start = end
03353 end += 12
03354 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.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 self.action_result.header.frame_id = str[start:end].decode('utf-8')
03362 else:
03363 self.action_result.header.frame_id = str[start:end]
03364 _x = self
03365 start = end
03366 end += 8
03367 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
03368 start = end
03369 end += 4
03370 (length,) = _struct_I.unpack(str[start:end])
03371 start = end
03372 end += length
03373 if python3:
03374 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
03375 else:
03376 self.action_result.status.goal_id.id = str[start:end]
03377 start = end
03378 end += 1
03379 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
03380 start = end
03381 end += 4
03382 (length,) = _struct_I.unpack(str[start:end])
03383 start = end
03384 end += length
03385 if python3:
03386 self.action_result.status.text = str[start:end].decode('utf-8')
03387 else:
03388 self.action_result.status.text = str[start:end]
03389 start = end
03390 end += 4
03391 (length,) = _struct_I.unpack(str[start:end])
03392 self.action_result.result.grasps = []
03393 for i in range(0, length):
03394 val1 = object_manipulation_msgs.msg.Grasp()
03395 _v229 = val1.pre_grasp_posture
03396 _v230 = _v229.header
03397 start = end
03398 end += 4
03399 (_v230.seq,) = _struct_I.unpack(str[start:end])
03400 _v231 = _v230.stamp
03401 _x = _v231
03402 start = end
03403 end += 8
03404 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03405 start = end
03406 end += 4
03407 (length,) = _struct_I.unpack(str[start:end])
03408 start = end
03409 end += length
03410 if python3:
03411 _v230.frame_id = str[start:end].decode('utf-8')
03412 else:
03413 _v230.frame_id = str[start:end]
03414 start = end
03415 end += 4
03416 (length,) = _struct_I.unpack(str[start:end])
03417 _v229.name = []
03418 for i in range(0, length):
03419 start = end
03420 end += 4
03421 (length,) = _struct_I.unpack(str[start:end])
03422 start = end
03423 end += length
03424 if python3:
03425 val3 = str[start:end].decode('utf-8')
03426 else:
03427 val3 = str[start:end]
03428 _v229.name.append(val3)
03429 start = end
03430 end += 4
03431 (length,) = _struct_I.unpack(str[start:end])
03432 pattern = '<%sd'%length
03433 start = end
03434 end += struct.calcsize(pattern)
03435 _v229.position = struct.unpack(pattern, str[start:end])
03436 start = end
03437 end += 4
03438 (length,) = _struct_I.unpack(str[start:end])
03439 pattern = '<%sd'%length
03440 start = end
03441 end += struct.calcsize(pattern)
03442 _v229.velocity = struct.unpack(pattern, str[start:end])
03443 start = end
03444 end += 4
03445 (length,) = _struct_I.unpack(str[start:end])
03446 pattern = '<%sd'%length
03447 start = end
03448 end += struct.calcsize(pattern)
03449 _v229.effort = struct.unpack(pattern, str[start:end])
03450 _v232 = val1.grasp_posture
03451 _v233 = _v232.header
03452 start = end
03453 end += 4
03454 (_v233.seq,) = _struct_I.unpack(str[start:end])
03455 _v234 = _v233.stamp
03456 _x = _v234
03457 start = end
03458 end += 8
03459 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03460 start = end
03461 end += 4
03462 (length,) = _struct_I.unpack(str[start:end])
03463 start = end
03464 end += length
03465 if python3:
03466 _v233.frame_id = str[start:end].decode('utf-8')
03467 else:
03468 _v233.frame_id = str[start:end]
03469 start = end
03470 end += 4
03471 (length,) = _struct_I.unpack(str[start:end])
03472 _v232.name = []
03473 for i in range(0, length):
03474 start = end
03475 end += 4
03476 (length,) = _struct_I.unpack(str[start:end])
03477 start = end
03478 end += length
03479 if python3:
03480 val3 = str[start:end].decode('utf-8')
03481 else:
03482 val3 = str[start:end]
03483 _v232.name.append(val3)
03484 start = end
03485 end += 4
03486 (length,) = _struct_I.unpack(str[start:end])
03487 pattern = '<%sd'%length
03488 start = end
03489 end += struct.calcsize(pattern)
03490 _v232.position = struct.unpack(pattern, str[start:end])
03491 start = end
03492 end += 4
03493 (length,) = _struct_I.unpack(str[start:end])
03494 pattern = '<%sd'%length
03495 start = end
03496 end += struct.calcsize(pattern)
03497 _v232.velocity = struct.unpack(pattern, str[start:end])
03498 start = end
03499 end += 4
03500 (length,) = _struct_I.unpack(str[start:end])
03501 pattern = '<%sd'%length
03502 start = end
03503 end += struct.calcsize(pattern)
03504 _v232.effort = struct.unpack(pattern, str[start:end])
03505 _v235 = val1.grasp_pose
03506 _v236 = _v235.position
03507 _x = _v236
03508 start = end
03509 end += 24
03510 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03511 _v237 = _v235.orientation
03512 _x = _v237
03513 start = end
03514 end += 32
03515 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03516 _x = val1
03517 start = end
03518 end += 17
03519 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
03520 val1.cluster_rep = bool(val1.cluster_rep)
03521 start = end
03522 end += 4
03523 (length,) = _struct_I.unpack(str[start:end])
03524 val1.moved_obstacles = []
03525 for i in range(0, length):
03526 val2 = object_manipulation_msgs.msg.GraspableObject()
03527 start = end
03528 end += 4
03529 (length,) = _struct_I.unpack(str[start:end])
03530 start = end
03531 end += length
03532 if python3:
03533 val2.reference_frame_id = str[start:end].decode('utf-8')
03534 else:
03535 val2.reference_frame_id = str[start:end]
03536 start = end
03537 end += 4
03538 (length,) = _struct_I.unpack(str[start:end])
03539 val2.potential_models = []
03540 for i in range(0, length):
03541 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
03542 start = end
03543 end += 4
03544 (val3.model_id,) = _struct_i.unpack(str[start:end])
03545 _v238 = val3.pose
03546 _v239 = _v238.header
03547 start = end
03548 end += 4
03549 (_v239.seq,) = _struct_I.unpack(str[start:end])
03550 _v240 = _v239.stamp
03551 _x = _v240
03552 start = end
03553 end += 8
03554 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03555 start = end
03556 end += 4
03557 (length,) = _struct_I.unpack(str[start:end])
03558 start = end
03559 end += length
03560 if python3:
03561 _v239.frame_id = str[start:end].decode('utf-8')
03562 else:
03563 _v239.frame_id = str[start:end]
03564 _v241 = _v238.pose
03565 _v242 = _v241.position
03566 _x = _v242
03567 start = end
03568 end += 24
03569 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03570 _v243 = _v241.orientation
03571 _x = _v243
03572 start = end
03573 end += 32
03574 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03575 start = end
03576 end += 4
03577 (val3.confidence,) = _struct_f.unpack(str[start:end])
03578 start = end
03579 end += 4
03580 (length,) = _struct_I.unpack(str[start:end])
03581 start = end
03582 end += length
03583 if python3:
03584 val3.detector_name = str[start:end].decode('utf-8')
03585 else:
03586 val3.detector_name = str[start:end]
03587 val2.potential_models.append(val3)
03588 _v244 = val2.cluster
03589 _v245 = _v244.header
03590 start = end
03591 end += 4
03592 (_v245.seq,) = _struct_I.unpack(str[start:end])
03593 _v246 = _v245.stamp
03594 _x = _v246
03595 start = end
03596 end += 8
03597 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03598 start = end
03599 end += 4
03600 (length,) = _struct_I.unpack(str[start:end])
03601 start = end
03602 end += length
03603 if python3:
03604 _v245.frame_id = str[start:end].decode('utf-8')
03605 else:
03606 _v245.frame_id = str[start:end]
03607 start = end
03608 end += 4
03609 (length,) = _struct_I.unpack(str[start:end])
03610 _v244.points = []
03611 for i in range(0, length):
03612 val4 = geometry_msgs.msg.Point32()
03613 _x = val4
03614 start = end
03615 end += 12
03616 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03617 _v244.points.append(val4)
03618 start = end
03619 end += 4
03620 (length,) = _struct_I.unpack(str[start:end])
03621 _v244.channels = []
03622 for i in range(0, length):
03623 val4 = sensor_msgs.msg.ChannelFloat32()
03624 start = end
03625 end += 4
03626 (length,) = _struct_I.unpack(str[start:end])
03627 start = end
03628 end += length
03629 if python3:
03630 val4.name = str[start:end].decode('utf-8')
03631 else:
03632 val4.name = str[start:end]
03633 start = end
03634 end += 4
03635 (length,) = _struct_I.unpack(str[start:end])
03636 pattern = '<%sf'%length
03637 start = end
03638 end += struct.calcsize(pattern)
03639 val4.values = struct.unpack(pattern, str[start:end])
03640 _v244.channels.append(val4)
03641 _v247 = val2.region
03642 _v248 = _v247.cloud
03643 _v249 = _v248.header
03644 start = end
03645 end += 4
03646 (_v249.seq,) = _struct_I.unpack(str[start:end])
03647 _v250 = _v249.stamp
03648 _x = _v250
03649 start = end
03650 end += 8
03651 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03652 start = end
03653 end += 4
03654 (length,) = _struct_I.unpack(str[start:end])
03655 start = end
03656 end += length
03657 if python3:
03658 _v249.frame_id = str[start:end].decode('utf-8')
03659 else:
03660 _v249.frame_id = str[start:end]
03661 _x = _v248
03662 start = end
03663 end += 8
03664 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03665 start = end
03666 end += 4
03667 (length,) = _struct_I.unpack(str[start:end])
03668 _v248.fields = []
03669 for i in range(0, length):
03670 val5 = sensor_msgs.msg.PointField()
03671 start = end
03672 end += 4
03673 (length,) = _struct_I.unpack(str[start:end])
03674 start = end
03675 end += length
03676 if python3:
03677 val5.name = str[start:end].decode('utf-8')
03678 else:
03679 val5.name = str[start:end]
03680 _x = val5
03681 start = end
03682 end += 9
03683 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
03684 _v248.fields.append(val5)
03685 _x = _v248
03686 start = end
03687 end += 9
03688 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
03689 _v248.is_bigendian = bool(_v248.is_bigendian)
03690 start = end
03691 end += 4
03692 (length,) = _struct_I.unpack(str[start:end])
03693 start = end
03694 end += length
03695 if python3:
03696 _v248.data = str[start:end].decode('utf-8')
03697 else:
03698 _v248.data = str[start:end]
03699 start = end
03700 end += 1
03701 (_v248.is_dense,) = _struct_B.unpack(str[start:end])
03702 _v248.is_dense = bool(_v248.is_dense)
03703 start = end
03704 end += 4
03705 (length,) = _struct_I.unpack(str[start:end])
03706 pattern = '<%si'%length
03707 start = end
03708 end += struct.calcsize(pattern)
03709 _v247.mask = struct.unpack(pattern, str[start:end])
03710 _v251 = _v247.image
03711 _v252 = _v251.header
03712 start = end
03713 end += 4
03714 (_v252.seq,) = _struct_I.unpack(str[start:end])
03715 _v253 = _v252.stamp
03716 _x = _v253
03717 start = end
03718 end += 8
03719 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03720 start = end
03721 end += 4
03722 (length,) = _struct_I.unpack(str[start:end])
03723 start = end
03724 end += length
03725 if python3:
03726 _v252.frame_id = str[start:end].decode('utf-8')
03727 else:
03728 _v252.frame_id = str[start:end]
03729 _x = _v251
03730 start = end
03731 end += 8
03732 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03733 start = end
03734 end += 4
03735 (length,) = _struct_I.unpack(str[start:end])
03736 start = end
03737 end += length
03738 if python3:
03739 _v251.encoding = str[start:end].decode('utf-8')
03740 else:
03741 _v251.encoding = str[start:end]
03742 _x = _v251
03743 start = end
03744 end += 5
03745 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03746 start = end
03747 end += 4
03748 (length,) = _struct_I.unpack(str[start:end])
03749 start = end
03750 end += length
03751 if python3:
03752 _v251.data = str[start:end].decode('utf-8')
03753 else:
03754 _v251.data = str[start:end]
03755 _v254 = _v247.disparity_image
03756 _v255 = _v254.header
03757 start = end
03758 end += 4
03759 (_v255.seq,) = _struct_I.unpack(str[start:end])
03760 _v256 = _v255.stamp
03761 _x = _v256
03762 start = end
03763 end += 8
03764 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03765 start = end
03766 end += 4
03767 (length,) = _struct_I.unpack(str[start:end])
03768 start = end
03769 end += length
03770 if python3:
03771 _v255.frame_id = str[start:end].decode('utf-8')
03772 else:
03773 _v255.frame_id = str[start:end]
03774 _x = _v254
03775 start = end
03776 end += 8
03777 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03778 start = end
03779 end += 4
03780 (length,) = _struct_I.unpack(str[start:end])
03781 start = end
03782 end += length
03783 if python3:
03784 _v254.encoding = str[start:end].decode('utf-8')
03785 else:
03786 _v254.encoding = str[start:end]
03787 _x = _v254
03788 start = end
03789 end += 5
03790 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03791 start = end
03792 end += 4
03793 (length,) = _struct_I.unpack(str[start:end])
03794 start = end
03795 end += length
03796 if python3:
03797 _v254.data = str[start:end].decode('utf-8')
03798 else:
03799 _v254.data = str[start:end]
03800 _v257 = _v247.cam_info
03801 _v258 = _v257.header
03802 start = end
03803 end += 4
03804 (_v258.seq,) = _struct_I.unpack(str[start:end])
03805 _v259 = _v258.stamp
03806 _x = _v259
03807 start = end
03808 end += 8
03809 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03810 start = end
03811 end += 4
03812 (length,) = _struct_I.unpack(str[start:end])
03813 start = end
03814 end += length
03815 if python3:
03816 _v258.frame_id = str[start:end].decode('utf-8')
03817 else:
03818 _v258.frame_id = str[start:end]
03819 _x = _v257
03820 start = end
03821 end += 8
03822 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03823 start = end
03824 end += 4
03825 (length,) = _struct_I.unpack(str[start:end])
03826 start = end
03827 end += length
03828 if python3:
03829 _v257.distortion_model = str[start:end].decode('utf-8')
03830 else:
03831 _v257.distortion_model = str[start:end]
03832 start = end
03833 end += 4
03834 (length,) = _struct_I.unpack(str[start:end])
03835 pattern = '<%sd'%length
03836 start = end
03837 end += struct.calcsize(pattern)
03838 _v257.D = struct.unpack(pattern, str[start:end])
03839 start = end
03840 end += 72
03841 _v257.K = _struct_9d.unpack(str[start:end])
03842 start = end
03843 end += 72
03844 _v257.R = _struct_9d.unpack(str[start:end])
03845 start = end
03846 end += 96
03847 _v257.P = _struct_12d.unpack(str[start:end])
03848 _x = _v257
03849 start = end
03850 end += 8
03851 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
03852 _v260 = _v257.roi
03853 _x = _v260
03854 start = end
03855 end += 17
03856 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
03857 _v260.do_rectify = bool(_v260.do_rectify)
03858 _v261 = _v247.roi_box_pose
03859 _v262 = _v261.header
03860 start = end
03861 end += 4
03862 (_v262.seq,) = _struct_I.unpack(str[start:end])
03863 _v263 = _v262.stamp
03864 _x = _v263
03865 start = end
03866 end += 8
03867 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03868 start = end
03869 end += 4
03870 (length,) = _struct_I.unpack(str[start:end])
03871 start = end
03872 end += length
03873 if python3:
03874 _v262.frame_id = str[start:end].decode('utf-8')
03875 else:
03876 _v262.frame_id = str[start:end]
03877 _v264 = _v261.pose
03878 _v265 = _v264.position
03879 _x = _v265
03880 start = end
03881 end += 24
03882 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03883 _v266 = _v264.orientation
03884 _x = _v266
03885 start = end
03886 end += 32
03887 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03888 _v267 = _v247.roi_box_dims
03889 _x = _v267
03890 start = end
03891 end += 24
03892 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03893 start = end
03894 end += 4
03895 (length,) = _struct_I.unpack(str[start:end])
03896 start = end
03897 end += length
03898 if python3:
03899 val2.collision_name = str[start:end].decode('utf-8')
03900 else:
03901 val2.collision_name = str[start:end]
03902 val1.moved_obstacles.append(val2)
03903 self.action_result.result.grasps.append(val1)
03904 _x = self
03905 start = end
03906 end += 16
03907 (_x.action_result.result.error_code.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
03908 start = end
03909 end += 4
03910 (length,) = _struct_I.unpack(str[start:end])
03911 start = end
03912 end += length
03913 if python3:
03914 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
03915 else:
03916 self.action_feedback.header.frame_id = str[start:end]
03917 _x = self
03918 start = end
03919 end += 8
03920 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
03921 start = end
03922 end += 4
03923 (length,) = _struct_I.unpack(str[start:end])
03924 start = end
03925 end += length
03926 if python3:
03927 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
03928 else:
03929 self.action_feedback.status.goal_id.id = str[start:end]
03930 start = end
03931 end += 1
03932 (self.action_feedback.status.status,) = _struct_B.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 self.action_feedback.status.text = str[start:end].decode('utf-8')
03940 else:
03941 self.action_feedback.status.text = str[start:end]
03942 start = end
03943 end += 4
03944 (length,) = _struct_I.unpack(str[start:end])
03945 self.action_feedback.feedback.grasps = []
03946 for i in range(0, length):
03947 val1 = object_manipulation_msgs.msg.Grasp()
03948 _v268 = val1.pre_grasp_posture
03949 _v269 = _v268.header
03950 start = end
03951 end += 4
03952 (_v269.seq,) = _struct_I.unpack(str[start:end])
03953 _v270 = _v269.stamp
03954 _x = _v270
03955 start = end
03956 end += 8
03957 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03958 start = end
03959 end += 4
03960 (length,) = _struct_I.unpack(str[start:end])
03961 start = end
03962 end += length
03963 if python3:
03964 _v269.frame_id = str[start:end].decode('utf-8')
03965 else:
03966 _v269.frame_id = str[start:end]
03967 start = end
03968 end += 4
03969 (length,) = _struct_I.unpack(str[start:end])
03970 _v268.name = []
03971 for i in range(0, length):
03972 start = end
03973 end += 4
03974 (length,) = _struct_I.unpack(str[start:end])
03975 start = end
03976 end += length
03977 if python3:
03978 val3 = str[start:end].decode('utf-8')
03979 else:
03980 val3 = str[start:end]
03981 _v268.name.append(val3)
03982 start = end
03983 end += 4
03984 (length,) = _struct_I.unpack(str[start:end])
03985 pattern = '<%sd'%length
03986 start = end
03987 end += struct.calcsize(pattern)
03988 _v268.position = struct.unpack(pattern, str[start:end])
03989 start = end
03990 end += 4
03991 (length,) = _struct_I.unpack(str[start:end])
03992 pattern = '<%sd'%length
03993 start = end
03994 end += struct.calcsize(pattern)
03995 _v268.velocity = struct.unpack(pattern, str[start:end])
03996 start = end
03997 end += 4
03998 (length,) = _struct_I.unpack(str[start:end])
03999 pattern = '<%sd'%length
04000 start = end
04001 end += struct.calcsize(pattern)
04002 _v268.effort = struct.unpack(pattern, str[start:end])
04003 _v271 = val1.grasp_posture
04004 _v272 = _v271.header
04005 start = end
04006 end += 4
04007 (_v272.seq,) = _struct_I.unpack(str[start:end])
04008 _v273 = _v272.stamp
04009 _x = _v273
04010 start = end
04011 end += 8
04012 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04013 start = end
04014 end += 4
04015 (length,) = _struct_I.unpack(str[start:end])
04016 start = end
04017 end += length
04018 if python3:
04019 _v272.frame_id = str[start:end].decode('utf-8')
04020 else:
04021 _v272.frame_id = str[start:end]
04022 start = end
04023 end += 4
04024 (length,) = _struct_I.unpack(str[start:end])
04025 _v271.name = []
04026 for i in range(0, length):
04027 start = end
04028 end += 4
04029 (length,) = _struct_I.unpack(str[start:end])
04030 start = end
04031 end += length
04032 if python3:
04033 val3 = str[start:end].decode('utf-8')
04034 else:
04035 val3 = str[start:end]
04036 _v271.name.append(val3)
04037 start = end
04038 end += 4
04039 (length,) = _struct_I.unpack(str[start:end])
04040 pattern = '<%sd'%length
04041 start = end
04042 end += struct.calcsize(pattern)
04043 _v271.position = struct.unpack(pattern, str[start:end])
04044 start = end
04045 end += 4
04046 (length,) = _struct_I.unpack(str[start:end])
04047 pattern = '<%sd'%length
04048 start = end
04049 end += struct.calcsize(pattern)
04050 _v271.velocity = struct.unpack(pattern, str[start:end])
04051 start = end
04052 end += 4
04053 (length,) = _struct_I.unpack(str[start:end])
04054 pattern = '<%sd'%length
04055 start = end
04056 end += struct.calcsize(pattern)
04057 _v271.effort = struct.unpack(pattern, str[start:end])
04058 _v274 = val1.grasp_pose
04059 _v275 = _v274.position
04060 _x = _v275
04061 start = end
04062 end += 24
04063 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04064 _v276 = _v274.orientation
04065 _x = _v276
04066 start = end
04067 end += 32
04068 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04069 _x = val1
04070 start = end
04071 end += 17
04072 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
04073 val1.cluster_rep = bool(val1.cluster_rep)
04074 start = end
04075 end += 4
04076 (length,) = _struct_I.unpack(str[start:end])
04077 val1.moved_obstacles = []
04078 for i in range(0, length):
04079 val2 = object_manipulation_msgs.msg.GraspableObject()
04080 start = end
04081 end += 4
04082 (length,) = _struct_I.unpack(str[start:end])
04083 start = end
04084 end += length
04085 if python3:
04086 val2.reference_frame_id = str[start:end].decode('utf-8')
04087 else:
04088 val2.reference_frame_id = str[start:end]
04089 start = end
04090 end += 4
04091 (length,) = _struct_I.unpack(str[start:end])
04092 val2.potential_models = []
04093 for i in range(0, length):
04094 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
04095 start = end
04096 end += 4
04097 (val3.model_id,) = _struct_i.unpack(str[start:end])
04098 _v277 = val3.pose
04099 _v278 = _v277.header
04100 start = end
04101 end += 4
04102 (_v278.seq,) = _struct_I.unpack(str[start:end])
04103 _v279 = _v278.stamp
04104 _x = _v279
04105 start = end
04106 end += 8
04107 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04108 start = end
04109 end += 4
04110 (length,) = _struct_I.unpack(str[start:end])
04111 start = end
04112 end += length
04113 if python3:
04114 _v278.frame_id = str[start:end].decode('utf-8')
04115 else:
04116 _v278.frame_id = str[start:end]
04117 _v280 = _v277.pose
04118 _v281 = _v280.position
04119 _x = _v281
04120 start = end
04121 end += 24
04122 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04123 _v282 = _v280.orientation
04124 _x = _v282
04125 start = end
04126 end += 32
04127 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04128 start = end
04129 end += 4
04130 (val3.confidence,) = _struct_f.unpack(str[start:end])
04131 start = end
04132 end += 4
04133 (length,) = _struct_I.unpack(str[start:end])
04134 start = end
04135 end += length
04136 if python3:
04137 val3.detector_name = str[start:end].decode('utf-8')
04138 else:
04139 val3.detector_name = str[start:end]
04140 val2.potential_models.append(val3)
04141 _v283 = val2.cluster
04142 _v284 = _v283.header
04143 start = end
04144 end += 4
04145 (_v284.seq,) = _struct_I.unpack(str[start:end])
04146 _v285 = _v284.stamp
04147 _x = _v285
04148 start = end
04149 end += 8
04150 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04151 start = end
04152 end += 4
04153 (length,) = _struct_I.unpack(str[start:end])
04154 start = end
04155 end += length
04156 if python3:
04157 _v284.frame_id = str[start:end].decode('utf-8')
04158 else:
04159 _v284.frame_id = str[start:end]
04160 start = end
04161 end += 4
04162 (length,) = _struct_I.unpack(str[start:end])
04163 _v283.points = []
04164 for i in range(0, length):
04165 val4 = geometry_msgs.msg.Point32()
04166 _x = val4
04167 start = end
04168 end += 12
04169 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
04170 _v283.points.append(val4)
04171 start = end
04172 end += 4
04173 (length,) = _struct_I.unpack(str[start:end])
04174 _v283.channels = []
04175 for i in range(0, length):
04176 val4 = sensor_msgs.msg.ChannelFloat32()
04177 start = end
04178 end += 4
04179 (length,) = _struct_I.unpack(str[start:end])
04180 start = end
04181 end += length
04182 if python3:
04183 val4.name = str[start:end].decode('utf-8')
04184 else:
04185 val4.name = str[start:end]
04186 start = end
04187 end += 4
04188 (length,) = _struct_I.unpack(str[start:end])
04189 pattern = '<%sf'%length
04190 start = end
04191 end += struct.calcsize(pattern)
04192 val4.values = struct.unpack(pattern, str[start:end])
04193 _v283.channels.append(val4)
04194 _v286 = val2.region
04195 _v287 = _v286.cloud
04196 _v288 = _v287.header
04197 start = end
04198 end += 4
04199 (_v288.seq,) = _struct_I.unpack(str[start:end])
04200 _v289 = _v288.stamp
04201 _x = _v289
04202 start = end
04203 end += 8
04204 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04205 start = end
04206 end += 4
04207 (length,) = _struct_I.unpack(str[start:end])
04208 start = end
04209 end += length
04210 if python3:
04211 _v288.frame_id = str[start:end].decode('utf-8')
04212 else:
04213 _v288.frame_id = str[start:end]
04214 _x = _v287
04215 start = end
04216 end += 8
04217 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
04218 start = end
04219 end += 4
04220 (length,) = _struct_I.unpack(str[start:end])
04221 _v287.fields = []
04222 for i in range(0, length):
04223 val5 = sensor_msgs.msg.PointField()
04224 start = end
04225 end += 4
04226 (length,) = _struct_I.unpack(str[start:end])
04227 start = end
04228 end += length
04229 if python3:
04230 val5.name = str[start:end].decode('utf-8')
04231 else:
04232 val5.name = str[start:end]
04233 _x = val5
04234 start = end
04235 end += 9
04236 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
04237 _v287.fields.append(val5)
04238 _x = _v287
04239 start = end
04240 end += 9
04241 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
04242 _v287.is_bigendian = bool(_v287.is_bigendian)
04243 start = end
04244 end += 4
04245 (length,) = _struct_I.unpack(str[start:end])
04246 start = end
04247 end += length
04248 if python3:
04249 _v287.data = str[start:end].decode('utf-8')
04250 else:
04251 _v287.data = str[start:end]
04252 start = end
04253 end += 1
04254 (_v287.is_dense,) = _struct_B.unpack(str[start:end])
04255 _v287.is_dense = bool(_v287.is_dense)
04256 start = end
04257 end += 4
04258 (length,) = _struct_I.unpack(str[start:end])
04259 pattern = '<%si'%length
04260 start = end
04261 end += struct.calcsize(pattern)
04262 _v286.mask = struct.unpack(pattern, str[start:end])
04263 _v290 = _v286.image
04264 _v291 = _v290.header
04265 start = end
04266 end += 4
04267 (_v291.seq,) = _struct_I.unpack(str[start:end])
04268 _v292 = _v291.stamp
04269 _x = _v292
04270 start = end
04271 end += 8
04272 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04273 start = end
04274 end += 4
04275 (length,) = _struct_I.unpack(str[start:end])
04276 start = end
04277 end += length
04278 if python3:
04279 _v291.frame_id = str[start:end].decode('utf-8')
04280 else:
04281 _v291.frame_id = str[start:end]
04282 _x = _v290
04283 start = end
04284 end += 8
04285 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
04286 start = end
04287 end += 4
04288 (length,) = _struct_I.unpack(str[start:end])
04289 start = end
04290 end += length
04291 if python3:
04292 _v290.encoding = str[start:end].decode('utf-8')
04293 else:
04294 _v290.encoding = str[start:end]
04295 _x = _v290
04296 start = end
04297 end += 5
04298 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
04299 start = end
04300 end += 4
04301 (length,) = _struct_I.unpack(str[start:end])
04302 start = end
04303 end += length
04304 if python3:
04305 _v290.data = str[start:end].decode('utf-8')
04306 else:
04307 _v290.data = str[start:end]
04308 _v293 = _v286.disparity_image
04309 _v294 = _v293.header
04310 start = end
04311 end += 4
04312 (_v294.seq,) = _struct_I.unpack(str[start:end])
04313 _v295 = _v294.stamp
04314 _x = _v295
04315 start = end
04316 end += 8
04317 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04318 start = end
04319 end += 4
04320 (length,) = _struct_I.unpack(str[start:end])
04321 start = end
04322 end += length
04323 if python3:
04324 _v294.frame_id = str[start:end].decode('utf-8')
04325 else:
04326 _v294.frame_id = str[start:end]
04327 _x = _v293
04328 start = end
04329 end += 8
04330 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
04331 start = end
04332 end += 4
04333 (length,) = _struct_I.unpack(str[start:end])
04334 start = end
04335 end += length
04336 if python3:
04337 _v293.encoding = str[start:end].decode('utf-8')
04338 else:
04339 _v293.encoding = str[start:end]
04340 _x = _v293
04341 start = end
04342 end += 5
04343 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
04344 start = end
04345 end += 4
04346 (length,) = _struct_I.unpack(str[start:end])
04347 start = end
04348 end += length
04349 if python3:
04350 _v293.data = str[start:end].decode('utf-8')
04351 else:
04352 _v293.data = str[start:end]
04353 _v296 = _v286.cam_info
04354 _v297 = _v296.header
04355 start = end
04356 end += 4
04357 (_v297.seq,) = _struct_I.unpack(str[start:end])
04358 _v298 = _v297.stamp
04359 _x = _v298
04360 start = end
04361 end += 8
04362 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04363 start = end
04364 end += 4
04365 (length,) = _struct_I.unpack(str[start:end])
04366 start = end
04367 end += length
04368 if python3:
04369 _v297.frame_id = str[start:end].decode('utf-8')
04370 else:
04371 _v297.frame_id = str[start:end]
04372 _x = _v296
04373 start = end
04374 end += 8
04375 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
04376 start = end
04377 end += 4
04378 (length,) = _struct_I.unpack(str[start:end])
04379 start = end
04380 end += length
04381 if python3:
04382 _v296.distortion_model = str[start:end].decode('utf-8')
04383 else:
04384 _v296.distortion_model = str[start:end]
04385 start = end
04386 end += 4
04387 (length,) = _struct_I.unpack(str[start:end])
04388 pattern = '<%sd'%length
04389 start = end
04390 end += struct.calcsize(pattern)
04391 _v296.D = struct.unpack(pattern, str[start:end])
04392 start = end
04393 end += 72
04394 _v296.K = _struct_9d.unpack(str[start:end])
04395 start = end
04396 end += 72
04397 _v296.R = _struct_9d.unpack(str[start:end])
04398 start = end
04399 end += 96
04400 _v296.P = _struct_12d.unpack(str[start:end])
04401 _x = _v296
04402 start = end
04403 end += 8
04404 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
04405 _v299 = _v296.roi
04406 _x = _v299
04407 start = end
04408 end += 17
04409 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
04410 _v299.do_rectify = bool(_v299.do_rectify)
04411 _v300 = _v286.roi_box_pose
04412 _v301 = _v300.header
04413 start = end
04414 end += 4
04415 (_v301.seq,) = _struct_I.unpack(str[start:end])
04416 _v302 = _v301.stamp
04417 _x = _v302
04418 start = end
04419 end += 8
04420 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04421 start = end
04422 end += 4
04423 (length,) = _struct_I.unpack(str[start:end])
04424 start = end
04425 end += length
04426 if python3:
04427 _v301.frame_id = str[start:end].decode('utf-8')
04428 else:
04429 _v301.frame_id = str[start:end]
04430 _v303 = _v300.pose
04431 _v304 = _v303.position
04432 _x = _v304
04433 start = end
04434 end += 24
04435 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04436 _v305 = _v303.orientation
04437 _x = _v305
04438 start = end
04439 end += 32
04440 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04441 _v306 = _v286.roi_box_dims
04442 _x = _v306
04443 start = end
04444 end += 24
04445 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04446 start = end
04447 end += 4
04448 (length,) = _struct_I.unpack(str[start:end])
04449 start = end
04450 end += length
04451 if python3:
04452 val2.collision_name = str[start:end].decode('utf-8')
04453 else:
04454 val2.collision_name = str[start:end]
04455 val1.moved_obstacles.append(val2)
04456 self.action_feedback.feedback.grasps.append(val1)
04457 return self
04458 except struct.error as e:
04459 raise genpy.DeserializationError(e) #most likely buffer underfill
04460
04461
04462 def serialize_numpy(self, buff, numpy):
04463 """
04464 serialize message with numpy array types into buffer
04465 :param buff: buffer, ``StringIO``
04466 :param numpy: numpy python module
04467 """
04468 try:
04469 _x = self
04470 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
04471 _x = self.action_goal.header.frame_id
04472 length = len(_x)
04473 if python3 or type(_x) == unicode:
04474 _x = _x.encode('utf-8')
04475 length = len(_x)
04476 buff.write(struct.pack('<I%ss'%length, length, _x))
04477 _x = self
04478 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
04479 _x = self.action_goal.goal_id.id
04480 length = len(_x)
04481 if python3 or type(_x) == unicode:
04482 _x = _x.encode('utf-8')
04483 length = len(_x)
04484 buff.write(struct.pack('<I%ss'%length, length, _x))
04485 _x = self.action_goal.goal.arm_name
04486 length = len(_x)
04487 if python3 or type(_x) == unicode:
04488 _x = _x.encode('utf-8')
04489 length = len(_x)
04490 buff.write(struct.pack('<I%ss'%length, length, _x))
04491 _x = self.action_goal.goal.target.reference_frame_id
04492 length = len(_x)
04493 if python3 or type(_x) == unicode:
04494 _x = _x.encode('utf-8')
04495 length = len(_x)
04496 buff.write(struct.pack('<I%ss'%length, length, _x))
04497 length = len(self.action_goal.goal.target.potential_models)
04498 buff.write(_struct_I.pack(length))
04499 for val1 in self.action_goal.goal.target.potential_models:
04500 buff.write(_struct_i.pack(val1.model_id))
04501 _v307 = val1.pose
04502 _v308 = _v307.header
04503 buff.write(_struct_I.pack(_v308.seq))
04504 _v309 = _v308.stamp
04505 _x = _v309
04506 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04507 _x = _v308.frame_id
04508 length = len(_x)
04509 if python3 or type(_x) == unicode:
04510 _x = _x.encode('utf-8')
04511 length = len(_x)
04512 buff.write(struct.pack('<I%ss'%length, length, _x))
04513 _v310 = _v307.pose
04514 _v311 = _v310.position
04515 _x = _v311
04516 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04517 _v312 = _v310.orientation
04518 _x = _v312
04519 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
04520 buff.write(_struct_f.pack(val1.confidence))
04521 _x = val1.detector_name
04522 length = len(_x)
04523 if python3 or type(_x) == unicode:
04524 _x = _x.encode('utf-8')
04525 length = len(_x)
04526 buff.write(struct.pack('<I%ss'%length, length, _x))
04527 _x = self
04528 buff.write(_struct_3I.pack(_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs))
04529 _x = self.action_goal.goal.target.cluster.header.frame_id
04530 length = len(_x)
04531 if python3 or type(_x) == unicode:
04532 _x = _x.encode('utf-8')
04533 length = len(_x)
04534 buff.write(struct.pack('<I%ss'%length, length, _x))
04535 length = len(self.action_goal.goal.target.cluster.points)
04536 buff.write(_struct_I.pack(length))
04537 for val1 in self.action_goal.goal.target.cluster.points:
04538 _x = val1
04539 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
04540 length = len(self.action_goal.goal.target.cluster.channels)
04541 buff.write(_struct_I.pack(length))
04542 for val1 in self.action_goal.goal.target.cluster.channels:
04543 _x = val1.name
04544 length = len(_x)
04545 if python3 or type(_x) == unicode:
04546 _x = _x.encode('utf-8')
04547 length = len(_x)
04548 buff.write(struct.pack('<I%ss'%length, length, _x))
04549 length = len(val1.values)
04550 buff.write(_struct_I.pack(length))
04551 pattern = '<%sf'%length
04552 buff.write(val1.values.tostring())
04553 _x = self
04554 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs))
04555 _x = self.action_goal.goal.target.region.cloud.header.frame_id
04556 length = len(_x)
04557 if python3 or type(_x) == unicode:
04558 _x = _x.encode('utf-8')
04559 length = len(_x)
04560 buff.write(struct.pack('<I%ss'%length, length, _x))
04561 _x = self
04562 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width))
04563 length = len(self.action_goal.goal.target.region.cloud.fields)
04564 buff.write(_struct_I.pack(length))
04565 for val1 in self.action_goal.goal.target.region.cloud.fields:
04566 _x = val1.name
04567 length = len(_x)
04568 if python3 or type(_x) == unicode:
04569 _x = _x.encode('utf-8')
04570 length = len(_x)
04571 buff.write(struct.pack('<I%ss'%length, length, _x))
04572 _x = val1
04573 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
04574 _x = self
04575 buff.write(_struct_B2I.pack(_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step))
04576 _x = self.action_goal.goal.target.region.cloud.data
04577 length = len(_x)
04578 # - if encoded as a list instead, serialize as bytes instead of string
04579 if type(_x) in [list, tuple]:
04580 buff.write(struct.pack('<I%sB'%length, length, *_x))
04581 else:
04582 buff.write(struct.pack('<I%ss'%length, length, _x))
04583 buff.write(_struct_B.pack(self.action_goal.goal.target.region.cloud.is_dense))
04584 length = len(self.action_goal.goal.target.region.mask)
04585 buff.write(_struct_I.pack(length))
04586 pattern = '<%si'%length
04587 buff.write(self.action_goal.goal.target.region.mask.tostring())
04588 _x = self
04589 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs))
04590 _x = self.action_goal.goal.target.region.image.header.frame_id
04591 length = len(_x)
04592 if python3 or type(_x) == unicode:
04593 _x = _x.encode('utf-8')
04594 length = len(_x)
04595 buff.write(struct.pack('<I%ss'%length, length, _x))
04596 _x = self
04597 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width))
04598 _x = self.action_goal.goal.target.region.image.encoding
04599 length = len(_x)
04600 if python3 or type(_x) == unicode:
04601 _x = _x.encode('utf-8')
04602 length = len(_x)
04603 buff.write(struct.pack('<I%ss'%length, length, _x))
04604 _x = self
04605 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step))
04606 _x = self.action_goal.goal.target.region.image.data
04607 length = len(_x)
04608 # - if encoded as a list instead, serialize as bytes instead of string
04609 if type(_x) in [list, tuple]:
04610 buff.write(struct.pack('<I%sB'%length, length, *_x))
04611 else:
04612 buff.write(struct.pack('<I%ss'%length, length, _x))
04613 _x = self
04614 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs))
04615 _x = self.action_goal.goal.target.region.disparity_image.header.frame_id
04616 length = len(_x)
04617 if python3 or type(_x) == unicode:
04618 _x = _x.encode('utf-8')
04619 length = len(_x)
04620 buff.write(struct.pack('<I%ss'%length, length, _x))
04621 _x = self
04622 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width))
04623 _x = self.action_goal.goal.target.region.disparity_image.encoding
04624 length = len(_x)
04625 if python3 or type(_x) == unicode:
04626 _x = _x.encode('utf-8')
04627 length = len(_x)
04628 buff.write(struct.pack('<I%ss'%length, length, _x))
04629 _x = self
04630 buff.write(_struct_BI.pack(_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step))
04631 _x = self.action_goal.goal.target.region.disparity_image.data
04632 length = len(_x)
04633 # - if encoded as a list instead, serialize as bytes instead of string
04634 if type(_x) in [list, tuple]:
04635 buff.write(struct.pack('<I%sB'%length, length, *_x))
04636 else:
04637 buff.write(struct.pack('<I%ss'%length, length, _x))
04638 _x = self
04639 buff.write(_struct_3I.pack(_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs))
04640 _x = self.action_goal.goal.target.region.cam_info.header.frame_id
04641 length = len(_x)
04642 if python3 or type(_x) == unicode:
04643 _x = _x.encode('utf-8')
04644 length = len(_x)
04645 buff.write(struct.pack('<I%ss'%length, length, _x))
04646 _x = self
04647 buff.write(_struct_2I.pack(_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width))
04648 _x = self.action_goal.goal.target.region.cam_info.distortion_model
04649 length = len(_x)
04650 if python3 or type(_x) == unicode:
04651 _x = _x.encode('utf-8')
04652 length = len(_x)
04653 buff.write(struct.pack('<I%ss'%length, length, _x))
04654 length = len(self.action_goal.goal.target.region.cam_info.D)
04655 buff.write(_struct_I.pack(length))
04656 pattern = '<%sd'%length
04657 buff.write(self.action_goal.goal.target.region.cam_info.D.tostring())
04658 buff.write(self.action_goal.goal.target.region.cam_info.K.tostring())
04659 buff.write(self.action_goal.goal.target.region.cam_info.R.tostring())
04660 buff.write(self.action_goal.goal.target.region.cam_info.P.tostring())
04661 _x = self
04662 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs))
04663 _x = self.action_goal.goal.target.region.roi_box_pose.header.frame_id
04664 length = len(_x)
04665 if python3 or type(_x) == unicode:
04666 _x = _x.encode('utf-8')
04667 length = len(_x)
04668 buff.write(struct.pack('<I%ss'%length, length, _x))
04669 _x = self
04670 buff.write(_struct_10d.pack(_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z))
04671 _x = self.action_goal.goal.target.collision_name
04672 length = len(_x)
04673 if python3 or type(_x) == unicode:
04674 _x = _x.encode('utf-8')
04675 length = len(_x)
04676 buff.write(struct.pack('<I%ss'%length, length, _x))
04677 _x = self.action_goal.goal.collision_object_name
04678 length = len(_x)
04679 if python3 or type(_x) == unicode:
04680 _x = _x.encode('utf-8')
04681 length = len(_x)
04682 buff.write(struct.pack('<I%ss'%length, length, _x))
04683 _x = self.action_goal.goal.collision_support_surface_name
04684 length = len(_x)
04685 if python3 or type(_x) == unicode:
04686 _x = _x.encode('utf-8')
04687 length = len(_x)
04688 buff.write(struct.pack('<I%ss'%length, length, _x))
04689 length = len(self.action_goal.goal.grasps_to_evaluate)
04690 buff.write(_struct_I.pack(length))
04691 for val1 in self.action_goal.goal.grasps_to_evaluate:
04692 _v313 = val1.pre_grasp_posture
04693 _v314 = _v313.header
04694 buff.write(_struct_I.pack(_v314.seq))
04695 _v315 = _v314.stamp
04696 _x = _v315
04697 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04698 _x = _v314.frame_id
04699 length = len(_x)
04700 if python3 or type(_x) == unicode:
04701 _x = _x.encode('utf-8')
04702 length = len(_x)
04703 buff.write(struct.pack('<I%ss'%length, length, _x))
04704 length = len(_v313.name)
04705 buff.write(_struct_I.pack(length))
04706 for val3 in _v313.name:
04707 length = len(val3)
04708 if python3 or type(val3) == unicode:
04709 val3 = val3.encode('utf-8')
04710 length = len(val3)
04711 buff.write(struct.pack('<I%ss'%length, length, val3))
04712 length = len(_v313.position)
04713 buff.write(_struct_I.pack(length))
04714 pattern = '<%sd'%length
04715 buff.write(_v313.position.tostring())
04716 length = len(_v313.velocity)
04717 buff.write(_struct_I.pack(length))
04718 pattern = '<%sd'%length
04719 buff.write(_v313.velocity.tostring())
04720 length = len(_v313.effort)
04721 buff.write(_struct_I.pack(length))
04722 pattern = '<%sd'%length
04723 buff.write(_v313.effort.tostring())
04724 _v316 = val1.grasp_posture
04725 _v317 = _v316.header
04726 buff.write(_struct_I.pack(_v317.seq))
04727 _v318 = _v317.stamp
04728 _x = _v318
04729 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04730 _x = _v317.frame_id
04731 length = len(_x)
04732 if python3 or type(_x) == unicode:
04733 _x = _x.encode('utf-8')
04734 length = len(_x)
04735 buff.write(struct.pack('<I%ss'%length, length, _x))
04736 length = len(_v316.name)
04737 buff.write(_struct_I.pack(length))
04738 for val3 in _v316.name:
04739 length = len(val3)
04740 if python3 or type(val3) == unicode:
04741 val3 = val3.encode('utf-8')
04742 length = len(val3)
04743 buff.write(struct.pack('<I%ss'%length, length, val3))
04744 length = len(_v316.position)
04745 buff.write(_struct_I.pack(length))
04746 pattern = '<%sd'%length
04747 buff.write(_v316.position.tostring())
04748 length = len(_v316.velocity)
04749 buff.write(_struct_I.pack(length))
04750 pattern = '<%sd'%length
04751 buff.write(_v316.velocity.tostring())
04752 length = len(_v316.effort)
04753 buff.write(_struct_I.pack(length))
04754 pattern = '<%sd'%length
04755 buff.write(_v316.effort.tostring())
04756 _v319 = val1.grasp_pose
04757 _v320 = _v319.position
04758 _x = _v320
04759 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04760 _v321 = _v319.orientation
04761 _x = _v321
04762 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
04763 _x = val1
04764 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
04765 length = len(val1.moved_obstacles)
04766 buff.write(_struct_I.pack(length))
04767 for val2 in val1.moved_obstacles:
04768 _x = val2.reference_frame_id
04769 length = len(_x)
04770 if python3 or type(_x) == unicode:
04771 _x = _x.encode('utf-8')
04772 length = len(_x)
04773 buff.write(struct.pack('<I%ss'%length, length, _x))
04774 length = len(val2.potential_models)
04775 buff.write(_struct_I.pack(length))
04776 for val3 in val2.potential_models:
04777 buff.write(_struct_i.pack(val3.model_id))
04778 _v322 = val3.pose
04779 _v323 = _v322.header
04780 buff.write(_struct_I.pack(_v323.seq))
04781 _v324 = _v323.stamp
04782 _x = _v324
04783 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04784 _x = _v323.frame_id
04785 length = len(_x)
04786 if python3 or type(_x) == unicode:
04787 _x = _x.encode('utf-8')
04788 length = len(_x)
04789 buff.write(struct.pack('<I%ss'%length, length, _x))
04790 _v325 = _v322.pose
04791 _v326 = _v325.position
04792 _x = _v326
04793 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04794 _v327 = _v325.orientation
04795 _x = _v327
04796 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
04797 buff.write(_struct_f.pack(val3.confidence))
04798 _x = val3.detector_name
04799 length = len(_x)
04800 if python3 or type(_x) == unicode:
04801 _x = _x.encode('utf-8')
04802 length = len(_x)
04803 buff.write(struct.pack('<I%ss'%length, length, _x))
04804 _v328 = val2.cluster
04805 _v329 = _v328.header
04806 buff.write(_struct_I.pack(_v329.seq))
04807 _v330 = _v329.stamp
04808 _x = _v330
04809 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04810 _x = _v329.frame_id
04811 length = len(_x)
04812 if python3 or type(_x) == unicode:
04813 _x = _x.encode('utf-8')
04814 length = len(_x)
04815 buff.write(struct.pack('<I%ss'%length, length, _x))
04816 length = len(_v328.points)
04817 buff.write(_struct_I.pack(length))
04818 for val4 in _v328.points:
04819 _x = val4
04820 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
04821 length = len(_v328.channels)
04822 buff.write(_struct_I.pack(length))
04823 for val4 in _v328.channels:
04824 _x = val4.name
04825 length = len(_x)
04826 if python3 or type(_x) == unicode:
04827 _x = _x.encode('utf-8')
04828 length = len(_x)
04829 buff.write(struct.pack('<I%ss'%length, length, _x))
04830 length = len(val4.values)
04831 buff.write(_struct_I.pack(length))
04832 pattern = '<%sf'%length
04833 buff.write(val4.values.tostring())
04834 _v331 = val2.region
04835 _v332 = _v331.cloud
04836 _v333 = _v332.header
04837 buff.write(_struct_I.pack(_v333.seq))
04838 _v334 = _v333.stamp
04839 _x = _v334
04840 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04841 _x = _v333.frame_id
04842 length = len(_x)
04843 if python3 or type(_x) == unicode:
04844 _x = _x.encode('utf-8')
04845 length = len(_x)
04846 buff.write(struct.pack('<I%ss'%length, length, _x))
04847 _x = _v332
04848 buff.write(_struct_2I.pack(_x.height, _x.width))
04849 length = len(_v332.fields)
04850 buff.write(_struct_I.pack(length))
04851 for val5 in _v332.fields:
04852 _x = val5.name
04853 length = len(_x)
04854 if python3 or type(_x) == unicode:
04855 _x = _x.encode('utf-8')
04856 length = len(_x)
04857 buff.write(struct.pack('<I%ss'%length, length, _x))
04858 _x = val5
04859 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
04860 _x = _v332
04861 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
04862 _x = _v332.data
04863 length = len(_x)
04864 # - if encoded as a list instead, serialize as bytes instead of string
04865 if type(_x) in [list, tuple]:
04866 buff.write(struct.pack('<I%sB'%length, length, *_x))
04867 else:
04868 buff.write(struct.pack('<I%ss'%length, length, _x))
04869 buff.write(_struct_B.pack(_v332.is_dense))
04870 length = len(_v331.mask)
04871 buff.write(_struct_I.pack(length))
04872 pattern = '<%si'%length
04873 buff.write(_v331.mask.tostring())
04874 _v335 = _v331.image
04875 _v336 = _v335.header
04876 buff.write(_struct_I.pack(_v336.seq))
04877 _v337 = _v336.stamp
04878 _x = _v337
04879 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04880 _x = _v336.frame_id
04881 length = len(_x)
04882 if python3 or type(_x) == unicode:
04883 _x = _x.encode('utf-8')
04884 length = len(_x)
04885 buff.write(struct.pack('<I%ss'%length, length, _x))
04886 _x = _v335
04887 buff.write(_struct_2I.pack(_x.height, _x.width))
04888 _x = _v335.encoding
04889 length = len(_x)
04890 if python3 or type(_x) == unicode:
04891 _x = _x.encode('utf-8')
04892 length = len(_x)
04893 buff.write(struct.pack('<I%ss'%length, length, _x))
04894 _x = _v335
04895 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
04896 _x = _v335.data
04897 length = len(_x)
04898 # - if encoded as a list instead, serialize as bytes instead of string
04899 if type(_x) in [list, tuple]:
04900 buff.write(struct.pack('<I%sB'%length, length, *_x))
04901 else:
04902 buff.write(struct.pack('<I%ss'%length, length, _x))
04903 _v338 = _v331.disparity_image
04904 _v339 = _v338.header
04905 buff.write(_struct_I.pack(_v339.seq))
04906 _v340 = _v339.stamp
04907 _x = _v340
04908 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04909 _x = _v339.frame_id
04910 length = len(_x)
04911 if python3 or type(_x) == unicode:
04912 _x = _x.encode('utf-8')
04913 length = len(_x)
04914 buff.write(struct.pack('<I%ss'%length, length, _x))
04915 _x = _v338
04916 buff.write(_struct_2I.pack(_x.height, _x.width))
04917 _x = _v338.encoding
04918 length = len(_x)
04919 if python3 or type(_x) == unicode:
04920 _x = _x.encode('utf-8')
04921 length = len(_x)
04922 buff.write(struct.pack('<I%ss'%length, length, _x))
04923 _x = _v338
04924 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
04925 _x = _v338.data
04926 length = len(_x)
04927 # - if encoded as a list instead, serialize as bytes instead of string
04928 if type(_x) in [list, tuple]:
04929 buff.write(struct.pack('<I%sB'%length, length, *_x))
04930 else:
04931 buff.write(struct.pack('<I%ss'%length, length, _x))
04932 _v341 = _v331.cam_info
04933 _v342 = _v341.header
04934 buff.write(_struct_I.pack(_v342.seq))
04935 _v343 = _v342.stamp
04936 _x = _v343
04937 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04938 _x = _v342.frame_id
04939 length = len(_x)
04940 if python3 or type(_x) == unicode:
04941 _x = _x.encode('utf-8')
04942 length = len(_x)
04943 buff.write(struct.pack('<I%ss'%length, length, _x))
04944 _x = _v341
04945 buff.write(_struct_2I.pack(_x.height, _x.width))
04946 _x = _v341.distortion_model
04947 length = len(_x)
04948 if python3 or type(_x) == unicode:
04949 _x = _x.encode('utf-8')
04950 length = len(_x)
04951 buff.write(struct.pack('<I%ss'%length, length, _x))
04952 length = len(_v341.D)
04953 buff.write(_struct_I.pack(length))
04954 pattern = '<%sd'%length
04955 buff.write(_v341.D.tostring())
04956 buff.write(_v341.K.tostring())
04957 buff.write(_v341.R.tostring())
04958 buff.write(_v341.P.tostring())
04959 _x = _v341
04960 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
04961 _v344 = _v341.roi
04962 _x = _v344
04963 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
04964 _v345 = _v331.roi_box_pose
04965 _v346 = _v345.header
04966 buff.write(_struct_I.pack(_v346.seq))
04967 _v347 = _v346.stamp
04968 _x = _v347
04969 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04970 _x = _v346.frame_id
04971 length = len(_x)
04972 if python3 or type(_x) == unicode:
04973 _x = _x.encode('utf-8')
04974 length = len(_x)
04975 buff.write(struct.pack('<I%ss'%length, length, _x))
04976 _v348 = _v345.pose
04977 _v349 = _v348.position
04978 _x = _v349
04979 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04980 _v350 = _v348.orientation
04981 _x = _v350
04982 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
04983 _v351 = _v331.roi_box_dims
04984 _x = _v351
04985 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04986 _x = val2.collision_name
04987 length = len(_x)
04988 if python3 or type(_x) == unicode:
04989 _x = _x.encode('utf-8')
04990 length = len(_x)
04991 buff.write(struct.pack('<I%ss'%length, length, _x))
04992 length = len(self.action_goal.goal.movable_obstacles)
04993 buff.write(_struct_I.pack(length))
04994 for val1 in self.action_goal.goal.movable_obstacles:
04995 _x = val1.reference_frame_id
04996 length = len(_x)
04997 if python3 or type(_x) == unicode:
04998 _x = _x.encode('utf-8')
04999 length = len(_x)
05000 buff.write(struct.pack('<I%ss'%length, length, _x))
05001 length = len(val1.potential_models)
05002 buff.write(_struct_I.pack(length))
05003 for val2 in val1.potential_models:
05004 buff.write(_struct_i.pack(val2.model_id))
05005 _v352 = val2.pose
05006 _v353 = _v352.header
05007 buff.write(_struct_I.pack(_v353.seq))
05008 _v354 = _v353.stamp
05009 _x = _v354
05010 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05011 _x = _v353.frame_id
05012 length = len(_x)
05013 if python3 or type(_x) == unicode:
05014 _x = _x.encode('utf-8')
05015 length = len(_x)
05016 buff.write(struct.pack('<I%ss'%length, length, _x))
05017 _v355 = _v352.pose
05018 _v356 = _v355.position
05019 _x = _v356
05020 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05021 _v357 = _v355.orientation
05022 _x = _v357
05023 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05024 buff.write(_struct_f.pack(val2.confidence))
05025 _x = val2.detector_name
05026 length = len(_x)
05027 if python3 or type(_x) == unicode:
05028 _x = _x.encode('utf-8')
05029 length = len(_x)
05030 buff.write(struct.pack('<I%ss'%length, length, _x))
05031 _v358 = val1.cluster
05032 _v359 = _v358.header
05033 buff.write(_struct_I.pack(_v359.seq))
05034 _v360 = _v359.stamp
05035 _x = _v360
05036 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05037 _x = _v359.frame_id
05038 length = len(_x)
05039 if python3 or type(_x) == unicode:
05040 _x = _x.encode('utf-8')
05041 length = len(_x)
05042 buff.write(struct.pack('<I%ss'%length, length, _x))
05043 length = len(_v358.points)
05044 buff.write(_struct_I.pack(length))
05045 for val3 in _v358.points:
05046 _x = val3
05047 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
05048 length = len(_v358.channels)
05049 buff.write(_struct_I.pack(length))
05050 for val3 in _v358.channels:
05051 _x = val3.name
05052 length = len(_x)
05053 if python3 or type(_x) == unicode:
05054 _x = _x.encode('utf-8')
05055 length = len(_x)
05056 buff.write(struct.pack('<I%ss'%length, length, _x))
05057 length = len(val3.values)
05058 buff.write(_struct_I.pack(length))
05059 pattern = '<%sf'%length
05060 buff.write(val3.values.tostring())
05061 _v361 = val1.region
05062 _v362 = _v361.cloud
05063 _v363 = _v362.header
05064 buff.write(_struct_I.pack(_v363.seq))
05065 _v364 = _v363.stamp
05066 _x = _v364
05067 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05068 _x = _v363.frame_id
05069 length = len(_x)
05070 if python3 or type(_x) == unicode:
05071 _x = _x.encode('utf-8')
05072 length = len(_x)
05073 buff.write(struct.pack('<I%ss'%length, length, _x))
05074 _x = _v362
05075 buff.write(_struct_2I.pack(_x.height, _x.width))
05076 length = len(_v362.fields)
05077 buff.write(_struct_I.pack(length))
05078 for val4 in _v362.fields:
05079 _x = val4.name
05080 length = len(_x)
05081 if python3 or type(_x) == unicode:
05082 _x = _x.encode('utf-8')
05083 length = len(_x)
05084 buff.write(struct.pack('<I%ss'%length, length, _x))
05085 _x = val4
05086 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
05087 _x = _v362
05088 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
05089 _x = _v362.data
05090 length = len(_x)
05091 # - if encoded as a list instead, serialize as bytes instead of string
05092 if type(_x) in [list, tuple]:
05093 buff.write(struct.pack('<I%sB'%length, length, *_x))
05094 else:
05095 buff.write(struct.pack('<I%ss'%length, length, _x))
05096 buff.write(_struct_B.pack(_v362.is_dense))
05097 length = len(_v361.mask)
05098 buff.write(_struct_I.pack(length))
05099 pattern = '<%si'%length
05100 buff.write(_v361.mask.tostring())
05101 _v365 = _v361.image
05102 _v366 = _v365.header
05103 buff.write(_struct_I.pack(_v366.seq))
05104 _v367 = _v366.stamp
05105 _x = _v367
05106 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05107 _x = _v366.frame_id
05108 length = len(_x)
05109 if python3 or type(_x) == unicode:
05110 _x = _x.encode('utf-8')
05111 length = len(_x)
05112 buff.write(struct.pack('<I%ss'%length, length, _x))
05113 _x = _v365
05114 buff.write(_struct_2I.pack(_x.height, _x.width))
05115 _x = _v365.encoding
05116 length = len(_x)
05117 if python3 or type(_x) == unicode:
05118 _x = _x.encode('utf-8')
05119 length = len(_x)
05120 buff.write(struct.pack('<I%ss'%length, length, _x))
05121 _x = _v365
05122 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
05123 _x = _v365.data
05124 length = len(_x)
05125 # - if encoded as a list instead, serialize as bytes instead of string
05126 if type(_x) in [list, tuple]:
05127 buff.write(struct.pack('<I%sB'%length, length, *_x))
05128 else:
05129 buff.write(struct.pack('<I%ss'%length, length, _x))
05130 _v368 = _v361.disparity_image
05131 _v369 = _v368.header
05132 buff.write(_struct_I.pack(_v369.seq))
05133 _v370 = _v369.stamp
05134 _x = _v370
05135 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05136 _x = _v369.frame_id
05137 length = len(_x)
05138 if python3 or type(_x) == unicode:
05139 _x = _x.encode('utf-8')
05140 length = len(_x)
05141 buff.write(struct.pack('<I%ss'%length, length, _x))
05142 _x = _v368
05143 buff.write(_struct_2I.pack(_x.height, _x.width))
05144 _x = _v368.encoding
05145 length = len(_x)
05146 if python3 or type(_x) == unicode:
05147 _x = _x.encode('utf-8')
05148 length = len(_x)
05149 buff.write(struct.pack('<I%ss'%length, length, _x))
05150 _x = _v368
05151 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
05152 _x = _v368.data
05153 length = len(_x)
05154 # - if encoded as a list instead, serialize as bytes instead of string
05155 if type(_x) in [list, tuple]:
05156 buff.write(struct.pack('<I%sB'%length, length, *_x))
05157 else:
05158 buff.write(struct.pack('<I%ss'%length, length, _x))
05159 _v371 = _v361.cam_info
05160 _v372 = _v371.header
05161 buff.write(_struct_I.pack(_v372.seq))
05162 _v373 = _v372.stamp
05163 _x = _v373
05164 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05165 _x = _v372.frame_id
05166 length = len(_x)
05167 if python3 or type(_x) == unicode:
05168 _x = _x.encode('utf-8')
05169 length = len(_x)
05170 buff.write(struct.pack('<I%ss'%length, length, _x))
05171 _x = _v371
05172 buff.write(_struct_2I.pack(_x.height, _x.width))
05173 _x = _v371.distortion_model
05174 length = len(_x)
05175 if python3 or type(_x) == unicode:
05176 _x = _x.encode('utf-8')
05177 length = len(_x)
05178 buff.write(struct.pack('<I%ss'%length, length, _x))
05179 length = len(_v371.D)
05180 buff.write(_struct_I.pack(length))
05181 pattern = '<%sd'%length
05182 buff.write(_v371.D.tostring())
05183 buff.write(_v371.K.tostring())
05184 buff.write(_v371.R.tostring())
05185 buff.write(_v371.P.tostring())
05186 _x = _v371
05187 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
05188 _v374 = _v371.roi
05189 _x = _v374
05190 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
05191 _v375 = _v361.roi_box_pose
05192 _v376 = _v375.header
05193 buff.write(_struct_I.pack(_v376.seq))
05194 _v377 = _v376.stamp
05195 _x = _v377
05196 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05197 _x = _v376.frame_id
05198 length = len(_x)
05199 if python3 or type(_x) == unicode:
05200 _x = _x.encode('utf-8')
05201 length = len(_x)
05202 buff.write(struct.pack('<I%ss'%length, length, _x))
05203 _v378 = _v375.pose
05204 _v379 = _v378.position
05205 _x = _v379
05206 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05207 _v380 = _v378.orientation
05208 _x = _v380
05209 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05210 _v381 = _v361.roi_box_dims
05211 _x = _v381
05212 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05213 _x = val1.collision_name
05214 length = len(_x)
05215 if python3 or type(_x) == unicode:
05216 _x = _x.encode('utf-8')
05217 length = len(_x)
05218 buff.write(struct.pack('<I%ss'%length, length, _x))
05219 _x = self
05220 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
05221 _x = self.action_result.header.frame_id
05222 length = len(_x)
05223 if python3 or type(_x) == unicode:
05224 _x = _x.encode('utf-8')
05225 length = len(_x)
05226 buff.write(struct.pack('<I%ss'%length, length, _x))
05227 _x = self
05228 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
05229 _x = self.action_result.status.goal_id.id
05230 length = len(_x)
05231 if python3 or type(_x) == unicode:
05232 _x = _x.encode('utf-8')
05233 length = len(_x)
05234 buff.write(struct.pack('<I%ss'%length, length, _x))
05235 buff.write(_struct_B.pack(self.action_result.status.status))
05236 _x = self.action_result.status.text
05237 length = len(_x)
05238 if python3 or type(_x) == unicode:
05239 _x = _x.encode('utf-8')
05240 length = len(_x)
05241 buff.write(struct.pack('<I%ss'%length, length, _x))
05242 length = len(self.action_result.result.grasps)
05243 buff.write(_struct_I.pack(length))
05244 for val1 in self.action_result.result.grasps:
05245 _v382 = val1.pre_grasp_posture
05246 _v383 = _v382.header
05247 buff.write(_struct_I.pack(_v383.seq))
05248 _v384 = _v383.stamp
05249 _x = _v384
05250 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05251 _x = _v383.frame_id
05252 length = len(_x)
05253 if python3 or type(_x) == unicode:
05254 _x = _x.encode('utf-8')
05255 length = len(_x)
05256 buff.write(struct.pack('<I%ss'%length, length, _x))
05257 length = len(_v382.name)
05258 buff.write(_struct_I.pack(length))
05259 for val3 in _v382.name:
05260 length = len(val3)
05261 if python3 or type(val3) == unicode:
05262 val3 = val3.encode('utf-8')
05263 length = len(val3)
05264 buff.write(struct.pack('<I%ss'%length, length, val3))
05265 length = len(_v382.position)
05266 buff.write(_struct_I.pack(length))
05267 pattern = '<%sd'%length
05268 buff.write(_v382.position.tostring())
05269 length = len(_v382.velocity)
05270 buff.write(_struct_I.pack(length))
05271 pattern = '<%sd'%length
05272 buff.write(_v382.velocity.tostring())
05273 length = len(_v382.effort)
05274 buff.write(_struct_I.pack(length))
05275 pattern = '<%sd'%length
05276 buff.write(_v382.effort.tostring())
05277 _v385 = val1.grasp_posture
05278 _v386 = _v385.header
05279 buff.write(_struct_I.pack(_v386.seq))
05280 _v387 = _v386.stamp
05281 _x = _v387
05282 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05283 _x = _v386.frame_id
05284 length = len(_x)
05285 if python3 or type(_x) == unicode:
05286 _x = _x.encode('utf-8')
05287 length = len(_x)
05288 buff.write(struct.pack('<I%ss'%length, length, _x))
05289 length = len(_v385.name)
05290 buff.write(_struct_I.pack(length))
05291 for val3 in _v385.name:
05292 length = len(val3)
05293 if python3 or type(val3) == unicode:
05294 val3 = val3.encode('utf-8')
05295 length = len(val3)
05296 buff.write(struct.pack('<I%ss'%length, length, val3))
05297 length = len(_v385.position)
05298 buff.write(_struct_I.pack(length))
05299 pattern = '<%sd'%length
05300 buff.write(_v385.position.tostring())
05301 length = len(_v385.velocity)
05302 buff.write(_struct_I.pack(length))
05303 pattern = '<%sd'%length
05304 buff.write(_v385.velocity.tostring())
05305 length = len(_v385.effort)
05306 buff.write(_struct_I.pack(length))
05307 pattern = '<%sd'%length
05308 buff.write(_v385.effort.tostring())
05309 _v388 = val1.grasp_pose
05310 _v389 = _v388.position
05311 _x = _v389
05312 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05313 _v390 = _v388.orientation
05314 _x = _v390
05315 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05316 _x = val1
05317 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
05318 length = len(val1.moved_obstacles)
05319 buff.write(_struct_I.pack(length))
05320 for val2 in val1.moved_obstacles:
05321 _x = val2.reference_frame_id
05322 length = len(_x)
05323 if python3 or type(_x) == unicode:
05324 _x = _x.encode('utf-8')
05325 length = len(_x)
05326 buff.write(struct.pack('<I%ss'%length, length, _x))
05327 length = len(val2.potential_models)
05328 buff.write(_struct_I.pack(length))
05329 for val3 in val2.potential_models:
05330 buff.write(_struct_i.pack(val3.model_id))
05331 _v391 = val3.pose
05332 _v392 = _v391.header
05333 buff.write(_struct_I.pack(_v392.seq))
05334 _v393 = _v392.stamp
05335 _x = _v393
05336 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05337 _x = _v392.frame_id
05338 length = len(_x)
05339 if python3 or type(_x) == unicode:
05340 _x = _x.encode('utf-8')
05341 length = len(_x)
05342 buff.write(struct.pack('<I%ss'%length, length, _x))
05343 _v394 = _v391.pose
05344 _v395 = _v394.position
05345 _x = _v395
05346 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05347 _v396 = _v394.orientation
05348 _x = _v396
05349 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05350 buff.write(_struct_f.pack(val3.confidence))
05351 _x = val3.detector_name
05352 length = len(_x)
05353 if python3 or type(_x) == unicode:
05354 _x = _x.encode('utf-8')
05355 length = len(_x)
05356 buff.write(struct.pack('<I%ss'%length, length, _x))
05357 _v397 = val2.cluster
05358 _v398 = _v397.header
05359 buff.write(_struct_I.pack(_v398.seq))
05360 _v399 = _v398.stamp
05361 _x = _v399
05362 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05363 _x = _v398.frame_id
05364 length = len(_x)
05365 if python3 or type(_x) == unicode:
05366 _x = _x.encode('utf-8')
05367 length = len(_x)
05368 buff.write(struct.pack('<I%ss'%length, length, _x))
05369 length = len(_v397.points)
05370 buff.write(_struct_I.pack(length))
05371 for val4 in _v397.points:
05372 _x = val4
05373 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
05374 length = len(_v397.channels)
05375 buff.write(_struct_I.pack(length))
05376 for val4 in _v397.channels:
05377 _x = val4.name
05378 length = len(_x)
05379 if python3 or type(_x) == unicode:
05380 _x = _x.encode('utf-8')
05381 length = len(_x)
05382 buff.write(struct.pack('<I%ss'%length, length, _x))
05383 length = len(val4.values)
05384 buff.write(_struct_I.pack(length))
05385 pattern = '<%sf'%length
05386 buff.write(val4.values.tostring())
05387 _v400 = val2.region
05388 _v401 = _v400.cloud
05389 _v402 = _v401.header
05390 buff.write(_struct_I.pack(_v402.seq))
05391 _v403 = _v402.stamp
05392 _x = _v403
05393 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05394 _x = _v402.frame_id
05395 length = len(_x)
05396 if python3 or type(_x) == unicode:
05397 _x = _x.encode('utf-8')
05398 length = len(_x)
05399 buff.write(struct.pack('<I%ss'%length, length, _x))
05400 _x = _v401
05401 buff.write(_struct_2I.pack(_x.height, _x.width))
05402 length = len(_v401.fields)
05403 buff.write(_struct_I.pack(length))
05404 for val5 in _v401.fields:
05405 _x = val5.name
05406 length = len(_x)
05407 if python3 or type(_x) == unicode:
05408 _x = _x.encode('utf-8')
05409 length = len(_x)
05410 buff.write(struct.pack('<I%ss'%length, length, _x))
05411 _x = val5
05412 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
05413 _x = _v401
05414 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
05415 _x = _v401.data
05416 length = len(_x)
05417 # - if encoded as a list instead, serialize as bytes instead of string
05418 if type(_x) in [list, tuple]:
05419 buff.write(struct.pack('<I%sB'%length, length, *_x))
05420 else:
05421 buff.write(struct.pack('<I%ss'%length, length, _x))
05422 buff.write(_struct_B.pack(_v401.is_dense))
05423 length = len(_v400.mask)
05424 buff.write(_struct_I.pack(length))
05425 pattern = '<%si'%length
05426 buff.write(_v400.mask.tostring())
05427 _v404 = _v400.image
05428 _v405 = _v404.header
05429 buff.write(_struct_I.pack(_v405.seq))
05430 _v406 = _v405.stamp
05431 _x = _v406
05432 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05433 _x = _v405.frame_id
05434 length = len(_x)
05435 if python3 or type(_x) == unicode:
05436 _x = _x.encode('utf-8')
05437 length = len(_x)
05438 buff.write(struct.pack('<I%ss'%length, length, _x))
05439 _x = _v404
05440 buff.write(_struct_2I.pack(_x.height, _x.width))
05441 _x = _v404.encoding
05442 length = len(_x)
05443 if python3 or type(_x) == unicode:
05444 _x = _x.encode('utf-8')
05445 length = len(_x)
05446 buff.write(struct.pack('<I%ss'%length, length, _x))
05447 _x = _v404
05448 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
05449 _x = _v404.data
05450 length = len(_x)
05451 # - if encoded as a list instead, serialize as bytes instead of string
05452 if type(_x) in [list, tuple]:
05453 buff.write(struct.pack('<I%sB'%length, length, *_x))
05454 else:
05455 buff.write(struct.pack('<I%ss'%length, length, _x))
05456 _v407 = _v400.disparity_image
05457 _v408 = _v407.header
05458 buff.write(_struct_I.pack(_v408.seq))
05459 _v409 = _v408.stamp
05460 _x = _v409
05461 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05462 _x = _v408.frame_id
05463 length = len(_x)
05464 if python3 or type(_x) == unicode:
05465 _x = _x.encode('utf-8')
05466 length = len(_x)
05467 buff.write(struct.pack('<I%ss'%length, length, _x))
05468 _x = _v407
05469 buff.write(_struct_2I.pack(_x.height, _x.width))
05470 _x = _v407.encoding
05471 length = len(_x)
05472 if python3 or type(_x) == unicode:
05473 _x = _x.encode('utf-8')
05474 length = len(_x)
05475 buff.write(struct.pack('<I%ss'%length, length, _x))
05476 _x = _v407
05477 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
05478 _x = _v407.data
05479 length = len(_x)
05480 # - if encoded as a list instead, serialize as bytes instead of string
05481 if type(_x) in [list, tuple]:
05482 buff.write(struct.pack('<I%sB'%length, length, *_x))
05483 else:
05484 buff.write(struct.pack('<I%ss'%length, length, _x))
05485 _v410 = _v400.cam_info
05486 _v411 = _v410.header
05487 buff.write(_struct_I.pack(_v411.seq))
05488 _v412 = _v411.stamp
05489 _x = _v412
05490 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05491 _x = _v411.frame_id
05492 length = len(_x)
05493 if python3 or type(_x) == unicode:
05494 _x = _x.encode('utf-8')
05495 length = len(_x)
05496 buff.write(struct.pack('<I%ss'%length, length, _x))
05497 _x = _v410
05498 buff.write(_struct_2I.pack(_x.height, _x.width))
05499 _x = _v410.distortion_model
05500 length = len(_x)
05501 if python3 or type(_x) == unicode:
05502 _x = _x.encode('utf-8')
05503 length = len(_x)
05504 buff.write(struct.pack('<I%ss'%length, length, _x))
05505 length = len(_v410.D)
05506 buff.write(_struct_I.pack(length))
05507 pattern = '<%sd'%length
05508 buff.write(_v410.D.tostring())
05509 buff.write(_v410.K.tostring())
05510 buff.write(_v410.R.tostring())
05511 buff.write(_v410.P.tostring())
05512 _x = _v410
05513 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
05514 _v413 = _v410.roi
05515 _x = _v413
05516 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
05517 _v414 = _v400.roi_box_pose
05518 _v415 = _v414.header
05519 buff.write(_struct_I.pack(_v415.seq))
05520 _v416 = _v415.stamp
05521 _x = _v416
05522 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05523 _x = _v415.frame_id
05524 length = len(_x)
05525 if python3 or type(_x) == unicode:
05526 _x = _x.encode('utf-8')
05527 length = len(_x)
05528 buff.write(struct.pack('<I%ss'%length, length, _x))
05529 _v417 = _v414.pose
05530 _v418 = _v417.position
05531 _x = _v418
05532 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05533 _v419 = _v417.orientation
05534 _x = _v419
05535 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05536 _v420 = _v400.roi_box_dims
05537 _x = _v420
05538 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05539 _x = val2.collision_name
05540 length = len(_x)
05541 if python3 or type(_x) == unicode:
05542 _x = _x.encode('utf-8')
05543 length = len(_x)
05544 buff.write(struct.pack('<I%ss'%length, length, _x))
05545 _x = self
05546 buff.write(_struct_i3I.pack(_x.action_result.result.error_code.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
05547 _x = self.action_feedback.header.frame_id
05548 length = len(_x)
05549 if python3 or type(_x) == unicode:
05550 _x = _x.encode('utf-8')
05551 length = len(_x)
05552 buff.write(struct.pack('<I%ss'%length, length, _x))
05553 _x = self
05554 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
05555 _x = self.action_feedback.status.goal_id.id
05556 length = len(_x)
05557 if python3 or type(_x) == unicode:
05558 _x = _x.encode('utf-8')
05559 length = len(_x)
05560 buff.write(struct.pack('<I%ss'%length, length, _x))
05561 buff.write(_struct_B.pack(self.action_feedback.status.status))
05562 _x = self.action_feedback.status.text
05563 length = len(_x)
05564 if python3 or type(_x) == unicode:
05565 _x = _x.encode('utf-8')
05566 length = len(_x)
05567 buff.write(struct.pack('<I%ss'%length, length, _x))
05568 length = len(self.action_feedback.feedback.grasps)
05569 buff.write(_struct_I.pack(length))
05570 for val1 in self.action_feedback.feedback.grasps:
05571 _v421 = val1.pre_grasp_posture
05572 _v422 = _v421.header
05573 buff.write(_struct_I.pack(_v422.seq))
05574 _v423 = _v422.stamp
05575 _x = _v423
05576 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05577 _x = _v422.frame_id
05578 length = len(_x)
05579 if python3 or type(_x) == unicode:
05580 _x = _x.encode('utf-8')
05581 length = len(_x)
05582 buff.write(struct.pack('<I%ss'%length, length, _x))
05583 length = len(_v421.name)
05584 buff.write(_struct_I.pack(length))
05585 for val3 in _v421.name:
05586 length = len(val3)
05587 if python3 or type(val3) == unicode:
05588 val3 = val3.encode('utf-8')
05589 length = len(val3)
05590 buff.write(struct.pack('<I%ss'%length, length, val3))
05591 length = len(_v421.position)
05592 buff.write(_struct_I.pack(length))
05593 pattern = '<%sd'%length
05594 buff.write(_v421.position.tostring())
05595 length = len(_v421.velocity)
05596 buff.write(_struct_I.pack(length))
05597 pattern = '<%sd'%length
05598 buff.write(_v421.velocity.tostring())
05599 length = len(_v421.effort)
05600 buff.write(_struct_I.pack(length))
05601 pattern = '<%sd'%length
05602 buff.write(_v421.effort.tostring())
05603 _v424 = val1.grasp_posture
05604 _v425 = _v424.header
05605 buff.write(_struct_I.pack(_v425.seq))
05606 _v426 = _v425.stamp
05607 _x = _v426
05608 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05609 _x = _v425.frame_id
05610 length = len(_x)
05611 if python3 or type(_x) == unicode:
05612 _x = _x.encode('utf-8')
05613 length = len(_x)
05614 buff.write(struct.pack('<I%ss'%length, length, _x))
05615 length = len(_v424.name)
05616 buff.write(_struct_I.pack(length))
05617 for val3 in _v424.name:
05618 length = len(val3)
05619 if python3 or type(val3) == unicode:
05620 val3 = val3.encode('utf-8')
05621 length = len(val3)
05622 buff.write(struct.pack('<I%ss'%length, length, val3))
05623 length = len(_v424.position)
05624 buff.write(_struct_I.pack(length))
05625 pattern = '<%sd'%length
05626 buff.write(_v424.position.tostring())
05627 length = len(_v424.velocity)
05628 buff.write(_struct_I.pack(length))
05629 pattern = '<%sd'%length
05630 buff.write(_v424.velocity.tostring())
05631 length = len(_v424.effort)
05632 buff.write(_struct_I.pack(length))
05633 pattern = '<%sd'%length
05634 buff.write(_v424.effort.tostring())
05635 _v427 = val1.grasp_pose
05636 _v428 = _v427.position
05637 _x = _v428
05638 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05639 _v429 = _v427.orientation
05640 _x = _v429
05641 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05642 _x = val1
05643 buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
05644 length = len(val1.moved_obstacles)
05645 buff.write(_struct_I.pack(length))
05646 for val2 in val1.moved_obstacles:
05647 _x = val2.reference_frame_id
05648 length = len(_x)
05649 if python3 or type(_x) == unicode:
05650 _x = _x.encode('utf-8')
05651 length = len(_x)
05652 buff.write(struct.pack('<I%ss'%length, length, _x))
05653 length = len(val2.potential_models)
05654 buff.write(_struct_I.pack(length))
05655 for val3 in val2.potential_models:
05656 buff.write(_struct_i.pack(val3.model_id))
05657 _v430 = val3.pose
05658 _v431 = _v430.header
05659 buff.write(_struct_I.pack(_v431.seq))
05660 _v432 = _v431.stamp
05661 _x = _v432
05662 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05663 _x = _v431.frame_id
05664 length = len(_x)
05665 if python3 or type(_x) == unicode:
05666 _x = _x.encode('utf-8')
05667 length = len(_x)
05668 buff.write(struct.pack('<I%ss'%length, length, _x))
05669 _v433 = _v430.pose
05670 _v434 = _v433.position
05671 _x = _v434
05672 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05673 _v435 = _v433.orientation
05674 _x = _v435
05675 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05676 buff.write(_struct_f.pack(val3.confidence))
05677 _x = val3.detector_name
05678 length = len(_x)
05679 if python3 or type(_x) == unicode:
05680 _x = _x.encode('utf-8')
05681 length = len(_x)
05682 buff.write(struct.pack('<I%ss'%length, length, _x))
05683 _v436 = val2.cluster
05684 _v437 = _v436.header
05685 buff.write(_struct_I.pack(_v437.seq))
05686 _v438 = _v437.stamp
05687 _x = _v438
05688 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05689 _x = _v437.frame_id
05690 length = len(_x)
05691 if python3 or type(_x) == unicode:
05692 _x = _x.encode('utf-8')
05693 length = len(_x)
05694 buff.write(struct.pack('<I%ss'%length, length, _x))
05695 length = len(_v436.points)
05696 buff.write(_struct_I.pack(length))
05697 for val4 in _v436.points:
05698 _x = val4
05699 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
05700 length = len(_v436.channels)
05701 buff.write(_struct_I.pack(length))
05702 for val4 in _v436.channels:
05703 _x = val4.name
05704 length = len(_x)
05705 if python3 or type(_x) == unicode:
05706 _x = _x.encode('utf-8')
05707 length = len(_x)
05708 buff.write(struct.pack('<I%ss'%length, length, _x))
05709 length = len(val4.values)
05710 buff.write(_struct_I.pack(length))
05711 pattern = '<%sf'%length
05712 buff.write(val4.values.tostring())
05713 _v439 = val2.region
05714 _v440 = _v439.cloud
05715 _v441 = _v440.header
05716 buff.write(_struct_I.pack(_v441.seq))
05717 _v442 = _v441.stamp
05718 _x = _v442
05719 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05720 _x = _v441.frame_id
05721 length = len(_x)
05722 if python3 or type(_x) == unicode:
05723 _x = _x.encode('utf-8')
05724 length = len(_x)
05725 buff.write(struct.pack('<I%ss'%length, length, _x))
05726 _x = _v440
05727 buff.write(_struct_2I.pack(_x.height, _x.width))
05728 length = len(_v440.fields)
05729 buff.write(_struct_I.pack(length))
05730 for val5 in _v440.fields:
05731 _x = val5.name
05732 length = len(_x)
05733 if python3 or type(_x) == unicode:
05734 _x = _x.encode('utf-8')
05735 length = len(_x)
05736 buff.write(struct.pack('<I%ss'%length, length, _x))
05737 _x = val5
05738 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
05739 _x = _v440
05740 buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
05741 _x = _v440.data
05742 length = len(_x)
05743 # - if encoded as a list instead, serialize as bytes instead of string
05744 if type(_x) in [list, tuple]:
05745 buff.write(struct.pack('<I%sB'%length, length, *_x))
05746 else:
05747 buff.write(struct.pack('<I%ss'%length, length, _x))
05748 buff.write(_struct_B.pack(_v440.is_dense))
05749 length = len(_v439.mask)
05750 buff.write(_struct_I.pack(length))
05751 pattern = '<%si'%length
05752 buff.write(_v439.mask.tostring())
05753 _v443 = _v439.image
05754 _v444 = _v443.header
05755 buff.write(_struct_I.pack(_v444.seq))
05756 _v445 = _v444.stamp
05757 _x = _v445
05758 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05759 _x = _v444.frame_id
05760 length = len(_x)
05761 if python3 or type(_x) == unicode:
05762 _x = _x.encode('utf-8')
05763 length = len(_x)
05764 buff.write(struct.pack('<I%ss'%length, length, _x))
05765 _x = _v443
05766 buff.write(_struct_2I.pack(_x.height, _x.width))
05767 _x = _v443.encoding
05768 length = len(_x)
05769 if python3 or type(_x) == unicode:
05770 _x = _x.encode('utf-8')
05771 length = len(_x)
05772 buff.write(struct.pack('<I%ss'%length, length, _x))
05773 _x = _v443
05774 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
05775 _x = _v443.data
05776 length = len(_x)
05777 # - if encoded as a list instead, serialize as bytes instead of string
05778 if type(_x) in [list, tuple]:
05779 buff.write(struct.pack('<I%sB'%length, length, *_x))
05780 else:
05781 buff.write(struct.pack('<I%ss'%length, length, _x))
05782 _v446 = _v439.disparity_image
05783 _v447 = _v446.header
05784 buff.write(_struct_I.pack(_v447.seq))
05785 _v448 = _v447.stamp
05786 _x = _v448
05787 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05788 _x = _v447.frame_id
05789 length = len(_x)
05790 if python3 or type(_x) == unicode:
05791 _x = _x.encode('utf-8')
05792 length = len(_x)
05793 buff.write(struct.pack('<I%ss'%length, length, _x))
05794 _x = _v446
05795 buff.write(_struct_2I.pack(_x.height, _x.width))
05796 _x = _v446.encoding
05797 length = len(_x)
05798 if python3 or type(_x) == unicode:
05799 _x = _x.encode('utf-8')
05800 length = len(_x)
05801 buff.write(struct.pack('<I%ss'%length, length, _x))
05802 _x = _v446
05803 buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
05804 _x = _v446.data
05805 length = len(_x)
05806 # - if encoded as a list instead, serialize as bytes instead of string
05807 if type(_x) in [list, tuple]:
05808 buff.write(struct.pack('<I%sB'%length, length, *_x))
05809 else:
05810 buff.write(struct.pack('<I%ss'%length, length, _x))
05811 _v449 = _v439.cam_info
05812 _v450 = _v449.header
05813 buff.write(_struct_I.pack(_v450.seq))
05814 _v451 = _v450.stamp
05815 _x = _v451
05816 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05817 _x = _v450.frame_id
05818 length = len(_x)
05819 if python3 or type(_x) == unicode:
05820 _x = _x.encode('utf-8')
05821 length = len(_x)
05822 buff.write(struct.pack('<I%ss'%length, length, _x))
05823 _x = _v449
05824 buff.write(_struct_2I.pack(_x.height, _x.width))
05825 _x = _v449.distortion_model
05826 length = len(_x)
05827 if python3 or type(_x) == unicode:
05828 _x = _x.encode('utf-8')
05829 length = len(_x)
05830 buff.write(struct.pack('<I%ss'%length, length, _x))
05831 length = len(_v449.D)
05832 buff.write(_struct_I.pack(length))
05833 pattern = '<%sd'%length
05834 buff.write(_v449.D.tostring())
05835 buff.write(_v449.K.tostring())
05836 buff.write(_v449.R.tostring())
05837 buff.write(_v449.P.tostring())
05838 _x = _v449
05839 buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
05840 _v452 = _v449.roi
05841 _x = _v452
05842 buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
05843 _v453 = _v439.roi_box_pose
05844 _v454 = _v453.header
05845 buff.write(_struct_I.pack(_v454.seq))
05846 _v455 = _v454.stamp
05847 _x = _v455
05848 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
05849 _x = _v454.frame_id
05850 length = len(_x)
05851 if python3 or type(_x) == unicode:
05852 _x = _x.encode('utf-8')
05853 length = len(_x)
05854 buff.write(struct.pack('<I%ss'%length, length, _x))
05855 _v456 = _v453.pose
05856 _v457 = _v456.position
05857 _x = _v457
05858 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05859 _v458 = _v456.orientation
05860 _x = _v458
05861 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
05862 _v459 = _v439.roi_box_dims
05863 _x = _v459
05864 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
05865 _x = val2.collision_name
05866 length = len(_x)
05867 if python3 or type(_x) == unicode:
05868 _x = _x.encode('utf-8')
05869 length = len(_x)
05870 buff.write(struct.pack('<I%ss'%length, length, _x))
05871 except struct.error as se: self._check_types(se)
05872 except TypeError as te: self._check_types(te)
05873
05874 def deserialize_numpy(self, str, numpy):
05875 """
05876 unpack serialized message in str into this message instance using numpy for array types
05877 :param str: byte array of serialized message, ``str``
05878 :param numpy: numpy python module
05879 """
05880 try:
05881 if self.action_goal is None:
05882 self.action_goal = object_manipulation_msgs.msg.GraspPlanningActionGoal()
05883 if self.action_result is None:
05884 self.action_result = object_manipulation_msgs.msg.GraspPlanningActionResult()
05885 if self.action_feedback is None:
05886 self.action_feedback = object_manipulation_msgs.msg.GraspPlanningActionFeedback()
05887 end = 0
05888 _x = self
05889 start = end
05890 end += 12
05891 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
05892 start = end
05893 end += 4
05894 (length,) = _struct_I.unpack(str[start:end])
05895 start = end
05896 end += length
05897 if python3:
05898 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
05899 else:
05900 self.action_goal.header.frame_id = str[start:end]
05901 _x = self
05902 start = end
05903 end += 8
05904 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
05905 start = end
05906 end += 4
05907 (length,) = _struct_I.unpack(str[start:end])
05908 start = end
05909 end += length
05910 if python3:
05911 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
05912 else:
05913 self.action_goal.goal_id.id = str[start:end]
05914 start = end
05915 end += 4
05916 (length,) = _struct_I.unpack(str[start:end])
05917 start = end
05918 end += length
05919 if python3:
05920 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
05921 else:
05922 self.action_goal.goal.arm_name = str[start:end]
05923 start = end
05924 end += 4
05925 (length,) = _struct_I.unpack(str[start:end])
05926 start = end
05927 end += length
05928 if python3:
05929 self.action_goal.goal.target.reference_frame_id = str[start:end].decode('utf-8')
05930 else:
05931 self.action_goal.goal.target.reference_frame_id = str[start:end]
05932 start = end
05933 end += 4
05934 (length,) = _struct_I.unpack(str[start:end])
05935 self.action_goal.goal.target.potential_models = []
05936 for i in range(0, length):
05937 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
05938 start = end
05939 end += 4
05940 (val1.model_id,) = _struct_i.unpack(str[start:end])
05941 _v460 = val1.pose
05942 _v461 = _v460.header
05943 start = end
05944 end += 4
05945 (_v461.seq,) = _struct_I.unpack(str[start:end])
05946 _v462 = _v461.stamp
05947 _x = _v462
05948 start = end
05949 end += 8
05950 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05951 start = end
05952 end += 4
05953 (length,) = _struct_I.unpack(str[start:end])
05954 start = end
05955 end += length
05956 if python3:
05957 _v461.frame_id = str[start:end].decode('utf-8')
05958 else:
05959 _v461.frame_id = str[start:end]
05960 _v463 = _v460.pose
05961 _v464 = _v463.position
05962 _x = _v464
05963 start = end
05964 end += 24
05965 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05966 _v465 = _v463.orientation
05967 _x = _v465
05968 start = end
05969 end += 32
05970 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05971 start = end
05972 end += 4
05973 (val1.confidence,) = _struct_f.unpack(str[start:end])
05974 start = end
05975 end += 4
05976 (length,) = _struct_I.unpack(str[start:end])
05977 start = end
05978 end += length
05979 if python3:
05980 val1.detector_name = str[start:end].decode('utf-8')
05981 else:
05982 val1.detector_name = str[start:end]
05983 self.action_goal.goal.target.potential_models.append(val1)
05984 _x = self
05985 start = end
05986 end += 12
05987 (_x.action_goal.goal.target.cluster.header.seq, _x.action_goal.goal.target.cluster.header.stamp.secs, _x.action_goal.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
05988 start = end
05989 end += 4
05990 (length,) = _struct_I.unpack(str[start:end])
05991 start = end
05992 end += length
05993 if python3:
05994 self.action_goal.goal.target.cluster.header.frame_id = str[start:end].decode('utf-8')
05995 else:
05996 self.action_goal.goal.target.cluster.header.frame_id = str[start:end]
05997 start = end
05998 end += 4
05999 (length,) = _struct_I.unpack(str[start:end])
06000 self.action_goal.goal.target.cluster.points = []
06001 for i in range(0, length):
06002 val1 = geometry_msgs.msg.Point32()
06003 _x = val1
06004 start = end
06005 end += 12
06006 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
06007 self.action_goal.goal.target.cluster.points.append(val1)
06008 start = end
06009 end += 4
06010 (length,) = _struct_I.unpack(str[start:end])
06011 self.action_goal.goal.target.cluster.channels = []
06012 for i in range(0, length):
06013 val1 = sensor_msgs.msg.ChannelFloat32()
06014 start = end
06015 end += 4
06016 (length,) = _struct_I.unpack(str[start:end])
06017 start = end
06018 end += length
06019 if python3:
06020 val1.name = str[start:end].decode('utf-8')
06021 else:
06022 val1.name = str[start:end]
06023 start = end
06024 end += 4
06025 (length,) = _struct_I.unpack(str[start:end])
06026 pattern = '<%sf'%length
06027 start = end
06028 end += struct.calcsize(pattern)
06029 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
06030 self.action_goal.goal.target.cluster.channels.append(val1)
06031 _x = self
06032 start = end
06033 end += 12
06034 (_x.action_goal.goal.target.region.cloud.header.seq, _x.action_goal.goal.target.region.cloud.header.stamp.secs, _x.action_goal.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
06035 start = end
06036 end += 4
06037 (length,) = _struct_I.unpack(str[start:end])
06038 start = end
06039 end += length
06040 if python3:
06041 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end].decode('utf-8')
06042 else:
06043 self.action_goal.goal.target.region.cloud.header.frame_id = str[start:end]
06044 _x = self
06045 start = end
06046 end += 8
06047 (_x.action_goal.goal.target.region.cloud.height, _x.action_goal.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
06048 start = end
06049 end += 4
06050 (length,) = _struct_I.unpack(str[start:end])
06051 self.action_goal.goal.target.region.cloud.fields = []
06052 for i in range(0, length):
06053 val1 = sensor_msgs.msg.PointField()
06054 start = end
06055 end += 4
06056 (length,) = _struct_I.unpack(str[start:end])
06057 start = end
06058 end += length
06059 if python3:
06060 val1.name = str[start:end].decode('utf-8')
06061 else:
06062 val1.name = str[start:end]
06063 _x = val1
06064 start = end
06065 end += 9
06066 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
06067 self.action_goal.goal.target.region.cloud.fields.append(val1)
06068 _x = self
06069 start = end
06070 end += 9
06071 (_x.action_goal.goal.target.region.cloud.is_bigendian, _x.action_goal.goal.target.region.cloud.point_step, _x.action_goal.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
06072 self.action_goal.goal.target.region.cloud.is_bigendian = bool(self.action_goal.goal.target.region.cloud.is_bigendian)
06073 start = end
06074 end += 4
06075 (length,) = _struct_I.unpack(str[start:end])
06076 start = end
06077 end += length
06078 if python3:
06079 self.action_goal.goal.target.region.cloud.data = str[start:end].decode('utf-8')
06080 else:
06081 self.action_goal.goal.target.region.cloud.data = str[start:end]
06082 start = end
06083 end += 1
06084 (self.action_goal.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
06085 self.action_goal.goal.target.region.cloud.is_dense = bool(self.action_goal.goal.target.region.cloud.is_dense)
06086 start = end
06087 end += 4
06088 (length,) = _struct_I.unpack(str[start:end])
06089 pattern = '<%si'%length
06090 start = end
06091 end += struct.calcsize(pattern)
06092 self.action_goal.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
06093 _x = self
06094 start = end
06095 end += 12
06096 (_x.action_goal.goal.target.region.image.header.seq, _x.action_goal.goal.target.region.image.header.stamp.secs, _x.action_goal.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
06097 start = end
06098 end += 4
06099 (length,) = _struct_I.unpack(str[start:end])
06100 start = end
06101 end += length
06102 if python3:
06103 self.action_goal.goal.target.region.image.header.frame_id = str[start:end].decode('utf-8')
06104 else:
06105 self.action_goal.goal.target.region.image.header.frame_id = str[start:end]
06106 _x = self
06107 start = end
06108 end += 8
06109 (_x.action_goal.goal.target.region.image.height, _x.action_goal.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
06110 start = end
06111 end += 4
06112 (length,) = _struct_I.unpack(str[start:end])
06113 start = end
06114 end += length
06115 if python3:
06116 self.action_goal.goal.target.region.image.encoding = str[start:end].decode('utf-8')
06117 else:
06118 self.action_goal.goal.target.region.image.encoding = str[start:end]
06119 _x = self
06120 start = end
06121 end += 5
06122 (_x.action_goal.goal.target.region.image.is_bigendian, _x.action_goal.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
06123 start = end
06124 end += 4
06125 (length,) = _struct_I.unpack(str[start:end])
06126 start = end
06127 end += length
06128 if python3:
06129 self.action_goal.goal.target.region.image.data = str[start:end].decode('utf-8')
06130 else:
06131 self.action_goal.goal.target.region.image.data = str[start:end]
06132 _x = self
06133 start = end
06134 end += 12
06135 (_x.action_goal.goal.target.region.disparity_image.header.seq, _x.action_goal.goal.target.region.disparity_image.header.stamp.secs, _x.action_goal.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
06136 start = end
06137 end += 4
06138 (length,) = _struct_I.unpack(str[start:end])
06139 start = end
06140 end += length
06141 if python3:
06142 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
06143 else:
06144 self.action_goal.goal.target.region.disparity_image.header.frame_id = str[start:end]
06145 _x = self
06146 start = end
06147 end += 8
06148 (_x.action_goal.goal.target.region.disparity_image.height, _x.action_goal.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
06149 start = end
06150 end += 4
06151 (length,) = _struct_I.unpack(str[start:end])
06152 start = end
06153 end += length
06154 if python3:
06155 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end].decode('utf-8')
06156 else:
06157 self.action_goal.goal.target.region.disparity_image.encoding = str[start:end]
06158 _x = self
06159 start = end
06160 end += 5
06161 (_x.action_goal.goal.target.region.disparity_image.is_bigendian, _x.action_goal.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
06162 start = end
06163 end += 4
06164 (length,) = _struct_I.unpack(str[start:end])
06165 start = end
06166 end += length
06167 if python3:
06168 self.action_goal.goal.target.region.disparity_image.data = str[start:end].decode('utf-8')
06169 else:
06170 self.action_goal.goal.target.region.disparity_image.data = str[start:end]
06171 _x = self
06172 start = end
06173 end += 12
06174 (_x.action_goal.goal.target.region.cam_info.header.seq, _x.action_goal.goal.target.region.cam_info.header.stamp.secs, _x.action_goal.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
06175 start = end
06176 end += 4
06177 (length,) = _struct_I.unpack(str[start:end])
06178 start = end
06179 end += length
06180 if python3:
06181 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
06182 else:
06183 self.action_goal.goal.target.region.cam_info.header.frame_id = str[start:end]
06184 _x = self
06185 start = end
06186 end += 8
06187 (_x.action_goal.goal.target.region.cam_info.height, _x.action_goal.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
06188 start = end
06189 end += 4
06190 (length,) = _struct_I.unpack(str[start:end])
06191 start = end
06192 end += length
06193 if python3:
06194 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end].decode('utf-8')
06195 else:
06196 self.action_goal.goal.target.region.cam_info.distortion_model = str[start:end]
06197 start = end
06198 end += 4
06199 (length,) = _struct_I.unpack(str[start:end])
06200 pattern = '<%sd'%length
06201 start = end
06202 end += struct.calcsize(pattern)
06203 self.action_goal.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06204 start = end
06205 end += 72
06206 self.action_goal.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
06207 start = end
06208 end += 72
06209 self.action_goal.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
06210 start = end
06211 end += 96
06212 self.action_goal.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
06213 _x = self
06214 start = end
06215 end += 37
06216 (_x.action_goal.goal.target.region.cam_info.binning_x, _x.action_goal.goal.target.region.cam_info.binning_y, _x.action_goal.goal.target.region.cam_info.roi.x_offset, _x.action_goal.goal.target.region.cam_info.roi.y_offset, _x.action_goal.goal.target.region.cam_info.roi.height, _x.action_goal.goal.target.region.cam_info.roi.width, _x.action_goal.goal.target.region.cam_info.roi.do_rectify, _x.action_goal.goal.target.region.roi_box_pose.header.seq, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
06217 self.action_goal.goal.target.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.target.region.cam_info.roi.do_rectify)
06218 start = end
06219 end += 4
06220 (length,) = _struct_I.unpack(str[start:end])
06221 start = end
06222 end += length
06223 if python3:
06224 self.action_goal.goal.target.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
06225 else:
06226 self.action_goal.goal.target.region.roi_box_pose.header.frame_id = str[start:end]
06227 _x = self
06228 start = end
06229 end += 80
06230 (_x.action_goal.goal.target.region.roi_box_pose.pose.position.x, _x.action_goal.goal.target.region.roi_box_pose.pose.position.y, _x.action_goal.goal.target.region.roi_box_pose.pose.position.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.target.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.target.region.roi_box_dims.x, _x.action_goal.goal.target.region.roi_box_dims.y, _x.action_goal.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
06231 start = end
06232 end += 4
06233 (length,) = _struct_I.unpack(str[start:end])
06234 start = end
06235 end += length
06236 if python3:
06237 self.action_goal.goal.target.collision_name = str[start:end].decode('utf-8')
06238 else:
06239 self.action_goal.goal.target.collision_name = str[start:end]
06240 start = end
06241 end += 4
06242 (length,) = _struct_I.unpack(str[start:end])
06243 start = end
06244 end += length
06245 if python3:
06246 self.action_goal.goal.collision_object_name = str[start:end].decode('utf-8')
06247 else:
06248 self.action_goal.goal.collision_object_name = str[start:end]
06249 start = end
06250 end += 4
06251 (length,) = _struct_I.unpack(str[start:end])
06252 start = end
06253 end += length
06254 if python3:
06255 self.action_goal.goal.collision_support_surface_name = str[start:end].decode('utf-8')
06256 else:
06257 self.action_goal.goal.collision_support_surface_name = str[start:end]
06258 start = end
06259 end += 4
06260 (length,) = _struct_I.unpack(str[start:end])
06261 self.action_goal.goal.grasps_to_evaluate = []
06262 for i in range(0, length):
06263 val1 = object_manipulation_msgs.msg.Grasp()
06264 _v466 = val1.pre_grasp_posture
06265 _v467 = _v466.header
06266 start = end
06267 end += 4
06268 (_v467.seq,) = _struct_I.unpack(str[start:end])
06269 _v468 = _v467.stamp
06270 _x = _v468
06271 start = end
06272 end += 8
06273 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06274 start = end
06275 end += 4
06276 (length,) = _struct_I.unpack(str[start:end])
06277 start = end
06278 end += length
06279 if python3:
06280 _v467.frame_id = str[start:end].decode('utf-8')
06281 else:
06282 _v467.frame_id = str[start:end]
06283 start = end
06284 end += 4
06285 (length,) = _struct_I.unpack(str[start:end])
06286 _v466.name = []
06287 for i in range(0, length):
06288 start = end
06289 end += 4
06290 (length,) = _struct_I.unpack(str[start:end])
06291 start = end
06292 end += length
06293 if python3:
06294 val3 = str[start:end].decode('utf-8')
06295 else:
06296 val3 = str[start:end]
06297 _v466.name.append(val3)
06298 start = end
06299 end += 4
06300 (length,) = _struct_I.unpack(str[start:end])
06301 pattern = '<%sd'%length
06302 start = end
06303 end += struct.calcsize(pattern)
06304 _v466.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06305 start = end
06306 end += 4
06307 (length,) = _struct_I.unpack(str[start:end])
06308 pattern = '<%sd'%length
06309 start = end
06310 end += struct.calcsize(pattern)
06311 _v466.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06312 start = end
06313 end += 4
06314 (length,) = _struct_I.unpack(str[start:end])
06315 pattern = '<%sd'%length
06316 start = end
06317 end += struct.calcsize(pattern)
06318 _v466.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06319 _v469 = val1.grasp_posture
06320 _v470 = _v469.header
06321 start = end
06322 end += 4
06323 (_v470.seq,) = _struct_I.unpack(str[start:end])
06324 _v471 = _v470.stamp
06325 _x = _v471
06326 start = end
06327 end += 8
06328 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06329 start = end
06330 end += 4
06331 (length,) = _struct_I.unpack(str[start:end])
06332 start = end
06333 end += length
06334 if python3:
06335 _v470.frame_id = str[start:end].decode('utf-8')
06336 else:
06337 _v470.frame_id = str[start:end]
06338 start = end
06339 end += 4
06340 (length,) = _struct_I.unpack(str[start:end])
06341 _v469.name = []
06342 for i in range(0, length):
06343 start = end
06344 end += 4
06345 (length,) = _struct_I.unpack(str[start:end])
06346 start = end
06347 end += length
06348 if python3:
06349 val3 = str[start:end].decode('utf-8')
06350 else:
06351 val3 = str[start:end]
06352 _v469.name.append(val3)
06353 start = end
06354 end += 4
06355 (length,) = _struct_I.unpack(str[start:end])
06356 pattern = '<%sd'%length
06357 start = end
06358 end += struct.calcsize(pattern)
06359 _v469.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06360 start = end
06361 end += 4
06362 (length,) = _struct_I.unpack(str[start:end])
06363 pattern = '<%sd'%length
06364 start = end
06365 end += struct.calcsize(pattern)
06366 _v469.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06367 start = end
06368 end += 4
06369 (length,) = _struct_I.unpack(str[start:end])
06370 pattern = '<%sd'%length
06371 start = end
06372 end += struct.calcsize(pattern)
06373 _v469.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06374 _v472 = val1.grasp_pose
06375 _v473 = _v472.position
06376 _x = _v473
06377 start = end
06378 end += 24
06379 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
06380 _v474 = _v472.orientation
06381 _x = _v474
06382 start = end
06383 end += 32
06384 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
06385 _x = val1
06386 start = end
06387 end += 17
06388 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
06389 val1.cluster_rep = bool(val1.cluster_rep)
06390 start = end
06391 end += 4
06392 (length,) = _struct_I.unpack(str[start:end])
06393 val1.moved_obstacles = []
06394 for i in range(0, length):
06395 val2 = object_manipulation_msgs.msg.GraspableObject()
06396 start = end
06397 end += 4
06398 (length,) = _struct_I.unpack(str[start:end])
06399 start = end
06400 end += length
06401 if python3:
06402 val2.reference_frame_id = str[start:end].decode('utf-8')
06403 else:
06404 val2.reference_frame_id = str[start:end]
06405 start = end
06406 end += 4
06407 (length,) = _struct_I.unpack(str[start:end])
06408 val2.potential_models = []
06409 for i in range(0, length):
06410 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
06411 start = end
06412 end += 4
06413 (val3.model_id,) = _struct_i.unpack(str[start:end])
06414 _v475 = val3.pose
06415 _v476 = _v475.header
06416 start = end
06417 end += 4
06418 (_v476.seq,) = _struct_I.unpack(str[start:end])
06419 _v477 = _v476.stamp
06420 _x = _v477
06421 start = end
06422 end += 8
06423 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06424 start = end
06425 end += 4
06426 (length,) = _struct_I.unpack(str[start:end])
06427 start = end
06428 end += length
06429 if python3:
06430 _v476.frame_id = str[start:end].decode('utf-8')
06431 else:
06432 _v476.frame_id = str[start:end]
06433 _v478 = _v475.pose
06434 _v479 = _v478.position
06435 _x = _v479
06436 start = end
06437 end += 24
06438 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
06439 _v480 = _v478.orientation
06440 _x = _v480
06441 start = end
06442 end += 32
06443 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
06444 start = end
06445 end += 4
06446 (val3.confidence,) = _struct_f.unpack(str[start:end])
06447 start = end
06448 end += 4
06449 (length,) = _struct_I.unpack(str[start:end])
06450 start = end
06451 end += length
06452 if python3:
06453 val3.detector_name = str[start:end].decode('utf-8')
06454 else:
06455 val3.detector_name = str[start:end]
06456 val2.potential_models.append(val3)
06457 _v481 = val2.cluster
06458 _v482 = _v481.header
06459 start = end
06460 end += 4
06461 (_v482.seq,) = _struct_I.unpack(str[start:end])
06462 _v483 = _v482.stamp
06463 _x = _v483
06464 start = end
06465 end += 8
06466 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06467 start = end
06468 end += 4
06469 (length,) = _struct_I.unpack(str[start:end])
06470 start = end
06471 end += length
06472 if python3:
06473 _v482.frame_id = str[start:end].decode('utf-8')
06474 else:
06475 _v482.frame_id = str[start:end]
06476 start = end
06477 end += 4
06478 (length,) = _struct_I.unpack(str[start:end])
06479 _v481.points = []
06480 for i in range(0, length):
06481 val4 = geometry_msgs.msg.Point32()
06482 _x = val4
06483 start = end
06484 end += 12
06485 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
06486 _v481.points.append(val4)
06487 start = end
06488 end += 4
06489 (length,) = _struct_I.unpack(str[start:end])
06490 _v481.channels = []
06491 for i in range(0, length):
06492 val4 = sensor_msgs.msg.ChannelFloat32()
06493 start = end
06494 end += 4
06495 (length,) = _struct_I.unpack(str[start:end])
06496 start = end
06497 end += length
06498 if python3:
06499 val4.name = str[start:end].decode('utf-8')
06500 else:
06501 val4.name = str[start:end]
06502 start = end
06503 end += 4
06504 (length,) = _struct_I.unpack(str[start:end])
06505 pattern = '<%sf'%length
06506 start = end
06507 end += struct.calcsize(pattern)
06508 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
06509 _v481.channels.append(val4)
06510 _v484 = val2.region
06511 _v485 = _v484.cloud
06512 _v486 = _v485.header
06513 start = end
06514 end += 4
06515 (_v486.seq,) = _struct_I.unpack(str[start:end])
06516 _v487 = _v486.stamp
06517 _x = _v487
06518 start = end
06519 end += 8
06520 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06521 start = end
06522 end += 4
06523 (length,) = _struct_I.unpack(str[start:end])
06524 start = end
06525 end += length
06526 if python3:
06527 _v486.frame_id = str[start:end].decode('utf-8')
06528 else:
06529 _v486.frame_id = str[start:end]
06530 _x = _v485
06531 start = end
06532 end += 8
06533 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
06534 start = end
06535 end += 4
06536 (length,) = _struct_I.unpack(str[start:end])
06537 _v485.fields = []
06538 for i in range(0, length):
06539 val5 = sensor_msgs.msg.PointField()
06540 start = end
06541 end += 4
06542 (length,) = _struct_I.unpack(str[start:end])
06543 start = end
06544 end += length
06545 if python3:
06546 val5.name = str[start:end].decode('utf-8')
06547 else:
06548 val5.name = str[start:end]
06549 _x = val5
06550 start = end
06551 end += 9
06552 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
06553 _v485.fields.append(val5)
06554 _x = _v485
06555 start = end
06556 end += 9
06557 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
06558 _v485.is_bigendian = bool(_v485.is_bigendian)
06559 start = end
06560 end += 4
06561 (length,) = _struct_I.unpack(str[start:end])
06562 start = end
06563 end += length
06564 if python3:
06565 _v485.data = str[start:end].decode('utf-8')
06566 else:
06567 _v485.data = str[start:end]
06568 start = end
06569 end += 1
06570 (_v485.is_dense,) = _struct_B.unpack(str[start:end])
06571 _v485.is_dense = bool(_v485.is_dense)
06572 start = end
06573 end += 4
06574 (length,) = _struct_I.unpack(str[start:end])
06575 pattern = '<%si'%length
06576 start = end
06577 end += struct.calcsize(pattern)
06578 _v484.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
06579 _v488 = _v484.image
06580 _v489 = _v488.header
06581 start = end
06582 end += 4
06583 (_v489.seq,) = _struct_I.unpack(str[start:end])
06584 _v490 = _v489.stamp
06585 _x = _v490
06586 start = end
06587 end += 8
06588 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06589 start = end
06590 end += 4
06591 (length,) = _struct_I.unpack(str[start:end])
06592 start = end
06593 end += length
06594 if python3:
06595 _v489.frame_id = str[start:end].decode('utf-8')
06596 else:
06597 _v489.frame_id = str[start:end]
06598 _x = _v488
06599 start = end
06600 end += 8
06601 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
06602 start = end
06603 end += 4
06604 (length,) = _struct_I.unpack(str[start:end])
06605 start = end
06606 end += length
06607 if python3:
06608 _v488.encoding = str[start:end].decode('utf-8')
06609 else:
06610 _v488.encoding = str[start:end]
06611 _x = _v488
06612 start = end
06613 end += 5
06614 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
06615 start = end
06616 end += 4
06617 (length,) = _struct_I.unpack(str[start:end])
06618 start = end
06619 end += length
06620 if python3:
06621 _v488.data = str[start:end].decode('utf-8')
06622 else:
06623 _v488.data = str[start:end]
06624 _v491 = _v484.disparity_image
06625 _v492 = _v491.header
06626 start = end
06627 end += 4
06628 (_v492.seq,) = _struct_I.unpack(str[start:end])
06629 _v493 = _v492.stamp
06630 _x = _v493
06631 start = end
06632 end += 8
06633 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06634 start = end
06635 end += 4
06636 (length,) = _struct_I.unpack(str[start:end])
06637 start = end
06638 end += length
06639 if python3:
06640 _v492.frame_id = str[start:end].decode('utf-8')
06641 else:
06642 _v492.frame_id = str[start:end]
06643 _x = _v491
06644 start = end
06645 end += 8
06646 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
06647 start = end
06648 end += 4
06649 (length,) = _struct_I.unpack(str[start:end])
06650 start = end
06651 end += length
06652 if python3:
06653 _v491.encoding = str[start:end].decode('utf-8')
06654 else:
06655 _v491.encoding = str[start:end]
06656 _x = _v491
06657 start = end
06658 end += 5
06659 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
06660 start = end
06661 end += 4
06662 (length,) = _struct_I.unpack(str[start:end])
06663 start = end
06664 end += length
06665 if python3:
06666 _v491.data = str[start:end].decode('utf-8')
06667 else:
06668 _v491.data = str[start:end]
06669 _v494 = _v484.cam_info
06670 _v495 = _v494.header
06671 start = end
06672 end += 4
06673 (_v495.seq,) = _struct_I.unpack(str[start:end])
06674 _v496 = _v495.stamp
06675 _x = _v496
06676 start = end
06677 end += 8
06678 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06679 start = end
06680 end += 4
06681 (length,) = _struct_I.unpack(str[start:end])
06682 start = end
06683 end += length
06684 if python3:
06685 _v495.frame_id = str[start:end].decode('utf-8')
06686 else:
06687 _v495.frame_id = str[start:end]
06688 _x = _v494
06689 start = end
06690 end += 8
06691 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
06692 start = end
06693 end += 4
06694 (length,) = _struct_I.unpack(str[start:end])
06695 start = end
06696 end += length
06697 if python3:
06698 _v494.distortion_model = str[start:end].decode('utf-8')
06699 else:
06700 _v494.distortion_model = str[start:end]
06701 start = end
06702 end += 4
06703 (length,) = _struct_I.unpack(str[start:end])
06704 pattern = '<%sd'%length
06705 start = end
06706 end += struct.calcsize(pattern)
06707 _v494.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
06708 start = end
06709 end += 72
06710 _v494.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
06711 start = end
06712 end += 72
06713 _v494.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
06714 start = end
06715 end += 96
06716 _v494.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
06717 _x = _v494
06718 start = end
06719 end += 8
06720 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
06721 _v497 = _v494.roi
06722 _x = _v497
06723 start = end
06724 end += 17
06725 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
06726 _v497.do_rectify = bool(_v497.do_rectify)
06727 _v498 = _v484.roi_box_pose
06728 _v499 = _v498.header
06729 start = end
06730 end += 4
06731 (_v499.seq,) = _struct_I.unpack(str[start:end])
06732 _v500 = _v499.stamp
06733 _x = _v500
06734 start = end
06735 end += 8
06736 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06737 start = end
06738 end += 4
06739 (length,) = _struct_I.unpack(str[start:end])
06740 start = end
06741 end += length
06742 if python3:
06743 _v499.frame_id = str[start:end].decode('utf-8')
06744 else:
06745 _v499.frame_id = str[start:end]
06746 _v501 = _v498.pose
06747 _v502 = _v501.position
06748 _x = _v502
06749 start = end
06750 end += 24
06751 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
06752 _v503 = _v501.orientation
06753 _x = _v503
06754 start = end
06755 end += 32
06756 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
06757 _v504 = _v484.roi_box_dims
06758 _x = _v504
06759 start = end
06760 end += 24
06761 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
06762 start = end
06763 end += 4
06764 (length,) = _struct_I.unpack(str[start:end])
06765 start = end
06766 end += length
06767 if python3:
06768 val2.collision_name = str[start:end].decode('utf-8')
06769 else:
06770 val2.collision_name = str[start:end]
06771 val1.moved_obstacles.append(val2)
06772 self.action_goal.goal.grasps_to_evaluate.append(val1)
06773 start = end
06774 end += 4
06775 (length,) = _struct_I.unpack(str[start:end])
06776 self.action_goal.goal.movable_obstacles = []
06777 for i in range(0, length):
06778 val1 = object_manipulation_msgs.msg.GraspableObject()
06779 start = end
06780 end += 4
06781 (length,) = _struct_I.unpack(str[start:end])
06782 start = end
06783 end += length
06784 if python3:
06785 val1.reference_frame_id = str[start:end].decode('utf-8')
06786 else:
06787 val1.reference_frame_id = str[start:end]
06788 start = end
06789 end += 4
06790 (length,) = _struct_I.unpack(str[start:end])
06791 val1.potential_models = []
06792 for i in range(0, length):
06793 val2 = household_objects_database_msgs.msg.DatabaseModelPose()
06794 start = end
06795 end += 4
06796 (val2.model_id,) = _struct_i.unpack(str[start:end])
06797 _v505 = val2.pose
06798 _v506 = _v505.header
06799 start = end
06800 end += 4
06801 (_v506.seq,) = _struct_I.unpack(str[start:end])
06802 _v507 = _v506.stamp
06803 _x = _v507
06804 start = end
06805 end += 8
06806 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06807 start = end
06808 end += 4
06809 (length,) = _struct_I.unpack(str[start:end])
06810 start = end
06811 end += length
06812 if python3:
06813 _v506.frame_id = str[start:end].decode('utf-8')
06814 else:
06815 _v506.frame_id = str[start:end]
06816 _v508 = _v505.pose
06817 _v509 = _v508.position
06818 _x = _v509
06819 start = end
06820 end += 24
06821 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
06822 _v510 = _v508.orientation
06823 _x = _v510
06824 start = end
06825 end += 32
06826 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
06827 start = end
06828 end += 4
06829 (val2.confidence,) = _struct_f.unpack(str[start:end])
06830 start = end
06831 end += 4
06832 (length,) = _struct_I.unpack(str[start:end])
06833 start = end
06834 end += length
06835 if python3:
06836 val2.detector_name = str[start:end].decode('utf-8')
06837 else:
06838 val2.detector_name = str[start:end]
06839 val1.potential_models.append(val2)
06840 _v511 = val1.cluster
06841 _v512 = _v511.header
06842 start = end
06843 end += 4
06844 (_v512.seq,) = _struct_I.unpack(str[start:end])
06845 _v513 = _v512.stamp
06846 _x = _v513
06847 start = end
06848 end += 8
06849 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06850 start = end
06851 end += 4
06852 (length,) = _struct_I.unpack(str[start:end])
06853 start = end
06854 end += length
06855 if python3:
06856 _v512.frame_id = str[start:end].decode('utf-8')
06857 else:
06858 _v512.frame_id = str[start:end]
06859 start = end
06860 end += 4
06861 (length,) = _struct_I.unpack(str[start:end])
06862 _v511.points = []
06863 for i in range(0, length):
06864 val3 = geometry_msgs.msg.Point32()
06865 _x = val3
06866 start = end
06867 end += 12
06868 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
06869 _v511.points.append(val3)
06870 start = end
06871 end += 4
06872 (length,) = _struct_I.unpack(str[start:end])
06873 _v511.channels = []
06874 for i in range(0, length):
06875 val3 = sensor_msgs.msg.ChannelFloat32()
06876 start = end
06877 end += 4
06878 (length,) = _struct_I.unpack(str[start:end])
06879 start = end
06880 end += length
06881 if python3:
06882 val3.name = str[start:end].decode('utf-8')
06883 else:
06884 val3.name = str[start:end]
06885 start = end
06886 end += 4
06887 (length,) = _struct_I.unpack(str[start:end])
06888 pattern = '<%sf'%length
06889 start = end
06890 end += struct.calcsize(pattern)
06891 val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
06892 _v511.channels.append(val3)
06893 _v514 = val1.region
06894 _v515 = _v514.cloud
06895 _v516 = _v515.header
06896 start = end
06897 end += 4
06898 (_v516.seq,) = _struct_I.unpack(str[start:end])
06899 _v517 = _v516.stamp
06900 _x = _v517
06901 start = end
06902 end += 8
06903 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06904 start = end
06905 end += 4
06906 (length,) = _struct_I.unpack(str[start:end])
06907 start = end
06908 end += length
06909 if python3:
06910 _v516.frame_id = str[start:end].decode('utf-8')
06911 else:
06912 _v516.frame_id = str[start:end]
06913 _x = _v515
06914 start = end
06915 end += 8
06916 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
06917 start = end
06918 end += 4
06919 (length,) = _struct_I.unpack(str[start:end])
06920 _v515.fields = []
06921 for i in range(0, length):
06922 val4 = sensor_msgs.msg.PointField()
06923 start = end
06924 end += 4
06925 (length,) = _struct_I.unpack(str[start:end])
06926 start = end
06927 end += length
06928 if python3:
06929 val4.name = str[start:end].decode('utf-8')
06930 else:
06931 val4.name = str[start:end]
06932 _x = val4
06933 start = end
06934 end += 9
06935 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
06936 _v515.fields.append(val4)
06937 _x = _v515
06938 start = end
06939 end += 9
06940 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
06941 _v515.is_bigendian = bool(_v515.is_bigendian)
06942 start = end
06943 end += 4
06944 (length,) = _struct_I.unpack(str[start:end])
06945 start = end
06946 end += length
06947 if python3:
06948 _v515.data = str[start:end].decode('utf-8')
06949 else:
06950 _v515.data = str[start:end]
06951 start = end
06952 end += 1
06953 (_v515.is_dense,) = _struct_B.unpack(str[start:end])
06954 _v515.is_dense = bool(_v515.is_dense)
06955 start = end
06956 end += 4
06957 (length,) = _struct_I.unpack(str[start:end])
06958 pattern = '<%si'%length
06959 start = end
06960 end += struct.calcsize(pattern)
06961 _v514.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
06962 _v518 = _v514.image
06963 _v519 = _v518.header
06964 start = end
06965 end += 4
06966 (_v519.seq,) = _struct_I.unpack(str[start:end])
06967 _v520 = _v519.stamp
06968 _x = _v520
06969 start = end
06970 end += 8
06971 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
06972 start = end
06973 end += 4
06974 (length,) = _struct_I.unpack(str[start:end])
06975 start = end
06976 end += length
06977 if python3:
06978 _v519.frame_id = str[start:end].decode('utf-8')
06979 else:
06980 _v519.frame_id = str[start:end]
06981 _x = _v518
06982 start = end
06983 end += 8
06984 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
06985 start = end
06986 end += 4
06987 (length,) = _struct_I.unpack(str[start:end])
06988 start = end
06989 end += length
06990 if python3:
06991 _v518.encoding = str[start:end].decode('utf-8')
06992 else:
06993 _v518.encoding = str[start:end]
06994 _x = _v518
06995 start = end
06996 end += 5
06997 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
06998 start = end
06999 end += 4
07000 (length,) = _struct_I.unpack(str[start:end])
07001 start = end
07002 end += length
07003 if python3:
07004 _v518.data = str[start:end].decode('utf-8')
07005 else:
07006 _v518.data = str[start:end]
07007 _v521 = _v514.disparity_image
07008 _v522 = _v521.header
07009 start = end
07010 end += 4
07011 (_v522.seq,) = _struct_I.unpack(str[start:end])
07012 _v523 = _v522.stamp
07013 _x = _v523
07014 start = end
07015 end += 8
07016 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07017 start = end
07018 end += 4
07019 (length,) = _struct_I.unpack(str[start:end])
07020 start = end
07021 end += length
07022 if python3:
07023 _v522.frame_id = str[start:end].decode('utf-8')
07024 else:
07025 _v522.frame_id = str[start:end]
07026 _x = _v521
07027 start = end
07028 end += 8
07029 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
07030 start = end
07031 end += 4
07032 (length,) = _struct_I.unpack(str[start:end])
07033 start = end
07034 end += length
07035 if python3:
07036 _v521.encoding = str[start:end].decode('utf-8')
07037 else:
07038 _v521.encoding = str[start:end]
07039 _x = _v521
07040 start = end
07041 end += 5
07042 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
07043 start = end
07044 end += 4
07045 (length,) = _struct_I.unpack(str[start:end])
07046 start = end
07047 end += length
07048 if python3:
07049 _v521.data = str[start:end].decode('utf-8')
07050 else:
07051 _v521.data = str[start:end]
07052 _v524 = _v514.cam_info
07053 _v525 = _v524.header
07054 start = end
07055 end += 4
07056 (_v525.seq,) = _struct_I.unpack(str[start:end])
07057 _v526 = _v525.stamp
07058 _x = _v526
07059 start = end
07060 end += 8
07061 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07062 start = end
07063 end += 4
07064 (length,) = _struct_I.unpack(str[start:end])
07065 start = end
07066 end += length
07067 if python3:
07068 _v525.frame_id = str[start:end].decode('utf-8')
07069 else:
07070 _v525.frame_id = str[start:end]
07071 _x = _v524
07072 start = end
07073 end += 8
07074 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
07075 start = end
07076 end += 4
07077 (length,) = _struct_I.unpack(str[start:end])
07078 start = end
07079 end += length
07080 if python3:
07081 _v524.distortion_model = str[start:end].decode('utf-8')
07082 else:
07083 _v524.distortion_model = str[start:end]
07084 start = end
07085 end += 4
07086 (length,) = _struct_I.unpack(str[start:end])
07087 pattern = '<%sd'%length
07088 start = end
07089 end += struct.calcsize(pattern)
07090 _v524.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07091 start = end
07092 end += 72
07093 _v524.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
07094 start = end
07095 end += 72
07096 _v524.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
07097 start = end
07098 end += 96
07099 _v524.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
07100 _x = _v524
07101 start = end
07102 end += 8
07103 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
07104 _v527 = _v524.roi
07105 _x = _v527
07106 start = end
07107 end += 17
07108 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
07109 _v527.do_rectify = bool(_v527.do_rectify)
07110 _v528 = _v514.roi_box_pose
07111 _v529 = _v528.header
07112 start = end
07113 end += 4
07114 (_v529.seq,) = _struct_I.unpack(str[start:end])
07115 _v530 = _v529.stamp
07116 _x = _v530
07117 start = end
07118 end += 8
07119 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07120 start = end
07121 end += 4
07122 (length,) = _struct_I.unpack(str[start:end])
07123 start = end
07124 end += length
07125 if python3:
07126 _v529.frame_id = str[start:end].decode('utf-8')
07127 else:
07128 _v529.frame_id = str[start:end]
07129 _v531 = _v528.pose
07130 _v532 = _v531.position
07131 _x = _v532
07132 start = end
07133 end += 24
07134 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07135 _v533 = _v531.orientation
07136 _x = _v533
07137 start = end
07138 end += 32
07139 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
07140 _v534 = _v514.roi_box_dims
07141 _x = _v534
07142 start = end
07143 end += 24
07144 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07145 start = end
07146 end += 4
07147 (length,) = _struct_I.unpack(str[start:end])
07148 start = end
07149 end += length
07150 if python3:
07151 val1.collision_name = str[start:end].decode('utf-8')
07152 else:
07153 val1.collision_name = str[start:end]
07154 self.action_goal.goal.movable_obstacles.append(val1)
07155 _x = self
07156 start = end
07157 end += 12
07158 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
07159 start = end
07160 end += 4
07161 (length,) = _struct_I.unpack(str[start:end])
07162 start = end
07163 end += length
07164 if python3:
07165 self.action_result.header.frame_id = str[start:end].decode('utf-8')
07166 else:
07167 self.action_result.header.frame_id = str[start:end]
07168 _x = self
07169 start = end
07170 end += 8
07171 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
07172 start = end
07173 end += 4
07174 (length,) = _struct_I.unpack(str[start:end])
07175 start = end
07176 end += length
07177 if python3:
07178 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
07179 else:
07180 self.action_result.status.goal_id.id = str[start:end]
07181 start = end
07182 end += 1
07183 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
07184 start = end
07185 end += 4
07186 (length,) = _struct_I.unpack(str[start:end])
07187 start = end
07188 end += length
07189 if python3:
07190 self.action_result.status.text = str[start:end].decode('utf-8')
07191 else:
07192 self.action_result.status.text = str[start:end]
07193 start = end
07194 end += 4
07195 (length,) = _struct_I.unpack(str[start:end])
07196 self.action_result.result.grasps = []
07197 for i in range(0, length):
07198 val1 = object_manipulation_msgs.msg.Grasp()
07199 _v535 = val1.pre_grasp_posture
07200 _v536 = _v535.header
07201 start = end
07202 end += 4
07203 (_v536.seq,) = _struct_I.unpack(str[start:end])
07204 _v537 = _v536.stamp
07205 _x = _v537
07206 start = end
07207 end += 8
07208 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07209 start = end
07210 end += 4
07211 (length,) = _struct_I.unpack(str[start:end])
07212 start = end
07213 end += length
07214 if python3:
07215 _v536.frame_id = str[start:end].decode('utf-8')
07216 else:
07217 _v536.frame_id = str[start:end]
07218 start = end
07219 end += 4
07220 (length,) = _struct_I.unpack(str[start:end])
07221 _v535.name = []
07222 for i in range(0, length):
07223 start = end
07224 end += 4
07225 (length,) = _struct_I.unpack(str[start:end])
07226 start = end
07227 end += length
07228 if python3:
07229 val3 = str[start:end].decode('utf-8')
07230 else:
07231 val3 = str[start:end]
07232 _v535.name.append(val3)
07233 start = end
07234 end += 4
07235 (length,) = _struct_I.unpack(str[start:end])
07236 pattern = '<%sd'%length
07237 start = end
07238 end += struct.calcsize(pattern)
07239 _v535.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07240 start = end
07241 end += 4
07242 (length,) = _struct_I.unpack(str[start:end])
07243 pattern = '<%sd'%length
07244 start = end
07245 end += struct.calcsize(pattern)
07246 _v535.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07247 start = end
07248 end += 4
07249 (length,) = _struct_I.unpack(str[start:end])
07250 pattern = '<%sd'%length
07251 start = end
07252 end += struct.calcsize(pattern)
07253 _v535.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07254 _v538 = val1.grasp_posture
07255 _v539 = _v538.header
07256 start = end
07257 end += 4
07258 (_v539.seq,) = _struct_I.unpack(str[start:end])
07259 _v540 = _v539.stamp
07260 _x = _v540
07261 start = end
07262 end += 8
07263 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07264 start = end
07265 end += 4
07266 (length,) = _struct_I.unpack(str[start:end])
07267 start = end
07268 end += length
07269 if python3:
07270 _v539.frame_id = str[start:end].decode('utf-8')
07271 else:
07272 _v539.frame_id = str[start:end]
07273 start = end
07274 end += 4
07275 (length,) = _struct_I.unpack(str[start:end])
07276 _v538.name = []
07277 for i in range(0, length):
07278 start = end
07279 end += 4
07280 (length,) = _struct_I.unpack(str[start:end])
07281 start = end
07282 end += length
07283 if python3:
07284 val3 = str[start:end].decode('utf-8')
07285 else:
07286 val3 = str[start:end]
07287 _v538.name.append(val3)
07288 start = end
07289 end += 4
07290 (length,) = _struct_I.unpack(str[start:end])
07291 pattern = '<%sd'%length
07292 start = end
07293 end += struct.calcsize(pattern)
07294 _v538.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07295 start = end
07296 end += 4
07297 (length,) = _struct_I.unpack(str[start:end])
07298 pattern = '<%sd'%length
07299 start = end
07300 end += struct.calcsize(pattern)
07301 _v538.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07302 start = end
07303 end += 4
07304 (length,) = _struct_I.unpack(str[start:end])
07305 pattern = '<%sd'%length
07306 start = end
07307 end += struct.calcsize(pattern)
07308 _v538.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07309 _v541 = val1.grasp_pose
07310 _v542 = _v541.position
07311 _x = _v542
07312 start = end
07313 end += 24
07314 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07315 _v543 = _v541.orientation
07316 _x = _v543
07317 start = end
07318 end += 32
07319 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
07320 _x = val1
07321 start = end
07322 end += 17
07323 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
07324 val1.cluster_rep = bool(val1.cluster_rep)
07325 start = end
07326 end += 4
07327 (length,) = _struct_I.unpack(str[start:end])
07328 val1.moved_obstacles = []
07329 for i in range(0, length):
07330 val2 = object_manipulation_msgs.msg.GraspableObject()
07331 start = end
07332 end += 4
07333 (length,) = _struct_I.unpack(str[start:end])
07334 start = end
07335 end += length
07336 if python3:
07337 val2.reference_frame_id = str[start:end].decode('utf-8')
07338 else:
07339 val2.reference_frame_id = str[start:end]
07340 start = end
07341 end += 4
07342 (length,) = _struct_I.unpack(str[start:end])
07343 val2.potential_models = []
07344 for i in range(0, length):
07345 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
07346 start = end
07347 end += 4
07348 (val3.model_id,) = _struct_i.unpack(str[start:end])
07349 _v544 = val3.pose
07350 _v545 = _v544.header
07351 start = end
07352 end += 4
07353 (_v545.seq,) = _struct_I.unpack(str[start:end])
07354 _v546 = _v545.stamp
07355 _x = _v546
07356 start = end
07357 end += 8
07358 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07359 start = end
07360 end += 4
07361 (length,) = _struct_I.unpack(str[start:end])
07362 start = end
07363 end += length
07364 if python3:
07365 _v545.frame_id = str[start:end].decode('utf-8')
07366 else:
07367 _v545.frame_id = str[start:end]
07368 _v547 = _v544.pose
07369 _v548 = _v547.position
07370 _x = _v548
07371 start = end
07372 end += 24
07373 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07374 _v549 = _v547.orientation
07375 _x = _v549
07376 start = end
07377 end += 32
07378 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
07379 start = end
07380 end += 4
07381 (val3.confidence,) = _struct_f.unpack(str[start:end])
07382 start = end
07383 end += 4
07384 (length,) = _struct_I.unpack(str[start:end])
07385 start = end
07386 end += length
07387 if python3:
07388 val3.detector_name = str[start:end].decode('utf-8')
07389 else:
07390 val3.detector_name = str[start:end]
07391 val2.potential_models.append(val3)
07392 _v550 = val2.cluster
07393 _v551 = _v550.header
07394 start = end
07395 end += 4
07396 (_v551.seq,) = _struct_I.unpack(str[start:end])
07397 _v552 = _v551.stamp
07398 _x = _v552
07399 start = end
07400 end += 8
07401 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07402 start = end
07403 end += 4
07404 (length,) = _struct_I.unpack(str[start:end])
07405 start = end
07406 end += length
07407 if python3:
07408 _v551.frame_id = str[start:end].decode('utf-8')
07409 else:
07410 _v551.frame_id = str[start:end]
07411 start = end
07412 end += 4
07413 (length,) = _struct_I.unpack(str[start:end])
07414 _v550.points = []
07415 for i in range(0, length):
07416 val4 = geometry_msgs.msg.Point32()
07417 _x = val4
07418 start = end
07419 end += 12
07420 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
07421 _v550.points.append(val4)
07422 start = end
07423 end += 4
07424 (length,) = _struct_I.unpack(str[start:end])
07425 _v550.channels = []
07426 for i in range(0, length):
07427 val4 = sensor_msgs.msg.ChannelFloat32()
07428 start = end
07429 end += 4
07430 (length,) = _struct_I.unpack(str[start:end])
07431 start = end
07432 end += length
07433 if python3:
07434 val4.name = str[start:end].decode('utf-8')
07435 else:
07436 val4.name = str[start:end]
07437 start = end
07438 end += 4
07439 (length,) = _struct_I.unpack(str[start:end])
07440 pattern = '<%sf'%length
07441 start = end
07442 end += struct.calcsize(pattern)
07443 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
07444 _v550.channels.append(val4)
07445 _v553 = val2.region
07446 _v554 = _v553.cloud
07447 _v555 = _v554.header
07448 start = end
07449 end += 4
07450 (_v555.seq,) = _struct_I.unpack(str[start:end])
07451 _v556 = _v555.stamp
07452 _x = _v556
07453 start = end
07454 end += 8
07455 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07456 start = end
07457 end += 4
07458 (length,) = _struct_I.unpack(str[start:end])
07459 start = end
07460 end += length
07461 if python3:
07462 _v555.frame_id = str[start:end].decode('utf-8')
07463 else:
07464 _v555.frame_id = str[start:end]
07465 _x = _v554
07466 start = end
07467 end += 8
07468 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
07469 start = end
07470 end += 4
07471 (length,) = _struct_I.unpack(str[start:end])
07472 _v554.fields = []
07473 for i in range(0, length):
07474 val5 = sensor_msgs.msg.PointField()
07475 start = end
07476 end += 4
07477 (length,) = _struct_I.unpack(str[start:end])
07478 start = end
07479 end += length
07480 if python3:
07481 val5.name = str[start:end].decode('utf-8')
07482 else:
07483 val5.name = str[start:end]
07484 _x = val5
07485 start = end
07486 end += 9
07487 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
07488 _v554.fields.append(val5)
07489 _x = _v554
07490 start = end
07491 end += 9
07492 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
07493 _v554.is_bigendian = bool(_v554.is_bigendian)
07494 start = end
07495 end += 4
07496 (length,) = _struct_I.unpack(str[start:end])
07497 start = end
07498 end += length
07499 if python3:
07500 _v554.data = str[start:end].decode('utf-8')
07501 else:
07502 _v554.data = str[start:end]
07503 start = end
07504 end += 1
07505 (_v554.is_dense,) = _struct_B.unpack(str[start:end])
07506 _v554.is_dense = bool(_v554.is_dense)
07507 start = end
07508 end += 4
07509 (length,) = _struct_I.unpack(str[start:end])
07510 pattern = '<%si'%length
07511 start = end
07512 end += struct.calcsize(pattern)
07513 _v553.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
07514 _v557 = _v553.image
07515 _v558 = _v557.header
07516 start = end
07517 end += 4
07518 (_v558.seq,) = _struct_I.unpack(str[start:end])
07519 _v559 = _v558.stamp
07520 _x = _v559
07521 start = end
07522 end += 8
07523 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07524 start = end
07525 end += 4
07526 (length,) = _struct_I.unpack(str[start:end])
07527 start = end
07528 end += length
07529 if python3:
07530 _v558.frame_id = str[start:end].decode('utf-8')
07531 else:
07532 _v558.frame_id = str[start:end]
07533 _x = _v557
07534 start = end
07535 end += 8
07536 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
07537 start = end
07538 end += 4
07539 (length,) = _struct_I.unpack(str[start:end])
07540 start = end
07541 end += length
07542 if python3:
07543 _v557.encoding = str[start:end].decode('utf-8')
07544 else:
07545 _v557.encoding = str[start:end]
07546 _x = _v557
07547 start = end
07548 end += 5
07549 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
07550 start = end
07551 end += 4
07552 (length,) = _struct_I.unpack(str[start:end])
07553 start = end
07554 end += length
07555 if python3:
07556 _v557.data = str[start:end].decode('utf-8')
07557 else:
07558 _v557.data = str[start:end]
07559 _v560 = _v553.disparity_image
07560 _v561 = _v560.header
07561 start = end
07562 end += 4
07563 (_v561.seq,) = _struct_I.unpack(str[start:end])
07564 _v562 = _v561.stamp
07565 _x = _v562
07566 start = end
07567 end += 8
07568 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07569 start = end
07570 end += 4
07571 (length,) = _struct_I.unpack(str[start:end])
07572 start = end
07573 end += length
07574 if python3:
07575 _v561.frame_id = str[start:end].decode('utf-8')
07576 else:
07577 _v561.frame_id = str[start:end]
07578 _x = _v560
07579 start = end
07580 end += 8
07581 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
07582 start = end
07583 end += 4
07584 (length,) = _struct_I.unpack(str[start:end])
07585 start = end
07586 end += length
07587 if python3:
07588 _v560.encoding = str[start:end].decode('utf-8')
07589 else:
07590 _v560.encoding = str[start:end]
07591 _x = _v560
07592 start = end
07593 end += 5
07594 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
07595 start = end
07596 end += 4
07597 (length,) = _struct_I.unpack(str[start:end])
07598 start = end
07599 end += length
07600 if python3:
07601 _v560.data = str[start:end].decode('utf-8')
07602 else:
07603 _v560.data = str[start:end]
07604 _v563 = _v553.cam_info
07605 _v564 = _v563.header
07606 start = end
07607 end += 4
07608 (_v564.seq,) = _struct_I.unpack(str[start:end])
07609 _v565 = _v564.stamp
07610 _x = _v565
07611 start = end
07612 end += 8
07613 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07614 start = end
07615 end += 4
07616 (length,) = _struct_I.unpack(str[start:end])
07617 start = end
07618 end += length
07619 if python3:
07620 _v564.frame_id = str[start:end].decode('utf-8')
07621 else:
07622 _v564.frame_id = str[start:end]
07623 _x = _v563
07624 start = end
07625 end += 8
07626 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
07627 start = end
07628 end += 4
07629 (length,) = _struct_I.unpack(str[start:end])
07630 start = end
07631 end += length
07632 if python3:
07633 _v563.distortion_model = str[start:end].decode('utf-8')
07634 else:
07635 _v563.distortion_model = str[start:end]
07636 start = end
07637 end += 4
07638 (length,) = _struct_I.unpack(str[start:end])
07639 pattern = '<%sd'%length
07640 start = end
07641 end += struct.calcsize(pattern)
07642 _v563.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07643 start = end
07644 end += 72
07645 _v563.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
07646 start = end
07647 end += 72
07648 _v563.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
07649 start = end
07650 end += 96
07651 _v563.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
07652 _x = _v563
07653 start = end
07654 end += 8
07655 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
07656 _v566 = _v563.roi
07657 _x = _v566
07658 start = end
07659 end += 17
07660 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
07661 _v566.do_rectify = bool(_v566.do_rectify)
07662 _v567 = _v553.roi_box_pose
07663 _v568 = _v567.header
07664 start = end
07665 end += 4
07666 (_v568.seq,) = _struct_I.unpack(str[start:end])
07667 _v569 = _v568.stamp
07668 _x = _v569
07669 start = end
07670 end += 8
07671 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07672 start = end
07673 end += 4
07674 (length,) = _struct_I.unpack(str[start:end])
07675 start = end
07676 end += length
07677 if python3:
07678 _v568.frame_id = str[start:end].decode('utf-8')
07679 else:
07680 _v568.frame_id = str[start:end]
07681 _v570 = _v567.pose
07682 _v571 = _v570.position
07683 _x = _v571
07684 start = end
07685 end += 24
07686 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07687 _v572 = _v570.orientation
07688 _x = _v572
07689 start = end
07690 end += 32
07691 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
07692 _v573 = _v553.roi_box_dims
07693 _x = _v573
07694 start = end
07695 end += 24
07696 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07697 start = end
07698 end += 4
07699 (length,) = _struct_I.unpack(str[start:end])
07700 start = end
07701 end += length
07702 if python3:
07703 val2.collision_name = str[start:end].decode('utf-8')
07704 else:
07705 val2.collision_name = str[start:end]
07706 val1.moved_obstacles.append(val2)
07707 self.action_result.result.grasps.append(val1)
07708 _x = self
07709 start = end
07710 end += 16
07711 (_x.action_result.result.error_code.value, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_i3I.unpack(str[start:end])
07712 start = end
07713 end += 4
07714 (length,) = _struct_I.unpack(str[start:end])
07715 start = end
07716 end += length
07717 if python3:
07718 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
07719 else:
07720 self.action_feedback.header.frame_id = str[start:end]
07721 _x = self
07722 start = end
07723 end += 8
07724 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
07725 start = end
07726 end += 4
07727 (length,) = _struct_I.unpack(str[start:end])
07728 start = end
07729 end += length
07730 if python3:
07731 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
07732 else:
07733 self.action_feedback.status.goal_id.id = str[start:end]
07734 start = end
07735 end += 1
07736 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
07737 start = end
07738 end += 4
07739 (length,) = _struct_I.unpack(str[start:end])
07740 start = end
07741 end += length
07742 if python3:
07743 self.action_feedback.status.text = str[start:end].decode('utf-8')
07744 else:
07745 self.action_feedback.status.text = str[start:end]
07746 start = end
07747 end += 4
07748 (length,) = _struct_I.unpack(str[start:end])
07749 self.action_feedback.feedback.grasps = []
07750 for i in range(0, length):
07751 val1 = object_manipulation_msgs.msg.Grasp()
07752 _v574 = val1.pre_grasp_posture
07753 _v575 = _v574.header
07754 start = end
07755 end += 4
07756 (_v575.seq,) = _struct_I.unpack(str[start:end])
07757 _v576 = _v575.stamp
07758 _x = _v576
07759 start = end
07760 end += 8
07761 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07762 start = end
07763 end += 4
07764 (length,) = _struct_I.unpack(str[start:end])
07765 start = end
07766 end += length
07767 if python3:
07768 _v575.frame_id = str[start:end].decode('utf-8')
07769 else:
07770 _v575.frame_id = str[start:end]
07771 start = end
07772 end += 4
07773 (length,) = _struct_I.unpack(str[start:end])
07774 _v574.name = []
07775 for i in range(0, length):
07776 start = end
07777 end += 4
07778 (length,) = _struct_I.unpack(str[start:end])
07779 start = end
07780 end += length
07781 if python3:
07782 val3 = str[start:end].decode('utf-8')
07783 else:
07784 val3 = str[start:end]
07785 _v574.name.append(val3)
07786 start = end
07787 end += 4
07788 (length,) = _struct_I.unpack(str[start:end])
07789 pattern = '<%sd'%length
07790 start = end
07791 end += struct.calcsize(pattern)
07792 _v574.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07793 start = end
07794 end += 4
07795 (length,) = _struct_I.unpack(str[start:end])
07796 pattern = '<%sd'%length
07797 start = end
07798 end += struct.calcsize(pattern)
07799 _v574.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07800 start = end
07801 end += 4
07802 (length,) = _struct_I.unpack(str[start:end])
07803 pattern = '<%sd'%length
07804 start = end
07805 end += struct.calcsize(pattern)
07806 _v574.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07807 _v577 = val1.grasp_posture
07808 _v578 = _v577.header
07809 start = end
07810 end += 4
07811 (_v578.seq,) = _struct_I.unpack(str[start:end])
07812 _v579 = _v578.stamp
07813 _x = _v579
07814 start = end
07815 end += 8
07816 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07817 start = end
07818 end += 4
07819 (length,) = _struct_I.unpack(str[start:end])
07820 start = end
07821 end += length
07822 if python3:
07823 _v578.frame_id = str[start:end].decode('utf-8')
07824 else:
07825 _v578.frame_id = str[start:end]
07826 start = end
07827 end += 4
07828 (length,) = _struct_I.unpack(str[start:end])
07829 _v577.name = []
07830 for i in range(0, length):
07831 start = end
07832 end += 4
07833 (length,) = _struct_I.unpack(str[start:end])
07834 start = end
07835 end += length
07836 if python3:
07837 val3 = str[start:end].decode('utf-8')
07838 else:
07839 val3 = str[start:end]
07840 _v577.name.append(val3)
07841 start = end
07842 end += 4
07843 (length,) = _struct_I.unpack(str[start:end])
07844 pattern = '<%sd'%length
07845 start = end
07846 end += struct.calcsize(pattern)
07847 _v577.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07848 start = end
07849 end += 4
07850 (length,) = _struct_I.unpack(str[start:end])
07851 pattern = '<%sd'%length
07852 start = end
07853 end += struct.calcsize(pattern)
07854 _v577.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07855 start = end
07856 end += 4
07857 (length,) = _struct_I.unpack(str[start:end])
07858 pattern = '<%sd'%length
07859 start = end
07860 end += struct.calcsize(pattern)
07861 _v577.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
07862 _v580 = val1.grasp_pose
07863 _v581 = _v580.position
07864 _x = _v581
07865 start = end
07866 end += 24
07867 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07868 _v582 = _v580.orientation
07869 _x = _v582
07870 start = end
07871 end += 32
07872 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
07873 _x = val1
07874 start = end
07875 end += 17
07876 (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
07877 val1.cluster_rep = bool(val1.cluster_rep)
07878 start = end
07879 end += 4
07880 (length,) = _struct_I.unpack(str[start:end])
07881 val1.moved_obstacles = []
07882 for i in range(0, length):
07883 val2 = object_manipulation_msgs.msg.GraspableObject()
07884 start = end
07885 end += 4
07886 (length,) = _struct_I.unpack(str[start:end])
07887 start = end
07888 end += length
07889 if python3:
07890 val2.reference_frame_id = str[start:end].decode('utf-8')
07891 else:
07892 val2.reference_frame_id = str[start:end]
07893 start = end
07894 end += 4
07895 (length,) = _struct_I.unpack(str[start:end])
07896 val2.potential_models = []
07897 for i in range(0, length):
07898 val3 = household_objects_database_msgs.msg.DatabaseModelPose()
07899 start = end
07900 end += 4
07901 (val3.model_id,) = _struct_i.unpack(str[start:end])
07902 _v583 = val3.pose
07903 _v584 = _v583.header
07904 start = end
07905 end += 4
07906 (_v584.seq,) = _struct_I.unpack(str[start:end])
07907 _v585 = _v584.stamp
07908 _x = _v585
07909 start = end
07910 end += 8
07911 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07912 start = end
07913 end += 4
07914 (length,) = _struct_I.unpack(str[start:end])
07915 start = end
07916 end += length
07917 if python3:
07918 _v584.frame_id = str[start:end].decode('utf-8')
07919 else:
07920 _v584.frame_id = str[start:end]
07921 _v586 = _v583.pose
07922 _v587 = _v586.position
07923 _x = _v587
07924 start = end
07925 end += 24
07926 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
07927 _v588 = _v586.orientation
07928 _x = _v588
07929 start = end
07930 end += 32
07931 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
07932 start = end
07933 end += 4
07934 (val3.confidence,) = _struct_f.unpack(str[start:end])
07935 start = end
07936 end += 4
07937 (length,) = _struct_I.unpack(str[start:end])
07938 start = end
07939 end += length
07940 if python3:
07941 val3.detector_name = str[start:end].decode('utf-8')
07942 else:
07943 val3.detector_name = str[start:end]
07944 val2.potential_models.append(val3)
07945 _v589 = val2.cluster
07946 _v590 = _v589.header
07947 start = end
07948 end += 4
07949 (_v590.seq,) = _struct_I.unpack(str[start:end])
07950 _v591 = _v590.stamp
07951 _x = _v591
07952 start = end
07953 end += 8
07954 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
07955 start = end
07956 end += 4
07957 (length,) = _struct_I.unpack(str[start:end])
07958 start = end
07959 end += length
07960 if python3:
07961 _v590.frame_id = str[start:end].decode('utf-8')
07962 else:
07963 _v590.frame_id = str[start:end]
07964 start = end
07965 end += 4
07966 (length,) = _struct_I.unpack(str[start:end])
07967 _v589.points = []
07968 for i in range(0, length):
07969 val4 = geometry_msgs.msg.Point32()
07970 _x = val4
07971 start = end
07972 end += 12
07973 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
07974 _v589.points.append(val4)
07975 start = end
07976 end += 4
07977 (length,) = _struct_I.unpack(str[start:end])
07978 _v589.channels = []
07979 for i in range(0, length):
07980 val4 = sensor_msgs.msg.ChannelFloat32()
07981 start = end
07982 end += 4
07983 (length,) = _struct_I.unpack(str[start:end])
07984 start = end
07985 end += length
07986 if python3:
07987 val4.name = str[start:end].decode('utf-8')
07988 else:
07989 val4.name = str[start:end]
07990 start = end
07991 end += 4
07992 (length,) = _struct_I.unpack(str[start:end])
07993 pattern = '<%sf'%length
07994 start = end
07995 end += struct.calcsize(pattern)
07996 val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
07997 _v589.channels.append(val4)
07998 _v592 = val2.region
07999 _v593 = _v592.cloud
08000 _v594 = _v593.header
08001 start = end
08002 end += 4
08003 (_v594.seq,) = _struct_I.unpack(str[start:end])
08004 _v595 = _v594.stamp
08005 _x = _v595
08006 start = end
08007 end += 8
08008 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
08009 start = end
08010 end += 4
08011 (length,) = _struct_I.unpack(str[start:end])
08012 start = end
08013 end += length
08014 if python3:
08015 _v594.frame_id = str[start:end].decode('utf-8')
08016 else:
08017 _v594.frame_id = str[start:end]
08018 _x = _v593
08019 start = end
08020 end += 8
08021 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
08022 start = end
08023 end += 4
08024 (length,) = _struct_I.unpack(str[start:end])
08025 _v593.fields = []
08026 for i in range(0, length):
08027 val5 = sensor_msgs.msg.PointField()
08028 start = end
08029 end += 4
08030 (length,) = _struct_I.unpack(str[start:end])
08031 start = end
08032 end += length
08033 if python3:
08034 val5.name = str[start:end].decode('utf-8')
08035 else:
08036 val5.name = str[start:end]
08037 _x = val5
08038 start = end
08039 end += 9
08040 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
08041 _v593.fields.append(val5)
08042 _x = _v593
08043 start = end
08044 end += 9
08045 (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
08046 _v593.is_bigendian = bool(_v593.is_bigendian)
08047 start = end
08048 end += 4
08049 (length,) = _struct_I.unpack(str[start:end])
08050 start = end
08051 end += length
08052 if python3:
08053 _v593.data = str[start:end].decode('utf-8')
08054 else:
08055 _v593.data = str[start:end]
08056 start = end
08057 end += 1
08058 (_v593.is_dense,) = _struct_B.unpack(str[start:end])
08059 _v593.is_dense = bool(_v593.is_dense)
08060 start = end
08061 end += 4
08062 (length,) = _struct_I.unpack(str[start:end])
08063 pattern = '<%si'%length
08064 start = end
08065 end += struct.calcsize(pattern)
08066 _v592.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
08067 _v596 = _v592.image
08068 _v597 = _v596.header
08069 start = end
08070 end += 4
08071 (_v597.seq,) = _struct_I.unpack(str[start:end])
08072 _v598 = _v597.stamp
08073 _x = _v598
08074 start = end
08075 end += 8
08076 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
08077 start = end
08078 end += 4
08079 (length,) = _struct_I.unpack(str[start:end])
08080 start = end
08081 end += length
08082 if python3:
08083 _v597.frame_id = str[start:end].decode('utf-8')
08084 else:
08085 _v597.frame_id = str[start:end]
08086 _x = _v596
08087 start = end
08088 end += 8
08089 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
08090 start = end
08091 end += 4
08092 (length,) = _struct_I.unpack(str[start:end])
08093 start = end
08094 end += length
08095 if python3:
08096 _v596.encoding = str[start:end].decode('utf-8')
08097 else:
08098 _v596.encoding = str[start:end]
08099 _x = _v596
08100 start = end
08101 end += 5
08102 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
08103 start = end
08104 end += 4
08105 (length,) = _struct_I.unpack(str[start:end])
08106 start = end
08107 end += length
08108 if python3:
08109 _v596.data = str[start:end].decode('utf-8')
08110 else:
08111 _v596.data = str[start:end]
08112 _v599 = _v592.disparity_image
08113 _v600 = _v599.header
08114 start = end
08115 end += 4
08116 (_v600.seq,) = _struct_I.unpack(str[start:end])
08117 _v601 = _v600.stamp
08118 _x = _v601
08119 start = end
08120 end += 8
08121 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
08122 start = end
08123 end += 4
08124 (length,) = _struct_I.unpack(str[start:end])
08125 start = end
08126 end += length
08127 if python3:
08128 _v600.frame_id = str[start:end].decode('utf-8')
08129 else:
08130 _v600.frame_id = str[start:end]
08131 _x = _v599
08132 start = end
08133 end += 8
08134 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
08135 start = end
08136 end += 4
08137 (length,) = _struct_I.unpack(str[start:end])
08138 start = end
08139 end += length
08140 if python3:
08141 _v599.encoding = str[start:end].decode('utf-8')
08142 else:
08143 _v599.encoding = str[start:end]
08144 _x = _v599
08145 start = end
08146 end += 5
08147 (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
08148 start = end
08149 end += 4
08150 (length,) = _struct_I.unpack(str[start:end])
08151 start = end
08152 end += length
08153 if python3:
08154 _v599.data = str[start:end].decode('utf-8')
08155 else:
08156 _v599.data = str[start:end]
08157 _v602 = _v592.cam_info
08158 _v603 = _v602.header
08159 start = end
08160 end += 4
08161 (_v603.seq,) = _struct_I.unpack(str[start:end])
08162 _v604 = _v603.stamp
08163 _x = _v604
08164 start = end
08165 end += 8
08166 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
08167 start = end
08168 end += 4
08169 (length,) = _struct_I.unpack(str[start:end])
08170 start = end
08171 end += length
08172 if python3:
08173 _v603.frame_id = str[start:end].decode('utf-8')
08174 else:
08175 _v603.frame_id = str[start:end]
08176 _x = _v602
08177 start = end
08178 end += 8
08179 (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
08180 start = end
08181 end += 4
08182 (length,) = _struct_I.unpack(str[start:end])
08183 start = end
08184 end += length
08185 if python3:
08186 _v602.distortion_model = str[start:end].decode('utf-8')
08187 else:
08188 _v602.distortion_model = str[start:end]
08189 start = end
08190 end += 4
08191 (length,) = _struct_I.unpack(str[start:end])
08192 pattern = '<%sd'%length
08193 start = end
08194 end += struct.calcsize(pattern)
08195 _v602.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
08196 start = end
08197 end += 72
08198 _v602.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
08199 start = end
08200 end += 72
08201 _v602.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
08202 start = end
08203 end += 96
08204 _v602.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
08205 _x = _v602
08206 start = end
08207 end += 8
08208 (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
08209 _v605 = _v602.roi
08210 _x = _v605
08211 start = end
08212 end += 17
08213 (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
08214 _v605.do_rectify = bool(_v605.do_rectify)
08215 _v606 = _v592.roi_box_pose
08216 _v607 = _v606.header
08217 start = end
08218 end += 4
08219 (_v607.seq,) = _struct_I.unpack(str[start:end])
08220 _v608 = _v607.stamp
08221 _x = _v608
08222 start = end
08223 end += 8
08224 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
08225 start = end
08226 end += 4
08227 (length,) = _struct_I.unpack(str[start:end])
08228 start = end
08229 end += length
08230 if python3:
08231 _v607.frame_id = str[start:end].decode('utf-8')
08232 else:
08233 _v607.frame_id = str[start:end]
08234 _v609 = _v606.pose
08235 _v610 = _v609.position
08236 _x = _v610
08237 start = end
08238 end += 24
08239 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
08240 _v611 = _v609.orientation
08241 _x = _v611
08242 start = end
08243 end += 32
08244 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
08245 _v612 = _v592.roi_box_dims
08246 _x = _v612
08247 start = end
08248 end += 24
08249 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
08250 start = end
08251 end += 4
08252 (length,) = _struct_I.unpack(str[start:end])
08253 start = end
08254 end += length
08255 if python3:
08256 val2.collision_name = str[start:end].decode('utf-8')
08257 else:
08258 val2.collision_name = str[start:end]
08259 val1.moved_obstacles.append(val2)
08260 self.action_feedback.feedback.grasps.append(val1)
08261 return self
08262 except struct.error as e:
08263 raise genpy.DeserializationError(e) #most likely buffer underfill
08264
08265 _struct_I = genpy.struct_I
08266 _struct_IBI = struct.Struct("<IBI")
08267 _struct_6IB3I = struct.Struct("<6IB3I")
08268 _struct_B = struct.Struct("<B")
08269 _struct_12d = struct.Struct("<12d")
08270 _struct_f = struct.Struct("<f")
08271 _struct_i = struct.Struct("<i")
08272 _struct_dB2f = struct.Struct("<dB2f")
08273 _struct_BI = struct.Struct("<BI")
08274 _struct_10d = struct.Struct("<10d")
08275 _struct_3f = struct.Struct("<3f")
08276 _struct_3I = struct.Struct("<3I")
08277 _struct_9d = struct.Struct("<9d")
08278 _struct_B2I = struct.Struct("<B2I")
08279 _struct_i3I = struct.Struct("<i3I")
08280 _struct_4d = struct.Struct("<4d")
08281 _struct_2I = struct.Struct("<2I")
08282 _struct_4IB = struct.Struct("<4IB")
08283 _struct_3d = struct.Struct("<3d")