Cleanup vision algortihms and setup for debugging and testing in vision_state_machine node