update

Sampling-based Planning/rrt_2D/dynamic_rrt.py

@@ -6,6 +6,7 @@ DYNAMIC_RRT_2D
 import os
 import sys
 import math
+import copy
 import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib.patches as patches
@@ -23,12 +24,14 @@ class Node:
         self.x = n[0]
         self.y = n[1]
         self.parent = None
+        self.flag = "VALID"
 
 
 class Edge:
     def __init__(self, n_p, n_c):
         self.parent = n_p
         self.child = n_c
+        self.flag = "VALID"
 
 
 class DynamicRrt:
@@ -40,7 +43,9 @@ class DynamicRrt:
         self.waypoint_sample_rate = waypoint_sample_rate
         self.iter_max = iter_max
         self.vertex = [self.s_start]
-        self.edges = set()
+        self.vertex_old = []
+        self.vertex_new = []
+        self.edges = []
 
         self.env = env.Env()
         self.plotting = plotting.Plotting(s_start, s_goal)
@@ -52,6 +57,7 @@ class DynamicRrt:
         self.obs_circle = self.env.obs_circle
         self.obs_rectangle = self.env.obs_rectangle
         self.obs_boundary = self.env.obs_boundary
+        self.obs_add = [0, 0, 0]
 
         self.path = []
         self.waypoint = []
@@ -64,13 +70,14 @@ class DynamicRrt:
 
             if node_new and not self.utils.is_collision(node_near, node_new):
                 self.vertex.append(node_new)
+                self.edges.append(Edge(node_near, node_new))
                 dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
 
                 if dist <= self.step_len:
                     self.new_state(node_new, self.s_goal)
 
                     path = self.extract_path(node_new)
-                    self.plot_grid("Extended_RRT")
+                    self.plot_grid("Dynamic_RRT")
                     self.plot_visited()
                     self.plot_path(path)
                     self.path = path
@@ -89,21 +96,68 @@ class DynamicRrt:
         else:
             x, y = int(x), int(y)
             print("Add circle obstacle at: x =", x, ",", "y =", y)
+            self.obs_add = [x, y, 2]
             self.obs_circle.append([x, y, 2])
             self.utils.update_obs(self.obs_circle, self.obs_boundary, self.obs_rectangle)
-            path, waypoint = self.replanning()
-
-            plt.cla()
-            self.plot_grid("Extended_RRT")
-            self.plot_path(self.path, color='blue')
-            self.plot_visited()
-            self.plot_path(path)
-            self.path = path
-            self.waypoint = waypoint
+            self.InvalidateNodes()
+
+            if self.is_path_invalid():
+                print("Path is Replanning ...")
+                path, waypoint = self.replanning()
+
+                print("len_vertex: ", len(self.vertex))
+                print("len_vertex_old: ", len(self.vertex_old))
+                print("len_vertex_new: ", len(self.vertex_new))
+
+                plt.cla()
+                self.plot_grid("Dynamic_RRT")
+                self.plot_vertex_old()
+                self.plot_path(self.path, color='blue')
+                self.plot_vertex_new()
+                self.vertex_new = []
+                self.plot_path(path)
+                self.path = path
+                self.waypoint = waypoint
+            else:
+                print("Trimming Invalid Nodes ...")
+                self.TrimRRT()
+
+                plt.cla()
+                self.plot_grid("Dynamic_RRT")
+                self.plot_visited(animation=False)
+                self.plot_path(self.path)
+
             self.fig.canvas.draw_idle()
 
+    def InvalidateNodes(self):
+        for edge in self.edges:
+            if self.is_collision_obs_add(edge.parent, edge.child):
+                edge.child.flag = "INVALID"
+                edge.flag = "INVALID"
+
+    def is_path_invalid(self):
+        for node in self.waypoint:
+            if node.flag == "INVALID":
+                return True
+
+    def is_collision_obs_add(self, start, end):
+        delta = self.utils.delta
+        obs_add = self.obs_add
+
+        if math.hypot(start.x - obs_add[0], start.y - obs_add[1]) <= obs_add[2] + delta:
+            return True
+
+        if math.hypot(end.x - obs_add[0], end.y - obs_add[1]) <= obs_add[2] + delta:
+            return True
+
+        o, d = self.utils.get_ray(start, end)
+        if self.utils.is_intersect_circle(o, d, [obs_add[0], obs_add[1]], obs_add[2]):
+            return True
+
+        return False
+
     def replanning(self):
-        self.vertex = [self.s_start]
+        self.TrimRRT()
 
         for i in range(self.iter_max):
             node_rand = self.generate_random_node_replanning(self.goal_sample_rate, self.waypoint_sample_rate)
@@ -112,17 +166,32 @@ class DynamicRrt:
 
             if node_new and not self.utils.is_collision(node_near, node_new):
                 self.vertex.append(node_new)
+                self.vertex_new.append(node_new)
+                self.edges.append(Edge(node_near, node_new))
                 dist, _ = self.get_distance_and_angle(node_new, self.s_goal)
 
                 if dist <= self.step_len:
                     self.new_state(node_new, self.s_goal)
                     path = self.extract_path(node_new)
                     waypoint = self.extract_waypoint(node_new)
+                    print("path: ", len(path))
+                    print("waypoint: ", len(waypoint))
 
                     return path, waypoint
 
         return None
 
+    def TrimRRT(self):
+        for i in range(1, len(self.vertex)):
+            node = self.vertex[i]
+            node_p = node.parent
+            if node_p.flag == "INVALID":
+                node.flag = "INVALID"
+
+        self.vertex = [node for node in self.vertex if node.flag == "VALID"]
+        self.vertex_old = copy.deepcopy(self.vertex)
+        self.edges = [Edge(node.parent, node) for node in self.vertex[1:len(self.vertex)]]
+
     def generate_random_node(self, goal_sample_rate):
         delta = self.utils.delta
 
@@ -139,12 +208,11 @@ class DynamicRrt:
         if p < goal_sample_rate:
             return self.s_goal
         elif goal_sample_rate < p < goal_sample_rate + waypoint_sample_rate:
-            return self.waypoint[np.random.randint(0, len(self.path) - 1)]
+            return self.waypoint[np.random.randint(0, len(self.waypoint) - 1)]
         else:
             return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
                          np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
 
-
     @staticmethod
     def nearest_neighbor(node_list, n):
         return node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
@@ -224,8 +292,7 @@ class DynamicRrt:
         plt.title(name)
         plt.axis("equal")
 
-    def plot_visited(self):
-        animation = True
+    def plot_visited(self, animation=True):
         if animation:
             count = 0
             for node in self.vertex:
@@ -242,6 +309,24 @@ class DynamicRrt:
                 if node.parent:
                     plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
 
+    def plot_vertex_old(self):
+        for node in self.vertex_old:
+            if node.parent:
+                plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g")
+
+    def plot_vertex_new(self):
+        count = 0
+
+        for node in self.vertex_new:
+            count += 1
+            if node.parent:
+                plt.plot([node.parent.x, node.x], [node.parent.y, node.y], color='darkorange')
+                plt.gcf().canvas.mpl_connect('key_release_event',
+                                             lambda event:
+                                             [exit(0) if event.key == 'escape' else None])
+                if count % 10 == 0:
+                    plt.pause(0.001)
+
     @staticmethod
     def plot_path(path, color='red'):
         plt.plot([x[0] for x in path], [x[1] for x in path], linewidth=2, color=color)