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])
+
+def plot_car_corners(pose):
+ """Return ``xcoords``, ``ycoords`` arrays of car frame corners.
+
+ Keyword arguments:
+ pose -- The pose of a car.
+ """
+ lfx = pose[0]
+ lfx += (BCAR_W / 2.0) * cos(pose[2] + pi / 2.0)
+ lfx += BCAR_DF * cos(pose[2])
+ lfx += 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])
+ lrx += -BCAR_SD * cos(pose[2])
+
+ rrx = pose[0]
+ rrx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rrx += -BCAR_DR * cos(pose[2])
+ rrx += -BCAR_SD * cos(pose[2])
+
+ rfx = pose[0]
+ rfx += (BCAR_W / 2.0) * cos(pose[2] - pi / 2.0)
+ rfx += BCAR_DF * cos(pose[2])
+ rfx += 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])
+
+ lry = pose[1]
+ lry += (BCAR_W / 2.0) * sin(pose[2] + pi / 2.0)
+ lry += -BCAR_DR * sin(pose[2])
+ lry += -BCAR_SD * sin(pose[2])
+
+ rry = pose[1]
+ rry += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rry += -BCAR_DR * sin(pose[2])
+ rry += -BCAR_SD * sin(pose[2])
+
+ rfy = pose[1]
+ rfy += (BCAR_W / 2.0) * sin(pose[2] - pi / 2.0)
+ rfy += BCAR_DF * sin(pose[2])
+ rfy += BCAR_SD * sin(pose[2])
+
+ xcoords = (lfx, lrx, rrx, rfx)
+ ycoords = (lfy, lry, rry, rfy)
+ 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
- plt.rcParams['hatch.linewidth'] = 1.0
+ # Font size to be approximately the same in the paper:
+ # - sc1-0: 16
+ # - sc3-2, Intro: 12
+ # - sc4-0+: 22
+ plt.rc('axes', unicode_minus=False)
+ plt.rcParams["font.family"] = "cmr10"
+ plt.rcParams["font.size"] = 12
+ plt.rcParams['hatch.linewidth'] = 0.5
+ 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([34, 53]) # 19
- #ax.set_ylim([-1, 16]) # 17
- #ax.set_xlim([28, 53]) # 25
- #ax.set_ylim([-1, 21]) # 22
+ # For Possible Entry Points (Possible Entry Configurations) use:
+ #ax.set_xlim([37.6, 45.6])
+ #ax.set_ylim([2.4, 8.0])
+ #ax.set_title("Possible Entry Configurations")
+
+ # 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([33, 48.5])
+ #ax.set_ylim([2.4, 10.9])
+ #ax.text(34, 9.2, "Initial configuration", color="red")
+ #ax.text(43.5, 8.5, "Final path", color="blue")
+ #ax.text(48.25, 5.5, "Entry\nconfigurations", color="orange", ha="right")
+ #ax.text(38, 3.8, "Parking\nslot", color="blue", ha="right", backgroundcolor="white")
+ #ax.text(35.2, 5.5, "Goal configuration", color="green")
+
+ # 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 "last" in scenario and len(scenario["last"]) > 0:
- for n in scenario["last"]:
- plt.plot(*plot_car(n), color="peachpuff")
- 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 "last" in scenario and len(scenario["last"]) > 0:
- plt.plot(*plot_nodes(scenario["last"]), color="orange")
+ if not o:
+ continue
+ ax.fill(*plot_nodes(o), color="black", fill=False, hatch="//")
if "slot" in scenario and len(scenario["slot"]) > 0:
- plt.plot(*plot_nodes(scenario["slot"][0]), color="gray")
- for s in scenario["slot"]:
- pass#plt.plot(*plot_nodes(s), color="gray")
+ plt.plot(*plot_nodes(scenario["slot"][0]), color="blue", linewidth=2)
+ #for s in scenario["slot"]:
+ # plt.plot(*plot_nodes(s), color="black")
+
+ # For the Possible Entry Configurations from the paper, use:
+ if False and "inits" in scenario:
+ max_i = len(scenario["inits"]) - 1
+ ii = 0
+ i = scenario["inits"][ii]
+ plt.plot(*plot_car(i), color="gray")
+ plt.plot(i[0] - MINX, i[1] - MINY, color="gray", marker="+", ms=12)
+ ii = int(max_i / 4)
+ i = scenario["inits"][ii]
+ plt.plot(*plot_car(i), color="gray")
+ plt.plot(i[0] - MINX, i[1] - MINY, color="gray", marker="+", ms=12)
+ ii = int(max_i / 2)
+ i = scenario["inits"][ii]
+ plt.plot(*plot_car(i), color="gray")
+ plt.plot(i[0] - MINX, i[1] - MINY, color="gray", marker="+", ms=12)
+ ii = int(max_i * 3/4)
+ i = scenario["inits"][ii]
+ plt.plot(*plot_car(i), color="gray")
+ plt.plot(i[0] - MINX, i[1] - MINY, color="gray", marker="+", ms=12)
+ ii = max_i
+ i = scenario["inits"][ii]
+ plt.plot(*plot_car(i), color="gray")
+ plt.plot(i[0] - MINX, i[1] - MINY, color="gray", 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 "goal" in scenario and len(scenario["goal"]) == 3:
- # plot entry point
- plt.plot(*plot_car(scenario["goal"]), color="magenta")
+ 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["goal"][0] - MINX,
- scenario["goal"][1] - MINY,
+ 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
)
- # plot gc
- if "last" in scenario:
- plt.plot(*plot_car(scenario["last"][0]), color="green")
+ if "goal" in scenario and len(scenario["goal"]) == 3:
+ plt.plot(*plot_car(scenario["goal"]), color="green")
plt.plot(
- scenario["last"][0][0] - MINX,
- scenario["last"][0][1] - MINY,
+ scenario["goal"][0] - MINX,
+ scenario["goal"][1] - MINY,
color="green",
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 "orig_path" in scenario and len(scenario["orig_path"]) > 0:
+ plt.plot(
+ *plot_nodes(scenario["orig_path"]),
+ color="blue",
+ linewidth=2,
+ linestyle="dotted",
+ )
+ if "path" in scenario and len(scenario["path"]) > 0:
+ plt.plot(
+ *plot_nodes(scenario["path"]),
+ color="blue",
+ linewidth=2,
+ )
+ for p in scenario["path"]:
+ #plt.plot(*plot_car(p), color="green")
+ pass
+ #cc = plot_car_corners(p)
+ #plt.plot(cc[0][0], cc[1][0], color="red", marker=".", ms=1)
+ #plt.plot(cc[0][1], cc[1][1], color="red", marker=".", ms=1)
+ #plt.plot(cc[0][2], cc[1][2], color="red", marker=".", ms=1)
+ #plt.plot(cc[0][3], cc[1][3], color="red", marker=".", ms=1)
+
+ # 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
+ #)
+
+ # 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