search-based planning

Search-based Planning/.idea/.gitignore

@@ -1,3 +0,0 @@
-
-# Default ignored files
-/workspace.xml

Search-based Planning/.idea/Search-based Planning.iml

@@ -2,11 +2,10 @@
 <module type="PYTHON_MODULE" version="4">
   <component name="NewModuleRootManager">
     <content url="file://$MODULE_DIR$" />
-    <orderEntry type="inheritedJdk" />
+    <orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
   <component name="TestRunnerService">
-    <option name="projectConfiguration" value="pytest" />
-    <option name="PROJECT_TEST_RUNNER" value="pytest" />
+    <option name="PROJECT_TEST_RUNNER" value="Unittests" />
   </component>
 </module>

Search-based Planning/.idea/dictionaries/Huiming_Zhou.xml

@@ -0,0 +1,10 @@
+<component name="ProjectDictionaryState">
+  <dictionary name="Huiming Zhou">
+    <words>
+      <w>astar</w>
+      <w>dijk</w>
+      <w>huiming</w>
+      <w>zhou</w>
+    </words>
+  </dictionary>
+</component>

Search-based Planning/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

Search-based Planning/.idea/workspace.xml

@@ -0,0 +1,166 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ChangeListManager">
+    <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
+    <option name="SHOW_DIALOG" value="false" />
+    <option name="HIGHLIGHT_CONFLICTS" value="true" />
+    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
+    <option name="LAST_RESOLUTION" value="IGNORE" />
+  </component>
+  <component name="FileTemplateManagerImpl">
+    <option name="RECENT_TEMPLATES">
+      <list>
+        <option value="Python Script" />
+      </list>
+    </option>
+  </component>
+  <component name="ProjectId" id="1dQBIivqkvFljqtAc1O2MqInYWf" />
+  <component name="ProjectLevelVcsManager" settingsEditedManually="true" />
+  <component name="PropertiesComponent">
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../path planning algorithms/Search-based Planning" />
+    <property name="restartRequiresConfirmation" value="false" />
+    <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
+  </component>
+  <component name="RunDashboard">
+    <option name="ruleStates">
+      <list>
+        <RuleState>
+          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
+        </RuleState>
+        <RuleState>
+          <option name="name" value="StatusDashboardGroupingRule" />
+        </RuleState>
+      </list>
+    </option>
+  </component>
+  <component name="RunManager" selected="Python.a_star">
+    <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/a_star.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="bfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/bfs.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="dfs" type="PythonConfigurationType" factoryName="Python" temporary="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/dfs.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" temporary="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/dijkstra.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <configuration name="searching" type="PythonConfigurationType" factoryName="Python" temporary="true">
+      <module name="Search-based Planning" />
+      <option name="INTERPRETER_OPTIONS" value="" />
+      <option name="PARENT_ENVS" value="true" />
+      <envs>
+        <env name="PYTHONUNBUFFERED" value="1" />
+      </envs>
+      <option name="SDK_HOME" value="" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <option name="IS_MODULE_SDK" value="true" />
+      <option name="ADD_CONTENT_ROOTS" value="true" />
+      <option name="ADD_SOURCE_ROOTS" value="true" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/searching.py" />
+      <option name="PARAMETERS" value="" />
+      <option name="SHOW_COMMAND_LINE" value="false" />
+      <option name="EMULATE_TERMINAL" value="false" />
+      <option name="MODULE_MODE" value="false" />
+      <option name="REDIRECT_INPUT" value="false" />
+      <option name="INPUT_FILE" value="" />
+      <method v="2" />
+    </configuration>
+    <recent_temporary>
+      <list>
+        <item itemvalue="Python.a_star" />
+        <item itemvalue="Python.dfs" />
+        <item itemvalue="Python.dijkstra" />
+        <item itemvalue="Python.bfs" />
+        <item itemvalue="Python.searching" />
+      </list>
+    </recent_temporary>
+  </component>
+  <component name="SvnConfiguration">
+    <configuration />
+  </component>
+  <component name="TaskManager">
+    <task active="true" id="Default" summary="Default task">
+      <changelist id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
+      <created>1592347358698</created>
+      <option name="number" value="Default" />
+      <option name="presentableId" value="Default" />
+      <updated>1592347358698</updated>
+    </task>
+    <servers />
+  </component>
+</project>

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

Search-based Planning/astar.py

@@ -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!")

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
-
-from queue import *
-from mazemods import *
-
-
-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
+class BFS:
+    """
+    BFS -> Breadth-first Searching
+    """
+
+    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
+
+
+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

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
-
-from queue import *
-from mazemods import *
-
-
-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
+class DFS:
+    """
+    DFS -> Depth-first Searching
+    """
+
+    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
+
+
+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

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
-
-from queue import *
-from mazemods import *
-
-
-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!")
+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
+
+    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
+
+
+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')

Search-based Planning/environment.py

@@ -1,16 +1,49 @@
-n, m = 36, 18
-
-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]]
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: huiming zhou
+"""
+
+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

Search-based Planning/mazemods.py

@@ -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()
-
-
-

Search-based Planning/searching.py

@@ -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')

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()
+