Implemented lane callback and lots of bug fixes