From fddfb8b1968ec9f4d300a1a289c02d73c4a82273 Mon Sep 17 00:00:00 2001
From: Aidan Lincke <aidan@lincke.org>
Date: Wed, 4 Oct 2023 10:01:14 -0400
Subject: [PATCH] Add testing framework

---
 .../src/planning/testing_framework/ex.json    | 21 +++++++++++++++
 .../planning/testing_framework/readfile.py    | 27 +++++++++++++++++++
 2 files changed, 48 insertions(+)
 create mode 100644 driverless_ws/src/planning/testing_framework/ex.json
 create mode 100644 driverless_ws/src/planning/testing_framework/readfile.py

diff --git a/driverless_ws/src/planning/testing_framework/ex.json b/driverless_ws/src/planning/testing_framework/ex.json
new file mode 100644
index 000000000..15bc9912b
--- /dev/null
+++ b/driverless_ws/src/planning/testing_framework/ex.json
@@ -0,0 +1,21 @@
+{
+    "blue_cones": [
+        {
+            "x": 1,
+            "y": 2,
+            "z": 3
+        },
+        {
+            "x": 3,
+            "y": 2,
+            "z": 1
+        }
+    ],
+    "yellow_cones": [
+        {
+            "x": 1,
+            "y": 2,
+            "z": 3
+        }
+    ]
+}
\ No newline at end of file
diff --git a/driverless_ws/src/planning/testing_framework/readfile.py b/driverless_ws/src/planning/testing_framework/readfile.py
new file mode 100644
index 000000000..617e48e8f
--- /dev/null
+++ b/driverless_ws/src/planning/testing_framework/readfile.py
@@ -0,0 +1,27 @@
+from eufs_msgs.msg import ConeArray as ConeArray
+from geometry_msgs.msg import Point
+import json
+
+def pointList(cones): 
+    lst = []
+    for cone in cones:
+        p = Point()
+        p.x = float(cone["x"])
+        p.y = float(cone["y"])
+        p.z = float(cone["z"])
+        lst.append(p)
+    return lst
+
+def read():
+    f = open("ex.json")
+    json_data = json.load(f)
+    f.close()
+    blue_cones = json_data["blue_cones"]
+    yellow_cones = json_data["yellow_cones"]
+    cones = ConeArray()
+    cones.blue_cones = pointList(blue_cones)
+    cones.yellow_cones = pointList(yellow_cones)
+    return cones
+
+if __name__ ==  '__main__':
+    print(read())
\ No newline at end of file