github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-14 01:28:23 -05:00
Code Issues Packages Projects Releases Wiki Activity

add reeds_shepp_path_planning code

Browse Source
This commit is contained in:
AtsushiSakai
2017-05-25 13:14:31 -07:00
parent a1b17b8b1f
commit 7e750b42bd
4 changed files with 466 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
scripts/PathPlanning/RRTStarCar_reeds_sheep/matplotrecorder.py Normal file
View File

@@ -0,0 +1,59 @@
"""
A simple Python module for recording matplotlib animation
This tool use convert command of ImageMagick
author: Atsushi Sakai
"""
import matplotlib.pyplot as plt
import subprocess
iframe = 0
donothing = False
def save_frame():
"""
Save a frame for movie
"""
if not donothing:
global iframe
plt.savefig("recoder" + '{0:04d}'.format(iframe) + '.png')
iframe += 1
def save_movie(fname, d_pause):
"""
Save movie as gif
"""
if not donothing:
cmd = "convert -delay " + str(int(d_pause * 100)) + \
" recoder*.png " + fname
subprocess.call(cmd, shell=True)
cmd = "rm recoder*.png"
subprocess.call(cmd, shell=True)
if __name__ == '__main__':
print("A sample recording start")
import math
time = range(50)
x1 = [math.cos(t / 10.0) for t in time]
y1 = [math.sin(t / 10.0) for t in time]
x2 = [math.cos(t / 10.0) + 2 for t in time]
y2 = [math.sin(t / 10.0) + 2 for t in time]
for ix1, iy1, ix2, iy2 in zip(x1, y1, x2, y2):
plt.plot(ix1, iy1, "xr")
plt.plot(ix2, iy2, "xb")
plt.axis("equal")
plt.pause(0.1)
save_frame() # save each frame
save_movie("animation.gif", 0.1)
# save_movie("animation.mp4", 0.1)

90
scripts/PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py Normal file
View File

@@ -0,0 +1,90 @@
#! /usr/bin/python
# -*- coding: utf-8 -*-
"""
Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
License MIT
"""
import reeds_shepp
import math
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u"""
Plot arrow
"""
import matplotlib.pyplot as plt
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):
q0 = [start_x, start_y, start_yaw]
q1 = [end_x, end_y, end_yaw]
step_size = 0.1
qs = reeds_shepp.path_sample(q0, q1, 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, curvature)
pathtypeTuple = reeds_shepp.path_type(q0, q1, 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
if __name__ == '__main__':
print("Reeds Shepp path planner sample start!!")
import matplotlib.pyplot as plt
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)
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()

309
scripts/PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py Normal file
View File

@@ -0,0 +1,309 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
"""
@brief: Path Planning Sample Code with RRT for car like robot.
@author: AtsushiSakai(@Atsushi_twi)
@license: MIT
"""
import random
import math
import copy
import numpy as np
import reeds_shepp_path_planning
class RRT():
u"""
Class for RRT Planning
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=1000):
u"""
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.goalSampleRate = goalSampleRate
self.maxIter = maxIter
def Planning(self, animation=True):
u"""
Pathplanning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
newNode = self.steer(rnd, nind)
# print(newNode.cost)
if self.CollisionCheck(newNode, obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
if animation and i % 5 == 0:
self.DrawGraph(rnd=rnd)
matplotrecorder.save_frame() # save each frame
# generate coruse
lastIndex = self.get_best_last_index()
# print(lastIndex)
path = self.gen_final_course(lastIndex)
return path
def choose_parent(self, newNode, nearinds):
if len(nearinds) == 0:
return newNode
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if self.CollisionCheck(tNode, 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)
return newNode
def pi_2_pi(self, angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
def steer(self, rnd, nind):
# print(rnd)
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)
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 += clen
newNode.parent = nind
return newNode
def get_random_point(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
else: # goal point sampling
rnd = [self.end.x, self.end.y, self.end.yaw]
node = Node(rnd[0], rnd[1], rnd[2])
return node
def get_best_last_index(self):
# print("get_best_last_index")
YAWTH = math.radians(3.0)
XYTH = 0.5
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
goalinds.append(i)
print("OK XY TH num is")
print(len(goalinds))
# angle check
fgoalinds = []
for i in goalinds:
if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
fgoalinds.append(i)
print("OK YAW TH num is")
print(len(fgoalinds))
mincost = min([self.nodeList[i].cost for i in fgoalinds])
for i in fgoalinds:
if self.nodeList[i].cost == mincost:
return i
return None
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
# path.append([node.x, node.y])
goalind = node.parent
path.append([self.start.x, self.start.y])
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)
obstacleOK = self.CollisionCheck(tNode, obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def DrawGraph(self, rnd=None):
u"""
Draw Graph
"""
import matplotlib.pyplot as plt
plt.clf()
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")
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in 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)
# plt.show()
# input()
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
class Node():
u"""
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
if __name__ == '__main__':
print("Start rrt start planning")
import matplotlib.pyplot as plt
import matplotrecorder
matplotrecorder.donothing = True
# ====Search Path with RRT====
obstacleList = [
(5, 5, 1),
(3, 6, 2),
(3, 8, 2),
(3, 10, 2),
(7, 5, 2),
(9, 5, 2)
] # [x,y,size(radius)]
# Set Initial parameters
start = [0.0, 0.0, math.radians(0.0)]
goal = [5.0, 10.0, math.radians(0.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=False)
# Draw final path
rrt.DrawGraph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.001)
for i in range(10):
matplotrecorder.save_frame() # save each frame
plt.show()
matplotrecorder.save_movie("animation.gif", 0.1)

12
scripts/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -38,6 +38,10 @@ def reeds_shepp_path_planning(start_x, start_y, start_yaw,
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, curvature)
pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
@@ -57,9 +61,9 @@ if __name__ == '__main__':
print("Reeds Shepp path planner sample start!!")
import matplotlib.pyplot as plt
start_x = 1.0 # [m]
start_x = 10.0 # [m]
start_y = 1.0 # [m]
start_yaw = math.radians(0.0) # [rad]
start_yaw = math.radians(180.0) # [rad]
end_x = -0.0 # [m]
end_y = -3.0 # [m]
@@ -76,8 +80,8 @@ if __name__ == '__main__':
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")
# for (ix, iy, iyaw) in zip(px, py, pyaw):
# plot_arrow(ix, iy, iyaw, fc="b")
# print(clen)
plt.legend()