Add plot python script

diff --git a/CHANGELOG.md b/CHANGELOG.md
index 1866ccfaf0e2df2f87fb2a9c34f06f74dd6969e1..3f40e8f5466e9a38361b8df5ee3d21071ad77cf5 100644 (file)
--- 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 (file)
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)