Adding occ grid query.
Merge branch 'master' of https://github.com/tjcs470/BZRFlag_470
Fixed error with angles.