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