Fixed bugs in thread availability. Cleaned Lidar Code.