added the ability to add nodes when robot looses the wall
Did lots of stuff
Can now go to in-between nodes and explore much better
Render path in red
Started working with the go_to_root method. Also changed path to green in render
Fixed issues with in-between nodes. Now rendering better colors for different nodes
Now goes back to root properly