diff --git a/Search-based Planning/Astar_3D/Astar3D.py b/Search-based Planning/Astar_3D/Astar3D.py
index 9ebe7a7..ef72dd7 100644
--- a/Search-based Planning/Astar_3D/Astar3D.py	
+++ b/Search-based Planning/Astar_3D/Astar3D.py	
@@ -11,8 +11,8 @@ import sys
 
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Astar_3D.env3D import env
-from Astar_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic
-from queue import QueuePrior
+from Astar_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest
+import queue
 
 
 class Weighted_A_star(object):
@@ -22,10 +22,10 @@ class Weighted_A_star(object):
                       [1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
                       [1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
         self.env = env()
-        self.Space = StateSpace(env.boundary) # key is the point, store g value
-        self.OPEN = QueuePrior() # store [point,priority]
-        self.start = getNearest(self.Space,env.start)
-        self.goal = getNearest(self.Space,env.goal)
+        self.Space = StateSpace(self.env.boundary) # key is the point, store g value
+        self.OPEN = queue.QueuePrior() # store [point,priority]
+        self.start = getNearest(self.Space,self.env.start)
+        self.goal = getNearest(self.Space,self.env.goal)
         self.h = Heuristic(self.Space,self.goal)
         self.Parent = {}
         self.CLOSED = {}
diff --git a/Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc
new file mode 100644
index 0000000..cf4be50
Binary files /dev/null and b/Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc differ
diff --git a/Search-based Planning/Astar_3D/queue.py b/Search-based Planning/Astar_3D/queue.py
new file mode 100644
index 0000000..8f481ae
--- /dev/null
+++ b/Search-based Planning/Astar_3D/queue.py	
@@ -0,0 +1,62 @@
+import collections
+import heapq
+
+
+class QueueFIFO:
+    """
+    Class: QueueFIFO
+    Description: QueueFIFO is designed for First-in-First-out rule.
+    """
+
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)  # enter from back
+
+    def get(self):
+        return self.queue.popleft()  # leave from front
+
+
+class QueueLIFO:
+    """
+    Class: QueueLIFO
+    Description: QueueLIFO is designed for Last-in-First-out rule.
+    """
+
+    def __init__(self):
+        self.queue = collections.deque()
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, node):
+        self.queue.append(node)  # enter from back
+
+    def get(self):
+        return self.queue.pop()  # leave from back
+
+
+class QueuePrior:
+    """
+    Class: QueuePrior
+    Description: QueuePrior reorders elements using value [priority]
+    """
+
+    def __init__(self):
+        self.queue = []
+
+    def empty(self):
+        return len(self.queue) == 0
+
+    def put(self, item, priority):
+        heapq.heappush(self.queue, (priority, item))  # reorder x using priority
+
+    def get(self):
+        return heapq.heappop(self.queue)[1]  # pop out the smallest item
+
+    def enumerate(self):
+        return self.queue