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