Benchmark CSV saving with evaluation py script #1

Author: horverno

CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(tf2 REQUIRED)
 find_package(tf2_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(wayp_plan_tools REQUIRED)
 
 # uncomment the following section in order to fill in
 # further dependencies manually.
@@ -29,11 +30,17 @@ set(ament_dependencies
   tf2_msgs
   tf2_ros
   tf2_geometry_msgs  
+  wayp_plan_tools
 )
 
 include_directories(
-  include
+  # include
+  # share/${wayp_plan_tools}
+  # ${wayp_plan_tools_INCLUDE_DIRS}
+  ../wayp_plan_tools/include # TODO: this solution only works for the current project structure
 )
+# ament_export_include_directories(include) 
+# ament_export_dependencies(${ament_dependencies})
 
 add_executable(pose_arr_to_tf src/pose_arr_to_tf.cpp)
 ament_target_dependencies(pose_arr_to_tf  ${ament_dependencies} )
@@ -68,5 +75,6 @@ install(TARGETS
   pose_arr_to_tf
   visuals
   csv_saver
-  DESTINATION lib/${PROJECT_NAME})
+  DESTINATION lib/${PROJECT_NAME}
+)
 ament_package()

csv/README.md

@@ -12,3 +12,8 @@ A [Hungaroring](https://en.wikipedia.org/wiki/Hungaroring) inspired track.
 # `sim_waypoints3.csv`
 
 ![](../img/sim_waypoints3.svg)
+
+
+# Evaluation example
+
+![](../img/csv_eval01.svg)

csv/csv_eval_measurement.py

@@ -0,0 +1,93 @@
+# load csv and  plot it
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.gridspec as gridspec
+
+# waypoint csv header: x,y,z,yaw,mps,change_flag
+W_X = 0
+W_Y = 1
+W_Z = 2
+W_YAW = 3
+W_MPS = 4
+# measure csv header: x,y,time,steering_angle,speed_mps,cur_lat_dist_abs,trg_way_lon_dist
+M_X = 0
+M_Y = 1
+M_TIME = 2
+M_STEER = 3
+M_SPEED = 4
+M_LAT_DIST = 5
+M_LON_DIST = 6
+
+# create plot layout
+# f, (ax_xy, ax_st, ax_dist) = plt.subplots(nrows=3, ncols=1, sharex=False, gridspec_kw={"height_ratios": [3, 1, 1]})
+
+f = plt.figure(constrained_layout=True)
+gs = f.add_gridspec(3, 3, width_ratios=[3, 1, 1], height_ratios=[3, 1, 1])
+ax_xy = f.add_subplot(gs[0, :])
+ax_st = f.add_subplot(gs[1, :-1])
+ax_txt = f.add_subplot(gs[1:, -1])
+ax_dist = f.add_subplot(gs[-1, :-1])
+
+# ax_xy.set_xlabel("x") # , fontsize=6
+# ax_xy.set_ylabel("y")
+ax_st.set_xlabel("time")
+ax_st.set_ylabel("steer ang")
+ax_dist.set_xlabel("time")
+ax_dist.set_ylabel("lat dist")
+# ax_st.set_title("steering angle")
+# ax_dist.set_title("lat distance (abs)")
+ax_xy.axis("equal")
+ax_xy.grid()
+ax_st.grid()
+ax_dist.grid()
+ax_xy.legend(['first stock name', 'second stock name'])
+ax_txt.axis("off")
+
+
+
+# read the data
+def read_data(filename):
+    points = np.genfromtxt(filename, delimiter=",", skip_header=1)
+    return points
+
+def read_data_arr(filenames):
+    points = []
+    for filename in filenames:
+        points.append(np.genfromtxt(filename + "_xypose.csv", delimiter=",", skip_header=1))
+    return points
+
+# plot x y data
+def plot_xy(points, fmt="", labl=""):
+    ax_xy.plot(points[:, W_X], points[:, W_Y], fmt, label=labl, alpha=0.8)
+    ax_xy.legend()
+
+def plot_all(points, fmt="", labl=""):
+    ax_xy.plot(points[:, W_X], points[:, W_Y], label=labl, alpha=0.8)
+    ax_st.plot(points[:, M_TIME], points[:, M_STEER], fmt, label=labl, alpha=0.8)
+    ax_dist.plot(points[:, M_TIME], points[:, M_LAT_DIST], fmt, label=labl, alpha=0.8)
+    ax_xy.legend()
+
+def read_text(filenames):
+    text = ""
+    for filename in filenames:
+        text += "\n\n" + filename + "\n"
+        with open(filename + "_metrics.csv", "r") as file:
+            text += file.read()
+    return text
+            
+
+# main function
+if __name__ == "__main__":
+    meas_arr = ["csv/tmp01", "csv/tmp02", "csv/tmp03"]
+    wp_file = "csv/sim_waypoints3.csv"
+    points_wp = read_data(wp_file)
+    points_meas = read_data_arr(meas_arr)
+    # ax_xy.plot(points_meas[0][:, M_X], points_meas[0][:, M_Y], "r.-", label="xy", alpha=0.1, linewidth=4.2)
+    plot_xy(points_wp, fmt="k--", labl="waypoints")
+    plot_all(points_meas[0], labl="p pursuit A")
+    plot_all(points_meas[1], labl="p pursuit B")
+    plot_all(points_meas[2], labl="p pursuit C")
+    ax_txt.text(0.5, 0.5, read_text(meas_arr), ha="center", va="center", fontsize=8)
+    # f.tight_layout()
+    # f.savefig("img/csv_eval01.svg")
+    plt.show()

csv/csv_vizu.py

@@ -17,12 +17,14 @@ def read_data(filename = 'csv/sim_waypoints1.csv'):
 
 # main function
 if __name__ == '__main__':
-    points = read_data('csv/sim_waypoints2.csv')
-    plt.plot(points[:, 0], points[:, 1], 'r.-', label='xy', alpha=0.1, linewidth=4.2)
+    my_file = 'csv/sim_waypoints2.csv'
+    points = read_data(my_file)
+    plt.plot(points[:, 0], points[:, 1], 'r.-', label=my_file, alpha=0.1, linewidth=4.2)
     # plt.quiver(points[:, 0], points[:, 1], np.cos(points[:, 3]), np.sin(points[:, 3]), angles='xy', scale_units='xy', scale=2.2, alpha=0.4)
     plt.xlabel('x', fontsize=6)
     plt.ylabel('y', fontsize=6)
-    plt.title('sim_waypoints2', fontsize=6)
+    plt.title(my_file, fontsize=6)
     plt.axis('equal')
     plt.grid()
+    plt.legend()
     plt.show()

csv/tmp01_metrics.csv

@@ -0,0 +1,4 @@
+MAX_LAT_DISTANCE,9.8051
+AVG_LAT_DISTANCE,0.7500
+lookahead_min,2.5
+lookahead_max,6.5