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
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")