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