Indent the code sections.
Add testing environment.
Add graphical repr of the collision controller obstacles and path.