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