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