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