00001 """autogenerated by genpy from pr2_object_manipulation_msgs/GetGripperPoseAction.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 pr2_object_manipulation_msgs.msg
00008 import actionlib_msgs.msg
00009 import manipulation_msgs.msg
00010 import geometry_msgs.msg
00011 import object_recognition_msgs.msg
00012 import sensor_msgs.msg
00013 import household_objects_database_msgs.msg
00014 import genpy
00015 import std_msgs.msg
00016
00017 class GetGripperPoseAction(genpy.Message):
00018 _md5sum = "a514ad85b1422e407ceb3ba106378828"
00019 _type = "pr2_object_manipulation_msgs/GetGripperPoseAction"
00020 _has_header = False
00021 _full_text = """# ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00022
00023 GetGripperPoseActionGoal action_goal
00024 GetGripperPoseActionResult action_result
00025 GetGripperPoseActionFeedback action_feedback
00026
00027 ================================================================================
00028 MSG: pr2_object_manipulation_msgs/GetGripperPoseActionGoal
00029 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00030
00031 Header header
00032 actionlib_msgs/GoalID goal_id
00033 GetGripperPoseGoal goal
00034
00035 ================================================================================
00036 MSG: std_msgs/Header
00037 # Standard metadata for higher-level stamped data types.
00038 # This is generally used to communicate timestamped data
00039 # in a particular coordinate frame.
00040 #
00041 # sequence ID: consecutively increasing ID
00042 uint32 seq
00043 #Two-integer timestamp that is expressed as:
00044 # * stamp.secs: seconds (stamp_secs) since epoch
00045 # * stamp.nsecs: nanoseconds since stamp_secs
00046 # time-handling sugar is provided by the client library
00047 time stamp
00048 #Frame this data is associated with
00049 # 0: no frame
00050 # 1: global frame
00051 string frame_id
00052
00053 ================================================================================
00054 MSG: actionlib_msgs/GoalID
00055 # The stamp should store the time at which this goal was requested.
00056 # It is used by an action server when it tries to preempt all
00057 # goals that were requested before a certain time
00058 time stamp
00059
00060 # The id provides a way to associate feedback and
00061 # result message with specific goal requests. The id
00062 # specified must be unique.
00063 string id
00064
00065
00066 ================================================================================
00067 MSG: pr2_object_manipulation_msgs/GetGripperPoseGoal
00068 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00069
00070 # for which arm are we requesting a pose
00071 # useful for knowing which arm to initialize from when seed is empty
00072 string arm_name
00073
00074 # a seed position for the ghosted gripper. If emtpy (quaternion of norm 0)
00075 # the ghosted gripper will be initialized at the current position of the gripper
00076 geometry_msgs/PoseStamped gripper_pose
00077 float32 gripper_opening
00078
00079 # An object that the gripper is holding. May be empty.
00080 manipulation_msgs/GraspableObject object
00081
00082 # how we are holding the object, if any.
00083 manipulation_msgs/Grasp grasp
00084
00085
00086 ================================================================================
00087 MSG: geometry_msgs/PoseStamped
00088 # A Pose with reference coordinate frame and timestamp
00089 Header header
00090 Pose pose
00091
00092 ================================================================================
00093 MSG: geometry_msgs/Pose
00094 # A representation of pose in free space, composed of postion and orientation.
00095 Point position
00096 Quaternion orientation
00097
00098 ================================================================================
00099 MSG: geometry_msgs/Point
00100 # This contains the position of a point in free space
00101 float64 x
00102 float64 y
00103 float64 z
00104
00105 ================================================================================
00106 MSG: geometry_msgs/Quaternion
00107 # This represents an orientation in free space in quaternion form.
00108
00109 float64 x
00110 float64 y
00111 float64 z
00112 float64 w
00113
00114 ================================================================================
00115 MSG: manipulation_msgs/GraspableObject
00116 # an object that the object_manipulator can work on
00117
00118 # a graspable object can be represented in multiple ways. This message
00119 # can contain all of them. Which one is actually used is up to the receiver
00120 # of this message. When adding new representations, one must be careful that
00121 # they have reasonable lightweight defaults indicating that that particular
00122 # representation is not available.
00123
00124 # the tf frame to be used as a reference frame when combining information from
00125 # the different representations below
00126 string reference_frame_id
00127
00128 # potential recognition results from a database of models
00129 # all poses are relative to the object reference pose
00130 household_objects_database_msgs/DatabaseModelPose[] potential_models
00131
00132 # the point cloud itself
00133 sensor_msgs/PointCloud cluster
00134
00135 # a region of a PointCloud2 of interest
00136 SceneRegion region
00137
00138 # the name that this object has in the collision environment
00139 string collision_name
00140 ================================================================================
00141 MSG: household_objects_database_msgs/DatabaseModelPose
00142 # Informs that a specific model from the Model Database has been
00143 # identified at a certain location
00144
00145 # the database id of the model
00146 int32 model_id
00147
00148 # if the object was recognized by the ORK pipeline, its type will be in here
00149 # if this is not empty, then the string in here will be converted to a household_objects_database id
00150 # leave this empty if providing an id in the model_id field
00151 object_recognition_msgs/ObjectType type
00152
00153 # the pose that it can be found in
00154 geometry_msgs/PoseStamped pose
00155
00156 # a measure of the confidence level in this detection result
00157 float32 confidence
00158
00159 # the name of the object detector that generated this detection result
00160 string detector_name
00161
00162 ================================================================================
00163 MSG: object_recognition_msgs/ObjectType
00164 ################################################## OBJECT ID #########################################################
00165
00166 # Contains information about the type of a found object. Those two sets of parameters together uniquely define an
00167 # object
00168
00169 # The key of the found object: the unique identifier in the given db
00170 string key
00171
00172 # The db parameters stored as a JSON/compressed YAML string. An object id does not make sense without the corresponding
00173 # database. E.g., in object_recognition, it can look like: "{'type':'CouchDB', 'root':'http://localhost'}"
00174 # There is no conventional format for those parameters and it's nice to keep that flexibility.
00175 # The object_recognition_core as a generic DB type that can read those fields
00176 # Current examples:
00177 # For CouchDB:
00178 # type: 'CouchDB'
00179 # root: 'http://localhost:5984'
00180 # collection: 'object_recognition'
00181 # For SQL household database:
00182 # type: 'SqlHousehold'
00183 # host: 'wgs36'
00184 # port: 5432
00185 # user: 'willow'
00186 # password: 'willow'
00187 # name: 'household_objects'
00188 # module: 'tabletop'
00189 string db
00190
00191 ================================================================================
00192 MSG: sensor_msgs/PointCloud
00193 # This message holds a collection of 3d points, plus optional additional
00194 # information about each point.
00195
00196 # Time of sensor data acquisition, coordinate frame ID.
00197 Header header
00198
00199 # Array of 3d points. Each Point32 should be interpreted as a 3d point
00200 # in the frame given in the header.
00201 geometry_msgs/Point32[] points
00202
00203 # Each channel should have the same number of elements as points array,
00204 # and the data in each channel should correspond 1:1 with each point.
00205 # Channel names in common practice are listed in ChannelFloat32.msg.
00206 ChannelFloat32[] channels
00207
00208 ================================================================================
00209 MSG: geometry_msgs/Point32
00210 # This contains the position of a point in free space(with 32 bits of precision).
00211 # It is recommeded to use Point wherever possible instead of Point32.
00212 #
00213 # This recommendation is to promote interoperability.
00214 #
00215 # This message is designed to take up less space when sending
00216 # lots of points at once, as in the case of a PointCloud.
00217
00218 float32 x
00219 float32 y
00220 float32 z
00221 ================================================================================
00222 MSG: sensor_msgs/ChannelFloat32
00223 # This message is used by the PointCloud message to hold optional data
00224 # associated with each point in the cloud. The length of the values
00225 # array should be the same as the length of the points array in the
00226 # PointCloud, and each value should be associated with the corresponding
00227 # point.
00228
00229 # Channel names in existing practice include:
00230 # "u", "v" - row and column (respectively) in the left stereo image.
00231 # This is opposite to usual conventions but remains for
00232 # historical reasons. The newer PointCloud2 message has no
00233 # such problem.
00234 # "rgb" - For point clouds produced by color stereo cameras. uint8
00235 # (R,G,B) values packed into the least significant 24 bits,
00236 # in order.
00237 # "intensity" - laser or pixel intensity.
00238 # "distance"
00239
00240 # The channel name should give semantics of the channel (e.g.
00241 # "intensity" instead of "value").
00242 string name
00243
00244 # The values array should be 1-1 with the elements of the associated
00245 # PointCloud.
00246 float32[] values
00247
00248 ================================================================================
00249 MSG: manipulation_msgs/SceneRegion
00250 # Point cloud
00251 sensor_msgs/PointCloud2 cloud
00252
00253 # Indices for the region of interest
00254 int32[] mask
00255
00256 # One of the corresponding 2D images, if applicable
00257 sensor_msgs/Image image
00258
00259 # The disparity image, if applicable
00260 sensor_msgs/Image disparity_image
00261
00262 # Camera info for the camera that took the image
00263 sensor_msgs/CameraInfo cam_info
00264
00265 # a 3D region of interest for grasp planning
00266 geometry_msgs/PoseStamped roi_box_pose
00267 geometry_msgs/Vector3 roi_box_dims
00268
00269 ================================================================================
00270 MSG: sensor_msgs/PointCloud2
00271 # This message holds a collection of N-dimensional points, which may
00272 # contain additional information such as normals, intensity, etc. The
00273 # point data is stored as a binary blob, its layout described by the
00274 # contents of the "fields" array.
00275
00276 # The point cloud data may be organized 2d (image-like) or 1d
00277 # (unordered). Point clouds organized as 2d images may be produced by
00278 # camera depth sensors such as stereo or time-of-flight.
00279
00280 # Time of sensor data acquisition, and the coordinate frame ID (for 3d
00281 # points).
00282 Header header
00283
00284 # 2D structure of the point cloud. If the cloud is unordered, height is
00285 # 1 and width is the length of the point cloud.
00286 uint32 height
00287 uint32 width
00288
00289 # Describes the channels and their layout in the binary data blob.
00290 PointField[] fields
00291
00292 bool is_bigendian # Is this data bigendian?
00293 uint32 point_step # Length of a point in bytes
00294 uint32 row_step # Length of a row in bytes
00295 uint8[] data # Actual point data, size is (row_step*height)
00296
00297 bool is_dense # True if there are no invalid points
00298
00299 ================================================================================
00300 MSG: sensor_msgs/PointField
00301 # This message holds the description of one point entry in the
00302 # PointCloud2 message format.
00303 uint8 INT8 = 1
00304 uint8 UINT8 = 2
00305 uint8 INT16 = 3
00306 uint8 UINT16 = 4
00307 uint8 INT32 = 5
00308 uint8 UINT32 = 6
00309 uint8 FLOAT32 = 7
00310 uint8 FLOAT64 = 8
00311
00312 string name # Name of field
00313 uint32 offset # Offset from start of point struct
00314 uint8 datatype # Datatype enumeration, see above
00315 uint32 count # How many elements in the field
00316
00317 ================================================================================
00318 MSG: sensor_msgs/Image
00319 # This message contains an uncompressed image
00320 # (0, 0) is at top-left corner of image
00321 #
00322
00323 Header header # Header timestamp should be acquisition time of image
00324 # Header frame_id should be optical frame of camera
00325 # origin of frame should be optical center of cameara
00326 # +x should point to the right in the image
00327 # +y should point down in the image
00328 # +z should point into to plane of the image
00329 # If the frame_id here and the frame_id of the CameraInfo
00330 # message associated with the image conflict
00331 # the behavior is undefined
00332
00333 uint32 height # image height, that is, number of rows
00334 uint32 width # image width, that is, number of columns
00335
00336 # The legal values for encoding are in file src/image_encodings.cpp
00337 # If you want to standardize a new string format, join
00338 # ros-users@lists.sourceforge.net and send an email proposing a new encoding.
00339
00340 string encoding # Encoding of pixels -- channel meaning, ordering, size
00341 # taken from the list of strings in include/sensor_msgs/image_encodings.h
00342
00343 uint8 is_bigendian # is this data bigendian?
00344 uint32 step # Full row length in bytes
00345 uint8[] data # actual matrix data, size is (step * rows)
00346
00347 ================================================================================
00348 MSG: sensor_msgs/CameraInfo
00349 # This message defines meta information for a camera. It should be in a
00350 # camera namespace on topic "camera_info" and accompanied by up to five
00351 # image topics named:
00352 #
00353 # image_raw - raw data from the camera driver, possibly Bayer encoded
00354 # image - monochrome, distorted
00355 # image_color - color, distorted
00356 # image_rect - monochrome, rectified
00357 # image_rect_color - color, rectified
00358 #
00359 # The image_pipeline contains packages (image_proc, stereo_image_proc)
00360 # for producing the four processed image topics from image_raw and
00361 # camera_info. The meaning of the camera parameters are described in
00362 # detail at http://www.ros.org/wiki/image_pipeline/CameraInfo.
00363 #
00364 # The image_geometry package provides a user-friendly interface to
00365 # common operations using this meta information. If you want to, e.g.,
00366 # project a 3d point into image coordinates, we strongly recommend
00367 # using image_geometry.
00368 #
00369 # If the camera is uncalibrated, the matrices D, K, R, P should be left
00370 # zeroed out. In particular, clients may assume that K[0] == 0.0
00371 # indicates an uncalibrated camera.
00372
00373 #######################################################################
00374 # Image acquisition info #
00375 #######################################################################
00376
00377 # Time of image acquisition, camera coordinate frame ID
00378 Header header # Header timestamp should be acquisition time of image
00379 # Header frame_id should be optical frame of camera
00380 # origin of frame should be optical center of camera
00381 # +x should point to the right in the image
00382 # +y should point down in the image
00383 # +z should point into the plane of the image
00384
00385
00386 #######################################################################
00387 # Calibration Parameters #
00388 #######################################################################
00389 # These are fixed during camera calibration. Their values will be the #
00390 # same in all messages until the camera is recalibrated. Note that #
00391 # self-calibrating systems may "recalibrate" frequently. #
00392 # #
00393 # The internal parameters can be used to warp a raw (distorted) image #
00394 # to: #
00395 # 1. An undistorted image (requires D and K) #
00396 # 2. A rectified image (requires D, K, R) #
00397 # The projection matrix P projects 3D points into the rectified image.#
00398 #######################################################################
00399
00400 # The image dimensions with which the camera was calibrated. Normally
00401 # this will be the full camera resolution in pixels.
00402 uint32 height
00403 uint32 width
00404
00405 # The distortion model used. Supported models are listed in
00406 # sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a
00407 # simple model of radial and tangential distortion - is sufficent.
00408 string distortion_model
00409
00410 # The distortion parameters, size depending on the distortion model.
00411 # For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
00412 float64[] D
00413
00414 # Intrinsic camera matrix for the raw (distorted) images.
00415 # [fx 0 cx]
00416 # K = [ 0 fy cy]
00417 # [ 0 0 1]
00418 # Projects 3D points in the camera coordinate frame to 2D pixel
00419 # coordinates using the focal lengths (fx, fy) and principal point
00420 # (cx, cy).
00421 float64[9] K # 3x3 row-major matrix
00422
00423 # Rectification matrix (stereo cameras only)
00424 # A rotation matrix aligning the camera coordinate system to the ideal
00425 # stereo image plane so that epipolar lines in both stereo images are
00426 # parallel.
00427 float64[9] R # 3x3 row-major matrix
00428
00429 # Projection/camera matrix
00430 # [fx' 0 cx' Tx]
00431 # P = [ 0 fy' cy' Ty]
00432 # [ 0 0 1 0]
00433 # By convention, this matrix specifies the intrinsic (camera) matrix
00434 # of the processed (rectified) image. That is, the left 3x3 portion
00435 # is the normal camera intrinsic matrix for the rectified image.
00436 # It projects 3D points in the camera coordinate frame to 2D pixel
00437 # coordinates using the focal lengths (fx', fy') and principal point
00438 # (cx', cy') - these may differ from the values in K.
00439 # For monocular cameras, Tx = Ty = 0. Normally, monocular cameras will
00440 # also have R = the identity and P[1:3,1:3] = K.
00441 # For a stereo pair, the fourth column [Tx Ty 0]' is related to the
00442 # position of the optical center of the second camera in the first
00443 # camera's frame. We assume Tz = 0 so both cameras are in the same
00444 # stereo image plane. The first camera always has Tx = Ty = 0. For
00445 # the right (second) camera of a horizontal stereo pair, Ty = 0 and
00446 # Tx = -fx' * B, where B is the baseline between the cameras.
00447 # Given a 3D point [X Y Z]', the projection (x, y) of the point onto
00448 # the rectified image is given by:
00449 # [u v w]' = P * [X Y Z 1]'
00450 # x = u / w
00451 # y = v / w
00452 # This holds for both images of a stereo pair.
00453 float64[12] P # 3x4 row-major matrix
00454
00455
00456 #######################################################################
00457 # Operational Parameters #
00458 #######################################################################
00459 # These define the image region actually captured by the camera #
00460 # driver. Although they affect the geometry of the output image, they #
00461 # may be changed freely without recalibrating the camera. #
00462 #######################################################################
00463
00464 # Binning refers here to any camera setting which combines rectangular
00465 # neighborhoods of pixels into larger "super-pixels." It reduces the
00466 # resolution of the output image to
00467 # (width / binning_x) x (height / binning_y).
00468 # The default values binning_x = binning_y = 0 is considered the same
00469 # as binning_x = binning_y = 1 (no subsampling).
00470 uint32 binning_x
00471 uint32 binning_y
00472
00473 # Region of interest (subwindow of full camera resolution), given in
00474 # full resolution (unbinned) image coordinates. A particular ROI
00475 # always denotes the same window of pixels on the camera sensor,
00476 # regardless of binning settings.
00477 # The default setting of roi (all values 0) is considered the same as
00478 # full resolution (roi.width = width, roi.height = height).
00479 RegionOfInterest roi
00480
00481 ================================================================================
00482 MSG: sensor_msgs/RegionOfInterest
00483 # This message is used to specify a region of interest within an image.
00484 #
00485 # When used to specify the ROI setting of the camera when the image was
00486 # taken, the height and width fields should either match the height and
00487 # width fields for the associated image; or height = width = 0
00488 # indicates that the full resolution image was captured.
00489
00490 uint32 x_offset # Leftmost pixel of the ROI
00491 # (0 if the ROI includes the left edge of the image)
00492 uint32 y_offset # Topmost pixel of the ROI
00493 # (0 if the ROI includes the top edge of the image)
00494 uint32 height # Height of ROI
00495 uint32 width # Width of ROI
00496
00497 # True if a distinct rectified ROI should be calculated from the "raw"
00498 # ROI in this message. Typically this should be False if the full image
00499 # is captured (ROI not used), and True if a subwindow is captured (ROI
00500 # used).
00501 bool do_rectify
00502
00503 ================================================================================
00504 MSG: geometry_msgs/Vector3
00505 # This represents a vector in free space.
00506
00507 float64 x
00508 float64 y
00509 float64 z
00510 ================================================================================
00511 MSG: manipulation_msgs/Grasp
00512 # A name for this grasp
00513 string id
00514
00515 # The internal posture of the hand for the pre-grasp
00516 # only positions are used
00517 sensor_msgs/JointState pre_grasp_posture
00518
00519 # The internal posture of the hand for the grasp
00520 # positions and efforts are used
00521 sensor_msgs/JointState grasp_posture
00522
00523 # The position of the end-effector for the grasp relative to a reference frame
00524 # (that is always specified elsewhere, not in this message)
00525 geometry_msgs/PoseStamped grasp_pose
00526
00527 # The estimated probability of success for this grasp, or some other
00528 # measure of how "good" it is.
00529 float64 grasp_quality
00530
00531 # The approach motion
00532 GripperTranslation approach
00533
00534 # The retreat motion
00535 GripperTranslation retreat
00536
00537 # the maximum contact force to use while grasping (<=0 to disable)
00538 float32 max_contact_force
00539
00540 # an optional list of obstacles that we have semantic information about
00541 # and that can be touched/pushed/moved in the course of grasping
00542 string[] allowed_touch_objects
00543
00544 ================================================================================
00545 MSG: sensor_msgs/JointState
00546 # This is a message that holds data to describe the state of a set of torque controlled joints.
00547 #
00548 # The state of each joint (revolute or prismatic) is defined by:
00549 # * the position of the joint (rad or m),
00550 # * the velocity of the joint (rad/s or m/s) and
00551 # * the effort that is applied in the joint (Nm or N).
00552 #
00553 # Each joint is uniquely identified by its name
00554 # The header specifies the time at which the joint states were recorded. All the joint states
00555 # in one message have to be recorded at the same time.
00556 #
00557 # This message consists of a multiple arrays, one for each part of the joint state.
00558 # The goal is to make each of the fields optional. When e.g. your joints have no
00559 # effort associated with them, you can leave the effort array empty.
00560 #
00561 # All arrays in this message should have the same size, or be empty.
00562 # This is the only way to uniquely associate the joint name with the correct
00563 # states.
00564
00565
00566 Header header
00567
00568 string[] name
00569 float64[] position
00570 float64[] velocity
00571 float64[] effort
00572
00573 ================================================================================
00574 MSG: manipulation_msgs/GripperTranslation
00575 # defines a translation for the gripper, used in pickup or place tasks
00576 # for example for lifting an object off a table or approaching the table for placing
00577
00578 # the direction of the translation
00579 geometry_msgs/Vector3Stamped direction
00580
00581 # the desired translation distance
00582 float32 desired_distance
00583
00584 # the min distance that must be considered feasible before the
00585 # grasp is even attempted
00586 float32 min_distance
00587
00588 ================================================================================
00589 MSG: geometry_msgs/Vector3Stamped
00590 # This represents a Vector3 with reference coordinate frame and timestamp
00591 Header header
00592 Vector3 vector
00593
00594 ================================================================================
00595 MSG: pr2_object_manipulation_msgs/GetGripperPoseActionResult
00596 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00597
00598 Header header
00599 actionlib_msgs/GoalStatus status
00600 GetGripperPoseResult result
00601
00602 ================================================================================
00603 MSG: actionlib_msgs/GoalStatus
00604 GoalID goal_id
00605 uint8 status
00606 uint8 PENDING = 0 # The goal has yet to be processed by the action server
00607 uint8 ACTIVE = 1 # The goal is currently being processed by the action server
00608 uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
00609 # and has since completed its execution (Terminal State)
00610 uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
00611 uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
00612 # to some failure (Terminal State)
00613 uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
00614 # because the goal was unattainable or invalid (Terminal State)
00615 uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
00616 # and has not yet completed execution
00617 uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
00618 # but the action server has not yet confirmed that the goal is canceled
00619 uint8 RECALLED = 8 # The goal received a cancel request before it started executing
00620 # and was successfully cancelled (Terminal State)
00621 uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
00622 # sent over the wire by an action server
00623
00624 #Allow for the user to associate a string with GoalStatus for debugging
00625 string text
00626
00627
00628 ================================================================================
00629 MSG: pr2_object_manipulation_msgs/GetGripperPoseResult
00630 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00631 # the returned gripper pose, assuming the user has clicked "accept"
00632 # if the user clicks "cancel" the action aborts
00633 geometry_msgs/PoseStamped gripper_pose
00634 float32 gripper_opening
00635
00636 ================================================================================
00637 MSG: pr2_object_manipulation_msgs/GetGripperPoseActionFeedback
00638 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00639
00640 Header header
00641 actionlib_msgs/GoalStatus status
00642 GetGripperPoseFeedback feedback
00643
00644 ================================================================================
00645 MSG: pr2_object_manipulation_msgs/GetGripperPoseFeedback
00646 # ====== DO NOT MODIFY! AUTOGENERATED FROM AN ACTION DEFINITION ======
00647
00648 # the feedback gripper pose, which is basically raw and unfiltered but may be useful
00649 geometry_msgs/PoseStamped gripper_pose
00650 float32 gripper_opening
00651
00652
00653 """
00654 __slots__ = ['action_goal','action_result','action_feedback']
00655 _slot_types = ['pr2_object_manipulation_msgs/GetGripperPoseActionGoal','pr2_object_manipulation_msgs/GetGripperPoseActionResult','pr2_object_manipulation_msgs/GetGripperPoseActionFeedback']
00656
00657 def __init__(self, *args, **kwds):
00658 """
00659 Constructor. Any message fields that are implicitly/explicitly
00660 set to None will be assigned a default value. The recommend
00661 use is keyword arguments as this is more robust to future message
00662 changes. You cannot mix in-order arguments and keyword arguments.
00663
00664 The available fields are:
00665 action_goal,action_result,action_feedback
00666
00667 :param args: complete set of field values, in .msg order
00668 :param kwds: use keyword arguments corresponding to message field names
00669 to set specific fields.
00670 """
00671 if args or kwds:
00672 super(GetGripperPoseAction, self).__init__(*args, **kwds)
00673 #message fields cannot be None, assign default values for those that are
00674 if self.action_goal is None:
00675 self.action_goal = pr2_object_manipulation_msgs.msg.GetGripperPoseActionGoal()
00676 if self.action_result is None:
00677 self.action_result = pr2_object_manipulation_msgs.msg.GetGripperPoseActionResult()
00678 if self.action_feedback is None:
00679 self.action_feedback = pr2_object_manipulation_msgs.msg.GetGripperPoseActionFeedback()
00680 else:
00681 self.action_goal = pr2_object_manipulation_msgs.msg.GetGripperPoseActionGoal()
00682 self.action_result = pr2_object_manipulation_msgs.msg.GetGripperPoseActionResult()
00683 self.action_feedback = pr2_object_manipulation_msgs.msg.GetGripperPoseActionFeedback()
00684
00685 def _get_types(self):
00686 """
00687 internal API method
00688 """
00689 return self._slot_types
00690
00691 def serialize(self, buff):
00692 """
00693 serialize message into buffer
00694 :param buff: buffer, ``StringIO``
00695 """
00696 try:
00697 _x = self
00698 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
00699 _x = self.action_goal.header.frame_id
00700 length = len(_x)
00701 if python3 or type(_x) == unicode:
00702 _x = _x.encode('utf-8')
00703 length = len(_x)
00704 buff.write(struct.pack('<I%ss'%length, length, _x))
00705 _x = self
00706 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
00707 _x = self.action_goal.goal_id.id
00708 length = len(_x)
00709 if python3 or type(_x) == unicode:
00710 _x = _x.encode('utf-8')
00711 length = len(_x)
00712 buff.write(struct.pack('<I%ss'%length, length, _x))
00713 _x = self.action_goal.goal.arm_name
00714 length = len(_x)
00715 if python3 or type(_x) == unicode:
00716 _x = _x.encode('utf-8')
00717 length = len(_x)
00718 buff.write(struct.pack('<I%ss'%length, length, _x))
00719 _x = self
00720 buff.write(_struct_3I.pack(_x.action_goal.goal.gripper_pose.header.seq, _x.action_goal.goal.gripper_pose.header.stamp.secs, _x.action_goal.goal.gripper_pose.header.stamp.nsecs))
00721 _x = self.action_goal.goal.gripper_pose.header.frame_id
00722 length = len(_x)
00723 if python3 or type(_x) == unicode:
00724 _x = _x.encode('utf-8')
00725 length = len(_x)
00726 buff.write(struct.pack('<I%ss'%length, length, _x))
00727 _x = self
00728 buff.write(_struct_7df.pack(_x.action_goal.goal.gripper_pose.pose.position.x, _x.action_goal.goal.gripper_pose.pose.position.y, _x.action_goal.goal.gripper_pose.pose.position.z, _x.action_goal.goal.gripper_pose.pose.orientation.x, _x.action_goal.goal.gripper_pose.pose.orientation.y, _x.action_goal.goal.gripper_pose.pose.orientation.z, _x.action_goal.goal.gripper_pose.pose.orientation.w, _x.action_goal.goal.gripper_opening))
00729 _x = self.action_goal.goal.object.reference_frame_id
00730 length = len(_x)
00731 if python3 or type(_x) == unicode:
00732 _x = _x.encode('utf-8')
00733 length = len(_x)
00734 buff.write(struct.pack('<I%ss'%length, length, _x))
00735 length = len(self.action_goal.goal.object.potential_models)
00736 buff.write(_struct_I.pack(length))
00737 for val1 in self.action_goal.goal.object.potential_models:
00738 buff.write(_struct_i.pack(val1.model_id))
00739 _v1 = val1.type
00740 _x = _v1.key
00741 length = len(_x)
00742 if python3 or type(_x) == unicode:
00743 _x = _x.encode('utf-8')
00744 length = len(_x)
00745 buff.write(struct.pack('<I%ss'%length, length, _x))
00746 _x = _v1.db
00747 length = len(_x)
00748 if python3 or type(_x) == unicode:
00749 _x = _x.encode('utf-8')
00750 length = len(_x)
00751 buff.write(struct.pack('<I%ss'%length, length, _x))
00752 _v2 = val1.pose
00753 _v3 = _v2.header
00754 buff.write(_struct_I.pack(_v3.seq))
00755 _v4 = _v3.stamp
00756 _x = _v4
00757 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00758 _x = _v3.frame_id
00759 length = len(_x)
00760 if python3 or type(_x) == unicode:
00761 _x = _x.encode('utf-8')
00762 length = len(_x)
00763 buff.write(struct.pack('<I%ss'%length, length, _x))
00764 _v5 = _v2.pose
00765 _v6 = _v5.position
00766 _x = _v6
00767 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00768 _v7 = _v5.orientation
00769 _x = _v7
00770 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00771 buff.write(_struct_f.pack(val1.confidence))
00772 _x = val1.detector_name
00773 length = len(_x)
00774 if python3 or type(_x) == unicode:
00775 _x = _x.encode('utf-8')
00776 length = len(_x)
00777 buff.write(struct.pack('<I%ss'%length, length, _x))
00778 _x = self
00779 buff.write(_struct_3I.pack(_x.action_goal.goal.object.cluster.header.seq, _x.action_goal.goal.object.cluster.header.stamp.secs, _x.action_goal.goal.object.cluster.header.stamp.nsecs))
00780 _x = self.action_goal.goal.object.cluster.header.frame_id
00781 length = len(_x)
00782 if python3 or type(_x) == unicode:
00783 _x = _x.encode('utf-8')
00784 length = len(_x)
00785 buff.write(struct.pack('<I%ss'%length, length, _x))
00786 length = len(self.action_goal.goal.object.cluster.points)
00787 buff.write(_struct_I.pack(length))
00788 for val1 in self.action_goal.goal.object.cluster.points:
00789 _x = val1
00790 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
00791 length = len(self.action_goal.goal.object.cluster.channels)
00792 buff.write(_struct_I.pack(length))
00793 for val1 in self.action_goal.goal.object.cluster.channels:
00794 _x = val1.name
00795 length = len(_x)
00796 if python3 or type(_x) == unicode:
00797 _x = _x.encode('utf-8')
00798 length = len(_x)
00799 buff.write(struct.pack('<I%ss'%length, length, _x))
00800 length = len(val1.values)
00801 buff.write(_struct_I.pack(length))
00802 pattern = '<%sf'%length
00803 buff.write(struct.pack(pattern, *val1.values))
00804 _x = self
00805 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.cloud.header.seq, _x.action_goal.goal.object.region.cloud.header.stamp.secs, _x.action_goal.goal.object.region.cloud.header.stamp.nsecs))
00806 _x = self.action_goal.goal.object.region.cloud.header.frame_id
00807 length = len(_x)
00808 if python3 or type(_x) == unicode:
00809 _x = _x.encode('utf-8')
00810 length = len(_x)
00811 buff.write(struct.pack('<I%ss'%length, length, _x))
00812 _x = self
00813 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.cloud.height, _x.action_goal.goal.object.region.cloud.width))
00814 length = len(self.action_goal.goal.object.region.cloud.fields)
00815 buff.write(_struct_I.pack(length))
00816 for val1 in self.action_goal.goal.object.region.cloud.fields:
00817 _x = val1.name
00818 length = len(_x)
00819 if python3 or type(_x) == unicode:
00820 _x = _x.encode('utf-8')
00821 length = len(_x)
00822 buff.write(struct.pack('<I%ss'%length, length, _x))
00823 _x = val1
00824 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
00825 _x = self
00826 buff.write(_struct_B2I.pack(_x.action_goal.goal.object.region.cloud.is_bigendian, _x.action_goal.goal.object.region.cloud.point_step, _x.action_goal.goal.object.region.cloud.row_step))
00827 _x = self.action_goal.goal.object.region.cloud.data
00828 length = len(_x)
00829 # - if encoded as a list instead, serialize as bytes instead of string
00830 if type(_x) in [list, tuple]:
00831 buff.write(struct.pack('<I%sB'%length, length, *_x))
00832 else:
00833 buff.write(struct.pack('<I%ss'%length, length, _x))
00834 buff.write(_struct_B.pack(self.action_goal.goal.object.region.cloud.is_dense))
00835 length = len(self.action_goal.goal.object.region.mask)
00836 buff.write(_struct_I.pack(length))
00837 pattern = '<%si'%length
00838 buff.write(struct.pack(pattern, *self.action_goal.goal.object.region.mask))
00839 _x = self
00840 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.image.header.seq, _x.action_goal.goal.object.region.image.header.stamp.secs, _x.action_goal.goal.object.region.image.header.stamp.nsecs))
00841 _x = self.action_goal.goal.object.region.image.header.frame_id
00842 length = len(_x)
00843 if python3 or type(_x) == unicode:
00844 _x = _x.encode('utf-8')
00845 length = len(_x)
00846 buff.write(struct.pack('<I%ss'%length, length, _x))
00847 _x = self
00848 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.image.height, _x.action_goal.goal.object.region.image.width))
00849 _x = self.action_goal.goal.object.region.image.encoding
00850 length = len(_x)
00851 if python3 or type(_x) == unicode:
00852 _x = _x.encode('utf-8')
00853 length = len(_x)
00854 buff.write(struct.pack('<I%ss'%length, length, _x))
00855 _x = self
00856 buff.write(_struct_BI.pack(_x.action_goal.goal.object.region.image.is_bigendian, _x.action_goal.goal.object.region.image.step))
00857 _x = self.action_goal.goal.object.region.image.data
00858 length = len(_x)
00859 # - if encoded as a list instead, serialize as bytes instead of string
00860 if type(_x) in [list, tuple]:
00861 buff.write(struct.pack('<I%sB'%length, length, *_x))
00862 else:
00863 buff.write(struct.pack('<I%ss'%length, length, _x))
00864 _x = self
00865 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.disparity_image.header.seq, _x.action_goal.goal.object.region.disparity_image.header.stamp.secs, _x.action_goal.goal.object.region.disparity_image.header.stamp.nsecs))
00866 _x = self.action_goal.goal.object.region.disparity_image.header.frame_id
00867 length = len(_x)
00868 if python3 or type(_x) == unicode:
00869 _x = _x.encode('utf-8')
00870 length = len(_x)
00871 buff.write(struct.pack('<I%ss'%length, length, _x))
00872 _x = self
00873 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.disparity_image.height, _x.action_goal.goal.object.region.disparity_image.width))
00874 _x = self.action_goal.goal.object.region.disparity_image.encoding
00875 length = len(_x)
00876 if python3 or type(_x) == unicode:
00877 _x = _x.encode('utf-8')
00878 length = len(_x)
00879 buff.write(struct.pack('<I%ss'%length, length, _x))
00880 _x = self
00881 buff.write(_struct_BI.pack(_x.action_goal.goal.object.region.disparity_image.is_bigendian, _x.action_goal.goal.object.region.disparity_image.step))
00882 _x = self.action_goal.goal.object.region.disparity_image.data
00883 length = len(_x)
00884 # - if encoded as a list instead, serialize as bytes instead of string
00885 if type(_x) in [list, tuple]:
00886 buff.write(struct.pack('<I%sB'%length, length, *_x))
00887 else:
00888 buff.write(struct.pack('<I%ss'%length, length, _x))
00889 _x = self
00890 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.cam_info.header.seq, _x.action_goal.goal.object.region.cam_info.header.stamp.secs, _x.action_goal.goal.object.region.cam_info.header.stamp.nsecs))
00891 _x = self.action_goal.goal.object.region.cam_info.header.frame_id
00892 length = len(_x)
00893 if python3 or type(_x) == unicode:
00894 _x = _x.encode('utf-8')
00895 length = len(_x)
00896 buff.write(struct.pack('<I%ss'%length, length, _x))
00897 _x = self
00898 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.cam_info.height, _x.action_goal.goal.object.region.cam_info.width))
00899 _x = self.action_goal.goal.object.region.cam_info.distortion_model
00900 length = len(_x)
00901 if python3 or type(_x) == unicode:
00902 _x = _x.encode('utf-8')
00903 length = len(_x)
00904 buff.write(struct.pack('<I%ss'%length, length, _x))
00905 length = len(self.action_goal.goal.object.region.cam_info.D)
00906 buff.write(_struct_I.pack(length))
00907 pattern = '<%sd'%length
00908 buff.write(struct.pack(pattern, *self.action_goal.goal.object.region.cam_info.D))
00909 buff.write(_struct_9d.pack(*self.action_goal.goal.object.region.cam_info.K))
00910 buff.write(_struct_9d.pack(*self.action_goal.goal.object.region.cam_info.R))
00911 buff.write(_struct_12d.pack(*self.action_goal.goal.object.region.cam_info.P))
00912 _x = self
00913 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.object.region.cam_info.binning_x, _x.action_goal.goal.object.region.cam_info.binning_y, _x.action_goal.goal.object.region.cam_info.roi.x_offset, _x.action_goal.goal.object.region.cam_info.roi.y_offset, _x.action_goal.goal.object.region.cam_info.roi.height, _x.action_goal.goal.object.region.cam_info.roi.width, _x.action_goal.goal.object.region.cam_info.roi.do_rectify, _x.action_goal.goal.object.region.roi_box_pose.header.seq, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.nsecs))
00914 _x = self.action_goal.goal.object.region.roi_box_pose.header.frame_id
00915 length = len(_x)
00916 if python3 or type(_x) == unicode:
00917 _x = _x.encode('utf-8')
00918 length = len(_x)
00919 buff.write(struct.pack('<I%ss'%length, length, _x))
00920 _x = self
00921 buff.write(_struct_10d.pack(_x.action_goal.goal.object.region.roi_box_pose.pose.position.x, _x.action_goal.goal.object.region.roi_box_pose.pose.position.y, _x.action_goal.goal.object.region.roi_box_pose.pose.position.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.object.region.roi_box_dims.x, _x.action_goal.goal.object.region.roi_box_dims.y, _x.action_goal.goal.object.region.roi_box_dims.z))
00922 _x = self.action_goal.goal.object.collision_name
00923 length = len(_x)
00924 if python3 or type(_x) == unicode:
00925 _x = _x.encode('utf-8')
00926 length = len(_x)
00927 buff.write(struct.pack('<I%ss'%length, length, _x))
00928 _x = self.action_goal.goal.grasp.id
00929 length = len(_x)
00930 if python3 or type(_x) == unicode:
00931 _x = _x.encode('utf-8')
00932 length = len(_x)
00933 buff.write(struct.pack('<I%ss'%length, length, _x))
00934 _x = self
00935 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs))
00936 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id
00937 length = len(_x)
00938 if python3 or type(_x) == unicode:
00939 _x = _x.encode('utf-8')
00940 length = len(_x)
00941 buff.write(struct.pack('<I%ss'%length, length, _x))
00942 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name)
00943 buff.write(_struct_I.pack(length))
00944 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name:
00945 length = len(val1)
00946 if python3 or type(val1) == unicode:
00947 val1 = val1.encode('utf-8')
00948 length = len(val1)
00949 buff.write(struct.pack('<I%ss'%length, length, val1))
00950 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position)
00951 buff.write(_struct_I.pack(length))
00952 pattern = '<%sd'%length
00953 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.position))
00954 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity)
00955 buff.write(_struct_I.pack(length))
00956 pattern = '<%sd'%length
00957 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.velocity))
00958 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort)
00959 buff.write(_struct_I.pack(length))
00960 pattern = '<%sd'%length
00961 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.pre_grasp_posture.effort))
00962 _x = self
00963 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs))
00964 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id
00965 length = len(_x)
00966 if python3 or type(_x) == unicode:
00967 _x = _x.encode('utf-8')
00968 length = len(_x)
00969 buff.write(struct.pack('<I%ss'%length, length, _x))
00970 length = len(self.action_goal.goal.grasp.grasp_posture.name)
00971 buff.write(_struct_I.pack(length))
00972 for val1 in self.action_goal.goal.grasp.grasp_posture.name:
00973 length = len(val1)
00974 if python3 or type(val1) == unicode:
00975 val1 = val1.encode('utf-8')
00976 length = len(val1)
00977 buff.write(struct.pack('<I%ss'%length, length, val1))
00978 length = len(self.action_goal.goal.grasp.grasp_posture.position)
00979 buff.write(_struct_I.pack(length))
00980 pattern = '<%sd'%length
00981 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.position))
00982 length = len(self.action_goal.goal.grasp.grasp_posture.velocity)
00983 buff.write(_struct_I.pack(length))
00984 pattern = '<%sd'%length
00985 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.velocity))
00986 length = len(self.action_goal.goal.grasp.grasp_posture.effort)
00987 buff.write(_struct_I.pack(length))
00988 pattern = '<%sd'%length
00989 buff.write(struct.pack(pattern, *self.action_goal.goal.grasp.grasp_posture.effort))
00990 _x = self
00991 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs))
00992 _x = self.action_goal.goal.grasp.grasp_pose.header.frame_id
00993 length = len(_x)
00994 if python3 or type(_x) == unicode:
00995 _x = _x.encode('utf-8')
00996 length = len(_x)
00997 buff.write(struct.pack('<I%ss'%length, length, _x))
00998 _x = self
00999 buff.write(_struct_8d3I.pack(_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs))
01000 _x = self.action_goal.goal.grasp.approach.direction.header.frame_id
01001 length = len(_x)
01002 if python3 or type(_x) == unicode:
01003 _x = _x.encode('utf-8')
01004 length = len(_x)
01005 buff.write(struct.pack('<I%ss'%length, length, _x))
01006 _x = self
01007 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs))
01008 _x = self.action_goal.goal.grasp.retreat.direction.header.frame_id
01009 length = len(_x)
01010 if python3 or type(_x) == unicode:
01011 _x = _x.encode('utf-8')
01012 length = len(_x)
01013 buff.write(struct.pack('<I%ss'%length, length, _x))
01014 _x = self
01015 buff.write(_struct_3d3f.pack(_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force))
01016 length = len(self.action_goal.goal.grasp.allowed_touch_objects)
01017 buff.write(_struct_I.pack(length))
01018 for val1 in self.action_goal.goal.grasp.allowed_touch_objects:
01019 length = len(val1)
01020 if python3 or type(val1) == unicode:
01021 val1 = val1.encode('utf-8')
01022 length = len(val1)
01023 buff.write(struct.pack('<I%ss'%length, length, val1))
01024 _x = self
01025 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
01026 _x = self.action_result.header.frame_id
01027 length = len(_x)
01028 if python3 or type(_x) == unicode:
01029 _x = _x.encode('utf-8')
01030 length = len(_x)
01031 buff.write(struct.pack('<I%ss'%length, length, _x))
01032 _x = self
01033 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
01034 _x = self.action_result.status.goal_id.id
01035 length = len(_x)
01036 if python3 or type(_x) == unicode:
01037 _x = _x.encode('utf-8')
01038 length = len(_x)
01039 buff.write(struct.pack('<I%ss'%length, length, _x))
01040 buff.write(_struct_B.pack(self.action_result.status.status))
01041 _x = self.action_result.status.text
01042 length = len(_x)
01043 if python3 or type(_x) == unicode:
01044 _x = _x.encode('utf-8')
01045 length = len(_x)
01046 buff.write(struct.pack('<I%ss'%length, length, _x))
01047 _x = self
01048 buff.write(_struct_3I.pack(_x.action_result.result.gripper_pose.header.seq, _x.action_result.result.gripper_pose.header.stamp.secs, _x.action_result.result.gripper_pose.header.stamp.nsecs))
01049 _x = self.action_result.result.gripper_pose.header.frame_id
01050 length = len(_x)
01051 if python3 or type(_x) == unicode:
01052 _x = _x.encode('utf-8')
01053 length = len(_x)
01054 buff.write(struct.pack('<I%ss'%length, length, _x))
01055 _x = self
01056 buff.write(_struct_7df3I.pack(_x.action_result.result.gripper_pose.pose.position.x, _x.action_result.result.gripper_pose.pose.position.y, _x.action_result.result.gripper_pose.pose.position.z, _x.action_result.result.gripper_pose.pose.orientation.x, _x.action_result.result.gripper_pose.pose.orientation.y, _x.action_result.result.gripper_pose.pose.orientation.z, _x.action_result.result.gripper_pose.pose.orientation.w, _x.action_result.result.gripper_opening, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
01057 _x = self.action_feedback.header.frame_id
01058 length = len(_x)
01059 if python3 or type(_x) == unicode:
01060 _x = _x.encode('utf-8')
01061 length = len(_x)
01062 buff.write(struct.pack('<I%ss'%length, length, _x))
01063 _x = self
01064 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
01065 _x = self.action_feedback.status.goal_id.id
01066 length = len(_x)
01067 if python3 or type(_x) == unicode:
01068 _x = _x.encode('utf-8')
01069 length = len(_x)
01070 buff.write(struct.pack('<I%ss'%length, length, _x))
01071 buff.write(_struct_B.pack(self.action_feedback.status.status))
01072 _x = self.action_feedback.status.text
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_feedback.feedback.gripper_pose.header.seq, _x.action_feedback.feedback.gripper_pose.header.stamp.secs, _x.action_feedback.feedback.gripper_pose.header.stamp.nsecs))
01080 _x = self.action_feedback.feedback.gripper_pose.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 _x = self
01087 buff.write(_struct_7df.pack(_x.action_feedback.feedback.gripper_pose.pose.position.x, _x.action_feedback.feedback.gripper_pose.pose.position.y, _x.action_feedback.feedback.gripper_pose.pose.position.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.x, _x.action_feedback.feedback.gripper_pose.pose.orientation.y, _x.action_feedback.feedback.gripper_pose.pose.orientation.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.w, _x.action_feedback.feedback.gripper_opening))
01088 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
01089 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
01090
01091 def deserialize(self, str):
01092 """
01093 unpack serialized message in str into this message instance
01094 :param str: byte array of serialized message, ``str``
01095 """
01096 try:
01097 if self.action_goal is None:
01098 self.action_goal = pr2_object_manipulation_msgs.msg.GetGripperPoseActionGoal()
01099 if self.action_result is None:
01100 self.action_result = pr2_object_manipulation_msgs.msg.GetGripperPoseActionResult()
01101 if self.action_feedback is None:
01102 self.action_feedback = pr2_object_manipulation_msgs.msg.GetGripperPoseActionFeedback()
01103 end = 0
01104 _x = self
01105 start = end
01106 end += 12
01107 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01108 start = end
01109 end += 4
01110 (length,) = _struct_I.unpack(str[start:end])
01111 start = end
01112 end += length
01113 if python3:
01114 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
01115 else:
01116 self.action_goal.header.frame_id = str[start:end]
01117 _x = self
01118 start = end
01119 end += 8
01120 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01121 start = end
01122 end += 4
01123 (length,) = _struct_I.unpack(str[start:end])
01124 start = end
01125 end += length
01126 if python3:
01127 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
01128 else:
01129 self.action_goal.goal_id.id = str[start:end]
01130 start = end
01131 end += 4
01132 (length,) = _struct_I.unpack(str[start:end])
01133 start = end
01134 end += length
01135 if python3:
01136 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
01137 else:
01138 self.action_goal.goal.arm_name = str[start:end]
01139 _x = self
01140 start = end
01141 end += 12
01142 (_x.action_goal.goal.gripper_pose.header.seq, _x.action_goal.goal.gripper_pose.header.stamp.secs, _x.action_goal.goal.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01143 start = end
01144 end += 4
01145 (length,) = _struct_I.unpack(str[start:end])
01146 start = end
01147 end += length
01148 if python3:
01149 self.action_goal.goal.gripper_pose.header.frame_id = str[start:end].decode('utf-8')
01150 else:
01151 self.action_goal.goal.gripper_pose.header.frame_id = str[start:end]
01152 _x = self
01153 start = end
01154 end += 60
01155 (_x.action_goal.goal.gripper_pose.pose.position.x, _x.action_goal.goal.gripper_pose.pose.position.y, _x.action_goal.goal.gripper_pose.pose.position.z, _x.action_goal.goal.gripper_pose.pose.orientation.x, _x.action_goal.goal.gripper_pose.pose.orientation.y, _x.action_goal.goal.gripper_pose.pose.orientation.z, _x.action_goal.goal.gripper_pose.pose.orientation.w, _x.action_goal.goal.gripper_opening,) = _struct_7df.unpack(str[start:end])
01156 start = end
01157 end += 4
01158 (length,) = _struct_I.unpack(str[start:end])
01159 start = end
01160 end += length
01161 if python3:
01162 self.action_goal.goal.object.reference_frame_id = str[start:end].decode('utf-8')
01163 else:
01164 self.action_goal.goal.object.reference_frame_id = str[start:end]
01165 start = end
01166 end += 4
01167 (length,) = _struct_I.unpack(str[start:end])
01168 self.action_goal.goal.object.potential_models = []
01169 for i in range(0, length):
01170 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
01171 start = end
01172 end += 4
01173 (val1.model_id,) = _struct_i.unpack(str[start:end])
01174 _v8 = val1.type
01175 start = end
01176 end += 4
01177 (length,) = _struct_I.unpack(str[start:end])
01178 start = end
01179 end += length
01180 if python3:
01181 _v8.key = str[start:end].decode('utf-8')
01182 else:
01183 _v8.key = str[start:end]
01184 start = end
01185 end += 4
01186 (length,) = _struct_I.unpack(str[start:end])
01187 start = end
01188 end += length
01189 if python3:
01190 _v8.db = str[start:end].decode('utf-8')
01191 else:
01192 _v8.db = str[start:end]
01193 _v9 = val1.pose
01194 _v10 = _v9.header
01195 start = end
01196 end += 4
01197 (_v10.seq,) = _struct_I.unpack(str[start:end])
01198 _v11 = _v10.stamp
01199 _x = _v11
01200 start = end
01201 end += 8
01202 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
01203 start = end
01204 end += 4
01205 (length,) = _struct_I.unpack(str[start:end])
01206 start = end
01207 end += length
01208 if python3:
01209 _v10.frame_id = str[start:end].decode('utf-8')
01210 else:
01211 _v10.frame_id = str[start:end]
01212 _v12 = _v9.pose
01213 _v13 = _v12.position
01214 _x = _v13
01215 start = end
01216 end += 24
01217 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
01218 _v14 = _v12.orientation
01219 _x = _v14
01220 start = end
01221 end += 32
01222 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
01223 start = end
01224 end += 4
01225 (val1.confidence,) = _struct_f.unpack(str[start:end])
01226 start = end
01227 end += 4
01228 (length,) = _struct_I.unpack(str[start:end])
01229 start = end
01230 end += length
01231 if python3:
01232 val1.detector_name = str[start:end].decode('utf-8')
01233 else:
01234 val1.detector_name = str[start:end]
01235 self.action_goal.goal.object.potential_models.append(val1)
01236 _x = self
01237 start = end
01238 end += 12
01239 (_x.action_goal.goal.object.cluster.header.seq, _x.action_goal.goal.object.cluster.header.stamp.secs, _x.action_goal.goal.object.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01240 start = end
01241 end += 4
01242 (length,) = _struct_I.unpack(str[start:end])
01243 start = end
01244 end += length
01245 if python3:
01246 self.action_goal.goal.object.cluster.header.frame_id = str[start:end].decode('utf-8')
01247 else:
01248 self.action_goal.goal.object.cluster.header.frame_id = str[start:end]
01249 start = end
01250 end += 4
01251 (length,) = _struct_I.unpack(str[start:end])
01252 self.action_goal.goal.object.cluster.points = []
01253 for i in range(0, length):
01254 val1 = geometry_msgs.msg.Point32()
01255 _x = val1
01256 start = end
01257 end += 12
01258 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
01259 self.action_goal.goal.object.cluster.points.append(val1)
01260 start = end
01261 end += 4
01262 (length,) = _struct_I.unpack(str[start:end])
01263 self.action_goal.goal.object.cluster.channels = []
01264 for i in range(0, length):
01265 val1 = sensor_msgs.msg.ChannelFloat32()
01266 start = end
01267 end += 4
01268 (length,) = _struct_I.unpack(str[start:end])
01269 start = end
01270 end += length
01271 if python3:
01272 val1.name = str[start:end].decode('utf-8')
01273 else:
01274 val1.name = str[start:end]
01275 start = end
01276 end += 4
01277 (length,) = _struct_I.unpack(str[start:end])
01278 pattern = '<%sf'%length
01279 start = end
01280 end += struct.calcsize(pattern)
01281 val1.values = struct.unpack(pattern, str[start:end])
01282 self.action_goal.goal.object.cluster.channels.append(val1)
01283 _x = self
01284 start = end
01285 end += 12
01286 (_x.action_goal.goal.object.region.cloud.header.seq, _x.action_goal.goal.object.region.cloud.header.stamp.secs, _x.action_goal.goal.object.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01287 start = end
01288 end += 4
01289 (length,) = _struct_I.unpack(str[start:end])
01290 start = end
01291 end += length
01292 if python3:
01293 self.action_goal.goal.object.region.cloud.header.frame_id = str[start:end].decode('utf-8')
01294 else:
01295 self.action_goal.goal.object.region.cloud.header.frame_id = str[start:end]
01296 _x = self
01297 start = end
01298 end += 8
01299 (_x.action_goal.goal.object.region.cloud.height, _x.action_goal.goal.object.region.cloud.width,) = _struct_2I.unpack(str[start:end])
01300 start = end
01301 end += 4
01302 (length,) = _struct_I.unpack(str[start:end])
01303 self.action_goal.goal.object.region.cloud.fields = []
01304 for i in range(0, length):
01305 val1 = sensor_msgs.msg.PointField()
01306 start = end
01307 end += 4
01308 (length,) = _struct_I.unpack(str[start:end])
01309 start = end
01310 end += length
01311 if python3:
01312 val1.name = str[start:end].decode('utf-8')
01313 else:
01314 val1.name = str[start:end]
01315 _x = val1
01316 start = end
01317 end += 9
01318 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
01319 self.action_goal.goal.object.region.cloud.fields.append(val1)
01320 _x = self
01321 start = end
01322 end += 9
01323 (_x.action_goal.goal.object.region.cloud.is_bigendian, _x.action_goal.goal.object.region.cloud.point_step, _x.action_goal.goal.object.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
01324 self.action_goal.goal.object.region.cloud.is_bigendian = bool(self.action_goal.goal.object.region.cloud.is_bigendian)
01325 start = end
01326 end += 4
01327 (length,) = _struct_I.unpack(str[start:end])
01328 start = end
01329 end += length
01330 self.action_goal.goal.object.region.cloud.data = str[start:end]
01331 start = end
01332 end += 1
01333 (self.action_goal.goal.object.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
01334 self.action_goal.goal.object.region.cloud.is_dense = bool(self.action_goal.goal.object.region.cloud.is_dense)
01335 start = end
01336 end += 4
01337 (length,) = _struct_I.unpack(str[start:end])
01338 pattern = '<%si'%length
01339 start = end
01340 end += struct.calcsize(pattern)
01341 self.action_goal.goal.object.region.mask = struct.unpack(pattern, str[start:end])
01342 _x = self
01343 start = end
01344 end += 12
01345 (_x.action_goal.goal.object.region.image.header.seq, _x.action_goal.goal.object.region.image.header.stamp.secs, _x.action_goal.goal.object.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01346 start = end
01347 end += 4
01348 (length,) = _struct_I.unpack(str[start:end])
01349 start = end
01350 end += length
01351 if python3:
01352 self.action_goal.goal.object.region.image.header.frame_id = str[start:end].decode('utf-8')
01353 else:
01354 self.action_goal.goal.object.region.image.header.frame_id = str[start:end]
01355 _x = self
01356 start = end
01357 end += 8
01358 (_x.action_goal.goal.object.region.image.height, _x.action_goal.goal.object.region.image.width,) = _struct_2I.unpack(str[start:end])
01359 start = end
01360 end += 4
01361 (length,) = _struct_I.unpack(str[start:end])
01362 start = end
01363 end += length
01364 if python3:
01365 self.action_goal.goal.object.region.image.encoding = str[start:end].decode('utf-8')
01366 else:
01367 self.action_goal.goal.object.region.image.encoding = str[start:end]
01368 _x = self
01369 start = end
01370 end += 5
01371 (_x.action_goal.goal.object.region.image.is_bigendian, _x.action_goal.goal.object.region.image.step,) = _struct_BI.unpack(str[start:end])
01372 start = end
01373 end += 4
01374 (length,) = _struct_I.unpack(str[start:end])
01375 start = end
01376 end += length
01377 self.action_goal.goal.object.region.image.data = str[start:end]
01378 _x = self
01379 start = end
01380 end += 12
01381 (_x.action_goal.goal.object.region.disparity_image.header.seq, _x.action_goal.goal.object.region.disparity_image.header.stamp.secs, _x.action_goal.goal.object.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01382 start = end
01383 end += 4
01384 (length,) = _struct_I.unpack(str[start:end])
01385 start = end
01386 end += length
01387 if python3:
01388 self.action_goal.goal.object.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
01389 else:
01390 self.action_goal.goal.object.region.disparity_image.header.frame_id = str[start:end]
01391 _x = self
01392 start = end
01393 end += 8
01394 (_x.action_goal.goal.object.region.disparity_image.height, _x.action_goal.goal.object.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
01395 start = end
01396 end += 4
01397 (length,) = _struct_I.unpack(str[start:end])
01398 start = end
01399 end += length
01400 if python3:
01401 self.action_goal.goal.object.region.disparity_image.encoding = str[start:end].decode('utf-8')
01402 else:
01403 self.action_goal.goal.object.region.disparity_image.encoding = str[start:end]
01404 _x = self
01405 start = end
01406 end += 5
01407 (_x.action_goal.goal.object.region.disparity_image.is_bigendian, _x.action_goal.goal.object.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
01408 start = end
01409 end += 4
01410 (length,) = _struct_I.unpack(str[start:end])
01411 start = end
01412 end += length
01413 self.action_goal.goal.object.region.disparity_image.data = str[start:end]
01414 _x = self
01415 start = end
01416 end += 12
01417 (_x.action_goal.goal.object.region.cam_info.header.seq, _x.action_goal.goal.object.region.cam_info.header.stamp.secs, _x.action_goal.goal.object.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01418 start = end
01419 end += 4
01420 (length,) = _struct_I.unpack(str[start:end])
01421 start = end
01422 end += length
01423 if python3:
01424 self.action_goal.goal.object.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
01425 else:
01426 self.action_goal.goal.object.region.cam_info.header.frame_id = str[start:end]
01427 _x = self
01428 start = end
01429 end += 8
01430 (_x.action_goal.goal.object.region.cam_info.height, _x.action_goal.goal.object.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
01431 start = end
01432 end += 4
01433 (length,) = _struct_I.unpack(str[start:end])
01434 start = end
01435 end += length
01436 if python3:
01437 self.action_goal.goal.object.region.cam_info.distortion_model = str[start:end].decode('utf-8')
01438 else:
01439 self.action_goal.goal.object.region.cam_info.distortion_model = str[start:end]
01440 start = end
01441 end += 4
01442 (length,) = _struct_I.unpack(str[start:end])
01443 pattern = '<%sd'%length
01444 start = end
01445 end += struct.calcsize(pattern)
01446 self.action_goal.goal.object.region.cam_info.D = struct.unpack(pattern, str[start:end])
01447 start = end
01448 end += 72
01449 self.action_goal.goal.object.region.cam_info.K = _struct_9d.unpack(str[start:end])
01450 start = end
01451 end += 72
01452 self.action_goal.goal.object.region.cam_info.R = _struct_9d.unpack(str[start:end])
01453 start = end
01454 end += 96
01455 self.action_goal.goal.object.region.cam_info.P = _struct_12d.unpack(str[start:end])
01456 _x = self
01457 start = end
01458 end += 37
01459 (_x.action_goal.goal.object.region.cam_info.binning_x, _x.action_goal.goal.object.region.cam_info.binning_y, _x.action_goal.goal.object.region.cam_info.roi.x_offset, _x.action_goal.goal.object.region.cam_info.roi.y_offset, _x.action_goal.goal.object.region.cam_info.roi.height, _x.action_goal.goal.object.region.cam_info.roi.width, _x.action_goal.goal.object.region.cam_info.roi.do_rectify, _x.action_goal.goal.object.region.roi_box_pose.header.seq, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
01460 self.action_goal.goal.object.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.object.region.cam_info.roi.do_rectify)
01461 start = end
01462 end += 4
01463 (length,) = _struct_I.unpack(str[start:end])
01464 start = end
01465 end += length
01466 if python3:
01467 self.action_goal.goal.object.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
01468 else:
01469 self.action_goal.goal.object.region.roi_box_pose.header.frame_id = str[start:end]
01470 _x = self
01471 start = end
01472 end += 80
01473 (_x.action_goal.goal.object.region.roi_box_pose.pose.position.x, _x.action_goal.goal.object.region.roi_box_pose.pose.position.y, _x.action_goal.goal.object.region.roi_box_pose.pose.position.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.object.region.roi_box_dims.x, _x.action_goal.goal.object.region.roi_box_dims.y, _x.action_goal.goal.object.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
01474 start = end
01475 end += 4
01476 (length,) = _struct_I.unpack(str[start:end])
01477 start = end
01478 end += length
01479 if python3:
01480 self.action_goal.goal.object.collision_name = str[start:end].decode('utf-8')
01481 else:
01482 self.action_goal.goal.object.collision_name = str[start:end]
01483 start = end
01484 end += 4
01485 (length,) = _struct_I.unpack(str[start:end])
01486 start = end
01487 end += length
01488 if python3:
01489 self.action_goal.goal.grasp.id = str[start:end].decode('utf-8')
01490 else:
01491 self.action_goal.goal.grasp.id = str[start:end]
01492 _x = self
01493 start = end
01494 end += 12
01495 (_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01496 start = end
01497 end += 4
01498 (length,) = _struct_I.unpack(str[start:end])
01499 start = end
01500 end += length
01501 if python3:
01502 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01503 else:
01504 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end]
01505 start = end
01506 end += 4
01507 (length,) = _struct_I.unpack(str[start:end])
01508 self.action_goal.goal.grasp.pre_grasp_posture.name = []
01509 for i in range(0, length):
01510 start = end
01511 end += 4
01512 (length,) = _struct_I.unpack(str[start:end])
01513 start = end
01514 end += length
01515 if python3:
01516 val1 = str[start:end].decode('utf-8')
01517 else:
01518 val1 = str[start:end]
01519 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1)
01520 start = end
01521 end += 4
01522 (length,) = _struct_I.unpack(str[start:end])
01523 pattern = '<%sd'%length
01524 start = end
01525 end += struct.calcsize(pattern)
01526 self.action_goal.goal.grasp.pre_grasp_posture.position = struct.unpack(pattern, str[start:end])
01527 start = end
01528 end += 4
01529 (length,) = _struct_I.unpack(str[start:end])
01530 pattern = '<%sd'%length
01531 start = end
01532 end += struct.calcsize(pattern)
01533 self.action_goal.goal.grasp.pre_grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01534 start = end
01535 end += 4
01536 (length,) = _struct_I.unpack(str[start:end])
01537 pattern = '<%sd'%length
01538 start = end
01539 end += struct.calcsize(pattern)
01540 self.action_goal.goal.grasp.pre_grasp_posture.effort = struct.unpack(pattern, str[start:end])
01541 _x = self
01542 start = end
01543 end += 12
01544 (_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01545 start = end
01546 end += 4
01547 (length,) = _struct_I.unpack(str[start:end])
01548 start = end
01549 end += length
01550 if python3:
01551 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
01552 else:
01553 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end]
01554 start = end
01555 end += 4
01556 (length,) = _struct_I.unpack(str[start:end])
01557 self.action_goal.goal.grasp.grasp_posture.name = []
01558 for i in range(0, length):
01559 start = end
01560 end += 4
01561 (length,) = _struct_I.unpack(str[start:end])
01562 start = end
01563 end += length
01564 if python3:
01565 val1 = str[start:end].decode('utf-8')
01566 else:
01567 val1 = str[start:end]
01568 self.action_goal.goal.grasp.grasp_posture.name.append(val1)
01569 start = end
01570 end += 4
01571 (length,) = _struct_I.unpack(str[start:end])
01572 pattern = '<%sd'%length
01573 start = end
01574 end += struct.calcsize(pattern)
01575 self.action_goal.goal.grasp.grasp_posture.position = struct.unpack(pattern, str[start:end])
01576 start = end
01577 end += 4
01578 (length,) = _struct_I.unpack(str[start:end])
01579 pattern = '<%sd'%length
01580 start = end
01581 end += struct.calcsize(pattern)
01582 self.action_goal.goal.grasp.grasp_posture.velocity = struct.unpack(pattern, str[start:end])
01583 start = end
01584 end += 4
01585 (length,) = _struct_I.unpack(str[start:end])
01586 pattern = '<%sd'%length
01587 start = end
01588 end += struct.calcsize(pattern)
01589 self.action_goal.goal.grasp.grasp_posture.effort = struct.unpack(pattern, str[start:end])
01590 _x = self
01591 start = end
01592 end += 12
01593 (_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01594 start = end
01595 end += 4
01596 (length,) = _struct_I.unpack(str[start:end])
01597 start = end
01598 end += length
01599 if python3:
01600 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
01601 else:
01602 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end]
01603 _x = self
01604 start = end
01605 end += 76
01606 (_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
01607 start = end
01608 end += 4
01609 (length,) = _struct_I.unpack(str[start:end])
01610 start = end
01611 end += length
01612 if python3:
01613 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
01614 else:
01615 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end]
01616 _x = self
01617 start = end
01618 end += 44
01619 (_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
01620 start = end
01621 end += 4
01622 (length,) = _struct_I.unpack(str[start:end])
01623 start = end
01624 end += length
01625 if python3:
01626 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
01627 else:
01628 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end]
01629 _x = self
01630 start = end
01631 end += 36
01632 (_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
01633 start = end
01634 end += 4
01635 (length,) = _struct_I.unpack(str[start:end])
01636 self.action_goal.goal.grasp.allowed_touch_objects = []
01637 for i in range(0, length):
01638 start = end
01639 end += 4
01640 (length,) = _struct_I.unpack(str[start:end])
01641 start = end
01642 end += length
01643 if python3:
01644 val1 = str[start:end].decode('utf-8')
01645 else:
01646 val1 = str[start:end]
01647 self.action_goal.goal.grasp.allowed_touch_objects.append(val1)
01648 _x = self
01649 start = end
01650 end += 12
01651 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01652 start = end
01653 end += 4
01654 (length,) = _struct_I.unpack(str[start:end])
01655 start = end
01656 end += length
01657 if python3:
01658 self.action_result.header.frame_id = str[start:end].decode('utf-8')
01659 else:
01660 self.action_result.header.frame_id = str[start:end]
01661 _x = self
01662 start = end
01663 end += 8
01664 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01665 start = end
01666 end += 4
01667 (length,) = _struct_I.unpack(str[start:end])
01668 start = end
01669 end += length
01670 if python3:
01671 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
01672 else:
01673 self.action_result.status.goal_id.id = str[start:end]
01674 start = end
01675 end += 1
01676 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
01677 start = end
01678 end += 4
01679 (length,) = _struct_I.unpack(str[start:end])
01680 start = end
01681 end += length
01682 if python3:
01683 self.action_result.status.text = str[start:end].decode('utf-8')
01684 else:
01685 self.action_result.status.text = str[start:end]
01686 _x = self
01687 start = end
01688 end += 12
01689 (_x.action_result.result.gripper_pose.header.seq, _x.action_result.result.gripper_pose.header.stamp.secs, _x.action_result.result.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01690 start = end
01691 end += 4
01692 (length,) = _struct_I.unpack(str[start:end])
01693 start = end
01694 end += length
01695 if python3:
01696 self.action_result.result.gripper_pose.header.frame_id = str[start:end].decode('utf-8')
01697 else:
01698 self.action_result.result.gripper_pose.header.frame_id = str[start:end]
01699 _x = self
01700 start = end
01701 end += 72
01702 (_x.action_result.result.gripper_pose.pose.position.x, _x.action_result.result.gripper_pose.pose.position.y, _x.action_result.result.gripper_pose.pose.position.z, _x.action_result.result.gripper_pose.pose.orientation.x, _x.action_result.result.gripper_pose.pose.orientation.y, _x.action_result.result.gripper_pose.pose.orientation.z, _x.action_result.result.gripper_pose.pose.orientation.w, _x.action_result.result.gripper_opening, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_7df3I.unpack(str[start:end])
01703 start = end
01704 end += 4
01705 (length,) = _struct_I.unpack(str[start:end])
01706 start = end
01707 end += length
01708 if python3:
01709 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
01710 else:
01711 self.action_feedback.header.frame_id = str[start:end]
01712 _x = self
01713 start = end
01714 end += 8
01715 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
01716 start = end
01717 end += 4
01718 (length,) = _struct_I.unpack(str[start:end])
01719 start = end
01720 end += length
01721 if python3:
01722 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
01723 else:
01724 self.action_feedback.status.goal_id.id = str[start:end]
01725 start = end
01726 end += 1
01727 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
01728 start = end
01729 end += 4
01730 (length,) = _struct_I.unpack(str[start:end])
01731 start = end
01732 end += length
01733 if python3:
01734 self.action_feedback.status.text = str[start:end].decode('utf-8')
01735 else:
01736 self.action_feedback.status.text = str[start:end]
01737 _x = self
01738 start = end
01739 end += 12
01740 (_x.action_feedback.feedback.gripper_pose.header.seq, _x.action_feedback.feedback.gripper_pose.header.stamp.secs, _x.action_feedback.feedback.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
01741 start = end
01742 end += 4
01743 (length,) = _struct_I.unpack(str[start:end])
01744 start = end
01745 end += length
01746 if python3:
01747 self.action_feedback.feedback.gripper_pose.header.frame_id = str[start:end].decode('utf-8')
01748 else:
01749 self.action_feedback.feedback.gripper_pose.header.frame_id = str[start:end]
01750 _x = self
01751 start = end
01752 end += 60
01753 (_x.action_feedback.feedback.gripper_pose.pose.position.x, _x.action_feedback.feedback.gripper_pose.pose.position.y, _x.action_feedback.feedback.gripper_pose.pose.position.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.x, _x.action_feedback.feedback.gripper_pose.pose.orientation.y, _x.action_feedback.feedback.gripper_pose.pose.orientation.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.w, _x.action_feedback.feedback.gripper_opening,) = _struct_7df.unpack(str[start:end])
01754 return self
01755 except struct.error as e:
01756 raise genpy.DeserializationError(e) #most likely buffer underfill
01757
01758
01759 def serialize_numpy(self, buff, numpy):
01760 """
01761 serialize message with numpy array types into buffer
01762 :param buff: buffer, ``StringIO``
01763 :param numpy: numpy python module
01764 """
01765 try:
01766 _x = self
01767 buff.write(_struct_3I.pack(_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs))
01768 _x = self.action_goal.header.frame_id
01769 length = len(_x)
01770 if python3 or type(_x) == unicode:
01771 _x = _x.encode('utf-8')
01772 length = len(_x)
01773 buff.write(struct.pack('<I%ss'%length, length, _x))
01774 _x = self
01775 buff.write(_struct_2I.pack(_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs))
01776 _x = self.action_goal.goal_id.id
01777 length = len(_x)
01778 if python3 or type(_x) == unicode:
01779 _x = _x.encode('utf-8')
01780 length = len(_x)
01781 buff.write(struct.pack('<I%ss'%length, length, _x))
01782 _x = self.action_goal.goal.arm_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_3I.pack(_x.action_goal.goal.gripper_pose.header.seq, _x.action_goal.goal.gripper_pose.header.stamp.secs, _x.action_goal.goal.gripper_pose.header.stamp.nsecs))
01790 _x = self.action_goal.goal.gripper_pose.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_7df.pack(_x.action_goal.goal.gripper_pose.pose.position.x, _x.action_goal.goal.gripper_pose.pose.position.y, _x.action_goal.goal.gripper_pose.pose.position.z, _x.action_goal.goal.gripper_pose.pose.orientation.x, _x.action_goal.goal.gripper_pose.pose.orientation.y, _x.action_goal.goal.gripper_pose.pose.orientation.z, _x.action_goal.goal.gripper_pose.pose.orientation.w, _x.action_goal.goal.gripper_opening))
01798 _x = self.action_goal.goal.object.reference_frame_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 length = len(self.action_goal.goal.object.potential_models)
01805 buff.write(_struct_I.pack(length))
01806 for val1 in self.action_goal.goal.object.potential_models:
01807 buff.write(_struct_i.pack(val1.model_id))
01808 _v15 = val1.type
01809 _x = _v15.key
01810 length = len(_x)
01811 if python3 or type(_x) == unicode:
01812 _x = _x.encode('utf-8')
01813 length = len(_x)
01814 buff.write(struct.pack('<I%ss'%length, length, _x))
01815 _x = _v15.db
01816 length = len(_x)
01817 if python3 or type(_x) == unicode:
01818 _x = _x.encode('utf-8')
01819 length = len(_x)
01820 buff.write(struct.pack('<I%ss'%length, length, _x))
01821 _v16 = val1.pose
01822 _v17 = _v16.header
01823 buff.write(_struct_I.pack(_v17.seq))
01824 _v18 = _v17.stamp
01825 _x = _v18
01826 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
01827 _x = _v17.frame_id
01828 length = len(_x)
01829 if python3 or type(_x) == unicode:
01830 _x = _x.encode('utf-8')
01831 length = len(_x)
01832 buff.write(struct.pack('<I%ss'%length, length, _x))
01833 _v19 = _v16.pose
01834 _v20 = _v19.position
01835 _x = _v20
01836 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
01837 _v21 = _v19.orientation
01838 _x = _v21
01839 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
01840 buff.write(_struct_f.pack(val1.confidence))
01841 _x = val1.detector_name
01842 length = len(_x)
01843 if python3 or type(_x) == unicode:
01844 _x = _x.encode('utf-8')
01845 length = len(_x)
01846 buff.write(struct.pack('<I%ss'%length, length, _x))
01847 _x = self
01848 buff.write(_struct_3I.pack(_x.action_goal.goal.object.cluster.header.seq, _x.action_goal.goal.object.cluster.header.stamp.secs, _x.action_goal.goal.object.cluster.header.stamp.nsecs))
01849 _x = self.action_goal.goal.object.cluster.header.frame_id
01850 length = len(_x)
01851 if python3 or type(_x) == unicode:
01852 _x = _x.encode('utf-8')
01853 length = len(_x)
01854 buff.write(struct.pack('<I%ss'%length, length, _x))
01855 length = len(self.action_goal.goal.object.cluster.points)
01856 buff.write(_struct_I.pack(length))
01857 for val1 in self.action_goal.goal.object.cluster.points:
01858 _x = val1
01859 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z))
01860 length = len(self.action_goal.goal.object.cluster.channels)
01861 buff.write(_struct_I.pack(length))
01862 for val1 in self.action_goal.goal.object.cluster.channels:
01863 _x = val1.name
01864 length = len(_x)
01865 if python3 or type(_x) == unicode:
01866 _x = _x.encode('utf-8')
01867 length = len(_x)
01868 buff.write(struct.pack('<I%ss'%length, length, _x))
01869 length = len(val1.values)
01870 buff.write(_struct_I.pack(length))
01871 pattern = '<%sf'%length
01872 buff.write(val1.values.tostring())
01873 _x = self
01874 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.cloud.header.seq, _x.action_goal.goal.object.region.cloud.header.stamp.secs, _x.action_goal.goal.object.region.cloud.header.stamp.nsecs))
01875 _x = self.action_goal.goal.object.region.cloud.header.frame_id
01876 length = len(_x)
01877 if python3 or type(_x) == unicode:
01878 _x = _x.encode('utf-8')
01879 length = len(_x)
01880 buff.write(struct.pack('<I%ss'%length, length, _x))
01881 _x = self
01882 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.cloud.height, _x.action_goal.goal.object.region.cloud.width))
01883 length = len(self.action_goal.goal.object.region.cloud.fields)
01884 buff.write(_struct_I.pack(length))
01885 for val1 in self.action_goal.goal.object.region.cloud.fields:
01886 _x = val1.name
01887 length = len(_x)
01888 if python3 or type(_x) == unicode:
01889 _x = _x.encode('utf-8')
01890 length = len(_x)
01891 buff.write(struct.pack('<I%ss'%length, length, _x))
01892 _x = val1
01893 buff.write(_struct_IBI.pack(_x.offset, _x.datatype, _x.count))
01894 _x = self
01895 buff.write(_struct_B2I.pack(_x.action_goal.goal.object.region.cloud.is_bigendian, _x.action_goal.goal.object.region.cloud.point_step, _x.action_goal.goal.object.region.cloud.row_step))
01896 _x = self.action_goal.goal.object.region.cloud.data
01897 length = len(_x)
01898 # - if encoded as a list instead, serialize as bytes instead of string
01899 if type(_x) in [list, tuple]:
01900 buff.write(struct.pack('<I%sB'%length, length, *_x))
01901 else:
01902 buff.write(struct.pack('<I%ss'%length, length, _x))
01903 buff.write(_struct_B.pack(self.action_goal.goal.object.region.cloud.is_dense))
01904 length = len(self.action_goal.goal.object.region.mask)
01905 buff.write(_struct_I.pack(length))
01906 pattern = '<%si'%length
01907 buff.write(self.action_goal.goal.object.region.mask.tostring())
01908 _x = self
01909 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.image.header.seq, _x.action_goal.goal.object.region.image.header.stamp.secs, _x.action_goal.goal.object.region.image.header.stamp.nsecs))
01910 _x = self.action_goal.goal.object.region.image.header.frame_id
01911 length = len(_x)
01912 if python3 or type(_x) == unicode:
01913 _x = _x.encode('utf-8')
01914 length = len(_x)
01915 buff.write(struct.pack('<I%ss'%length, length, _x))
01916 _x = self
01917 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.image.height, _x.action_goal.goal.object.region.image.width))
01918 _x = self.action_goal.goal.object.region.image.encoding
01919 length = len(_x)
01920 if python3 or type(_x) == unicode:
01921 _x = _x.encode('utf-8')
01922 length = len(_x)
01923 buff.write(struct.pack('<I%ss'%length, length, _x))
01924 _x = self
01925 buff.write(_struct_BI.pack(_x.action_goal.goal.object.region.image.is_bigendian, _x.action_goal.goal.object.region.image.step))
01926 _x = self.action_goal.goal.object.region.image.data
01927 length = len(_x)
01928 # - if encoded as a list instead, serialize as bytes instead of string
01929 if type(_x) in [list, tuple]:
01930 buff.write(struct.pack('<I%sB'%length, length, *_x))
01931 else:
01932 buff.write(struct.pack('<I%ss'%length, length, _x))
01933 _x = self
01934 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.disparity_image.header.seq, _x.action_goal.goal.object.region.disparity_image.header.stamp.secs, _x.action_goal.goal.object.region.disparity_image.header.stamp.nsecs))
01935 _x = self.action_goal.goal.object.region.disparity_image.header.frame_id
01936 length = len(_x)
01937 if python3 or type(_x) == unicode:
01938 _x = _x.encode('utf-8')
01939 length = len(_x)
01940 buff.write(struct.pack('<I%ss'%length, length, _x))
01941 _x = self
01942 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.disparity_image.height, _x.action_goal.goal.object.region.disparity_image.width))
01943 _x = self.action_goal.goal.object.region.disparity_image.encoding
01944 length = len(_x)
01945 if python3 or type(_x) == unicode:
01946 _x = _x.encode('utf-8')
01947 length = len(_x)
01948 buff.write(struct.pack('<I%ss'%length, length, _x))
01949 _x = self
01950 buff.write(_struct_BI.pack(_x.action_goal.goal.object.region.disparity_image.is_bigendian, _x.action_goal.goal.object.region.disparity_image.step))
01951 _x = self.action_goal.goal.object.region.disparity_image.data
01952 length = len(_x)
01953 # - if encoded as a list instead, serialize as bytes instead of string
01954 if type(_x) in [list, tuple]:
01955 buff.write(struct.pack('<I%sB'%length, length, *_x))
01956 else:
01957 buff.write(struct.pack('<I%ss'%length, length, _x))
01958 _x = self
01959 buff.write(_struct_3I.pack(_x.action_goal.goal.object.region.cam_info.header.seq, _x.action_goal.goal.object.region.cam_info.header.stamp.secs, _x.action_goal.goal.object.region.cam_info.header.stamp.nsecs))
01960 _x = self.action_goal.goal.object.region.cam_info.header.frame_id
01961 length = len(_x)
01962 if python3 or type(_x) == unicode:
01963 _x = _x.encode('utf-8')
01964 length = len(_x)
01965 buff.write(struct.pack('<I%ss'%length, length, _x))
01966 _x = self
01967 buff.write(_struct_2I.pack(_x.action_goal.goal.object.region.cam_info.height, _x.action_goal.goal.object.region.cam_info.width))
01968 _x = self.action_goal.goal.object.region.cam_info.distortion_model
01969 length = len(_x)
01970 if python3 or type(_x) == unicode:
01971 _x = _x.encode('utf-8')
01972 length = len(_x)
01973 buff.write(struct.pack('<I%ss'%length, length, _x))
01974 length = len(self.action_goal.goal.object.region.cam_info.D)
01975 buff.write(_struct_I.pack(length))
01976 pattern = '<%sd'%length
01977 buff.write(self.action_goal.goal.object.region.cam_info.D.tostring())
01978 buff.write(self.action_goal.goal.object.region.cam_info.K.tostring())
01979 buff.write(self.action_goal.goal.object.region.cam_info.R.tostring())
01980 buff.write(self.action_goal.goal.object.region.cam_info.P.tostring())
01981 _x = self
01982 buff.write(_struct_6IB3I.pack(_x.action_goal.goal.object.region.cam_info.binning_x, _x.action_goal.goal.object.region.cam_info.binning_y, _x.action_goal.goal.object.region.cam_info.roi.x_offset, _x.action_goal.goal.object.region.cam_info.roi.y_offset, _x.action_goal.goal.object.region.cam_info.roi.height, _x.action_goal.goal.object.region.cam_info.roi.width, _x.action_goal.goal.object.region.cam_info.roi.do_rectify, _x.action_goal.goal.object.region.roi_box_pose.header.seq, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.nsecs))
01983 _x = self.action_goal.goal.object.region.roi_box_pose.header.frame_id
01984 length = len(_x)
01985 if python3 or type(_x) == unicode:
01986 _x = _x.encode('utf-8')
01987 length = len(_x)
01988 buff.write(struct.pack('<I%ss'%length, length, _x))
01989 _x = self
01990 buff.write(_struct_10d.pack(_x.action_goal.goal.object.region.roi_box_pose.pose.position.x, _x.action_goal.goal.object.region.roi_box_pose.pose.position.y, _x.action_goal.goal.object.region.roi_box_pose.pose.position.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.object.region.roi_box_dims.x, _x.action_goal.goal.object.region.roi_box_dims.y, _x.action_goal.goal.object.region.roi_box_dims.z))
01991 _x = self.action_goal.goal.object.collision_name
01992 length = len(_x)
01993 if python3 or type(_x) == unicode:
01994 _x = _x.encode('utf-8')
01995 length = len(_x)
01996 buff.write(struct.pack('<I%ss'%length, length, _x))
01997 _x = self.action_goal.goal.grasp.id
01998 length = len(_x)
01999 if python3 or type(_x) == unicode:
02000 _x = _x.encode('utf-8')
02001 length = len(_x)
02002 buff.write(struct.pack('<I%ss'%length, length, _x))
02003 _x = self
02004 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs))
02005 _x = self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id
02006 length = len(_x)
02007 if python3 or type(_x) == unicode:
02008 _x = _x.encode('utf-8')
02009 length = len(_x)
02010 buff.write(struct.pack('<I%ss'%length, length, _x))
02011 length = len(self.action_goal.goal.grasp.pre_grasp_posture.name)
02012 buff.write(_struct_I.pack(length))
02013 for val1 in self.action_goal.goal.grasp.pre_grasp_posture.name:
02014 length = len(val1)
02015 if python3 or type(val1) == unicode:
02016 val1 = val1.encode('utf-8')
02017 length = len(val1)
02018 buff.write(struct.pack('<I%ss'%length, length, val1))
02019 length = len(self.action_goal.goal.grasp.pre_grasp_posture.position)
02020 buff.write(_struct_I.pack(length))
02021 pattern = '<%sd'%length
02022 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.position.tostring())
02023 length = len(self.action_goal.goal.grasp.pre_grasp_posture.velocity)
02024 buff.write(_struct_I.pack(length))
02025 pattern = '<%sd'%length
02026 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.velocity.tostring())
02027 length = len(self.action_goal.goal.grasp.pre_grasp_posture.effort)
02028 buff.write(_struct_I.pack(length))
02029 pattern = '<%sd'%length
02030 buff.write(self.action_goal.goal.grasp.pre_grasp_posture.effort.tostring())
02031 _x = self
02032 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs))
02033 _x = self.action_goal.goal.grasp.grasp_posture.header.frame_id
02034 length = len(_x)
02035 if python3 or type(_x) == unicode:
02036 _x = _x.encode('utf-8')
02037 length = len(_x)
02038 buff.write(struct.pack('<I%ss'%length, length, _x))
02039 length = len(self.action_goal.goal.grasp.grasp_posture.name)
02040 buff.write(_struct_I.pack(length))
02041 for val1 in self.action_goal.goal.grasp.grasp_posture.name:
02042 length = len(val1)
02043 if python3 or type(val1) == unicode:
02044 val1 = val1.encode('utf-8')
02045 length = len(val1)
02046 buff.write(struct.pack('<I%ss'%length, length, val1))
02047 length = len(self.action_goal.goal.grasp.grasp_posture.position)
02048 buff.write(_struct_I.pack(length))
02049 pattern = '<%sd'%length
02050 buff.write(self.action_goal.goal.grasp.grasp_posture.position.tostring())
02051 length = len(self.action_goal.goal.grasp.grasp_posture.velocity)
02052 buff.write(_struct_I.pack(length))
02053 pattern = '<%sd'%length
02054 buff.write(self.action_goal.goal.grasp.grasp_posture.velocity.tostring())
02055 length = len(self.action_goal.goal.grasp.grasp_posture.effort)
02056 buff.write(_struct_I.pack(length))
02057 pattern = '<%sd'%length
02058 buff.write(self.action_goal.goal.grasp.grasp_posture.effort.tostring())
02059 _x = self
02060 buff.write(_struct_3I.pack(_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs))
02061 _x = self.action_goal.goal.grasp.grasp_pose.header.frame_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 _x = self
02068 buff.write(_struct_8d3I.pack(_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs))
02069 _x = self.action_goal.goal.grasp.approach.direction.header.frame_id
02070 length = len(_x)
02071 if python3 or type(_x) == unicode:
02072 _x = _x.encode('utf-8')
02073 length = len(_x)
02074 buff.write(struct.pack('<I%ss'%length, length, _x))
02075 _x = self
02076 buff.write(_struct_3d2f3I.pack(_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs))
02077 _x = self.action_goal.goal.grasp.retreat.direction.header.frame_id
02078 length = len(_x)
02079 if python3 or type(_x) == unicode:
02080 _x = _x.encode('utf-8')
02081 length = len(_x)
02082 buff.write(struct.pack('<I%ss'%length, length, _x))
02083 _x = self
02084 buff.write(_struct_3d3f.pack(_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force))
02085 length = len(self.action_goal.goal.grasp.allowed_touch_objects)
02086 buff.write(_struct_I.pack(length))
02087 for val1 in self.action_goal.goal.grasp.allowed_touch_objects:
02088 length = len(val1)
02089 if python3 or type(val1) == unicode:
02090 val1 = val1.encode('utf-8')
02091 length = len(val1)
02092 buff.write(struct.pack('<I%ss'%length, length, val1))
02093 _x = self
02094 buff.write(_struct_3I.pack(_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs))
02095 _x = self.action_result.header.frame_id
02096 length = len(_x)
02097 if python3 or type(_x) == unicode:
02098 _x = _x.encode('utf-8')
02099 length = len(_x)
02100 buff.write(struct.pack('<I%ss'%length, length, _x))
02101 _x = self
02102 buff.write(_struct_2I.pack(_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs))
02103 _x = self.action_result.status.goal_id.id
02104 length = len(_x)
02105 if python3 or type(_x) == unicode:
02106 _x = _x.encode('utf-8')
02107 length = len(_x)
02108 buff.write(struct.pack('<I%ss'%length, length, _x))
02109 buff.write(_struct_B.pack(self.action_result.status.status))
02110 _x = self.action_result.status.text
02111 length = len(_x)
02112 if python3 or type(_x) == unicode:
02113 _x = _x.encode('utf-8')
02114 length = len(_x)
02115 buff.write(struct.pack('<I%ss'%length, length, _x))
02116 _x = self
02117 buff.write(_struct_3I.pack(_x.action_result.result.gripper_pose.header.seq, _x.action_result.result.gripper_pose.header.stamp.secs, _x.action_result.result.gripper_pose.header.stamp.nsecs))
02118 _x = self.action_result.result.gripper_pose.header.frame_id
02119 length = len(_x)
02120 if python3 or type(_x) == unicode:
02121 _x = _x.encode('utf-8')
02122 length = len(_x)
02123 buff.write(struct.pack('<I%ss'%length, length, _x))
02124 _x = self
02125 buff.write(_struct_7df3I.pack(_x.action_result.result.gripper_pose.pose.position.x, _x.action_result.result.gripper_pose.pose.position.y, _x.action_result.result.gripper_pose.pose.position.z, _x.action_result.result.gripper_pose.pose.orientation.x, _x.action_result.result.gripper_pose.pose.orientation.y, _x.action_result.result.gripper_pose.pose.orientation.z, _x.action_result.result.gripper_pose.pose.orientation.w, _x.action_result.result.gripper_opening, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs))
02126 _x = self.action_feedback.header.frame_id
02127 length = len(_x)
02128 if python3 or type(_x) == unicode:
02129 _x = _x.encode('utf-8')
02130 length = len(_x)
02131 buff.write(struct.pack('<I%ss'%length, length, _x))
02132 _x = self
02133 buff.write(_struct_2I.pack(_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs))
02134 _x = self.action_feedback.status.goal_id.id
02135 length = len(_x)
02136 if python3 or type(_x) == unicode:
02137 _x = _x.encode('utf-8')
02138 length = len(_x)
02139 buff.write(struct.pack('<I%ss'%length, length, _x))
02140 buff.write(_struct_B.pack(self.action_feedback.status.status))
02141 _x = self.action_feedback.status.text
02142 length = len(_x)
02143 if python3 or type(_x) == unicode:
02144 _x = _x.encode('utf-8')
02145 length = len(_x)
02146 buff.write(struct.pack('<I%ss'%length, length, _x))
02147 _x = self
02148 buff.write(_struct_3I.pack(_x.action_feedback.feedback.gripper_pose.header.seq, _x.action_feedback.feedback.gripper_pose.header.stamp.secs, _x.action_feedback.feedback.gripper_pose.header.stamp.nsecs))
02149 _x = self.action_feedback.feedback.gripper_pose.header.frame_id
02150 length = len(_x)
02151 if python3 or type(_x) == unicode:
02152 _x = _x.encode('utf-8')
02153 length = len(_x)
02154 buff.write(struct.pack('<I%ss'%length, length, _x))
02155 _x = self
02156 buff.write(_struct_7df.pack(_x.action_feedback.feedback.gripper_pose.pose.position.x, _x.action_feedback.feedback.gripper_pose.pose.position.y, _x.action_feedback.feedback.gripper_pose.pose.position.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.x, _x.action_feedback.feedback.gripper_pose.pose.orientation.y, _x.action_feedback.feedback.gripper_pose.pose.orientation.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.w, _x.action_feedback.feedback.gripper_opening))
02157 except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
02158 except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
02159
02160 def deserialize_numpy(self, str, numpy):
02161 """
02162 unpack serialized message in str into this message instance using numpy for array types
02163 :param str: byte array of serialized message, ``str``
02164 :param numpy: numpy python module
02165 """
02166 try:
02167 if self.action_goal is None:
02168 self.action_goal = pr2_object_manipulation_msgs.msg.GetGripperPoseActionGoal()
02169 if self.action_result is None:
02170 self.action_result = pr2_object_manipulation_msgs.msg.GetGripperPoseActionResult()
02171 if self.action_feedback is None:
02172 self.action_feedback = pr2_object_manipulation_msgs.msg.GetGripperPoseActionFeedback()
02173 end = 0
02174 _x = self
02175 start = end
02176 end += 12
02177 (_x.action_goal.header.seq, _x.action_goal.header.stamp.secs, _x.action_goal.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02178 start = end
02179 end += 4
02180 (length,) = _struct_I.unpack(str[start:end])
02181 start = end
02182 end += length
02183 if python3:
02184 self.action_goal.header.frame_id = str[start:end].decode('utf-8')
02185 else:
02186 self.action_goal.header.frame_id = str[start:end]
02187 _x = self
02188 start = end
02189 end += 8
02190 (_x.action_goal.goal_id.stamp.secs, _x.action_goal.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02191 start = end
02192 end += 4
02193 (length,) = _struct_I.unpack(str[start:end])
02194 start = end
02195 end += length
02196 if python3:
02197 self.action_goal.goal_id.id = str[start:end].decode('utf-8')
02198 else:
02199 self.action_goal.goal_id.id = str[start:end]
02200 start = end
02201 end += 4
02202 (length,) = _struct_I.unpack(str[start:end])
02203 start = end
02204 end += length
02205 if python3:
02206 self.action_goal.goal.arm_name = str[start:end].decode('utf-8')
02207 else:
02208 self.action_goal.goal.arm_name = str[start:end]
02209 _x = self
02210 start = end
02211 end += 12
02212 (_x.action_goal.goal.gripper_pose.header.seq, _x.action_goal.goal.gripper_pose.header.stamp.secs, _x.action_goal.goal.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02213 start = end
02214 end += 4
02215 (length,) = _struct_I.unpack(str[start:end])
02216 start = end
02217 end += length
02218 if python3:
02219 self.action_goal.goal.gripper_pose.header.frame_id = str[start:end].decode('utf-8')
02220 else:
02221 self.action_goal.goal.gripper_pose.header.frame_id = str[start:end]
02222 _x = self
02223 start = end
02224 end += 60
02225 (_x.action_goal.goal.gripper_pose.pose.position.x, _x.action_goal.goal.gripper_pose.pose.position.y, _x.action_goal.goal.gripper_pose.pose.position.z, _x.action_goal.goal.gripper_pose.pose.orientation.x, _x.action_goal.goal.gripper_pose.pose.orientation.y, _x.action_goal.goal.gripper_pose.pose.orientation.z, _x.action_goal.goal.gripper_pose.pose.orientation.w, _x.action_goal.goal.gripper_opening,) = _struct_7df.unpack(str[start:end])
02226 start = end
02227 end += 4
02228 (length,) = _struct_I.unpack(str[start:end])
02229 start = end
02230 end += length
02231 if python3:
02232 self.action_goal.goal.object.reference_frame_id = str[start:end].decode('utf-8')
02233 else:
02234 self.action_goal.goal.object.reference_frame_id = str[start:end]
02235 start = end
02236 end += 4
02237 (length,) = _struct_I.unpack(str[start:end])
02238 self.action_goal.goal.object.potential_models = []
02239 for i in range(0, length):
02240 val1 = household_objects_database_msgs.msg.DatabaseModelPose()
02241 start = end
02242 end += 4
02243 (val1.model_id,) = _struct_i.unpack(str[start:end])
02244 _v22 = val1.type
02245 start = end
02246 end += 4
02247 (length,) = _struct_I.unpack(str[start:end])
02248 start = end
02249 end += length
02250 if python3:
02251 _v22.key = str[start:end].decode('utf-8')
02252 else:
02253 _v22.key = str[start:end]
02254 start = end
02255 end += 4
02256 (length,) = _struct_I.unpack(str[start:end])
02257 start = end
02258 end += length
02259 if python3:
02260 _v22.db = str[start:end].decode('utf-8')
02261 else:
02262 _v22.db = str[start:end]
02263 _v23 = val1.pose
02264 _v24 = _v23.header
02265 start = end
02266 end += 4
02267 (_v24.seq,) = _struct_I.unpack(str[start:end])
02268 _v25 = _v24.stamp
02269 _x = _v25
02270 start = end
02271 end += 8
02272 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
02273 start = end
02274 end += 4
02275 (length,) = _struct_I.unpack(str[start:end])
02276 start = end
02277 end += length
02278 if python3:
02279 _v24.frame_id = str[start:end].decode('utf-8')
02280 else:
02281 _v24.frame_id = str[start:end]
02282 _v26 = _v23.pose
02283 _v27 = _v26.position
02284 _x = _v27
02285 start = end
02286 end += 24
02287 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
02288 _v28 = _v26.orientation
02289 _x = _v28
02290 start = end
02291 end += 32
02292 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
02293 start = end
02294 end += 4
02295 (val1.confidence,) = _struct_f.unpack(str[start:end])
02296 start = end
02297 end += 4
02298 (length,) = _struct_I.unpack(str[start:end])
02299 start = end
02300 end += length
02301 if python3:
02302 val1.detector_name = str[start:end].decode('utf-8')
02303 else:
02304 val1.detector_name = str[start:end]
02305 self.action_goal.goal.object.potential_models.append(val1)
02306 _x = self
02307 start = end
02308 end += 12
02309 (_x.action_goal.goal.object.cluster.header.seq, _x.action_goal.goal.object.cluster.header.stamp.secs, _x.action_goal.goal.object.cluster.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02310 start = end
02311 end += 4
02312 (length,) = _struct_I.unpack(str[start:end])
02313 start = end
02314 end += length
02315 if python3:
02316 self.action_goal.goal.object.cluster.header.frame_id = str[start:end].decode('utf-8')
02317 else:
02318 self.action_goal.goal.object.cluster.header.frame_id = str[start:end]
02319 start = end
02320 end += 4
02321 (length,) = _struct_I.unpack(str[start:end])
02322 self.action_goal.goal.object.cluster.points = []
02323 for i in range(0, length):
02324 val1 = geometry_msgs.msg.Point32()
02325 _x = val1
02326 start = end
02327 end += 12
02328 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end])
02329 self.action_goal.goal.object.cluster.points.append(val1)
02330 start = end
02331 end += 4
02332 (length,) = _struct_I.unpack(str[start:end])
02333 self.action_goal.goal.object.cluster.channels = []
02334 for i in range(0, length):
02335 val1 = sensor_msgs.msg.ChannelFloat32()
02336 start = end
02337 end += 4
02338 (length,) = _struct_I.unpack(str[start:end])
02339 start = end
02340 end += length
02341 if python3:
02342 val1.name = str[start:end].decode('utf-8')
02343 else:
02344 val1.name = str[start:end]
02345 start = end
02346 end += 4
02347 (length,) = _struct_I.unpack(str[start:end])
02348 pattern = '<%sf'%length
02349 start = end
02350 end += struct.calcsize(pattern)
02351 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length)
02352 self.action_goal.goal.object.cluster.channels.append(val1)
02353 _x = self
02354 start = end
02355 end += 12
02356 (_x.action_goal.goal.object.region.cloud.header.seq, _x.action_goal.goal.object.region.cloud.header.stamp.secs, _x.action_goal.goal.object.region.cloud.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02357 start = end
02358 end += 4
02359 (length,) = _struct_I.unpack(str[start:end])
02360 start = end
02361 end += length
02362 if python3:
02363 self.action_goal.goal.object.region.cloud.header.frame_id = str[start:end].decode('utf-8')
02364 else:
02365 self.action_goal.goal.object.region.cloud.header.frame_id = str[start:end]
02366 _x = self
02367 start = end
02368 end += 8
02369 (_x.action_goal.goal.object.region.cloud.height, _x.action_goal.goal.object.region.cloud.width,) = _struct_2I.unpack(str[start:end])
02370 start = end
02371 end += 4
02372 (length,) = _struct_I.unpack(str[start:end])
02373 self.action_goal.goal.object.region.cloud.fields = []
02374 for i in range(0, length):
02375 val1 = sensor_msgs.msg.PointField()
02376 start = end
02377 end += 4
02378 (length,) = _struct_I.unpack(str[start:end])
02379 start = end
02380 end += length
02381 if python3:
02382 val1.name = str[start:end].decode('utf-8')
02383 else:
02384 val1.name = str[start:end]
02385 _x = val1
02386 start = end
02387 end += 9
02388 (_x.offset, _x.datatype, _x.count,) = _struct_IBI.unpack(str[start:end])
02389 self.action_goal.goal.object.region.cloud.fields.append(val1)
02390 _x = self
02391 start = end
02392 end += 9
02393 (_x.action_goal.goal.object.region.cloud.is_bigendian, _x.action_goal.goal.object.region.cloud.point_step, _x.action_goal.goal.object.region.cloud.row_step,) = _struct_B2I.unpack(str[start:end])
02394 self.action_goal.goal.object.region.cloud.is_bigendian = bool(self.action_goal.goal.object.region.cloud.is_bigendian)
02395 start = end
02396 end += 4
02397 (length,) = _struct_I.unpack(str[start:end])
02398 start = end
02399 end += length
02400 self.action_goal.goal.object.region.cloud.data = str[start:end]
02401 start = end
02402 end += 1
02403 (self.action_goal.goal.object.region.cloud.is_dense,) = _struct_B.unpack(str[start:end])
02404 self.action_goal.goal.object.region.cloud.is_dense = bool(self.action_goal.goal.object.region.cloud.is_dense)
02405 start = end
02406 end += 4
02407 (length,) = _struct_I.unpack(str[start:end])
02408 pattern = '<%si'%length
02409 start = end
02410 end += struct.calcsize(pattern)
02411 self.action_goal.goal.object.region.mask = numpy.frombuffer(str[start:end], dtype=numpy.int32, count=length)
02412 _x = self
02413 start = end
02414 end += 12
02415 (_x.action_goal.goal.object.region.image.header.seq, _x.action_goal.goal.object.region.image.header.stamp.secs, _x.action_goal.goal.object.region.image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02416 start = end
02417 end += 4
02418 (length,) = _struct_I.unpack(str[start:end])
02419 start = end
02420 end += length
02421 if python3:
02422 self.action_goal.goal.object.region.image.header.frame_id = str[start:end].decode('utf-8')
02423 else:
02424 self.action_goal.goal.object.region.image.header.frame_id = str[start:end]
02425 _x = self
02426 start = end
02427 end += 8
02428 (_x.action_goal.goal.object.region.image.height, _x.action_goal.goal.object.region.image.width,) = _struct_2I.unpack(str[start:end])
02429 start = end
02430 end += 4
02431 (length,) = _struct_I.unpack(str[start:end])
02432 start = end
02433 end += length
02434 if python3:
02435 self.action_goal.goal.object.region.image.encoding = str[start:end].decode('utf-8')
02436 else:
02437 self.action_goal.goal.object.region.image.encoding = str[start:end]
02438 _x = self
02439 start = end
02440 end += 5
02441 (_x.action_goal.goal.object.region.image.is_bigendian, _x.action_goal.goal.object.region.image.step,) = _struct_BI.unpack(str[start:end])
02442 start = end
02443 end += 4
02444 (length,) = _struct_I.unpack(str[start:end])
02445 start = end
02446 end += length
02447 self.action_goal.goal.object.region.image.data = str[start:end]
02448 _x = self
02449 start = end
02450 end += 12
02451 (_x.action_goal.goal.object.region.disparity_image.header.seq, _x.action_goal.goal.object.region.disparity_image.header.stamp.secs, _x.action_goal.goal.object.region.disparity_image.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02452 start = end
02453 end += 4
02454 (length,) = _struct_I.unpack(str[start:end])
02455 start = end
02456 end += length
02457 if python3:
02458 self.action_goal.goal.object.region.disparity_image.header.frame_id = str[start:end].decode('utf-8')
02459 else:
02460 self.action_goal.goal.object.region.disparity_image.header.frame_id = str[start:end]
02461 _x = self
02462 start = end
02463 end += 8
02464 (_x.action_goal.goal.object.region.disparity_image.height, _x.action_goal.goal.object.region.disparity_image.width,) = _struct_2I.unpack(str[start:end])
02465 start = end
02466 end += 4
02467 (length,) = _struct_I.unpack(str[start:end])
02468 start = end
02469 end += length
02470 if python3:
02471 self.action_goal.goal.object.region.disparity_image.encoding = str[start:end].decode('utf-8')
02472 else:
02473 self.action_goal.goal.object.region.disparity_image.encoding = str[start:end]
02474 _x = self
02475 start = end
02476 end += 5
02477 (_x.action_goal.goal.object.region.disparity_image.is_bigendian, _x.action_goal.goal.object.region.disparity_image.step,) = _struct_BI.unpack(str[start:end])
02478 start = end
02479 end += 4
02480 (length,) = _struct_I.unpack(str[start:end])
02481 start = end
02482 end += length
02483 self.action_goal.goal.object.region.disparity_image.data = str[start:end]
02484 _x = self
02485 start = end
02486 end += 12
02487 (_x.action_goal.goal.object.region.cam_info.header.seq, _x.action_goal.goal.object.region.cam_info.header.stamp.secs, _x.action_goal.goal.object.region.cam_info.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02488 start = end
02489 end += 4
02490 (length,) = _struct_I.unpack(str[start:end])
02491 start = end
02492 end += length
02493 if python3:
02494 self.action_goal.goal.object.region.cam_info.header.frame_id = str[start:end].decode('utf-8')
02495 else:
02496 self.action_goal.goal.object.region.cam_info.header.frame_id = str[start:end]
02497 _x = self
02498 start = end
02499 end += 8
02500 (_x.action_goal.goal.object.region.cam_info.height, _x.action_goal.goal.object.region.cam_info.width,) = _struct_2I.unpack(str[start:end])
02501 start = end
02502 end += 4
02503 (length,) = _struct_I.unpack(str[start:end])
02504 start = end
02505 end += length
02506 if python3:
02507 self.action_goal.goal.object.region.cam_info.distortion_model = str[start:end].decode('utf-8')
02508 else:
02509 self.action_goal.goal.object.region.cam_info.distortion_model = 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 self.action_goal.goal.object.region.cam_info.D = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02517 start = end
02518 end += 72
02519 self.action_goal.goal.object.region.cam_info.K = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02520 start = end
02521 end += 72
02522 self.action_goal.goal.object.region.cam_info.R = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=9)
02523 start = end
02524 end += 96
02525 self.action_goal.goal.object.region.cam_info.P = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=12)
02526 _x = self
02527 start = end
02528 end += 37
02529 (_x.action_goal.goal.object.region.cam_info.binning_x, _x.action_goal.goal.object.region.cam_info.binning_y, _x.action_goal.goal.object.region.cam_info.roi.x_offset, _x.action_goal.goal.object.region.cam_info.roi.y_offset, _x.action_goal.goal.object.region.cam_info.roi.height, _x.action_goal.goal.object.region.cam_info.roi.width, _x.action_goal.goal.object.region.cam_info.roi.do_rectify, _x.action_goal.goal.object.region.roi_box_pose.header.seq, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.secs, _x.action_goal.goal.object.region.roi_box_pose.header.stamp.nsecs,) = _struct_6IB3I.unpack(str[start:end])
02530 self.action_goal.goal.object.region.cam_info.roi.do_rectify = bool(self.action_goal.goal.object.region.cam_info.roi.do_rectify)
02531 start = end
02532 end += 4
02533 (length,) = _struct_I.unpack(str[start:end])
02534 start = end
02535 end += length
02536 if python3:
02537 self.action_goal.goal.object.region.roi_box_pose.header.frame_id = str[start:end].decode('utf-8')
02538 else:
02539 self.action_goal.goal.object.region.roi_box_pose.header.frame_id = str[start:end]
02540 _x = self
02541 start = end
02542 end += 80
02543 (_x.action_goal.goal.object.region.roi_box_pose.pose.position.x, _x.action_goal.goal.object.region.roi_box_pose.pose.position.y, _x.action_goal.goal.object.region.roi_box_pose.pose.position.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.x, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.y, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.z, _x.action_goal.goal.object.region.roi_box_pose.pose.orientation.w, _x.action_goal.goal.object.region.roi_box_dims.x, _x.action_goal.goal.object.region.roi_box_dims.y, _x.action_goal.goal.object.region.roi_box_dims.z,) = _struct_10d.unpack(str[start:end])
02544 start = end
02545 end += 4
02546 (length,) = _struct_I.unpack(str[start:end])
02547 start = end
02548 end += length
02549 if python3:
02550 self.action_goal.goal.object.collision_name = str[start:end].decode('utf-8')
02551 else:
02552 self.action_goal.goal.object.collision_name = str[start:end]
02553 start = end
02554 end += 4
02555 (length,) = _struct_I.unpack(str[start:end])
02556 start = end
02557 end += length
02558 if python3:
02559 self.action_goal.goal.grasp.id = str[start:end].decode('utf-8')
02560 else:
02561 self.action_goal.goal.grasp.id = str[start:end]
02562 _x = self
02563 start = end
02564 end += 12
02565 (_x.action_goal.goal.grasp.pre_grasp_posture.header.seq, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.pre_grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02566 start = end
02567 end += 4
02568 (length,) = _struct_I.unpack(str[start:end])
02569 start = end
02570 end += length
02571 if python3:
02572 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02573 else:
02574 self.action_goal.goal.grasp.pre_grasp_posture.header.frame_id = str[start:end]
02575 start = end
02576 end += 4
02577 (length,) = _struct_I.unpack(str[start:end])
02578 self.action_goal.goal.grasp.pre_grasp_posture.name = []
02579 for i in range(0, length):
02580 start = end
02581 end += 4
02582 (length,) = _struct_I.unpack(str[start:end])
02583 start = end
02584 end += length
02585 if python3:
02586 val1 = str[start:end].decode('utf-8')
02587 else:
02588 val1 = str[start:end]
02589 self.action_goal.goal.grasp.pre_grasp_posture.name.append(val1)
02590 start = end
02591 end += 4
02592 (length,) = _struct_I.unpack(str[start:end])
02593 pattern = '<%sd'%length
02594 start = end
02595 end += struct.calcsize(pattern)
02596 self.action_goal.goal.grasp.pre_grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02597 start = end
02598 end += 4
02599 (length,) = _struct_I.unpack(str[start:end])
02600 pattern = '<%sd'%length
02601 start = end
02602 end += struct.calcsize(pattern)
02603 self.action_goal.goal.grasp.pre_grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02604 start = end
02605 end += 4
02606 (length,) = _struct_I.unpack(str[start:end])
02607 pattern = '<%sd'%length
02608 start = end
02609 end += struct.calcsize(pattern)
02610 self.action_goal.goal.grasp.pre_grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02611 _x = self
02612 start = end
02613 end += 12
02614 (_x.action_goal.goal.grasp.grasp_posture.header.seq, _x.action_goal.goal.grasp.grasp_posture.header.stamp.secs, _x.action_goal.goal.grasp.grasp_posture.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02615 start = end
02616 end += 4
02617 (length,) = _struct_I.unpack(str[start:end])
02618 start = end
02619 end += length
02620 if python3:
02621 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end].decode('utf-8')
02622 else:
02623 self.action_goal.goal.grasp.grasp_posture.header.frame_id = str[start:end]
02624 start = end
02625 end += 4
02626 (length,) = _struct_I.unpack(str[start:end])
02627 self.action_goal.goal.grasp.grasp_posture.name = []
02628 for i in range(0, length):
02629 start = end
02630 end += 4
02631 (length,) = _struct_I.unpack(str[start:end])
02632 start = end
02633 end += length
02634 if python3:
02635 val1 = str[start:end].decode('utf-8')
02636 else:
02637 val1 = str[start:end]
02638 self.action_goal.goal.grasp.grasp_posture.name.append(val1)
02639 start = end
02640 end += 4
02641 (length,) = _struct_I.unpack(str[start:end])
02642 pattern = '<%sd'%length
02643 start = end
02644 end += struct.calcsize(pattern)
02645 self.action_goal.goal.grasp.grasp_posture.position = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02646 start = end
02647 end += 4
02648 (length,) = _struct_I.unpack(str[start:end])
02649 pattern = '<%sd'%length
02650 start = end
02651 end += struct.calcsize(pattern)
02652 self.action_goal.goal.grasp.grasp_posture.velocity = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02653 start = end
02654 end += 4
02655 (length,) = _struct_I.unpack(str[start:end])
02656 pattern = '<%sd'%length
02657 start = end
02658 end += struct.calcsize(pattern)
02659 self.action_goal.goal.grasp.grasp_posture.effort = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=length)
02660 _x = self
02661 start = end
02662 end += 12
02663 (_x.action_goal.goal.grasp.grasp_pose.header.seq, _x.action_goal.goal.grasp.grasp_pose.header.stamp.secs, _x.action_goal.goal.grasp.grasp_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02664 start = end
02665 end += 4
02666 (length,) = _struct_I.unpack(str[start:end])
02667 start = end
02668 end += length
02669 if python3:
02670 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end].decode('utf-8')
02671 else:
02672 self.action_goal.goal.grasp.grasp_pose.header.frame_id = str[start:end]
02673 _x = self
02674 start = end
02675 end += 76
02676 (_x.action_goal.goal.grasp.grasp_pose.pose.position.x, _x.action_goal.goal.grasp.grasp_pose.pose.position.y, _x.action_goal.goal.grasp.grasp_pose.pose.position.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.x, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.y, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.z, _x.action_goal.goal.grasp.grasp_pose.pose.orientation.w, _x.action_goal.goal.grasp.grasp_quality, _x.action_goal.goal.grasp.approach.direction.header.seq, _x.action_goal.goal.grasp.approach.direction.header.stamp.secs, _x.action_goal.goal.grasp.approach.direction.header.stamp.nsecs,) = _struct_8d3I.unpack(str[start:end])
02677 start = end
02678 end += 4
02679 (length,) = _struct_I.unpack(str[start:end])
02680 start = end
02681 end += length
02682 if python3:
02683 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end].decode('utf-8')
02684 else:
02685 self.action_goal.goal.grasp.approach.direction.header.frame_id = str[start:end]
02686 _x = self
02687 start = end
02688 end += 44
02689 (_x.action_goal.goal.grasp.approach.direction.vector.x, _x.action_goal.goal.grasp.approach.direction.vector.y, _x.action_goal.goal.grasp.approach.direction.vector.z, _x.action_goal.goal.grasp.approach.desired_distance, _x.action_goal.goal.grasp.approach.min_distance, _x.action_goal.goal.grasp.retreat.direction.header.seq, _x.action_goal.goal.grasp.retreat.direction.header.stamp.secs, _x.action_goal.goal.grasp.retreat.direction.header.stamp.nsecs,) = _struct_3d2f3I.unpack(str[start:end])
02690 start = end
02691 end += 4
02692 (length,) = _struct_I.unpack(str[start:end])
02693 start = end
02694 end += length
02695 if python3:
02696 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end].decode('utf-8')
02697 else:
02698 self.action_goal.goal.grasp.retreat.direction.header.frame_id = str[start:end]
02699 _x = self
02700 start = end
02701 end += 36
02702 (_x.action_goal.goal.grasp.retreat.direction.vector.x, _x.action_goal.goal.grasp.retreat.direction.vector.y, _x.action_goal.goal.grasp.retreat.direction.vector.z, _x.action_goal.goal.grasp.retreat.desired_distance, _x.action_goal.goal.grasp.retreat.min_distance, _x.action_goal.goal.grasp.max_contact_force,) = _struct_3d3f.unpack(str[start:end])
02703 start = end
02704 end += 4
02705 (length,) = _struct_I.unpack(str[start:end])
02706 self.action_goal.goal.grasp.allowed_touch_objects = []
02707 for i in range(0, length):
02708 start = end
02709 end += 4
02710 (length,) = _struct_I.unpack(str[start:end])
02711 start = end
02712 end += length
02713 if python3:
02714 val1 = str[start:end].decode('utf-8')
02715 else:
02716 val1 = str[start:end]
02717 self.action_goal.goal.grasp.allowed_touch_objects.append(val1)
02718 _x = self
02719 start = end
02720 end += 12
02721 (_x.action_result.header.seq, _x.action_result.header.stamp.secs, _x.action_result.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02722 start = end
02723 end += 4
02724 (length,) = _struct_I.unpack(str[start:end])
02725 start = end
02726 end += length
02727 if python3:
02728 self.action_result.header.frame_id = str[start:end].decode('utf-8')
02729 else:
02730 self.action_result.header.frame_id = str[start:end]
02731 _x = self
02732 start = end
02733 end += 8
02734 (_x.action_result.status.goal_id.stamp.secs, _x.action_result.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02735 start = end
02736 end += 4
02737 (length,) = _struct_I.unpack(str[start:end])
02738 start = end
02739 end += length
02740 if python3:
02741 self.action_result.status.goal_id.id = str[start:end].decode('utf-8')
02742 else:
02743 self.action_result.status.goal_id.id = str[start:end]
02744 start = end
02745 end += 1
02746 (self.action_result.status.status,) = _struct_B.unpack(str[start:end])
02747 start = end
02748 end += 4
02749 (length,) = _struct_I.unpack(str[start:end])
02750 start = end
02751 end += length
02752 if python3:
02753 self.action_result.status.text = str[start:end].decode('utf-8')
02754 else:
02755 self.action_result.status.text = str[start:end]
02756 _x = self
02757 start = end
02758 end += 12
02759 (_x.action_result.result.gripper_pose.header.seq, _x.action_result.result.gripper_pose.header.stamp.secs, _x.action_result.result.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02760 start = end
02761 end += 4
02762 (length,) = _struct_I.unpack(str[start:end])
02763 start = end
02764 end += length
02765 if python3:
02766 self.action_result.result.gripper_pose.header.frame_id = str[start:end].decode('utf-8')
02767 else:
02768 self.action_result.result.gripper_pose.header.frame_id = str[start:end]
02769 _x = self
02770 start = end
02771 end += 72
02772 (_x.action_result.result.gripper_pose.pose.position.x, _x.action_result.result.gripper_pose.pose.position.y, _x.action_result.result.gripper_pose.pose.position.z, _x.action_result.result.gripper_pose.pose.orientation.x, _x.action_result.result.gripper_pose.pose.orientation.y, _x.action_result.result.gripper_pose.pose.orientation.z, _x.action_result.result.gripper_pose.pose.orientation.w, _x.action_result.result.gripper_opening, _x.action_feedback.header.seq, _x.action_feedback.header.stamp.secs, _x.action_feedback.header.stamp.nsecs,) = _struct_7df3I.unpack(str[start:end])
02773 start = end
02774 end += 4
02775 (length,) = _struct_I.unpack(str[start:end])
02776 start = end
02777 end += length
02778 if python3:
02779 self.action_feedback.header.frame_id = str[start:end].decode('utf-8')
02780 else:
02781 self.action_feedback.header.frame_id = str[start:end]
02782 _x = self
02783 start = end
02784 end += 8
02785 (_x.action_feedback.status.goal_id.stamp.secs, _x.action_feedback.status.goal_id.stamp.nsecs,) = _struct_2I.unpack(str[start:end])
02786 start = end
02787 end += 4
02788 (length,) = _struct_I.unpack(str[start:end])
02789 start = end
02790 end += length
02791 if python3:
02792 self.action_feedback.status.goal_id.id = str[start:end].decode('utf-8')
02793 else:
02794 self.action_feedback.status.goal_id.id = str[start:end]
02795 start = end
02796 end += 1
02797 (self.action_feedback.status.status,) = _struct_B.unpack(str[start:end])
02798 start = end
02799 end += 4
02800 (length,) = _struct_I.unpack(str[start:end])
02801 start = end
02802 end += length
02803 if python3:
02804 self.action_feedback.status.text = str[start:end].decode('utf-8')
02805 else:
02806 self.action_feedback.status.text = str[start:end]
02807 _x = self
02808 start = end
02809 end += 12
02810 (_x.action_feedback.feedback.gripper_pose.header.seq, _x.action_feedback.feedback.gripper_pose.header.stamp.secs, _x.action_feedback.feedback.gripper_pose.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
02811 start = end
02812 end += 4
02813 (length,) = _struct_I.unpack(str[start:end])
02814 start = end
02815 end += length
02816 if python3:
02817 self.action_feedback.feedback.gripper_pose.header.frame_id = str[start:end].decode('utf-8')
02818 else:
02819 self.action_feedback.feedback.gripper_pose.header.frame_id = str[start:end]
02820 _x = self
02821 start = end
02822 end += 60
02823 (_x.action_feedback.feedback.gripper_pose.pose.position.x, _x.action_feedback.feedback.gripper_pose.pose.position.y, _x.action_feedback.feedback.gripper_pose.pose.position.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.x, _x.action_feedback.feedback.gripper_pose.pose.orientation.y, _x.action_feedback.feedback.gripper_pose.pose.orientation.z, _x.action_feedback.feedback.gripper_pose.pose.orientation.w, _x.action_feedback.feedback.gripper_opening,) = _struct_7df.unpack(str[start:end])
02824 return self
02825 except struct.error as e:
02826 raise genpy.DeserializationError(e) #most likely buffer underfill
02827
02828 _struct_I = genpy.struct_I
02829 _struct_IBI = struct.Struct("<IBI")
02830 _struct_6IB3I = struct.Struct("<6IB3I")
02831 _struct_B = struct.Struct("<B")
02832 _struct_12d = struct.Struct("<12d")
02833 _struct_8d3I = struct.Struct("<8d3I")
02834 _struct_7df = struct.Struct("<7df")
02835 _struct_f = struct.Struct("<f")
02836 _struct_i = struct.Struct("<i")
02837 _struct_BI = struct.Struct("<BI")
02838 _struct_10d = struct.Struct("<10d")
02839 _struct_3f = struct.Struct("<3f")
02840 _struct_3I = struct.Struct("<3I")
02841 _struct_9d = struct.Struct("<9d")
02842 _struct_3d3f = struct.Struct("<3d3f")
02843 _struct_B2I = struct.Struct("<B2I")
02844 _struct_3d2f3I = struct.Struct("<3d2f3I")
02845 _struct_7df3I = struct.Struct("<7df3I")
02846 _struct_4d = struct.Struct("<4d")
02847 _struct_2I = struct.Struct("<2I")
02848 _struct_3d = struct.Struct("<3d")