lfx += BCAR_DF * cos(pose[2])
lfx += BCAR_SD * cos(pose[2])
+ lf3x = pose[0]
+ lf3x += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lf3x += 2/3 * BCAR_DF * cos(pose[2])
+ lf3x += BCAR_SD * cos(pose[2])
+
lrx = pose[0]
lrx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
lrx += -BCAR_DR * cos(pose[2])
rfx += BCAR_DF * cos(pose[2])
rfx += BCAR_SD * cos(pose[2])
+ rf3x = pose[0]
+ rf3x += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rf3x += 2/3 * BCAR_DF * cos(pose[2])
+ rf3x += BCAR_SD * cos(pose[2])
+
lfy = pose[1]
lfy += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
lfy += BCAR_DF * sin(pose[2])
lfy += BCAR_SD * sin(pose[2])
+ lf3y = pose[1]
+ lf3y += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lf3y += 2/3 * BCAR_DF * sin(pose[2])
+ lf3y += BCAR_SD * sin(pose[2])
+
lry = pose[1]
lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
lry += -BCAR_DR * sin(pose[2])
rfy += BCAR_DF * sin(pose[2])
rfy += BCAR_SD * sin(pose[2])
- xcoords = (lfx - MINX, lrx - MINX, rrx - MINX, rfx - MINX)
- ycoords = (lfy - MINY, lry - MINY, rry - MINY, rfy - MINY)
- return (xcoords, ycoords)
+ rf3y = pose[1]
+ rf3y += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rf3y += 2/3 * BCAR_DF * sin(pose[2])
+ rf3y += BCAR_SD * sin(pose[2])
+
+ cfx = pose[0]
+ cfx += BCAR_DF * cos(pose[2])
+ cfx += BCAR_SD * cos(pose[2])
+
+ cfy = pose[1]
+ cfy += BCAR_DF * sin(pose[2])
+ cfy += BCAR_SD * sin(pose[2])
+
+ xcoords = (lfx, lrx, rrx, rfx, cfx, rf3x, lf3x, cfx, lfx)
+ ycoords = (lfy, lry, rry, rfy, cfy, rf3y, lf3y, cfy, lfy)
+ return ([x - MINX for x in xcoords], [y - MINY for y in ycoords])
if __name__ == "__main__":
+ sc2 = None
if (len(argv) == 2):
SCEN_FILE = argv[1]
+ elif (len(argv) == 3):
+ SCEN_FILE = argv[1]
+ SCEN_FILE2 = argv[2]
+ sc2 = get_scenario(SCEN_FILE2)
else:
SCEN_FILE = "sc.json"
scenario = get_scenario(SCEN_FILE)
- plt.rcParams["font.size"] = 24
+ # Font size to be approximately the same in the paper:
+ # - sc1-0: 16
+ # - sc3-2, Intro: 12
+ # - sc4-0+: 22
+ plt.rcParams["font.family"] = "cmr10"
+ plt.rcParams["font.size"] = 12
plt.rcParams['hatch.linewidth'] = 1.0
+ plt.rcParams['lines.linewidth'] = 1.0
fig = plt.figure()
# here subplot starts
ax = fig.add_subplot(111)
ax.set_aspect("equal")
- ax.set_title("{}".format(SCEN_FILE))
+ ax.set_title("Real-world parking scenario")
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
- ax.set_xlim([-8, 37]) # 30
- ax.set_ylim([-5, 55]) # 60
+ # For Possible Entry Points (Possible Entry Configurations) use:
+ #ax.set_xlim([35.6, 46.4])
+ #ax.set_ylim([2.9, 2.9 + 6.84])
+
+ # For Last Maneuver use:
+ #ax.set_xlim([38, 44])
+ #ax.set_ylim([3.1, 6.9])
+
+ # For Scenario 3-2 detail use:
+ #ax.set_xlim([0, 15])
+ #ax.set_ylim([35, 50])
+
+ # For Real-world parking scenario in Introduction section use:
+ #ax.set_xlim([32, 47.5])
+ #ax.set_ylim([2.4, 9.9])
+
+ # Set min and max to center the plot.
MINX = scenario["init"][0]
MINY = scenario["init"][1]
if "obst" in scenario and len(scenario["obst"]) > 0:
for o in scenario["obst"]:
+ if not o:
+ continue
for n in o:
if n[0] < MINX:
MINX = n[0]
if n[1] < MINY:
MINY = n[1]
+ # Plot all the nodes (if exists.)
if "nodes_x" in scenario and "nodes_y" in scenario:
plt.plot(
- scenario["nodes_x"] - MINX,
- scenario["nodes_y"] - MINY,
+ [x - MINX for x in scenario["nodes_x"]],
+ [y - MINY for y in scenario["nodes_y"]],
color="lightgray",
marker="o",
ms=2,
lw=0,
)
- if "obst" in scenario and len(scenario["obst"]) > 0:
+ # Plot all the steered2 nodes (if exists.)
+ if "steered2_x" in scenario and "steered2_y" in scenario:
+ plt.plot(
+ [x - MINX for x in scenario["steered2_x"]],
+ [y - MINY for y in scenario["steered2_y"]],
+ color="orange",
+ marker="o",
+ ms=2,
+ lw=0,
+ )
+ # Plot all the steered1 nodes (if exists.)
+ if "steered1_x" in scenario and "steered1_y" in scenario:
+ plt.plot(
+ [x - MINX for x in scenario["steered1_x"]],
+ [y - MINY for y in scenario["steered1_y"]],
+ color="blue",
+ marker="o",
+ ms=2,
+ lw=0,
+ )
+ # Plot obstacles, slot.
+ if "obst" in scenario and len(scenario["obst"]) > 0:
for o in scenario["obst"]:
- plt.plot(*plot_nodes(o), color="blue")
- if "path" in scenario and len(scenario["path"]) > 0:
- for n in scenario["path"]:
- plt.plot(*plot_car(n), color="peachpuff")
- plt.plot(*plot_nodes(scenario["path"]), color="orange")
+ if not o:
+ continue
+ plt.plot(*plot_nodes(o), color="blue", linestyle=":")
if "slot" in scenario and len(scenario["slot"]) > 0:
- plt.plot(*plot_nodes(scenario["slot"][0]), color="gray")
+ plt.plot(*plot_nodes(scenario["slot"][0]), color="black")
#for s in scenario["slot"]:
- # plt.plot(*plot_nodes(s), color="gray")
+ # plt.plot(*plot_nodes(s), color="black")
+
+ # If there are possible starts specified, you may print and plot them.
+ #if "starts" in scenario and len(scenario["starts"]) > 0:
+ # print("possible starts:")
+ # for p in scenario["starts"]:
+ # plt.plot(*p, color="red", marker="+", ms=12)
+ # print(" {}".format(p))
+
+ # For the Last Maneuver figure from the paper, use:
+ # - `init2` -- orange
+ #plt.plot(*plot_car(scenario["init2"]), color="orange")
+ #plt.plot(
+ # scenario["init2"][0] - MINX,
+ # scenario["init2"][1] - MINY,
+ # color="orange",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `goal2` -- orange
+ #plt.plot(*plot_car(scenario["goal2"]), color="orange")
+ #plt.plot(
+ # scenario["goal2"][0] - MINX,
+ # scenario["goal2"][1] - MINY,
+ # color="orange",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `goal2` -- middle (orange)
+ #plt.plot(*plot_car(scenario["goals"][0]), color="orange")
+ #plt.plot(
+ # scenario["goal2"][0] - MINX,
+ # scenario["goal2"][1] - MINY,
+ # color="orange",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `init1` -- green
+ #plt.plot(*plot_car(scenario["init1"]), color="green")
+ #plt.plot(
+ # scenario["init1"][0] - MINX,
+ # scenario["init1"][1] - MINY,
+ # color="green",
+ # #marker="+",
+ # ms=12
+ #)
+ # - `goal1` -- green
+ #plt.plot(*plot_car(scenario["goal1"]), color="green")
+ #plt.plot(
+ # scenario["goal1"][0] - MINX,
+ # scenario["goal1"][1] - MINY,
+ # color="green",
+ # #marker="+",
+ # ms=12
+ #)
+
+ # For the Possible Entry Configurations from the paper, use:
+ #if "inits" in scenario:
+ # #for i in scenario["inits"]:
+ # for j in range(0, len(scenario["inits"]), 110):
+ # i = scenario["inits"][j]
+ # print(i)
+ # if len(i) == 3:
+ # plt.plot(*plot_car(i), color="gray")
+ # plt.plot(
+ # i[0] - MINX,
+ # i[1] - MINY,
+ # color="gray",
+ # marker="+",
+ # ms=12,
+ # )
+ # #plt.plot(*plot_car(scenario["inits"][0]), color="magenta")
+ # #plt.plot(
+ # # scenario["inits"][0][0] - MINX,
+ # # scenario["inits"][0][1] - MINY,
+ # # color="magenta",
+ # # marker="+",
+ # # ms=12,
+ # #)
+
+ # Plot `init`, `entry`, and `goal` configurations.
if "init" in scenario and len(scenario["init"]) == 3:
plt.plot(*plot_car(scenario["init"]), color="red")
plt.plot(
marker="+",
ms=12
)
- if "goals" in scenario:
- for i in scenario["goals"]:
- if len(i) == 3:
- plt.plot(*plot_car(i), color="darkseagreen")
- plt.plot(i[0] - MINX, i[1] - MINY, color="darkseagreen", 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 "goal" in scenario and len(scenario["goal"]) == 3:
plt.plot(*plot_car(scenario["goal"]), color="green")
plt.plot(
marker="+",
ms=12
)
- if "starts" in scenario and len(scenario["starts"]) > 0:
- print("possible starts:")
- for p in scenario["starts"]:
- plt.plot(*p, color="red", marker="+", ms=12)
- print(" {}".format(p))
+
+ # Plot `path` and `max_path`.
+ if sc2 and "path" in sc2 and len(sc2["path"]) > 0:
+ plt.plot(*plot_nodes(sc2["path"]), color="orange")
+ if "path" in scenario and len(scenario["path"]) > 0:
+ plt.plot(*plot_nodes(scenario["path"]), color="green")
+
+ # The `scenario` may also include:
+ # - `last` -- not sure what this is, see the source code. Maybe overlaps
+ # with the `goal`.
+ # - `last1` -- used to demonstrate In-Slot Planner (was Parking Slot
+ # Planner (PSP.))
+ # - `last2` -- used to demonstrate In-Slot Planner (was Parking Slot
+ # Planner (PSP.))
+ # - `max_orig_path` -- maximum original path. I used this when comparing
+ # original paths but I had to copy the `max_orig_path` by hand from
+ # different scenario result.
+ # - `orig_path` -- the path before the optimization.
+ # - `max_path` -- the maximum path after optimization. Must be copied by
+ # hand.
+ # - `path` -- optimized path of the scenario.
handles, labels = ax.get_legend_handles_labels()
- plt.show()
+ # Uncommnent the following line and comment the plt.show() to store to the
+ # file.
+ plt.savefig("out.pdf", bbox_inches="tight")
plt.close(fig)
projects / hubacji1 / iamcar2.git / blobdiff