# -*- coding: utf-8 -*-
"""This scipt loads scenario and result trajectory from files and plots it."""
from json import loads
-from math import cos, pi, sin
+from math import copysign, cos, pi, sin
from matplotlib import pyplot as plt
-from sys import argv
+from sys import argv, exit
-HEIGHT = 1.418
-LENGTH = 4.970
+sign = lambda x: copysign(1, x)
+
+HEIGHT = 1.450
+LENGTH = 3.760
SAFETY_DIST = 0
-WHEEL_BASE = 2.9
-WIDTH = 1.931
+WHEEL_BASE = 2.450
+WIDTH = 1.625
-SCEN_FILE = argv[1]
-TRAJ_FILE = argv[2]
PLOT = {
"enable" : True,
"node" : False,
"frame" : False,
"path" : False,
"traj" : True,
+ "slot" : True,
}
COLOR = {
"node": "lightgrey",
"start": "violet",
"goal": "red",
"sample": "magenta",
- "path": "lightgrey",
+ "path": "grey",
"trajectory": "blue",
"trajectory-frame": "lightblue",
"obstacle": "black",
"log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
+ "slot": "red"
}
def car_frame(pose):
Keyword arguments:
pose -- The pose of a car.
"""
+ dr = (LENGTH - WHEEL_BASE) / 2
+ df = LENGTH - dr
+
lfx = pose[0]
lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
- lfx += WHEEL_BASE * cos(pose[2])
+ lfx += df * cos(pose[2])
lfx += SAFETY_DIST * cos(pose[2])
lrx = pose[0]
lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
+ lrx += -dr * cos(pose[2])
lrx += -SAFETY_DIST * cos(pose[2])
rrx = pose[0]
rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
+ rrx += -dr * cos(pose[2])
rrx += -SAFETY_DIST * cos(pose[2])
rfx = pose[0]
rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
- rfx += WHEEL_BASE * cos(pose[2])
+ rfx += df * cos(pose[2])
rfx += SAFETY_DIST * cos(pose[2])
lfy = pose[1]
lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
- lfy += WHEEL_BASE * sin(pose[2])
+ lfy += df * sin(pose[2])
lfy += SAFETY_DIST * sin(pose[2])
lry = pose[1]
lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
+ lry += -dr * sin(pose[2])
lry += -SAFETY_DIST * sin(pose[2])
rry = pose[1]
rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
+ rry += -dr * sin(pose[2])
rry += -SAFETY_DIST * sin(pose[2])
rfy = pose[1]
rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
- rfy += WHEEL_BASE * sin(pose[2])
+ rfy += df * sin(pose[2])
rfy += SAFETY_DIST * sin(pose[2])
xcoords = (lfx, lrx, rrx, rfx)
pass
if __name__ == "__main__":
+ SCEN_FILE = argv[1]
+
s = load_scenario(SCEN_FILE)
try:
+ TRAJ_FILE = argv[2]
t = load_trajectory(TRAJ_FILE) # fixed to trajectories
except:
- pass
+ t = {"elap": 0}
plt.rcParams["font.size"] = 24
+ plt.rcParams['hatch.linewidth'] = 1.0
fig = plt.figure()
- ax = fig.add_subplot(111)
+
+ ################
+ ## 1st subplot
+
+ if "edge" in t.keys():
+ ax = fig.add_subplot(121)
+ else:
+ ax = fig.add_subplot(111)
ax.set_aspect("equal")
- ax.set_title("SCENARIO")
- ax.set_xlabel("Longitudinal direction [m]")
- ax.set_ylabel("Lateral direction [m]")
+ ax.set_title("Final path")
+ ax.set_xlabel("x [m]")
+ ax.set_ylabel("y [m]")
- # plot here
for o in s["obst"]:
try:
plt.plot(*plot_nodes(o["segment"]), color="black")
try:
ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
o["circle"][2],
- color="black", fill=False))
+ color="black", fill=False, hatch="//"))
except:
pass
if PLOT["node"]:
marker=".", linestyle = "None")
except:
print("No RRTNode")
- if PLOT["edge"]:
+ if False:
try:
- for e in t["edge"]:
- plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
- color=COLOR["edge"])
+ for edges in t["edge"]:
+ for e in edges:
+ plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
+ color=COLOR["edge"])
except:
print("No edges")
if PLOT["sample"]:
try:
- plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
- marker=".", linestyle = "None")
+ if PLOT["frame"]:
+ for i in t["samp"]:
+ plt.plot(*car_frame(i), color=COLOR["sample"])
+ else:
+ plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
+ marker=".", linestyle = "None")
except:
print("No RRTSample")
if PLOT["path"]:
except:
print("No path")
if PLOT["traj"]:
+ try:
+ for traj in range(len(t["traj"])):
+ if PLOT["frame"]:
+ for i in t["traj"][traj]:
+ plt.plot(*car_frame(i), color=COLOR["log"][traj],
+ label=t["cost"][traj], lw=1)
+ else:
+ try:
+ plt.plot(
+ *plot_nodes(t["traj"][traj]),
+ color=COLOR["log"][traj],
+ label=t["cost"][traj])
+ except:
+ plt.plot(
+ *plot_nodes(t["traj"][traj]),
+ color="black",
+ label=t["cost"][traj])
+ except:
+ print("No trajectory")
+
+ if PLOT["slot"]:
+ try:
+ plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
+ except:
+ print("No slot")
+
+ try: # init
+ plt.plot(*car_frame(t["init"]), color="red", lw=2)
+ plt.plot(*plot_nodes([t["init"]]), color="red", marker="+", ms=12)
+ except:
+ plt.plot(*car_frame(s["init"]), color="red", lw=2)
+ plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
+ try: # goal
+ plt.plot(*plot_nodes([t["goal"]]), color="red", marker="+", ms=12)
+ plt.plot(*car_frame(t["goal"]), color="red", lw=2)
+ except:
+ plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
+ plt.plot(*car_frame(s["goal"]), color="red", lw=2)
+ try: # middle
+ plt.plot(*car_frame(t["midd"]), color="red", lw=2)
+ plt.plot(*plot_nodes([t["midd"]]), color="red", marker="+", ms=12)
+ except:
+ pass
+
+ #plt.fill_between(
+ # [-2.7, 3.25, 3.25, -2.7],
+ # [-0.5, -0.5, 0, 0],
+ # [26, 26, 26.5, 26.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
+ # [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [3.25, 3.25, 2.75, 2.75, 3.25],
+ # [-0.5, 26.5, 26.5, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+
+ #plt.text(1, 0.2, s="1", color="red")
+ #plt.text(2, 12.2, s="2", color="red")
+ #plt.text(3.6, 10, s="3", color="black")
+ #plt.text(-0.5, 14.3, s="4", color="black")
+ #plt.text(3, 18, s="5", color="blue")
+ #plt.text(-4.7, 0.8, s="6", color="orange")
+ #plt.text(-2, 11, s="7", color="gray")
+
+ handles, labels = ax.get_legend_handles_labels()
+
+ if not "edge" in t.keys():
+ plt.show()
+ plt.close(fig)
+ exit(0)
+
+ ################
+ ## 2nd subplot
+
+ ax = fig.add_subplot(122)
+ ax.set_aspect("equal")
+ ax.set_title("All edges")
+ ax.set_xlabel("x [m]")
+ ax.set_ylabel("y [m]")
+
+ for o in s["obst"]:
+ try:
+ plt.plot(*plot_nodes(o["segment"]), color="black")
+ except:
+ pass
+ try:
+ ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
+ o["circle"][2],
+ color="black", fill=False, hatch="//"))
+ except:
+ pass
+ if PLOT["node"]:
+ try:
+ plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
+ marker=".", linestyle = "None")
+ except:
+ print("No RRTNode")
+ if True:
+ try:
+ for edges in t["edge"]:
+ for e in edges:
+ plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
+ color=COLOR["edge"], lw=1)
+ except:
+ print("No edges")
+ if PLOT["sample"]:
+ try:
+ if PLOT["frame"]:
+ for i in t["samp"]:
+ plt.plot(*car_frame(i), color=COLOR["sample"])
+ else:
+ plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
+ marker=".", linestyle = "None")
+ except:
+ print("No RRTSample")
+ if PLOT["path"]:
+ try:
+ for path in range(len(t["path"])):
+ plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
+ except:
+ print("No path")
+ if False:
try:
for traj in range(len(t["traj"])):
if PLOT["frame"]:
label=t["cost"][traj])
except:
print("No trajectory")
- plt.plot(*plot_nodes([s["init"]]), color=COLOR["start"], marker=".")
- plt.plot(*plot_nodes([s["goal"]]), color=COLOR["goal"], marker=".")
- # end plot here
+
+ if PLOT["slot"]:
+ try:
+ plt.plot(*plot_nodes(s["slot"]["polygon"]), color=COLOR["slot"])
+ except:
+ print("No slot")
+
+ plt.plot(*car_frame(s["init"]), color="red", lw=2)
+ plt.plot(*car_frame(s["goal"]), color="red", lw=2)
+ plt.plot(*plot_nodes([s["init"]]), color="red", marker="+", ms=12)
+ plt.plot(*plot_nodes([s["goal"]]), color="red", marker="+", ms=12)
+
+ #plt.fill_between(
+ # [-2.7, 3.25, 3.25, -2.7],
+ # [-0.5, -0.5, 0, 0],
+ # [26, 26, 26.5, 26.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [-2.7, -2.7, -0, -0, -2.2, -2.2, 0, 0, -2.7],
+ # [-0.5, 26.5, 26.5, 19.5, 19.5, 13, 13, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
+ #plt.fill_between(
+ # [3.25, 3.25, 2.75, 2.75, 3.25],
+ # [-0.5, 26.5, 26.5, -0.5, -0.5],
+ # facecolor="none", hatch="//", edgecolor="black", linewidth=0)
handles, labels = ax.get_legend_handles_labels()
- lgd = ax.legend(handles, labels, loc="upper center",
- bbox_to_anchor=(0.5, -0.11), title="Cost")
+
+ # END OF SUBPLOTS
plt.show()
- #plt.savefig("{}.png".format(argv[2]), bbox_extra_artists=(lgd,),
- # bbox_inches='tight')
plt.close(fig)
projects / hubacji1 / iamcar.git / blobdiff