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