diff --git a/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py b/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py index 06a4d598..d00e8f43 100644 --- a/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py +++ b/PathPlanning/VisibilityGraphPlanner/visibility_graph_planner.py @@ -6,36 +6,127 @@ author: Atsushi Sakai (@Atsushi_twi) """ +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + + "/../VoronoiRoadMap/") + +import math +import numpy as np import matplotlib.pyplot as plt +from dijkstra_search import DijkstraSearch 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) - + nodes = self.extract_graph_node(start_x, start_y, goal_x, goal_y, + obstacles) graph = self.generate_graph(nodes, obstacles) - rx, ry = dijkstra_search(graph) + # rx, ry = DijkstraSearch().search(graph) + + rx, ry = [], [] return rx, ry - def exstract_graph_node(self, start_x, start_y, goal_x, goal_x, obstacles): - nodes = [] + def extract_graph_node(self, start_x, start_y, goal_x, goal_y, obstacles): + + # add start and goal as nodes + nodes = [DijkstraSearch.Node(start_x, start_y), + DijkstraSearch.Node(goal_x, goal_y, 0, None)] + + # add vertexes in configuration space as nodes + for obstacle in obstacles: + + cvx_list, cvy_list = self.calc_vertexes_in_configuration_space( + obstacle.x_list, obstacle.y_list) + + for (vx, vy) in zip(cvx_list, cvy_list): + nodes.append(DijkstraSearch.Node(vx, vy)) + + for node in nodes: + plt.plot(node.x, node.y, "xr") return nodes + def calc_vertexes_in_configuration_space(self, x_list, y_list): + x_list=x_list[0:-1] + y_list=y_list[0:-1] + cvx_list, cvy_list = [], [] + + n_data=len(x_list) + + for index in range(n_data): + offset_x, offset_y = self.calc_offset_xy( + x_list[index - 1], y_list[index - 1], + x_list[index], y_list[index], + x_list[(index + 1) % n_data], y_list[(index + 1) % n_data], + ) + cvx_list.append(offset_x) + cvy_list.append(offset_y) + + return cvx_list, cvy_list + def generate_graph(self, nodes, obstacles): graph = [] + for target_node in nodes: + for node in nodes: + for obstacle in obstacles: + if not self.is_edge_valid(target_node, node, obstacle): + print("bb") + break + print(target_node, node) + print("aa") + plt.plot([target_node.x, node.x],[target_node.y, node.y], "-r") + return graph + def is_edge_valid(self, target_node, node, obstacle): + + for i in range(len(obstacle.x_list)-1): + p1 = np.array([target_node.x, target_node.y]) + p2 = np.array([node.x, node.y]) + p3 = np.array([obstacle.x_list[i], obstacle.y_list[i]]) + p4 = np.array([obstacle.y_list[i+1], obstacle.y_list[i+1]]) + + if is_seg_intersect(p1, p2, p3, p4): + return False + + return True + + def calc_offset_xy(self, px, py, x, y, nx, ny): + p_vec = math.atan2(y - py, x - px) + n_vec = math.atan2(ny - y, nx - x) + offset_vec = math.atan2(math.sin(p_vec) + math.sin(n_vec), + math.cos(p_vec) + math.cos( + n_vec))+math.pi/2.0 + offset_x = x + self.robot_radius * math.cos(offset_vec) + offset_y = y + self.robot_radius * math.sin(offset_vec) + return offset_x, offset_y + + +def is_seg_intersect(a1, a2, b1, b2): + + xdiff = [a1[0] - a2[0], b1[0] - b2[0]] + ydiff = [a1[1] - a2[1], b1[1] - b2[1]] + + def det(a, b): + return a[0] * b[1] - a[1] * b[0] + + div = det(xdiff, ydiff) + if div == 0: + return False + else: + return True class ObstaclePolygon: @@ -44,6 +135,21 @@ class ObstaclePolygon: self.y_list = y_list self.close_polygon() + self.make_clockwise() + + def make_clockwise(self): + if not self.is_clockwise(): + self.x_list = list(reversed(self.x_list)) + self.y_list = list(reversed(self.y_list)) + + def is_clockwise(self): + n_data = len(self.x_list) + eval_sum = sum([(self.x_list[i + 1] - self.x_list[i]) * + (self.y_list[i + 1] + self.y_list[i]) + for i in range(n_data - 1)]) + eval_sum += (self.x_list[0] - self.x_list[n_data - 1]) * \ + (self.y_list[0] + self.y_list[n_data - 1]) + return eval_sum >= 0 def close_polygon(self): is_x_same = self.x_list[0] == self.x_list[-1] @@ -74,13 +180,15 @@ def main(): [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] + + rx, ry = VisibilityGraphPlanner(robot_radius).planning(sx, sy, gx, gy, + obstacles) + # assert rx, 'Cannot found path' + if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.axis("equal") plt.show() diff --git a/PathPlanning/VoronoiRoadMap/dijkstra_search.py b/PathPlanning/VoronoiRoadMap/dijkstra_search.py index 4d294ee5..7f248cd3 100644 --- a/PathPlanning/VoronoiRoadMap/dijkstra_search.py +++ b/PathPlanning/VoronoiRoadMap/dijkstra_search.py @@ -17,7 +17,7 @@ class DijkstraSearch: Node class for dijkstra search """ - def __init__(self, x, y, cost, parent): + def __init__(self, x, y, cost=None, parent=None): self.x = x self.y = y self.cost = cost diff --git a/tests/test_rrt_star_dubins.py b/tests/test_rrt_star_dubins.py index 8ee3d387..db1ca2bf 100644 --- a/tests/test_rrt_star_dubins.py +++ b/tests/test_rrt_star_dubins.py @@ -2,6 +2,7 @@ from unittest import TestCase import sys import os + sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../PathPlanning/RRTStarDubins/")