Updates towards Path Following

added poor collision detector
refactored command structure
broke out Rect class