_PickupActionGoal.py
Go to the documentation of this file.
00001 """autogenerated by genpy from object_manipulation_msgs/PickupActionGoal.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 arm_navigation_msgs.msg
00008 import object_manipulation_msgs.msg
00009 import actionlib_msgs.msg
00010 import geometry_msgs.msg
00011 import sensor_msgs.msg
00012 import std_msgs.msg
00013 import genpy
00014 import household_objects_database_msgs.msg
00015 
00016 class PickupActionGoal(genpy.Message):
00017   _md5sum = "c80a8991b5d6a3dabaeb053a18309c62"
00018   _type = "object_manipulation_msgs/PickupActionGoal"
00019   _has_header = True #flag to mark the presence of a Header object
00020   _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00021 
00022 Header header
00023 actionlib_msgs/GoalID goal_id
00024 PickupGoal goal
00025 
00026 ================================================================================
00027 MSG: std_msgs/Header
00028 # Standard metadata for higher-level stamped data types.
00029 # This is generally used to communicate timestamped data 
00030 # in a particular coordinate frame.
00031 # 
00032 # sequence ID: consecutively increasing ID 
00033 uint32 seq
00034 #Two-integer timestamp that is expressed as:
00035 # * stamp.secs: seconds (stamp_secs) since epoch
00036 # * stamp.nsecs: nanoseconds since stamp_secs
00037 # time-handling sugar is provided by the client library
00038 time stamp
00039 #Frame this data is associated with
00040 # 0: no frame
00041 # 1: global frame
00042 string frame_id
00043 
00044 ================================================================================
00045 MSG: actionlib_msgs/GoalID
00046 # The stamp should store the time at which this goal was requested.
00047 # It is used by an action server when it tries to preempt all
00048 # goals that were requested before a certain time
00049 time stamp
00050 
00051 # The id provides a way to associate feedback and
00052 # result message with specific goal requests. The id
00053 # specified must be unique.
00054 string id
00055 
00056 
00057 ================================================================================
00058 MSG: object_manipulation_msgs/PickupGoal
00059 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00060 # An action for picking up an object
00061 
00062 # which arm to be used for grasping
00063 string arm_name
00064 
00065 # the object to be grasped
00066 GraspableObject target
00067 
00068 # a list of grasps to be used
00069 # if empty, the grasp executive will call one of its own planners
00070 Grasp[] desired_grasps
00071 
00072 # how the object should be lifted after the grasp
00073 # the frame_id that this lift is specified in MUST be either the robot_frame 
00074 # or the gripper_frame specified in your hand description file
00075 GripperTranslation lift
00076 
00077 # the name that the target object has in the collision map
00078 # can be left empty if no name is available
00079 string collision_object_name
00080 
00081 # the name that the support surface (e.g. table) has in the collision map
00082 # can be left empty if no name is available
00083 string collision_support_surface_name
00084 
00085 # whether collisions between the gripper and the support surface should be acceptable
00086 # during move from pre-grasp to grasp and during lift. Collisions when moving to the
00087 # pre-grasp location are still not allowed even if this is set to true.
00088 bool allow_gripper_support_collision
00089 
00090 # whether reactive grasp execution using tactile sensors should be used
00091 bool use_reactive_execution
00092 
00093 # whether reactive object lifting based on tactile sensors should be used
00094 bool use_reactive_lift
00095 
00096 # set this to true if you only want to query the manipulation pipeline as to what 
00097 # grasps it thinks are feasible, without actually executing them. If this is set to 
00098 # true, the atempted_grasp_results field of the result will be populated, but no arm 
00099 # movement will be attempted
00100 bool only_perform_feasibility_test
00101 
00102 # set this to true if you want to ignore all collisions throughout the pickup 
00103 # and also move directly to the pre-grasp using Cartesian controllers
00104 bool ignore_collisions
00105 
00106 # OPTIONAL (These will not have to be filled out most of the time)
00107 # constraints to be imposed on every point in the motion of the arm
00108 arm_navigation_msgs/Constraints path_constraints
00109 
00110 # OPTIONAL (These will not have to be filled out most of the time)
00111 # additional collision operations to be used for every arm movement performed
00112 # during grasping. Note that these will be added on top of (and thus overide) other 
00113 # collision operations that the grasping pipeline deems necessary. Should be used
00114 # with care and only if special behaviors are desired
00115 arm_navigation_msgs/OrderedCollisionOperations additional_collision_operations
00116 
00117 # OPTIONAL (These will not have to be filled out most of the time)
00118 # additional link paddings to be used for every arm movement performed
00119 # during grasping. Note that these will be added on top of (and thus overide) other 
00120 # link paddings that the grasping pipeline deems necessary. Should be used
00121 # with care and only if special behaviors are desired
00122 arm_navigation_msgs/LinkPadding[] additional_link_padding
00123 
00124 # an optional list of obstacles that we have semantic information about
00125 # and that can be moved in the course of grasping
00126 GraspableObject[] movable_obstacles
00127 
00128 # the maximum contact force to use while grasping (<=0 to disable)
00129 float32 max_contact_force
00130 
00131 
00132 ================================================================================
00133 MSG: object_manipulation_msgs/GraspableObject
00134 # an object that the object_manipulator can work on
00135 
00136 # a graspable object can be represented in multiple ways. This message
00137 # can contain all of them. Which one is actually used is up to the receiver
00138 # of this message. When adding new representations, one must be careful that
00139 # they have reasonable lightweight defaults indicating that that particular
00140 # representation is not available.
00141 
00142 # the tf frame to be used as a reference frame when combining information from
00143 # the different representations below
00144 string reference_frame_id
00145 
00146 # potential recognition results from a database of models
00147 # all poses are relative to the object reference pose
00148 household_objects_database_msgs/DatabaseModelPose[] potential_models
00149 
00150 # the point cloud itself
00151 sensor_msgs/PointCloud cluster
00152 
00153 # a region of a PointCloud2 of interest
00154 object_manipulation_msgs/SceneRegion region
00155 
00156 # the name that this object has in the collision environment
00157 string collision_name
00158 ================================================================================
00159 MSG: household_objects_database_msgs/DatabaseModelPose
00160 # Informs that a specific model from the Model Database has been 
00161 # identified at a certain location
00162 
00163 # the database id of the model
00164 int32 model_id
00165 
00166 # the pose that it can be found in
00167 geometry_msgs/PoseStamped pose
00168 
00169 # a measure of the confidence level in this detection result
00170 float32 confidence
00171 
00172 # the name of the object detector that generated this detection result
00173 string detector_name
00174 
00175 ================================================================================
00176 MSG: geometry_msgs/PoseStamped
00177 # A Pose with reference coordinate frame and timestamp
00178 Header header
00179 Pose pose
00180 
00181 ================================================================================
00182 MSG: geometry_msgs/Pose
00183 # A representation of pose in free space, composed of postion and orientation. 
00184 Point position
00185 Quaternion orientation
00186 
00187 ================================================================================
00188 MSG: geometry_msgs/Point
00189 # This contains the position of a point in free space
00190 float64 x
00191 float64 y
00192 float64 z
00193 
00194 ================================================================================
00195 MSG: geometry_msgs/Quaternion
00196 # This represents an orientation in free space in quaternion form.
00197 
00198 float64 x
00199 float64 y
00200 float64 z
00201 float64 w
00202 
00203 ================================================================================
00204 MSG: sensor_msgs/PointCloud
00205 # This message holds a collection of 3d points, plus optional additional
00206 # information about each point.
00207 
00208 # Time of sensor data acquisition, coordinate frame ID.
00209 Header header
00210 
00211 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00212 # in the frame given in the header.
00213 geometry_msgs/Point32[] points
00214 
00215 # Each channel should have the same number of elements as points array,
00216 # and the data in each channel should correspond 1:1 with each point.
00217 # Channel names in common practice are listed in ChannelFloat32.msg.
00218 ChannelFloat32[] channels
00219 
00220 ================================================================================
00221 MSG: geometry_msgs/Point32
00222 # This contains the position of a point in free space(with 32 bits of precision).
00223 # It is recommeded to use Point wherever possible instead of Point32.  
00224 # 
00225 # This recommendation is to promote interoperability.  
00226 #
00227 # This message is designed to take up less space when sending
00228 # lots of points at once, as in the case of a PointCloud.  
00229 
00230 float32 x
00231 float32 y
00232 float32 z
00233 ================================================================================
00234 MSG: sensor_msgs/ChannelFloat32
00235 # This message is used by the PointCloud message to hold optional data
00236 # associated with each point in the cloud. The length of the values
00237 # array should be the same as the length of the points array in the
00238 # PointCloud, and each value should be associated with the corresponding
00239 # point.
00240 
00241 # Channel names in existing practice include:
00242 #   "u", "v" - row and column (respectively) in the left stereo image.
00243 #              This is opposite to usual conventions but remains for
00244 #              historical reasons. The newer PointCloud2 message has no
00245 #              such problem.
00246 #   "rgb" - For point clouds produced by color stereo cameras. uint8
00247 #           (R,G,B) values packed into the least significant 24 bits,
00248 #           in order.
00249 #   "intensity" - laser or pixel intensity.
00250 #   "distance"
00251 
00252 # The channel name should give semantics of the channel (e.g.
00253 # "intensity" instead of "value").
00254 string name
00255 
00256 # The values array should be 1-1 with the elements of the associated
00257 # PointCloud.
00258 float32[] values
00259 
00260 ================================================================================
00261 MSG: object_manipulation_msgs/SceneRegion
00262 # Point cloud
00263 sensor_msgs/PointCloud2 cloud
00264 
00265 # Indices for the region of interest
00266 int32[] mask
00267 
00268 # One of the corresponding 2D images, if applicable
00269 sensor_msgs/Image image
00270 
00271 # The disparity image, if applicable
00272 sensor_msgs/Image disparity_image
00273 
00274 # Camera info for the camera that took the image
00275 sensor_msgs/CameraInfo cam_info
00276 
00277 # a 3D region of interest for grasp planning
00278 geometry_msgs/PoseStamped  roi_box_pose
00279 geometry_msgs/Vector3      roi_box_dims
00280 
00281 ================================================================================
00282 MSG: sensor_msgs/PointCloud2
00283 # This message holds a collection of N-dimensional points, which may
00284 # contain additional information such as normals, intensity, etc. The
00285 # point data is stored as a binary blob, its layout described by the
00286 # contents of the "fields" array.
00287 
00288 # The point cloud data may be organized 2d (image-like) or 1d
00289 # (unordered). Point clouds organized as 2d images may be produced by
00290 # camera depth sensors such as stereo or time-of-flight.
00291 
00292 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00293 # points).
00294 Header header
00295 
00296 # 2D structure of the point cloud. If the cloud is unordered, height is
00297 # 1 and width is the length of the point cloud.
00298 uint32 height
00299 uint32 width
00300 
00301 # Describes the channels and their layout in the binary data blob.
00302 PointField[] fields
00303 
00304 bool    is_bigendian # Is this data bigendian?
00305 uint32  point_step   # Length of a point in bytes
00306 uint32  row_step     # Length of a row in bytes
00307 uint8[] data         # Actual point data, size is (row_step*height)
00308 
00309 bool is_dense        # True if there are no invalid points
00310 
00311 ================================================================================
00312 MSG: sensor_msgs/PointField
00313 # This message holds the description of one point entry in the
00314 # PointCloud2 message format.
00315 uint8 INT8    = 1
00316 uint8 UINT8   = 2
00317 uint8 INT16   = 3
00318 uint8 UINT16  = 4
00319 uint8 INT32   = 5
00320 uint8 UINT32  = 6
00321 uint8 FLOAT32 = 7
00322 uint8 FLOAT64 = 8
00323 
00324 string name      # Name of field
00325 uint32 offset    # Offset from start of point struct
00326 uint8  datatype  # Datatype enumeration, see above
00327 uint32 count     # How many elements in the field
00328 
00329 ================================================================================
00330 MSG: sensor_msgs/Image
00331 # This message contains an uncompressed image
00332 # (0, 0) is at top-left corner of image
00333 #
00334 
00335 Header header        # Header timestamp should be acquisition time of image
00336                      # Header frame_id should be optical frame of camera
00337                      # origin of frame should be optical center of cameara
00338                      # +x should point to the right in the image
00339                      # +y should point down in the image
00340                      # +z should point into to plane of the image
00341                      # If the frame_id here and the frame_id of the CameraInfo
00342                      # message associated with the image conflict
00343                      # the behavior is undefined
00344 
00345 uint32 height         # image height, that is, number of rows
00346 uint32 width          # image width, that is, number of columns
00347 
00348 # The legal values for encoding are in file src/image_encodings.cpp
00349 # If you want to standardize a new string format, join
00350 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00351 
00352 string encoding       # Encoding of pixels -- channel meaning, ordering, size
00353                       # taken from the list of strings in src/image_encodings.cpp
00354 
00355 uint8 is_bigendian    # is this data bigendian?
00356 uint32 step           # Full row length in bytes
00357 uint8[] data          # actual matrix data, size is (step * rows)
00358 
00359 ================================================================================
00360 MSG: sensor_msgs/CameraInfo
00361 # This message defines meta information for a camera. It should be in a
00362 # camera namespace on topic "camera_info" and accompanied by up to five
00363 # image topics named:
00364 #
00365 #   image_raw - raw data from the camera driver, possibly Bayer encoded
00366 #   image            - monochrome, distorted
00367 #   image_color      - color, distorted
00368 #   image_rect       - monochrome, rectified
00369 #   image_rect_color - color, rectified
00370 #
00371 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00372 # for producing the four processed image topics from image_raw and
00373 # camera_info. The meaning of the camera parameters are described in
00374 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00375 #
00376 # The image_geometry package provides a user-friendly interface to
00377 # common operations using this meta information. If you want to, e.g.,
00378 # project a 3d point into image coordinates, we strongly recommend
00379 # using image_geometry.
00380 #
00381 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00382 # zeroed out. In particular, clients may assume that K[0] == 0.0
00383 # indicates an uncalibrated camera.
00384 
00385 #######################################################################
00386 #                     Image acquisition info                          #
00387 #######################################################################
00388 
00389 # Time of image acquisition, camera coordinate frame ID
00390 Header header    # Header timestamp should be acquisition time of image
00391                  # Header frame_id should be optical frame of camera
00392                  # origin of frame should be optical center of camera
00393                  # +x should point to the right in the image
00394                  # +y should point down in the image
00395                  # +z should point into the plane of the image
00396 
00397 
00398 #######################################################################
00399 #                      Calibration Parameters                         #
00400 #######################################################################
00401 # These are fixed during camera calibration. Their values will be the #
00402 # same in all messages until the camera is recalibrated. Note that    #
00403 # self-calibrating systems may "recalibrate" frequently.              #
00404 #                                                                     #
00405 # The internal parameters can be used to warp a raw (distorted) image #
00406 # to:                                                                 #
00407 #   1. An undistorted image (requires D and K)                        #
00408 #   2. A rectified image (requires D, K, R)                           #
00409 # The projection matrix P projects 3D points into the rectified image.#
00410 #######################################################################
00411 
00412 # The image dimensions with which the camera was calibrated. Normally
00413 # this will be the full camera resolution in pixels.
00414 uint32 height
00415 uint32 width
00416 
00417 # The distortion model used. Supported models are listed in
00418 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00419 # simple model of radial and tangential distortion - is sufficent.
00420 string distortion_model
00421 
00422 # The distortion parameters, size depending on the distortion model.
00423 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00424 float64[] D
00425 
00426 # Intrinsic camera matrix for the raw (distorted) images.
00427 #     [fx  0 cx]
00428 # K = [ 0 fy cy]
00429 #     [ 0  0  1]
00430 # Projects 3D points in the camera coordinate frame to 2D pixel
00431 # coordinates using the focal lengths (fx, fy) and principal point
00432 # (cx, cy).
00433 float64[9]  K # 3x3 row-major matrix
00434 
00435 # Rectification matrix (stereo cameras only)
00436 # A rotation matrix aligning the camera coordinate system to the ideal
00437 # stereo image plane so that epipolar lines in both stereo images are
00438 # parallel.
00439 float64[9]  R # 3x3 row-major matrix
00440 
00441 # Projection/camera matrix
00442 #     [fx'  0  cx' Tx]
00443 # P = [ 0  fy' cy' Ty]
00444 #     [ 0   0   1   0]
00445 # By convention, this matrix specifies the intrinsic (camera) matrix
00446 #  of the processed (rectified) image. That is, the left 3x3 portion
00447 #  is the normal camera intrinsic matrix for the rectified image.
00448 # It projects 3D points in the camera coordinate frame to 2D pixel
00449 #  coordinates using the focal lengths (fx', fy') and principal point
00450 #  (cx', cy') - these may differ from the values in K.
00451 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00452 #  also have R = the identity and P[1:3,1:3] = K.
00453 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00454 #  position of the optical center of the second camera in the first
00455 #  camera's frame. We assume Tz = 0 so both cameras are in the same
00456 #  stereo image plane. The first camera always has Tx = Ty = 0. For
00457 #  the right (second) camera of a horizontal stereo pair, Ty = 0 and
00458 #  Tx = -fx' * B, where B is the baseline between the cameras.
00459 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00460 #  the rectified image is given by:
00461 #  [u v w]' = P * [X Y Z 1]'
00462 #         x = u / w
00463 #         y = v / w
00464 #  This holds for both images of a stereo pair.
00465 float64[12] P # 3x4 row-major matrix
00466 
00467 
00468 #######################################################################
00469 #                      Operational Parameters                         #
00470 #######################################################################
00471 # These define the image region actually captured by the camera       #
00472 # driver. Although they affect the geometry of the output image, they #
00473 # may be changed freely without recalibrating the camera.             #
00474 #######################################################################
00475 
00476 # Binning refers here to any camera setting which combines rectangular
00477 #  neighborhoods of pixels into larger "super-pixels." It reduces the
00478 #  resolution of the output image to
00479 #  (width / binning_x) x (height / binning_y).
00480 # The default values binning_x = binning_y = 0 is considered the same
00481 #  as binning_x = binning_y = 1 (no subsampling).
00482 uint32 binning_x
00483 uint32 binning_y
00484 
00485 # Region of interest (subwindow of full camera resolution), given in
00486 #  full resolution (unbinned) image coordinates. A particular ROI
00487 #  always denotes the same window of pixels on the camera sensor,
00488 #  regardless of binning settings.
00489 # The default setting of roi (all values 0) is considered the same as
00490 #  full resolution (roi.width = width, roi.height = height).
00491 RegionOfInterest roi
00492 
00493 ================================================================================
00494 MSG: sensor_msgs/RegionOfInterest
00495 # This message is used to specify a region of interest within an image.
00496 #
00497 # When used to specify the ROI setting of the camera when the image was
00498 # taken, the height and width fields should either match the height and
00499 # width fields for the associated image; or height = width = 0
00500 # indicates that the full resolution image was captured.
00501 
00502 uint32 x_offset  # Leftmost pixel of the ROI
00503                  # (0 if the ROI includes the left edge of the image)
00504 uint32 y_offset  # Topmost pixel of the ROI
00505                  # (0 if the ROI includes the top edge of the image)
00506 uint32 height    # Height of ROI
00507 uint32 width     # Width of ROI
00508 
00509 # True if a distinct rectified ROI should be calculated from the "raw"
00510 # ROI in this message. Typically this should be False if the full image
00511 # is captured (ROI not used), and True if a subwindow is captured (ROI
00512 # used).
00513 bool do_rectify
00514 
00515 ================================================================================
00516 MSG: geometry_msgs/Vector3
00517 # This represents a vector in free space. 
00518 
00519 float64 x
00520 float64 y
00521 float64 z
00522 ================================================================================
00523 MSG: object_manipulation_msgs/Grasp
00524 
00525 # The internal posture of the hand for the pre-grasp
00526 # only positions are used
00527 sensor_msgs/JointState pre_grasp_posture
00528 
00529 # The internal posture of the hand for the grasp
00530 # positions and efforts are used
00531 sensor_msgs/JointState grasp_posture
00532 
00533 # The position of the end-effector for the grasp relative to a reference frame 
00534 # (that is always specified elsewhere, not in this message)
00535 geometry_msgs/Pose grasp_pose
00536 
00537 # The estimated probability of success for this grasp
00538 float64 success_probability
00539 
00540 # Debug flag to indicate that this grasp would be the best in its cluster
00541 bool cluster_rep
00542 
00543 # how far the pre-grasp should ideally be away from the grasp
00544 float32 desired_approach_distance
00545 
00546 # how much distance between pre-grasp and grasp must actually be feasible 
00547 # for the grasp not to be rejected
00548 float32 min_approach_distance
00549 
00550 # an optional list of obstacles that we have semantic information about
00551 # and that we expect might move in the course of executing this grasp
00552 # the grasp planner is expected to make sure they move in an OK way; during
00553 # execution, grasp executors will not check for collisions against these objects
00554 GraspableObject[] moved_obstacles
00555 
00556 ================================================================================
00557 MSG: sensor_msgs/JointState
00558 # This is a message that holds data to describe the state of a set of torque controlled joints. 
00559 #
00560 # The state of each joint (revolute or prismatic) is defined by:
00561 #  * the position of the joint (rad or m),
00562 #  * the velocity of the joint (rad/s or m/s) and 
00563 #  * the effort that is applied in the joint (Nm or N).
00564 #
00565 # Each joint is uniquely identified by its name
00566 # The header specifies the time at which the joint states were recorded. All the joint states
00567 # in one message have to be recorded at the same time.
00568 #
00569 # This message consists of a multiple arrays, one for each part of the joint state. 
00570 # The goal is to make each of the fields optional. When e.g. your joints have no
00571 # effort associated with them, you can leave the effort array empty. 
00572 #
00573 # All arrays in this message should have the same size, or be empty.
00574 # This is the only way to uniquely associate the joint name with the correct
00575 # states.
00576 
00577 
00578 Header header
00579 
00580 string[] name
00581 float64[] position
00582 float64[] velocity
00583 float64[] effort
00584 
00585 ================================================================================
00586 MSG: object_manipulation_msgs/GripperTranslation
00587 # defines a translation for the gripper, used in pickup or place tasks
00588 # for example for lifting an object off a table or approaching the table for placing
00589 
00590 # the direction of the translation
00591 geometry_msgs/Vector3Stamped direction
00592 
00593 # the desired translation distance
00594 float32 desired_distance
00595 
00596 # the min distance that must be considered feasible before the
00597 # grasp is even attempted
00598 float32 min_distance
00599 ================================================================================
00600 MSG: geometry_msgs/Vector3Stamped
00601 # This represents a Vector3 with reference coordinate frame and timestamp
00602 Header header
00603 Vector3 vector
00604 
00605 ================================================================================
00606 MSG: arm_navigation_msgs/Constraints
00607 # This message contains a list of motion planning constraints.
00608 
00609 arm_navigation_msgs/JointConstraint[] joint_constraints
00610 arm_navigation_msgs/PositionConstraint[] position_constraints
00611 arm_navigation_msgs/OrientationConstraint[] orientation_constraints
00612 arm_navigation_msgs/VisibilityConstraint[] visibility_constraints
00613 
00614 ================================================================================
00615 MSG: arm_navigation_msgs/JointConstraint
00616 # Constrain the position of a joint to be within a certain bound
00617 string joint_name
00618 
00619 # the bound to be achieved is [position - tolerance_below, position + tolerance_above]
00620 float64 position
00621 float64 tolerance_above
00622 float64 tolerance_below
00623 
00624 # A weighting factor for this constraint
00625 float64 weight
00626 ================================================================================
00627 MSG: arm_navigation_msgs/PositionConstraint
00628 # This message contains the definition of a position constraint.
00629 Header header
00630 
00631 # The robot link this constraint refers to
00632 string link_name
00633 
00634 # The offset (in the link frame) for the target point on the link we are planning for
00635 geometry_msgs/Point target_point_offset
00636 
00637 # The nominal/target position for the point we are planning for
00638 geometry_msgs/Point position
00639 
00640 # The shape of the bounded region that constrains the position of the end-effector
00641 # This region is always centered at the position defined above
00642 arm_navigation_msgs/Shape constraint_region_shape
00643 
00644 # The orientation of the bounded region that constrains the position of the end-effector. 
00645 # This allows the specification of non-axis aligned constraints
00646 geometry_msgs/Quaternion constraint_region_orientation
00647 
00648 # Constraint weighting factor - a weight for this constraint
00649 float64 weight
00650 
00651 ================================================================================
00652 MSG: arm_navigation_msgs/Shape
00653 byte SPHERE=0
00654 byte BOX=1
00655 byte CYLINDER=2
00656 byte MESH=3
00657 
00658 byte type
00659 
00660 
00661 #### define sphere, box, cylinder ####
00662 # the origin of each shape is considered at the shape's center
00663 
00664 # for sphere
00665 # radius := dimensions[0]
00666 
00667 # for cylinder
00668 # radius := dimensions[0]
00669 # length := dimensions[1]
00670 # the length is along the Z axis
00671 
00672 # for box
00673 # size_x := dimensions[0]
00674 # size_y := dimensions[1]
00675 # size_z := dimensions[2]
00676 float64[] dimensions
00677 
00678 
00679 #### define mesh ####
00680 
00681 # list of triangles; triangle k is defined by tre vertices located
00682 # at indices triangles[3k], triangles[3k+1], triangles[3k+2]
00683 int32[] triangles
00684 geometry_msgs/Point[] vertices
00685 
00686 ================================================================================
00687 MSG: arm_navigation_msgs/OrientationConstraint
00688 # This message contains the definition of an orientation constraint.
00689 Header header
00690 
00691 # The robot link this constraint refers to
00692 string link_name
00693 
00694 # The type of the constraint
00695 int32 type
00696 int32 LINK_FRAME=0
00697 int32 HEADER_FRAME=1
00698 
00699 # The desired orientation of the robot link specified as a quaternion
00700 geometry_msgs/Quaternion orientation
00701 
00702 # optional RPY error tolerances specified if 
00703 float64 absolute_roll_tolerance
00704 float64 absolute_pitch_tolerance
00705 float64 absolute_yaw_tolerance
00706 
00707 # Constraint weighting factor - a weight for this constraint
00708 float64 weight
00709 
00710 ================================================================================
00711 MSG: arm_navigation_msgs/VisibilityConstraint
00712 # This message contains the definition of a visibility constraint.
00713 Header header
00714 
00715 # The point stamped target that needs to be kept within view of the sensor
00716 geometry_msgs/PointStamped target
00717 
00718 # The local pose of the frame in which visibility is to be maintained
00719 # The frame id should represent the robot link to which the sensor is attached
00720 # The visual axis of the sensor is assumed to be along the X axis of this frame
00721 geometry_msgs/PoseStamped sensor_pose
00722 
00723 # The deviation (in radians) that will be tolerated
00724 # Constraint error will be measured as the solid angle between the 
00725 # X axis of the frame defined above and the vector between the origin 
00726 # of the frame defined above and the target location
00727 float64 absolute_tolerance
00728 
00729 
00730 ================================================================================
00731 MSG: geometry_msgs/PointStamped
00732 # This represents a Point with reference coordinate frame and timestamp
00733 Header header
00734 Point point
00735 
00736 ================================================================================
00737 MSG: arm_navigation_msgs/OrderedCollisionOperations
00738 # A set of collision operations that will be performed in the order they are specified
00739 CollisionOperation[] collision_operations
00740 ================================================================================
00741 MSG: arm_navigation_msgs/CollisionOperation
00742 # A definition of a collision operation
00743 # E.g. ("gripper",COLLISION_SET_ALL,ENABLE) will enable collisions 
00744 # between the gripper and all objects in the collision space
00745 
00746 string object1
00747 string object2
00748 string COLLISION_SET_ALL="all"
00749 string COLLISION_SET_OBJECTS="objects"
00750 string COLLISION_SET_ATTACHED_OBJECTS="attached"
00751 
00752 # The penetration distance to which collisions are allowed. This is 0.0 by default.
00753 float64 penetration_distance
00754 
00755 # Flag that determines whether collisions will be enabled or disabled for the pair of objects specified above
00756 int32 operation
00757 int32 DISABLE=0
00758 int32 ENABLE=1
00759 
00760 ================================================================================
00761 MSG: arm_navigation_msgs/LinkPadding
00762 #name for the link
00763 string link_name
00764 
00765 # padding to apply to the link
00766 float64 padding
00767 
00768 """
00769   __slots__ = ['header','goal_id','goal']
00770   _slot_types = ['std_msgs/Header','actionlib_msgs/GoalID','object_manipulation_msgs/PickupGoal']
00771 
00772   def __init__(self, *args, **kwds):
00773     """
00774     Constructor. Any message fields that are implicitly/explicitly
00775     set to None will be assigned a default value. The recommend
00776     use is keyword arguments as this is more robust to future message
00777     changes.  You cannot mix in-order arguments and keyword arguments.
00778 
00779     The available fields are:
00780        header,goal_id,goal
00781 
00782     :param args: complete set of field values, in .msg order
00783     :param kwds: use keyword arguments corresponding to message field names
00784     to set specific fields.
00785     """
00786     if args or kwds:
00787       super(PickupActionGoal, self).__init__(*args, **kwds)
00788       #message fields cannot be None, assign default values for those that are
00789       if self.header is None:
00790         self.header = std_msgs.msg.Header()
00791       if self.goal_id is None:
00792         self.goal_id = actionlib_msgs.msg.GoalID()
00793       if self.goal is None:
00794         self.goal = object_manipulation_msgs.msg.PickupGoal()
00795     else:
00796       self.header = std_msgs.msg.Header()
00797       self.goal_id = actionlib_msgs.msg.GoalID()
00798       self.goal = object_manipulation_msgs.msg.PickupGoal()
00799 
00800   def _get_types(self):
00801     """
00802     internal API method
00803     """
00804     return self._slot_types
00805 
00806   def serialize(self, buff):
00807     """
00808     serialize message into buffer
00809     :param buff: buffer, ``StringIO``
00810     """
00811     try:
00812       _x = self
00813       buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00814       _x = self.header.frame_id
00815       length = len(_x)
00816       if python3 or type(_x) == unicode:
00817         _x = _x.encode('utf-8')
00818         length = len(_x)
00819       buff.write(struct.pack('<I%ss'%length, length, _x))
00820       _x = self
00821       buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs))
00822       _x = self.goal_id.id
00823       length = len(_x)
00824       if python3 or type(_x) == unicode:
00825         _x = _x.encode('utf-8')
00826         length = len(_x)
00827       buff.write(struct.pack('<I%ss'%length, length, _x))
00828       _x = self.goal.arm_name
00829       length = len(_x)
00830       if python3 or type(_x) == unicode:
00831         _x = _x.encode('utf-8')
00832         length = len(_x)
00833       buff.write(struct.pack('<I%ss'%length, length, _x))
00834       _x = self.goal.target.reference_frame_id
00835       length = len(_x)
00836       if python3 or type(_x) == unicode:
00837         _x = _x.encode('utf-8')
00838         length = len(_x)
00839       buff.write(struct.pack('<I%ss'%length, length, _x))
00840       length = len(self.goal.target.potential_models)
00841       buff.write(_struct_I.pack(length))
00842       for val1 in self.goal.target.potential_models:
00843         buff.write(_struct_i.pack(val1.model_id))
00844         _v1 = val1.pose
00845         _v2 = _v1.header
00846         buff.write(_struct_I.pack(_v2.seq))
00847         _v3 = _v2.stamp
00848         _x = _v3
00849         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00850         _x = _v2.frame_id
00851         length = len(_x)
00852         if python3 or type(_x) == unicode:
00853           _x = _x.encode('utf-8')
00854           length = len(_x)
00855         buff.write(struct.pack('<I%ss'%length, length, _x))
00856         _v4 = _v1.pose
00857         _v5 = _v4.position
00858         _x = _v5
00859         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00860         _v6 = _v4.orientation
00861         _x = _v6
00862         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00863         buff.write(_struct_f.pack(val1.confidence))
00864         _x = val1.detector_name
00865         length = len(_x)
00866         if python3 or type(_x) == unicode:
00867           _x = _x.encode('utf-8')
00868           length = len(_x)
00869         buff.write(struct.pack('<I%ss'%length, length, _x))
00870       _x = self
00871       buff.write(_struct_3I.pack(_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs))
00872       _x = self.goal.target.cluster.header.frame_id
00873       length = len(_x)
00874       if python3 or type(_x) == unicode:
00875         _x = _x.encode('utf-8')
00876         length = len(_x)
00877       buff.write(struct.pack('<I%ss'%length, length, _x))
00878       length = len(self.goal.target.cluster.points)
00879       buff.write(_struct_I.pack(length))
00880       for val1 in self.goal.target.cluster.points:
00881         _x = val1
00882         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00883       length = len(self.goal.target.cluster.channels)
00884       buff.write(_struct_I.pack(length))
00885       for val1 in self.goal.target.cluster.channels:
00886         _x = val1.name
00887         length = len(_x)
00888         if python3 or type(_x) == unicode:
00889           _x = _x.encode('utf-8')
00890           length = len(_x)
00891         buff.write(struct.pack('<I%ss'%length, length, _x))
00892         length = len(val1.values)
00893         buff.write(_struct_I.pack(length))
00894         pattern = '<%sf'%length
00895         buff.write(struct.pack(pattern, *val1.values))
00896       _x = self
00897       buff.write(_struct_3I.pack(_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs))
00898       _x = self.goal.target.region.cloud.header.frame_id
00899       length = len(_x)
00900       if python3 or type(_x) == unicode:
00901         _x = _x.encode('utf-8')
00902         length = len(_x)
00903       buff.write(struct.pack('<I%ss'%length, length, _x))
00904       _x = self
00905       buff.write(_struct_2I.pack(_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width))
00906       length = len(self.goal.target.region.cloud.fields)
00907       buff.write(_struct_I.pack(length))
00908       for val1 in self.goal.target.region.cloud.fields:
00909         _x = val1.name
00910         length = len(_x)
00911         if python3 or type(_x) == unicode:
00912           _x = _x.encode('utf-8')
00913           length = len(_x)
00914         buff.write(struct.pack('<I%ss'%length, length, _x))
00915         _x = val1
00916         buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00917       _x = self
00918       buff.write(_struct_B2I.pack(_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step))
00919       _x = self.goal.target.region.cloud.data
00920       length = len(_x)
00921       # - if encoded as a list instead, serialize as bytes instead of string
00922       if type(_x) in [list, tuple]:
00923         buff.write(struct.pack('<I%sB'%length, length, *_x))
00924       else:
00925         buff.write(struct.pack('<I%ss'%length, length, _x))
00926       buff.write(_struct_B.pack(self.goal.target.region.cloud.is_dense))
00927       length = len(self.goal.target.region.mask)
00928       buff.write(_struct_I.pack(length))
00929       pattern = '<%si'%length
00930       buff.write(struct.pack(pattern, *self.goal.target.region.mask))
00931       _x = self
00932       buff.write(_struct_3I.pack(_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs))
00933       _x = self.goal.target.region.image.header.frame_id
00934       length = len(_x)
00935       if python3 or type(_x) == unicode:
00936         _x = _x.encode('utf-8')
00937         length = len(_x)
00938       buff.write(struct.pack('<I%ss'%length, length, _x))
00939       _x = self
00940       buff.write(_struct_2I.pack(_x.goal.target.region.image.height, _x.goal.target.region.image.width))
00941       _x = self.goal.target.region.image.encoding
00942       length = len(_x)
00943       if python3 or type(_x) == unicode:
00944         _x = _x.encode('utf-8')
00945         length = len(_x)
00946       buff.write(struct.pack('<I%ss'%length, length, _x))
00947       _x = self
00948       buff.write(_struct_BI.pack(_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step))
00949       _x = self.goal.target.region.image.data
00950       length = len(_x)
00951       # - if encoded as a list instead, serialize as bytes instead of string
00952       if type(_x) in [list, tuple]:
00953         buff.write(struct.pack('<I%sB'%length, length, *_x))
00954       else:
00955         buff.write(struct.pack('<I%ss'%length, length, _x))
00956       _x = self
00957       buff.write(_struct_3I.pack(_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs))
00958       _x = self.goal.target.region.disparity_image.header.frame_id
00959       length = len(_x)
00960       if python3 or type(_x) == unicode:
00961         _x = _x.encode('utf-8')
00962         length = len(_x)
00963       buff.write(struct.pack('<I%ss'%length, length, _x))
00964       _x = self
00965       buff.write(_struct_2I.pack(_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width))
00966       _x = self.goal.target.region.disparity_image.encoding
00967       length = len(_x)
00968       if python3 or type(_x) == unicode:
00969         _x = _x.encode('utf-8')
00970         length = len(_x)
00971       buff.write(struct.pack('<I%ss'%length, length, _x))
00972       _x = self
00973       buff.write(_struct_BI.pack(_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step))
00974       _x = self.goal.target.region.disparity_image.data
00975       length = len(_x)
00976       # - if encoded as a list instead, serialize as bytes instead of string
00977       if type(_x) in [list, tuple]:
00978         buff.write(struct.pack('<I%sB'%length, length, *_x))
00979       else:
00980         buff.write(struct.pack('<I%ss'%length, length, _x))
00981       _x = self
00982       buff.write(_struct_3I.pack(_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs))
00983       _x = self.goal.target.region.cam_info.header.frame_id
00984       length = len(_x)
00985       if python3 or type(_x) == unicode:
00986         _x = _x.encode('utf-8')
00987         length = len(_x)
00988       buff.write(struct.pack('<I%ss'%length, length, _x))
00989       _x = self
00990       buff.write(_struct_2I.pack(_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width))
00991       _x = self.goal.target.region.cam_info.distortion_model
00992       length = len(_x)
00993       if python3 or type(_x) == unicode:
00994         _x = _x.encode('utf-8')
00995         length = len(_x)
00996       buff.write(struct.pack('<I%ss'%length, length, _x))
00997       length = len(self.goal.target.region.cam_info.D)
00998       buff.write(_struct_I.pack(length))
00999       pattern = '<%sd'%length
01000       buff.write(struct.pack(pattern, *self.goal.target.region.cam_info.D))
01001       buff.write(_struct_9d.pack(*self.goal.target.region.cam_info.K))
01002       buff.write(_struct_9d.pack(*self.goal.target.region.cam_info.R))
01003       buff.write(_struct_12d.pack(*self.goal.target.region.cam_info.P))
01004       _x = self
01005       buff.write(_struct_6IB3I.pack(_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs))
01006       _x = self.goal.target.region.roi_box_pose.header.frame_id
01007       length = len(_x)
01008       if python3 or type(_x) == unicode:
01009         _x = _x.encode('utf-8')
01010         length = len(_x)
01011       buff.write(struct.pack('<I%ss'%length, length, _x))
01012       _x = self
01013       buff.write(_struct_10d.pack(_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z))
01014       _x = self.goal.target.collision_name
01015       length = len(_x)
01016       if python3 or type(_x) == unicode:
01017         _x = _x.encode('utf-8')
01018         length = len(_x)
01019       buff.write(struct.pack('<I%ss'%length, length, _x))
01020       length = len(self.goal.desired_grasps)
01021       buff.write(_struct_I.pack(length))
01022       for val1 in self.goal.desired_grasps:
01023         _v7 = val1.pre_grasp_posture
01024         _v8 = _v7.header
01025         buff.write(_struct_I.pack(_v8.seq))
01026         _v9 = _v8.stamp
01027         _x = _v9
01028         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01029         _x = _v8.frame_id
01030         length = len(_x)
01031         if python3 or type(_x) == unicode:
01032           _x = _x.encode('utf-8')
01033           length = len(_x)
01034         buff.write(struct.pack('<I%ss'%length, length, _x))
01035         length = len(_v7.name)
01036         buff.write(_struct_I.pack(length))
01037         for val3 in _v7.name:
01038           length = len(val3)
01039           if python3 or type(val3) == unicode:
01040             val3 = val3.encode('utf-8')
01041             length = len(val3)
01042           buff.write(struct.pack('<I%ss'%length, length, val3))
01043         length = len(_v7.position)
01044         buff.write(_struct_I.pack(length))
01045         pattern = '<%sd'%length
01046         buff.write(struct.pack(pattern, *_v7.position))
01047         length = len(_v7.velocity)
01048         buff.write(_struct_I.pack(length))
01049         pattern = '<%sd'%length
01050         buff.write(struct.pack(pattern, *_v7.velocity))
01051         length = len(_v7.effort)
01052         buff.write(_struct_I.pack(length))
01053         pattern = '<%sd'%length
01054         buff.write(struct.pack(pattern, *_v7.effort))
01055         _v10 = val1.grasp_posture
01056         _v11 = _v10.header
01057         buff.write(_struct_I.pack(_v11.seq))
01058         _v12 = _v11.stamp
01059         _x = _v12
01060         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01061         _x = _v11.frame_id
01062         length = len(_x)
01063         if python3 or type(_x) == unicode:
01064           _x = _x.encode('utf-8')
01065           length = len(_x)
01066         buff.write(struct.pack('<I%ss'%length, length, _x))
01067         length = len(_v10.name)
01068         buff.write(_struct_I.pack(length))
01069         for val3 in _v10.name:
01070           length = len(val3)
01071           if python3 or type(val3) == unicode:
01072             val3 = val3.encode('utf-8')
01073             length = len(val3)
01074           buff.write(struct.pack('<I%ss'%length, length, val3))
01075         length = len(_v10.position)
01076         buff.write(_struct_I.pack(length))
01077         pattern = '<%sd'%length
01078         buff.write(struct.pack(pattern, *_v10.position))
01079         length = len(_v10.velocity)
01080         buff.write(_struct_I.pack(length))
01081         pattern = '<%sd'%length
01082         buff.write(struct.pack(pattern, *_v10.velocity))
01083         length = len(_v10.effort)
01084         buff.write(_struct_I.pack(length))
01085         pattern = '<%sd'%length
01086         buff.write(struct.pack(pattern, *_v10.effort))
01087         _v13 = val1.grasp_pose
01088         _v14 = _v13.position
01089         _x = _v14
01090         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01091         _v15 = _v13.orientation
01092         _x = _v15
01093         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01094         _x = val1
01095         buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
01096         length = len(val1.moved_obstacles)
01097         buff.write(_struct_I.pack(length))
01098         for val2 in val1.moved_obstacles:
01099           _x = val2.reference_frame_id
01100           length = len(_x)
01101           if python3 or type(_x) == unicode:
01102             _x = _x.encode('utf-8')
01103             length = len(_x)
01104           buff.write(struct.pack('<I%ss'%length, length, _x))
01105           length = len(val2.potential_models)
01106           buff.write(_struct_I.pack(length))
01107           for val3 in val2.potential_models:
01108             buff.write(_struct_i.pack(val3.model_id))
01109             _v16 = val3.pose
01110             _v17 = _v16.header
01111             buff.write(_struct_I.pack(_v17.seq))
01112             _v18 = _v17.stamp
01113             _x = _v18
01114             buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01115             _x = _v17.frame_id
01116             length = len(_x)
01117             if python3 or type(_x) == unicode:
01118               _x = _x.encode('utf-8')
01119               length = len(_x)
01120             buff.write(struct.pack('<I%ss'%length, length, _x))
01121             _v19 = _v16.pose
01122             _v20 = _v19.position
01123             _x = _v20
01124             buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01125             _v21 = _v19.orientation
01126             _x = _v21
01127             buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01128             buff.write(_struct_f.pack(val3.confidence))
01129             _x = val3.detector_name
01130             length = len(_x)
01131             if python3 or type(_x) == unicode:
01132               _x = _x.encode('utf-8')
01133               length = len(_x)
01134             buff.write(struct.pack('<I%ss'%length, length, _x))
01135           _v22 = val2.cluster
01136           _v23 = _v22.header
01137           buff.write(_struct_I.pack(_v23.seq))
01138           _v24 = _v23.stamp
01139           _x = _v24
01140           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01141           _x = _v23.frame_id
01142           length = len(_x)
01143           if python3 or type(_x) == unicode:
01144             _x = _x.encode('utf-8')
01145             length = len(_x)
01146           buff.write(struct.pack('<I%ss'%length, length, _x))
01147           length = len(_v22.points)
01148           buff.write(_struct_I.pack(length))
01149           for val4 in _v22.points:
01150             _x = val4
01151             buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01152           length = len(_v22.channels)
01153           buff.write(_struct_I.pack(length))
01154           for val4 in _v22.channels:
01155             _x = val4.name
01156             length = len(_x)
01157             if python3 or type(_x) == unicode:
01158               _x = _x.encode('utf-8')
01159               length = len(_x)
01160             buff.write(struct.pack('<I%ss'%length, length, _x))
01161             length = len(val4.values)
01162             buff.write(_struct_I.pack(length))
01163             pattern = '<%sf'%length
01164             buff.write(struct.pack(pattern, *val4.values))
01165           _v25 = val2.region
01166           _v26 = _v25.cloud
01167           _v27 = _v26.header
01168           buff.write(_struct_I.pack(_v27.seq))
01169           _v28 = _v27.stamp
01170           _x = _v28
01171           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01172           _x = _v27.frame_id
01173           length = len(_x)
01174           if python3 or type(_x) == unicode:
01175             _x = _x.encode('utf-8')
01176             length = len(_x)
01177           buff.write(struct.pack('<I%ss'%length, length, _x))
01178           _x = _v26
01179           buff.write(_struct_2I.pack(_x.height, _x.width))
01180           length = len(_v26.fields)
01181           buff.write(_struct_I.pack(length))
01182           for val5 in _v26.fields:
01183             _x = val5.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             _x = val5
01190             buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01191           _x = _v26
01192           buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01193           _x = _v26.data
01194           length = len(_x)
01195           # - if encoded as a list instead, serialize as bytes instead of string
01196           if type(_x) in [list, tuple]:
01197             buff.write(struct.pack('<I%sB'%length, length, *_x))
01198           else:
01199             buff.write(struct.pack('<I%ss'%length, length, _x))
01200           buff.write(_struct_B.pack(_v26.is_dense))
01201           length = len(_v25.mask)
01202           buff.write(_struct_I.pack(length))
01203           pattern = '<%si'%length
01204           buff.write(struct.pack(pattern, *_v25.mask))
01205           _v29 = _v25.image
01206           _v30 = _v29.header
01207           buff.write(_struct_I.pack(_v30.seq))
01208           _v31 = _v30.stamp
01209           _x = _v31
01210           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01211           _x = _v30.frame_id
01212           length = len(_x)
01213           if python3 or type(_x) == unicode:
01214             _x = _x.encode('utf-8')
01215             length = len(_x)
01216           buff.write(struct.pack('<I%ss'%length, length, _x))
01217           _x = _v29
01218           buff.write(_struct_2I.pack(_x.height, _x.width))
01219           _x = _v29.encoding
01220           length = len(_x)
01221           if python3 or type(_x) == unicode:
01222             _x = _x.encode('utf-8')
01223             length = len(_x)
01224           buff.write(struct.pack('<I%ss'%length, length, _x))
01225           _x = _v29
01226           buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01227           _x = _v29.data
01228           length = len(_x)
01229           # - if encoded as a list instead, serialize as bytes instead of string
01230           if type(_x) in [list, tuple]:
01231             buff.write(struct.pack('<I%sB'%length, length, *_x))
01232           else:
01233             buff.write(struct.pack('<I%ss'%length, length, _x))
01234           _v32 = _v25.disparity_image
01235           _v33 = _v32.header
01236           buff.write(_struct_I.pack(_v33.seq))
01237           _v34 = _v33.stamp
01238           _x = _v34
01239           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01240           _x = _v33.frame_id
01241           length = len(_x)
01242           if python3 or type(_x) == unicode:
01243             _x = _x.encode('utf-8')
01244             length = len(_x)
01245           buff.write(struct.pack('<I%ss'%length, length, _x))
01246           _x = _v32
01247           buff.write(_struct_2I.pack(_x.height, _x.width))
01248           _x = _v32.encoding
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           _x = _v32
01255           buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01256           _x = _v32.data
01257           length = len(_x)
01258           # - if encoded as a list instead, serialize as bytes instead of string
01259           if type(_x) in [list, tuple]:
01260             buff.write(struct.pack('<I%sB'%length, length, *_x))
01261           else:
01262             buff.write(struct.pack('<I%ss'%length, length, _x))
01263           _v35 = _v25.cam_info
01264           _v36 = _v35.header
01265           buff.write(_struct_I.pack(_v36.seq))
01266           _v37 = _v36.stamp
01267           _x = _v37
01268           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01269           _x = _v36.frame_id
01270           length = len(_x)
01271           if python3 or type(_x) == unicode:
01272             _x = _x.encode('utf-8')
01273             length = len(_x)
01274           buff.write(struct.pack('<I%ss'%length, length, _x))
01275           _x = _v35
01276           buff.write(_struct_2I.pack(_x.height, _x.width))
01277           _x = _v35.distortion_model
01278           length = len(_x)
01279           if python3 or type(_x) == unicode:
01280             _x = _x.encode('utf-8')
01281             length = len(_x)
01282           buff.write(struct.pack('<I%ss'%length, length, _x))
01283           length = len(_v35.D)
01284           buff.write(_struct_I.pack(length))
01285           pattern = '<%sd'%length
01286           buff.write(struct.pack(pattern, *_v35.D))
01287           buff.write(_struct_9d.pack(*_v35.K))
01288           buff.write(_struct_9d.pack(*_v35.R))
01289           buff.write(_struct_12d.pack(*_v35.P))
01290           _x = _v35
01291           buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01292           _v38 = _v35.roi
01293           _x = _v38
01294           buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01295           _v39 = _v25.roi_box_pose
01296           _v40 = _v39.header
01297           buff.write(_struct_I.pack(_v40.seq))
01298           _v41 = _v40.stamp
01299           _x = _v41
01300           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01301           _x = _v40.frame_id
01302           length = len(_x)
01303           if python3 or type(_x) == unicode:
01304             _x = _x.encode('utf-8')
01305             length = len(_x)
01306           buff.write(struct.pack('<I%ss'%length, length, _x))
01307           _v42 = _v39.pose
01308           _v43 = _v42.position
01309           _x = _v43
01310           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01311           _v44 = _v42.orientation
01312           _x = _v44
01313           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01314           _v45 = _v25.roi_box_dims
01315           _x = _v45
01316           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01317           _x = val2.collision_name
01318           length = len(_x)
01319           if python3 or type(_x) == unicode:
01320             _x = _x.encode('utf-8')
01321             length = len(_x)
01322           buff.write(struct.pack('<I%ss'%length, length, _x))
01323       _x = self
01324       buff.write(_struct_3I.pack(_x.goal.lift.direction.header.seq, _x.goal.lift.direction.header.stamp.secs, _x.goal.lift.direction.header.stamp.nsecs))
01325       _x = self.goal.lift.direction.header.frame_id
01326       length = len(_x)
01327       if python3 or type(_x) == unicode:
01328         _x = _x.encode('utf-8')
01329         length = len(_x)
01330       buff.write(struct.pack('<I%ss'%length, length, _x))
01331       _x = self
01332       buff.write(_struct_3d2f.pack(_x.goal.lift.direction.vector.x, _x.goal.lift.direction.vector.y, _x.goal.lift.direction.vector.z, _x.goal.lift.desired_distance, _x.goal.lift.min_distance))
01333       _x = self.goal.collision_object_name
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 = self.goal.collision_support_surface_name
01340       length = len(_x)
01341       if python3 or type(_x) == unicode:
01342         _x = _x.encode('utf-8')
01343         length = len(_x)
01344       buff.write(struct.pack('<I%ss'%length, length, _x))
01345       _x = self
01346       buff.write(_struct_5B.pack(_x.goal.allow_gripper_support_collision, _x.goal.use_reactive_execution, _x.goal.use_reactive_lift, _x.goal.only_perform_feasibility_test, _x.goal.ignore_collisions))
01347       length = len(self.goal.path_constraints.joint_constraints)
01348       buff.write(_struct_I.pack(length))
01349       for val1 in self.goal.path_constraints.joint_constraints:
01350         _x = val1.joint_name
01351         length = len(_x)
01352         if python3 or type(_x) == unicode:
01353           _x = _x.encode('utf-8')
01354           length = len(_x)
01355         buff.write(struct.pack('<I%ss'%length, length, _x))
01356         _x = val1
01357         buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
01358       length = len(self.goal.path_constraints.position_constraints)
01359       buff.write(_struct_I.pack(length))
01360       for val1 in self.goal.path_constraints.position_constraints:
01361         _v46 = val1.header
01362         buff.write(_struct_I.pack(_v46.seq))
01363         _v47 = _v46.stamp
01364         _x = _v47
01365         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01366         _x = _v46.frame_id
01367         length = len(_x)
01368         if python3 or type(_x) == unicode:
01369           _x = _x.encode('utf-8')
01370           length = len(_x)
01371         buff.write(struct.pack('<I%ss'%length, length, _x))
01372         _x = val1.link_name
01373         length = len(_x)
01374         if python3 or type(_x) == unicode:
01375           _x = _x.encode('utf-8')
01376           length = len(_x)
01377         buff.write(struct.pack('<I%ss'%length, length, _x))
01378         _v48 = val1.target_point_offset
01379         _x = _v48
01380         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01381         _v49 = val1.position
01382         _x = _v49
01383         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01384         _v50 = val1.constraint_region_shape
01385         buff.write(_struct_b.pack(_v50.type))
01386         length = len(_v50.dimensions)
01387         buff.write(_struct_I.pack(length))
01388         pattern = '<%sd'%length
01389         buff.write(struct.pack(pattern, *_v50.dimensions))
01390         length = len(_v50.triangles)
01391         buff.write(_struct_I.pack(length))
01392         pattern = '<%si'%length
01393         buff.write(struct.pack(pattern, *_v50.triangles))
01394         length = len(_v50.vertices)
01395         buff.write(_struct_I.pack(length))
01396         for val3 in _v50.vertices:
01397           _x = val3
01398           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01399         _v51 = val1.constraint_region_orientation
01400         _x = _v51
01401         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01402         buff.write(_struct_d.pack(val1.weight))
01403       length = len(self.goal.path_constraints.orientation_constraints)
01404       buff.write(_struct_I.pack(length))
01405       for val1 in self.goal.path_constraints.orientation_constraints:
01406         _v52 = val1.header
01407         buff.write(_struct_I.pack(_v52.seq))
01408         _v53 = _v52.stamp
01409         _x = _v53
01410         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01411         _x = _v52.frame_id
01412         length = len(_x)
01413         if python3 or type(_x) == unicode:
01414           _x = _x.encode('utf-8')
01415           length = len(_x)
01416         buff.write(struct.pack('<I%ss'%length, length, _x))
01417         _x = val1.link_name
01418         length = len(_x)
01419         if python3 or type(_x) == unicode:
01420           _x = _x.encode('utf-8')
01421           length = len(_x)
01422         buff.write(struct.pack('<I%ss'%length, length, _x))
01423         buff.write(_struct_i.pack(val1.type))
01424         _v54 = val1.orientation
01425         _x = _v54
01426         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01427         _x = val1
01428         buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
01429       length = len(self.goal.path_constraints.visibility_constraints)
01430       buff.write(_struct_I.pack(length))
01431       for val1 in self.goal.path_constraints.visibility_constraints:
01432         _v55 = val1.header
01433         buff.write(_struct_I.pack(_v55.seq))
01434         _v56 = _v55.stamp
01435         _x = _v56
01436         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01437         _x = _v55.frame_id
01438         length = len(_x)
01439         if python3 or type(_x) == unicode:
01440           _x = _x.encode('utf-8')
01441           length = len(_x)
01442         buff.write(struct.pack('<I%ss'%length, length, _x))
01443         _v57 = val1.target
01444         _v58 = _v57.header
01445         buff.write(_struct_I.pack(_v58.seq))
01446         _v59 = _v58.stamp
01447         _x = _v59
01448         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01449         _x = _v58.frame_id
01450         length = len(_x)
01451         if python3 or type(_x) == unicode:
01452           _x = _x.encode('utf-8')
01453           length = len(_x)
01454         buff.write(struct.pack('<I%ss'%length, length, _x))
01455         _v60 = _v57.point
01456         _x = _v60
01457         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01458         _v61 = val1.sensor_pose
01459         _v62 = _v61.header
01460         buff.write(_struct_I.pack(_v62.seq))
01461         _v63 = _v62.stamp
01462         _x = _v63
01463         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01464         _x = _v62.frame_id
01465         length = len(_x)
01466         if python3 or type(_x) == unicode:
01467           _x = _x.encode('utf-8')
01468           length = len(_x)
01469         buff.write(struct.pack('<I%ss'%length, length, _x))
01470         _v64 = _v61.pose
01471         _v65 = _v64.position
01472         _x = _v65
01473         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01474         _v66 = _v64.orientation
01475         _x = _v66
01476         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01477         buff.write(_struct_d.pack(val1.absolute_tolerance))
01478       length = len(self.goal.additional_collision_operations.collision_operations)
01479       buff.write(_struct_I.pack(length))
01480       for val1 in self.goal.additional_collision_operations.collision_operations:
01481         _x = val1.object1
01482         length = len(_x)
01483         if python3 or type(_x) == unicode:
01484           _x = _x.encode('utf-8')
01485           length = len(_x)
01486         buff.write(struct.pack('<I%ss'%length, length, _x))
01487         _x = val1.object2
01488         length = len(_x)
01489         if python3 or type(_x) == unicode:
01490           _x = _x.encode('utf-8')
01491           length = len(_x)
01492         buff.write(struct.pack('<I%ss'%length, length, _x))
01493         _x = val1
01494         buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
01495       length = len(self.goal.additional_link_padding)
01496       buff.write(_struct_I.pack(length))
01497       for val1 in self.goal.additional_link_padding:
01498         _x = val1.link_name
01499         length = len(_x)
01500         if python3 or type(_x) == unicode:
01501           _x = _x.encode('utf-8')
01502           length = len(_x)
01503         buff.write(struct.pack('<I%ss'%length, length, _x))
01504         buff.write(_struct_d.pack(val1.padding))
01505       length = len(self.goal.movable_obstacles)
01506       buff.write(_struct_I.pack(length))
01507       for val1 in self.goal.movable_obstacles:
01508         _x = val1.reference_frame_id
01509         length = len(_x)
01510         if python3 or type(_x) == unicode:
01511           _x = _x.encode('utf-8')
01512           length = len(_x)
01513         buff.write(struct.pack('<I%ss'%length, length, _x))
01514         length = len(val1.potential_models)
01515         buff.write(_struct_I.pack(length))
01516         for val2 in val1.potential_models:
01517           buff.write(_struct_i.pack(val2.model_id))
01518           _v67 = val2.pose
01519           _v68 = _v67.header
01520           buff.write(_struct_I.pack(_v68.seq))
01521           _v69 = _v68.stamp
01522           _x = _v69
01523           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01524           _x = _v68.frame_id
01525           length = len(_x)
01526           if python3 or type(_x) == unicode:
01527             _x = _x.encode('utf-8')
01528             length = len(_x)
01529           buff.write(struct.pack('<I%ss'%length, length, _x))
01530           _v70 = _v67.pose
01531           _v71 = _v70.position
01532           _x = _v71
01533           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01534           _v72 = _v70.orientation
01535           _x = _v72
01536           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01537           buff.write(_struct_f.pack(val2.confidence))
01538           _x = val2.detector_name
01539           length = len(_x)
01540           if python3 or type(_x) == unicode:
01541             _x = _x.encode('utf-8')
01542             length = len(_x)
01543           buff.write(struct.pack('<I%ss'%length, length, _x))
01544         _v73 = val1.cluster
01545         _v74 = _v73.header
01546         buff.write(_struct_I.pack(_v74.seq))
01547         _v75 = _v74.stamp
01548         _x = _v75
01549         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01550         _x = _v74.frame_id
01551         length = len(_x)
01552         if python3 or type(_x) == unicode:
01553           _x = _x.encode('utf-8')
01554           length = len(_x)
01555         buff.write(struct.pack('<I%ss'%length, length, _x))
01556         length = len(_v73.points)
01557         buff.write(_struct_I.pack(length))
01558         for val3 in _v73.points:
01559           _x = val3
01560           buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01561         length = len(_v73.channels)
01562         buff.write(_struct_I.pack(length))
01563         for val3 in _v73.channels:
01564           _x = val3.name
01565           length = len(_x)
01566           if python3 or type(_x) == unicode:
01567             _x = _x.encode('utf-8')
01568             length = len(_x)
01569           buff.write(struct.pack('<I%ss'%length, length, _x))
01570           length = len(val3.values)
01571           buff.write(_struct_I.pack(length))
01572           pattern = '<%sf'%length
01573           buff.write(struct.pack(pattern, *val3.values))
01574         _v76 = val1.region
01575         _v77 = _v76.cloud
01576         _v78 = _v77.header
01577         buff.write(_struct_I.pack(_v78.seq))
01578         _v79 = _v78.stamp
01579         _x = _v79
01580         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01581         _x = _v78.frame_id
01582         length = len(_x)
01583         if python3 or type(_x) == unicode:
01584           _x = _x.encode('utf-8')
01585           length = len(_x)
01586         buff.write(struct.pack('<I%ss'%length, length, _x))
01587         _x = _v77
01588         buff.write(_struct_2I.pack(_x.height, _x.width))
01589         length = len(_v77.fields)
01590         buff.write(_struct_I.pack(length))
01591         for val4 in _v77.fields:
01592           _x = val4.name
01593           length = len(_x)
01594           if python3 or type(_x) == unicode:
01595             _x = _x.encode('utf-8')
01596             length = len(_x)
01597           buff.write(struct.pack('<I%ss'%length, length, _x))
01598           _x = val4
01599           buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01600         _x = _v77
01601         buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
01602         _x = _v77.data
01603         length = len(_x)
01604         # - if encoded as a list instead, serialize as bytes instead of string
01605         if type(_x) in [list, tuple]:
01606           buff.write(struct.pack('<I%sB'%length, length, *_x))
01607         else:
01608           buff.write(struct.pack('<I%ss'%length, length, _x))
01609         buff.write(_struct_B.pack(_v77.is_dense))
01610         length = len(_v76.mask)
01611         buff.write(_struct_I.pack(length))
01612         pattern = '<%si'%length
01613         buff.write(struct.pack(pattern, *_v76.mask))
01614         _v80 = _v76.image
01615         _v81 = _v80.header
01616         buff.write(_struct_I.pack(_v81.seq))
01617         _v82 = _v81.stamp
01618         _x = _v82
01619         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01620         _x = _v81.frame_id
01621         length = len(_x)
01622         if python3 or type(_x) == unicode:
01623           _x = _x.encode('utf-8')
01624           length = len(_x)
01625         buff.write(struct.pack('<I%ss'%length, length, _x))
01626         _x = _v80
01627         buff.write(_struct_2I.pack(_x.height, _x.width))
01628         _x = _v80.encoding
01629         length = len(_x)
01630         if python3 or type(_x) == unicode:
01631           _x = _x.encode('utf-8')
01632           length = len(_x)
01633         buff.write(struct.pack('<I%ss'%length, length, _x))
01634         _x = _v80
01635         buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01636         _x = _v80.data
01637         length = len(_x)
01638         # - if encoded as a list instead, serialize as bytes instead of string
01639         if type(_x) in [list, tuple]:
01640           buff.write(struct.pack('<I%sB'%length, length, *_x))
01641         else:
01642           buff.write(struct.pack('<I%ss'%length, length, _x))
01643         _v83 = _v76.disparity_image
01644         _v84 = _v83.header
01645         buff.write(_struct_I.pack(_v84.seq))
01646         _v85 = _v84.stamp
01647         _x = _v85
01648         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01649         _x = _v84.frame_id
01650         length = len(_x)
01651         if python3 or type(_x) == unicode:
01652           _x = _x.encode('utf-8')
01653           length = len(_x)
01654         buff.write(struct.pack('<I%ss'%length, length, _x))
01655         _x = _v83
01656         buff.write(_struct_2I.pack(_x.height, _x.width))
01657         _x = _v83.encoding
01658         length = len(_x)
01659         if python3 or type(_x) == unicode:
01660           _x = _x.encode('utf-8')
01661           length = len(_x)
01662         buff.write(struct.pack('<I%ss'%length, length, _x))
01663         _x = _v83
01664         buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
01665         _x = _v83.data
01666         length = len(_x)
01667         # - if encoded as a list instead, serialize as bytes instead of string
01668         if type(_x) in [list, tuple]:
01669           buff.write(struct.pack('<I%sB'%length, length, *_x))
01670         else:
01671           buff.write(struct.pack('<I%ss'%length, length, _x))
01672         _v86 = _v76.cam_info
01673         _v87 = _v86.header
01674         buff.write(_struct_I.pack(_v87.seq))
01675         _v88 = _v87.stamp
01676         _x = _v88
01677         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01678         _x = _v87.frame_id
01679         length = len(_x)
01680         if python3 or type(_x) == unicode:
01681           _x = _x.encode('utf-8')
01682           length = len(_x)
01683         buff.write(struct.pack('<I%ss'%length, length, _x))
01684         _x = _v86
01685         buff.write(_struct_2I.pack(_x.height, _x.width))
01686         _x = _v86.distortion_model
01687         length = len(_x)
01688         if python3 or type(_x) == unicode:
01689           _x = _x.encode('utf-8')
01690           length = len(_x)
01691         buff.write(struct.pack('<I%ss'%length, length, _x))
01692         length = len(_v86.D)
01693         buff.write(_struct_I.pack(length))
01694         pattern = '<%sd'%length
01695         buff.write(struct.pack(pattern, *_v86.D))
01696         buff.write(_struct_9d.pack(*_v86.K))
01697         buff.write(_struct_9d.pack(*_v86.R))
01698         buff.write(_struct_12d.pack(*_v86.P))
01699         _x = _v86
01700         buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
01701         _v89 = _v86.roi
01702         _x = _v89
01703         buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
01704         _v90 = _v76.roi_box_pose
01705         _v91 = _v90.header
01706         buff.write(_struct_I.pack(_v91.seq))
01707         _v92 = _v91.stamp
01708         _x = _v92
01709         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01710         _x = _v91.frame_id
01711         length = len(_x)
01712         if python3 or type(_x) == unicode:
01713           _x = _x.encode('utf-8')
01714           length = len(_x)
01715         buff.write(struct.pack('<I%ss'%length, length, _x))
01716         _v93 = _v90.pose
01717         _v94 = _v93.position
01718         _x = _v94
01719         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01720         _v95 = _v93.orientation
01721         _x = _v95
01722         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01723         _v96 = _v76.roi_box_dims
01724         _x = _v96
01725         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01726         _x = val1.collision_name
01727         length = len(_x)
01728         if python3 or type(_x) == unicode:
01729           _x = _x.encode('utf-8')
01730           length = len(_x)
01731         buff.write(struct.pack('<I%ss'%length, length, _x))
01732       buff.write(_struct_f.pack(self.goal.max_contact_force))
01733     except struct.error as se: self._check_types(se)
01734     except TypeError as te: self._check_types(te)
01735 
01736   def deserialize(self, str):
01737     """
01738     unpack serialized message in str into this message instance
01739     :param str: byte array of serialized message, ``str``
01740     """
01741     try:
01742       if self.header is None:
01743         self.header = std_msgs.msg.Header()
01744       if self.goal_id is None:
01745         self.goal_id = actionlib_msgs.msg.GoalID()
01746       if self.goal is None:
01747         self.goal = object_manipulation_msgs.msg.PickupGoal()
01748       end = 0
01749       _x = self
01750       start = end
01751       end += 12
01752       (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01753       start = end
01754       end += 4
01755       (length,) = _struct_I.unpack(str[start:end])
01756       start = end
01757       end += length
01758       if python3:
01759         self.header.frame_id = str[start:end].decode('utf-8')
01760       else:
01761         self.header.frame_id = str[start:end]
01762       _x = self
01763       start = end
01764       end += 8
01765       (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01766       start = end
01767       end += 4
01768       (length,) = _struct_I.unpack(str[start:end])
01769       start = end
01770       end += length
01771       if python3:
01772         self.goal_id.id = str[start:end].decode('utf-8')
01773       else:
01774         self.goal_id.id = str[start:end]
01775       start = end
01776       end += 4
01777       (length,) = _struct_I.unpack(str[start:end])
01778       start = end
01779       end += length
01780       if python3:
01781         self.goal.arm_name = str[start:end].decode('utf-8')
01782       else:
01783         self.goal.arm_name = str[start:end]
01784       start = end
01785       end += 4
01786       (length,) = _struct_I.unpack(str[start:end])
01787       start = end
01788       end += length
01789       if python3:
01790         self.goal.target.reference_frame_id = str[start:end].decode('utf-8')
01791       else:
01792         self.goal.target.reference_frame_id = str[start:end]
01793       start = end
01794       end += 4
01795       (length,) = _struct_I.unpack(str[start:end])
01796       self.goal.target.potential_models = []
01797       for i in range(0, length):
01798         val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01799         start = end
01800         end += 4
01801         (val1.model_id,) = _struct_i.unpack(str[start:end])
01802         _v97 = val1.pose
01803         _v98 = _v97.header
01804         start = end
01805         end += 4
01806         (_v98.seq,) = _struct_I.unpack(str[start:end])
01807         _v99 = _v98.stamp
01808         _x = _v99
01809         start = end
01810         end += 8
01811         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01812         start = end
01813         end += 4
01814         (length,) = _struct_I.unpack(str[start:end])
01815         start = end
01816         end += length
01817         if python3:
01818           _v98.frame_id = str[start:end].decode('utf-8')
01819         else:
01820           _v98.frame_id = str[start:end]
01821         _v100 = _v97.pose
01822         _v101 = _v100.position
01823         _x = _v101
01824         start = end
01825         end += 24
01826         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01827         _v102 = _v100.orientation
01828         _x = _v102
01829         start = end
01830         end += 32
01831         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01832         start = end
01833         end += 4
01834         (val1.confidence,) = _struct_f.unpack(str[start:end])
01835         start = end
01836         end += 4
01837         (length,) = _struct_I.unpack(str[start:end])
01838         start = end
01839         end += length
01840         if python3:
01841           val1.detector_name = str[start:end].decode('utf-8')
01842         else:
01843           val1.detector_name = str[start:end]
01844         self.goal.target.potential_models.append(val1)
01845       _x = self
01846       start = end
01847       end += 12
01848       (_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01849       start = end
01850       end += 4
01851       (length,) = _struct_I.unpack(str[start:end])
01852       start = end
01853       end += length
01854       if python3:
01855         self.goal.target.cluster.header.frame_id = str[start:end].decode('utf-8')
01856       else:
01857         self.goal.target.cluster.header.frame_id = str[start:end]
01858       start = end
01859       end += 4
01860       (length,) = _struct_I.unpack(str[start:end])
01861       self.goal.target.cluster.points = []
01862       for i in range(0, length):
01863         val1 = geometry_msgs.msg.Point32()
01864         _x = val1
01865         start = end
01866         end += 12
01867         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01868         self.goal.target.cluster.points.append(val1)
01869       start = end
01870       end += 4
01871       (length,) = _struct_I.unpack(str[start:end])
01872       self.goal.target.cluster.channels = []
01873       for i in range(0, length):
01874         val1 = sensor_msgs.msg.ChannelFloat32()
01875         start = end
01876         end += 4
01877         (length,) = _struct_I.unpack(str[start:end])
01878         start = end
01879         end += length
01880         if python3:
01881           val1.name = str[start:end].decode('utf-8')
01882         else:
01883           val1.name = str[start:end]
01884         start = end
01885         end += 4
01886         (length,) = _struct_I.unpack(str[start:end])
01887         pattern = '<%sf'%length
01888         start = end
01889         end += struct.calcsize(pattern)
01890         val1.values = struct.unpack(pattern, str[start:end])
01891         self.goal.target.cluster.channels.append(val1)
01892       _x = self
01893       start = end
01894       end += 12
01895       (_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01896       start = end
01897       end += 4
01898       (length,) = _struct_I.unpack(str[start:end])
01899       start = end
01900       end += length
01901       if python3:
01902         self.goal.target.region.cloud.header.frame_id = str[start:end].decode('utf-8')
01903       else:
01904         self.goal.target.region.cloud.header.frame_id = str[start:end]
01905       _x = self
01906       start = end
01907       end += 8
01908       (_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01909       start = end
01910       end += 4
01911       (length,) = _struct_I.unpack(str[start:end])
01912       self.goal.target.region.cloud.fields = []
01913       for i in range(0, length):
01914         val1 = sensor_msgs.msg.PointField()
01915         start = end
01916         end += 4
01917         (length,) = _struct_I.unpack(str[start:end])
01918         start = end
01919         end += length
01920         if python3:
01921           val1.name = str[start:end].decode('utf-8')
01922         else:
01923           val1.name = str[start:end]
01924         _x = val1
01925         start = end
01926         end += 9
01927         (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01928         self.goal.target.region.cloud.fields.append(val1)
01929       _x = self
01930       start = end
01931       end += 9
01932       (_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01933       self.goal.target.region.cloud.is_bigendian = bool(self.goal.target.region.cloud.is_bigendian)
01934       start = end
01935       end += 4
01936       (length,) = _struct_I.unpack(str[start:end])
01937       start = end
01938       end += length
01939       if python3:
01940         self.goal.target.region.cloud.data = str[start:end].decode('utf-8')
01941       else:
01942         self.goal.target.region.cloud.data = str[start:end]
01943       start = end
01944       end += 1
01945       (self.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01946       self.goal.target.region.cloud.is_dense = bool(self.goal.target.region.cloud.is_dense)
01947       start = end
01948       end += 4
01949       (length,) = _struct_I.unpack(str[start:end])
01950       pattern = '<%si'%length
01951       start = end
01952       end += struct.calcsize(pattern)
01953       self.goal.target.region.mask = struct.unpack(pattern, str[start:end])
01954       _x = self
01955       start = end
01956       end += 12
01957       (_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01958       start = end
01959       end += 4
01960       (length,) = _struct_I.unpack(str[start:end])
01961       start = end
01962       end += length
01963       if python3:
01964         self.goal.target.region.image.header.frame_id = str[start:end].decode('utf-8')
01965       else:
01966         self.goal.target.region.image.header.frame_id = str[start:end]
01967       _x = self
01968       start = end
01969       end += 8
01970       (_x.goal.target.region.image.height, _x.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
01971       start = end
01972       end += 4
01973       (length,) = _struct_I.unpack(str[start:end])
01974       start = end
01975       end += length
01976       if python3:
01977         self.goal.target.region.image.encoding = str[start:end].decode('utf-8')
01978       else:
01979         self.goal.target.region.image.encoding = str[start:end]
01980       _x = self
01981       start = end
01982       end += 5
01983       (_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
01984       start = end
01985       end += 4
01986       (length,) = _struct_I.unpack(str[start:end])
01987       start = end
01988       end += length
01989       if python3:
01990         self.goal.target.region.image.data = str[start:end].decode('utf-8')
01991       else:
01992         self.goal.target.region.image.data = str[start:end]
01993       _x = self
01994       start = end
01995       end += 12
01996       (_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01997       start = end
01998       end += 4
01999       (length,) = _struct_I.unpack(str[start:end])
02000       start = end
02001       end += length
02002       if python3:
02003         self.goal.target.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
02004       else:
02005         self.goal.target.region.disparity_image.header.frame_id = str[start:end]
02006       _x = self
02007       start = end
02008       end += 8
02009       (_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
02010       start = end
02011       end += 4
02012       (length,) = _struct_I.unpack(str[start:end])
02013       start = end
02014       end += length
02015       if python3:
02016         self.goal.target.region.disparity_image.encoding = str[start:end].decode('utf-8')
02017       else:
02018         self.goal.target.region.disparity_image.encoding = str[start:end]
02019       _x = self
02020       start = end
02021       end += 5
02022       (_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
02023       start = end
02024       end += 4
02025       (length,) = _struct_I.unpack(str[start:end])
02026       start = end
02027       end += length
02028       if python3:
02029         self.goal.target.region.disparity_image.data = str[start:end].decode('utf-8')
02030       else:
02031         self.goal.target.region.disparity_image.data = str[start:end]
02032       _x = self
02033       start = end
02034       end += 12
02035       (_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02036       start = end
02037       end += 4
02038       (length,) = _struct_I.unpack(str[start:end])
02039       start = end
02040       end += length
02041       if python3:
02042         self.goal.target.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
02043       else:
02044         self.goal.target.region.cam_info.header.frame_id = str[start:end]
02045       _x = self
02046       start = end
02047       end += 8
02048       (_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
02049       start = end
02050       end += 4
02051       (length,) = _struct_I.unpack(str[start:end])
02052       start = end
02053       end += length
02054       if python3:
02055         self.goal.target.region.cam_info.distortion_model = str[start:end].decode('utf-8')
02056       else:
02057         self.goal.target.region.cam_info.distortion_model = str[start:end]
02058       start = end
02059       end += 4
02060       (length,) = _struct_I.unpack(str[start:end])
02061       pattern = '<%sd'%length
02062       start = end
02063       end += struct.calcsize(pattern)
02064       self.goal.target.region.cam_info.D = struct.unpack(pattern, str[start:end])
02065       start = end
02066       end += 72
02067       self.goal.target.region.cam_info.K = _struct_9d.unpack(str[start:end])
02068       start = end
02069       end += 72
02070       self.goal.target.region.cam_info.R = _struct_9d.unpack(str[start:end])
02071       start = end
02072       end += 96
02073       self.goal.target.region.cam_info.P = _struct_12d.unpack(str[start:end])
02074       _x = self
02075       start = end
02076       end += 37
02077       (_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
02078       self.goal.target.region.cam_info.roi.do_rectify = bool(self.goal.target.region.cam_info.roi.do_rectify)
02079       start = end
02080       end += 4
02081       (length,) = _struct_I.unpack(str[start:end])
02082       start = end
02083       end += length
02084       if python3:
02085         self.goal.target.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
02086       else:
02087         self.goal.target.region.roi_box_pose.header.frame_id = str[start:end]
02088       _x = self
02089       start = end
02090       end += 80
02091       (_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
02092       start = end
02093       end += 4
02094       (length,) = _struct_I.unpack(str[start:end])
02095       start = end
02096       end += length
02097       if python3:
02098         self.goal.target.collision_name = str[start:end].decode('utf-8')
02099       else:
02100         self.goal.target.collision_name = str[start:end]
02101       start = end
02102       end += 4
02103       (length,) = _struct_I.unpack(str[start:end])
02104       self.goal.desired_grasps = []
02105       for i in range(0, length):
02106         val1 = object_manipulation_msgs.msg.Grasp()
02107         _v103 = val1.pre_grasp_posture
02108         _v104 = _v103.header
02109         start = end
02110         end += 4
02111         (_v104.seq,) = _struct_I.unpack(str[start:end])
02112         _v105 = _v104.stamp
02113         _x = _v105
02114         start = end
02115         end += 8
02116         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02117         start = end
02118         end += 4
02119         (length,) = _struct_I.unpack(str[start:end])
02120         start = end
02121         end += length
02122         if python3:
02123           _v104.frame_id = str[start:end].decode('utf-8')
02124         else:
02125           _v104.frame_id = str[start:end]
02126         start = end
02127         end += 4
02128         (length,) = _struct_I.unpack(str[start:end])
02129         _v103.name = []
02130         for i in range(0, length):
02131           start = end
02132           end += 4
02133           (length,) = _struct_I.unpack(str[start:end])
02134           start = end
02135           end += length
02136           if python3:
02137             val3 = str[start:end].decode('utf-8')
02138           else:
02139             val3 = str[start:end]
02140           _v103.name.append(val3)
02141         start = end
02142         end += 4
02143         (length,) = _struct_I.unpack(str[start:end])
02144         pattern = '<%sd'%length
02145         start = end
02146         end += struct.calcsize(pattern)
02147         _v103.position = struct.unpack(pattern, str[start:end])
02148         start = end
02149         end += 4
02150         (length,) = _struct_I.unpack(str[start:end])
02151         pattern = '<%sd'%length
02152         start = end
02153         end += struct.calcsize(pattern)
02154         _v103.velocity = struct.unpack(pattern, str[start:end])
02155         start = end
02156         end += 4
02157         (length,) = _struct_I.unpack(str[start:end])
02158         pattern = '<%sd'%length
02159         start = end
02160         end += struct.calcsize(pattern)
02161         _v103.effort = struct.unpack(pattern, str[start:end])
02162         _v106 = val1.grasp_posture
02163         _v107 = _v106.header
02164         start = end
02165         end += 4
02166         (_v107.seq,) = _struct_I.unpack(str[start:end])
02167         _v108 = _v107.stamp
02168         _x = _v108
02169         start = end
02170         end += 8
02171         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02172         start = end
02173         end += 4
02174         (length,) = _struct_I.unpack(str[start:end])
02175         start = end
02176         end += length
02177         if python3:
02178           _v107.frame_id = str[start:end].decode('utf-8')
02179         else:
02180           _v107.frame_id = str[start:end]
02181         start = end
02182         end += 4
02183         (length,) = _struct_I.unpack(str[start:end])
02184         _v106.name = []
02185         for i in range(0, length):
02186           start = end
02187           end += 4
02188           (length,) = _struct_I.unpack(str[start:end])
02189           start = end
02190           end += length
02191           if python3:
02192             val3 = str[start:end].decode('utf-8')
02193           else:
02194             val3 = str[start:end]
02195           _v106.name.append(val3)
02196         start = end
02197         end += 4
02198         (length,) = _struct_I.unpack(str[start:end])
02199         pattern = '<%sd'%length
02200         start = end
02201         end += struct.calcsize(pattern)
02202         _v106.position = struct.unpack(pattern, str[start:end])
02203         start = end
02204         end += 4
02205         (length,) = _struct_I.unpack(str[start:end])
02206         pattern = '<%sd'%length
02207         start = end
02208         end += struct.calcsize(pattern)
02209         _v106.velocity = struct.unpack(pattern, str[start:end])
02210         start = end
02211         end += 4
02212         (length,) = _struct_I.unpack(str[start:end])
02213         pattern = '<%sd'%length
02214         start = end
02215         end += struct.calcsize(pattern)
02216         _v106.effort = struct.unpack(pattern, str[start:end])
02217         _v109 = val1.grasp_pose
02218         _v110 = _v109.position
02219         _x = _v110
02220         start = end
02221         end += 24
02222         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02223         _v111 = _v109.orientation
02224         _x = _v111
02225         start = end
02226         end += 32
02227         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02228         _x = val1
02229         start = end
02230         end += 17
02231         (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
02232         val1.cluster_rep = bool(val1.cluster_rep)
02233         start = end
02234         end += 4
02235         (length,) = _struct_I.unpack(str[start:end])
02236         val1.moved_obstacles = []
02237         for i in range(0, length):
02238           val2 = object_manipulation_msgs.msg.GraspableObject()
02239           start = end
02240           end += 4
02241           (length,) = _struct_I.unpack(str[start:end])
02242           start = end
02243           end += length
02244           if python3:
02245             val2.reference_frame_id = str[start:end].decode('utf-8')
02246           else:
02247             val2.reference_frame_id = str[start:end]
02248           start = end
02249           end += 4
02250           (length,) = _struct_I.unpack(str[start:end])
02251           val2.potential_models = []
02252           for i in range(0, length):
02253             val3 = household_objects_database_msgs.msg.DatabaseModelPose()
02254             start = end
02255             end += 4
02256             (val3.model_id,) = _struct_i.unpack(str[start:end])
02257             _v112 = val3.pose
02258             _v113 = _v112.header
02259             start = end
02260             end += 4
02261             (_v113.seq,) = _struct_I.unpack(str[start:end])
02262             _v114 = _v113.stamp
02263             _x = _v114
02264             start = end
02265             end += 8
02266             (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02267             start = end
02268             end += 4
02269             (length,) = _struct_I.unpack(str[start:end])
02270             start = end
02271             end += length
02272             if python3:
02273               _v113.frame_id = str[start:end].decode('utf-8')
02274             else:
02275               _v113.frame_id = str[start:end]
02276             _v115 = _v112.pose
02277             _v116 = _v115.position
02278             _x = _v116
02279             start = end
02280             end += 24
02281             (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02282             _v117 = _v115.orientation
02283             _x = _v117
02284             start = end
02285             end += 32
02286             (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02287             start = end
02288             end += 4
02289             (val3.confidence,) = _struct_f.unpack(str[start:end])
02290             start = end
02291             end += 4
02292             (length,) = _struct_I.unpack(str[start:end])
02293             start = end
02294             end += length
02295             if python3:
02296               val3.detector_name = str[start:end].decode('utf-8')
02297             else:
02298               val3.detector_name = str[start:end]
02299             val2.potential_models.append(val3)
02300           _v118 = val2.cluster
02301           _v119 = _v118.header
02302           start = end
02303           end += 4
02304           (_v119.seq,) = _struct_I.unpack(str[start:end])
02305           _v120 = _v119.stamp
02306           _x = _v120
02307           start = end
02308           end += 8
02309           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02310           start = end
02311           end += 4
02312           (length,) = _struct_I.unpack(str[start:end])
02313           start = end
02314           end += length
02315           if python3:
02316             _v119.frame_id = str[start:end].decode('utf-8')
02317           else:
02318             _v119.frame_id = str[start:end]
02319           start = end
02320           end += 4
02321           (length,) = _struct_I.unpack(str[start:end])
02322           _v118.points = []
02323           for i in range(0, length):
02324             val4 = geometry_msgs.msg.Point32()
02325             _x = val4
02326             start = end
02327             end += 12
02328             (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02329             _v118.points.append(val4)
02330           start = end
02331           end += 4
02332           (length,) = _struct_I.unpack(str[start:end])
02333           _v118.channels = []
02334           for i in range(0, length):
02335             val4 = sensor_msgs.msg.ChannelFloat32()
02336             start = end
02337             end += 4
02338             (length,) = _struct_I.unpack(str[start:end])
02339             start = end
02340             end += length
02341             if python3:
02342               val4.name = str[start:end].decode('utf-8')
02343             else:
02344               val4.name = str[start:end]
02345             start = end
02346             end += 4
02347             (length,) = _struct_I.unpack(str[start:end])
02348             pattern = '<%sf'%length
02349             start = end
02350             end += struct.calcsize(pattern)
02351             val4.values = struct.unpack(pattern, str[start:end])
02352             _v118.channels.append(val4)
02353           _v121 = val2.region
02354           _v122 = _v121.cloud
02355           _v123 = _v122.header
02356           start = end
02357           end += 4
02358           (_v123.seq,) = _struct_I.unpack(str[start:end])
02359           _v124 = _v123.stamp
02360           _x = _v124
02361           start = end
02362           end += 8
02363           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02364           start = end
02365           end += 4
02366           (length,) = _struct_I.unpack(str[start:end])
02367           start = end
02368           end += length
02369           if python3:
02370             _v123.frame_id = str[start:end].decode('utf-8')
02371           else:
02372             _v123.frame_id = str[start:end]
02373           _x = _v122
02374           start = end
02375           end += 8
02376           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02377           start = end
02378           end += 4
02379           (length,) = _struct_I.unpack(str[start:end])
02380           _v122.fields = []
02381           for i in range(0, length):
02382             val5 = sensor_msgs.msg.PointField()
02383             start = end
02384             end += 4
02385             (length,) = _struct_I.unpack(str[start:end])
02386             start = end
02387             end += length
02388             if python3:
02389               val5.name = str[start:end].decode('utf-8')
02390             else:
02391               val5.name = str[start:end]
02392             _x = val5
02393             start = end
02394             end += 9
02395             (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02396             _v122.fields.append(val5)
02397           _x = _v122
02398           start = end
02399           end += 9
02400           (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
02401           _v122.is_bigendian = bool(_v122.is_bigendian)
02402           start = end
02403           end += 4
02404           (length,) = _struct_I.unpack(str[start:end])
02405           start = end
02406           end += length
02407           if python3:
02408             _v122.data = str[start:end].decode('utf-8')
02409           else:
02410             _v122.data = str[start:end]
02411           start = end
02412           end += 1
02413           (_v122.is_dense,) = _struct_B.unpack(str[start:end])
02414           _v122.is_dense = bool(_v122.is_dense)
02415           start = end
02416           end += 4
02417           (length,) = _struct_I.unpack(str[start:end])
02418           pattern = '<%si'%length
02419           start = end
02420           end += struct.calcsize(pattern)
02421           _v121.mask = struct.unpack(pattern, str[start:end])
02422           _v125 = _v121.image
02423           _v126 = _v125.header
02424           start = end
02425           end += 4
02426           (_v126.seq,) = _struct_I.unpack(str[start:end])
02427           _v127 = _v126.stamp
02428           _x = _v127
02429           start = end
02430           end += 8
02431           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02432           start = end
02433           end += 4
02434           (length,) = _struct_I.unpack(str[start:end])
02435           start = end
02436           end += length
02437           if python3:
02438             _v126.frame_id = str[start:end].decode('utf-8')
02439           else:
02440             _v126.frame_id = str[start:end]
02441           _x = _v125
02442           start = end
02443           end += 8
02444           (_x.height, _x.width,) = _struct_2I.unpack(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             _v125.encoding = str[start:end].decode('utf-8')
02452           else:
02453             _v125.encoding = str[start:end]
02454           _x = _v125
02455           start = end
02456           end += 5
02457           (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02458           start = end
02459           end += 4
02460           (length,) = _struct_I.unpack(str[start:end])
02461           start = end
02462           end += length
02463           if python3:
02464             _v125.data = str[start:end].decode('utf-8')
02465           else:
02466             _v125.data = str[start:end]
02467           _v128 = _v121.disparity_image
02468           _v129 = _v128.header
02469           start = end
02470           end += 4
02471           (_v129.seq,) = _struct_I.unpack(str[start:end])
02472           _v130 = _v129.stamp
02473           _x = _v130
02474           start = end
02475           end += 8
02476           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02477           start = end
02478           end += 4
02479           (length,) = _struct_I.unpack(str[start:end])
02480           start = end
02481           end += length
02482           if python3:
02483             _v129.frame_id = str[start:end].decode('utf-8')
02484           else:
02485             _v129.frame_id = str[start:end]
02486           _x = _v128
02487           start = end
02488           end += 8
02489           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02490           start = end
02491           end += 4
02492           (length,) = _struct_I.unpack(str[start:end])
02493           start = end
02494           end += length
02495           if python3:
02496             _v128.encoding = str[start:end].decode('utf-8')
02497           else:
02498             _v128.encoding = str[start:end]
02499           _x = _v128
02500           start = end
02501           end += 5
02502           (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
02503           start = end
02504           end += 4
02505           (length,) = _struct_I.unpack(str[start:end])
02506           start = end
02507           end += length
02508           if python3:
02509             _v128.data = str[start:end].decode('utf-8')
02510           else:
02511             _v128.data = str[start:end]
02512           _v131 = _v121.cam_info
02513           _v132 = _v131.header
02514           start = end
02515           end += 4
02516           (_v132.seq,) = _struct_I.unpack(str[start:end])
02517           _v133 = _v132.stamp
02518           _x = _v133
02519           start = end
02520           end += 8
02521           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02522           start = end
02523           end += 4
02524           (length,) = _struct_I.unpack(str[start:end])
02525           start = end
02526           end += length
02527           if python3:
02528             _v132.frame_id = str[start:end].decode('utf-8')
02529           else:
02530             _v132.frame_id = str[start:end]
02531           _x = _v131
02532           start = end
02533           end += 8
02534           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
02535           start = end
02536           end += 4
02537           (length,) = _struct_I.unpack(str[start:end])
02538           start = end
02539           end += length
02540           if python3:
02541             _v131.distortion_model = str[start:end].decode('utf-8')
02542           else:
02543             _v131.distortion_model = str[start:end]
02544           start = end
02545           end += 4
02546           (length,) = _struct_I.unpack(str[start:end])
02547           pattern = '<%sd'%length
02548           start = end
02549           end += struct.calcsize(pattern)
02550           _v131.D = struct.unpack(pattern, str[start:end])
02551           start = end
02552           end += 72
02553           _v131.K = _struct_9d.unpack(str[start:end])
02554           start = end
02555           end += 72
02556           _v131.R = _struct_9d.unpack(str[start:end])
02557           start = end
02558           end += 96
02559           _v131.P = _struct_12d.unpack(str[start:end])
02560           _x = _v131
02561           start = end
02562           end += 8
02563           (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
02564           _v134 = _v131.roi
02565           _x = _v134
02566           start = end
02567           end += 17
02568           (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
02569           _v134.do_rectify = bool(_v134.do_rectify)
02570           _v135 = _v121.roi_box_pose
02571           _v136 = _v135.header
02572           start = end
02573           end += 4
02574           (_v136.seq,) = _struct_I.unpack(str[start:end])
02575           _v137 = _v136.stamp
02576           _x = _v137
02577           start = end
02578           end += 8
02579           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02580           start = end
02581           end += 4
02582           (length,) = _struct_I.unpack(str[start:end])
02583           start = end
02584           end += length
02585           if python3:
02586             _v136.frame_id = str[start:end].decode('utf-8')
02587           else:
02588             _v136.frame_id = str[start:end]
02589           _v138 = _v135.pose
02590           _v139 = _v138.position
02591           _x = _v139
02592           start = end
02593           end += 24
02594           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02595           _v140 = _v138.orientation
02596           _x = _v140
02597           start = end
02598           end += 32
02599           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02600           _v141 = _v121.roi_box_dims
02601           _x = _v141
02602           start = end
02603           end += 24
02604           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02605           start = end
02606           end += 4
02607           (length,) = _struct_I.unpack(str[start:end])
02608           start = end
02609           end += length
02610           if python3:
02611             val2.collision_name = str[start:end].decode('utf-8')
02612           else:
02613             val2.collision_name = str[start:end]
02614           val1.moved_obstacles.append(val2)
02615         self.goal.desired_grasps.append(val1)
02616       _x = self
02617       start = end
02618       end += 12
02619       (_x.goal.lift.direction.header.seq, _x.goal.lift.direction.header.stamp.secs, _x.goal.lift.direction.header.stamp.nsecs,) = _struct_3I.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         self.goal.lift.direction.header.frame_id = str[start:end].decode('utf-8')
02627       else:
02628         self.goal.lift.direction.header.frame_id = str[start:end]
02629       _x = self
02630       start = end
02631       end += 32
02632       (_x.goal.lift.direction.vector.x, _x.goal.lift.direction.vector.y, _x.goal.lift.direction.vector.z, _x.goal.lift.desired_distance, _x.goal.lift.min_distance,) = _struct_3d2f.unpack(str[start:end])
02633       start = end
02634       end += 4
02635       (length,) = _struct_I.unpack(str[start:end])
02636       start = end
02637       end += length
02638       if python3:
02639         self.goal.collision_object_name = str[start:end].decode('utf-8')
02640       else:
02641         self.goal.collision_object_name = str[start:end]
02642       start = end
02643       end += 4
02644       (length,) = _struct_I.unpack(str[start:end])
02645       start = end
02646       end += length
02647       if python3:
02648         self.goal.collision_support_surface_name = str[start:end].decode('utf-8')
02649       else:
02650         self.goal.collision_support_surface_name = str[start:end]
02651       _x = self
02652       start = end
02653       end += 5
02654       (_x.goal.allow_gripper_support_collision, _x.goal.use_reactive_execution, _x.goal.use_reactive_lift, _x.goal.only_perform_feasibility_test, _x.goal.ignore_collisions,) = _struct_5B.unpack(str[start:end])
02655       self.goal.allow_gripper_support_collision = bool(self.goal.allow_gripper_support_collision)
02656       self.goal.use_reactive_execution = bool(self.goal.use_reactive_execution)
02657       self.goal.use_reactive_lift = bool(self.goal.use_reactive_lift)
02658       self.goal.only_perform_feasibility_test = bool(self.goal.only_perform_feasibility_test)
02659       self.goal.ignore_collisions = bool(self.goal.ignore_collisions)
02660       start = end
02661       end += 4
02662       (length,) = _struct_I.unpack(str[start:end])
02663       self.goal.path_constraints.joint_constraints = []
02664       for i in range(0, length):
02665         val1 = arm_navigation_msgs.msg.JointConstraint()
02666         start = end
02667         end += 4
02668         (length,) = _struct_I.unpack(str[start:end])
02669         start = end
02670         end += length
02671         if python3:
02672           val1.joint_name = str[start:end].decode('utf-8')
02673         else:
02674           val1.joint_name = str[start:end]
02675         _x = val1
02676         start = end
02677         end += 32
02678         (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
02679         self.goal.path_constraints.joint_constraints.append(val1)
02680       start = end
02681       end += 4
02682       (length,) = _struct_I.unpack(str[start:end])
02683       self.goal.path_constraints.position_constraints = []
02684       for i in range(0, length):
02685         val1 = arm_navigation_msgs.msg.PositionConstraint()
02686         _v142 = val1.header
02687         start = end
02688         end += 4
02689         (_v142.seq,) = _struct_I.unpack(str[start:end])
02690         _v143 = _v142.stamp
02691         _x = _v143
02692         start = end
02693         end += 8
02694         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02695         start = end
02696         end += 4
02697         (length,) = _struct_I.unpack(str[start:end])
02698         start = end
02699         end += length
02700         if python3:
02701           _v142.frame_id = str[start:end].decode('utf-8')
02702         else:
02703           _v142.frame_id = str[start:end]
02704         start = end
02705         end += 4
02706         (length,) = _struct_I.unpack(str[start:end])
02707         start = end
02708         end += length
02709         if python3:
02710           val1.link_name = str[start:end].decode('utf-8')
02711         else:
02712           val1.link_name = str[start:end]
02713         _v144 = val1.target_point_offset
02714         _x = _v144
02715         start = end
02716         end += 24
02717         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02718         _v145 = val1.position
02719         _x = _v145
02720         start = end
02721         end += 24
02722         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02723         _v146 = val1.constraint_region_shape
02724         start = end
02725         end += 1
02726         (_v146.type,) = _struct_b.unpack(str[start:end])
02727         start = end
02728         end += 4
02729         (length,) = _struct_I.unpack(str[start:end])
02730         pattern = '<%sd'%length
02731         start = end
02732         end += struct.calcsize(pattern)
02733         _v146.dimensions = struct.unpack(pattern, str[start:end])
02734         start = end
02735         end += 4
02736         (length,) = _struct_I.unpack(str[start:end])
02737         pattern = '<%si'%length
02738         start = end
02739         end += struct.calcsize(pattern)
02740         _v146.triangles = struct.unpack(pattern, str[start:end])
02741         start = end
02742         end += 4
02743         (length,) = _struct_I.unpack(str[start:end])
02744         _v146.vertices = []
02745         for i in range(0, length):
02746           val3 = geometry_msgs.msg.Point()
02747           _x = val3
02748           start = end
02749           end += 24
02750           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02751           _v146.vertices.append(val3)
02752         _v147 = val1.constraint_region_orientation
02753         _x = _v147
02754         start = end
02755         end += 32
02756         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02757         start = end
02758         end += 8
02759         (val1.weight,) = _struct_d.unpack(str[start:end])
02760         self.goal.path_constraints.position_constraints.append(val1)
02761       start = end
02762       end += 4
02763       (length,) = _struct_I.unpack(str[start:end])
02764       self.goal.path_constraints.orientation_constraints = []
02765       for i in range(0, length):
02766         val1 = arm_navigation_msgs.msg.OrientationConstraint()
02767         _v148 = val1.header
02768         start = end
02769         end += 4
02770         (_v148.seq,) = _struct_I.unpack(str[start:end])
02771         _v149 = _v148.stamp
02772         _x = _v149
02773         start = end
02774         end += 8
02775         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02776         start = end
02777         end += 4
02778         (length,) = _struct_I.unpack(str[start:end])
02779         start = end
02780         end += length
02781         if python3:
02782           _v148.frame_id = str[start:end].decode('utf-8')
02783         else:
02784           _v148.frame_id = 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           val1.link_name = str[start:end].decode('utf-8')
02792         else:
02793           val1.link_name = str[start:end]
02794         start = end
02795         end += 4
02796         (val1.type,) = _struct_i.unpack(str[start:end])
02797         _v150 = val1.orientation
02798         _x = _v150
02799         start = end
02800         end += 32
02801         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02802         _x = val1
02803         start = end
02804         end += 32
02805         (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
02806         self.goal.path_constraints.orientation_constraints.append(val1)
02807       start = end
02808       end += 4
02809       (length,) = _struct_I.unpack(str[start:end])
02810       self.goal.path_constraints.visibility_constraints = []
02811       for i in range(0, length):
02812         val1 = arm_navigation_msgs.msg.VisibilityConstraint()
02813         _v151 = val1.header
02814         start = end
02815         end += 4
02816         (_v151.seq,) = _struct_I.unpack(str[start:end])
02817         _v152 = _v151.stamp
02818         _x = _v152
02819         start = end
02820         end += 8
02821         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02822         start = end
02823         end += 4
02824         (length,) = _struct_I.unpack(str[start:end])
02825         start = end
02826         end += length
02827         if python3:
02828           _v151.frame_id = str[start:end].decode('utf-8')
02829         else:
02830           _v151.frame_id = str[start:end]
02831         _v153 = val1.target
02832         _v154 = _v153.header
02833         start = end
02834         end += 4
02835         (_v154.seq,) = _struct_I.unpack(str[start:end])
02836         _v155 = _v154.stamp
02837         _x = _v155
02838         start = end
02839         end += 8
02840         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02841         start = end
02842         end += 4
02843         (length,) = _struct_I.unpack(str[start:end])
02844         start = end
02845         end += length
02846         if python3:
02847           _v154.frame_id = str[start:end].decode('utf-8')
02848         else:
02849           _v154.frame_id = str[start:end]
02850         _v156 = _v153.point
02851         _x = _v156
02852         start = end
02853         end += 24
02854         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02855         _v157 = val1.sensor_pose
02856         _v158 = _v157.header
02857         start = end
02858         end += 4
02859         (_v158.seq,) = _struct_I.unpack(str[start:end])
02860         _v159 = _v158.stamp
02861         _x = _v159
02862         start = end
02863         end += 8
02864         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02865         start = end
02866         end += 4
02867         (length,) = _struct_I.unpack(str[start:end])
02868         start = end
02869         end += length
02870         if python3:
02871           _v158.frame_id = str[start:end].decode('utf-8')
02872         else:
02873           _v158.frame_id = str[start:end]
02874         _v160 = _v157.pose
02875         _v161 = _v160.position
02876         _x = _v161
02877         start = end
02878         end += 24
02879         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02880         _v162 = _v160.orientation
02881         _x = _v162
02882         start = end
02883         end += 32
02884         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02885         start = end
02886         end += 8
02887         (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
02888         self.goal.path_constraints.visibility_constraints.append(val1)
02889       start = end
02890       end += 4
02891       (length,) = _struct_I.unpack(str[start:end])
02892       self.goal.additional_collision_operations.collision_operations = []
02893       for i in range(0, length):
02894         val1 = arm_navigation_msgs.msg.CollisionOperation()
02895         start = end
02896         end += 4
02897         (length,) = _struct_I.unpack(str[start:end])
02898         start = end
02899         end += length
02900         if python3:
02901           val1.object1 = str[start:end].decode('utf-8')
02902         else:
02903           val1.object1 = str[start:end]
02904         start = end
02905         end += 4
02906         (length,) = _struct_I.unpack(str[start:end])
02907         start = end
02908         end += length
02909         if python3:
02910           val1.object2 = str[start:end].decode('utf-8')
02911         else:
02912           val1.object2 = str[start:end]
02913         _x = val1
02914         start = end
02915         end += 12
02916         (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
02917         self.goal.additional_collision_operations.collision_operations.append(val1)
02918       start = end
02919       end += 4
02920       (length,) = _struct_I.unpack(str[start:end])
02921       self.goal.additional_link_padding = []
02922       for i in range(0, length):
02923         val1 = arm_navigation_msgs.msg.LinkPadding()
02924         start = end
02925         end += 4
02926         (length,) = _struct_I.unpack(str[start:end])
02927         start = end
02928         end += length
02929         if python3:
02930           val1.link_name = str[start:end].decode('utf-8')
02931         else:
02932           val1.link_name = str[start:end]
02933         start = end
02934         end += 8
02935         (val1.padding,) = _struct_d.unpack(str[start:end])
02936         self.goal.additional_link_padding.append(val1)
02937       start = end
02938       end += 4
02939       (length,) = _struct_I.unpack(str[start:end])
02940       self.goal.movable_obstacles = []
02941       for i in range(0, length):
02942         val1 = object_manipulation_msgs.msg.GraspableObject()
02943         start = end
02944         end += 4
02945         (length,) = _struct_I.unpack(str[start:end])
02946         start = end
02947         end += length
02948         if python3:
02949           val1.reference_frame_id = str[start:end].decode('utf-8')
02950         else:
02951           val1.reference_frame_id = str[start:end]
02952         start = end
02953         end += 4
02954         (length,) = _struct_I.unpack(str[start:end])
02955         val1.potential_models = []
02956         for i in range(0, length):
02957           val2 = household_objects_database_msgs.msg.DatabaseModelPose()
02958           start = end
02959           end += 4
02960           (val2.model_id,) = _struct_i.unpack(str[start:end])
02961           _v163 = val2.pose
02962           _v164 = _v163.header
02963           start = end
02964           end += 4
02965           (_v164.seq,) = _struct_I.unpack(str[start:end])
02966           _v165 = _v164.stamp
02967           _x = _v165
02968           start = end
02969           end += 8
02970           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02971           start = end
02972           end += 4
02973           (length,) = _struct_I.unpack(str[start:end])
02974           start = end
02975           end += length
02976           if python3:
02977             _v164.frame_id = str[start:end].decode('utf-8')
02978           else:
02979             _v164.frame_id = str[start:end]
02980           _v166 = _v163.pose
02981           _v167 = _v166.position
02982           _x = _v167
02983           start = end
02984           end += 24
02985           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02986           _v168 = _v166.orientation
02987           _x = _v168
02988           start = end
02989           end += 32
02990           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02991           start = end
02992           end += 4
02993           (val2.confidence,) = _struct_f.unpack(str[start:end])
02994           start = end
02995           end += 4
02996           (length,) = _struct_I.unpack(str[start:end])
02997           start = end
02998           end += length
02999           if python3:
03000             val2.detector_name = str[start:end].decode('utf-8')
03001           else:
03002             val2.detector_name = str[start:end]
03003           val1.potential_models.append(val2)
03004         _v169 = val1.cluster
03005         _v170 = _v169.header
03006         start = end
03007         end += 4
03008         (_v170.seq,) = _struct_I.unpack(str[start:end])
03009         _v171 = _v170.stamp
03010         _x = _v171
03011         start = end
03012         end += 8
03013         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03014         start = end
03015         end += 4
03016         (length,) = _struct_I.unpack(str[start:end])
03017         start = end
03018         end += length
03019         if python3:
03020           _v170.frame_id = str[start:end].decode('utf-8')
03021         else:
03022           _v170.frame_id = str[start:end]
03023         start = end
03024         end += 4
03025         (length,) = _struct_I.unpack(str[start:end])
03026         _v169.points = []
03027         for i in range(0, length):
03028           val3 = geometry_msgs.msg.Point32()
03029           _x = val3
03030           start = end
03031           end += 12
03032           (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
03033           _v169.points.append(val3)
03034         start = end
03035         end += 4
03036         (length,) = _struct_I.unpack(str[start:end])
03037         _v169.channels = []
03038         for i in range(0, length):
03039           val3 = sensor_msgs.msg.ChannelFloat32()
03040           start = end
03041           end += 4
03042           (length,) = _struct_I.unpack(str[start:end])
03043           start = end
03044           end += length
03045           if python3:
03046             val3.name = str[start:end].decode('utf-8')
03047           else:
03048             val3.name = str[start:end]
03049           start = end
03050           end += 4
03051           (length,) = _struct_I.unpack(str[start:end])
03052           pattern = '<%sf'%length
03053           start = end
03054           end += struct.calcsize(pattern)
03055           val3.values = struct.unpack(pattern, str[start:end])
03056           _v169.channels.append(val3)
03057         _v172 = val1.region
03058         _v173 = _v172.cloud
03059         _v174 = _v173.header
03060         start = end
03061         end += 4
03062         (_v174.seq,) = _struct_I.unpack(str[start:end])
03063         _v175 = _v174.stamp
03064         _x = _v175
03065         start = end
03066         end += 8
03067         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03068         start = end
03069         end += 4
03070         (length,) = _struct_I.unpack(str[start:end])
03071         start = end
03072         end += length
03073         if python3:
03074           _v174.frame_id = str[start:end].decode('utf-8')
03075         else:
03076           _v174.frame_id = str[start:end]
03077         _x = _v173
03078         start = end
03079         end += 8
03080         (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03081         start = end
03082         end += 4
03083         (length,) = _struct_I.unpack(str[start:end])
03084         _v173.fields = []
03085         for i in range(0, length):
03086           val4 = sensor_msgs.msg.PointField()
03087           start = end
03088           end += 4
03089           (length,) = _struct_I.unpack(str[start:end])
03090           start = end
03091           end += length
03092           if python3:
03093             val4.name = str[start:end].decode('utf-8')
03094           else:
03095             val4.name = str[start:end]
03096           _x = val4
03097           start = end
03098           end += 9
03099           (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
03100           _v173.fields.append(val4)
03101         _x = _v173
03102         start = end
03103         end += 9
03104         (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
03105         _v173.is_bigendian = bool(_v173.is_bigendian)
03106         start = end
03107         end += 4
03108         (length,) = _struct_I.unpack(str[start:end])
03109         start = end
03110         end += length
03111         if python3:
03112           _v173.data = str[start:end].decode('utf-8')
03113         else:
03114           _v173.data = str[start:end]
03115         start = end
03116         end += 1
03117         (_v173.is_dense,) = _struct_B.unpack(str[start:end])
03118         _v173.is_dense = bool(_v173.is_dense)
03119         start = end
03120         end += 4
03121         (length,) = _struct_I.unpack(str[start:end])
03122         pattern = '<%si'%length
03123         start = end
03124         end += struct.calcsize(pattern)
03125         _v172.mask = struct.unpack(pattern, str[start:end])
03126         _v176 = _v172.image
03127         _v177 = _v176.header
03128         start = end
03129         end += 4
03130         (_v177.seq,) = _struct_I.unpack(str[start:end])
03131         _v178 = _v177.stamp
03132         _x = _v178
03133         start = end
03134         end += 8
03135         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03136         start = end
03137         end += 4
03138         (length,) = _struct_I.unpack(str[start:end])
03139         start = end
03140         end += length
03141         if python3:
03142           _v177.frame_id = str[start:end].decode('utf-8')
03143         else:
03144           _v177.frame_id = str[start:end]
03145         _x = _v176
03146         start = end
03147         end += 8
03148         (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
03149         start = end
03150         end += 4
03151         (length,) = _struct_I.unpack(str[start:end])
03152         start = end
03153         end += length
03154         if python3:
03155           _v176.encoding = str[start:end].decode('utf-8')
03156         else:
03157           _v176.encoding = str[start:end]
03158         _x = _v176
03159         start = end
03160         end += 5
03161         (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03162         start = end
03163         end += 4
03164         (length,) = _struct_I.unpack(str[start:end])
03165         start = end
03166         end += length
03167         if python3:
03168           _v176.data = str[start:end].decode('utf-8')
03169         else:
03170           _v176.data = str[start:end]
03171         _v179 = _v172.disparity_image
03172         _v180 = _v179.header
03173         start = end
03174         end += 4
03175         (_v180.seq,) = _struct_I.unpack(str[start:end])
03176         _v181 = _v180.stamp
03177         _x = _v181
03178         start = end
03179         end += 8
03180         (_x.secs, _x.nsecs,) = _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           _v180.frame_id = str[start:end].decode('utf-8')
03188         else:
03189           _v180.frame_id = str[start:end]
03190         _x = _v179
03191         start = end
03192         end += 8
03193         (_x.height, _x.width,) = _struct_2I.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           _v179.encoding = str[start:end].decode('utf-8')
03201         else:
03202           _v179.encoding = str[start:end]
03203         _x = _v179
03204         start = end
03205         end += 5
03206         (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
03207         start = end
03208         end += 4
03209         (length,) = _struct_I.unpack(str[start:end])
03210         start = end
03211         end += length
03212         if python3:
03213           _v179.data = str[start:end].decode('utf-8')
03214         else:
03215           _v179.data = str[start:end]
03216         _v182 = _v172.cam_info
03217         _v183 = _v182.header
03218         start = end
03219         end += 4
03220         (_v183.seq,) = _struct_I.unpack(str[start:end])
03221         _v184 = _v183.stamp
03222         _x = _v184
03223         start = end
03224         end += 8
03225         (_x.secs, _x.nsecs,) = _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           _v183.frame_id = str[start:end].decode('utf-8')
03233         else:
03234           _v183.frame_id = str[start:end]
03235         _x = _v182
03236         start = end
03237         end += 8
03238         (_x.height, _x.width,) = _struct_2I.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           _v182.distortion_model = str[start:end].decode('utf-8')
03246         else:
03247           _v182.distortion_model = str[start:end]
03248         start = end
03249         end += 4
03250         (length,) = _struct_I.unpack(str[start:end])
03251         pattern = '<%sd'%length
03252         start = end
03253         end += struct.calcsize(pattern)
03254         _v182.D = struct.unpack(pattern, str[start:end])
03255         start = end
03256         end += 72
03257         _v182.K = _struct_9d.unpack(str[start:end])
03258         start = end
03259         end += 72
03260         _v182.R = _struct_9d.unpack(str[start:end])
03261         start = end
03262         end += 96
03263         _v182.P = _struct_12d.unpack(str[start:end])
03264         _x = _v182
03265         start = end
03266         end += 8
03267         (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
03268         _v185 = _v182.roi
03269         _x = _v185
03270         start = end
03271         end += 17
03272         (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
03273         _v185.do_rectify = bool(_v185.do_rectify)
03274         _v186 = _v172.roi_box_pose
03275         _v187 = _v186.header
03276         start = end
03277         end += 4
03278         (_v187.seq,) = _struct_I.unpack(str[start:end])
03279         _v188 = _v187.stamp
03280         _x = _v188
03281         start = end
03282         end += 8
03283         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
03284         start = end
03285         end += 4
03286         (length,) = _struct_I.unpack(str[start:end])
03287         start = end
03288         end += length
03289         if python3:
03290           _v187.frame_id = str[start:end].decode('utf-8')
03291         else:
03292           _v187.frame_id = str[start:end]
03293         _v189 = _v186.pose
03294         _v190 = _v189.position
03295         _x = _v190
03296         start = end
03297         end += 24
03298         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03299         _v191 = _v189.orientation
03300         _x = _v191
03301         start = end
03302         end += 32
03303         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
03304         _v192 = _v172.roi_box_dims
03305         _x = _v192
03306         start = end
03307         end += 24
03308         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
03309         start = end
03310         end += 4
03311         (length,) = _struct_I.unpack(str[start:end])
03312         start = end
03313         end += length
03314         if python3:
03315           val1.collision_name = str[start:end].decode('utf-8')
03316         else:
03317           val1.collision_name = str[start:end]
03318         self.goal.movable_obstacles.append(val1)
03319       start = end
03320       end += 4
03321       (self.goal.max_contact_force,) = _struct_f.unpack(str[start:end])
03322       return self
03323     except struct.error as e:
03324       raise genpy.DeserializationError(e) #most likely buffer underfill
03325 
03326 
03327   def serialize_numpy(self, buff, numpy):
03328     """
03329     serialize message with numpy array types into buffer
03330     :param buff: buffer, ``StringIO``
03331     :param numpy: numpy python module
03332     """
03333     try:
03334       _x = self
03335       buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
03336       _x = self.header.frame_id
03337       length = len(_x)
03338       if python3 or type(_x) == unicode:
03339         _x = _x.encode('utf-8')
03340         length = len(_x)
03341       buff.write(struct.pack('<I%ss'%length, length, _x))
03342       _x = self
03343       buff.write(_struct_2I.pack(_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs))
03344       _x = self.goal_id.id
03345       length = len(_x)
03346       if python3 or type(_x) == unicode:
03347         _x = _x.encode('utf-8')
03348         length = len(_x)
03349       buff.write(struct.pack('<I%ss'%length, length, _x))
03350       _x = self.goal.arm_name
03351       length = len(_x)
03352       if python3 or type(_x) == unicode:
03353         _x = _x.encode('utf-8')
03354         length = len(_x)
03355       buff.write(struct.pack('<I%ss'%length, length, _x))
03356       _x = self.goal.target.reference_frame_id
03357       length = len(_x)
03358       if python3 or type(_x) == unicode:
03359         _x = _x.encode('utf-8')
03360         length = len(_x)
03361       buff.write(struct.pack('<I%ss'%length, length, _x))
03362       length = len(self.goal.target.potential_models)
03363       buff.write(_struct_I.pack(length))
03364       for val1 in self.goal.target.potential_models:
03365         buff.write(_struct_i.pack(val1.model_id))
03366         _v193 = val1.pose
03367         _v194 = _v193.header
03368         buff.write(_struct_I.pack(_v194.seq))
03369         _v195 = _v194.stamp
03370         _x = _v195
03371         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03372         _x = _v194.frame_id
03373         length = len(_x)
03374         if python3 or type(_x) == unicode:
03375           _x = _x.encode('utf-8')
03376           length = len(_x)
03377         buff.write(struct.pack('<I%ss'%length, length, _x))
03378         _v196 = _v193.pose
03379         _v197 = _v196.position
03380         _x = _v197
03381         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03382         _v198 = _v196.orientation
03383         _x = _v198
03384         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03385         buff.write(_struct_f.pack(val1.confidence))
03386         _x = val1.detector_name
03387         length = len(_x)
03388         if python3 or type(_x) == unicode:
03389           _x = _x.encode('utf-8')
03390           length = len(_x)
03391         buff.write(struct.pack('<I%ss'%length, length, _x))
03392       _x = self
03393       buff.write(_struct_3I.pack(_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs))
03394       _x = self.goal.target.cluster.header.frame_id
03395       length = len(_x)
03396       if python3 or type(_x) == unicode:
03397         _x = _x.encode('utf-8')
03398         length = len(_x)
03399       buff.write(struct.pack('<I%ss'%length, length, _x))
03400       length = len(self.goal.target.cluster.points)
03401       buff.write(_struct_I.pack(length))
03402       for val1 in self.goal.target.cluster.points:
03403         _x = val1
03404         buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
03405       length = len(self.goal.target.cluster.channels)
03406       buff.write(_struct_I.pack(length))
03407       for val1 in self.goal.target.cluster.channels:
03408         _x = val1.name
03409         length = len(_x)
03410         if python3 or type(_x) == unicode:
03411           _x = _x.encode('utf-8')
03412           length = len(_x)
03413         buff.write(struct.pack('<I%ss'%length, length, _x))
03414         length = len(val1.values)
03415         buff.write(_struct_I.pack(length))
03416         pattern = '<%sf'%length
03417         buff.write(val1.values.tostring())
03418       _x = self
03419       buff.write(_struct_3I.pack(_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs))
03420       _x = self.goal.target.region.cloud.header.frame_id
03421       length = len(_x)
03422       if python3 or type(_x) == unicode:
03423         _x = _x.encode('utf-8')
03424         length = len(_x)
03425       buff.write(struct.pack('<I%ss'%length, length, _x))
03426       _x = self
03427       buff.write(_struct_2I.pack(_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width))
03428       length = len(self.goal.target.region.cloud.fields)
03429       buff.write(_struct_I.pack(length))
03430       for val1 in self.goal.target.region.cloud.fields:
03431         _x = val1.name
03432         length = len(_x)
03433         if python3 or type(_x) == unicode:
03434           _x = _x.encode('utf-8')
03435           length = len(_x)
03436         buff.write(struct.pack('<I%ss'%length, length, _x))
03437         _x = val1
03438         buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
03439       _x = self
03440       buff.write(_struct_B2I.pack(_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step))
03441       _x = self.goal.target.region.cloud.data
03442       length = len(_x)
03443       # - if encoded as a list instead, serialize as bytes instead of string
03444       if type(_x) in [list, tuple]:
03445         buff.write(struct.pack('<I%sB'%length, length, *_x))
03446       else:
03447         buff.write(struct.pack('<I%ss'%length, length, _x))
03448       buff.write(_struct_B.pack(self.goal.target.region.cloud.is_dense))
03449       length = len(self.goal.target.region.mask)
03450       buff.write(_struct_I.pack(length))
03451       pattern = '<%si'%length
03452       buff.write(self.goal.target.region.mask.tostring())
03453       _x = self
03454       buff.write(_struct_3I.pack(_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs))
03455       _x = self.goal.target.region.image.header.frame_id
03456       length = len(_x)
03457       if python3 or type(_x) == unicode:
03458         _x = _x.encode('utf-8')
03459         length = len(_x)
03460       buff.write(struct.pack('<I%ss'%length, length, _x))
03461       _x = self
03462       buff.write(_struct_2I.pack(_x.goal.target.region.image.height, _x.goal.target.region.image.width))
03463       _x = self.goal.target.region.image.encoding
03464       length = len(_x)
03465       if python3 or type(_x) == unicode:
03466         _x = _x.encode('utf-8')
03467         length = len(_x)
03468       buff.write(struct.pack('<I%ss'%length, length, _x))
03469       _x = self
03470       buff.write(_struct_BI.pack(_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step))
03471       _x = self.goal.target.region.image.data
03472       length = len(_x)
03473       # - if encoded as a list instead, serialize as bytes instead of string
03474       if type(_x) in [list, tuple]:
03475         buff.write(struct.pack('<I%sB'%length, length, *_x))
03476       else:
03477         buff.write(struct.pack('<I%ss'%length, length, _x))
03478       _x = self
03479       buff.write(_struct_3I.pack(_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs))
03480       _x = self.goal.target.region.disparity_image.header.frame_id
03481       length = len(_x)
03482       if python3 or type(_x) == unicode:
03483         _x = _x.encode('utf-8')
03484         length = len(_x)
03485       buff.write(struct.pack('<I%ss'%length, length, _x))
03486       _x = self
03487       buff.write(_struct_2I.pack(_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width))
03488       _x = self.goal.target.region.disparity_image.encoding
03489       length = len(_x)
03490       if python3 or type(_x) == unicode:
03491         _x = _x.encode('utf-8')
03492         length = len(_x)
03493       buff.write(struct.pack('<I%ss'%length, length, _x))
03494       _x = self
03495       buff.write(_struct_BI.pack(_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step))
03496       _x = self.goal.target.region.disparity_image.data
03497       length = len(_x)
03498       # - if encoded as a list instead, serialize as bytes instead of string
03499       if type(_x) in [list, tuple]:
03500         buff.write(struct.pack('<I%sB'%length, length, *_x))
03501       else:
03502         buff.write(struct.pack('<I%ss'%length, length, _x))
03503       _x = self
03504       buff.write(_struct_3I.pack(_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs))
03505       _x = self.goal.target.region.cam_info.header.frame_id
03506       length = len(_x)
03507       if python3 or type(_x) == unicode:
03508         _x = _x.encode('utf-8')
03509         length = len(_x)
03510       buff.write(struct.pack('<I%ss'%length, length, _x))
03511       _x = self
03512       buff.write(_struct_2I.pack(_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width))
03513       _x = self.goal.target.region.cam_info.distortion_model
03514       length = len(_x)
03515       if python3 or type(_x) == unicode:
03516         _x = _x.encode('utf-8')
03517         length = len(_x)
03518       buff.write(struct.pack('<I%ss'%length, length, _x))
03519       length = len(self.goal.target.region.cam_info.D)
03520       buff.write(_struct_I.pack(length))
03521       pattern = '<%sd'%length
03522       buff.write(self.goal.target.region.cam_info.D.tostring())
03523       buff.write(self.goal.target.region.cam_info.K.tostring())
03524       buff.write(self.goal.target.region.cam_info.R.tostring())
03525       buff.write(self.goal.target.region.cam_info.P.tostring())
03526       _x = self
03527       buff.write(_struct_6IB3I.pack(_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs))
03528       _x = self.goal.target.region.roi_box_pose.header.frame_id
03529       length = len(_x)
03530       if python3 or type(_x) == unicode:
03531         _x = _x.encode('utf-8')
03532         length = len(_x)
03533       buff.write(struct.pack('<I%ss'%length, length, _x))
03534       _x = self
03535       buff.write(_struct_10d.pack(_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z))
03536       _x = self.goal.target.collision_name
03537       length = len(_x)
03538       if python3 or type(_x) == unicode:
03539         _x = _x.encode('utf-8')
03540         length = len(_x)
03541       buff.write(struct.pack('<I%ss'%length, length, _x))
03542       length = len(self.goal.desired_grasps)
03543       buff.write(_struct_I.pack(length))
03544       for val1 in self.goal.desired_grasps:
03545         _v199 = val1.pre_grasp_posture
03546         _v200 = _v199.header
03547         buff.write(_struct_I.pack(_v200.seq))
03548         _v201 = _v200.stamp
03549         _x = _v201
03550         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03551         _x = _v200.frame_id
03552         length = len(_x)
03553         if python3 or type(_x) == unicode:
03554           _x = _x.encode('utf-8')
03555           length = len(_x)
03556         buff.write(struct.pack('<I%ss'%length, length, _x))
03557         length = len(_v199.name)
03558         buff.write(_struct_I.pack(length))
03559         for val3 in _v199.name:
03560           length = len(val3)
03561           if python3 or type(val3) == unicode:
03562             val3 = val3.encode('utf-8')
03563             length = len(val3)
03564           buff.write(struct.pack('<I%ss'%length, length, val3))
03565         length = len(_v199.position)
03566         buff.write(_struct_I.pack(length))
03567         pattern = '<%sd'%length
03568         buff.write(_v199.position.tostring())
03569         length = len(_v199.velocity)
03570         buff.write(_struct_I.pack(length))
03571         pattern = '<%sd'%length
03572         buff.write(_v199.velocity.tostring())
03573         length = len(_v199.effort)
03574         buff.write(_struct_I.pack(length))
03575         pattern = '<%sd'%length
03576         buff.write(_v199.effort.tostring())
03577         _v202 = val1.grasp_posture
03578         _v203 = _v202.header
03579         buff.write(_struct_I.pack(_v203.seq))
03580         _v204 = _v203.stamp
03581         _x = _v204
03582         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03583         _x = _v203.frame_id
03584         length = len(_x)
03585         if python3 or type(_x) == unicode:
03586           _x = _x.encode('utf-8')
03587           length = len(_x)
03588         buff.write(struct.pack('<I%ss'%length, length, _x))
03589         length = len(_v202.name)
03590         buff.write(_struct_I.pack(length))
03591         for val3 in _v202.name:
03592           length = len(val3)
03593           if python3 or type(val3) == unicode:
03594             val3 = val3.encode('utf-8')
03595             length = len(val3)
03596           buff.write(struct.pack('<I%ss'%length, length, val3))
03597         length = len(_v202.position)
03598         buff.write(_struct_I.pack(length))
03599         pattern = '<%sd'%length
03600         buff.write(_v202.position.tostring())
03601         length = len(_v202.velocity)
03602         buff.write(_struct_I.pack(length))
03603         pattern = '<%sd'%length
03604         buff.write(_v202.velocity.tostring())
03605         length = len(_v202.effort)
03606         buff.write(_struct_I.pack(length))
03607         pattern = '<%sd'%length
03608         buff.write(_v202.effort.tostring())
03609         _v205 = val1.grasp_pose
03610         _v206 = _v205.position
03611         _x = _v206
03612         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03613         _v207 = _v205.orientation
03614         _x = _v207
03615         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03616         _x = val1
03617         buff.write(_struct_dB2f.pack(_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance))
03618         length = len(val1.moved_obstacles)
03619         buff.write(_struct_I.pack(length))
03620         for val2 in val1.moved_obstacles:
03621           _x = val2.reference_frame_id
03622           length = len(_x)
03623           if python3 or type(_x) == unicode:
03624             _x = _x.encode('utf-8')
03625             length = len(_x)
03626           buff.write(struct.pack('<I%ss'%length, length, _x))
03627           length = len(val2.potential_models)
03628           buff.write(_struct_I.pack(length))
03629           for val3 in val2.potential_models:
03630             buff.write(_struct_i.pack(val3.model_id))
03631             _v208 = val3.pose
03632             _v209 = _v208.header
03633             buff.write(_struct_I.pack(_v209.seq))
03634             _v210 = _v209.stamp
03635             _x = _v210
03636             buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03637             _x = _v209.frame_id
03638             length = len(_x)
03639             if python3 or type(_x) == unicode:
03640               _x = _x.encode('utf-8')
03641               length = len(_x)
03642             buff.write(struct.pack('<I%ss'%length, length, _x))
03643             _v211 = _v208.pose
03644             _v212 = _v211.position
03645             _x = _v212
03646             buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03647             _v213 = _v211.orientation
03648             _x = _v213
03649             buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03650             buff.write(_struct_f.pack(val3.confidence))
03651             _x = val3.detector_name
03652             length = len(_x)
03653             if python3 or type(_x) == unicode:
03654               _x = _x.encode('utf-8')
03655               length = len(_x)
03656             buff.write(struct.pack('<I%ss'%length, length, _x))
03657           _v214 = val2.cluster
03658           _v215 = _v214.header
03659           buff.write(_struct_I.pack(_v215.seq))
03660           _v216 = _v215.stamp
03661           _x = _v216
03662           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03663           _x = _v215.frame_id
03664           length = len(_x)
03665           if python3 or type(_x) == unicode:
03666             _x = _x.encode('utf-8')
03667             length = len(_x)
03668           buff.write(struct.pack('<I%ss'%length, length, _x))
03669           length = len(_v214.points)
03670           buff.write(_struct_I.pack(length))
03671           for val4 in _v214.points:
03672             _x = val4
03673             buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
03674           length = len(_v214.channels)
03675           buff.write(_struct_I.pack(length))
03676           for val4 in _v214.channels:
03677             _x = val4.name
03678             length = len(_x)
03679             if python3 or type(_x) == unicode:
03680               _x = _x.encode('utf-8')
03681               length = len(_x)
03682             buff.write(struct.pack('<I%ss'%length, length, _x))
03683             length = len(val4.values)
03684             buff.write(_struct_I.pack(length))
03685             pattern = '<%sf'%length
03686             buff.write(val4.values.tostring())
03687           _v217 = val2.region
03688           _v218 = _v217.cloud
03689           _v219 = _v218.header
03690           buff.write(_struct_I.pack(_v219.seq))
03691           _v220 = _v219.stamp
03692           _x = _v220
03693           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03694           _x = _v219.frame_id
03695           length = len(_x)
03696           if python3 or type(_x) == unicode:
03697             _x = _x.encode('utf-8')
03698             length = len(_x)
03699           buff.write(struct.pack('<I%ss'%length, length, _x))
03700           _x = _v218
03701           buff.write(_struct_2I.pack(_x.height, _x.width))
03702           length = len(_v218.fields)
03703           buff.write(_struct_I.pack(length))
03704           for val5 in _v218.fields:
03705             _x = val5.name
03706             length = len(_x)
03707             if python3 or type(_x) == unicode:
03708               _x = _x.encode('utf-8')
03709               length = len(_x)
03710             buff.write(struct.pack('<I%ss'%length, length, _x))
03711             _x = val5
03712             buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
03713           _x = _v218
03714           buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
03715           _x = _v218.data
03716           length = len(_x)
03717           # - if encoded as a list instead, serialize as bytes instead of string
03718           if type(_x) in [list, tuple]:
03719             buff.write(struct.pack('<I%sB'%length, length, *_x))
03720           else:
03721             buff.write(struct.pack('<I%ss'%length, length, _x))
03722           buff.write(_struct_B.pack(_v218.is_dense))
03723           length = len(_v217.mask)
03724           buff.write(_struct_I.pack(length))
03725           pattern = '<%si'%length
03726           buff.write(_v217.mask.tostring())
03727           _v221 = _v217.image
03728           _v222 = _v221.header
03729           buff.write(_struct_I.pack(_v222.seq))
03730           _v223 = _v222.stamp
03731           _x = _v223
03732           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03733           _x = _v222.frame_id
03734           length = len(_x)
03735           if python3 or type(_x) == unicode:
03736             _x = _x.encode('utf-8')
03737             length = len(_x)
03738           buff.write(struct.pack('<I%ss'%length, length, _x))
03739           _x = _v221
03740           buff.write(_struct_2I.pack(_x.height, _x.width))
03741           _x = _v221.encoding
03742           length = len(_x)
03743           if python3 or type(_x) == unicode:
03744             _x = _x.encode('utf-8')
03745             length = len(_x)
03746           buff.write(struct.pack('<I%ss'%length, length, _x))
03747           _x = _v221
03748           buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
03749           _x = _v221.data
03750           length = len(_x)
03751           # - if encoded as a list instead, serialize as bytes instead of string
03752           if type(_x) in [list, tuple]:
03753             buff.write(struct.pack('<I%sB'%length, length, *_x))
03754           else:
03755             buff.write(struct.pack('<I%ss'%length, length, _x))
03756           _v224 = _v217.disparity_image
03757           _v225 = _v224.header
03758           buff.write(_struct_I.pack(_v225.seq))
03759           _v226 = _v225.stamp
03760           _x = _v226
03761           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03762           _x = _v225.frame_id
03763           length = len(_x)
03764           if python3 or type(_x) == unicode:
03765             _x = _x.encode('utf-8')
03766             length = len(_x)
03767           buff.write(struct.pack('<I%ss'%length, length, _x))
03768           _x = _v224
03769           buff.write(_struct_2I.pack(_x.height, _x.width))
03770           _x = _v224.encoding
03771           length = len(_x)
03772           if python3 or type(_x) == unicode:
03773             _x = _x.encode('utf-8')
03774             length = len(_x)
03775           buff.write(struct.pack('<I%ss'%length, length, _x))
03776           _x = _v224
03777           buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
03778           _x = _v224.data
03779           length = len(_x)
03780           # - if encoded as a list instead, serialize as bytes instead of string
03781           if type(_x) in [list, tuple]:
03782             buff.write(struct.pack('<I%sB'%length, length, *_x))
03783           else:
03784             buff.write(struct.pack('<I%ss'%length, length, _x))
03785           _v227 = _v217.cam_info
03786           _v228 = _v227.header
03787           buff.write(_struct_I.pack(_v228.seq))
03788           _v229 = _v228.stamp
03789           _x = _v229
03790           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03791           _x = _v228.frame_id
03792           length = len(_x)
03793           if python3 or type(_x) == unicode:
03794             _x = _x.encode('utf-8')
03795             length = len(_x)
03796           buff.write(struct.pack('<I%ss'%length, length, _x))
03797           _x = _v227
03798           buff.write(_struct_2I.pack(_x.height, _x.width))
03799           _x = _v227.distortion_model
03800           length = len(_x)
03801           if python3 or type(_x) == unicode:
03802             _x = _x.encode('utf-8')
03803             length = len(_x)
03804           buff.write(struct.pack('<I%ss'%length, length, _x))
03805           length = len(_v227.D)
03806           buff.write(_struct_I.pack(length))
03807           pattern = '<%sd'%length
03808           buff.write(_v227.D.tostring())
03809           buff.write(_v227.K.tostring())
03810           buff.write(_v227.R.tostring())
03811           buff.write(_v227.P.tostring())
03812           _x = _v227
03813           buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
03814           _v230 = _v227.roi
03815           _x = _v230
03816           buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
03817           _v231 = _v217.roi_box_pose
03818           _v232 = _v231.header
03819           buff.write(_struct_I.pack(_v232.seq))
03820           _v233 = _v232.stamp
03821           _x = _v233
03822           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03823           _x = _v232.frame_id
03824           length = len(_x)
03825           if python3 or type(_x) == unicode:
03826             _x = _x.encode('utf-8')
03827             length = len(_x)
03828           buff.write(struct.pack('<I%ss'%length, length, _x))
03829           _v234 = _v231.pose
03830           _v235 = _v234.position
03831           _x = _v235
03832           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03833           _v236 = _v234.orientation
03834           _x = _v236
03835           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03836           _v237 = _v217.roi_box_dims
03837           _x = _v237
03838           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03839           _x = val2.collision_name
03840           length = len(_x)
03841           if python3 or type(_x) == unicode:
03842             _x = _x.encode('utf-8')
03843             length = len(_x)
03844           buff.write(struct.pack('<I%ss'%length, length, _x))
03845       _x = self
03846       buff.write(_struct_3I.pack(_x.goal.lift.direction.header.seq, _x.goal.lift.direction.header.stamp.secs, _x.goal.lift.direction.header.stamp.nsecs))
03847       _x = self.goal.lift.direction.header.frame_id
03848       length = len(_x)
03849       if python3 or type(_x) == unicode:
03850         _x = _x.encode('utf-8')
03851         length = len(_x)
03852       buff.write(struct.pack('<I%ss'%length, length, _x))
03853       _x = self
03854       buff.write(_struct_3d2f.pack(_x.goal.lift.direction.vector.x, _x.goal.lift.direction.vector.y, _x.goal.lift.direction.vector.z, _x.goal.lift.desired_distance, _x.goal.lift.min_distance))
03855       _x = self.goal.collision_object_name
03856       length = len(_x)
03857       if python3 or type(_x) == unicode:
03858         _x = _x.encode('utf-8')
03859         length = len(_x)
03860       buff.write(struct.pack('<I%ss'%length, length, _x))
03861       _x = self.goal.collision_support_surface_name
03862       length = len(_x)
03863       if python3 or type(_x) == unicode:
03864         _x = _x.encode('utf-8')
03865         length = len(_x)
03866       buff.write(struct.pack('<I%ss'%length, length, _x))
03867       _x = self
03868       buff.write(_struct_5B.pack(_x.goal.allow_gripper_support_collision, _x.goal.use_reactive_execution, _x.goal.use_reactive_lift, _x.goal.only_perform_feasibility_test, _x.goal.ignore_collisions))
03869       length = len(self.goal.path_constraints.joint_constraints)
03870       buff.write(_struct_I.pack(length))
03871       for val1 in self.goal.path_constraints.joint_constraints:
03872         _x = val1.joint_name
03873         length = len(_x)
03874         if python3 or type(_x) == unicode:
03875           _x = _x.encode('utf-8')
03876           length = len(_x)
03877         buff.write(struct.pack('<I%ss'%length, length, _x))
03878         _x = val1
03879         buff.write(_struct_4d.pack(_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight))
03880       length = len(self.goal.path_constraints.position_constraints)
03881       buff.write(_struct_I.pack(length))
03882       for val1 in self.goal.path_constraints.position_constraints:
03883         _v238 = val1.header
03884         buff.write(_struct_I.pack(_v238.seq))
03885         _v239 = _v238.stamp
03886         _x = _v239
03887         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03888         _x = _v238.frame_id
03889         length = len(_x)
03890         if python3 or type(_x) == unicode:
03891           _x = _x.encode('utf-8')
03892           length = len(_x)
03893         buff.write(struct.pack('<I%ss'%length, length, _x))
03894         _x = val1.link_name
03895         length = len(_x)
03896         if python3 or type(_x) == unicode:
03897           _x = _x.encode('utf-8')
03898           length = len(_x)
03899         buff.write(struct.pack('<I%ss'%length, length, _x))
03900         _v240 = val1.target_point_offset
03901         _x = _v240
03902         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03903         _v241 = val1.position
03904         _x = _v241
03905         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03906         _v242 = val1.constraint_region_shape
03907         buff.write(_struct_b.pack(_v242.type))
03908         length = len(_v242.dimensions)
03909         buff.write(_struct_I.pack(length))
03910         pattern = '<%sd'%length
03911         buff.write(_v242.dimensions.tostring())
03912         length = len(_v242.triangles)
03913         buff.write(_struct_I.pack(length))
03914         pattern = '<%si'%length
03915         buff.write(_v242.triangles.tostring())
03916         length = len(_v242.vertices)
03917         buff.write(_struct_I.pack(length))
03918         for val3 in _v242.vertices:
03919           _x = val3
03920           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03921         _v243 = val1.constraint_region_orientation
03922         _x = _v243
03923         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03924         buff.write(_struct_d.pack(val1.weight))
03925       length = len(self.goal.path_constraints.orientation_constraints)
03926       buff.write(_struct_I.pack(length))
03927       for val1 in self.goal.path_constraints.orientation_constraints:
03928         _v244 = val1.header
03929         buff.write(_struct_I.pack(_v244.seq))
03930         _v245 = _v244.stamp
03931         _x = _v245
03932         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03933         _x = _v244.frame_id
03934         length = len(_x)
03935         if python3 or type(_x) == unicode:
03936           _x = _x.encode('utf-8')
03937           length = len(_x)
03938         buff.write(struct.pack('<I%ss'%length, length, _x))
03939         _x = val1.link_name
03940         length = len(_x)
03941         if python3 or type(_x) == unicode:
03942           _x = _x.encode('utf-8')
03943           length = len(_x)
03944         buff.write(struct.pack('<I%ss'%length, length, _x))
03945         buff.write(_struct_i.pack(val1.type))
03946         _v246 = val1.orientation
03947         _x = _v246
03948         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03949         _x = val1
03950         buff.write(_struct_4d.pack(_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight))
03951       length = len(self.goal.path_constraints.visibility_constraints)
03952       buff.write(_struct_I.pack(length))
03953       for val1 in self.goal.path_constraints.visibility_constraints:
03954         _v247 = val1.header
03955         buff.write(_struct_I.pack(_v247.seq))
03956         _v248 = _v247.stamp
03957         _x = _v248
03958         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03959         _x = _v247.frame_id
03960         length = len(_x)
03961         if python3 or type(_x) == unicode:
03962           _x = _x.encode('utf-8')
03963           length = len(_x)
03964         buff.write(struct.pack('<I%ss'%length, length, _x))
03965         _v249 = val1.target
03966         _v250 = _v249.header
03967         buff.write(_struct_I.pack(_v250.seq))
03968         _v251 = _v250.stamp
03969         _x = _v251
03970         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03971         _x = _v250.frame_id
03972         length = len(_x)
03973         if python3 or type(_x) == unicode:
03974           _x = _x.encode('utf-8')
03975           length = len(_x)
03976         buff.write(struct.pack('<I%ss'%length, length, _x))
03977         _v252 = _v249.point
03978         _x = _v252
03979         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03980         _v253 = val1.sensor_pose
03981         _v254 = _v253.header
03982         buff.write(_struct_I.pack(_v254.seq))
03983         _v255 = _v254.stamp
03984         _x = _v255
03985         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
03986         _x = _v254.frame_id
03987         length = len(_x)
03988         if python3 or type(_x) == unicode:
03989           _x = _x.encode('utf-8')
03990           length = len(_x)
03991         buff.write(struct.pack('<I%ss'%length, length, _x))
03992         _v256 = _v253.pose
03993         _v257 = _v256.position
03994         _x = _v257
03995         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
03996         _v258 = _v256.orientation
03997         _x = _v258
03998         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
03999         buff.write(_struct_d.pack(val1.absolute_tolerance))
04000       length = len(self.goal.additional_collision_operations.collision_operations)
04001       buff.write(_struct_I.pack(length))
04002       for val1 in self.goal.additional_collision_operations.collision_operations:
04003         _x = val1.object1
04004         length = len(_x)
04005         if python3 or type(_x) == unicode:
04006           _x = _x.encode('utf-8')
04007           length = len(_x)
04008         buff.write(struct.pack('<I%ss'%length, length, _x))
04009         _x = val1.object2
04010         length = len(_x)
04011         if python3 or type(_x) == unicode:
04012           _x = _x.encode('utf-8')
04013           length = len(_x)
04014         buff.write(struct.pack('<I%ss'%length, length, _x))
04015         _x = val1
04016         buff.write(_struct_di.pack(_x.penetration_distance, _x.operation))
04017       length = len(self.goal.additional_link_padding)
04018       buff.write(_struct_I.pack(length))
04019       for val1 in self.goal.additional_link_padding:
04020         _x = val1.link_name
04021         length = len(_x)
04022         if python3 or type(_x) == unicode:
04023           _x = _x.encode('utf-8')
04024           length = len(_x)
04025         buff.write(struct.pack('<I%ss'%length, length, _x))
04026         buff.write(_struct_d.pack(val1.padding))
04027       length = len(self.goal.movable_obstacles)
04028       buff.write(_struct_I.pack(length))
04029       for val1 in self.goal.movable_obstacles:
04030         _x = val1.reference_frame_id
04031         length = len(_x)
04032         if python3 or type(_x) == unicode:
04033           _x = _x.encode('utf-8')
04034           length = len(_x)
04035         buff.write(struct.pack('<I%ss'%length, length, _x))
04036         length = len(val1.potential_models)
04037         buff.write(_struct_I.pack(length))
04038         for val2 in val1.potential_models:
04039           buff.write(_struct_i.pack(val2.model_id))
04040           _v259 = val2.pose
04041           _v260 = _v259.header
04042           buff.write(_struct_I.pack(_v260.seq))
04043           _v261 = _v260.stamp
04044           _x = _v261
04045           buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04046           _x = _v260.frame_id
04047           length = len(_x)
04048           if python3 or type(_x) == unicode:
04049             _x = _x.encode('utf-8')
04050             length = len(_x)
04051           buff.write(struct.pack('<I%ss'%length, length, _x))
04052           _v262 = _v259.pose
04053           _v263 = _v262.position
04054           _x = _v263
04055           buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04056           _v264 = _v262.orientation
04057           _x = _v264
04058           buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
04059           buff.write(_struct_f.pack(val2.confidence))
04060           _x = val2.detector_name
04061           length = len(_x)
04062           if python3 or type(_x) == unicode:
04063             _x = _x.encode('utf-8')
04064             length = len(_x)
04065           buff.write(struct.pack('<I%ss'%length, length, _x))
04066         _v265 = val1.cluster
04067         _v266 = _v265.header
04068         buff.write(_struct_I.pack(_v266.seq))
04069         _v267 = _v266.stamp
04070         _x = _v267
04071         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04072         _x = _v266.frame_id
04073         length = len(_x)
04074         if python3 or type(_x) == unicode:
04075           _x = _x.encode('utf-8')
04076           length = len(_x)
04077         buff.write(struct.pack('<I%ss'%length, length, _x))
04078         length = len(_v265.points)
04079         buff.write(_struct_I.pack(length))
04080         for val3 in _v265.points:
04081           _x = val3
04082           buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
04083         length = len(_v265.channels)
04084         buff.write(_struct_I.pack(length))
04085         for val3 in _v265.channels:
04086           _x = val3.name
04087           length = len(_x)
04088           if python3 or type(_x) == unicode:
04089             _x = _x.encode('utf-8')
04090             length = len(_x)
04091           buff.write(struct.pack('<I%ss'%length, length, _x))
04092           length = len(val3.values)
04093           buff.write(_struct_I.pack(length))
04094           pattern = '<%sf'%length
04095           buff.write(val3.values.tostring())
04096         _v268 = val1.region
04097         _v269 = _v268.cloud
04098         _v270 = _v269.header
04099         buff.write(_struct_I.pack(_v270.seq))
04100         _v271 = _v270.stamp
04101         _x = _v271
04102         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04103         _x = _v270.frame_id
04104         length = len(_x)
04105         if python3 or type(_x) == unicode:
04106           _x = _x.encode('utf-8')
04107           length = len(_x)
04108         buff.write(struct.pack('<I%ss'%length, length, _x))
04109         _x = _v269
04110         buff.write(_struct_2I.pack(_x.height, _x.width))
04111         length = len(_v269.fields)
04112         buff.write(_struct_I.pack(length))
04113         for val4 in _v269.fields:
04114           _x = val4.name
04115           length = len(_x)
04116           if python3 or type(_x) == unicode:
04117             _x = _x.encode('utf-8')
04118             length = len(_x)
04119           buff.write(struct.pack('<I%ss'%length, length, _x))
04120           _x = val4
04121           buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
04122         _x = _v269
04123         buff.write(_struct_B2I.pack(_x.is_bigendian, _x.point_step, _x.row_step))
04124         _x = _v269.data
04125         length = len(_x)
04126         # - if encoded as a list instead, serialize as bytes instead of string
04127         if type(_x) in [list, tuple]:
04128           buff.write(struct.pack('<I%sB'%length, length, *_x))
04129         else:
04130           buff.write(struct.pack('<I%ss'%length, length, _x))
04131         buff.write(_struct_B.pack(_v269.is_dense))
04132         length = len(_v268.mask)
04133         buff.write(_struct_I.pack(length))
04134         pattern = '<%si'%length
04135         buff.write(_v268.mask.tostring())
04136         _v272 = _v268.image
04137         _v273 = _v272.header
04138         buff.write(_struct_I.pack(_v273.seq))
04139         _v274 = _v273.stamp
04140         _x = _v274
04141         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04142         _x = _v273.frame_id
04143         length = len(_x)
04144         if python3 or type(_x) == unicode:
04145           _x = _x.encode('utf-8')
04146           length = len(_x)
04147         buff.write(struct.pack('<I%ss'%length, length, _x))
04148         _x = _v272
04149         buff.write(_struct_2I.pack(_x.height, _x.width))
04150         _x = _v272.encoding
04151         length = len(_x)
04152         if python3 or type(_x) == unicode:
04153           _x = _x.encode('utf-8')
04154           length = len(_x)
04155         buff.write(struct.pack('<I%ss'%length, length, _x))
04156         _x = _v272
04157         buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
04158         _x = _v272.data
04159         length = len(_x)
04160         # - if encoded as a list instead, serialize as bytes instead of string
04161         if type(_x) in [list, tuple]:
04162           buff.write(struct.pack('<I%sB'%length, length, *_x))
04163         else:
04164           buff.write(struct.pack('<I%ss'%length, length, _x))
04165         _v275 = _v268.disparity_image
04166         _v276 = _v275.header
04167         buff.write(_struct_I.pack(_v276.seq))
04168         _v277 = _v276.stamp
04169         _x = _v277
04170         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04171         _x = _v276.frame_id
04172         length = len(_x)
04173         if python3 or type(_x) == unicode:
04174           _x = _x.encode('utf-8')
04175           length = len(_x)
04176         buff.write(struct.pack('<I%ss'%length, length, _x))
04177         _x = _v275
04178         buff.write(_struct_2I.pack(_x.height, _x.width))
04179         _x = _v275.encoding
04180         length = len(_x)
04181         if python3 or type(_x) == unicode:
04182           _x = _x.encode('utf-8')
04183           length = len(_x)
04184         buff.write(struct.pack('<I%ss'%length, length, _x))
04185         _x = _v275
04186         buff.write(_struct_BI.pack(_x.is_bigendian, _x.step))
04187         _x = _v275.data
04188         length = len(_x)
04189         # - if encoded as a list instead, serialize as bytes instead of string
04190         if type(_x) in [list, tuple]:
04191           buff.write(struct.pack('<I%sB'%length, length, *_x))
04192         else:
04193           buff.write(struct.pack('<I%ss'%length, length, _x))
04194         _v278 = _v268.cam_info
04195         _v279 = _v278.header
04196         buff.write(_struct_I.pack(_v279.seq))
04197         _v280 = _v279.stamp
04198         _x = _v280
04199         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04200         _x = _v279.frame_id
04201         length = len(_x)
04202         if python3 or type(_x) == unicode:
04203           _x = _x.encode('utf-8')
04204           length = len(_x)
04205         buff.write(struct.pack('<I%ss'%length, length, _x))
04206         _x = _v278
04207         buff.write(_struct_2I.pack(_x.height, _x.width))
04208         _x = _v278.distortion_model
04209         length = len(_x)
04210         if python3 or type(_x) == unicode:
04211           _x = _x.encode('utf-8')
04212           length = len(_x)
04213         buff.write(struct.pack('<I%ss'%length, length, _x))
04214         length = len(_v278.D)
04215         buff.write(_struct_I.pack(length))
04216         pattern = '<%sd'%length
04217         buff.write(_v278.D.tostring())
04218         buff.write(_v278.K.tostring())
04219         buff.write(_v278.R.tostring())
04220         buff.write(_v278.P.tostring())
04221         _x = _v278
04222         buff.write(_struct_2I.pack(_x.binning_x, _x.binning_y))
04223         _v281 = _v278.roi
04224         _x = _v281
04225         buff.write(_struct_4IB.pack(_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify))
04226         _v282 = _v268.roi_box_pose
04227         _v283 = _v282.header
04228         buff.write(_struct_I.pack(_v283.seq))
04229         _v284 = _v283.stamp
04230         _x = _v284
04231         buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
04232         _x = _v283.frame_id
04233         length = len(_x)
04234         if python3 or type(_x) == unicode:
04235           _x = _x.encode('utf-8')
04236           length = len(_x)
04237         buff.write(struct.pack('<I%ss'%length, length, _x))
04238         _v285 = _v282.pose
04239         _v286 = _v285.position
04240         _x = _v286
04241         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04242         _v287 = _v285.orientation
04243         _x = _v287
04244         buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
04245         _v288 = _v268.roi_box_dims
04246         _x = _v288
04247         buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
04248         _x = val1.collision_name
04249         length = len(_x)
04250         if python3 or type(_x) == unicode:
04251           _x = _x.encode('utf-8')
04252           length = len(_x)
04253         buff.write(struct.pack('<I%ss'%length, length, _x))
04254       buff.write(_struct_f.pack(self.goal.max_contact_force))
04255     except struct.error as se: self._check_types(se)
04256     except TypeError as te: self._check_types(te)
04257 
04258   def deserialize_numpy(self, str, numpy):
04259     """
04260     unpack serialized message in str into this message instance using numpy for array types
04261     :param str: byte array of serialized message, ``str``
04262     :param numpy: numpy python module
04263     """
04264     try:
04265       if self.header is None:
04266         self.header = std_msgs.msg.Header()
04267       if self.goal_id is None:
04268         self.goal_id = actionlib_msgs.msg.GoalID()
04269       if self.goal is None:
04270         self.goal = object_manipulation_msgs.msg.PickupGoal()
04271       end = 0
04272       _x = self
04273       start = end
04274       end += 12
04275       (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
04276       start = end
04277       end += 4
04278       (length,) = _struct_I.unpack(str[start:end])
04279       start = end
04280       end += length
04281       if python3:
04282         self.header.frame_id = str[start:end].decode('utf-8')
04283       else:
04284         self.header.frame_id = str[start:end]
04285       _x = self
04286       start = end
04287       end += 8
04288       (_x.goal_id.stamp.secs, _x.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
04289       start = end
04290       end += 4
04291       (length,) = _struct_I.unpack(str[start:end])
04292       start = end
04293       end += length
04294       if python3:
04295         self.goal_id.id = str[start:end].decode('utf-8')
04296       else:
04297         self.goal_id.id = str[start:end]
04298       start = end
04299       end += 4
04300       (length,) = _struct_I.unpack(str[start:end])
04301       start = end
04302       end += length
04303       if python3:
04304         self.goal.arm_name = str[start:end].decode('utf-8')
04305       else:
04306         self.goal.arm_name = str[start:end]
04307       start = end
04308       end += 4
04309       (length,) = _struct_I.unpack(str[start:end])
04310       start = end
04311       end += length
04312       if python3:
04313         self.goal.target.reference_frame_id = str[start:end].decode('utf-8')
04314       else:
04315         self.goal.target.reference_frame_id = str[start:end]
04316       start = end
04317       end += 4
04318       (length,) = _struct_I.unpack(str[start:end])
04319       self.goal.target.potential_models = []
04320       for i in range(0, length):
04321         val1 = household_objects_database_msgs.msg.DatabaseModelPose()
04322         start = end
04323         end += 4
04324         (val1.model_id,) = _struct_i.unpack(str[start:end])
04325         _v289 = val1.pose
04326         _v290 = _v289.header
04327         start = end
04328         end += 4
04329         (_v290.seq,) = _struct_I.unpack(str[start:end])
04330         _v291 = _v290.stamp
04331         _x = _v291
04332         start = end
04333         end += 8
04334         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04335         start = end
04336         end += 4
04337         (length,) = _struct_I.unpack(str[start:end])
04338         start = end
04339         end += length
04340         if python3:
04341           _v290.frame_id = str[start:end].decode('utf-8')
04342         else:
04343           _v290.frame_id = str[start:end]
04344         _v292 = _v289.pose
04345         _v293 = _v292.position
04346         _x = _v293
04347         start = end
04348         end += 24
04349         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04350         _v294 = _v292.orientation
04351         _x = _v294
04352         start = end
04353         end += 32
04354         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04355         start = end
04356         end += 4
04357         (val1.confidence,) = _struct_f.unpack(str[start:end])
04358         start = end
04359         end += 4
04360         (length,) = _struct_I.unpack(str[start:end])
04361         start = end
04362         end += length
04363         if python3:
04364           val1.detector_name = str[start:end].decode('utf-8')
04365         else:
04366           val1.detector_name = str[start:end]
04367         self.goal.target.potential_models.append(val1)
04368       _x = self
04369       start = end
04370       end += 12
04371       (_x.goal.target.cluster.header.seq, _x.goal.target.cluster.header.stamp.secs, _x.goal.target.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
04372       start = end
04373       end += 4
04374       (length,) = _struct_I.unpack(str[start:end])
04375       start = end
04376       end += length
04377       if python3:
04378         self.goal.target.cluster.header.frame_id = str[start:end].decode('utf-8')
04379       else:
04380         self.goal.target.cluster.header.frame_id = str[start:end]
04381       start = end
04382       end += 4
04383       (length,) = _struct_I.unpack(str[start:end])
04384       self.goal.target.cluster.points = []
04385       for i in range(0, length):
04386         val1 = geometry_msgs.msg.Point32()
04387         _x = val1
04388         start = end
04389         end += 12
04390         (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
04391         self.goal.target.cluster.points.append(val1)
04392       start = end
04393       end += 4
04394       (length,) = _struct_I.unpack(str[start:end])
04395       self.goal.target.cluster.channels = []
04396       for i in range(0, length):
04397         val1 = sensor_msgs.msg.ChannelFloat32()
04398         start = end
04399         end += 4
04400         (length,) = _struct_I.unpack(str[start:end])
04401         start = end
04402         end += length
04403         if python3:
04404           val1.name = str[start:end].decode('utf-8')
04405         else:
04406           val1.name = str[start:end]
04407         start = end
04408         end += 4
04409         (length,) = _struct_I.unpack(str[start:end])
04410         pattern = '<%sf'%length
04411         start = end
04412         end += struct.calcsize(pattern)
04413         val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
04414         self.goal.target.cluster.channels.append(val1)
04415       _x = self
04416       start = end
04417       end += 12
04418       (_x.goal.target.region.cloud.header.seq, _x.goal.target.region.cloud.header.stamp.secs, _x.goal.target.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
04419       start = end
04420       end += 4
04421       (length,) = _struct_I.unpack(str[start:end])
04422       start = end
04423       end += length
04424       if python3:
04425         self.goal.target.region.cloud.header.frame_id = str[start:end].decode('utf-8')
04426       else:
04427         self.goal.target.region.cloud.header.frame_id = str[start:end]
04428       _x = self
04429       start = end
04430       end += 8
04431       (_x.goal.target.region.cloud.height, _x.goal.target.region.cloud.width,) = _struct_2I.unpack(str[start:end])
04432       start = end
04433       end += 4
04434       (length,) = _struct_I.unpack(str[start:end])
04435       self.goal.target.region.cloud.fields = []
04436       for i in range(0, length):
04437         val1 = sensor_msgs.msg.PointField()
04438         start = end
04439         end += 4
04440         (length,) = _struct_I.unpack(str[start:end])
04441         start = end
04442         end += length
04443         if python3:
04444           val1.name = str[start:end].decode('utf-8')
04445         else:
04446           val1.name = str[start:end]
04447         _x = val1
04448         start = end
04449         end += 9
04450         (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
04451         self.goal.target.region.cloud.fields.append(val1)
04452       _x = self
04453       start = end
04454       end += 9
04455       (_x.goal.target.region.cloud.is_bigendian, _x.goal.target.region.cloud.point_step, _x.goal.target.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
04456       self.goal.target.region.cloud.is_bigendian = bool(self.goal.target.region.cloud.is_bigendian)
04457       start = end
04458       end += 4
04459       (length,) = _struct_I.unpack(str[start:end])
04460       start = end
04461       end += length
04462       if python3:
04463         self.goal.target.region.cloud.data = str[start:end].decode('utf-8')
04464       else:
04465         self.goal.target.region.cloud.data = str[start:end]
04466       start = end
04467       end += 1
04468       (self.goal.target.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
04469       self.goal.target.region.cloud.is_dense = bool(self.goal.target.region.cloud.is_dense)
04470       start = end
04471       end += 4
04472       (length,) = _struct_I.unpack(str[start:end])
04473       pattern = '<%si'%length
04474       start = end
04475       end += struct.calcsize(pattern)
04476       self.goal.target.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
04477       _x = self
04478       start = end
04479       end += 12
04480       (_x.goal.target.region.image.header.seq, _x.goal.target.region.image.header.stamp.secs, _x.goal.target.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
04481       start = end
04482       end += 4
04483       (length,) = _struct_I.unpack(str[start:end])
04484       start = end
04485       end += length
04486       if python3:
04487         self.goal.target.region.image.header.frame_id = str[start:end].decode('utf-8')
04488       else:
04489         self.goal.target.region.image.header.frame_id = str[start:end]
04490       _x = self
04491       start = end
04492       end += 8
04493       (_x.goal.target.region.image.height, _x.goal.target.region.image.width,) = _struct_2I.unpack(str[start:end])
04494       start = end
04495       end += 4
04496       (length,) = _struct_I.unpack(str[start:end])
04497       start = end
04498       end += length
04499       if python3:
04500         self.goal.target.region.image.encoding = str[start:end].decode('utf-8')
04501       else:
04502         self.goal.target.region.image.encoding = str[start:end]
04503       _x = self
04504       start = end
04505       end += 5
04506       (_x.goal.target.region.image.is_bigendian, _x.goal.target.region.image.step,) = _struct_BI.unpack(str[start:end])
04507       start = end
04508       end += 4
04509       (length,) = _struct_I.unpack(str[start:end])
04510       start = end
04511       end += length
04512       if python3:
04513         self.goal.target.region.image.data = str[start:end].decode('utf-8')
04514       else:
04515         self.goal.target.region.image.data = str[start:end]
04516       _x = self
04517       start = end
04518       end += 12
04519       (_x.goal.target.region.disparity_image.header.seq, _x.goal.target.region.disparity_image.header.stamp.secs, _x.goal.target.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
04520       start = end
04521       end += 4
04522       (length,) = _struct_I.unpack(str[start:end])
04523       start = end
04524       end += length
04525       if python3:
04526         self.goal.target.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
04527       else:
04528         self.goal.target.region.disparity_image.header.frame_id = str[start:end]
04529       _x = self
04530       start = end
04531       end += 8
04532       (_x.goal.target.region.disparity_image.height, _x.goal.target.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
04533       start = end
04534       end += 4
04535       (length,) = _struct_I.unpack(str[start:end])
04536       start = end
04537       end += length
04538       if python3:
04539         self.goal.target.region.disparity_image.encoding = str[start:end].decode('utf-8')
04540       else:
04541         self.goal.target.region.disparity_image.encoding = str[start:end]
04542       _x = self
04543       start = end
04544       end += 5
04545       (_x.goal.target.region.disparity_image.is_bigendian, _x.goal.target.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
04546       start = end
04547       end += 4
04548       (length,) = _struct_I.unpack(str[start:end])
04549       start = end
04550       end += length
04551       if python3:
04552         self.goal.target.region.disparity_image.data = str[start:end].decode('utf-8')
04553       else:
04554         self.goal.target.region.disparity_image.data = str[start:end]
04555       _x = self
04556       start = end
04557       end += 12
04558       (_x.goal.target.region.cam_info.header.seq, _x.goal.target.region.cam_info.header.stamp.secs, _x.goal.target.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
04559       start = end
04560       end += 4
04561       (length,) = _struct_I.unpack(str[start:end])
04562       start = end
04563       end += length
04564       if python3:
04565         self.goal.target.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
04566       else:
04567         self.goal.target.region.cam_info.header.frame_id = str[start:end]
04568       _x = self
04569       start = end
04570       end += 8
04571       (_x.goal.target.region.cam_info.height, _x.goal.target.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
04572       start = end
04573       end += 4
04574       (length,) = _struct_I.unpack(str[start:end])
04575       start = end
04576       end += length
04577       if python3:
04578         self.goal.target.region.cam_info.distortion_model = str[start:end].decode('utf-8')
04579       else:
04580         self.goal.target.region.cam_info.distortion_model = str[start:end]
04581       start = end
04582       end += 4
04583       (length,) = _struct_I.unpack(str[start:end])
04584       pattern = '<%sd'%length
04585       start = end
04586       end += struct.calcsize(pattern)
04587       self.goal.target.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04588       start = end
04589       end += 72
04590       self.goal.target.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
04591       start = end
04592       end += 72
04593       self.goal.target.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
04594       start = end
04595       end += 96
04596       self.goal.target.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
04597       _x = self
04598       start = end
04599       end += 37
04600       (_x.goal.target.region.cam_info.binning_x, _x.goal.target.region.cam_info.binning_y, _x.goal.target.region.cam_info.roi.x_offset, _x.goal.target.region.cam_info.roi.y_offset, _x.goal.target.region.cam_info.roi.height, _x.goal.target.region.cam_info.roi.width, _x.goal.target.region.cam_info.roi.do_rectify, _x.goal.target.region.roi_box_pose.header.seq, _x.goal.target.region.roi_box_pose.header.stamp.secs, _x.goal.target.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
04601       self.goal.target.region.cam_info.roi.do_rectify = bool(self.goal.target.region.cam_info.roi.do_rectify)
04602       start = end
04603       end += 4
04604       (length,) = _struct_I.unpack(str[start:end])
04605       start = end
04606       end += length
04607       if python3:
04608         self.goal.target.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
04609       else:
04610         self.goal.target.region.roi_box_pose.header.frame_id = str[start:end]
04611       _x = self
04612       start = end
04613       end += 80
04614       (_x.goal.target.region.roi_box_pose.pose.position.x, _x.goal.target.region.roi_box_pose.pose.position.y, _x.goal.target.region.roi_box_pose.pose.position.z, _x.goal.target.region.roi_box_pose.pose.orientation.x, _x.goal.target.region.roi_box_pose.pose.orientation.y, _x.goal.target.region.roi_box_pose.pose.orientation.z, _x.goal.target.region.roi_box_pose.pose.orientation.w, _x.goal.target.region.roi_box_dims.x, _x.goal.target.region.roi_box_dims.y, _x.goal.target.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
04615       start = end
04616       end += 4
04617       (length,) = _struct_I.unpack(str[start:end])
04618       start = end
04619       end += length
04620       if python3:
04621         self.goal.target.collision_name = str[start:end].decode('utf-8')
04622       else:
04623         self.goal.target.collision_name = str[start:end]
04624       start = end
04625       end += 4
04626       (length,) = _struct_I.unpack(str[start:end])
04627       self.goal.desired_grasps = []
04628       for i in range(0, length):
04629         val1 = object_manipulation_msgs.msg.Grasp()
04630         _v295 = val1.pre_grasp_posture
04631         _v296 = _v295.header
04632         start = end
04633         end += 4
04634         (_v296.seq,) = _struct_I.unpack(str[start:end])
04635         _v297 = _v296.stamp
04636         _x = _v297
04637         start = end
04638         end += 8
04639         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04640         start = end
04641         end += 4
04642         (length,) = _struct_I.unpack(str[start:end])
04643         start = end
04644         end += length
04645         if python3:
04646           _v296.frame_id = str[start:end].decode('utf-8')
04647         else:
04648           _v296.frame_id = str[start:end]
04649         start = end
04650         end += 4
04651         (length,) = _struct_I.unpack(str[start:end])
04652         _v295.name = []
04653         for i in range(0, length):
04654           start = end
04655           end += 4
04656           (length,) = _struct_I.unpack(str[start:end])
04657           start = end
04658           end += length
04659           if python3:
04660             val3 = str[start:end].decode('utf-8')
04661           else:
04662             val3 = str[start:end]
04663           _v295.name.append(val3)
04664         start = end
04665         end += 4
04666         (length,) = _struct_I.unpack(str[start:end])
04667         pattern = '<%sd'%length
04668         start = end
04669         end += struct.calcsize(pattern)
04670         _v295.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04671         start = end
04672         end += 4
04673         (length,) = _struct_I.unpack(str[start:end])
04674         pattern = '<%sd'%length
04675         start = end
04676         end += struct.calcsize(pattern)
04677         _v295.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04678         start = end
04679         end += 4
04680         (length,) = _struct_I.unpack(str[start:end])
04681         pattern = '<%sd'%length
04682         start = end
04683         end += struct.calcsize(pattern)
04684         _v295.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04685         _v298 = val1.grasp_posture
04686         _v299 = _v298.header
04687         start = end
04688         end += 4
04689         (_v299.seq,) = _struct_I.unpack(str[start:end])
04690         _v300 = _v299.stamp
04691         _x = _v300
04692         start = end
04693         end += 8
04694         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04695         start = end
04696         end += 4
04697         (length,) = _struct_I.unpack(str[start:end])
04698         start = end
04699         end += length
04700         if python3:
04701           _v299.frame_id = str[start:end].decode('utf-8')
04702         else:
04703           _v299.frame_id = str[start:end]
04704         start = end
04705         end += 4
04706         (length,) = _struct_I.unpack(str[start:end])
04707         _v298.name = []
04708         for i in range(0, length):
04709           start = end
04710           end += 4
04711           (length,) = _struct_I.unpack(str[start:end])
04712           start = end
04713           end += length
04714           if python3:
04715             val3 = str[start:end].decode('utf-8')
04716           else:
04717             val3 = str[start:end]
04718           _v298.name.append(val3)
04719         start = end
04720         end += 4
04721         (length,) = _struct_I.unpack(str[start:end])
04722         pattern = '<%sd'%length
04723         start = end
04724         end += struct.calcsize(pattern)
04725         _v298.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04726         start = end
04727         end += 4
04728         (length,) = _struct_I.unpack(str[start:end])
04729         pattern = '<%sd'%length
04730         start = end
04731         end += struct.calcsize(pattern)
04732         _v298.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04733         start = end
04734         end += 4
04735         (length,) = _struct_I.unpack(str[start:end])
04736         pattern = '<%sd'%length
04737         start = end
04738         end += struct.calcsize(pattern)
04739         _v298.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
04740         _v301 = val1.grasp_pose
04741         _v302 = _v301.position
04742         _x = _v302
04743         start = end
04744         end += 24
04745         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04746         _v303 = _v301.orientation
04747         _x = _v303
04748         start = end
04749         end += 32
04750         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04751         _x = val1
04752         start = end
04753         end += 17
04754         (_x.success_probability, _x.cluster_rep, _x.desired_approach_distance, _x.min_approach_distance,) = _struct_dB2f.unpack(str[start:end])
04755         val1.cluster_rep = bool(val1.cluster_rep)
04756         start = end
04757         end += 4
04758         (length,) = _struct_I.unpack(str[start:end])
04759         val1.moved_obstacles = []
04760         for i in range(0, length):
04761           val2 = object_manipulation_msgs.msg.GraspableObject()
04762           start = end
04763           end += 4
04764           (length,) = _struct_I.unpack(str[start:end])
04765           start = end
04766           end += length
04767           if python3:
04768             val2.reference_frame_id = str[start:end].decode('utf-8')
04769           else:
04770             val2.reference_frame_id = str[start:end]
04771           start = end
04772           end += 4
04773           (length,) = _struct_I.unpack(str[start:end])
04774           val2.potential_models = []
04775           for i in range(0, length):
04776             val3 = household_objects_database_msgs.msg.DatabaseModelPose()
04777             start = end
04778             end += 4
04779             (val3.model_id,) = _struct_i.unpack(str[start:end])
04780             _v304 = val3.pose
04781             _v305 = _v304.header
04782             start = end
04783             end += 4
04784             (_v305.seq,) = _struct_I.unpack(str[start:end])
04785             _v306 = _v305.stamp
04786             _x = _v306
04787             start = end
04788             end += 8
04789             (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04790             start = end
04791             end += 4
04792             (length,) = _struct_I.unpack(str[start:end])
04793             start = end
04794             end += length
04795             if python3:
04796               _v305.frame_id = str[start:end].decode('utf-8')
04797             else:
04798               _v305.frame_id = str[start:end]
04799             _v307 = _v304.pose
04800             _v308 = _v307.position
04801             _x = _v308
04802             start = end
04803             end += 24
04804             (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
04805             _v309 = _v307.orientation
04806             _x = _v309
04807             start = end
04808             end += 32
04809             (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
04810             start = end
04811             end += 4
04812             (val3.confidence,) = _struct_f.unpack(str[start:end])
04813             start = end
04814             end += 4
04815             (length,) = _struct_I.unpack(str[start:end])
04816             start = end
04817             end += length
04818             if python3:
04819               val3.detector_name = str[start:end].decode('utf-8')
04820             else:
04821               val3.detector_name = str[start:end]
04822             val2.potential_models.append(val3)
04823           _v310 = val2.cluster
04824           _v311 = _v310.header
04825           start = end
04826           end += 4
04827           (_v311.seq,) = _struct_I.unpack(str[start:end])
04828           _v312 = _v311.stamp
04829           _x = _v312
04830           start = end
04831           end += 8
04832           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04833           start = end
04834           end += 4
04835           (length,) = _struct_I.unpack(str[start:end])
04836           start = end
04837           end += length
04838           if python3:
04839             _v311.frame_id = str[start:end].decode('utf-8')
04840           else:
04841             _v311.frame_id = str[start:end]
04842           start = end
04843           end += 4
04844           (length,) = _struct_I.unpack(str[start:end])
04845           _v310.points = []
04846           for i in range(0, length):
04847             val4 = geometry_msgs.msg.Point32()
04848             _x = val4
04849             start = end
04850             end += 12
04851             (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
04852             _v310.points.append(val4)
04853           start = end
04854           end += 4
04855           (length,) = _struct_I.unpack(str[start:end])
04856           _v310.channels = []
04857           for i in range(0, length):
04858             val4 = sensor_msgs.msg.ChannelFloat32()
04859             start = end
04860             end += 4
04861             (length,) = _struct_I.unpack(str[start:end])
04862             start = end
04863             end += length
04864             if python3:
04865               val4.name = str[start:end].decode('utf-8')
04866             else:
04867               val4.name = str[start:end]
04868             start = end
04869             end += 4
04870             (length,) = _struct_I.unpack(str[start:end])
04871             pattern = '<%sf'%length
04872             start = end
04873             end += struct.calcsize(pattern)
04874             val4.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
04875             _v310.channels.append(val4)
04876           _v313 = val2.region
04877           _v314 = _v313.cloud
04878           _v315 = _v314.header
04879           start = end
04880           end += 4
04881           (_v315.seq,) = _struct_I.unpack(str[start:end])
04882           _v316 = _v315.stamp
04883           _x = _v316
04884           start = end
04885           end += 8
04886           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04887           start = end
04888           end += 4
04889           (length,) = _struct_I.unpack(str[start:end])
04890           start = end
04891           end += length
04892           if python3:
04893             _v315.frame_id = str[start:end].decode('utf-8')
04894           else:
04895             _v315.frame_id = str[start:end]
04896           _x = _v314
04897           start = end
04898           end += 8
04899           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
04900           start = end
04901           end += 4
04902           (length,) = _struct_I.unpack(str[start:end])
04903           _v314.fields = []
04904           for i in range(0, length):
04905             val5 = sensor_msgs.msg.PointField()
04906             start = end
04907             end += 4
04908             (length,) = _struct_I.unpack(str[start:end])
04909             start = end
04910             end += length
04911             if python3:
04912               val5.name = str[start:end].decode('utf-8')
04913             else:
04914               val5.name = str[start:end]
04915             _x = val5
04916             start = end
04917             end += 9
04918             (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
04919             _v314.fields.append(val5)
04920           _x = _v314
04921           start = end
04922           end += 9
04923           (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
04924           _v314.is_bigendian = bool(_v314.is_bigendian)
04925           start = end
04926           end += 4
04927           (length,) = _struct_I.unpack(str[start:end])
04928           start = end
04929           end += length
04930           if python3:
04931             _v314.data = str[start:end].decode('utf-8')
04932           else:
04933             _v314.data = str[start:end]
04934           start = end
04935           end += 1
04936           (_v314.is_dense,) = _struct_B.unpack(str[start:end])
04937           _v314.is_dense = bool(_v314.is_dense)
04938           start = end
04939           end += 4
04940           (length,) = _struct_I.unpack(str[start:end])
04941           pattern = '<%si'%length
04942           start = end
04943           end += struct.calcsize(pattern)
04944           _v313.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
04945           _v317 = _v313.image
04946           _v318 = _v317.header
04947           start = end
04948           end += 4
04949           (_v318.seq,) = _struct_I.unpack(str[start:end])
04950           _v319 = _v318.stamp
04951           _x = _v319
04952           start = end
04953           end += 8
04954           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
04955           start = end
04956           end += 4
04957           (length,) = _struct_I.unpack(str[start:end])
04958           start = end
04959           end += length
04960           if python3:
04961             _v318.frame_id = str[start:end].decode('utf-8')
04962           else:
04963             _v318.frame_id = str[start:end]
04964           _x = _v317
04965           start = end
04966           end += 8
04967           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
04968           start = end
04969           end += 4
04970           (length,) = _struct_I.unpack(str[start:end])
04971           start = end
04972           end += length
04973           if python3:
04974             _v317.encoding = str[start:end].decode('utf-8')
04975           else:
04976             _v317.encoding = str[start:end]
04977           _x = _v317
04978           start = end
04979           end += 5
04980           (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
04981           start = end
04982           end += 4
04983           (length,) = _struct_I.unpack(str[start:end])
04984           start = end
04985           end += length
04986           if python3:
04987             _v317.data = str[start:end].decode('utf-8')
04988           else:
04989             _v317.data = str[start:end]
04990           _v320 = _v313.disparity_image
04991           _v321 = _v320.header
04992           start = end
04993           end += 4
04994           (_v321.seq,) = _struct_I.unpack(str[start:end])
04995           _v322 = _v321.stamp
04996           _x = _v322
04997           start = end
04998           end += 8
04999           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05000           start = end
05001           end += 4
05002           (length,) = _struct_I.unpack(str[start:end])
05003           start = end
05004           end += length
05005           if python3:
05006             _v321.frame_id = str[start:end].decode('utf-8')
05007           else:
05008             _v321.frame_id = str[start:end]
05009           _x = _v320
05010           start = end
05011           end += 8
05012           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
05013           start = end
05014           end += 4
05015           (length,) = _struct_I.unpack(str[start:end])
05016           start = end
05017           end += length
05018           if python3:
05019             _v320.encoding = str[start:end].decode('utf-8')
05020           else:
05021             _v320.encoding = str[start:end]
05022           _x = _v320
05023           start = end
05024           end += 5
05025           (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
05026           start = end
05027           end += 4
05028           (length,) = _struct_I.unpack(str[start:end])
05029           start = end
05030           end += length
05031           if python3:
05032             _v320.data = str[start:end].decode('utf-8')
05033           else:
05034             _v320.data = str[start:end]
05035           _v323 = _v313.cam_info
05036           _v324 = _v323.header
05037           start = end
05038           end += 4
05039           (_v324.seq,) = _struct_I.unpack(str[start:end])
05040           _v325 = _v324.stamp
05041           _x = _v325
05042           start = end
05043           end += 8
05044           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05045           start = end
05046           end += 4
05047           (length,) = _struct_I.unpack(str[start:end])
05048           start = end
05049           end += length
05050           if python3:
05051             _v324.frame_id = str[start:end].decode('utf-8')
05052           else:
05053             _v324.frame_id = str[start:end]
05054           _x = _v323
05055           start = end
05056           end += 8
05057           (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
05058           start = end
05059           end += 4
05060           (length,) = _struct_I.unpack(str[start:end])
05061           start = end
05062           end += length
05063           if python3:
05064             _v323.distortion_model = str[start:end].decode('utf-8')
05065           else:
05066             _v323.distortion_model = str[start:end]
05067           start = end
05068           end += 4
05069           (length,) = _struct_I.unpack(str[start:end])
05070           pattern = '<%sd'%length
05071           start = end
05072           end += struct.calcsize(pattern)
05073           _v323.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
05074           start = end
05075           end += 72
05076           _v323.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
05077           start = end
05078           end += 72
05079           _v323.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
05080           start = end
05081           end += 96
05082           _v323.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
05083           _x = _v323
05084           start = end
05085           end += 8
05086           (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
05087           _v326 = _v323.roi
05088           _x = _v326
05089           start = end
05090           end += 17
05091           (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
05092           _v326.do_rectify = bool(_v326.do_rectify)
05093           _v327 = _v313.roi_box_pose
05094           _v328 = _v327.header
05095           start = end
05096           end += 4
05097           (_v328.seq,) = _struct_I.unpack(str[start:end])
05098           _v329 = _v328.stamp
05099           _x = _v329
05100           start = end
05101           end += 8
05102           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05103           start = end
05104           end += 4
05105           (length,) = _struct_I.unpack(str[start:end])
05106           start = end
05107           end += length
05108           if python3:
05109             _v328.frame_id = str[start:end].decode('utf-8')
05110           else:
05111             _v328.frame_id = str[start:end]
05112           _v330 = _v327.pose
05113           _v331 = _v330.position
05114           _x = _v331
05115           start = end
05116           end += 24
05117           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05118           _v332 = _v330.orientation
05119           _x = _v332
05120           start = end
05121           end += 32
05122           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05123           _v333 = _v313.roi_box_dims
05124           _x = _v333
05125           start = end
05126           end += 24
05127           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05128           start = end
05129           end += 4
05130           (length,) = _struct_I.unpack(str[start:end])
05131           start = end
05132           end += length
05133           if python3:
05134             val2.collision_name = str[start:end].decode('utf-8')
05135           else:
05136             val2.collision_name = str[start:end]
05137           val1.moved_obstacles.append(val2)
05138         self.goal.desired_grasps.append(val1)
05139       _x = self
05140       start = end
05141       end += 12
05142       (_x.goal.lift.direction.header.seq, _x.goal.lift.direction.header.stamp.secs, _x.goal.lift.direction.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
05143       start = end
05144       end += 4
05145       (length,) = _struct_I.unpack(str[start:end])
05146       start = end
05147       end += length
05148       if python3:
05149         self.goal.lift.direction.header.frame_id = str[start:end].decode('utf-8')
05150       else:
05151         self.goal.lift.direction.header.frame_id = str[start:end]
05152       _x = self
05153       start = end
05154       end += 32
05155       (_x.goal.lift.direction.vector.x, _x.goal.lift.direction.vector.y, _x.goal.lift.direction.vector.z, _x.goal.lift.desired_distance, _x.goal.lift.min_distance,) = _struct_3d2f.unpack(str[start:end])
05156       start = end
05157       end += 4
05158       (length,) = _struct_I.unpack(str[start:end])
05159       start = end
05160       end += length
05161       if python3:
05162         self.goal.collision_object_name = str[start:end].decode('utf-8')
05163       else:
05164         self.goal.collision_object_name = str[start:end]
05165       start = end
05166       end += 4
05167       (length,) = _struct_I.unpack(str[start:end])
05168       start = end
05169       end += length
05170       if python3:
05171         self.goal.collision_support_surface_name = str[start:end].decode('utf-8')
05172       else:
05173         self.goal.collision_support_surface_name = str[start:end]
05174       _x = self
05175       start = end
05176       end += 5
05177       (_x.goal.allow_gripper_support_collision, _x.goal.use_reactive_execution, _x.goal.use_reactive_lift, _x.goal.only_perform_feasibility_test, _x.goal.ignore_collisions,) = _struct_5B.unpack(str[start:end])
05178       self.goal.allow_gripper_support_collision = bool(self.goal.allow_gripper_support_collision)
05179       self.goal.use_reactive_execution = bool(self.goal.use_reactive_execution)
05180       self.goal.use_reactive_lift = bool(self.goal.use_reactive_lift)
05181       self.goal.only_perform_feasibility_test = bool(self.goal.only_perform_feasibility_test)
05182       self.goal.ignore_collisions = bool(self.goal.ignore_collisions)
05183       start = end
05184       end += 4
05185       (length,) = _struct_I.unpack(str[start:end])
05186       self.goal.path_constraints.joint_constraints = []
05187       for i in range(0, length):
05188         val1 = arm_navigation_msgs.msg.JointConstraint()
05189         start = end
05190         end += 4
05191         (length,) = _struct_I.unpack(str[start:end])
05192         start = end
05193         end += length
05194         if python3:
05195           val1.joint_name = str[start:end].decode('utf-8')
05196         else:
05197           val1.joint_name = str[start:end]
05198         _x = val1
05199         start = end
05200         end += 32
05201         (_x.position, _x.tolerance_above, _x.tolerance_below, _x.weight,) = _struct_4d.unpack(str[start:end])
05202         self.goal.path_constraints.joint_constraints.append(val1)
05203       start = end
05204       end += 4
05205       (length,) = _struct_I.unpack(str[start:end])
05206       self.goal.path_constraints.position_constraints = []
05207       for i in range(0, length):
05208         val1 = arm_navigation_msgs.msg.PositionConstraint()
05209         _v334 = val1.header
05210         start = end
05211         end += 4
05212         (_v334.seq,) = _struct_I.unpack(str[start:end])
05213         _v335 = _v334.stamp
05214         _x = _v335
05215         start = end
05216         end += 8
05217         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05218         start = end
05219         end += 4
05220         (length,) = _struct_I.unpack(str[start:end])
05221         start = end
05222         end += length
05223         if python3:
05224           _v334.frame_id = str[start:end].decode('utf-8')
05225         else:
05226           _v334.frame_id = str[start:end]
05227         start = end
05228         end += 4
05229         (length,) = _struct_I.unpack(str[start:end])
05230         start = end
05231         end += length
05232         if python3:
05233           val1.link_name = str[start:end].decode('utf-8')
05234         else:
05235           val1.link_name = str[start:end]
05236         _v336 = val1.target_point_offset
05237         _x = _v336
05238         start = end
05239         end += 24
05240         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05241         _v337 = val1.position
05242         _x = _v337
05243         start = end
05244         end += 24
05245         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05246         _v338 = val1.constraint_region_shape
05247         start = end
05248         end += 1
05249         (_v338.type,) = _struct_b.unpack(str[start:end])
05250         start = end
05251         end += 4
05252         (length,) = _struct_I.unpack(str[start:end])
05253         pattern = '<%sd'%length
05254         start = end
05255         end += struct.calcsize(pattern)
05256         _v338.dimensions = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
05257         start = end
05258         end += 4
05259         (length,) = _struct_I.unpack(str[start:end])
05260         pattern = '<%si'%length
05261         start = end
05262         end += struct.calcsize(pattern)
05263         _v338.triangles = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
05264         start = end
05265         end += 4
05266         (length,) = _struct_I.unpack(str[start:end])
05267         _v338.vertices = []
05268         for i in range(0, length):
05269           val3 = geometry_msgs.msg.Point()
05270           _x = val3
05271           start = end
05272           end += 24
05273           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05274           _v338.vertices.append(val3)
05275         _v339 = val1.constraint_region_orientation
05276         _x = _v339
05277         start = end
05278         end += 32
05279         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05280         start = end
05281         end += 8
05282         (val1.weight,) = _struct_d.unpack(str[start:end])
05283         self.goal.path_constraints.position_constraints.append(val1)
05284       start = end
05285       end += 4
05286       (length,) = _struct_I.unpack(str[start:end])
05287       self.goal.path_constraints.orientation_constraints = []
05288       for i in range(0, length):
05289         val1 = arm_navigation_msgs.msg.OrientationConstraint()
05290         _v340 = val1.header
05291         start = end
05292         end += 4
05293         (_v340.seq,) = _struct_I.unpack(str[start:end])
05294         _v341 = _v340.stamp
05295         _x = _v341
05296         start = end
05297         end += 8
05298         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05299         start = end
05300         end += 4
05301         (length,) = _struct_I.unpack(str[start:end])
05302         start = end
05303         end += length
05304         if python3:
05305           _v340.frame_id = str[start:end].decode('utf-8')
05306         else:
05307           _v340.frame_id = str[start:end]
05308         start = end
05309         end += 4
05310         (length,) = _struct_I.unpack(str[start:end])
05311         start = end
05312         end += length
05313         if python3:
05314           val1.link_name = str[start:end].decode('utf-8')
05315         else:
05316           val1.link_name = str[start:end]
05317         start = end
05318         end += 4
05319         (val1.type,) = _struct_i.unpack(str[start:end])
05320         _v342 = val1.orientation
05321         _x = _v342
05322         start = end
05323         end += 32
05324         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05325         _x = val1
05326         start = end
05327         end += 32
05328         (_x.absolute_roll_tolerance, _x.absolute_pitch_tolerance, _x.absolute_yaw_tolerance, _x.weight,) = _struct_4d.unpack(str[start:end])
05329         self.goal.path_constraints.orientation_constraints.append(val1)
05330       start = end
05331       end += 4
05332       (length,) = _struct_I.unpack(str[start:end])
05333       self.goal.path_constraints.visibility_constraints = []
05334       for i in range(0, length):
05335         val1 = arm_navigation_msgs.msg.VisibilityConstraint()
05336         _v343 = val1.header
05337         start = end
05338         end += 4
05339         (_v343.seq,) = _struct_I.unpack(str[start:end])
05340         _v344 = _v343.stamp
05341         _x = _v344
05342         start = end
05343         end += 8
05344         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05345         start = end
05346         end += 4
05347         (length,) = _struct_I.unpack(str[start:end])
05348         start = end
05349         end += length
05350         if python3:
05351           _v343.frame_id = str[start:end].decode('utf-8')
05352         else:
05353           _v343.frame_id = str[start:end]
05354         _v345 = val1.target
05355         _v346 = _v345.header
05356         start = end
05357         end += 4
05358         (_v346.seq,) = _struct_I.unpack(str[start:end])
05359         _v347 = _v346.stamp
05360         _x = _v347
05361         start = end
05362         end += 8
05363         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05364         start = end
05365         end += 4
05366         (length,) = _struct_I.unpack(str[start:end])
05367         start = end
05368         end += length
05369         if python3:
05370           _v346.frame_id = str[start:end].decode('utf-8')
05371         else:
05372           _v346.frame_id = str[start:end]
05373         _v348 = _v345.point
05374         _x = _v348
05375         start = end
05376         end += 24
05377         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05378         _v349 = val1.sensor_pose
05379         _v350 = _v349.header
05380         start = end
05381         end += 4
05382         (_v350.seq,) = _struct_I.unpack(str[start:end])
05383         _v351 = _v350.stamp
05384         _x = _v351
05385         start = end
05386         end += 8
05387         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05388         start = end
05389         end += 4
05390         (length,) = _struct_I.unpack(str[start:end])
05391         start = end
05392         end += length
05393         if python3:
05394           _v350.frame_id = str[start:end].decode('utf-8')
05395         else:
05396           _v350.frame_id = str[start:end]
05397         _v352 = _v349.pose
05398         _v353 = _v352.position
05399         _x = _v353
05400         start = end
05401         end += 24
05402         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05403         _v354 = _v352.orientation
05404         _x = _v354
05405         start = end
05406         end += 32
05407         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05408         start = end
05409         end += 8
05410         (val1.absolute_tolerance,) = _struct_d.unpack(str[start:end])
05411         self.goal.path_constraints.visibility_constraints.append(val1)
05412       start = end
05413       end += 4
05414       (length,) = _struct_I.unpack(str[start:end])
05415       self.goal.additional_collision_operations.collision_operations = []
05416       for i in range(0, length):
05417         val1 = arm_navigation_msgs.msg.CollisionOperation()
05418         start = end
05419         end += 4
05420         (length,) = _struct_I.unpack(str[start:end])
05421         start = end
05422         end += length
05423         if python3:
05424           val1.object1 = str[start:end].decode('utf-8')
05425         else:
05426           val1.object1 = str[start:end]
05427         start = end
05428         end += 4
05429         (length,) = _struct_I.unpack(str[start:end])
05430         start = end
05431         end += length
05432         if python3:
05433           val1.object2 = str[start:end].decode('utf-8')
05434         else:
05435           val1.object2 = str[start:end]
05436         _x = val1
05437         start = end
05438         end += 12
05439         (_x.penetration_distance, _x.operation,) = _struct_di.unpack(str[start:end])
05440         self.goal.additional_collision_operations.collision_operations.append(val1)
05441       start = end
05442       end += 4
05443       (length,) = _struct_I.unpack(str[start:end])
05444       self.goal.additional_link_padding = []
05445       for i in range(0, length):
05446         val1 = arm_navigation_msgs.msg.LinkPadding()
05447         start = end
05448         end += 4
05449         (length,) = _struct_I.unpack(str[start:end])
05450         start = end
05451         end += length
05452         if python3:
05453           val1.link_name = str[start:end].decode('utf-8')
05454         else:
05455           val1.link_name = str[start:end]
05456         start = end
05457         end += 8
05458         (val1.padding,) = _struct_d.unpack(str[start:end])
05459         self.goal.additional_link_padding.append(val1)
05460       start = end
05461       end += 4
05462       (length,) = _struct_I.unpack(str[start:end])
05463       self.goal.movable_obstacles = []
05464       for i in range(0, length):
05465         val1 = object_manipulation_msgs.msg.GraspableObject()
05466         start = end
05467         end += 4
05468         (length,) = _struct_I.unpack(str[start:end])
05469         start = end
05470         end += length
05471         if python3:
05472           val1.reference_frame_id = str[start:end].decode('utf-8')
05473         else:
05474           val1.reference_frame_id = str[start:end]
05475         start = end
05476         end += 4
05477         (length,) = _struct_I.unpack(str[start:end])
05478         val1.potential_models = []
05479         for i in range(0, length):
05480           val2 = household_objects_database_msgs.msg.DatabaseModelPose()
05481           start = end
05482           end += 4
05483           (val2.model_id,) = _struct_i.unpack(str[start:end])
05484           _v355 = val2.pose
05485           _v356 = _v355.header
05486           start = end
05487           end += 4
05488           (_v356.seq,) = _struct_I.unpack(str[start:end])
05489           _v357 = _v356.stamp
05490           _x = _v357
05491           start = end
05492           end += 8
05493           (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05494           start = end
05495           end += 4
05496           (length,) = _struct_I.unpack(str[start:end])
05497           start = end
05498           end += length
05499           if python3:
05500             _v356.frame_id = str[start:end].decode('utf-8')
05501           else:
05502             _v356.frame_id = str[start:end]
05503           _v358 = _v355.pose
05504           _v359 = _v358.position
05505           _x = _v359
05506           start = end
05507           end += 24
05508           (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05509           _v360 = _v358.orientation
05510           _x = _v360
05511           start = end
05512           end += 32
05513           (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05514           start = end
05515           end += 4
05516           (val2.confidence,) = _struct_f.unpack(str[start:end])
05517           start = end
05518           end += 4
05519           (length,) = _struct_I.unpack(str[start:end])
05520           start = end
05521           end += length
05522           if python3:
05523             val2.detector_name = str[start:end].decode('utf-8')
05524           else:
05525             val2.detector_name = str[start:end]
05526           val1.potential_models.append(val2)
05527         _v361 = val1.cluster
05528         _v362 = _v361.header
05529         start = end
05530         end += 4
05531         (_v362.seq,) = _struct_I.unpack(str[start:end])
05532         _v363 = _v362.stamp
05533         _x = _v363
05534         start = end
05535         end += 8
05536         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05537         start = end
05538         end += 4
05539         (length,) = _struct_I.unpack(str[start:end])
05540         start = end
05541         end += length
05542         if python3:
05543           _v362.frame_id = str[start:end].decode('utf-8')
05544         else:
05545           _v362.frame_id = str[start:end]
05546         start = end
05547         end += 4
05548         (length,) = _struct_I.unpack(str[start:end])
05549         _v361.points = []
05550         for i in range(0, length):
05551           val3 = geometry_msgs.msg.Point32()
05552           _x = val3
05553           start = end
05554           end += 12
05555           (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
05556           _v361.points.append(val3)
05557         start = end
05558         end += 4
05559         (length,) = _struct_I.unpack(str[start:end])
05560         _v361.channels = []
05561         for i in range(0, length):
05562           val3 = sensor_msgs.msg.ChannelFloat32()
05563           start = end
05564           end += 4
05565           (length,) = _struct_I.unpack(str[start:end])
05566           start = end
05567           end += length
05568           if python3:
05569             val3.name = str[start:end].decode('utf-8')
05570           else:
05571             val3.name = str[start:end]
05572           start = end
05573           end += 4
05574           (length,) = _struct_I.unpack(str[start:end])
05575           pattern = '<%sf'%length
05576           start = end
05577           end += struct.calcsize(pattern)
05578           val3.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
05579           _v361.channels.append(val3)
05580         _v364 = val1.region
05581         _v365 = _v364.cloud
05582         _v366 = _v365.header
05583         start = end
05584         end += 4
05585         (_v366.seq,) = _struct_I.unpack(str[start:end])
05586         _v367 = _v366.stamp
05587         _x = _v367
05588         start = end
05589         end += 8
05590         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05591         start = end
05592         end += 4
05593         (length,) = _struct_I.unpack(str[start:end])
05594         start = end
05595         end += length
05596         if python3:
05597           _v366.frame_id = str[start:end].decode('utf-8')
05598         else:
05599           _v366.frame_id = str[start:end]
05600         _x = _v365
05601         start = end
05602         end += 8
05603         (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
05604         start = end
05605         end += 4
05606         (length,) = _struct_I.unpack(str[start:end])
05607         _v365.fields = []
05608         for i in range(0, length):
05609           val4 = sensor_msgs.msg.PointField()
05610           start = end
05611           end += 4
05612           (length,) = _struct_I.unpack(str[start:end])
05613           start = end
05614           end += length
05615           if python3:
05616             val4.name = str[start:end].decode('utf-8')
05617           else:
05618             val4.name = str[start:end]
05619           _x = val4
05620           start = end
05621           end += 9
05622           (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
05623           _v365.fields.append(val4)
05624         _x = _v365
05625         start = end
05626         end += 9
05627         (_x.is_bigendian, _x.point_step, _x.row_step,) = _struct_B2I.unpack(str[start:end])
05628         _v365.is_bigendian = bool(_v365.is_bigendian)
05629         start = end
05630         end += 4
05631         (length,) = _struct_I.unpack(str[start:end])
05632         start = end
05633         end += length
05634         if python3:
05635           _v365.data = str[start:end].decode('utf-8')
05636         else:
05637           _v365.data = str[start:end]
05638         start = end
05639         end += 1
05640         (_v365.is_dense,) = _struct_B.unpack(str[start:end])
05641         _v365.is_dense = bool(_v365.is_dense)
05642         start = end
05643         end += 4
05644         (length,) = _struct_I.unpack(str[start:end])
05645         pattern = '<%si'%length
05646         start = end
05647         end += struct.calcsize(pattern)
05648         _v364.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
05649         _v368 = _v364.image
05650         _v369 = _v368.header
05651         start = end
05652         end += 4
05653         (_v369.seq,) = _struct_I.unpack(str[start:end])
05654         _v370 = _v369.stamp
05655         _x = _v370
05656         start = end
05657         end += 8
05658         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05659         start = end
05660         end += 4
05661         (length,) = _struct_I.unpack(str[start:end])
05662         start = end
05663         end += length
05664         if python3:
05665           _v369.frame_id = str[start:end].decode('utf-8')
05666         else:
05667           _v369.frame_id = str[start:end]
05668         _x = _v368
05669         start = end
05670         end += 8
05671         (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
05672         start = end
05673         end += 4
05674         (length,) = _struct_I.unpack(str[start:end])
05675         start = end
05676         end += length
05677         if python3:
05678           _v368.encoding = str[start:end].decode('utf-8')
05679         else:
05680           _v368.encoding = str[start:end]
05681         _x = _v368
05682         start = end
05683         end += 5
05684         (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
05685         start = end
05686         end += 4
05687         (length,) = _struct_I.unpack(str[start:end])
05688         start = end
05689         end += length
05690         if python3:
05691           _v368.data = str[start:end].decode('utf-8')
05692         else:
05693           _v368.data = str[start:end]
05694         _v371 = _v364.disparity_image
05695         _v372 = _v371.header
05696         start = end
05697         end += 4
05698         (_v372.seq,) = _struct_I.unpack(str[start:end])
05699         _v373 = _v372.stamp
05700         _x = _v373
05701         start = end
05702         end += 8
05703         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05704         start = end
05705         end += 4
05706         (length,) = _struct_I.unpack(str[start:end])
05707         start = end
05708         end += length
05709         if python3:
05710           _v372.frame_id = str[start:end].decode('utf-8')
05711         else:
05712           _v372.frame_id = str[start:end]
05713         _x = _v371
05714         start = end
05715         end += 8
05716         (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
05717         start = end
05718         end += 4
05719         (length,) = _struct_I.unpack(str[start:end])
05720         start = end
05721         end += length
05722         if python3:
05723           _v371.encoding = str[start:end].decode('utf-8')
05724         else:
05725           _v371.encoding = str[start:end]
05726         _x = _v371
05727         start = end
05728         end += 5
05729         (_x.is_bigendian, _x.step,) = _struct_BI.unpack(str[start:end])
05730         start = end
05731         end += 4
05732         (length,) = _struct_I.unpack(str[start:end])
05733         start = end
05734         end += length
05735         if python3:
05736           _v371.data = str[start:end].decode('utf-8')
05737         else:
05738           _v371.data = str[start:end]
05739         _v374 = _v364.cam_info
05740         _v375 = _v374.header
05741         start = end
05742         end += 4
05743         (_v375.seq,) = _struct_I.unpack(str[start:end])
05744         _v376 = _v375.stamp
05745         _x = _v376
05746         start = end
05747         end += 8
05748         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05749         start = end
05750         end += 4
05751         (length,) = _struct_I.unpack(str[start:end])
05752         start = end
05753         end += length
05754         if python3:
05755           _v375.frame_id = str[start:end].decode('utf-8')
05756         else:
05757           _v375.frame_id = str[start:end]
05758         _x = _v374
05759         start = end
05760         end += 8
05761         (_x.height, _x.width,) = _struct_2I.unpack(str[start:end])
05762         start = end
05763         end += 4
05764         (length,) = _struct_I.unpack(str[start:end])
05765         start = end
05766         end += length
05767         if python3:
05768           _v374.distortion_model = str[start:end].decode('utf-8')
05769         else:
05770           _v374.distortion_model = str[start:end]
05771         start = end
05772         end += 4
05773         (length,) = _struct_I.unpack(str[start:end])
05774         pattern = '<%sd'%length
05775         start = end
05776         end += struct.calcsize(pattern)
05777         _v374.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
05778         start = end
05779         end += 72
05780         _v374.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
05781         start = end
05782         end += 72
05783         _v374.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
05784         start = end
05785         end += 96
05786         _v374.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
05787         _x = _v374
05788         start = end
05789         end += 8
05790         (_x.binning_x, _x.binning_y,) = _struct_2I.unpack(str[start:end])
05791         _v377 = _v374.roi
05792         _x = _v377
05793         start = end
05794         end += 17
05795         (_x.x_offset, _x.y_offset, _x.height, _x.width, _x.do_rectify,) = _struct_4IB.unpack(str[start:end])
05796         _v377.do_rectify = bool(_v377.do_rectify)
05797         _v378 = _v364.roi_box_pose
05798         _v379 = _v378.header
05799         start = end
05800         end += 4
05801         (_v379.seq,) = _struct_I.unpack(str[start:end])
05802         _v380 = _v379.stamp
05803         _x = _v380
05804         start = end
05805         end += 8
05806         (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
05807         start = end
05808         end += 4
05809         (length,) = _struct_I.unpack(str[start:end])
05810         start = end
05811         end += length
05812         if python3:
05813           _v379.frame_id = str[start:end].decode('utf-8')
05814         else:
05815           _v379.frame_id = str[start:end]
05816         _v381 = _v378.pose
05817         _v382 = _v381.position
05818         _x = _v382
05819         start = end
05820         end += 24
05821         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05822         _v383 = _v381.orientation
05823         _x = _v383
05824         start = end
05825         end += 32
05826         (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
05827         _v384 = _v364.roi_box_dims
05828         _x = _v384
05829         start = end
05830         end += 24
05831         (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
05832         start = end
05833         end += 4
05834         (length,) = _struct_I.unpack(str[start:end])
05835         start = end
05836         end += length
05837         if python3:
05838           val1.collision_name = str[start:end].decode('utf-8')
05839         else:
05840           val1.collision_name = str[start:end]
05841         self.goal.movable_obstacles.append(val1)
05842       start = end
05843       end += 4
05844       (self.goal.max_contact_force,) = _struct_f.unpack(str[start:end])
05845       return self
05846     except struct.error as e:
05847       raise genpy.DeserializationError(e) #most likely buffer underfill
05848 
05849 _struct_I = genpy.struct_I
05850 _struct_IBI = struct.Struct("<IBI")
05851 _struct_12d = struct.Struct("<12d")
05852 _struct_dB2f = struct.Struct("<dB2f")
05853 _struct_BI = struct.Struct("<BI")
05854 _struct_10d = struct.Struct("<10d")
05855 _struct_3I = struct.Struct("<3I")
05856 _struct_B2I = struct.Struct("<B2I")
05857 _struct_5B = struct.Struct("<5B")
05858 _struct_3f = struct.Struct("<3f")
05859 _struct_3d = struct.Struct("<3d")
05860 _struct_B = struct.Struct("<B")
05861 _struct_di = struct.Struct("<di")
05862 _struct_9d = struct.Struct("<9d")
05863 _struct_2I = struct.Struct("<2I")
05864 _struct_6IB3I = struct.Struct("<6IB3I")
05865 _struct_b = struct.Struct("<b")
05866 _struct_d = struct.Struct("<d")
05867 _struct_f = struct.Struct("<f")
05868 _struct_i = struct.Struct("<i")
05869 _struct_3d2f = struct.Struct("<3d2f")
05870 _struct_4d = struct.Struct("<4d")
05871 _struct_4IB = struct.Struct("<4IB")


object_manipulation_msgs
Author(s): Matei Ciocarlie
autogenerated on Thu Jan 2 2014 11:38:11