mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
zhm-real
116 lines
3.3 KiB
Python
116 lines
3.3 KiB
Python
"""
|
|
RRT_2D
|
|
@author: huiming zhou
|
|
"""
|
|
|
|
import math
|
|
import numpy as np
|
|
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 import plotting
|
|
from rrt_2D import utils
|
|
|
|
|
|
class Node:
|
|
def __init__(self, n):
|
|
self.x = n[0]
|
|
self.y = n[1]
|
|
self.parent = None
|
|
|
|
|
|
class Rrt:
|
|
def __init__(self, s_start, s_goal, step_len, goal_sample_rate, iter_max):
|
|
self.s_start = Node(s_start)
|
|
self.s_goal = Node(s_goal)
|
|
self.step_len = step_len
|
|
self.goal_sample_rate = goal_sample_rate
|
|
self.iter_max = iter_max
|
|
self.vertex = [self.s_start]
|
|
|
|
self.env = env.Env()
|
|
self.plotting = plotting.Plotting(s_start, s_goal)
|
|
self.utils = utils.Utils()
|
|
|
|
self.x_range = self.env.x_range
|
|
self.y_range = self.env.y_range
|
|
self.obs_circle = self.env.obs_circle
|
|
self.obs_rectangle = self.env.obs_rectangle
|
|
self.obs_boundary = self.env.obs_boundary
|
|
|
|
def planning(self):
|
|
for i in range(self.iter_max):
|
|
node_rand = self.random_state(self.goal_sample_rate)
|
|
node_near = self.nearest_neighbor(self.vertex, node_rand)
|
|
node_new = self.new_state(node_near, node_rand)
|
|
|
|
if node_new and not self.utils.is_collision(node_near, node_new):
|
|
self.vertex.append(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)
|
|
return self.extract_path(node_new)
|
|
|
|
return None
|
|
|
|
def random_state(self, goal_sample_rate):
|
|
delta = self.utils.delta
|
|
|
|
if np.random.random() > goal_sample_rate:
|
|
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)))
|
|
|
|
return self.s_goal
|
|
|
|
def nearest_neighbor(self, node_list, n):
|
|
return self.vertex[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y)
|
|
for nd in node_list]))]
|
|
|
|
def new_state(self, node_start, node_end):
|
|
dist, theta = self.get_distance_and_angle(node_start, node_end)
|
|
|
|
dist = min(self.step_len, dist)
|
|
node_new = Node((node_start.x + dist * math.cos(theta),
|
|
node_start.y + dist * math.sin(theta)))
|
|
node_new.parent = node_start
|
|
|
|
return node_new
|
|
|
|
def extract_path(self, node_end):
|
|
path = [(self.s_goal.x, self.s_goal.y)]
|
|
node_now = node_end
|
|
|
|
while node_now.parent is not None:
|
|
node_now = node_now.parent
|
|
path.append((node_now.x, node_now.y))
|
|
|
|
return path
|
|
|
|
@staticmethod
|
|
def get_distance_and_angle(node_start, node_end):
|
|
dx = node_end.x - node_start.x
|
|
dy = node_end.y - node_start.y
|
|
return math.hypot(dx, dy), math.atan2(dy, dx)
|
|
|
|
|
|
def main():
|
|
x_start = (2, 2) # Starting node
|
|
x_goal = (49, 24) # Goal node
|
|
|
|
rrt = Rrt(x_start, x_goal, 0.5, 0.03, 5000)
|
|
path = rrt.planning()
|
|
|
|
if path:
|
|
rrt.plotting.animation(rrt.vertex, path, "RRT", True)
|
|
else:
|
|
print("No Path Found!")
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|