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