1 # -*- coding: utf-8 -*-
2 """This scipt loads scenario and result trajectory from files and plots it."""
4 from math import cos, pi, sin
5 from matplotlib import pyplot as plt
33 "trajectory-frame": "lightblue",
35 "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"),
39 """Return `xcoords`, `ycoords` arrays of car frame.
42 pose -- The pose of a car.
45 lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
46 lfx += WHEEL_BASE * cos(pose[2])
47 lfx += SAFETY_DIST * cos(pose[2])
50 lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0)
51 lrx += -SAFETY_DIST * cos(pose[2])
54 rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
55 rrx += -SAFETY_DIST * cos(pose[2])
58 rfx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0)
59 rfx += WHEEL_BASE * cos(pose[2])
60 rfx += SAFETY_DIST * cos(pose[2])
63 lfy += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
64 lfy += WHEEL_BASE * sin(pose[2])
65 lfy += SAFETY_DIST * sin(pose[2])
68 lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0)
69 lry += -SAFETY_DIST * sin(pose[2])
72 rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
73 rry += -SAFETY_DIST * sin(pose[2])
76 rfy += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0)
77 rfy += WHEEL_BASE * sin(pose[2])
78 rfy += SAFETY_DIST * sin(pose[2])
80 xcoords = (lfx, lrx, rrx, rfx)
81 ycoords = (lfy, lry, rry, rfy)
82 return (xcoords, ycoords)
84 def load_scenario(fname):
85 """Load scenario from file."""
87 raise ValueError("File name as argument needed")
88 with open(fname, "r") as f:
89 scenario = loads(f.read())
92 def load_trajectory(fname):
93 """Load trajectory from file."""
95 raise ValueError("File name as argument needed")
96 with open(fname, "r") as f:
97 trajectory = loads(f.read())
100 def plot_nodes(nodes=[]):
101 """Return xcoords, ycoords of nodes to plot.
104 nodes -- The list of nodes to plot.
111 return (xcoords, ycoords)
113 def plot_segments(segments=[]):
114 """Return xcoords, ycoords of segments to plot.
117 segments -- The list of segments to plot.
121 if __name__ == "__main__":
122 s = load_scenario(SCEN_FILE)
124 t = load_trajectory(TRAJ_FILE) # fixed to trajectories
129 ax = fig.add_subplot(111)
130 ax.set_aspect("equal")
131 ax.set_title("SCENARIO")
132 ax.set_xlabel("Longitudinal direction [m]")
133 ax.set_ylabel("Lateral direction [m]")
138 plt.plot(*plot_nodes(o["segment"]), color="black")
142 ax.add_artist(plt.Circle((o["circle"][0], o["circle"][1]),
144 color="black", fill=False))
149 plt.plot(*plot_nodes(t["node"]), color=COLOR["node"],
150 marker=".", linestyle = "None")
156 plt.plot([e[0][0], e[1][0]], [e[0][1], e[1][1]],
162 plt.plot(*plot_nodes(t["samp"]), color=COLOR["sample"],
163 marker=".", linestyle = "None")
165 print("No RRTSample")
168 for path in range(len(t["path"])):
169 plt.plot(*plot_nodes(t["path"][path]), color=COLOR["path"])
174 for traj in range(len(t["traj"])):
176 for i in t["traj"][traj]:
177 plt.plot(*car_frame(i), color=COLOR["log"][traj],
178 label=t["cost"][traj])
182 *plot_nodes(t["traj"][traj]),
183 color=COLOR["log"][traj],
184 label=t["cost"][traj])
187 *plot_nodes(t["traj"][traj]),
189 label=t["cost"][traj])
191 print("No trajectory")
192 plt.plot(*plot_nodes([s["init"]]), color=COLOR["start"], marker=".")
193 plt.plot(*plot_nodes([s["goal"]]), color=COLOR["goal"], marker=".")
196 ax.legend(loc='upper center', bbox_to_anchor=(0.5, -0.05), title="Cost")
198 #plt.savefig("plot.pdf".format(p, p_id))