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