00001 """autogenerated by genmsg_py from DynamicObstacles.msg. Do not edit."""
00002 import roslib.message
00003 import struct
00004
00005 import dynamic_obs_msgs.msg
00006 import geometry_msgs.msg
00007 import std_msgs.msg
00008
00009 class DynamicObstacles(roslib.message.Message):
00010 _md5sum = "92b063c75c79b3baa03490e44c9c700b"
00011 _type = "dynamic_obs_msgs/DynamicObstacles"
00012 _has_header = True
00013 _full_text = """#an array of dynamic obstacles
00014 Header header
00015 DynamicObstacle[] dyn_obs
00016
00017 ================================================================================
00018 MSG: std_msgs/Header
00019 # Standard metadata for higher-level stamped data types.
00020 # This is generally used to communicate timestamped data
00021 # in a particular coordinate frame.
00022 #
00023 # sequence ID: consecutively increasing ID
00024 uint32 seq
00025 #Two-integer timestamp that is expressed as:
00026 # * stamp.secs: seconds (stamp_secs) since epoch
00027 # * stamp.nsecs: nanoseconds since stamp_secs
00028 # time-handling sugar is provided by the client library
00029 time stamp
00030 #Frame this data is associated with
00031 # 0: no frame
00032 # 1: global frame
00033 string frame_id
00034
00035 ================================================================================
00036 MSG: dynamic_obs_msgs/DynamicObstacle
00037 #the radius of the dynamic obstacle
00038 float64 radius
00039
00040 #a list of possible trajectories it may follow (with probabilities summing to 1)
00041 DynObsTrajectory[] trajectories
00042
00043 ================================================================================
00044 MSG: dynamic_obs_msgs/DynObsTrajectory
00045 #a dynamic obstacle trajectory
00046
00047 #the probability of the obstacle following this trajectory
00048 float64 probability
00049
00050 #whether this obstacle should be treated like a static obstacle at the end of its trajectory (true)
00051 #or the obstacle is ignored after the end of its trajectory (false)
00052 bool exists_after
00053
00054 #the time parameterized path
00055 geometry_msgs/PoseWithCovarianceStamped[] points
00056
00057 ================================================================================
00058 MSG: geometry_msgs/PoseWithCovarianceStamped
00059 # This expresses an estimated pose with a reference coordinate frame and timestamp
00060
00061 Header header
00062 PoseWithCovariance pose
00063
00064 ================================================================================
00065 MSG: geometry_msgs/PoseWithCovariance
00066 # This represents a pose in free space with uncertainty.
00067
00068 Pose pose
00069
00070 # Row-major representation of the 6x6 covariance matrix
00071 # The orientation parameters use a fixed-axis representation.
00072 # In order, the parameters are:
00073 # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
00074 float64[36] covariance
00075
00076 ================================================================================
00077 MSG: geometry_msgs/Pose
00078 # A representation of pose in free space, composed of postion and orientation.
00079 Point position
00080 Quaternion orientation
00081
00082 ================================================================================
00083 MSG: geometry_msgs/Point
00084 # This contains the position of a point in free space
00085 float64 x
00086 float64 y
00087 float64 z
00088
00089 ================================================================================
00090 MSG: geometry_msgs/Quaternion
00091 # This represents an orientation in free space in quaternion form.
00092
00093 float64 x
00094 float64 y
00095 float64 z
00096 float64 w
00097
00098 """
00099 __slots__ = ['header','dyn_obs']
00100 _slot_types = ['Header','dynamic_obs_msgs/DynamicObstacle[]']
00101
00102 def __init__(self, *args, **kwds):
00103 """
00104 Constructor. Any message fields that are implicitly/explicitly
00105 set to None will be assigned a default value. The recommend
00106 use is keyword arguments as this is more robust to future message
00107 changes. You cannot mix in-order arguments and keyword arguments.
00108
00109 The available fields are:
00110 header,dyn_obs
00111
00112 @param args: complete set of field values, in .msg order
00113 @param kwds: use keyword arguments corresponding to message field names
00114 to set specific fields.
00115 """
00116 if args or kwds:
00117 super(DynamicObstacles, self).__init__(*args, **kwds)
00118
00119 if self.header is None:
00120 self.header = std_msgs.msg._Header.Header()
00121 if self.dyn_obs is None:
00122 self.dyn_obs = []
00123 else:
00124 self.header = std_msgs.msg._Header.Header()
00125 self.dyn_obs = []
00126
00127 def _get_types(self):
00128 """
00129 internal API method
00130 """
00131 return self._slot_types
00132
00133 def serialize(self, buff):
00134 """
00135 serialize message into buffer
00136 @param buff: buffer
00137 @type buff: StringIO
00138 """
00139 try:
00140 _x = self
00141 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00142 _x = self.header.frame_id
00143 length = len(_x)
00144 buff.write(struct.pack('<I%ss'%length, length, _x))
00145 length = len(self.dyn_obs)
00146 buff.write(_struct_I.pack(length))
00147 for val1 in self.dyn_obs:
00148 buff.write(_struct_d.pack(val1.radius))
00149 length = len(val1.trajectories)
00150 buff.write(_struct_I.pack(length))
00151 for val2 in val1.trajectories:
00152 _x = val2
00153 buff.write(_struct_dB.pack(_x.probability, _x.exists_after))
00154 length = len(val2.points)
00155 buff.write(_struct_I.pack(length))
00156 for val3 in val2.points:
00157 _v1 = val3.header
00158 buff.write(_struct_I.pack(_v1.seq))
00159 _v2 = _v1.stamp
00160 _x = _v2
00161 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00162 _x = _v1.frame_id
00163 length = len(_x)
00164 buff.write(struct.pack('<I%ss'%length, length, _x))
00165 _v3 = val3.pose
00166 _v4 = _v3.pose
00167 _v5 = _v4.position
00168 _x = _v5
00169 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00170 _v6 = _v4.orientation
00171 _x = _v6
00172 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00173 buff.write(_struct_36d.pack(*_v3.covariance))
00174 except struct.error, se: self._check_types(se)
00175 except TypeError, te: self._check_types(te)
00176
00177 def deserialize(self, str):
00178 """
00179 unpack serialized message in str into this message instance
00180 @param str: byte array of serialized message
00181 @type str: str
00182 """
00183 try:
00184 if self.header is None:
00185 self.header = std_msgs.msg._Header.Header()
00186 end = 0
00187 _x = self
00188 start = end
00189 end += 12
00190 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00191 start = end
00192 end += 4
00193 (length,) = _struct_I.unpack(str[start:end])
00194 start = end
00195 end += length
00196 self.header.frame_id = str[start:end]
00197 start = end
00198 end += 4
00199 (length,) = _struct_I.unpack(str[start:end])
00200 self.dyn_obs = []
00201 for i in xrange(0, length):
00202 val1 = dynamic_obs_msgs.msg.DynamicObstacle()
00203 start = end
00204 end += 8
00205 (val1.radius,) = _struct_d.unpack(str[start:end])
00206 start = end
00207 end += 4
00208 (length,) = _struct_I.unpack(str[start:end])
00209 val1.trajectories = []
00210 for i in xrange(0, length):
00211 val2 = dynamic_obs_msgs.msg.DynObsTrajectory()
00212 _x = val2
00213 start = end
00214 end += 9
00215 (_x.probability, _x.exists_after,) = _struct_dB.unpack(str[start:end])
00216 val2.exists_after = bool(val2.exists_after)
00217 start = end
00218 end += 4
00219 (length,) = _struct_I.unpack(str[start:end])
00220 val2.points = []
00221 for i in xrange(0, length):
00222 val3 = geometry_msgs.msg.PoseWithCovarianceStamped()
00223 _v7 = val3.header
00224 start = end
00225 end += 4
00226 (_v7.seq,) = _struct_I.unpack(str[start:end])
00227 _v8 = _v7.stamp
00228 _x = _v8
00229 start = end
00230 end += 8
00231 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00232 start = end
00233 end += 4
00234 (length,) = _struct_I.unpack(str[start:end])
00235 start = end
00236 end += length
00237 _v7.frame_id = str[start:end]
00238 _v9 = val3.pose
00239 _v10 = _v9.pose
00240 _v11 = _v10.position
00241 _x = _v11
00242 start = end
00243 end += 24
00244 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00245 _v12 = _v10.orientation
00246 _x = _v12
00247 start = end
00248 end += 32
00249 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00250 start = end
00251 end += 288
00252 _v9.covariance = _struct_36d.unpack(str[start:end])
00253 val2.points.append(val3)
00254 val1.trajectories.append(val2)
00255 self.dyn_obs.append(val1)
00256 return self
00257 except struct.error, e:
00258 raise roslib.message.DeserializationError(e)
00259
00260
00261 def serialize_numpy(self, buff, numpy):
00262 """
00263 serialize message with numpy array types into buffer
00264 @param buff: buffer
00265 @type buff: StringIO
00266 @param numpy: numpy python module
00267 @type numpy module
00268 """
00269 try:
00270 _x = self
00271 buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
00272 _x = self.header.frame_id
00273 length = len(_x)
00274 buff.write(struct.pack('<I%ss'%length, length, _x))
00275 length = len(self.dyn_obs)
00276 buff.write(_struct_I.pack(length))
00277 for val1 in self.dyn_obs:
00278 buff.write(_struct_d.pack(val1.radius))
00279 length = len(val1.trajectories)
00280 buff.write(_struct_I.pack(length))
00281 for val2 in val1.trajectories:
00282 _x = val2
00283 buff.write(_struct_dB.pack(_x.probability, _x.exists_after))
00284 length = len(val2.points)
00285 buff.write(_struct_I.pack(length))
00286 for val3 in val2.points:
00287 _v13 = val3.header
00288 buff.write(_struct_I.pack(_v13.seq))
00289 _v14 = _v13.stamp
00290 _x = _v14
00291 buff.write(_struct_2I.pack(_x.secs, _x.nsecs))
00292 _x = _v13.frame_id
00293 length = len(_x)
00294 buff.write(struct.pack('<I%ss'%length, length, _x))
00295 _v15 = val3.pose
00296 _v16 = _v15.pose
00297 _v17 = _v16.position
00298 _x = _v17
00299 buff.write(_struct_3d.pack(_x.x, _x.y, _x.z))
00300 _v18 = _v16.orientation
00301 _x = _v18
00302 buff.write(_struct_4d.pack(_x.x, _x.y, _x.z, _x.w))
00303 buff.write(_v15.covariance.tostring())
00304 except struct.error, se: self._check_types(se)
00305 except TypeError, te: self._check_types(te)
00306
00307 def deserialize_numpy(self, str, numpy):
00308 """
00309 unpack serialized message in str into this message instance using numpy for array types
00310 @param str: byte array of serialized message
00311 @type str: str
00312 @param numpy: numpy python module
00313 @type numpy: module
00314 """
00315 try:
00316 if self.header is None:
00317 self.header = std_msgs.msg._Header.Header()
00318 end = 0
00319 _x = self
00320 start = end
00321 end += 12
00322 (_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
00323 start = end
00324 end += 4
00325 (length,) = _struct_I.unpack(str[start:end])
00326 start = end
00327 end += length
00328 self.header.frame_id = str[start:end]
00329 start = end
00330 end += 4
00331 (length,) = _struct_I.unpack(str[start:end])
00332 self.dyn_obs = []
00333 for i in xrange(0, length):
00334 val1 = dynamic_obs_msgs.msg.DynamicObstacle()
00335 start = end
00336 end += 8
00337 (val1.radius,) = _struct_d.unpack(str[start:end])
00338 start = end
00339 end += 4
00340 (length,) = _struct_I.unpack(str[start:end])
00341 val1.trajectories = []
00342 for i in xrange(0, length):
00343 val2 = dynamic_obs_msgs.msg.DynObsTrajectory()
00344 _x = val2
00345 start = end
00346 end += 9
00347 (_x.probability, _x.exists_after,) = _struct_dB.unpack(str[start:end])
00348 val2.exists_after = bool(val2.exists_after)
00349 start = end
00350 end += 4
00351 (length,) = _struct_I.unpack(str[start:end])
00352 val2.points = []
00353 for i in xrange(0, length):
00354 val3 = geometry_msgs.msg.PoseWithCovarianceStamped()
00355 _v19 = val3.header
00356 start = end
00357 end += 4
00358 (_v19.seq,) = _struct_I.unpack(str[start:end])
00359 _v20 = _v19.stamp
00360 _x = _v20
00361 start = end
00362 end += 8
00363 (_x.secs, _x.nsecs,) = _struct_2I.unpack(str[start:end])
00364 start = end
00365 end += 4
00366 (length,) = _struct_I.unpack(str[start:end])
00367 start = end
00368 end += length
00369 _v19.frame_id = str[start:end]
00370 _v21 = val3.pose
00371 _v22 = _v21.pose
00372 _v23 = _v22.position
00373 _x = _v23
00374 start = end
00375 end += 24
00376 (_x.x, _x.y, _x.z,) = _struct_3d.unpack(str[start:end])
00377 _v24 = _v22.orientation
00378 _x = _v24
00379 start = end
00380 end += 32
00381 (_x.x, _x.y, _x.z, _x.w,) = _struct_4d.unpack(str[start:end])
00382 start = end
00383 end += 288
00384 _v21.covariance = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=36)
00385 val2.points.append(val3)
00386 val1.trajectories.append(val2)
00387 self.dyn_obs.append(val1)
00388 return self
00389 except struct.error, e:
00390 raise roslib.message.DeserializationError(e)
00391
00392 _struct_I = roslib.message.struct_I
00393 _struct_d = struct.Struct("<d")
00394 _struct_36d = struct.Struct("<36d")
00395 _struct_dB = struct.Struct("<dB")
00396 _struct_3I = struct.Struct("<3I")
00397 _struct_4d = struct.Struct("<4d")
00398 _struct_2I = struct.Struct("<2I")
00399 _struct_3d = struct.Struct("<3d")