From ba833b324ce29c7e4a3a43ab6a6e4c1cf39af71e Mon Sep 17 00:00:00 2001 From: Jiri Hubacek Date: Wed, 4 Jul 2018 11:46:40 +0200 Subject: [PATCH] Add plot python script --- CHANGELOG.md | 1 + plot.py | 195 +++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 196 insertions(+) create mode 100644 plot.py diff --git a/CHANGELOG.md b/CHANGELOG.md index 1866ccf..3f40e8f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,7 @@ The format is based on [Keep a Changelog][] and this project adheres to - Closed loop controller with PI for speed and PurePursuit based on [Coulter1992] paper for steering. - Testing parking scenarios. +- Plot python script. ### Changed - Adding JSON ouput for edges, samples. diff --git a/plot.py b/plot.py new file mode 100644 index 0000000..9006a26 --- /dev/null +++ b/plot.py @@ -0,0 +1,195 @@ +# -*- 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 matplotlib import pyplot as plt +from sys import argv + +HEIGHT = 1.418 +LENGTH = 4.970 +SAFETY_DIST = 0 +WHEEL_BASE = 2.9 +WIDTH = 1.931 + +SCEN_FILE = argv[1] +TRAJ_FILE = argv[2] +PLOT = { + "enable" : True, + "node" : False, + "edge" : True, + "sample" : False, + "frame" : False, + "path" : False, + "traj" : True, + } +COLOR = { + "node": "lightgrey", + "edge": "lightgrey", + "start": "violet", + "goal": "red", + "sample": "magenta", + "path": "lightgrey", + "trajectory": "blue", + "trajectory-frame": "lightblue", + "obstacle": "black", + "log": ("gold", "orange", "blueviolet", "blue", "navy", "black"), + } + +def car_frame(pose): + """Return `xcoords`, `ycoords` arrays of car frame. + + Keyword arguments: + pose -- The pose of a car. + """ + lfx = pose[0] + lfx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0) + lfx += WHEEL_BASE * cos(pose[2]) + lfx += SAFETY_DIST * cos(pose[2]) + + lrx = pose[0] + lrx += (WIDTH / 2.0) * cos(pose[2] + pi / 2.0) + lrx += -SAFETY_DIST * cos(pose[2]) + + rrx = pose[0] + rrx += (WIDTH / 2.0) * cos(pose[2] - pi / 2.0) + 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 += 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 += SAFETY_DIST * sin(pose[2]) + + lry = pose[1] + lry += (WIDTH / 2.0) * sin(pose[2] + pi / 2.0) + lry += -SAFETY_DIST * sin(pose[2]) + + rry = pose[1] + rry += (WIDTH / 2.0) * sin(pose[2] - pi / 2.0) + 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 += SAFETY_DIST * sin(pose[2]) + + xcoords = (lfx, lrx, rrx, rfx) + ycoords = (lfy, lry, rry, rfy) + return (xcoords, ycoords) + +def load_scenario(fname): + """Load scenario from file.""" + if fname is None: + raise ValueError("File name as argument needed") + with open(fname, "r") as f: + scenario = loads(f.read()) + return scenario + +def load_trajectory(fname): + """Load trajectory from file.""" + if fname is None: + raise ValueError("File name as argument needed") + with open(fname, "r") as f: + trajectory = loads(f.read()) + return trajectory + +def plot_nodes(nodes=[]): + """Return xcoords, ycoords of nodes to plot. + + Keyword arguments: + nodes -- The list of nodes to plot. + """ + xcoords = [] + ycoords = [] + for n in nodes: + xcoords.append(n[0]) + ycoords.append(n[1]) + return (xcoords, ycoords) + +def plot_segments(segments=[]): + """Return xcoords, ycoords of segments to plot. + + Keyword arguments: + segments -- The list of segments to plot. + """ + pass + +if __name__ == "__main__": + s = load_scenario(SCEN_FILE) + try: + t = load_trajectory(TRAJ_FILE) # fixed to trajectories + except: + pass + + fig = plt.figure() + 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]") + + # plot here + 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)) + except: + pass + if PLOT["node"]: + try: + plt.plot(*plot_nodes(t["node"]), color=COLOR["node"], + marker=".", linestyle = "None") + except: + print("No RRTNode") + if PLOT["edge"]: + try: + for e in t["edge"]: + 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") + 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 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]) + else: + try: + plt.plot( + *plot_nodes(t["traj"][traj]), + color=COLOR["log"][traj]) + except: + plt.plot( + *plot_nodes(t["traj"][traj]), + color="black") + 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 + + plt.show() + #plt.savefig("plot.pdf".format(p, p_id)) + plt.close(fig) -- 2.39.2