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