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


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