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