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