Upgrade plot scripts

diff --git a/scripts/plot.py b/scripts/plot.py
old mode 100644 (file)
new mode 100755 (executable)

diff --git a/scripts/plot_cost.py b/scripts/plot_cost.py
index a11d0ba89ed280a04a95fc5adaf4e3859a1fd13c..5450e7e4067e3236ce4ae010af05d9be93560997 100755 (executable)
--- a/scripts/plot_cost.py
+++ b/scripts/plot_cost.py
@@ -66,6 +66,8 @@ Where:
     icnt_list = []
     bcnt_list = []
     rcnt_list = []
     icnt_list = []
     bcnt_list = []
     rcnt_list = []

+    ncnt_list = []
+    cc_list = []

     time_list = []
     otime_list = []
     min_s = 0
     time_list = []
     otime_list = []
     min_s = 0


@@ -87,10 +89,13 @@ Where:
         if s["goal_cc"] > max_c:
             max_c = s["goal_cc"]
             max_s = s["subdn"]
         if s["goal_cc"] > max_c:
             max_c = s["goal_cc"]
             max_s = s["subdn"]

+        if s["goal_cc"] > 0:
+            cc_list.append(s["goal_cc"])

         try:
             icnt_list.append(s["icnt"])
             bcnt_list.append(s["bcnt"])
             rcnt_list.append(s["rcnt"])
         try:
             icnt_list.append(s["icnt"])
             bcnt_list.append(s["bcnt"])
             rcnt_list.append(s["rcnt"])

+            ncnt_list.append(s["ncnt"])

         except:
             pass
         try:
         except:
             pass
         try:


@@ -102,18 +107,49 @@ Where:
         except:
             pass
     o_err_hist = list(err_hist)
         except:
             pass
     o_err_hist = list(err_hist)

-    err_hist = [i * 100.0 / RUNS for i in o_err_hist]
+    err_hist = [i * 100.0 / RUNS if i > 0 else -6 for i in o_err_hist]
+    # this is failure rate
+    w_it = False
+    if w_it:
+        print("Failure {} % in the iteration {}".format(err_hist[w_it], w_it))
+    else:
+        try:
+            print("Failure 0.0 % in the iteration {}".format(err_hist.index(0.0)))
+        except:
+            print("Failure 0.0 % not reached")
+    # --- exit(1)

     print("{} {}".format(min_s, max_s))
     icnt_avg = np.average(icnt_list)
     bcnt_avg = np.average(bcnt_list)
     rcnt_avg = np.average(rcnt_list)
     print("{} {}".format(min_s, max_s))
     icnt_avg = np.average(icnt_list)
     bcnt_avg = np.average(bcnt_list)
     rcnt_avg = np.average(rcnt_list)

+    ncnt_avg = np.average(ncnt_list)
+    cc_avg = np.average(cc_list)
+    try:
+        cc_max = np.max(cc_list)
+    except:
+        cc_max = 0
+    time_avg = np.average(time_list)
+    time_max = np.max(time_list)
+    otime_avg = np.average(otime_list)
+    try:
+        otime_max = np.max(otime_list)
+    except:
+        otime_max = 0

     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))
     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("avg. ncnt = {}".format(ncnt_avg))
+    print("avg. cc = {}".format(cc_avg))
+    print("max. cc = {}".format(cc_max))
+    print("avg. time = {}".format(time_avg))
+    print("max. time = {}".format(time_max))
+    print("avg. otime = {}".format(otime_avg))
+    print("max. otime = {}".format(otime_max))

     print("b/r avg. {}".format(br_avg))
     print("last avg. cost = {}".format(np.average(val_list[-1])))
     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("last max. cost = {}".format(perc(val_list[-1], 100)))
+    #exit(1)

 
     plt.rcParams["figure.figsize"] = [14, 7]
     plt.rcParams["font.size"] = 24
 
     plt.rcParams["figure.figsize"] = [14, 7]
     plt.rcParams["font.size"] = 24


@@ -124,19 +160,21 @@ Where:
 
     f, ax = plt.subplots()
     eax = ax.twinx()
 
     f, ax = plt.subplots()
     eax = ax.twinx()

