$search
00001 """autogenerated by genmsg_py from LocalizedCloud.msg. Do not edit.""" 00002 import roslib.message 00003 import struct 00004 00005 import sensor_msgs.msg 00006 import geometry_msgs.msg 00007 import std_msgs.msg 00008 00009 class LocalizedCloud(roslib.message.Message): 00010 _md5sum = "e868d77f7e7116b7eb215e43caa0ea5d" 00011 _type = "occupancy_grid_utils/LocalizedCloud" 00012 _has_header = True #flag to mark the presence of a Header object 00013 _full_text = """# Represents a point cloud (in a sensor frame) together with the pose of the sensor in 00014 # reference frame header.frame_id 00015 # The header of the cloud is ignored 00016 Header header 00017 geometry_msgs/Pose sensor_pose 00018 sensor_msgs/PointCloud cloud 00019 ================================================================================ 00020 MSG: std_msgs/Header 00021 # Standard metadata for higher-level stamped data types. 00022 # This is generally used to communicate timestamped data 00023 # in a particular coordinate frame. 00024 # 00025 # sequence ID: consecutively increasing ID 00026 uint32 seq 00027 #Two-integer timestamp that is expressed as: 00028 # * stamp.secs: seconds (stamp_secs) since epoch 00029 # * stamp.nsecs: nanoseconds since stamp_secs 00030 # time-handling sugar is provided by the client library 00031 time stamp 00032 #Frame this data is associated with 00033 # 0: no frame 00034 # 1: global frame 00035 string frame_id 00036 00037 ================================================================================ 00038 MSG: geometry_msgs/Pose 00039 # A representation of pose in free space, composed of postion and orientation. 00040 Point position 00041 Quaternion orientation 00042 00043 ================================================================================ 00044 MSG: geometry_msgs/Point 00045 # This contains the position of a point in free space 00046 float64 x 00047 float64 y 00048 float64 z 00049 00050 ================================================================================ 00051 MSG: geometry_msgs/Quaternion 00052 # This represents an orientation in free space in quaternion form. 00053 00054 float64 x 00055 float64 y 00056 float64 z 00057 float64 w 00058 00059 ================================================================================ 00060 MSG: sensor_msgs/PointCloud 00061 # This message holds a collection of 3d points, plus optional additional 00062 # information about each point. 00063 00064 # Time of sensor data acquisition, coordinate frame ID. 00065 Header header 00066 00067 # Array of 3d points. Each Point32 should be interpreted as a 3d point 00068 # in the frame given in the header. 00069 geometry_msgs/Point32[] points 00070 00071 # Each channel should have the same number of elements as points array, 00072 # and the data in each channel should correspond 1:1 with each point. 00073 # Channel names in common practice are listed in ChannelFloat32.msg. 00074 ChannelFloat32[] channels 00075 00076 ================================================================================ 00077 MSG: geometry_msgs/Point32 00078 # This contains the position of a point in free space(with 32 bits of precision). 00079 # It is recommeded to use Point wherever possible instead of Point32. 00080 # 00081 # This recommendation is to promote interoperability. 00082 # 00083 # This message is designed to take up less space when sending 00084 # lots of points at once, as in the case of a PointCloud. 00085 00086 float32 x 00087 float32 y 00088 float32 z 00089 ================================================================================ 00090 MSG: sensor_msgs/ChannelFloat32 00091 # This message is used by the PointCloud message to hold optional data 00092 # associated with each point in the cloud. The length of the values 00093 # array should be the same as the length of the points array in the 00094 # PointCloud, and each value should be associated with the corresponding 00095 # point. 00096 00097 # Channel names in existing practice include: 00098 # "u", "v" - row and column (respectively) in the left stereo image. 00099 # This is opposite to usual conventions but remains for 00100 # historical reasons. The newer PointCloud2 message has no 00101 # such problem. 00102 # "rgb" - For point clouds produced by color stereo cameras. uint8 00103 # (R,G,B) values packed into the least significant 24 bits, 00104 # in order. 00105 # "intensity" - laser or pixel intensity. 00106 # "distance" 00107 00108 # The channel name should give semantics of the channel (e.g. 00109 # "intensity" instead of "value"). 00110 string name 00111 00112 # The values array should be 1-1 with the elements of the associated 00113 # PointCloud. 00114 float32[] values 00115 00116 """ 00117 __slots__ = ['header','sensor_pose','cloud'] 00118 _slot_types = ['Header','geometry_msgs/Pose','sensor_msgs/PointCloud'] 00119 00120 def __init__(self, *args, **kwds): 00121 """ 00122 Constructor. Any message fields that are implicitly/explicitly 00123 set to None will be assigned a default value. The recommend 00124 use is keyword arguments as this is more robust to future message 00125 changes. You cannot mix in-order arguments and keyword arguments. 00126 00127 The available fields are: 00128 header,sensor_pose,cloud 00129 00130 @param args: complete set of field values, in .msg order 00131 @param kwds: use keyword arguments corresponding to message field names 00132 to set specific fields. 00133 """ 00134 if args or kwds: 00135 super(LocalizedCloud, self).__init__(*args, **kwds) 00136 #message fields cannot be None, assign default values for those that are 00137 if self.header is None: 00138 self.header = std_msgs.msg._Header.Header() 00139 if self.sensor_pose is None: 00140 self.sensor_pose = geometry_msgs.msg.Pose() 00141 if self.cloud is None: 00142 self.cloud = sensor_msgs.msg.PointCloud() 00143 else: 00144 self.header = std_msgs.msg._Header.Header() 00145 self.sensor_pose = geometry_msgs.msg.Pose() 00146 self.cloud = sensor_msgs.msg.PointCloud() 00147 00148 def _get_types(self): 00149 """ 00150 internal API method 00151 """ 00152 return self._slot_types 00153 00154 def serialize(self, buff): 00155 """ 00156 serialize message into buffer 00157 @param buff: buffer 00158 @type buff: StringIO 00159 """ 00160 try: 00161 _x = self 00162 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00163 _x = self.header.frame_id 00164 length = len(_x) 00165 buff.write(struct.pack('<I%ss'%length, length, _x)) 00166 _x = self 00167 buff.write(_struct_7d3I.pack(_x.sensor_pose.position.x, _x.sensor_pose.position.y, _x.sensor_pose.position.z, _x.sensor_pose.orientation.x, _x.sensor_pose.orientation.y, _x.sensor_pose.orientation.z, _x.sensor_pose.orientation.w, _x.cloud.header.seq, _x.cloud.header.stamp.secs, _x.cloud.header.stamp.nsecs)) 00168 _x = self.cloud.header.frame_id 00169 length = len(_x) 00170 buff.write(struct.pack('<I%ss'%length, length, _x)) 00171 length = len(self.cloud.points) 00172 buff.write(_struct_I.pack(length)) 00173 for val1 in self.cloud.points: 00174 _x = val1 00175 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00176 length = len(self.cloud.channels) 00177 buff.write(_struct_I.pack(length)) 00178 for val1 in self.cloud.channels: 00179 _x = val1.name 00180 length = len(_x) 00181 buff.write(struct.pack('<I%ss'%length, length, _x)) 00182 length = len(val1.values) 00183 buff.write(_struct_I.pack(length)) 00184 pattern = '<%sf'%length 00185 buff.write(struct.pack(pattern, *val1.values)) 00186 except struct.error as se: self._check_types(se) 00187 except TypeError as te: self._check_types(te) 00188 00189 def deserialize(self, str): 00190 """ 00191 unpack serialized message in str into this message instance 00192 @param str: byte array of serialized message 00193 @type str: str 00194 """ 00195 try: 00196 if self.header is None: 00197 self.header = std_msgs.msg._Header.Header() 00198 if self.sensor_pose is None: 00199 self.sensor_pose = geometry_msgs.msg.Pose() 00200 if self.cloud is None: 00201 self.cloud = sensor_msgs.msg.PointCloud() 00202 end = 0 00203 _x = self 00204 start = end 00205 end += 12 00206 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00207 start = end 00208 end += 4 00209 (length,) = _struct_I.unpack(str[start:end]) 00210 start = end 00211 end += length 00212 self.header.frame_id = str[start:end] 00213 _x = self 00214 start = end 00215 end += 68 00216 (_x.sensor_pose.position.x, _x.sensor_pose.position.y, _x.sensor_pose.position.z, _x.sensor_pose.orientation.x, _x.sensor_pose.orientation.y, _x.sensor_pose.orientation.z, _x.sensor_pose.orientation.w, _x.cloud.header.seq, _x.cloud.header.stamp.secs, _x.cloud.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end]) 00217 start = end 00218 end += 4 00219 (length,) = _struct_I.unpack(str[start:end]) 00220 start = end 00221 end += length 00222 self.cloud.header.frame_id = str[start:end] 00223 start = end 00224 end += 4 00225 (length,) = _struct_I.unpack(str[start:end]) 00226 self.cloud.points = [] 00227 for i in range(0, length): 00228 val1 = geometry_msgs.msg.Point32() 00229 _x = val1 00230 start = end 00231 end += 12 00232 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00233 self.cloud.points.append(val1) 00234 start = end 00235 end += 4 00236 (length,) = _struct_I.unpack(str[start:end]) 00237 self.cloud.channels = [] 00238 for i in range(0, length): 00239 val1 = sensor_msgs.msg.ChannelFloat32() 00240 start = end 00241 end += 4 00242 (length,) = _struct_I.unpack(str[start:end]) 00243 start = end 00244 end += length 00245 val1.name = str[start:end] 00246 start = end 00247 end += 4 00248 (length,) = _struct_I.unpack(str[start:end]) 00249 pattern = '<%sf'%length 00250 start = end 00251 end += struct.calcsize(pattern) 00252 val1.values = struct.unpack(pattern, str[start:end]) 00253 self.cloud.channels.append(val1) 00254 return self 00255 except struct.error as e: 00256 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00257 00258 00259 def serialize_numpy(self, buff, numpy): 00260 """ 00261 serialize message with numpy array types into buffer 00262 @param buff: buffer 00263 @type buff: StringIO 00264 @param numpy: numpy python module 00265 @type numpy module 00266 """ 00267 try: 00268 _x = self 00269 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs)) 00270 _x = self.header.frame_id 00271 length = len(_x) 00272 buff.write(struct.pack('<I%ss'%length, length, _x)) 00273 _x = self 00274 buff.write(_struct_7d3I.pack(_x.sensor_pose.position.x, _x.sensor_pose.position.y, _x.sensor_pose.position.z, _x.sensor_pose.orientation.x, _x.sensor_pose.orientation.y, _x.sensor_pose.orientation.z, _x.sensor_pose.orientation.w, _x.cloud.header.seq, _x.cloud.header.stamp.secs, _x.cloud.header.stamp.nsecs)) 00275 _x = self.cloud.header.frame_id 00276 length = len(_x) 00277 buff.write(struct.pack('<I%ss'%length, length, _x)) 00278 length = len(self.cloud.points) 00279 buff.write(_struct_I.pack(length)) 00280 for val1 in self.cloud.points: 00281 _x = val1 00282 buff.write(_struct_3f.pack(_x.x, _x.y, _x.z)) 00283 length = len(self.cloud.channels) 00284 buff.write(_struct_I.pack(length)) 00285 for val1 in self.cloud.channels: 00286 _x = val1.name 00287 length = len(_x) 00288 buff.write(struct.pack('<I%ss'%length, length, _x)) 00289 length = len(val1.values) 00290 buff.write(_struct_I.pack(length)) 00291 pattern = '<%sf'%length 00292 buff.write(val1.values.tostring()) 00293 except struct.error as se: self._check_types(se) 00294 except TypeError as te: self._check_types(te) 00295 00296 def deserialize_numpy(self, str, numpy): 00297 """ 00298 unpack serialized message in str into this message instance using numpy for array types 00299 @param str: byte array of serialized message 00300 @type str: str 00301 @param numpy: numpy python module 00302 @type numpy: module 00303 """ 00304 try: 00305 if self.header is None: 00306 self.header = std_msgs.msg._Header.Header() 00307 if self.sensor_pose is None: 00308 self.sensor_pose = geometry_msgs.msg.Pose() 00309 if self.cloud is None: 00310 self.cloud = sensor_msgs.msg.PointCloud() 00311 end = 0 00312 _x = self 00313 start = end 00314 end += 12 00315 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end]) 00316 start = end 00317 end += 4 00318 (length,) = _struct_I.unpack(str[start:end]) 00319 start = end 00320 end += length 00321 self.header.frame_id = str[start:end] 00322 _x = self 00323 start = end 00324 end += 68 00325 (_x.sensor_pose.position.x, _x.sensor_pose.position.y, _x.sensor_pose.position.z, _x.sensor_pose.orientation.x, _x.sensor_pose.orientation.y, _x.sensor_pose.orientation.z, _x.sensor_pose.orientation.w, _x.cloud.header.seq, _x.cloud.header.stamp.secs, _x.cloud.header.stamp.nsecs,) = _struct_7d3I.unpack(str[start:end]) 00326 start = end 00327 end += 4 00328 (length,) = _struct_I.unpack(str[start:end]) 00329 start = end 00330 end += length 00331 self.cloud.header.frame_id = str[start:end] 00332 start = end 00333 end += 4 00334 (length,) = _struct_I.unpack(str[start:end]) 00335 self.cloud.points = [] 00336 for i in range(0, length): 00337 val1 = geometry_msgs.msg.Point32() 00338 _x = val1 00339 start = end 00340 end += 12 00341 (_x.x, _x.y, _x.z,) = _struct_3f.unpack(str[start:end]) 00342 self.cloud.points.append(val1) 00343 start = end 00344 end += 4 00345 (length,) = _struct_I.unpack(str[start:end]) 00346 self.cloud.channels = [] 00347 for i in range(0, length): 00348 val1 = sensor_msgs.msg.ChannelFloat32() 00349 start = end 00350 end += 4 00351 (length,) = _struct_I.unpack(str[start:end]) 00352 start = end 00353 end += length 00354 val1.name = str[start:end] 00355 start = end 00356 end += 4 00357 (length,) = _struct_I.unpack(str[start:end]) 00358 pattern = '<%sf'%length 00359 start = end 00360 end += struct.calcsize(pattern) 00361 val1.values = numpy.frombuffer(str[start:end], dtype=numpy.float32, count=length) 00362 self.cloud.channels.append(val1) 00363 return self 00364 except struct.error as e: 00365 raise roslib.message.DeserializationError(e) #most likely buffer underfill 00366 00367 _struct_I = roslib.message.struct_I 00368 _struct_3I = struct.Struct("<3I") 00369 _struct_7d3I = struct.Struct("<7d3I") 00370 _struct_3f = struct.Struct("<3f")