Add FP as first node in slot cusps

[hubacji1/iamcar.git] / gplot.py

diff --git a/gplot.py b/gplot.py
index 8b1685cc936bfadd0e087e028e4735fa61c08d8f..0cc1168665b16ff3173d6eaf6b0005e82e6f209c 100644 (file)
--- a/gplot.py
+++ b/gplot.py
@@ -5,6 +5,7 @@ from matplotlib import pyplot as plt
 from os import listdir
 from sys import argv, exit
 import numpy as np
+import scipy.stats as ss
 
 # ed - euclidean distance
 # rs - reeds and shepp path length
@@ -70,6 +71,22 @@ def gplot():
     plt.show()
     #plt.savefig("WHATEVER")
 
+def mean_conf_int(data, conf=0.95):
+    """Return (mean, lower, uppper) of data.
+
+    see https://stackoverflow.com/questions/15033511/compute-a-confidence-interval-from-sample-data
+
+    Keyword arguments:
+    data -- A list of data.
+    conf -- Confidence interval.
+    """
+    a = np.array(data)
+    n = len(a)
+    m = np.mean(a)
+    se = ss.sem(a)
+    h = se * ss.t.ppf((1 + conf) / 2, n - 1)
+    return (m, m - h, m + h)
+
 def count_if_exist(trajectories, what):
     """From multiple trajectories compute the number of occurences.
 
@@ -248,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()