fix unittest animation bugs (AtsushiSakai#429)

* fix unittest animation bugs

* exstract a function

Author: AtsushiSakai

PathPlanning/AStar/A_Star_searching_from_two_side.py renamed to PathPlanning/AStar/a_star_searching_from_two_side.py

@@ -1,13 +1,13 @@
 """
-astar variants
+a star variants
 author: Sarim Mehdi([email protected])
 Source: http://theory.stanford.edu/~amitp/GameProgramming/Variations.html
 """
 
 import numpy as np
 import matplotlib.pyplot as plt
 
-show_animation = False
+show_animation = True
 use_beam_search = False
 use_iterative_deepening = False
 use_dynamic_weighting = False
@@ -89,7 +89,8 @@ def key_points(o_dict):
                 obs_count += 1
             if obs_count == 3 or obs_count == 1:
                 c_list.append((x, y))
-                plt.plot(x, y, ".y")
+                if show_animation:
+                    plt.plot(x, y, ".y")
                 break
     if only_corners:
         return c_list
@@ -148,7 +149,8 @@ def __init__(self, obs_grid, goal_x, goal_y, start_x, start_y,
              'open': True, 'in_open_list': True}
         self.open_set.append(self.all_nodes[tuple(self.start_pt)])
 
-    def get_hval(self, x1, y1, x2, y2):
+    @staticmethod
+    def get_hval(x1, y1, x2, y2):
         x, y = x1, y1
         val = 0
         while x != x2 or y != y2:
@@ -178,7 +180,6 @@ def get_farthest_point(self, x, y, i, j):
     def jump_point(self):
         """Jump point: Instead of exploring all empty spaces of the
         map, just explore the corners."""
-        plt.title('Jump Point')
 
         goal_found = False
         while len(self.open_set) > 0:
@@ -232,35 +233,40 @@ def jump_point(self):
                     if not self.all_nodes[cand_pt]['in_open_list']:
                         self.open_set.append(self.all_nodes[cand_pt])
                         self.all_nodes[cand_pt]['in_open_list'] = True
-                    plt.plot(cand_pt[0], cand_pt[1], "r*")
+                    if show_animation:
+                        plt.plot(cand_pt[0], cand_pt[1], "r*")
 
                 if goal_found:
                     break
-            plt.pause(0.001)
+            if show_animation:
+                plt.pause(0.001)
             if goal_found:
                 current_node = self.all_nodes[tuple(self.goal_pt)]
             while goal_found:
                 if current_node['pred'] is None:
                     break
                 x = [current_node['pos'][0], current_node['pred'][0]]
                 y = [current_node['pos'][1], current_node['pred'][1]]
-                plt.plot(x, y, "b")
                 current_node = self.all_nodes[tuple(current_node['pred'])]
-                plt.pause(0.001)
+                if show_animation:
+                    plt.plot(x, y, "b")
+                    plt.pause(0.001)
             if goal_found:
                 break
 
             current_node['open'] = False
             current_node['in_open_list'] = False
-            plt.plot(current_node['pos'][0], current_node['pos'][1], "g*")
+            if show_animation:
+                plt.plot(current_node['pos'][0], current_node['pos'][1], "g*")
             del self.open_set[p]
             current_node['fcost'], current_node['hcost'] = np.inf, np.inf
         if show_animation:
+            plt.title('Jump Point')
             plt.show()
 
-    def astar(self):
+    def a_star(self):
         """Beam search: Maintain an open list of just 30 nodes.
-        If more than 30 nodes, then get rid of ndoes with high
+        If more than 30 nodes, then get rid of nodes with high
         f values.
         Iterative deepening: At every iteration, get a cut-off
         value for the f cost. This cut-off is minimum of the f
@@ -275,21 +281,23 @@ def astar(self):
         one neighbor at a time. In fact, you can look for the
         next node as far out as you can as long as there is a
         clear line of sight from your current node to that node."""
-        if use_beam_search:
-            plt.title('A* with beam search')
-        elif use_iterative_deepening:
-            plt.title('A* with iterative deepening')
-        elif use_dynamic_weighting:
-            plt.title('A* with dynamic weighting')
-        elif use_theta_star:
-            plt.title('Theta*')
-        else:
-            plt.title('A*')
+        if show_animation:
+            if use_beam_search:
+                plt.title('A* with beam search')
+            elif use_iterative_deepening:
+                plt.title('A* with iterative deepening')
+            elif use_dynamic_weighting:
+                plt.title('A* with dynamic weighting')
+            elif use_theta_star:
+                plt.title('Theta*')
+            else:
+                plt.title('A*')
 
         goal_found = False
         curr_f_thresh = np.inf
         depth = 0
         no_valid_f = False
+        w = None
         while len(self.open_set) > 0:
             self.open_set = sorted(self.open_set, key=lambda x: x['fcost'])
             lowest_f = self.open_set[0]['fcost']
@@ -351,30 +359,14 @@ def astar(self):
                         break
 
                     cand_pt = tuple(cand_pt)
-                    if not self.obs_grid[tuple(cand_pt)] and \
-                            self.all_nodes[cand_pt]['open']:
-                        g_cost = offset + current_node['gcost']
-                        h_cost = self.all_nodes[cand_pt]['hcost']
-                        if use_dynamic_weighting:
-                            h_cost = h_cost * w
-                        f_cost = g_cost + h_cost
-                        if f_cost < self.all_nodes[cand_pt]['fcost'] and \
-                                f_cost <= curr_f_thresh:
-                            f_cost_list.append(f_cost)
-                            self.all_nodes[cand_pt]['pred'] = \
-                                current_node['pos']
-                            self.all_nodes[cand_pt]['gcost'] = g_cost
-                            self.all_nodes[cand_pt]['fcost'] = f_cost
-                            if not self.all_nodes[cand_pt]['in_open_list']:
-                                self.open_set.append(self.all_nodes[cand_pt])
-                                self.all_nodes[cand_pt]['in_open_list'] = True
-                            plt.plot(cand_pt[0], cand_pt[1], "r*")
-                        if curr_f_thresh < f_cost < \
-                                self.all_nodes[cand_pt]['fcost']:
-                            no_valid_f = True
+                    no_valid_f = self.update_node_cost(cand_pt, curr_f_thresh,
+                                                       current_node,
+                                                       f_cost_list, no_valid_f,
+                                                       offset, w)
                 if goal_found:
                     break
-            plt.pause(0.001)
+            if show_animation:
+                plt.pause(0.001)
             if goal_found:
                 current_node = self.all_nodes[tuple(self.goal_pt)]
             while goal_found:
@@ -383,12 +375,13 @@ def astar(self):
                 if use_theta_star or use_jump_point:
                     x, y = [current_node['pos'][0], current_node['pred'][0]], \
                              [current_node['pos'][1], current_node['pred'][1]]
-                    plt.plot(x, y, "b")
+                    if show_animation:
+                        plt.plot(x, y, "b")
                 else:
-                    plt.plot(current_node['pred'][0],
-                             current_node['pred'][1], "b*")
+                    if show_animation:
+                        plt.plot(current_node['pred'][0],
+                                 current_node['pred'][1], "b*")
                 current_node = self.all_nodes[tuple(current_node['pred'])]
-                plt.pause(0.001)
             if goal_found:
                 break
 
@@ -402,13 +395,40 @@ def astar(self):
 
             current_node['open'] = False
             current_node['in_open_list'] = False
-            plt.plot(current_node['pos'][0], current_node['pos'][1], "g*")
+            if show_animation:
+                plt.plot(current_node['pos'][0], current_node['pos'][1], "g*")
             del self.open_set[p]
             current_node['fcost'], current_node['hcost'] = np.inf, np.inf
             depth += 1
         if show_animation:
             plt.show()
 
+    def update_node_cost(self, cand_pt, curr_f_thresh, current_node,
+                         f_cost_list, no_valid_f, offset, w):
+        if not self.obs_grid[tuple(cand_pt)] and \
+                self.all_nodes[cand_pt]['open']:
+            g_cost = offset + current_node['gcost']
+            h_cost = self.all_nodes[cand_pt]['hcost']
+            if use_dynamic_weighting:
+                h_cost = h_cost * w
+            f_cost = g_cost + h_cost
+            if f_cost < self.all_nodes[cand_pt]['fcost'] and \
+                    f_cost <= curr_f_thresh:
+                f_cost_list.append(f_cost)
+                self.all_nodes[cand_pt]['pred'] = \
+                    current_node['pos']
+                self.all_nodes[cand_pt]['gcost'] = g_cost
+                self.all_nodes[cand_pt]['fcost'] = f_cost
+                if not self.all_nodes[cand_pt]['in_open_list']:
+                    self.open_set.append(self.all_nodes[cand_pt])
+                    self.all_nodes[cand_pt]['in_open_list'] = True
+                if show_animation:
+                    plt.plot(cand_pt[0], cand_pt[1], "r*")
+            if curr_f_thresh < f_cost < \
+                    self.all_nodes[cand_pt]['fcost']:
+                no_valid_f = True
+        return no_valid_f
+
 
 def main():
     # set obstacle positions
@@ -443,10 +463,11 @@ def main():
     for x, y, l in zip(all_x, all_y, all_len):
         draw_horizontal_line(x, y, l, o_x, o_y, obs_dict)
 
-    plt.plot(o_x, o_y, ".k")
-    plt.plot(s_x, s_y, "og")
-    plt.plot(g_x, g_y, "xb")
-    plt.grid(True)
+    if show_animation:
+        plt.plot(o_x, o_y, ".k")
+        plt.plot(s_x, s_y, "og")
+        plt.plot(g_x, g_y, "xb")
+        plt.grid(True)
 
     if use_jump_point:
         keypoint_list = key_points(obs_dict)
@@ -455,7 +476,7 @@ def main():
         search_obj.jump_point()
     else:
         search_obj = SearchAlgo(obs_dict, g_x, g_y, s_x, s_y, 101, 101)
-        search_obj.astar()
+        search_obj.a_star()
 
 
 if __name__ == '__main__':

PathPlanning/AStar/a_star_variants.py

@@ -129,9 +129,12 @@ def assign_vectors(self):
     def follow_vectors(self):
         curr_x, curr_y = self.start_pt
         while curr_x is not None and curr_y is not None:
-            plt.plot(curr_x, curr_y, "b*")
             curr_x, curr_y = self.vector_field[(curr_x, curr_y)]
-            plt.pause(0.001)
+
+            if show_animation:
+                plt.plot(curr_x, curr_y, "b*")
+                plt.pause(0.001)
+
         if show_animation:
             plt.show()
 
@@ -208,12 +211,13 @@ def main():
     for x, y, l in zip(all_x, all_y, all_len):
         draw_horizontal_line(x, y, l, h_x, h_y, obs_dict, 'hard')
 
-    plt.plot(o_x, o_y, "sr")
-    plt.plot(m_x, m_y, "sg")
-    plt.plot(h_x, h_y, "sy")
-    plt.plot(s_x, s_y, "og")
-    plt.plot(g_x, g_y, "o")
-    plt.grid(True)
+    if show_animation:
+        plt.plot(o_x, o_y, "sr")
+        plt.plot(m_x, m_y, "sg")
+        plt.plot(h_x, h_y, "sy")
+        plt.plot(s_x, s_y, "og")
+        plt.plot(g_x, g_y, "o")
+        plt.grid(True)
 
     flow_obj = FlowField(obs_dict, g_x, g_y, s_x, s_y, 50, 50)
     flow_obj.find_path()

PathPlanning/FlowField/flowfield.py

@@ -5,7 +5,7 @@
 sys.path.append(os.path.dirname(__file__) + '/../')
 
 try:
-    from PathPlanning.AStar import A_Star_searching_from_two_side as m
+    from PathPlanning.AStar import a_star_searching_from_two_side as m
 except ImportError:
     raise
 

tests/test_a_star_searching_two_side.py

@@ -10,6 +10,7 @@ class Test(TestCase):
     def test(self):
         # A* with beam search
         astar.show_animation = False
+
         astar.use_beam_search = True
         astar.main()
         self.reset_all()
@@ -34,7 +35,9 @@ def test(self):
         astar.main()
         self.reset_all()
 
-    def reset_all(self):
+    @staticmethod
+    def reset_all():
+        astar.show_animation = False
         astar.use_beam_search = False
         astar.use_iterative_deepening = False
         astar.use_dynamic_weighting = False