diff --git a/README.md b/README.md index 88795ce3..ab291b1f 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ Path Planning Samples. Rapidly Randamized Tree Path plainning sample. -![PythonによるRapidly-Exploring Random Trees (RRT)パスプランニングサンプルプログラム - MyEnigma](http://cdn-ak.f.st-hatena.com/images/fotolife/m/meison_amsl/20160320/20160320175657.png) +![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/scripts/PathPlanning/RRT/animation.gif) This script is a simple path planning code with Rapidly-Exploring Random Trees (RRT) @@ -30,7 +30,7 @@ see (in Japanese) : ## RRTStar -![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/scripts/PathPlanning/RRTstar/figure_1.png) +![PythonRobotics/figure_1.png at master · AtsushiSakai/PythonRobotics](https://github.com/AtsushiSakai/PythonRobotics/blob/master/scripts/PathPlanning/RRTStar/animation.gif) This script is a path planning code with RRT \* diff --git a/scripts/PathPlanning/RRT/animation.gif b/scripts/PathPlanning/RRT/animation.gif new file mode 100644 index 00000000..72c7fe59 Binary files /dev/null and b/scripts/PathPlanning/RRT/animation.gif differ diff --git a/scripts/PathPlanning/RRT/simple_rrt.py b/scripts/PathPlanning/RRT/simple_rrt.py index 2c5c9842..a71b8503 100644 --- a/scripts/PathPlanning/RRT/simple_rrt.py +++ b/scripts/PathPlanning/RRT/simple_rrt.py @@ -13,6 +13,8 @@ u""" import random import math import copy +import matplotrecorder +matplotrecorder.donothing = True class RRT(): @@ -115,6 +117,7 @@ class RRT(): plt.axis([-2, 15, -2, 15]) plt.grid(True) plt.pause(0.01) + matplotrecorder.save_frame() # save each frame def GetNearestListIndex(self, nodeList, rnd): dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) @@ -166,4 +169,8 @@ if __name__ == '__main__': plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.grid(True) plt.pause(0.01) # Need for Mac - plt.show() + # plt.show() + for i in range(10): + matplotrecorder.save_frame() # save each frame + + matplotrecorder.save_movie("animation.gif", 0.1) diff --git a/scripts/PathPlanning/RRTCar/matplotrecorder.py b/scripts/PathPlanning/RRTCar/matplotrecorder.py index 9948cb5b..7d5ae858 100644 --- a/scripts/PathPlanning/RRTCar/matplotrecorder.py +++ b/scripts/PathPlanning/RRTCar/matplotrecorder.py @@ -10,27 +10,30 @@ import matplotlib.pyplot as plt import subprocess iframe = 0 +donothing = False def save_frame(): """ Save a frame for movie """ - global iframe - plt.savefig("recoder" + '{0:04d}'.format(iframe) + '.png') - iframe += 1 + 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 """ - 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 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__': diff --git a/scripts/PathPlanning/RRTstar/matplotrecorder.py b/scripts/PathPlanning/RRTstar/matplotrecorder.py new file mode 100644 index 00000000..7d5ae858 --- /dev/null +++ b/scripts/PathPlanning/RRTstar/matplotrecorder.py @@ -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) diff --git a/scripts/PathPlanning/RRTstar/rrt_star.py b/scripts/PathPlanning/RRTstar/rrt_star.py index dc944759..721696fa 100644 --- a/scripts/PathPlanning/RRTstar/rrt_star.py +++ b/scripts/PathPlanning/RRTstar/rrt_star.py @@ -13,6 +13,8 @@ import random import math import copy import numpy as np +import matplotrecorder +matplotrecorder.donothing = False class RRT(): @@ -21,7 +23,7 @@ class RRT(): """ def __init__(self, start, goal, obstacleList, randArea, - expandDis=0.5, goalSampleRate=20, maxIter=5000): + expandDis=0.5, goalSampleRate=20, maxIter=1000): u""" Setting Parameter @@ -45,7 +47,6 @@ class RRT(): animation: flag for animation on or off """ - animation = False self.nodeList = [self.start] for i in range(self.maxIter): @@ -204,6 +205,7 @@ class RRT(): plt.axis([-2, 15, -2, 15]) plt.grid(True) plt.pause(0.01) + matplotrecorder.save_frame() # save each frame def GetNearestListIndex(self, nodeList, rnd): dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) @@ -258,4 +260,9 @@ if __name__ == '__main__': plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.grid(True) plt.pause(0.01) # Need for Mac + for i in range(10): + matplotrecorder.save_frame() # save each frame + plt.show() + + matplotrecorder.save_movie("animation.gif", 0.1)