github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 07:27:56 -05:00
Code Issues Packages Projects Releases Wiki Activity

test fixed

Browse Source
This commit is contained in:
Atsushi Sakai
2018-02-07 23:42:30 -08:00
parent 0b1ca74408
commit fa3692da3e
7 changed files with 46 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -21,7 +21,7 @@ show_animation = True
target_speed = 10.0 / 3.6
STEP_SIZE = 0.5
STEP_SIZE = 0.1
class RRT():
@@ -52,6 +52,8 @@ class RRT():
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")
@@ -74,10 +76,14 @@ class RRT():
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)
@@ -97,6 +103,8 @@ class RRT():
def search_best_feasible_path(self, path_indexs):
print("Start search feasible path")
best_time = float("inf")
# pure pursuit tracking
@@ -189,6 +197,9 @@ class RRT():
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:
@@ -202,6 +213,8 @@ class RRT():
return newNode
newNode = self.steer(newNode, minind)
if newNode is None:
return None
return newNode
@@ -223,6 +236,9 @@ class RRT():
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]
@@ -231,7 +247,7 @@ class RRT():
newNode.path_x = px
newNode.path_y = py
newNode.path_yaw = pyaw
newNode.cost += sum(clen)
newNode.cost += sum([abs(c) for c in clen])
newNode.parent = nind
return newNode
@@ -308,6 +324,9 @@ class RRT():
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

92
PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py
View File

@@ -1,92 +0,0 @@
"""
Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
"""
import reeds_shepp
import math
import matplotlib.pyplot as plt
show_animation = True
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
"""
Plot arrow
"""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
def reeds_shepp_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature):
step_size = 0.1
q0 = [start_x, start_y, start_yaw]
q1 = [end_x, end_y, end_yaw]
qs = reeds_shepp.path_sample(q0, q1, 1.0 / curvature, step_size)
xs = [q[0] for q in qs]
ys = [q[1] for q in qs]
yaw = [q[2] for q in qs]
xs.append(end_x)
ys.append(end_y)
yaw.append(end_yaw)
clen = reeds_shepp.path_length(q0, q1, 1.0 / curvature)
pathtypeTuple = reeds_shepp.path_type(q0, q1, 1.0 / curvature)
ptype = ""
for t in pathtypeTuple:
if t == 1:
ptype += "L"
elif t == 2:
ptype += "S"
elif t == 3:
ptype += "R"
return xs, ys, yaw, ptype, clen
def main():
print("Reeds Shepp path planner sample start!!")
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = math.radians(0.0) # [rad]
end_x = -0.0 # [m]
end_y = -3.0 # [m]
end_yaw = math.radians(-45.0) # [rad]
curvature = 1.0
px, py, pyaw, mode, clen = reeds_shepp_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
if show_animation:
plt.plot(px, py, label="final course " + str(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
for (ix, iy, iyaw) in zip(px, py, pyaw):
plot_arrow(ix, iy, iyaw, fc="b")
# print(clen)
plt.legend()
plt.grid(True)
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main()

22
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
View File

@@ -6,6 +6,9 @@ author: AtsushiSakai(@Atsushi_twi)
"""
import sys
sys.path.append("../ReedsSheppPath/")
import random
import math
import copy
@@ -14,6 +17,8 @@ import reeds_shepp_path_planning
import matplotlib.pyplot as plt
show_animation = True
STEP_SIZE = 0.1
curvature = 1.0
class RRT():
@@ -53,10 +58,14 @@ class RRT():
nind = self.GetNearestListIndex(self.nodeList, rnd)
newNode = self.steer(rnd, nind)
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)
@@ -77,6 +86,9 @@ class RRT():
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:
@@ -103,12 +115,14 @@ class RRT():
return angle
def steer(self, rnd, nind):
curvature = 1.0
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, curvature)
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature, STEP_SIZE)
if px is None:
return None
newNode = copy.deepcopy(nearestNode)
newNode.x = px[-1]
@@ -118,7 +132,7 @@ class RRT():
newNode.path_x = px
newNode.path_y = py
newNode.path_yaw = pyaw
newNode.cost += clen
newNode.cost += sum([abs(c) for c in clen])
newNode.parent = nind
return newNode
@@ -200,6 +214,8 @@ class RRT():
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

2
PathPlanning/RRTstar/rrt_star.py
View File

@@ -65,6 +65,8 @@ class RRT():
# generate coruse
lastIndex = self.get_best_last_index()
if lastIndex is None:
return None
path = self.gen_final_course(lastIndex)
return path

5
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -369,7 +369,8 @@ def reeds_shepp_path_planning(sx, sy, syaw,
paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)
if len(paths) == 0:
print("No path")
# print("No path")
# print(sx, sy, syaw, gx, gy, gyaw)
return None, None, None, None, None
minL = float("Inf")
@@ -443,7 +444,7 @@ def main():
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, step_size)
if show_animation:
plt.plot(px, py, label="final course " + str(mode))
# plt.plot(px, py, label="final course " + str(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)

1
tests/test_reeds_shepp_path_planning.py
View File

@@ -7,4 +7,3 @@ class Test(TestCase):
def test1(self):
m.show_animation = False
m.main()
m.test()

1
tests/test_rrt_star_reeds_shepp.py
View File

@@ -2,6 +2,7 @@ from unittest import TestCase
import sys
sys.path.append("./PathPlanning/RRTStarReedsShepp/")
sys.path.append("./PathPlanning/ReedsSheppPath/")
from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m