Add FP as first node in slot cusps

[hubacji1/iamcar.git] / gplot.py

diff --git a/gplot.py b/gplot.py
index 709a1fc025cf4a9aeed2f54c2fd405ab33071d28..0cc1168665b16ff3173d6eaf6b0005e82e6f209c 100644 (file)
--- a/gplot.py
+++ b/gplot.py
@@ -265,14 +265,137 @@ def print_successrate():
 if __name__ == "__main__":
     plt.rcParams["font.size"] = 29
     res = []
-    for d in listdir("testlog"):
-        LOGF="testlog/{}".format(d)
+    LOGF = "log-slotplanner"
+    for d in listdir(LOGF):
         r = {}
         for sf in [i["f"] for i in LOG]:
-            r[sf] = load_trajectories("{}/{}".format(LOGF, sf))
+            r[sf] = load_trajectories("{}/{}/{}".format(LOGF, d, sf))
             res.append({
                 "f": d,
                 "elap": get_val_if_exist(r["T2"], "elap"),
                 "rrte": get_val_if_exist(r["T2"], "rrte"),
                 "ppse": get_val_if_exist(r["T2"], "ppse"),
+                "succ": (
+                    count_if_exist(r["T2"], "traj") /
+                        count_if_exist(r["T2"], "elap")
+                ),
             })
+    res2 = []
+    LOGF = "log-rrt"
+    for d in listdir(LOGF):
+        r = {}
+        for sf in [i["f"] for i in LOG]:
+            r[sf] = load_trajectories("{}/{}/{}".format(LOGF, d, sf))
+            res2.append({
+                "f": d,
+                "elap": get_val_if_exist(r["T2"], "elap"),
+                "rrte": get_val_if_exist(r["T2"], "rrte"),
+                "ppse": get_val_if_exist(r["T2"], "ppse"),
+                "succ": (
+                    count_if_exist(r["T2"], "traj") /
+                        count_if_exist(r["T2"], "elap")
+                ),
+            })
+
+    fig = plt.figure()
+    # For color scheme
+    # see https://github.com/vega/vega/wiki/Scales#scale-range-literals
+    ax = fig.add_subplot(111)
+    ax.set_title("""Elapsed time for different lengths
+                of parallel parking slot""")
+
+    ax.set_ylabel("Time [s]")
+    ax.set_xlabel("Parking slot length [m]")
+
+    # res Slot Planner
+    coord = [float(r["f"].split("_")[1]) for r in res]
+
+    #val = [sum(r["ppse"])/len(r["ppse"]) for r in res]
+    #fin = [(x, y) for (x, y) in zip(coord, val)]
+    #fin.sort()
+    #plt.plot(
+    #    [x for (x, y) in fin],
+    #    [y for (x, y) in fin],
+    #    color = "g",
+    #    label = "Slot Planner",
+    #)
+
+    val = [max(r["elap"]) for r in res]
+    fin = [(x, y) for (x, y) in zip(coord, val)]
+    fin.sort()
+    plt.plot(
+        [x for (x, y) in fin],
+        [y for (x, y) in fin],
+        color = "#e6550d",
+        linestyle = "--",
+        label = "Elapsed worst",
+    )
+
+    #val = [sum(r["rrte"])/len(r["rrte"]) for r in res]
+    #fin = [(x, y) for (x, y) in zip(coord, val)]
+    #fin.sort()
+    #plt.plot(
+    #    [x for (x, y) in fin],
+    #    [y for (x, y) in fin],
+    #    color = "#fd8d3c",
+    #    label = "RRT",
+    #)
+
+    val = [sum(r["elap"])/len(r["elap"]) for r in res]
+    fin = [(x, y) for (x, y) in zip(coord, val)]
+    fin.sort()
+    plt.plot(
+        [x for (x, y) in fin],
+        [y for (x, y) in fin],
+        color = "#e6550d",
+        label = "Elapsed average",
+    )
+
+    val = [r["succ"] for r in res]
+    fin = [(x, y) for (x, y) in zip(coord, val)]
+    fin.sort()
+    plt.plot(
+        [x for (x, y) in fin],
+        [y for (x, y) in fin],
+        #color = "#fd8d3c",
+        color = "#fdae6b",
+        label = "Success rate",
+    )
+
+    # res2 RRT
+    coord = [float(r["f"].split("_")[1]) for r in res2]
+
+    val = [max(r["elap"]) for r in res2]
+    fin = [(x, y) for (x, y) in zip(coord, val)]
+    fin.sort()
+    plt.plot(
+        [x for (x, y) in fin],
+        [y for (x, y) in fin],
+        color = "#3182bd",
+        linestyle = "--",
+        label = "Elapsed worst",
+    )
+
+    val = [sum(r["elap"])/len(r["elap"]) for r in res2]
+    fin = [(x, y) for (x, y) in zip(coord, val)]
+    fin.sort()
+    plt.plot(
+        [x for (x, y) in fin],
+        [y for (x, y) in fin],
+        color = "#3182bd",
+        label = "Elapsed average",
+    )
+
+    val = [r["succ"] for r in res2]
+    fin = [(x, y) for (x, y) in zip(coord, val)]
+    fin.sort()
+    plt.plot(
+        [x for (x, y) in fin],
+        [y for (x, y) in fin],
+        #color = "#6baed6",
+        color = "#9ecae1",
+        label = "Success rate",
+    )
+
+    plt.legend(bbox_to_anchor=(1, 1), loc=1, borderaxespad=0)
+    plt.show()