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


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