Improve, tune, change plot json scenario script

diff --git a/scripts/plot_json_objects_scenario.py b/scripts/plot_json_objects_scenario.py
index d6599f42a672da25fc760369ddfa2b2bb805752c..cf9b5787b43e0d27d0785e5d416d268a63873d13 100644 (file)
--- a/scripts/plot_json_objects_scenario.py
+++ b/scripts/plot_json_objects_scenario.py
@@ -184,7 +184,7 @@ if __name__ == "__main__":
     #   - sc4-0+: 22
     plt.rc('axes', unicode_minus=False)
     plt.rcParams["font.family"] = "cmr10"
-    plt.rcParams["font.size"] = 12
+    plt.rcParams["font.size"] = 22
     plt.rcParams['hatch.linewidth'] = 0.5
     plt.rcParams['lines.linewidth'] = 1.0
     fig = plt.figure()
@@ -193,10 +193,37 @@ if __name__ == "__main__":
     ax = fig.add_subplot(111)
     ax.set_aspect("equal")
     ax.set_title("Real-world parking scenario")
+    ax.set_title("Simple parking scenario")
     ax.set_xlabel("x [m]")
     ax.set_ylabel("y [m]")
+    # For stage, comment upper, uncomment following:
+    #plt.xticks([])
+    #plt.yticks([])
+
+    # sps 99
+    #ax.set_xlim([4, 32]) # 28
+    #ax.set_ylim([7, 29]) # 22
+    # sps 551
+    #ax.set_xlim([-1, 27]) # 28
+    #ax.set_ylim([-1, 21]) # 22
+    # sps 591
+    #ax.set_xlim([-9.9, 18.1]) # 28
+    #ax.set_ylim([-4.9, 17.1]) # 22
+    # sps 783
+    #ax.set_xlim([4, 32]) # 28
+    #ax.set_ylim([-4, 18]) # 22
+    # sps 510
+    #ax.set_xlim([-7, 21]) # 28
+    #ax.set_ylim([7, 29]) # 22
+    # sps 501
+    #ax.set_xlim([-7, 21]) # 28
+    #ax.set_ylim([2, 24]) # 22
+    # sps 167
+    #ax.set_xlim([-1, 27]) # 28
+    #ax.set_ylim([-1, 21]) # 22
 
     # For Possible Entry Points (Possible Entry Configurations) use:
+    #plt.rcParams["font.size"] = 26
     #ax.set_xlim([37.6, 45.6])
     #ax.set_ylim([2.4, 8.5])
     #ax.set_title("Possible configurations")
@@ -214,13 +241,13 @@ if __name__ == "__main__":
     # For Scenario 4-0 detail use:
     # - font size 22
     #ax.set_title("Scenario 2")
-    #ax.set_xlim([32, 51]) # w=19
-    #ax.set_ylim([1, 19]) # h=18
+    #ax.set_xlim([32, 53]) # w=19
+    #ax.set_ylim([0.5, 19.5]) # h=18
 
     # For Scenario 4-1 detail use:
     # - font size 22
     #ax.set_title("Scenario 3")
-    #ax.set_xlim([32, 47]) # w=15
+    #ax.set_xlim([32.5, 48.5]) # w=16
     #ax.set_ylim([1, 16]) # h=15
 
     # For Scenario 5-1 detail use:
@@ -232,14 +259,20 @@ if __name__ == "__main__":
     # For Scenario 4-1-{0,14} detail use:
     # - font size 22
     #ax.set_title("Scenario 5") # Scenario 6
-    #ax.set_xlim([31, 47.5]) # w=16.5
+    #ax.set_xlim([32.5, 48.5]) # w=16
     #ax.set_ylim([1, 16.3]) # h=15.3
 
-    # For Scenario 5-3-{35,} detail use:
+    # For Scenario 5-3-34 detail use:
     # - font size 22
     #ax.set_title("Scenario 7") # Scenario 8
     #ax.set_xlim([5.5, 27.5])
-    #ax.set_ylim([8.1, 28])
+    #ax.set_ylim([15.1, 35])
+
+    # For Scenario 5-3-29 detail use:
+    # - font size 22
+    #ax.set_title("Scenario 8")
+    #ax.set_xlim([5.5, 27.5])
+    #ax.set_ylim([10.1, 30])
 
     # For Real-world parking scenario in Introduction section use:
     #ax.set_title("Real-world parking scenario with artificial obstacle")
@@ -252,7 +285,7 @@ if __name__ == "__main__":
     #ax.text(35.2, 5.5, "Goal configuration", color="green")
 
     # For scenario 5-3
-    #ax.set_title("Guessed goal")
+    #ax.set_title("Computed goal")
     #ax.set_xlim([6.8, 16.2])
     #ax.set_ylim([15, 20])
 
@@ -265,6 +298,8 @@ if __name__ == "__main__":
     # Set min and max to center the plot.
     MINX = scenario["init"][0]
     MINY = scenario["init"][1]
+    MAXX = scenario["init"][0]
+    MAXY = scenario["init"][1]
     if "obst" in scenario and  len(scenario["obst"]) > 0:
         for o in scenario["obst"]:
             if not o:
@@ -274,6 +309,11 @@ if __name__ == "__main__":
                     MINX = n[0]
                 if n[1] < MINY:
                     MINY = n[1]
