diff --git a/Search-based Planning/.idea/.gitignore b/Search-based Planning/.idea/.gitignore
deleted file mode 100644
index 0e40fe8..0000000
--- a/Search-based Planning/.idea/.gitignore
+++ /dev/null
@@ -1,3 +0,0 @@
-
-# Default ignored files
-/workspace.xml
\ No newline at end of file
diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml
index 7c9d48f..f3d7bc9 100644
--- a/Search-based Planning/.idea/Search-based Planning.iml
+++ b/Search-based Planning/.idea/Search-based Planning.iml
@@ -2,11 +2,10 @@
-
+
-
-
+
\ No newline at end of file
diff --git a/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml b/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml
new file mode 100644
index 0000000..f550db9
--- /dev/null
+++ b/Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml
@@ -0,0 +1,10 @@
+
+
+
+ astar
+ dijk
+ huiming
+ zhou
+
+
+
\ No newline at end of file
diff --git a/Search-based Planning/.idea/vcs.xml b/Search-based Planning/.idea/vcs.xml
deleted file mode 100644
index 6c0b863..0000000
--- a/Search-based Planning/.idea/vcs.xml
+++ /dev/null
@@ -1,6 +0,0 @@
-
-
-
-
-
-
\ No newline at end of file
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
new file mode 100644
index 0000000..e522e54
--- /dev/null
+++ b/Search-based Planning/.idea/workspace.xml
@@ -0,0 +1,166 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 1592347358698
+
+
+ 1592347358698
+
+
+
+
\ No newline at end of file
diff --git a/Search-based Planning/__pycache__/astar.cpython-37.pyc b/Search-based Planning/__pycache__/astar.cpython-37.pyc
deleted file mode 100644
index e577f7b..0000000
Binary files a/Search-based Planning/__pycache__/astar.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/bfs.cpython-37.pyc b/Search-based Planning/__pycache__/bfs.cpython-37.pyc
deleted file mode 100644
index bb9e43e..0000000
Binary files a/Search-based Planning/__pycache__/bfs.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/dijkstra.cpython-37.pyc b/Search-based Planning/__pycache__/dijkstra.cpython-37.pyc
deleted file mode 100644
index 2e9ea78..0000000
Binary files a/Search-based Planning/__pycache__/dijkstra.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/environment.cpython-37.pyc b/Search-based Planning/__pycache__/environment.cpython-37.pyc
index 30a3d62..fbb784b 100644
Binary files a/Search-based Planning/__pycache__/environment.cpython-37.pyc and b/Search-based Planning/__pycache__/environment.cpython-37.pyc differ
diff --git a/Search-based Planning/__pycache__/mazemods.cpython-37.pyc b/Search-based Planning/__pycache__/mazemods.cpython-37.pyc
deleted file mode 100644
index f326950..0000000
Binary files a/Search-based Planning/__pycache__/mazemods.cpython-37.pyc and /dev/null differ
diff --git a/Search-based Planning/__pycache__/queue.cpython-37.pyc b/Search-based Planning/__pycache__/queue.cpython-37.pyc
index ce8d5d7..65db690 100644
Binary files a/Search-based Planning/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/__pycache__/queue.cpython-37.pyc differ
diff --git a/Search-based Planning/__pycache__/tools.cpython-37.pyc b/Search-based Planning/__pycache__/tools.cpython-37.pyc
new file mode 100644
index 0000000..9ac48b1
Binary files /dev/null and b/Search-based Planning/__pycache__/tools.cpython-37.pyc differ
diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py
new file mode 100644
index 0000000..6fa6329
--- /dev/null
+++ b/Search-based Planning/a_star.py
@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+import queue
+import environment
+import tools
+
+class Astar:
+ def __init__(self, Start_State, Goal_State, n, m, heuristic_type):
+ self.xI = Start_State
+ self.xG = Goal_State
+ self.u_set = environment.motions # feasible input set
+ self.obs_map = environment.map_obs() # position of obstacles
+ self.n = n
+ self.m = m
+ self.heuristic_type = heuristic_type
+
+ def searching(self):
+ """
+ Searching using A_star.
+
+ :return: planning path, action in each node, visited nodes in the planning process
+ """
+
+ q_astar = queue.QueuePrior() # priority queue
+ q_astar.put(self.xI, 0)
+ parent = {self.xI: self.xI} # record parents of nodes
+ actions = {self.xI: (0, 0)} # record actions of nodes
+ cost = {self.xI: 0}
+ visited = []
+
+ while not q_astar.empty():
+ x_current = q_astar.get()
+ visited.append(x_current) # record visited nodes
+ if x_current == self.xG: # stop condition
+ break
+ for u_next in self.u_set: # explore neighborhoods of current node
+ x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+ # if neighbor node is not in obstacles -> ...
+ if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
+ and not tools.obs_detect(x_current, u_next, self.obs_map):
+ new_cost = cost[x_current] + int(self.get_cost(x_current, u_next))
+ if x_next not in cost or new_cost < cost[x_next]: # conditions for updating cost
+ cost[x_next] = new_cost
+ priority = new_cost + self.Heuristic(x_next, self.xG, self.heuristic_type)
+ q_astar.put(x_next, priority) # put node into queue using priority "f+h"
+ parent[x_next] = x_current
+ actions[x_next] = u_next
+ [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, actions)
+ return path_astar, actions_astar, visited
+
+ def get_cost(self, x, u):
+ """
+ Calculate cost for this motion
+
+ :param x: current node
+ :param u: input
+ :return: cost for this motion
+ :note: cost function could be more complicate!
+ """
+
+ return 1
+
+ def Heuristic(self, state, goal, heuristic_type):
+ """
+ Calculate heuristic.
+
+ :param state: current node (state)
+ :param goal: goal node (state)
+ :param heuristic_type: choosing different heuristic functions
+ :return: heuristic
+ """
+
+ if heuristic_type == "manhattan":
+ return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
+ elif heuristic_type == "euclidean":
+ return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
+ else:
+ print("Please choose right heuristic type!")
+
+
+if __name__ == '__main__':
+ x_Start = (15, 10) # Starting node
+ x_Goal = (48, 15) # Goal node
+ astar = Astar(x_Start, x_Goal, environment.col, environment.row, "manhattan")
+ [path_astar, actions_astar, visited_astar] = astar.searching()
+ tools.showPath(x_Start, x_Goal, path_astar, visited_astar, 'Astar_searching') # Plot path and visited nodes
\ No newline at end of file
diff --git a/Search-based Planning/astar.py b/Search-based Planning/astar.py
deleted file mode 100644
index b024240..0000000
--- a/Search-based Planning/astar.py
+++ /dev/null
@@ -1,52 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: Huiming Zhou
-"""
-
-
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import colors
-
-from queue import *
-from mazemods import *
-from environment import *
-
-
-def aStarSearch(xI, xG, n, m, O, heuristic_type):
- q_astar = QueuePrior()
- q_astar.put(xI, 0)
- parent = {xI: xI}
- actions = {xI: (0, 0)}
- rec_cost = {xI: 0}
- u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
-
- while not q_astar.empty():
- x_current = q_astar.get()
- if x_current == xG:
- break
- for u_next in u_set:
- x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
- if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
- and not collisionCheck(x_current, u_next, O):
- new_cost = rec_cost[x_current] + 1
- if x_next not in rec_cost or new_cost < rec_cost[x_next]:
- rec_cost[x_next] = new_cost
- priority = new_cost + Heuristic(x_next, xG, heuristic_type)
- q_astar.put(x_next, priority)
- parent[x_next] = x_current
- actions[x_next] = u_next
- [path_astar, actions_astar] = extractpath(xI, xG, parent, actions)
- [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_astar, O)
- return path_astar, actions_astar, len(parent), simple_cost, west_cost, east_cost
-
-
-# Heuristic function used in A* algorithm
-def Heuristic(state, goal, heuristic_type):
- if heuristic_type == "manhattanHeuristic":
- return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
- elif heuristic_type == "euclideanHeuristic":
- return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
- else:
- print("Please choose right heuristic type!")
diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py
index 7064110..4e82c5d 100644
--- a/Search-based Planning/bfs.py
+++ b/Search-based Planning/bfs.py
@@ -1,37 +1,59 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
-@author: Huiming Zhou
+@author: huiming zhou
"""
+import queue
+import environment
+import tools
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import colors
+class BFS:
+ """
+ BFS -> Breadth-first Searching
+ """
-from queue import *
-from mazemods import *
+ def __init__(self, Start_State, Goal_State, n, m):
+ self.xI = Start_State
+ self.xG = Goal_State
+ self.u_set = environment.motions # feasible input set
+ self.obs_map = environment.map_obs() # position of obstacles
+ self.n = n
+ self.m = m
+
+ def searching(self):
+ """
+ Searching using BFS.
+
+ :return: planning path, action in each node, visited nodes in the planning process
+ """
+
+ q_bfs = queue.QueueFIFO() # first-in-first-out queue
+ q_bfs.put(self.xI)
+ parent = {self.xI: self.xI} # record parents of nodes
+ actions = {self.xI: (0, 0)} # record actions of nodes
+ visited = []
+ while not q_bfs.empty():
+ x_current = q_bfs.get()
+ visited.append(x_current) # record visited nodes
+ if x_current == self.xG: # stop condition
+ break
+ for u_next in self.u_set: # explore neighborhoods of current node
+ x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) # neighbor node
+ # if neighbor node is not in obstacles and has not been visited -> ...
+ if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
+ and x_next not in parent \
+ and not tools.obs_detect(x_current, u_next, self.obs_map):
+ q_bfs.put(x_next)
+ parent[x_next] = x_current
+ actions[x_next] = u_next
+ [path_bfs, actions_bfs] = tools.extract_path(self.xI, self.xG, parent, actions) # extract path
+ return path_bfs, actions_bfs, visited
-def breadthFirstSearch(xI, xG, n, m, O):
- q_bfs = QueueFIFO()
- q_bfs.put(xI)
- parent = {xI: xI}
- actions = {xI: (0, 0)}
- u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
-
- while not q_bfs.empty():
- x_current = q_bfs.get()
- if x_current == xG:
- break
- for u_next in u_set:
- x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
- if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
- and x_next not in parent \
- and not collisionCheck(x_current, u_next, O):
- q_bfs.put(x_next)
- parent[x_next] = x_current
- actions[x_next] = u_next
- [path_bfs, actions_bfs] = extractpath(xI, xG, parent, actions)
- [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_bfs, O)
- return path_bfs, actions_bfs, len(parent), simple_cost, west_cost, east_cost
\ No newline at end of file
+if __name__ == '__main__':
+ x_Start = (15, 10) # Starting node
+ x_Goal = (48, 15) # Goal node
+ bfs = BFS(x_Start, x_Goal, environment.col, environment.row)
+ [path_bf, actions_bf, visited_bfs] = bfs.searching()
+ tools.showPath(x_Start, x_Goal, path_bf, visited_bfs, 'breadth_first_searching') # Plot path and visited nodes
\ No newline at end of file
diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py
index 6300851..04e4ab5 100644
--- a/Search-based Planning/dfs.py
+++ b/Search-based Planning/dfs.py
@@ -1,37 +1,60 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
-@author: Huiming Zhou
+@author: huiming zhou
"""
+import queue
+import environment
+import tools
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import colors
+class DFS:
+ """
+ DFS -> Depth-first Searching
+ """
-from queue import *
-from mazemods import *
+ def __init__(self, Start_State, Goal_State, n, m):
+ self.xI = Start_State
+ self.xG = Goal_State
+ self.u_set = environment.motions # feasible input set
+ self.obs_map = environment.map_obs() # position of obstacles
+ self.n = n
+ self.m = m
+
+ def searching(self):
+ """
+ Searching using DFS.
+
+ :return: planning path, action in each node, visited nodes in the planning process
+ """
+
+ q_dfs = queue.QueueLIFO() # last-in-first-out queue
+ q_dfs.put(self.xI)
+ parent = {self.xI: self.xI} # record parents of nodes
+ actions = {self.xI: (0, 0)} # record actions of nodes
+ visited = []
+
+ while not q_dfs.empty():
+ x_current = q_dfs.get()
+ visited.append(x_current) # record visited nodes
+ if x_current == self.xG: # stop condition
+ break
+ for u_next in self.u_set: # explore neighborhoods of current node
+ x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) # neighbor node
+ # if neighbor node is not in obstacles and has not been visited -> ...
+ if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
+ and x_next not in parent \
+ and not tools.obs_detect(x_current, u_next, self.obs_map):
+ q_dfs.put(x_next)
+ parent[x_next] = x_current
+ actions[x_next] = u_next
+ [path_dfs, actions_dfs] = tools.extract_path(self.xI, self.xG, parent, actions)
+ return path_dfs, actions_dfs, visited
-def depth_fist_search(xI, xG, n, m, O):
- q_dfs = QueueLIFO()
- q_dfs.put(xI)
- parent = {xI: xI}
- actions = {xI: (0, 0)}
- u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
-
- while not q_dfs.empty():
- x_current = q_dfs.get()
- if x_current == xG:
- break
- for u_next in u_set:
- x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
- if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
- and x_next not in parent \
- and not collisionCheck(x_current, u_next, O):
- q_dfs.put(x_next)
- parent[x_next] = x_current
- actions[x_next] = u_next
- [path_dfs, actions_dfs] = extractpath(xI, xG, parent, actions)
- [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_dfs, O)
- return path_dfs, actions_dfs, len(parent), simple_cost, west_cost, east_cost
\ No newline at end of file
+if __name__ == '__main__':
+ x_Start = (15, 10) # Starting node
+ x_Goal = (48, 15) # Goal node
+ dfs = DFS(x_Start, x_Goal, environment.col, environment.row)
+ [path_dfs, actions_dfs, visited_dfs] = dfs.searching()
+ tools.showPath(x_Start, x_Goal, path_dfs, visited_dfs, 'depth_first_searching') # Plot path and visited nodes
\ No newline at end of file
diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py
index 6341100..06dea0f 100644
--- a/Search-based Planning/dijkstra.py
+++ b/Search-based Planning/dijkstra.py
@@ -1,57 +1,72 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
-@author: Huiming Zhou
+@author: huiming zhou
"""
+import queue
+import environment
+import tools
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import colors
+class Dijkstra:
+ def __init__(self, Start_State, Goal_State, n, m):
+ self.xI = Start_State
+ self.xG = Goal_State
+ self.u_set = environment.motions # feasible input set
+ self.obs_map = environment.map_obs() # position of obstacles
+ self.n = n
+ self.m = m
-from queue import *
-from mazemods import *
+ def searching(self):
+ """
+ Searching using Dijkstra.
+
+ :return: planning path, action in each node, visited nodes in the planning process
+ """
+
+ q_dijk = queue.QueuePrior() # priority queue
+ q_dijk.put(self.xI, 0)
+ parent = {self.xI: self.xI} # record parents of nodes
+ actions = {self.xI: (0, 0)} # record actions of nodes
+ cost = {self.xI: 0}
+ visited = []
+
+ while not q_dijk.empty():
+ x_current = q_dijk.get()
+ visited.append(x_current) # record visited nodes
+ if x_current == self.xG: # stop condition
+ break
+ for u_next in self.u_set: # explore neighborhoods of current node
+ x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
+ # if neighbor node is not in obstacles -> ...
+ if 0 <= x_next[0] < self.n and 0 <= x_next[1] < self.m \
+ and not tools.obs_detect(x_current, u_next, self.obs_map):
+ new_cost = cost[x_current] + int(self.get_cost(x_current, u_next))
+ if x_next not in cost or new_cost < cost[x_next]:
+ cost[x_next] = new_cost
+ priority = new_cost
+ q_dijk.put(x_next, priority) # put node into queue using cost to come as priority
+ parent[x_next] = x_current
+ actions[x_next] = u_next
+ [path_dijk, actions_dijk] = tools.extract_path(self.xI, self.xG, parent, actions)
+ return path_dijk, actions_dijk, visited
+
+ def get_cost(self, x, u):
+ """
+ Calculate cost for this motion
+
+ :param x: current node
+ :param u: input
+ :return: cost for this motion
+ :note: cost function could be more complicate!
+ """
+
+ return 1
-def DijkstraSearch(xI, xG, n, m, O, cost_type):
- q_dijk = QueuePrior()
- q_dijk.put(xI, 0)
- parent = {xI: xI}
- actions = {xI: (0, 0)}
- rec_cost = {xI: 0}
- u_set = {(-1, 0), (1, 0), (0, 1), (0, -1)}
-
- while not q_dijk.empty():
- x_current = q_dijk.get()
- if x_current == xG:
- break
- for u_next in u_set:
- x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
- if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
- and not collisionCheck(x_current, u_next, O):
- cost_x = costfunc(x_current, x_next, O, cost_type)
- new_cost = rec_cost[x_current] + cost_x
- if x_next not in rec_cost or new_cost < rec_cost[x_next]:
- rec_cost[x_next] = new_cost
- priority = new_cost
- q_dijk.put(x_next, priority)
- parent[x_next] = x_current
- actions[x_next] = u_next
- [path_dijk, actions_dijk] = extractpath(xI, xG, parent, actions)
- [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_dijk, O)
- return path_dijk, actions_dijk, len(parent), simple_cost, west_cost, east_cost
-
-
-# Cost function used in Dijkstra's algorithm
-def costfunc(x_current, x_next, O, function_type):
- if function_type == "westcost":
- return x_next[0] ** 2
- elif function_type == "eastcost":
- maxX = 0
- for k in range(len(O)):
- westxO = O[k][1]
- if westxO > maxX:
- maxX = westxO
- return (maxX - x_next[0]) ** 2
- else:
- print("Please choose right cost function!")
+if __name__ == '__main__':
+ x_Start = (15, 10) # Starting node
+ x_Goal = (48, 15) # Goal node
+ dijkstra = Dijkstra(x_Start, x_Goal, environment.col, environment.row)
+ [path_dijk, actions_dijk, visited_dijk] = dijkstra.searching()
+ tools.showPath(x_Start, x_Goal, path_dijk, visited_dijk, 'dijkstra_searching')
\ No newline at end of file
diff --git a/Search-based Planning/environment.py b/Search-based Planning/environment.py
index ff22b3e..3a6533c 100644
--- a/Search-based Planning/environment.py
+++ b/Search-based Planning/environment.py
@@ -1,16 +1,49 @@
-n, m = 36, 18
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
-O=[[0,35,0,0],[0,0,0,17],[0,35,17,17],[35,35,0,17],[11,26,1,1],[1,9,2,2],
-[28,33,2,2],[9,14,3,3],[16,26,3,3],[28,29,3,3],[32,33,3,3],[2,7,4,4],
-[14,14,4,4],[22,26,4,4],[28,29,4,4],[7,12,5,5],[14,18,5,5],[20,20,5,5],
-[25,26,5,5],[28,29,5,5],[31,34,5,5],[1,5,6,6],[12,12,6,6],[20,23,6,6],
-[25,26,6,6],[28,28,6,6],[5,10,7,7],[12,18,7,7],[25,26,7,7],[28,33,7,7],
-[2,3,8,8],[5,5,8,8],[9,10,8,8],[18,26,8,8],[28,29,8,8],[2,3,9,9],[5,5,9,9],
-[7,7,9,9],[9,16,9,9],[18,19,9,9],[28,29,9,9],[31,34,9,9],[2,2,10,10],
-[5,5,10,10],[7,7,10,10],[9,9,10,10],[13,13,10,10],[18,19,10,10],[21,28,10,10],
-[2,3,11,11],[5,5,11,11],[7,7,11,11],[9,9,11,11],[11,11,11,11],[13,13,11,11],
-[15,18,11,11],[21,23,11,11],[28,33,11,11],[2,3,12,12],[5,5,12,12],[7,7,12,12],
-[9,9,12,12],[11,11,12,12],[25,26,12,12],[28,29,12,12],[2,3,13,13],[5,5,13,13],
-[7,7,13,13],[9,9,13,13],[11,11,13,13],[13,16,13,13],[18,26,13,13],
-[28,29,13,13],[31,34,13,13],[2,3,14,14],[7,7,14,14],[11,11,14,14],
-[18,24,14,14],[28,29,14,14],[2,24,15,15],[26,33,15,15]]
+import numpy as np
+
+col, row = 50, 30 # size of background
+motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
+
+
+def obstacles():
+ """
+ Design the obstacles' positions.
+ :return: the map of obstacles.
+ """
+
+ background = [[[1., 1., 1.]
+ for x in range(col)] for y in range(row)]
+ for j in range(col):
+ background[0][j] = [0., 0., 0.]
+ background[row - 1][j] = [0., 0., 0.]
+ for i in range(row):
+ background[i][0] = [0., 0., 0.]
+ background[i][col - 1] = [0., 0., 0.]
+ for i in range(10, 20):
+ background[15][i] = [0., 0., 0.]
+ for i in range(15):
+ background[row - 1 - i][30] = [0., 0., 0.]
+ background[i + 1][20] = [0., 0., 0.]
+ background[i + 1][40] = [0., 0., 0.]
+ return background
+
+
+def map_obs():
+ """
+ Using a matrix to represent the position of obstacles,
+ which is used for obstacle detection.
+ :return: a matrix, in which '1' represents obstacle.
+ """
+
+ obs_map = np.zeros((col, row))
+ pos_map = obstacles()
+ for i in range(col):
+ for j in range(row):
+ if pos_map[j][i] == [0., 0., 0.]:
+ obs_map[i][j] = 1
+ return obs_map
\ No newline at end of file
diff --git a/Search-based Planning/mazemods.py b/Search-based Planning/mazemods.py
deleted file mode 100644
index 5f93885..0000000
--- a/Search-based Planning/mazemods.py
+++ /dev/null
@@ -1,179 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: Huiming Zhou
-"""
-
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import colors
-
-
-# Makes the maze data with obstacles stored as black RGB values,
-# free space stored as white RGB values"
-def makeMaze(n, m, O):
- # Initialize to lists of 1. for RGB color index = white
- gridvals = [[[1. for i in range(3)] for col in range(n)] for row in range(m)]
- # Iterate through each obstacle
- for l in range(len(O)):
- # Find boundaries of current obstacle
- west, east = [O[l][0], O[l][1]]
- south, north = [O[l][2], O[l][3]]
- # Iterate through each cell of obstacle (clunky, but works)
- for i in range(west, east + 1):
- for j in range(south, north + 1):
- gridvals[j][i] = [0., 0., 0.] # Change entry to RGB black
- return gridvals
-
-
-# Function to actually plot the maze
-def maze(n, m, O):
- gridvals = makeMaze(n, m, O)
- fig, ax = plt.subplots() # make a figure + axes
- ax.imshow(gridvals) # Plot it
- ax.invert_yaxis() # Needed so that bottom left is (0,0)
- # ax.axis('off')
-
-
-# Checks for collisions given position x, control u, obstacle list O
-def collisionCheck(x, u, O):
- # Check input
- if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
- print('collision_check error: Invalid input u!')
- return
- nextx = [x[i] + u[i] for i in range(len(x))]
- for l in range(len(O)):
- # Find boundaries of current obstacle
- west, east = [O[l][0], O[l][1]]
- south, north = [O[l][2], O[l][3]]
- # Check if nextx is contained in obstacle boundaries
- if west <= nextx[0] <= east and south <= nextx[1] <= north:
- return True
- # If we iterate through whole list and don't trigger the "if", then no collisions
- return False
-
-
-# Makes a piece of data with obstacles stored as black RGB values,
-# free space stored as white RGB values, and path stored as increasing hue of
-# yellow RGB values
-def makePath(xI, xG, path, n, m, O):
- # Obtain the grid populated with obstacles and free space RGB values first
- gridpath = makeMaze(n, m, O)
- L = len(path)
- # Iterate through the path to plot as increasing shades of yellow
- for l in range(L - 1):
- gridpath[path[l][1]][path[l][0]] = [1., 1., 1 - l / (L - 1)] # white-->yellow
- gridpath[xI[1]][xI[0]] = [0., 0., 1.] # Initial node (plotted as blue)
- gridpath[xG[1]][xG[0]] = [0., 1., 0.] # Goal node (plotted as green)
- return gridpath
-
-
-# Constructs path list from initial point and list of actions
-def getPathFromActions(xI, actions):
- L = len(actions)
- path = []
- nextx = xI
- for l in range(L):
- u = actions[l]
- if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
- print('getPath error: Invalid input u!')
- return
- nextx = [nextx[i] + u[i] for i in range(len(nextx))] # nextx = nextx + u
- path.append(nextx) # Builds the path
- return path
-
-
-# If any collisions, cost is 999999, else cost is one for each action
-def getCostOfActions(xI, actions, O):
- L = len(actions)
- costsum = 0
- nextx = xI
- for l in range(L):
- u = actions[l]
- if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
- print('getCostOfActions error: Invalid input u!')
- return
- collision = collisionCheck(nextx, u, O)
- if collision: return 999999999
- nextx = [nextx[i] + u[i] for i in range(len(nextx))] # nextx = nextx + u
- costsum = costsum + 1
- return costsum
-
-
-# If any collisions, cost is 999999, else cost is 2^(x[0]) for each action
-def stayWestCost(xI, actions, O):
- L = len(actions)
- costsum = 0
- nextx = xI
- for l in range(L):
- u = actions[l]
- if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
- print('stayWestCost error: Invalid input u!')
- return
- collision = collisionCheck(nextx, u, O)
- if collision: return 999999999
- nextx = [nextx[i] + u[i] for i in range(len(nextx))] # nextx = nextx + u
- costsum = costsum + nextx[0] ** 2
- return costsum
-
-
-# If any collisions, cost is 999999, else cost is 2^(maxX - x[0]) for each action
-def stayEastCost(xI, actions, O):
- # Determine maximum x coordinate of workspace from obstacle list
- maxX = 0
- for k in range(len(O)):
- westxO = O[k][1]
- if westxO > maxX:
- maxX = westxO
-
- L = len(actions)
- costsum = 0
- nextx = xI
- for l in range(L):
- u = actions[l]
- if u != (-1, 0) and u != (1, 0) and u != (0, -1) and u != (0, 1):
- print('stayEastCost error: Invalid input u!')
- return
- collision = collisionCheck(nextx, u, O)
- if collision: return 999999999
- nextx = [nextx[i] + u[i] for i in range(len(nextx))] # nextx = nextx + u
- costsum = costsum + (maxX - nextx[0]) ** 2
- return costsum
-
-
-# Calculate different types of cost for searching algorithm
-def cost_calculation(xI, actions, O):
- simple_cost = getCostOfActions(xI, actions, O)
- west_cost = stayWestCost(xI, actions, O)
- east_cost = stayEastCost(xI, actions, O)
- return simple_cost, west_cost, east_cost
-
-
-# Extract path from results of searching algorithm
-def extractpath(xI, xG, parent, actions):
- pathback = [xG]
- actionsback = []
- actionsback.append(actions[xG])
- x_current = xG
- while parent[x_current] != x_current:
- x_current = parent[x_current]
- pathback.append(x_current)
- actionsback.append(actions[x_current])
- path_extract = list(reversed(pathback))
- actions_extract = list(reversed(actionsback))
- path_extract.pop(0)
- actions_extract.pop(0)
- return path_extract, actions_extract
-
-
-# Plots the path
-def showPath(xI, xG, path, n, m, O, name):
- gridpath = makePath(xI, xG, path, n, m, O)
- fig, ax = plt.subplots(1, 1) # make a figure + axes
- ax.imshow(gridpath) # Plot it
- ax.invert_yaxis() # Needed so that bottom left is (0,0)
- plt.title(name, fontdict=None)
- plt.show()
-
-
-
diff --git a/Search-based Planning/searching.py b/Search-based Planning/searching.py
deleted file mode 100644
index a7abb45..0000000
--- a/Search-based Planning/searching.py
+++ /dev/null
@@ -1,62 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: Huiming Zhou
-"""
-import numpy as np
-import matplotlib.pyplot as plt
-from matplotlib import colors
-
-from queue import *
-from mazemods import *
-from environment import *
-
-from bfs import *
-from dijkstra import *
-from astar import *
-
-
-class Searching:
- def __init__(self, Start_State, Goal_State, n, m, O):
- self.xI = Start_State
- self.xG = Goal_State
- self.u_set = [(-1, 0), (1, 0), (0, 1), (0, -1)]
- self.n = n
- self.m = m
- self.O = O
-
- def depth_fist_search(self):
- q_dfs = QueueLIFO()
- q_dfs.put(self.xI)
- parent = {self.xI: self.xI}
- actions = {self.xI: (0, 0)}
-
- while not q_dfs.empty():
- x_current = q_dfs.get()
- if x_current == xG:
- break
- for u_next in self.u_set:
- x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))])
- if 0 <= x_next[0] < n and 0 <= x_next[1] < m \
- and x_next not in parent \
- and not collisionCheck(x_current, u_next, O):
- q_dfs.put(x_next)
- parent[x_next] = x_current
- actions[x_next] = u_next
- [path_dfs, actions_dfs] = extractpath(xI, xG, parent, actions)
- [simple_cost, west_cost, east_cost] = cost_calculation(xI, actions_dfs, O)
- return path_dfs, actions_dfs, len(parent), simple_cost, west_cost, east_cost
-
-
-if __name__ == '__main__':
- xI = (1, 1)
- xG = (23, 12)
- searching = Searching(xI, xG, n, m, O)
-
- [path_df, actions_df, num_visited_df, simple_cost_df, west_cost_df, east_cost_df] = searching.depth_fist_search()
- print('1 - Depth_First_Searching algorithm: ')
- print('Legal control actions: \n', actions_df)
- print('Number of explored nodes was [%d], basic cost was [%d], stay west cost was [%d], stay east cost was [%d] \n'
- % (num_visited_df, simple_cost_df, west_cost_df, east_cost_df))
-
- showPath(xI, xG, path_df, n, m, O, 'Depth First Searching algorithm')
\ No newline at end of file
diff --git a/Search-based Planning/tools.py b/Search-based Planning/tools.py
new file mode 100644
index 0000000..ff7ae20
--- /dev/null
+++ b/Search-based Planning/tools.py
@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+import matplotlib.pyplot as plt
+import environment
+
+
+def obs_detect(x, u, obs_map):
+ """
+ Detect if the next state is in obstacles using this input.
+
+ :param x: current state
+ :param u: input
+ :param obs_map: map of obstacles
+ :return: in obstacles: True / not in obstacles: False
+ """
+
+ x_next = [x[0] + u[0], x[1] + u[1]] # next state using input 'u'
+ if u not in environment.motions or \
+ obs_map[x_next[0]][x_next[1]] == 1: # if 'u' is feasible and next state is not in obstacles
+ return True
+ return False
+
+
+def extract_path(xI, xG, parent, actions):
+ """
+ Extract the path based on the relationship of nodes.
+
+ :param xI: Starting node
+ :param xG: Goal node
+ :param parent: Relationship between nodes
+ :param actions: Action needed for transfer between two nodes
+ :return: The planning path
+ """
+
+ path_back = [xG]
+ acts_back = [actions[xG]]
+ x_current = xG
+ while True:
+ x_current = parent[x_current]
+ path_back.append(x_current)
+ acts_back.append(actions[x_current])
+ if x_current == xI: break
+ return list(reversed(path_back)), list(reversed(acts_back))
+
+
+def showPath(xI, xG, path, visited, name):
+ """
+ Plot the path.
+
+ :param xI: Starting node
+ :param xG: Goal node
+ :param path: Planning path
+ :param visited: Visited nodes
+ :param name: Name of this figure
+ :return: A plot
+ """
+
+ background = environment.obstacles()
+ fig, ax = plt.subplots()
+ for k in range(len(visited)):
+ background[visited[k][1]][visited[k][0]] = [.5, .5, .5] # visited nodes: gray color
+ for k in range(len(path)):
+ background[path[k][1]][path[k][0]] = [1., 0., 0.] # path: red color
+ background[xI[1]][xI[0]] = [0., 0., 1.] # starting node: blue color
+ background[xG[1]][xG[0]] = [0., 1., .5] # goal node: green color
+ ax.imshow(background)
+ ax.invert_yaxis() # put origin of coordinate to left-bottom
+ plt.title(name, fontdict=None)
+ plt.show()
+