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