Adding linear-only path and waypoint_metrics

- Computing linear-only path that uses nominal waypoints from start to
end
- Updating waypoint_metrics computed at end of a run