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
+# sh - reeds and shepp, same heading
+
+# ad - optimize with dijkstra, all nodes
+# as - optimize with smart, all nodes
+# ar - optimize with remove redundant points, all nodes
+# cd - optimize with dijkstra, cusp nodes
+# cs - optimize with smart, cusp nodes
+# cr - optimize with remove redundant points, cusp nodes
+
+LOGF = "log_wo"
+LOG = [
+ {"f": "T2", "c": "orange", "l": "T2"},
+]
+
+r = {}
def load_trajectory(fname):
"""Load trajectory from file.
return trajectories
def gplot():
- """Plot graph."""
+ """Plot graph. Template."""
plt.rcParams["font.size"] = 24
fig = plt.figure()
ax = fig.add_subplot(111)
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.
pass
return val
-def plot_successrate():
- """Plot success rate of single/multi-core implementations."""
- LOGF="logs_T2st3co2_lpar"
- LOGSF=["opt1_nn3", "opt1_nn4", "opt1_nn4_save-heading"]
- LEG={
- LOGSF[0]: "T2, st3, co2, nn3",
- LOGSF[1]: "T2, st3, co2, nn4",
- LOGSF[2]: "T2, st3, co2, nn4 save heading"}
-
- r={}
- for sf in LOGSF:
- r["{}".format(LEG[sf])] = load_trajectories("{}/{}".format(LOGF, sf))
-
- v={}
- for a in r.keys():
- v[a] = (100 * count_if_exist(r[a], "traj") /
- count_if_exist(r[a], "elap"))
- vs = sorted(v.items(), key=lambda kv: kv[1])
-
- plt.rcParams["font.size"] = 24
- fig = plt.figure()
- ax = fig.add_subplot(111)
- ax.set_title("Trajectories found in 10 seconds period")
-
- ax.set_ylabel("Framework tested [-]")
- ax.set_xlabel("Found successfully [%]")
- ax.set_xlim(0, 100)
- ax.set_yticklabels([])
- j = 0
- for (k, i) in vs:
- print("{}: {}%".format(k, i))
- ax.barh(j, float(i), 4, label=k)
- j += -5
- ax.legend()
+def get_val_if_exist(trajectories, what):
+ """From m ultiple trajectories get value.
- plt.show()
-
-def plot_mintrajcost():
- """Plot minimum trajectory cost found."""
- LOGF="logs_inf_sq"
- ALG=["Karaman2011", "Kuwata2008", "LaValle1998"]
- ALGT={"Karaman2011": "RRT*",
- "Kuwata2008": "RRT + heur",
- "LaValle1998": "RRT"}
- ST=["3"]
- STT={"3": "R&S"}
- CO=["5"]
- COT={"5": "E2"}
-
- r={}
- for a in ALG:
- for st in ST:
- for co in CO:
- r["{}, {}, {}".format(ALGT[a], STT[st], COT[co])] = (
- load_trajectories("{}/{}st{}co{}_lpar".format(
- LOGF, a, st, co)))
-
- v={}
- for a in r.keys():
- v[a] = get_lasts_if_exist(r[a], "cost")
-
- #plt.rcParams["font.size"] = 24
- fig = plt.figure()
- for i in range(len(r.keys())):
- ax = fig.add_subplot(3, 1, i + 1)
- ax.set_title("Minimum trajectory cost found in 10 seconds (sequential)")
-
- ax.set_ylabel("Framework tested [-]")
- ax.set_xlabel("Minimum trajectory cost [m]")
- #ax.set_yticklabels([])
-
- plt.bar(range(len(v[r.keys()[i]])), v[r.keys()[i]], label=r.keys()[i])
-
- ax.legend()
- plt.show()
- return
+ Keyword arguments:
+ trajectories -- The list of trajectories.
+ what -- What to take.
+ """
+ val = []
+ for t in trajectories:
+ try:
+ val.append(t[what])
+ except:
+ pass
+ return val
def plot_costdist():
"""Plot distribution of last costs across measured tests."""
- LOGF="logs_T2st3co2_lpar"
- LOGSF=["opt0", "opt1_nn4", "opt2_nn4"]
- LEG={
- LOGSF[0]: "T2, st3, co2, no opt",
- LOGSF[1]: "T2, st3, co2, Dijkstra opt",
- LOGSF[2]: "T2, st3, co2, Triangular opt"}
-
- r={}
- for sf in LOGSF:
- r["{}".format(LEG[sf])] = load_trajectories("{}/{}".format(LOGF, sf))
-
- v={}
+ v = {}
for a in r.keys():
v[a] = get_lasts_if_exist(r[a], "cost")
- plt.rcParams["font.size"] = 24
fig = plt.figure()
ax = fig.add_subplot(111)
- ax.set_title("Trajectories last costs distribution")
+ ax.set_title("Path cost histogram")
- ax.set_ylabel("[-]")
- ax.set_xlabel("Minimum trajectory cost [m]")
- ax.set_yticklabels([])
+ ax.set_ylabel("Number of paths with given cost [-]")
+ ax.set_xlabel("Path cost [m]")
ax.set_yscale("log")
- for a in r.keys():
- plt.hist(v[a], label=a, alpha=0.5, bins=200)
+ for a in LOG:
+ plt.hist(
+ v[a["f"]],
+ alpha = 0.5,
+ label = a["l"],
+ bins = 100,
+ histtype = "step",
+ color = a["c"])
+ try:
+ X_WHERE = np.percentile(v[a["f"]], [95])
+ plt.axvline(X_WHERE, lw=1, color=a["c"], linestyle="--")
+ except:
+ pass
plt.legend()
plt.show()
def plot_maxtime():
"""Plot time of the last traj (the maximum time)."""
- LOGF="logs_T2st3co2_lpar"
- LOGSF=["opt1_nn3", "opt1_nn4", "opt1_nn4_save-heading"]
- LEG={
- LOGSF[0]: "T2, st3, co2, nn3",
- LOGSF[1]: "T2, st3, co2, nn4",
- LOGSF[2]: "T2, st3, co2, nn4 save heading"}
-
- r={}
- for sf in LOGSF:
- r["{}".format(LEG[sf])] = load_trajectories("{}/{}".format(LOGF, sf))
-
- v={}
+ v = {}
for a in r.keys():
v[a] = get_lasts_if_exist(r[a], "secs")
- plt.rcParams["font.size"] = 24
fig = plt.figure()
ax = fig.add_subplot(111)
- ax.set_title("The last trajectory time")
+ ax.set_title("Histogram of time to find the path")
- ax.set_ylabel("[-]")
- ax.set_xlabel("The last trajectory time [s]")
- ax.set_yticklabels([])
+ ax.set_ylabel("Number of paths found [-]")
+ ax.set_xlabel("Algorithm computation time [s]")
ax.set_yscale("log")
+ for a in LOG:
+ plt.hist(
+ v[a["f"]],
+ alpha = 0.5,
+ label = a["l"],
+ bins = np.arange(0, 10, 0.1),
+ histtype = "step",
+ color = a["c"])
+ try:
+ X_WHERE = np.percentile(v[a["f"]], [95])
+ plt.axvline(X_WHERE, lw=1, color=a["c"], linestyle="--")
+ except:
+ pass
+
+ plt.legend()
+ plt.show()
+
+def plot_nothingdone():
+ """Plot *nothing done* time of ``overlaptrees`` procedure."""
+ v = {}
for a in r.keys():
- plt.hist(v[a], label=a, alpha=0.5, bins=np.arange(0, 10, 0.05))
+ v[a] = get_lasts_if_exist(r[a], "nodo")
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111)
+ ax.set_title("Histogram of nothing-done-time")
+
+ ax.set_ylabel("Occurences [-]")
+ ax.set_xlabel("Nothing-done-time percentage [-]")
+
+ for a in LOG:
+ plt.hist(
+ v[a["f"]],
+ alpha = 0.5,
+ label = a["l"],
+ bins = np.arange(0, 1, 0.1),
+ histtype = "step",
+ color = a["c"])
+ try:
+ X_WHERE = np.percentile(v[a["f"]], [95])
+ plt.axvline(X_WHERE, lw=1, color=a["c"], linestyle="--")
+ except:
+ pass
plt.legend()
plt.show()
def print_nofnodes():
"""Print average number of nodes."""
- LOGF="logs_T2st3co2_lpar"
- LOGSF=["opt0_nnedist", "opt2_nnsh", "opt1_nnedist", "opt1_nn4"]
- LEG={
- LOGSF[0]: "opt0 nn edist",
- LOGSF[1]: "opt2 nn same heading",
- LOGSF[2]: "opt1 nn edist",
- LOGSF[3]: "opt1 nn same heading"}
- HAT={
- LEG[LOGSF[0]]: "",
- LEG[LOGSF[1]]: "/",
- LEG[LOGSF[2]]: "x"}
-
- r={}
- for sf in LOGSF:
- r["{}".format(LEG[sf])] = load_trajectories("{}/{}".format(LOGF, sf))
-
v={}
for a in r.keys():
lasts = get_lasts_if_exist(r[a], "node")
v[a] = np.average(lasts)
- print("Average number of points:")
- for a in r.keys():
- print("{}: {}".format(a, v[a]))
+ print("Average number of nodes:")
+ for a in LOG:
+ print("{}: {}".format(a["f"], v[a["f"]]))
def print_successrate():
"""Print success rate of implementations."""
- LOGF="logs_T2st3co2_lpar"
- LOGSF=["opt0_nnedist", "opt2_nnsh", "opt1_nnedist", "opt1_nn4"]
- LEG={
- LOGSF[0]: "opt0 nn edist",
- LOGSF[1]: "opt2 nn same heading",
- LOGSF[2]: "opt1 nn edist",
- LOGSF[3]: "opt1 nn same heading"}
-
- r={}
- for sf in LOGSF:
- r["{}".format(LEG[sf])] = load_trajectories("{}/{}".format(LOGF, sf))
-
v={}
for a in r.keys():
- v[a] = (100 * count_if_exist(r[a], "traj") /
+ v[a] = (100.0 * count_if_exist(r[a], "traj") /
count_if_exist(r[a], "elap"))
print("Success rate:")
- for a in r.keys():
- print("{}: {}".format(a, v[a]))
+ for a in LOG:
+ print("{}: {}".format(a["f"], v[a["f"]]))
if __name__ == "__main__":
- plot_costdist()
+ plt.rcParams["font.size"] = 29
+ res = []
+ LOGF = "log-slotplanner"
+ for d in listdir(LOGF):
+ r = {}
+ for sf in [i["f"] for i in LOG]:
+ 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()