Optimizations 2

[SteveMacenski]

Additional optimizations, readding re-queuing of nodes, speed up without graph initialization, and replaced the graph/queue pointers with stack variables which sped things up a lot.

Also added time monitoring of each phase https://github.com/SteveMacenski/navigation2/issues/14

Working on:

• [x] 2D setup
• [x] graph back to vector from map now that I've removed runtime lookups
• [x] test speeds of queue and graph reset methods