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