Calc steps only if waypoint exists
Remove verbose warning
Fix A*

Initialise colour map properly.