diff --git a/src/planning/PriorityQueue.py b/src/planning/PriorityQueue.py
new file mode 100644
index 0000000..f492273
--- /dev/null
+++ b/src/planning/PriorityQueue.py
@@ -0,0 +1,15 @@
+import heapq #https://docs.python.org/3/library/heapq.html
+
+class PriorityQueue:
+    def __init__(self):
+        self.elements = []
+    
+    def empty(self):
+        return len(self.elements) == 0
+    
+    def put(self, item, priority):
+        heapq.heappush(self.elements, (priority, item))
+    
+    def get(self):
+        return heapq.heappop(self.elements)[1]
+
diff --git a/src/planning/astar_driver.py b/src/planning/astar_driver.py
new file mode 100644
index 0000000..c7f1553
--- /dev/null
+++ b/src/planning/astar_driver.py
@@ -0,0 +1,41 @@
+# -*- coding: utf-8 -*-
+"""kdtree_testing
+
+Automatically generated by Colaboratory.
+
+Original file is located at
+    https://colab.research.google.com/drive/15-biioPffqZoK2zW9Fvi9t53y0o14XbZ
+"""
+
+import sys
+import matplotlib.pyplot as plt
+import numpy as np
+import math
+import random
+import time
+import pdb
+
+import Astar
+
+def load_poses(pose_gt_file) :
+    pose_gt = np.loadtxt(pose_gt_file, delimiter = ",")
+    return pose_gt
+
+
+def main() :
+  #poses = load_poses('../dataset/ground_truth/groundtruth_2012-01-08.csv')
+  poses = load_poses('../dataset/ground_truth/debug.csv')
+  sparseness = 10
+  k=50
+
+  sparse_poses = poses[1::sparseness, 1:3]
+  astar = Astar.Astar(poses=sparse_poses, k=k)
+  
+  start_idx = np.random.randint(sparse_poses.shape[0])
+  goal_idx = np.random.randint(sparse_poses.shape[0])
+  path, optimal = astar.find_path(start_idx, goal_idx)
+
+  np.save('optimal_path', path)
+
+if __name__ == '__main__' :
+  main()