AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2018-01-13 10:03:45 -08:00
parent 6bd4942400
commit ebeb3d955b
4 changed files with 32 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
PathPlanning/RRTStarReedsShepp/matplotrecorder.py
View File

@@ -1,59 +0,0 @@
"""
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)

15
PathPlanning/RRTStarReedsShepp/reeds_shepp_path_planning.py
View File

@@ -1,23 +1,19 @@
#! /usr/bin/python
# -*- coding: utf-8 -*-
"""
Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
License MIT
"""
import reeds_shepp
import math
import matplotlib.pyplot as plt
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):
@@ -57,9 +53,8 @@ def reeds_shepp_path_planning(start_x, start_y, start_yaw,
return xs, ys, yaw, ptype, clen
if __name__ == '__main__':
def main():
print("Reeds Shepp path planner sample start!!")
import matplotlib.pyplot as plt
start_x = 1.0 # [m]
start_y = 1.0 # [m]
@@ -88,3 +83,7 @@ if __name__ == '__main__':
plt.grid(True)
plt.axis("equal")
plt.show()
if __name__ == '__main__':
main()

60
PathPlanning/RRTStarReedsShepp/rrt_star_car.py → PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
View File

@@ -1,11 +1,8 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
"""
@brief: Path Planning Sample Code with RRT for car like robot.
@author: AtsushiSakai(@Atsushi_twi)
Path Planning Sample Code with RRT for car like robot.
@license: MIT
author: AtsushiSakai(@Atsushi_twi)
"""
@@ -14,16 +11,19 @@ import math
import copy
import numpy as np
import reeds_shepp_path_planning
import matplotlib.pyplot as plt
show_animation = True
class RRT():
u"""
"""
Class for RRT Planning
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=1000):
u"""
goalSampleRate=10, maxIter=200):
"""
Setting Parameter
start:Start Position [x,y]
@@ -38,9 +38,10 @@ class RRT():
self.maxrand = randArea[1]
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
def Planning(self, animation=True):
u"""
"""
Pathplanning
animation: flag for animation on or off
@@ -48,14 +49,12 @@ class RRT():
self.nodeList = [self.start]
for i in range(self.maxIter):
print(i)
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
newNode = self.steer(rnd, nind)
# print(newNode.cost)
if self.CollisionCheck(newNode, obstacleList):
if self.CollisionCheck(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
self.nodeList.append(newNode)
@@ -63,11 +62,9 @@ class RRT():
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
@@ -78,7 +75,7 @@ class RRT():
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if self.CollisionCheck(tNode, obstacleList):
if self.CollisionCheck(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
@@ -104,7 +101,6 @@ class RRT():
return angle
def steer(self, rnd, nind):
# print(rnd)
curvature = 1.0
nearestNode = self.nodeList[nind]
@@ -149,16 +145,16 @@ class RRT():
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))
# 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))
# print("OK YAW TH num is")
# print(len(fgoalinds))
mincost = min([self.nodeList[i].cost for i in fgoalinds])
for i in fgoalinds:
@@ -200,7 +196,7 @@ class RRT():
nearNode = self.nodeList[i]
tNode = self.steer(nearNode, nnode - 1)
obstacleOK = self.CollisionCheck(tNode, obstacleList)
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
@@ -208,10 +204,9 @@ class RRT():
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")
@@ -221,7 +216,7 @@ class RRT():
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in obstacleList:
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(
@@ -258,7 +253,7 @@ class RRT():
class Node():
u"""
"""
RRT Node
"""
@@ -273,11 +268,8 @@ class Node():
self.parent = None
if __name__ == '__main__':
def main():
print("Start rrt start planning")
import matplotlib.pyplot as plt
import matplotrecorder
matplotrecorder.donothing = True
# ====Search Path with RRT====
# obstacleList = [
@@ -305,17 +297,15 @@ if __name__ == '__main__':
goal = [6.0, 7.0, math.radians(90.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=False)
path = rrt.Planning(animation=show_animation)
# 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)
if __name__ == '__main__':
main()

1
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -1,4 +1,3 @@
#! /usr/bin/python
"""
Reeds Shepp path planner sample code