Benchmark CSV saving

[horverno]

Save the following 3 CSVs:

• actual x and y coordinates, time (continuous) e.g. tmp01_xypose.csv
• actual time, command, wheel angle, speed /cmd_vel (continuous) e.g. tmp01_cmd.csv
• metrics data from the /metrics_wayp array topic (at the end) e.g. tmp01_metrics.csv
• visualize CSVs

The metrics data should include:

• current lateral distance to the waypoint (absolute value) CUR_LAT_DIST_ABS
• average lateral distance over time AVG_LAT_DISTANCE
• maximum lateral distance over time MAX_LAT_DISTANCE
• target waypoint longitudinal distanceTRG_WAY_LON_DIST