mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-13 04:28:04 -05:00
cleanup closed loop RRT star.
This commit is contained in:
Atsushi Sakai
@@ -1,14 +1,13 @@
|
||||
"""
|
||||
|
||||
Path planning Sample Code with Closed loop RRT for car like robot.
|
||||
|
||||
author: AtsushiSakai(@Atsushi_twi)
|
||||
|
||||
"""
|
||||
|
||||
import copy
|
||||
import math
|
||||
import os
|
||||
import random
|
||||
import sys
|
||||
|
||||
import matplotlib.pyplot as plt
|
||||
@@ -18,92 +17,49 @@ import pure_pursuit
|
||||
|
||||
sys.path.append(os.path.dirname(
|
||||
os.path.abspath(__file__)) + "/../ReedsSheppPath/")
|
||||
sys.path.append(os.path.dirname(
|
||||
os.path.abspath(__file__)) + "/../RRTStarReedsShepp/")
|
||||
|
||||
try:
|
||||
import reeds_shepp_path_planning
|
||||
import unicycle_model
|
||||
except:
|
||||
from rrt_star_reeds_shepp import RRTStarReedsShepp
|
||||
except ImportError:
|
||||
raise
|
||||
|
||||
show_animation = True
|
||||
|
||||
|
||||
target_speed = 10.0 / 3.6
|
||||
STEP_SIZE = 0.1
|
||||
|
||||
|
||||
class RRT():
|
||||
class ClosedLoopRRTStar(RRTStarReedsShepp):
|
||||
"""
|
||||
Class for RRT planning
|
||||
Class for Closed loop RRT star planning
|
||||
"""
|
||||
|
||||
def __init__(self, start, goal, obstacleList, randArea,
|
||||
maxIter=50):
|
||||
def __init__(self, start, goal, obstacle_list, rand_area,
|
||||
max_iter=200,
|
||||
connect_circle_dist=50.0
|
||||
):
|
||||
super().__init__(start, goal, obstacle_list, rand_area,
|
||||
max_iter=max_iter,
|
||||
connect_circle_dist=connect_circle_dist,
|
||||
)
|
||||
|
||||
self.target_speed = 10.0 / 3.6
|
||||
self.yaw_th = np.deg2rad(3.0)
|
||||
self.xy_th = 0.5
|
||||
self.invalid_travel_ratio = 5.0
|
||||
|
||||
def planning(self, animation=True):
|
||||
"""
|
||||
Setting Parameter
|
||||
|
||||
start:Start Position [x,y]
|
||||
goal:Goal Position [x,y]
|
||||
obstacleList:obstacle Positions [[x,y,size],...]
|
||||
randArea:Ramdom Samping Area [min,max]
|
||||
|
||||
"""
|
||||
self.start = Node(start[0], start[1], start[2])
|
||||
self.end = Node(goal[0], goal[1], goal[2])
|
||||
self.minrand = randArea[0]
|
||||
self.maxrand = randArea[1]
|
||||
self.obstacleList = obstacleList
|
||||
self.maxIter = maxIter
|
||||
|
||||
def try_goal_path(self):
|
||||
|
||||
goal = Node(self.end.x, self.end.y, self.end.yaw)
|
||||
|
||||
newNode = self.steer(goal, len(self.nodeList) - 1)
|
||||
if newNode is None:
|
||||
return
|
||||
|
||||
if self.CollisionCheck(newNode, self.obstacleList):
|
||||
# print("goal path is OK")
|
||||
self.nodeList.append(newNode)
|
||||
|
||||
def Planning(self, animation=True):
|
||||
"""
|
||||
Pathplanning
|
||||
do planning
|
||||
|
||||
animation: flag for animation on or off
|
||||
"""
|
||||
|
||||
self.nodeList = [self.start]
|
||||
|
||||
self.try_goal_path()
|
||||
|
||||
for i in range(self.maxIter):
|
||||
print("loop:", i)
|
||||
rnd = self.get_random_point()
|
||||
nind = self.GetNearestListIndex(self.nodeList, rnd)
|
||||
|
||||
newNode = self.steer(rnd, nind)
|
||||
# print(newNode.cost)
|
||||
if newNode is None:
|
||||
continue
|
||||
|
||||
if self.CollisionCheck(newNode, self.obstacleList):
|
||||
nearinds = self.find_near_nodes(newNode)
|
||||
newNode = self.choose_parent(newNode, nearinds)
|
||||
if newNode is None:
|
||||
continue
|
||||
|
||||
self.nodeList.append(newNode)
|
||||
self.rewire(newNode, nearinds)
|
||||
|
||||
self.try_goal_path()
|
||||
|
||||
if animation and i % 5 == 0:
|
||||
self.DrawGraph(rnd=rnd)
|
||||
# planning with RRTStarReedsShepp
|
||||
super().planning(animation=animation)
|
||||
|
||||
# generate coruse
|
||||
path_indexs = self.get_best_last_indexs()
|
||||
path_indexs = self.get_goal_indexes()
|
||||
|
||||
flag, x, y, yaw, v, t, a, d = self.search_best_feasible_path(
|
||||
path_indexs)
|
||||
@@ -120,10 +76,9 @@ class RRT():
|
||||
|
||||
# pure pursuit tracking
|
||||
for ind in path_indexs:
|
||||
path = self.gen_final_course(ind)
|
||||
path = self.generate_final_course(ind)
|
||||
|
||||
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(
|
||||
path)
|
||||
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(path)
|
||||
|
||||
if flag and best_time >= t[-1]:
|
||||
print("feasible path is found")
|
||||
@@ -142,121 +97,46 @@ class RRT():
|
||||
return False, None, None, None, None, None, None, None
|
||||
|
||||
def check_tracking_path_is_feasible(self, path):
|
||||
# print("check_tracking_path_is_feasible")
|
||||
cx = np.array(path[:, 0])
|
||||
cy = np.array(path[:, 1])
|
||||
cyaw = np.array(path[:, 2])
|
||||
cx = np.array([state[0] for state in path])[::-1]
|
||||
cy = np.array([state[1] for state in path])[::-1]
|
||||
cyaw = np.array([state[2] for state in path])[::-1]
|
||||
|
||||
goal = [cx[-1], cy[-1], cyaw[-1]]
|
||||
|
||||
cx, cy, cyaw = pure_pursuit.extend_path(cx, cy, cyaw)
|
||||
|
||||
speed_profile = pure_pursuit.calc_speed_profile(
|
||||
cx, cy, cyaw, target_speed)
|
||||
cx, cy, cyaw, self.target_speed)
|
||||
|
||||
t, x, y, yaw, v, a, d, find_goal = pure_pursuit.closed_loop_prediction(
|
||||
cx, cy, cyaw, speed_profile, goal)
|
||||
yaw = [self.pi_2_pi(iyaw) for iyaw in yaw]
|
||||
yaw = [reeds_shepp_path_planning.pi_2_pi(iyaw) for iyaw in yaw]
|
||||
|
||||
if not find_goal:
|
||||
print("cannot reach goal")
|
||||
|
||||
if abs(yaw[-1] - goal[2]) >= math.pi / 4.0:
|
||||
if abs(yaw[-1] - goal[2]) >= self.yaw_th * 10.0:
|
||||
print("final angle is bad")
|
||||
find_goal = False
|
||||
|
||||
travel = sum([abs(iv) * unicycle_model.dt for iv in v])
|
||||
# print(travel)
|
||||
origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2)
|
||||
for (dx, dy) in zip(np.diff(cx), np.diff(cy))])
|
||||
# print(origin_travel)
|
||||
|
||||
if (travel / origin_travel) >= 5.0:
|
||||
if (travel / origin_travel) >= self.invalid_travel_ratio:
|
||||
print("path is too long")
|
||||
find_goal = False
|
||||
|
||||
if not self.CollisionCheckWithXY(x, y, self.obstacleList):
|
||||
if not self.collision_check_with_xy(x, y, self.obstacle_list):
|
||||
print("This path is collision")
|
||||
find_goal = False
|
||||
|
||||
return find_goal, x, y, yaw, v, t, a, d
|
||||
|
||||
def choose_parent(self, newNode, nearinds):
|
||||
if not nearinds:
|
||||
return newNode
|
||||
|
||||
dlist = []
|
||||
for i in nearinds:
|
||||
tNode = self.steer(newNode, i)
|
||||
if tNode is None:
|
||||
continue
|
||||
|
||||
if self.CollisionCheck(tNode, self.obstacleList):
|
||||
dlist.append(tNode.cost)
|
||||
else:
|
||||
dlist.append(float("inf"))
|
||||
|
||||
mincost = min(dlist)
|
||||
minind = nearinds[dlist.index(mincost)]
|
||||
|
||||
if mincost == float("inf"):
|
||||
print("mincost is inf")
|
||||
return newNode
|
||||
|
||||
newNode = self.steer(newNode, minind)
|
||||
if newNode is None:
|
||||
return None
|
||||
|
||||
return newNode
|
||||
|
||||
def pi_2_pi(self, angle):
|
||||
return (angle + math.pi) % (2 * math.pi) - math.pi
|
||||
|
||||
def steer(self, rnd, nind):
|
||||
# print(rnd)
|
||||
|
||||
nearestNode = self.nodeList[nind]
|
||||
|
||||
px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
|
||||
nearestNode.x, nearestNode.y, nearestNode.yaw,
|
||||
rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE)
|
||||
|
||||
if px is None:
|
||||
return None
|
||||
|
||||
newNode = copy.deepcopy(nearestNode)
|
||||
newNode.x = px[-1]
|
||||
newNode.y = py[-1]
|
||||
newNode.yaw = pyaw[-1]
|
||||
|
||||
newNode.path_x = px
|
||||
newNode.path_y = py
|
||||
newNode.path_yaw = pyaw
|
||||
newNode.cost += sum([abs(c) for c in clen])
|
||||
newNode.parent = nind
|
||||
|
||||
return newNode
|
||||
|
||||
def get_random_point(self):
|
||||
|
||||
rnd = [random.uniform(self.minrand, self.maxrand),
|
||||
random.uniform(self.minrand, self.maxrand),
|
||||
random.uniform(-math.pi, math.pi)
|
||||
]
|
||||
|
||||
node = Node(rnd[0], rnd[1], rnd[2])
|
||||
|
||||
return node
|
||||
|
||||
def get_best_last_indexs(self):
|
||||
# print("search_finish_node")
|
||||
|
||||
YAWTH = np.deg2rad(1.0)
|
||||
XYTH = 0.5
|
||||
|
||||
def get_goal_indexes(self):
|
||||
goalinds = []
|
||||
for (i, node) in enumerate(self.nodeList):
|
||||
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
|
||||
for (i, node) in enumerate(self.node_list):
|
||||
if self.calc_dist_to_goal(node.x, node.y) <= self.xy_th:
|
||||
goalinds.append(i)
|
||||
print("OK XY TH num is")
|
||||
print(len(goalinds))
|
||||
@@ -264,106 +144,17 @@ class RRT():
|
||||
# angle check
|
||||
fgoalinds = []
|
||||
for i in goalinds:
|
||||
if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
|
||||
if abs(self.node_list[i].yaw - self.end.yaw) <= self.yaw_th:
|
||||
fgoalinds.append(i)
|
||||
print("OK YAW TH num is")
|
||||
print(len(fgoalinds))
|
||||
|
||||
return fgoalinds
|
||||
|
||||
def gen_final_course(self, goalind):
|
||||
path = [[self.end.x, self.end.y, self.end.yaw]]
|
||||
while self.nodeList[goalind].parent is not None:
|
||||
node = self.nodeList[goalind]
|
||||
path_x = reversed(node.path_x)
|
||||
path_y = reversed(node.path_y)
|
||||
path_yaw = reversed(node.path_yaw)
|
||||
for (ix, iy, iyaw) in zip(path_x, path_y, path_yaw):
|
||||
path.append([ix, iy, iyaw])
|
||||
# path.append([node.x, node.y])
|
||||
goalind = node.parent
|
||||
path.append([self.start.x, self.start.y, self.start.yaw])
|
||||
@staticmethod
|
||||
def collision_check_with_xy(x, y, obstacle_list):
|
||||
|
||||
path = np.array(path[::-1])
|
||||
return path
|
||||
|
||||
def calc_dist_to_goal(self, x, y):
|
||||
return np.linalg.norm([x - self.end.x, y - self.end.y])
|
||||
|
||||
def find_near_nodes(self, newNode):
|
||||
nnode = len(self.nodeList)
|
||||
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
|
||||
# r = self.expandDis * 5.0
|
||||
dlist = [(node.x - newNode.x) ** 2
|
||||
+ (node.y - newNode.y) ** 2
|
||||
+ (node.yaw - newNode.yaw) ** 2
|
||||
for node in self.nodeList]
|
||||
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
|
||||
return nearinds
|
||||
|
||||
def rewire(self, newNode, nearinds):
|
||||
|
||||
nnode = len(self.nodeList)
|
||||
|
||||
for i in nearinds:
|
||||
nearNode = self.nodeList[i]
|
||||
tNode = self.steer(nearNode, nnode - 1)
|
||||
|
||||
if tNode is None:
|
||||
continue
|
||||
|
||||
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
|
||||
imporveCost = nearNode.cost > tNode.cost
|
||||
|
||||
if obstacleOK and imporveCost:
|
||||
# print("rewire")
|
||||
self.nodeList[i] = tNode
|
||||
|
||||
def DrawGraph(self, rnd=None): # pragma: no cover
|
||||
"""
|
||||
Draw Graph
|
||||
"""
|
||||
if rnd is not None:
|
||||
plt.plot(rnd.x, rnd.y, "^k")
|
||||
for node in self.nodeList:
|
||||
if node.parent is not None:
|
||||
plt.plot(node.path_x, node.path_y, "-g")
|
||||
|
||||
for (ox, oy, size) in self.obstacleList:
|
||||
plt.plot(ox, oy, "ok", ms=30 * size)
|
||||
|
||||
reeds_shepp_path_planning.plot_arrow(
|
||||
self.start.x, self.start.y, self.start.yaw)
|
||||
reeds_shepp_path_planning.plot_arrow(
|
||||
self.end.x, self.end.y, self.end.yaw)
|
||||
|
||||
plt.axis([-2, 15, -2, 15])
|
||||
plt.grid(True)
|
||||
plt.pause(0.01)
|
||||
|
||||
def GetNearestListIndex(self, nodeList, rnd):
|
||||
dlist = [(node.x - rnd.x) ** 2
|
||||
+ (node.y - rnd.y) ** 2
|
||||
+ (node.yaw - rnd.yaw) ** 2 for node in nodeList]
|
||||
minind = dlist.index(min(dlist))
|
||||
|
||||
return minind
|
||||
|
||||
def CollisionCheck(self, node, obstacleList):
|
||||
|
||||
for (ox, oy, size) in obstacleList:
|
||||
for (ix, iy) in zip(node.path_x, node.path_y):
|
||||
dx = ox - ix
|
||||
dy = oy - iy
|
||||
d = dx * dx + dy * dy
|
||||
if d <= size ** 2:
|
||||
return False # collision
|
||||
|
||||
return True # safe
|
||||
|
||||
def CollisionCheckWithXY(self, x, y, obstacleList):
|
||||
|
||||
for (ox, oy, size) in obstacleList:
|
||||
for (ox, oy, size) in obstacle_list:
|
||||
for (ix, iy) in zip(x, y):
|
||||
dx = ox - ix
|
||||
dy = oy - iy
|
||||
@@ -374,26 +165,10 @@ class RRT():
|
||||
return True # safe
|
||||
|
||||
|
||||
class Node():
|
||||
"""
|
||||
RRT Node
|
||||
"""
|
||||
|
||||
def __init__(self, x, y, yaw):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.yaw = yaw
|
||||
self.path_x = []
|
||||
self.path_y = []
|
||||
self.path_yaw = []
|
||||
self.cost = 0.0
|
||||
self.parent = None
|
||||
|
||||
|
||||
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200):
|
||||
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100):
|
||||
print("Start" + __file__)
|
||||
# ====Search Path with RRT====
|
||||
obstacleList = [
|
||||
obstacle_list = [
|
||||
(5, 5, 1),
|
||||
(4, 6, 1),
|
||||
(4, 8, 1),
|
||||
@@ -409,16 +184,18 @@ def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200):
|
||||
start = [0.0, 0.0, np.deg2rad(0.0)]
|
||||
goal = [gx, gy, gyaw]
|
||||
|
||||
rrt = RRT(start, goal, randArea=[-2.0, 20.0],
|
||||
obstacleList=obstacleList, maxIter=maxIter)
|
||||
flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation)
|
||||
closed_loop_rrt_star = ClosedLoopRRTStar(start, goal,
|
||||
obstacle_list,
|
||||
[-2.0, 20.0],
|
||||
max_iter=max_iter)
|
||||
flag, x, y, yaw, v, t, a, d = closed_loop_rrt_star.planning(animation=show_animation)
|
||||
|
||||
if not flag:
|
||||
print("cannot find feasible path")
|
||||
|
||||
# Draw final path
|
||||
if show_animation:
|
||||
rrt.DrawGraph()
|
||||
closed_loop_rrt_star.draw_graph()
|
||||
plt.plot(x, y, '-r')
|
||||
plt.grid(True)
|
||||
plt.pause(0.001)
|
||||
|
||||
@@ -72,7 +72,7 @@ class LQRRRTStar(RRTStar):
|
||||
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_list_index(self.node_list, rnd)
|
||||
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
|
||||
new_node = self.steer(self.node_list[nearest_ind], rnd)
|
||||
|
||||
if self.check_collision(new_node, self.obstacle_list):
|
||||
|
||||
@@ -63,7 +63,7 @@ class RRT:
|
||||
self.node_list = [self.start]
|
||||
for i in range(self.max_iter):
|
||||
rnd_node = self.get_random_node()
|
||||
nearest_ind = self.get_nearest_list_index(self.node_list, rnd_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)
|
||||
@@ -154,7 +154,7 @@ class RRT:
|
||||
plt.pause(0.01)
|
||||
|
||||
@staticmethod
|
||||
def get_nearest_list_index(node_list, rnd_node):
|
||||
def get_nearest_node_index(node_list, rnd_node):
|
||||
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y)
|
||||
** 2 for node in node_list]
|
||||
minind = dlist.index(min(dlist))
|
||||
|
||||
@@ -80,7 +80,7 @@ class RRTDubins(RRT):
|
||||
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_list_index(self.node_list, rnd)
|
||||
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
|
||||
new_node = self.steer(self.node_list[nearest_ind], rnd)
|
||||
|
||||
if self.check_collision(new_node, self.obstacle_list):
|
||||
|
||||
@@ -65,7 +65,7 @@ class RRTStar(RRT):
|
||||
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_list_index(self.node_list, rnd)
|
||||
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):
|
||||
|
||||
@@ -81,7 +81,7 @@ class RRTStarDubins(RRTStar):
|
||||
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_list_index(self.node_list, rnd)
|
||||
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
|
||||
new_node = self.steer(self.node_list[nearest_ind], rnd)
|
||||
|
||||
if self.check_collision(new_node, self.obstacle_list):
|
||||
|
||||
@@ -44,7 +44,6 @@ class RRTStarReedsShepp(RRTStar):
|
||||
self.path_yaw = []
|
||||
|
||||
def __init__(self, start, goal, obstacle_list, rand_area,
|
||||
goal_sample_rate=10,
|
||||
max_iter=200,
|
||||
connect_circle_dist=50.0
|
||||
):
|
||||
@@ -61,7 +60,6 @@ class RRTStarReedsShepp(RRTStar):
|
||||
self.end = self.Node(goal[0], goal[1], goal[2])
|
||||
self.min_rand = rand_area[0]
|
||||
self.max_rand = rand_area[1]
|
||||
self.goal_sample_rate = goal_sample_rate
|
||||
self.max_iter = max_iter
|
||||
self.obstacle_list = obstacle_list
|
||||
self.connect_circle_dist = connect_circle_dist
|
||||
@@ -72,7 +70,7 @@ class RRTStarReedsShepp(RRTStar):
|
||||
|
||||
def planning(self, animation=True, search_until_max_iter=True):
|
||||
"""
|
||||
RRT Star planning
|
||||
planning
|
||||
|
||||
animation: flag for animation on or off
|
||||
"""
|
||||
@@ -81,7 +79,7 @@ class RRTStarReedsShepp(RRTStar):
|
||||
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_list_index(self.node_list, rnd)
|
||||
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
|
||||
new_node = self.steer(self.node_list[nearest_ind], rnd)
|
||||
|
||||
if self.check_collision(new_node, self.obstacle_list):
|
||||
@@ -90,6 +88,7 @@ class RRTStarReedsShepp(RRTStar):
|
||||
if new_node:
|
||||
self.node_list.append(new_node)
|
||||
self.rewire(new_node, near_indexes)
|
||||
self.try_goal_path(new_node)
|
||||
|
||||
if animation and i % 5 == 0:
|
||||
self.plot_start_goal_arrow()
|
||||
@@ -110,6 +109,17 @@ class RRTStarReedsShepp(RRTStar):
|
||||
|
||||
return None
|
||||
|
||||
def try_goal_path(self, node):
|
||||
|
||||
goal = self.Node(self.end.x, self.end.y, self.end.yaw)
|
||||
|
||||
new_node = self.steer(node, goal)
|
||||
if new_node is None:
|
||||
return
|
||||
|
||||
if self.check_collision(new_node, self.obstacle_list):
|
||||
self.node_list.append(new_node)
|
||||
|
||||
def draw_graph(self, rnd=None):
|
||||
plt.clf()
|
||||
if rnd is not None:
|
||||
@@ -151,7 +161,7 @@ class RRTStarReedsShepp(RRTStar):
|
||||
new_node.path_x = px
|
||||
new_node.path_y = py
|
||||
new_node.path_yaw = pyaw
|
||||
new_node.cost += sum(course_lengths)
|
||||
new_node.cost += sum([abs(l) for l in course_lengths])
|
||||
new_node.parent = from_node
|
||||
|
||||
return new_node
|
||||
@@ -164,17 +174,14 @@ class RRTStarReedsShepp(RRTStar):
|
||||
if not course_lengths:
|
||||
return float("inf")
|
||||
|
||||
return from_node.cost + sum(course_lengths)
|
||||
return from_node.cost + sum([abs(l) for l in course_lengths])
|
||||
|
||||
def get_random_node(self):
|
||||
|
||||
if random.randint(0, 100) > self.goal_sample_rate:
|
||||
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
|
||||
random.uniform(self.min_rand, self.max_rand),
|
||||
random.uniform(-math.pi, math.pi)
|
||||
)
|
||||
else: # goal point sampling
|
||||
rnd = self.Node(self.end.x, self.end.y, self.end.yaw)
|
||||
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
|
||||
random.uniform(self.min_rand, self.max_rand),
|
||||
random.uniform(-math.pi, math.pi)
|
||||
)
|
||||
|
||||
return rnd
|
||||
|
||||
@@ -184,6 +191,7 @@ class RRTStarReedsShepp(RRTStar):
|
||||
for (i, node) in enumerate(self.node_list):
|
||||
if self.calc_dist_to_goal(node.x, node.y) <= self.goal_xy_th:
|
||||
goal_indexes.append(i)
|
||||
print("goal_indexes:", len(goal_indexes))
|
||||
|
||||
# angle check
|
||||
final_goal_indexes = []
|
||||
@@ -191,10 +199,13 @@ class RRTStarReedsShepp(RRTStar):
|
||||
if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th:
|
||||
final_goal_indexes.append(i)
|
||||
|
||||
print("final_goal_indexes:", len(final_goal_indexes))
|
||||
|
||||
if not final_goal_indexes:
|
||||
return None
|
||||
|
||||
min_cost = min([self.node_list[i].cost for i in final_goal_indexes])
|
||||
print("min_cost:", min_cost)
|
||||
for i in final_goal_indexes:
|
||||
if self.node_list[i].cost == min_cost:
|
||||
return i
|
||||
@@ -202,18 +213,17 @@ class RRTStarReedsShepp(RRTStar):
|
||||
return None
|
||||
|
||||
def generate_final_course(self, goal_index):
|
||||
print("final")
|
||||
path = [[self.end.x, self.end.y]]
|
||||
path = [[self.end.x, self.end.y, self.end.yaw]]
|
||||
node = self.node_list[goal_index]
|
||||
while node.parent:
|
||||
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
|
||||
path.append([ix, iy])
|
||||
for (ix, iy, iyaw) in zip(reversed(node.path_x), reversed(node.path_y), reversed(node.path_yaw)):
|
||||
path.append([ix, iy, iyaw])
|
||||
node = node.parent
|
||||
path.append([self.start.x, self.start.y])
|
||||
path.append([self.start.x, self.start.y, self.start.yaw])
|
||||
return path
|
||||
|
||||
|
||||
def main(max_iter=200):
|
||||
def main(max_iter=100):
|
||||
print("Start " + __file__)
|
||||
|
||||
# ====Search Path with RRT====
|
||||
@@ -241,7 +251,7 @@ def main(max_iter=200):
|
||||
# Draw final path
|
||||
if path and show_animation: # pragma: no cover
|
||||
rrt_star_reeds_shepp.draw_graph()
|
||||
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
|
||||
plt.plot([x for (x, y, yaw) in path], [y for (x, y, yaw) in path], '-r')
|
||||
plt.grid(True)
|
||||
plt.pause(0.001)
|
||||
plt.show()
|
||||
|
||||
Reference in New Issue
Block a user