+                if n[0] > MAXX:
+                    MAXX = n[0]
+                if n[1] > MAXY:
+                    MAXY = n[1]
+    print("w: {}, h: {}".format(abs(MAXX - MINX), abs(MAXY - MINY)))
     #MINY = -25
     #MINX = -10
     c1 = plt.Circle(
@@ -340,7 +380,7 @@ if __name__ == "__main__":
         for o in scenario["obst"]:
             if not o:
                 continue
-            ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//")
+            ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//") #fill=True for stage
     if "slot" in scenario and len(scenario["slot"]) > 0:
         plt.plot(*plot_nodes(scenario["slot"]), color="blue", linewidth=1)
         #for s in scenario["slot"]:
@@ -374,7 +414,7 @@ if __name__ == "__main__":
         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
     inits = "inits"
     inits_c = "orange"
-    if False and inits in scenario:
+    if True and inits in scenario:
         max_i = len(scenario[inits]) - 1
         ii = 0
         i = scenario[inits][ii]
@@ -396,6 +436,32 @@ if __name__ == "__main__":
         i = scenario[inits][ii]
         plt.plot(*plot_car(i), color=inits_c)
         plt.plot(i[0] - MINX, i[1] - MINY, color=inits_c, marker="+", ms=12)
+    # Possible/Candidate entries
+    #ax.text(
+    #    44.95,
+    #    5.5,
+    #    "p",
+    #    color="blue",
+    #    fontfamily="serif",
+    #    fontstyle="italic",
+    #)
+    #ax.arrow(
+    #    scenario["slot"][-1][0] - MINX,
+    #    scenario["slot"][-1][1] - MINY,
+    #    -2,
+    #    0,
+    #    width=0.05,
+    #    color="blue",
+    #    zorder=2,
+    #)
+    #ax.text(
+    #    scenario["slot"][-1][0] - MINX - 2,
+    #    5.5,
+    #    "δ",
+    #    color="blue",
+    #    fontfamily="serif",
+    #    fontstyle="italic",
+    #)
 
     # Plot `init`, `entry`, and `goal` configurations.
     if "init" in scenario and len(scenario["init"]) == 3:
@@ -407,47 +473,47 @@ if __name__ == "__main__":
             marker="+",
             ms=12
         )
-    if "init" in scenario and len(scenario["init"]) == 4:
-        plt.plot(*plot_car(scenario["init"]), color="red")
-        scenario["init"][2] = scenario["init"][3]
-        plt.plot(*plot_car(scenario["init"]), color="red")
-        plt.plot(
-            scenario["init"][0] - MINX,
-            scenario["init"][1] - MINY,
-            color="red",
-            marker="+",
-            ms=12
-        )
-    if "entries" in scenario:
-        for e in scenario["entries"]:
-            plt.plot(*plot_car(e), color="orange")
-            plt.plot(
-                e[0] - MINX,
-                e[1] - MINY,
-                color="orange",
-                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"]) == 4:
-        esc = scenario["entry"]
-        plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
-        plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
-        plt.plot(
-            scenario["entry"][0] - MINX,
-            scenario["entry"][1] - MINY,
-            color="magenta",
-            marker="+",
-            ms=12
-        )
+    #if "init" in scenario and len(scenario["init"]) == 4:
+    #    plt.plot(*plot_car(scenario["init"]), color="red")
+    #    scenario["init"][2] = scenario["init"][3]
+    #    plt.plot(*plot_car(scenario["init"]), color="red")
+    #    plt.plot(
+    #        scenario["init"][0] - MINX,
+    #        scenario["init"][1] - MINY,
+    #        color="red",
+    #        marker="+",
+    #        ms=12
+    #    )
+    #if "entries" in scenario:
+    #    for e in scenario["entries"]:
+    #        plt.plot(*plot_car(e), color="orange")
+    #        plt.plot(
+    #            e[0] - MINX,
+    #            e[1] - MINY,
+    #            color="orange",
+    #            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"]) == 4:
+    #    esc = scenario["entry"]
+    #    plt.plot(*plot_car([esc[0], esc[1], esc[2]]), color="magenta")
+    #    plt.plot(*plot_car([esc[0], esc[1], esc[3]]), color="magenta")
+    #    plt.plot(
+    #        scenario["entry"][0] - MINX,
+    #        scenario["entry"][1] - MINY,
+    #        color="magenta",
+    #        marker="+",
+    #        ms=12
+    #    )
     if "goal" in scenario:
         if len(scenario["goal"]) == 3:
             plt.plot(*plot_car(scenario["goal"]), color="green")
@@ -458,18 +524,18 @@ if __name__ == "__main__":
                 marker="+",
                 ms=12
             )
-        elif len(scenario["goal"]) == 4:
-            ctp = scenario["goal"]
-            plt.plot(*plot_car(scenario["goal"]), color="green")
-            ctp[2] = ctp[3]
-            plt.plot(*plot_car(scenario["goal"]), color="green")
-            plt.plot(
-                scenario["goal"][0] - MINX,
-                scenario["goal"][1] - MINY,
-                color="green",
-                marker="+",
-                ms=12
-            )
+    #    elif len(scenario["goal"]) == 4:
+    #        ctp = scenario["goal"]
+    #        plt.plot(*plot_car(scenario["goal"]), color="green")
+    #        ctp[2] = ctp[3]
+    #        plt.plot(*plot_car(scenario["goal"]), color="green")
+    #        plt.plot(
+    #            scenario["goal"][0] - MINX,
+    #            scenario["goal"][1] - MINY,
+    #            color="green",
+    #            marker="+",
+    #            ms=12
+    #        )
 
     # Plot `path` and `max_path`.
     if (sc2 and "opath" in sc2 and isinstance(sc2["opath"], list)