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