Fix const problem for steering method
Allow optimization again and Remove debug outputs
Add debug outputs for solving time and edge creation