Everything works, added docs, roslint with zero errors