-    ax.set_title(scenarios[0]["fn"])
-    ax.set_title("Scenario 3: Final path cost after optimization")
+    ax.grid(which="major", linestyle=":", color="tab:gray")
+    #ax.grid(which="minor", linestyle=":", color="gray")
+    #ax.set_title(scenarios[0]["fn"])
+    #ax.set_title("Scenario 3: Final path cost after optimization")

     ax.set_xlabel("Number of iterations [-]")
     ax.set_ylabel("Cost [m]")
     eax.set_ylabel("Failure rate [%]")
     #ax.set_ylim([20.1, 57]) # scenario 7
     #ax.set_ylim([18, 43]) # scenario 2
     ax.set_ylim([10, 60])
     ax.set_xlabel("Number of iterations [-]")
     ax.set_ylabel("Cost [m]")
     eax.set_ylabel("Failure rate [%]")
     #ax.set_ylim([20.1, 57]) # scenario 7
     #ax.set_ylim([18, 43]) # scenario 2
     ax.set_ylim([10, 60])

-    eax.set_ylim([0, 100])
+    eax.set_ylim([-5, 100])

 
     eax.fill_between(
         x=range(L),
 
     eax.fill_between(
         x=range(L),

-        y1=0,
+        y1=-5,

         y2=[i for i in err_hist],
         color="tab:red",
         alpha=0.2,
         y2=[i for i in err_hist],
         color="tab:red",
         alpha=0.2,


@@ -163,7 +201,7 @@ Where:
     ax.plot(
         range(L),
         [np.average(val_list[i]) for i in range(L)],
     ax.plot(
         range(L),
         [np.average(val_list[i]) for i in range(L)],

-        color="tab:red",
+        color="tab:blue",

         alpha=0.45,
         label="Average cost",
     )
         alpha=0.45,
         label="Average cost",
     )

diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py
index 3c8ca1aa634f7b891ad691a9cefdb014e549354f..98ea89d64e3759decd88aa586ee789dcd0019f70 100755 (executable)
--- a/scripts/plot_json_objects_scenario.py
+++ b/scripts/plot_json_objects_scenario.py
@@ -1,10 +1,11 @@
 #!/usr/bin/env python3
 """Plot JSON formatted scenario."""
 from json import loads
 #!/usr/bin/env python3
 """Plot JSON formatted scenario."""
 from json import loads

-from math import cos, pi, sin
+from math import cos, pi, sin, atan, atan2

 from math import inf
 from matplotlib import pyplot as plt
 from sys import argv, exit
 from math import inf
 from matplotlib import pyplot as plt
 from sys import argv, exit

+import matplotlib

 
 BCAR_MTR = 10.820
 BCAR_WB = 2.450
 
 BCAR_MTR = 10.820
 BCAR_WB = 2.450


@@ -186,16 +187,15 @@ if __name__ == "__main__":
     plt.rc('axes', unicode_minus=False)
     plt.rcParams["figure.figsize"] = [14, 7]
     plt.rcParams["font.family"] = "cmr10"
     plt.rc('axes', unicode_minus=False)
     plt.rcParams["figure.figsize"] = [14, 7]
     plt.rcParams["font.family"] = "cmr10"

-    plt.rcParams["font.size"] = 22
-    plt.rcParams['hatch.linewidth'] = 0.5
-    plt.rcParams['lines.linewidth'] = 1.0
+    plt.rcParams["font.size"] = 24
+    plt.rcParams['hatch.linewidth'] = 6.0
+    plt.rcParams['lines.linewidth'] = 2.0

     fig = plt.figure()
 
     # here subplot starts
     ax = fig.add_subplot(111)
     ax.set_aspect("equal")
     fig = plt.figure()
 
     # here subplot starts
     ax = fig.add_subplot(111)
     ax.set_aspect("equal")

-    ax.set_title("Real-world parking scenario")
-    ax.set_title("Simple parking scenario")
+    #ax.set_title("Real-world parking scenario")

     ax.set_xlabel("x [m]")
     ax.set_ylabel("y [m]")
     # For stage, comment upper, uncomment following:
     ax.set_xlabel("x [m]")
     ax.set_ylabel("y [m]")
     # For stage, comment upper, uncomment following:


@@ -347,6 +347,156 @@ if __name__ == "__main__":
     )
     #ax.add_patch(c4)
 
     )
     #ax.add_patch(c4)
 

