"""

《基于智能優化與RRT算法的無人機任務規劃方法研究》博士論文

《基于改進人工勢場法的路徑規劃算法研究》碩士論文

"""

import matplotlib.pyplot as plt

import random

import math

import copy

show_animation = True

class Node(object):

"""

RRT Node

"""

def __init__(self, x, y):

self.x = x

self.y = y

self.parent = None

class RRT(object):

"""

Class for RRT Planning

"""

def __init__(self, start, goal, obstacle_list, rand_area):

"""

Setting Parameter

start:Start Position [x,y]

goal:Goal Position [x,y]

obstacleList:obstacle Positions [[x,y,size],...]

randArea:random sampling Area [min,max]

"""

self.start = Node(start[0], start[1])

self.end = Node(goal[0], goal[1])

self.min_rand = rand_area[0]

self.max_rand = rand_area[1]

self.expandDis = 1.0

self.goalSampleRate = 0.05 # 選擇終點的概率是0.05

self.maxIter = 500

self.obstacleList = obstacle_list

self.nodeList = [self.start]

def random_node(self):

"""

產生隨機節點

:return:

"""

node_x = random.uniform(self.min_rand, self.max_rand)

node_y = random.uniform(self.min_rand, self.max_rand)

node = [node_x, node_y]

return node

@staticmethod

def get_nearest_list_index(node_list, rnd):

"""

:param node_list:

:param rnd:

:return:

"""

d_list = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in node_list]

min_index = d_list.index(min(d_list))

return min_index

@staticmethod

def collision_check(new_node, obstacle_list):

a = 1

for (ox, oy, size) in obstacle_list:

dx = ox - new_node.x

dy = oy - new_node.y

d = math.sqrt(dx * dx + dy * dy)

if d <= size:

a = 0 # collision

return a # safe

def planning(self):

"""

Path planning

animation: flag for animation on or off

"""

while True:

# Random Sampling

if random.random() > self.goalSampleRate:

rnd = self.random_node()

else:

rnd = [self.end.x, self.end.y]

# Find nearest node

min_index = self.get_nearest_list_index(self.nodeList, rnd)

# print(min_index)

# expand tree

nearest_node = self.nodeList[min_index]

# 返回弧度制

theta = math.atan2(rnd[1] - nearest_node.y, rnd[0] - nearest_node.x)

new_node = copy.deepcopy(nearest_node)

new_node.x += self.expandDis * math.cos(theta)

new_node.y += self.expandDis * math.sin(theta)

new_node.parent = min_index

if not self.collision_check(new_node, self.obstacleList):

continue

self.nodeList.append(new_node)

# check goal

dx = new_node.x - self.end.x

dy = new_node.y - self.end.y

d = math.sqrt(dx * dx + dy * dy)

if d <= self.expandDis:

print("Goal!!")

break

if True:

self.draw_graph(rnd)

path = [[self.end.x, self.end.y]]

last_index = len(self.nodeList) - 1

while self.nodeList[last_index].parent is not None:

node = self.nodeList[last_index]

path.append([node.x, node.y])

last_index = node.parent

path.append([self.start.x, self.start.y])

return path

def draw_graph(self, rnd=None):

"""

Draw Graph

"""

print('aaa')

plt.clf() # 清除上次畫的圖

if rnd is not None:

plt.plot(rnd[0], rnd[1], "^g")

for node in self.nodeList:

if node.parent is not None:

plt.plot([node.x, self.nodeList[node.parent].x], [

node.y, self.nodeList[node.parent].y], "-g")

for (ox, oy, size) in self.obstacleList:

plt.plot(ox, oy, "sk", ms=10*size)

plt.plot(self.start.x, self.start.y, "^r")

plt.plot(self.end.x, self.end.y, "^b")

plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])

plt.grid(True)

plt.pause(0.01)

def draw_static(self, path):

"""

畫出靜態圖像

:return:

"""

plt.clf() # 清除上次畫的圖

for node in self.nodeList:

if node.parent is not None:

plt.plot([node.x, self.nodeList[node.parent].x], [

node.y, self.nodeList[node.parent].y], "-g")

for (ox, oy, size) in self.obstacleList:

plt.plot(ox, oy, "sk", ms=10*size)

plt.plot(self.start.x, self.start.y, "^r")

plt.plot(self.end.x, self.end.y, "^b")

plt.axis([self.min_rand, self.max_rand, self.min_rand, self.max_rand])

plt.plot([data[0] for data in path], [data[1] for data in path], '-r')

plt.grid(True)

plt.show()

def main():

print("start RRT path planning")

obstacle_list = [

(5, 1, 1),

(3, 6, 2),

(3, 8, 2),

(1, 1, 2),

(3, 5, 2),

(9, 5, 2)]

# Set Initial parameters

rrt = RRT(start=[0, 0], goal=[8, 9], rand_area=[-2, 10], obstacle_list=obstacle_list)

path = rrt.planning()

print(path)

# Draw final path

if show_animation:

plt.close()

rrt.draw_static(path)

if __name__ == '__main__':

main()