mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-15 00:28:27 -05:00
215 lines
6.5 KiB
Python
215 lines
6.5 KiB
Python
"""
|
|
|
|
Path planning Sample Code with RRT*
|
|
|
|
author: Atsushi Sakai(@Atsushi_twi)
|
|
|
|
"""
|
|
|
|
import math
|
|
import os
|
|
import sys
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
"/../RRT/")
|
|
|
|
try:
|
|
from rrt import RRT
|
|
except ImportError:
|
|
raise
|
|
|
|
show_animation = True
|
|
|
|
|
|
class RRTStar(RRT):
|
|
"""
|
|
Class for RRT Star planning
|
|
"""
|
|
|
|
class Node(RRT.Node):
|
|
def __init__(self, x, y):
|
|
super().__init__(x, y)
|
|
self.cost = 0.0
|
|
|
|
def __init__(self, start, goal, obstacle_list, rand_area,
|
|
expand_dis=3.0,
|
|
path_resolution=1.0,
|
|
goal_sample_rate=20,
|
|
max_iter=300,
|
|
connect_circle_dist=50.0
|
|
):
|
|
super().__init__(start, goal, obstacle_list,
|
|
rand_area, expand_dis, path_resolution, goal_sample_rate, max_iter)
|
|
"""
|
|
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.connect_circle_dist = connect_circle_dist
|
|
self.goal_node = self.Node(goal[0], goal[1])
|
|
|
|
def planning(self, animation=True, search_until_max_iter=True):
|
|
"""
|
|
rrt star path planning
|
|
|
|
animation: flag for animation on or off
|
|
search_until_max_iter: search until max iteration for path improving or not
|
|
"""
|
|
|
|
self.node_list = [self.start]
|
|
for i in range(self.max_iter):
|
|
print("Iter:", i, ", number of nodes:", len(self.node_list))
|
|
rnd = self.get_random_node()
|
|
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
|
|
new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
|
|
|
|
if self.check_collision(new_node, self.obstacle_list):
|
|
near_inds = self.find_near_nodes(new_node)
|
|
new_node = self.choose_parent(new_node, near_inds)
|
|
if new_node:
|
|
self.node_list.append(new_node)
|
|
self.rewire(new_node, near_inds)
|
|
|
|
if animation and i % 5 == 0:
|
|
self.draw_graph(rnd)
|
|
|
|
if (not search_until_max_iter) and new_node: # check reaching the goal
|
|
last_index = self.search_best_goal_node()
|
|
if last_index:
|
|
return self.generate_final_course(last_index)
|
|
|
|
print("reached max iteration")
|
|
|
|
last_index = self.search_best_goal_node()
|
|
if last_index:
|
|
return self.generate_final_course(last_index)
|
|
|
|
return None
|
|
|
|
def choose_parent(self, new_node, near_inds):
|
|
if not near_inds:
|
|
return None
|
|
|
|
# search nearest cost in near_inds
|
|
costs = []
|
|
for i in near_inds:
|
|
near_node = self.node_list[i]
|
|
t_node = self.steer(near_node, new_node)
|
|
if t_node and self.check_collision(t_node, self.obstacle_list):
|
|
costs.append(self.calc_new_cost(near_node, new_node))
|
|
else:
|
|
costs.append(float("inf")) # the cost of collision node
|
|
min_cost = min(costs)
|
|
|
|
if min_cost == float("inf"):
|
|
print("There is no good path.(min_cost is inf)")
|
|
return None
|
|
|
|
min_ind = near_inds[costs.index(min_cost)]
|
|
new_node = self.steer(self.node_list[min_ind], new_node)
|
|
new_node.parent = self.node_list[min_ind]
|
|
new_node.cost = min_cost
|
|
|
|
return new_node
|
|
|
|
def search_best_goal_node(self):
|
|
dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
|
|
goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_dis]
|
|
|
|
safe_goal_inds = []
|
|
for goal_ind in goal_inds:
|
|
t_node = self.steer(self.node_list[goal_ind], self.goal_node)
|
|
if self.check_collision(t_node, self.obstacle_list):
|
|
safe_goal_inds.append(goal_ind)
|
|
|
|
if not safe_goal_inds:
|
|
return None
|
|
|
|
min_cost = min([self.node_list[i].cost for i in safe_goal_inds])
|
|
for i in safe_goal_inds:
|
|
if self.node_list[i].cost == min_cost:
|
|
return i
|
|
|
|
return None
|
|
|
|
def find_near_nodes(self, new_node):
|
|
nnode = len(self.node_list) + 1
|
|
r = self.connect_circle_dist * math.sqrt((math.log(nnode) / nnode))
|
|
dist_list = [(node.x - new_node.x) ** 2 +
|
|
(node.y - new_node.y) ** 2 for node in self.node_list]
|
|
near_inds = [dist_list.index(i) for i in dist_list if i <= r ** 2]
|
|
return near_inds
|
|
|
|
def rewire(self, new_node, near_inds):
|
|
for i in near_inds:
|
|
near_node = self.node_list[i]
|
|
edge_node = self.steer(new_node, near_node)
|
|
if not edge_node:
|
|
continue
|
|
edge_node.cost = self.calc_new_cost(new_node, near_node)
|
|
|
|
no_collision = self.check_collision(edge_node, self.obstacle_list)
|
|
improved_cost = near_node.cost > edge_node.cost
|
|
|
|
if no_collision and improved_cost:
|
|
near_node = edge_node
|
|
near_node.parent = new_node
|
|
self.propagate_cost_to_leaves(new_node)
|
|
|
|
def calc_new_cost(self, from_node, to_node):
|
|
d, _ = self.calc_distance_and_angle(from_node, to_node)
|
|
return from_node.cost + d
|
|
|
|
def propagate_cost_to_leaves(self, parent_node):
|
|
|
|
for node in self.node_list:
|
|
if node.parent == parent_node:
|
|
node.cost = self.calc_new_cost(parent_node, node)
|
|
self.propagate_cost_to_leaves(node)
|
|
|
|
|
|
def main():
|
|
print("Start " + __file__)
|
|
|
|
# ====Search Path with RRT====
|
|
obstacle_list = [
|
|
(5, 5, 1),
|
|
(3, 6, 2),
|
|
(3, 8, 2),
|
|
(3, 10, 2),
|
|
(7, 5, 2),
|
|
(9, 5, 2),
|
|
(8, 10, 1),
|
|
(6, 12, 1),
|
|
] # [x,y,size(radius)]
|
|
|
|
# Set Initial parameters
|
|
rrt_star = RRTStar(start=[0, 0],
|
|
goal=[6, 10],
|
|
rand_area=[-2, 15],
|
|
obstacle_list=obstacle_list)
|
|
path = rrt_star.planning(animation=show_animation)
|
|
|
|
if path is None:
|
|
print("Cannot find path")
|
|
else:
|
|
print("found path!!")
|
|
|
|
# Draw final path
|
|
if show_animation:
|
|
rrt_star.draw_graph()
|
|
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
|
plt.grid(True)
|
|
plt.pause(0.01) # Need for Mac
|
|
plt.show()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|