+    # For Goal Zone figure, "Goal zone" file name in j1/figs/
+    def r2d(w):
+        return w*180.0/pi
+    ax.set_ylim([-4.8, 2.8])
+    ax.set_xlim([-13, 5])
+    gz_ccr = matplotlib.patches.Arc(
+        (-744206.185356 - MINX, -1044330.294266 - MINY),
+        5.207071 * 2, 5.207071 * 2,
+        theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
+        theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
+        color="magenta",
+        fill=False,
+        lw=2,
+    )
+    ax.add_patch(gz_ccr)
+    gz_ccr = matplotlib.patches.Arc(
+        (-744206.185356 - MINX + 3.99, -1044330.294266 - MINY + 2.05),
+        5.207071 * 2, 5.207071 * 2,
+        theta1=r2d(atan2(-1044325.281765 - -1044330.294266, -744204.775115 - -744206.185356)),
+        theta2=r2d(atan2(-1044325.6618554679 - -1044330.294266, -744208.5632466434 - -744206.185356)),
+        color="magenta",
+        fill=False,
+        lw=2, ls="dotted",
+    )
+    ax.add_patch(gz_ccr)
+    gz_gh = 0.47424360277825361
+    gz_ih = -0.27424360277825361
+    def li(x, y, h, le=10.0):
+        return (x, x + le * cos(h)), (y, y + le * sin(h))
+    # gz border
+    plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_gh),
+        color="orange", ls="dotted")
+    plt.plot(*li(-744204.775115 - MINX, -1044325.281765 - MINY, gz_ih),
+        color="red", ls="dotted")
+    # path
+    plt.plot(
+        *li(-744208.5632466434 - MINX, -1044325.6618554679 - MINY, gz_gh, 4.47),
+        color="orange", ls="solid")
+    plt.plot(
+        *li(-744199.2632466434 - MINX, -1044323.6618554679 - MINY, gz_ih, -1.55),
+        color="red", ls="solid")
+    ax.text(
+        -744208.5632466434 - MINX,
+        -1044325.6618554679 - MINY - 1.5,
+        "C",
+        color="orange",
+        fontfamily="serif",
+        fontstyle="italic",
+    )
+    ax.text(
+        -744208.5632466434 - MINX + 0.35,
+        -1044325.6618554679 - MINY - 1.7,
+        "E",
+        color="orange",
+        fontfamily="serif",
+        fontstyle="italic",
+        fontsize=16,
+    )
+    ax.text(
+        -744199.2632466434 - MINX,
+        -1044323.6618554679 - MINY - 1.5,
+        "C",
+        color="red",
+        fontfamily="serif",
+        fontstyle="italic",
+    )
+    ax.text(
+        -744199.2632466434 - MINX + 0.35,
+        -1044323.6618554679 - MINY - 1.7,
+        "g",
+        color="red",
+        fontfamily="serif",
+        fontstyle="italic",
+        fontsize=16,
+    )
+    ax.text(
+        -744199.2632466434 - MINX,
+        -1044323.6618554679 - MINY - 3.9,
+        "θ",
+        color="red",
+        fontfamily="serif",
+        fontstyle="italic",
+    )
+    ax.text(
+        -744199.2632466434 - MINX + 0.35,
+        -1044323.6618554679 - MINY - 4.1,
+        "G",
+        color="red",
+        fontfamily="serif",
+        fontstyle="italic",
+        fontsize=16,
+    )
+    ax.arrow(
+        -744199.2632466434 - MINX,
+        -1044323.6618554679 - MINY - 3.18,
+        cos(gz_ih),
+        sin(gz_ih),
+        width=0.05,
+        color="red",
+        zorder=2,
+    )
+    ax.text(
+        -744199.2632466434 - MINX,
+        -1044323.6618554679 - MINY + 1.9,
+        "θ",
+        color="orange",
+        fontfamily="serif",
+        fontstyle="italic",
+    )
+    ax.text(
+        -744199.2632466434 - MINX + 0.35,
+        -1044323.6618554679 - MINY + 1.7,
+        "E",
+        color="orange",
+        fontfamily="serif",
+        fontstyle="italic",
+        fontsize=16,
+    )
+    ax.arrow(
+        -744199.2632466434 - MINX,
+        -1044323.6618554679 - MINY + 1.22,
+        cos(gz_gh),
+        sin(gz_gh),
+        width=0.05,
+        color="orange",
+        zorder=2,
+    )
+    ax.text(
+        -744199.2632466434 - MINX + 2,
+        -1044323.6618554679 - MINY + -3,
+        "G",
+        color="dimgray",
+        fontfamily="serif",
+        fontstyle="normal",
+        fontweight="bold",
+        backgroundcolor="white",
+    )
+    ax.fill((
+            -MINX -744204.775115,
+            -MINX -744204.775115 + 15 * cos(0.47424360277825361),
+            -MINX -744204.775115 + 15 * cos(0.27424360277825361),
+            -MINX -744204.775115,
+        ), (
+            -MINY -1044325.281765,
+            -MINY -1044325.281765 + 15 * sin(0.47424360277825361),
+            -MINY -1044325.281765 - 15 * sin(0.27424360277825361),
+            -MINY -1044325.281765,
+        ), color="gainsboro", fill=False, hatch="x")
+    # --- End of Goal Zone figure ---
+

     # Plot all the nodes (if exists.)
     if "nodes_x" in scenario and "nodes_y" in scenario:
         plt.plot(
     # Plot all the nodes (if exists.)
     if "nodes_x" in scenario and "nodes_y" in scenario:
         plt.plot(


@@ -473,7 +623,8 @@ if __name__ == "__main__":
             scenario["init"][1] - MINY,
             color="red",
             marker="+",
             scenario["init"][1] - MINY,
             color="red",
             marker="+",

-            ms=12
+            markeredgewidth=2,
+            ms=24

         )
     #if "init" in scenario and len(scenario["init"]) == 4:
     #    plt.plot(*plot_car(scenario["init"]), color="red")
         )
     #if "init" in scenario and len(scenario["init"]) == 4:
     #    plt.plot(*plot_car(scenario["init"]), color="red")


