289 lines
10 KiB
Python
289 lines
10 KiB
Python
|
"""
|
|||
|
|
|
|||
|
|
Path planning Sample Code with RRT*
|
|||
|
|
|
|||
|
|
author: Atsushi Sakai(@Atsushi_twi)
|
|||
|
|
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
import math
|
|||
|
|
import sys
|
|||
|
|
import matplotlib.pyplot as plt
|
|||
|
|
import pathlib
|
|||
|
|
from rrt import RRT
|
|||
|
|
|
|||
|
|
sys.path.append(str(pathlib.Path(__file__).parent.parent))
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
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,
|
|||
|
|
rand_area,
|
|||
|
|
map,
|
|||
|
|
scale_ratio=5,
|
|||
|
|
expand_dis=100.0,
|
|||
|
|
path_resolution=10.0,
|
|||
|
|
goal_sample_rate=10,
|
|||
|
|
max_iter=300,
|
|||
|
|
connect_circle_dist=50.0, # new
|
|||
|
|
search_until_max_iter=False, # new
|
|||
|
|
robot_radius=5.0):
|
|||
|
|
|
|||
|
|
super().__init__(rand_area, map, scale_ratio, expand_dis,
|
|||
|
|
path_resolution, goal_sample_rate, max_iter,
|
|||
|
|
robot_radius=robot_radius)
|
|||
|
|
self.connect_circle_dist = connect_circle_dist
|
|||
|
|
self.search_until_max_iter = search_until_max_iter
|
|||
|
|
self.node_list = []
|
|||
|
|
|
|||
|
|
def planning(self, start, goal, path_smoothing=True, animation=False) -> [(float, float), ...]:
|
|||
|
|
"""
|
|||
|
|
rrt star path planning, return a path list
|
|||
|
|
animation: flag for animation on or off .
|
|||
|
|
"""
|
|||
|
|
(self.start.x, self.start.y) = start
|
|||
|
|
(self.goal.x, self.goal.y) = goal
|
|||
|
|
# self.update_play_area()
|
|||
|
|
self.node_list = [self.start]
|
|||
|
|
while len(self.node_list) < self.max_iter:
|
|||
|
|
#for i in range(self.max_iter):
|
|||
|
|
# print("Iter:", i, ", number of nodes:", len(self.node_list))
|
|||
|
|
print("number of nodes:", len(self.node_list))
|
|||
|
|
rnd_node = self.get_random_node()
|
|||
|
|
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
|
|||
|
|
nearest_node = self.node_list[nearest_ind]
|
|||
|
|
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
|
|||
|
|
|
|||
|
|
if animation:
|
|||
|
|
self.draw_graph(new_node)
|
|||
|
|
|
|||
|
|
if self.check_collision(new_node):
|
|||
|
|
new_node.cost = nearest_node.cost + \
|
|||
|
|
math.hypot(new_node.x - nearest_node.x,
|
|||
|
|
new_node.y - nearest_node.y)
|
|||
|
|
near_inds = self.find_near_nodes(new_node)
|
|||
|
|
# node_with_updated_parent: 已经找到父节点的new_node
|
|||
|
|
node_with_updated_parent = self.choose_parent(new_node, near_inds)
|
|||
|
|
if node_with_updated_parent:
|
|||
|
|
self.rewire(node_with_updated_parent, near_inds)
|
|||
|
|
self.node_list.append(node_with_updated_parent)
|
|||
|
|
else:
|
|||
|
|
self.node_list.append(new_node) # ??? 不可能发生
|
|||
|
|
|
|||
|
|
# 目标检测
|
|||
|
|
if not self.search_until_max_iter:
|
|||
|
|
last_index = self.search_best_goal_node() # 找到目标单步范围内的总距离最短的节点
|
|||
|
|
if last_index is not None:
|
|||
|
|
path = self.generate_final_course(last_index)
|
|||
|
|
if path_smoothing:
|
|||
|
|
return self.path_smoothing(path)
|
|||
|
|
else:
|
|||
|
|
return path
|
|||
|
|
|
|||
|
|
print("reached max iteration")
|
|||
|
|
|
|||
|
|
last_index = self.search_best_goal_node()
|
|||
|
|
if last_index is not None:
|
|||
|
|
path = self.generate_final_course(last_index)
|
|||
|
|
if path_smoothing:
|
|||
|
|
return self.path_smoothing(path)
|
|||
|
|
else:
|
|||
|
|
return path
|
|||
|
|
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
def choose_parent(self, new_node, near_inds):
|
|||
|
|
"""
|
|||
|
|
为 new_node 选择(从起点开始)总距离最小的父节点
|
|||
|
|
Computes the cheapest point to new_node contained in the list
|
|||
|
|
near_inds and set such a node as the parent of new_node.
|
|||
|
|
Arguments:
|
|||
|
|
--------
|
|||
|
|
new_node, Node
|
|||
|
|
randomly generated node with a path from its neared point
|
|||
|
|
There are not coalitions between this node and th tree.
|
|||
|
|
near_inds: list
|
|||
|
|
Indices of indices of the nodes what are near to new_node
|
|||
|
|
|
|||
|
|
Returns.
|
|||
|
|
------
|
|||
|
|
Node, a copy of new_node
|
|||
|
|
"""
|
|||
|
|
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):
|
|||
|
|
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设置父节点
|
|||
|
|
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)
|
|||
|
|
if self.check_collision(t_node):
|
|||
|
|
safe_goal_inds.append(goal_ind)
|
|||
|
|
|
|||
|
|
if not safe_goal_inds:
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
safe_goal_costs = [self.node_list[i].cost + # 从起点经过安全节点到目标的距离
|
|||
|
|
self.calc_dist_to_goal(self.node_list[i].x, self.node_list[i].y)
|
|||
|
|
for i in safe_goal_inds]
|
|||
|
|
|
|||
|
|
min_cost = min(safe_goal_costs)
|
|||
|
|
for i, cost in zip(safe_goal_inds, safe_goal_costs):
|
|||
|
|
if cost == min_cost:
|
|||
|
|
return i
|
|||
|
|
|
|||
|
|
return None
|
|||
|
|
|
|||
|
|
def find_near_nodes(self, new_node):
|
|||
|
|
"""
|
|||
|
|
找到 new_node 周围一定范围内的树中的节点
|
|||
|
|
1) defines a ball centered on new_node
|
|||
|
|
2) Returns all nodes of the three that are inside this ball
|
|||
|
|
Arguments:
|
|||
|
|
---------
|
|||
|
|
new_node: Node
|
|||
|
|
new randomly generated node, without collisions between
|
|||
|
|
its nearest node
|
|||
|
|
Returns:
|
|||
|
|
-------
|
|||
|
|
list
|
|||
|
|
List with the indices of the nodes inside the ball of
|
|||
|
|
radius r
|
|||
|
|
"""
|
|||
|
|
nnode = len(self.node_list) + 1
|
|||
|
|
r = self.connect_circle_dist * math.sqrt(math.log(nnode) / nnode)
|
|||
|
|
# if expand_dist exists, search vertices in a range no more than expand_dist
|
|||
|
|
if hasattr(self, 'expand_dis'):
|
|||
|
|
r = min(r, self.expand_dis)
|
|||
|
|
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 each node in near_inds, this will check if it is cheaper to
|
|||
|
|
arrive to them from new_node.
|
|||
|
|
In such a case, this will re-assign the parent of the nodes in
|
|||
|
|
near_inds to new_node.
|
|||
|
|
Parameters:
|
|||
|
|
----------
|
|||
|
|
new_node, Node
|
|||
|
|
Node randomly added which can be joined to the tree
|
|||
|
|
|
|||
|
|
near_inds, list of uints
|
|||
|
|
A list of indices of the self.new_node which contains
|
|||
|
|
nodes within a circle of a given radius.
|
|||
|
|
Remark: parent is designated in choose_parent.
|
|||
|
|
|
|||
|
|
"""
|
|||
|
|
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)
|
|||
|
|
improved_cost = near_node.cost > edge_node.cost
|
|||
|
|
|
|||
|
|
if no_collision and improved_cost:
|
|||
|
|
for node in self.node_list:
|
|||
|
|
if node.parent == near_node:
|
|||
|
|
node.parent = edge_node
|
|||
|
|
self.node_list[i] = edge_node
|
|||
|
|
self.propagate_cost_to_leaves(self.node_list[i])
|
|||
|
|
|
|||
|
|
def calc_new_cost(self, from_node, to_node):
|
|||
|
|
'''
|
|||
|
|
从起始位置经过 from_node 到 to_node 的 cost
|
|||
|
|
'''
|
|||
|
|
d, _ = self.calc_distance_and_angle(from_node, to_node)
|
|||
|
|
return from_node.cost + d
|
|||
|
|
|
|||
|
|
def propagate_cost_to_leaves(self, parent_node):
|
|||
|
|
'''
|
|||
|
|
(递归算法) 从父节点不断向子节点传播，计算cost
|
|||
|
|
'''
|
|||
|
|
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(rand_area=[-2, 15], expand_dis=1, robot_radius=0.8)
|
|||
|
|
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.show()
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == '__main__':
|
|||
|
|
main()
|