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