update RRT

Sampling-based Planning/rrt_2D/utils.py

@@ -0,0 +1,106 @@
+"""
+utils for collision check
+@author: huiming zhou
+"""
+
+import math
+import numpy as np
+import pyrr
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../Sampling-based Planning/")
+
+from rrt_2D import env
+from rrt_2D.rrt import Node
+
+
+class Utils:
+    def __init__(self):
+        self.env = env.Env()
+
+        self.delta = 0.2
+        self.obs_circle = self.env.obs_circle
+        self.obs_rectangle = self.env.obs_rectangle
+        self.obs_boundary = self.env.obs_boundary
+        self.obs_vertex = self.get_obs_vertex()
+
+    def get_obs_vertex(self):
+        delta = self.delta
+        obs_list = []
+
+        for (ox, oy, w, h) in self.obs_rectangle:
+            vertex_list = [[ox - delta, oy - delta],
+                           [ox + w + delta, oy - delta],
+                           [ox + w + delta, oy + h + delta],
+                           [ox - delta, oy + h + delta]]
+            obs_list.append(vertex_list)
+
+        return obs_list
+
+    def is_intersect_segment(self, start, end, a, b):
+        o, d = self.get_ray(start, end)
+
+        v1 = [o[0] - a[0], o[1] - a[1]]
+        v2 = [b[0] - a[0], b[1] - a[1]]
+        v3 = [-d[1], d[0]]
+
+        div = np.dot(v2, v3)
+
+        if div == 0:
+            div = 0.01
+
+        t1 = np.linalg.norm(np.cross(v2, v1)) / div
+        t2 = np.dot(v1, v3) / div
+
+        if t1 >= 0 and 0 <= t2 <= 1:
+            shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))
+            dist_obs = self.get_dist(start, shot)
+            dist_seg = self.get_dist(start, end)
+            if dist_obs <= dist_seg:
+                return True
+
+        return False
+
+    def is_collision(self, start, end):
+        if self.is_inside_obs(start) or self.is_inside_obs(end):
+            return True
+
+        for (v1, v2, v3, v4) in self.obs_vertex:
+            if self.is_intersect_segment(start, end, v1, v2) \
+                    or self.is_intersect_segment(start, end, v2, v3) \
+                    or self.is_intersect_segment(start, end, v3, v4) \
+                    or self.is_intersect_segment(start, end, v4, v1):
+                return True
+
+        return False
+
+    def is_inside_obs(self, node):
+        delta = self.delta
+
+        for (x, y, r) in self.obs_circle:
+            if math.hypot(node.x - x, node.y - y) <= r + delta:
+                return True
+
+        for (x, y, w, h) in self.obs_rectangle:
+            if 0 <= node.x - (x - delta) <= w + 2 * delta \
+                    and 0 <= node.y - (y - delta) <= h + 2 * delta:
+                return True
+
+        for (x, y, w, h) in self.obs_boundary:
+            if 0 <= node.x - (x - delta) <= w + 2 * delta \
+                    and 0 <= node.y - (y - delta) <= h + 2 * delta:
+                return True
+
+        return False
+
+    @staticmethod
+    def get_ray(start, end):
+        orig = [start.x, start.y]
+        direc = [end.x - start.x, end.y - start.y]
+        return orig, direc
+
+    @staticmethod
+    def get_dist(start, end):
+        return math.hypot(end.x - start.x, end.y - start.y)