From ae85e66d07dedc37739a7f81910befce2e4065cc Mon Sep 17 00:00:00 2001
From: Atsushi Sakai <asakai.amsl+github@gmail.com>
Date: Mon, 24 Feb 2020 16:40:57 +0900
Subject: [PATCH] fix tests

---
 .../visibility_graph_planner.py               | 90 +++++++++++++++++++
 tests/test_voronoi_path_planner.py            |  6 ++
 2 files changed, 96 insertions(+)

diff --git a/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py b/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py
index e69de29b..06a4d598 100644
--- a/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py
+++ b/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py
@@ -0,0 +1,90 @@
+"""
+
+Visibility Graph Planner
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+"""
+
+import matplotlib.pyplot as plt
+
+show_animation = True
+
+class VisibilityGraphPlanner:
+
+    def __init__(self, robot_radius):
+        self.robot_radius = robot_radius
+
+    def planning(self, start_x, start_y, goal_x, goal_y, obstacles):
+        nodes = self.exstract_graph_node(start_x, start_y, goal_x, goal_y,
+                                         obstacles)
+
+        graph = self.generate_graph(nodes, obstacles)
+
+        rx, ry = dijkstra_search(graph)
+
+        return rx, ry
+
+    def exstract_graph_node(self, start_x, start_y, goal_x, goal_x, obstacles):
+        nodes = []
+
+        return nodes
+
+    def generate_graph(self, nodes, obstacles):
+
+        graph = []
+
+        return graph
+
+
+class ObstaclePolygon:
+
+    def __init__(self, x_list, y_list):
+        self.x_list = x_list
+        self.y_list = y_list
+
+        self.close_polygon()
+
+    def close_polygon(self):
+        is_x_same = self.x_list[0] == self.x_list[-1]
+        is_y_same = self.y_list[0] == self.y_list[-1]
+        if is_x_same and is_y_same:
+            return  # no need to close
+
+        self.x_list.append(self.x_list[0])
+        self.y_list.append(self.y_list[0])
+
+    def plot(self):
+        plt.plot(self.x_list, self.y_list, "-b")
+
+
+def main():
+    print(__file__ + " start!!")
+
+    # start and goal position
+    sx, sy = 10.0, 10.0  # [m]
+    gx, gy = 50.0, 50.0  # [m]
+    robot_radius = 5.0  # [m]
+
+    obstacles = [ObstaclePolygon(
+        [20.0, 30.0, 15.0],
+        [20.0, 20.0, 30.0],
+    ), ObstaclePolygon(
+        [30.0, 45.0, 50.0, 40.0],
+        [50.0, 40.0, 20.0, 40.0],
+    )]
+
+    rx, ry = VisibilityGraphPlanner(robot_radius).planning(sx, sy, gx, gy,
+                                                           obstacles)
+    assert rx, 'Cannot found path'
+    if show_animation:  # pragma: no cover
+        plt.plot(sx, sy, "or")
+        plt.plot(gx, gy, "ob")
+        [ob.plot() for ob in obstacles]
+        plt.plot(rx, ry, "-r")
+        plt.axis("equal")
+        plt.show()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/tests/test_voronoi_path_planner.py b/tests/test_voronoi_path_planner.py
index 3e0ff961..5295769b 100644
--- a/tests/test_voronoi_path_planner.py
+++ b/tests/test_voronoi_path_planner.py
@@ -1,6 +1,12 @@
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__))
+                + "/../PathPlanning/VoronoiRoadMap/")
+
 from unittest import TestCase
 from PathPlanning.VoronoiRoadMap import voronoi_road_map as m
 
+
 print(__file__)