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