Add plot multidir scripts

diff --git a/scripts/plot_multidir_cost.py b/scripts/plot_multidir_cost.py
new file mode 100755 (executable)
index 0000000..9e330db
--- /dev/null
+++ b/scripts/plot_multidir_cost.py
@@ -0,0 +1,227 @@
+#!/usr/bin/env python3
+"""Plot scenario's runs cost."""
+from json import loads
+from os import listdir
+from sys import argv, exit
+import matplotlib.pyplot as plt
+import numpy as np
+
+FN = "test54"
+RUNS = 10000
+ITERS = 1000
+
+def load(fn=None):
+    """Load scenario from file.
+
+    Keyword arguments
+    -----------------
+
+    - `fn` -- File name.
+    """
+    if fn is None:
+        raise ValueError("File name as argument needed")
+    s = None
+    with open(fn, "r") as f:
+        s = loads(f.read())
+    return s
+
+def load_dir(dn=None, fn=None):
+    """Load scenarios from directory.
+
+    Keyword arguments
+    -----------------
+
+    - `dn` -- Directory name.
+    """
+    if dn is None:
+        raise ValueError("Directory name as argument needed")
+    scenarios = []
+    for d in listdir(dn):
+        if int(d) >= RUNS:
+            continue
+        s = load("{}/{}/{}.json".format(dn, d, fn))
+        s["dn"] = dn
+        s["subdn"] = d
+        s["fn"] = fn
+        scenarios.append(s)
+    return scenarios
+
+def perc(li=[], p=0):
+    return np.percentile(li, p) if len(li) > 0 else p
+
+if __name__ == "__main__":
+    WHAT_PLOT = 1
+    colors = [
+        "tab:red",
+        "tab:orange",
+        "tab:blue",
+        "tab:green",
+        "tab:red"]
+    names = [
+        "OSP-WK",
+        "OSP-WK + cost heur.",
+        "OSP-WK + goal zone",
+        "OSP-WK + path opt.",
+        "OSP-All"]
+    #names = [
+    #    "RRT",
+    #    "No cost heuristics",
+    #    "No goal zone",
+    #    "No path optimization",
+    #    "Out-of-slot planner"]
+    titles = [
+        "Failure rate",
+        "Average final path cost",
+        "Maximum final path cost"]
+
+    plt.rcParams["figure.figsize"] = [14, 7]
+    plt.rcParams["font.size"] = 24
+    plt.rcParams["font.family"] = "cmr10"
+    plt.rcParams["hatch.linewidth"] = 1.0
+    plt.rcParams["lines.linewidth"] = 1.0
+    plt.rc('axes', unicode_minus=False)
+
+    f, ax = plt.subplots()
+    ax.grid(which="major", linestyle=":", color="tab:gray")
+    #eax = ax.twinx()
+
+    #ax.set_title("Scenario 3: {}".format(titles[WHAT_PLOT - 1]))
+    #ax.set_title("Scenario 3: {}".format(titles[WHAT_PLOT - 1]))
+
+    #ax.set_title("Scenario 8: Final path cost after optimization")
+    #ax.set_title("Scenario 2: Final path cost after optimization")
+    #ax.set_title("Scenario 7: Final path cost after optimization")
+    ax.set_xlabel("Number of iterations [-]")
+    if WHAT_PLOT == 1:
+        ax.set_ylabel("Failure rate [%]")
+    else:
+        ax.set_ylabel("Cost [m]")
+    #ax.set_ylim([20.1, 57]) # scenario 7
+    #ax.set_ylim([18, 43]) # scenario 2
+    if WHAT_PLOT > 1:
+        ax.set_ylim([10, 40])
+    #ax.set_ylim([0, 100])
+
+    ci = -1
+    p1 = 0
+    p2 = 0
+    c1 = 0
+    c2 = 0
+    result_dir = "rwps-additional/rwps-1"
+    for test_fn in [
+        "test54-no-improvement",
+        "test54-w-cost-heuristics",
+        "test54-w-goal-zone",
+        "test54-w-opt",
+        "test55"]:
+        ci += 1
+        scenarios = load_dir(result_dir, test_fn)
+        L = ITERS
+        err_hist = [0 for i in range(L)]
+        val_list = [[] for i in range(L)]
+        icnt_list = []
+        bcnt_list = []
+        rcnt_list = []
+        time_list = []
+        otime_list = []
+        min_s = 0
+        min_c = 0.0
+        max_s = 0
+        max_c = 0.0
+        for s in scenarios:
+            lpc = s["log_path_cost"]
+            for i in range(L):
+                if len(lpc) <= i:
+                    lpc.append(lpc[-1])
+                if lpc[i] == 0:
+                    err_hist[i] += 1
+                else:
+                    val_list[i].append(lpc[i])
+            if min_c == 0.0 or s["goal_cc"] < min_c:
+                min_c = s["goal_cc"]
+                min_s = s["subdn"]
+            if s["goal_cc"] > max_c:
+                max_c = s["goal_cc"]
+                max_s = s["subdn"]
+            try:
+                icnt_list.append(s["icnt"])
+                bcnt_list.append(s["bcnt"])
+                rcnt_list.append(s["rcnt"])
+            except:
+                pass
+            try:
+                time_list.append(s["time"])
+            except:
+                pass
+            try:
+                otime_list.append(s["otime"])
+            except:
+                pass
+        o_err_hist = list(err_hist)
+        err_hist = [i * 100.0 / RUNS for i in o_err_hist]
+        #print("{} {}".format(min_s, max_s))
+        icnt_avg = np.average(icnt_list)
+        bcnt_avg = np.average(bcnt_list)
+        rcnt_avg = np.average(rcnt_list)
+        br_avg = np.average([b / r for (b, r) in zip(bcnt_list, rcnt_list)])
+        #print("avg. icnt = {}".format(icnt_avg))
+        #print("avg. bcnt = {}".format(bcnt_avg))
+        #print("avg. rcnt = {}".format(rcnt_avg))
+        #print("b/r avg. {}".format(br_avg))
+        print("last avg. cost = {}".format(np.average(val_list[-1])))
+        #print("last max. cost = {}".format(perc(val_list[-1], 100)))
+        print("failure rate = {}".format(err_hist[-1]))
+        for i in range(len(err_hist)):
+            if err_hist[i] == 0:
+                print(i)
+                break
+
+        #eax.fill_between(
+        #    x=range(L),
+        #    y1=0,
+        #    y2=[i for i in err_hist],
+        #    color=colors[ci],
+        #    alpha=0.2,
+        #    label="Succes rate {}".format(ci),
+        #)
+        if WHAT_PLOT == 1:
+            ax.plot(
+                range(L),
+                [i for i in err_hist],
+                color=colors[ci],
+                #alpha=0.45,
+                label=names[ci],
+                linestyle=(0, (5, 5)) if ci == 0 else "solid",
+            )
+        if WHAT_PLOT == 2:
+            ax.plot(
+                range(L),
+                [np.average(val_list[i]) for i in range(L)],
+                color=colors[ci],
+                #alpha=0.45,
+                label=names[ci],
+                linestyle=(0, (5, 5)) if ci == 0 else "solid",
+            )
+        if WHAT_PLOT == 3:
+            ax.plot(
+                range(L),
+                [perc(val_list[i], 100) for i in range(L)],
+                color=colors[ci],
+                #alpha=0.45,
+                label=names[ci],
+                linestyle=(0, (5, 5)) if ci == 0 else "solid",
+            )
+
+    ax.minorticks_on()
+    ax.tick_params(axis='x', labelrotation=45)
+    h1, la1 = ax.get_legend_handles_labels()
+    #h2, la2 = eax.get_legend_handles_labels()
+
+    h = h1# + h2
+    la = la1# + la2
+
+    ax.legend(h, la, loc="upper right", ncol=1)
+    if WHAT_PLOT == 2:
+        ax.legend(h, la, loc="upper right", ncol=2)
+    plt.savefig("out.pdf", bbox_inches="tight")
+    plt.close()