@@ -496,15 +647,16 @@ if __name__ == "__main__":
     #            marker="+",
     #            ms=12
     #        )
     #            marker="+",
     #            ms=12
     #        )

-    #if "entry" in scenario and len(scenario["entry"]) == 3:
-    #    plt.plot(*plot_car(scenario["entry"]), color="magenta")
-    #    plt.plot(
-    #        scenario["entry"][0] - MINX,
-    #        scenario["entry"][1] - MINY,
-    #        color="magenta",
-    #        marker="+",
-    #        ms=12
-    #    )
+    if "entry" in scenario and len(scenario["entry"]) == 3:
+        plt.plot(*plot_car(scenario["entry"]), color="magenta")
+        plt.plot(
+            scenario["entry"][0] - MINX,
+            scenario["entry"][1] - MINY,
+            color="magenta",
+            marker="+",
+            markeredgewidth=2,
+            ms=24
+        )

     #if "entry" in scenario and len(scenario["entry"]) == 4:
     #    esc = scenario["entry"]
     #    plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
     #if "entry" in scenario and len(scenario["entry"]) == 4:
     #    esc = scenario["entry"]
     #    plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")


@@ -518,13 +670,16 @@ if __name__ == "__main__":
     #    )
     if "goal" in scenario:
         if len(scenario["goal"]) == 3:
     #    )
     if "goal" in scenario:
         if len(scenario["goal"]) == 3:

-            plt.plot(*plot_car(scenario["goal"]), color="green")
+            #plt.plot(*plot_car(scenario["goal"]), color="green")
+            plt.plot(*plot_car(scenario["goal"]), color="orange")

             plt.plot(
                 scenario["goal"][0] - MINX,
                 scenario["goal"][1] - MINY,
             plt.plot(
                 scenario["goal"][0] - MINX,
                 scenario["goal"][1] - MINY,

-                color="green",
+                #color="green",
+                color="orange",

                 marker="+",
                 marker="+",

-                ms=12
+                markeredgewidth=2,
+                ms=24

             )
     #    elif len(scenario["goal"]) == 4:
     #        ctp = scenario["goal"]
             )
     #    elif len(scenario["goal"]) == 4:
     #        ctp = scenario["goal"]

diff --git a/scripts/scenario.py b/scripts/scenario.py
index 3ef8be3bae865a3040cdb66131088f982348619e..5ec8a31d2ca76ebd4333fb63c5691fee4d5883f9 100644 (file)
--- a/scripts/scenario.py
+++ b/scripts/scenario.py
@@ -30,7 +30,8 @@ PNAME = [
 ]
 PNAME = [
     0,
 ]
 PNAME = [
     0,

-    "T3H",
+    "55",
+    #"no", "sc", "gz", "opt", "osp"

 ]
 
 def sort_dict(f):
 ]
 
 def sort_dict(f):