first release universal sampling method

Author: AtsushiSakai

.idea/dictionaries/atsushisakai.xml

@@ -8,6 +8,7 @@
 import math
 import model_predictive_trajectory_generator as planner
 import motion_model
+import pandas as pd
 
 
 def calc_states_list():
@@ -22,13 +23,12 @@ def calc_states_list():
         for iy in y:
             for ix in x:
                 states.append([ix, iy, iyaw])
-    #  print(len(states))
+    # print(len(states))
 
     return states
 
 
 def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
-
     mind = float("inf")
     minid = -1
 
@@ -37,18 +37,33 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
         dx = tx - table[0]
         dy = ty - table[1]
         dyaw = tyaw - table[2]
-        d = math.sqrt(dx**2 + dy**2 + dyaw**2)
+        d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
         if d <= mind:
             minid = i
             mind = d
 
-    #  print(minid)
+    # print(minid)
 
     return lookuptable[minid]
 
 
-def generate_lookup_table():
+def save_lookup_table(fname, table):
+    mt = np.array(table)
+    print(mt)
+    # save csv
+    df = pd.DataFrame()
+    df["x"] = mt[:, 0]
+    df["y"] = mt[:, 1]
+    df["yaw"] = mt[:, 2]
+    df["s"] = mt[:, 3]
+    df["km"] = mt[:, 4]
+    df["kf"] = mt[:, 5]
+    df.to_csv(fname, index=None)
+
+    print("lookup table file is saved as " + fname)
 
+
+def generate_lookup_table():
     states = calc_states_list()
     k0 = 0.0
 
@@ -58,11 +73,10 @@ def generate_lookup_table():
     for state in states:
         bestp = search_nearest_one_from_lookuptable(
             state[0], state[1], state[2], lookuptable)
-        #  print(bestp)
 
         target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
         init_p = np.matrix(
-            [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
+            [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
 
         x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
 
@@ -71,7 +85,9 @@ def generate_lookup_table():
             lookuptable.append(
                 [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
 
-    print("finish lookuptable generation")
+    print("finish lookup table generation")
+
+    save_lookup_table("lookuptable.csv", lookuptable)
 
     for table in lookuptable:
         xc, yc, yawc = motion_model.generate_trajectory(

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -0,0 +1,25 @@
+x,y,yaw,s,km,kf
+1.0,0.0,0.0,1.0,0.0,0.0
+0.9734888894493215,-0.009758406565994977,0.5358080146312756,0.9922329557399788,-0.10222538550473198,3.0262632253145982
+10.980728996433243,-0.0003093605787364978,0.522622783944529,11.000391678142623,0.00010296091030877934,0.2731556687244648
+16.020309241920156,0.0001292339008200291,0.5243399938698222,16.100019813021202,0.00013263212395994706,0.18999138959173634
+20.963495745193626,-0.00033031017429944326,0.5226120033275024,21.10082901143343,0.00011687467551566884,0.14550546012583987
+6.032553475650599,2.008504211720188,0.5050517859971599,6.400329805864408,0.1520002249689879,-0.13105940607691127
+10.977487445230075,2.0078696810700034,0.5263634407901872,11.201040572298973,0.04895863722280565,0.08356555007223682
+15.994057699325753,2.025659106131227,0.5303858891065698,16.200300421483128,0.0235708657178127,0.10002225103921249
+20.977228843605943,2.0281289825388513,0.5300376140865567,21.20043308669372,0.013795675421657671,0.09331700188063087
+25.95453914157977,1.9926432818499131,0.5226203078411618,26.200880299840527,0.00888830054451281,0.0830622000626594
+0.9999999999999999,0.0,0.0,1.0,0.0,0.0
+5.999999999670752,5.231312388722274e-05,1.4636120911014667e-05,5.996117185283419,4.483756968024291e-06,-3.4422519205046243e-06
+10.999749470720566,-0.011978787043239986,0.022694802592583763,10.99783855994015,-0.00024209503125174496,0.01370491008661795
+15.999885224357776,-0.018937230084507616,0.011565580878015763,15.99839381389597,-0.0002086399372890716,0.005267247862667496
+20.999882688881286,-0.030304200126461317,0.009117836885732596,20.99783120184498,-0.00020013159571184735,0.0034529188783636866
+25.999911270030413,-0.025754431694664327,0.0074809531598503615,25.99674782258235,-0.0001111138115390646,0.0021946603965658368
+10.952178818975062,1.993067260835455,0.0045572480669707136,11.17961498195845,0.04836813285436623,-0.19328716251760758
+16.028306009258,2.0086286208315407,0.009306594796759554,16.122411866381054,0.02330689045417979,-0.08877002085985948
+20.971603115419715,1.9864158336174966,0.007016819403167119,21.093006725076872,0.013439123130720928,-0.05238318300611623
+25.997019676818372,1.9818581321430093,0.007020172975955249,26.074021794586585,0.00876496148602802,-0.03362579291686346
+16.017903482982604,4.009490840390534,-5.293114796312698e-05,16.600937712976638,0.044543450568614244,-0.17444651314466567
+20.98845988414163,3.956600199823583,-0.01050744134070728,21.40149119463485,0.02622674388276059,-0.10625681152519345
+25.979448381017914,3.9968223055054977,-0.00012819253386682928,26.30504721211744,0.017467093413146118,-0.06914750106424669
+25.96268055563514,5.9821266846168,4.931311239565056e-05,26.801388563459287,0.025426008913226557,-0.10047663078001536

PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder

@@ -0,0 +1,89 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+.hypothesis/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# IPython Notebook
+.ipynb_checkpoints
+
+# pyenv
+.python-version
+
+# celery beat schedule file
+celerybeat-schedule
+
+# dotenv
+.env
+
+# virtualenv
+venv/
+ENV/
+
+# Spyder project settings
+.spyderproject
+
+# Rope project settings
+.ropeproject

PathPlanning/StateLatticePlanner/Figure_1.png

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2017 Atsushi Sakai
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

PathPlanning/StateLatticePlanner/lookuptable.csv

@@ -0,0 +1,59 @@
+# matplotrecorder
+A simple Python module for recording matplotlib animation
+
+It can generate a matplotlib animation movie (mp4, gif, etc.)
+
+This tool use "convert" command of ImageMagick.
+
+# Sample gif
+
+![matplotrecorder/animation.gif at master · AtsushiSakai/matplotrecorder](https://github.com/AtsushiSakai/matplotrecorder/blob/master/animation.gif)
+
+# Requrements
+
+- [ImageMagic](https://www.imagemagick.org/script/index.php)
+
+
+# How to use
+
+Call save_frame() at each animation iteration,
+
+And then, call savemovie() for movie generation.
+
+A sample code:
+
+
+      import matplotrecorder
+
+      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)
+
+          matplotrecorder.save_frame()  # save each frame
+
+      # generate movie
+      matplotrecorder.save_movie("animation.mp4", 0.1)
+      #  matplotrecorder.save_movie("animation.gif", 0.1) #gif is ok.
+
+
+
+# License 
+
+MIT
+
+# Author
+
+Atsushi Sakai ([@Atsushi_twi](https://twitter.com/Atsushi_twi))
+