Remove pandas dependency (AtsushiSakai#725)

* Remove pandas dependency

* Remove pandas dependency

Author: AtsushiSakai

PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py

@@ -0,0 +1,110 @@
+"""
+
+Lookup Table generation for model predictive trajectory generator
+
+author: Atsushi Sakai
+
+"""
+import sys
+import pathlib
+path_planning_dir = pathlib.Path(__file__).parent.parent
+sys.path.append(str(path_planning_dir))
+
+from matplotlib import pyplot as plt
+import numpy as np
+import math
+
+from ModelPredictiveTrajectoryGenerator import trajectory_generator,\
+    motion_model
+
+
+def calc_states_list(max_yaw=np.deg2rad(-30.0)):
+
+    x = np.arange(10.0, 30.0, 5.0)
+    y = np.arange(0.0, 20.0, 2.0)
+    yaw = np.arange(-max_yaw, max_yaw, max_yaw)
+
+    states = []
+    for iyaw in yaw:
+        for iy in y:
+            for ix in x:
+                states.append([ix, iy, iyaw])
+    print("n_state:", len(states))
+
+    return states
+
+
+def search_nearest_one_from_lookup_table(tx, ty, tyaw, lookup_table):
+    mind = float("inf")
+    minid = -1
+
+    for (i, table) in enumerate(lookup_table):
+
+        dx = tx - table[0]
+        dy = ty - table[1]
+        dyaw = tyaw - table[2]
+        d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
+        if d <= mind:
+            minid = i
+            mind = d
+
+    # print(minid)
+
+    return lookup_table[minid]
+
+
+def save_lookup_table(file_name, table):
+    np.savetxt(file_name, np.array(table),
+               fmt='%s', delimiter=",", header="x,y,yaw,s,km,kf", comments="")
+
+    print("lookup table file is saved as " + file_name)
+
+
+def generate_lookup_table():
+    states = calc_states_list(max_yaw=np.deg2rad(-30.0))
+    k0 = 0.0
+
+    # x, y, yaw, s, km, kf
+    lookup_table = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
+
+    for state in states:
+        best_p = search_nearest_one_from_lookup_table(
+            state[0], state[1], state[2], lookup_table)
+
+        target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
+        init_p = np.array(
+            [np.hypot(state[0], state[1]), best_p[4], best_p[5]]).reshape(3, 1)
+
+        x, y, yaw, p = trajectory_generator.optimize_trajectory(target,
+                                                                k0, init_p)
+
+        if x is not None:
+            print("find good path")
+            lookup_table.append(
+                [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
+
+    print("finish lookup table generation")
+
+    save_lookup_table("lookup_table.csv", lookup_table)
+
+    for table in lookup_table:
+        x_c, y_c, yaw_c = motion_model.generate_trajectory(
+            table[3], table[4], table[5], k0)
+        plt.plot(x_c, y_c, "-r")
+        x_c, y_c, yaw_c = motion_model.generate_trajectory(
+            table[3], -table[4], -table[5], k0)
+        plt.plot(x_c, y_c, "-r")
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+    print("Done")
+
+
+def main():
+    generate_lookup_table()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -0,0 +1,82 @@
+x,y,yaw,s,km,kf
+1.0,0.0,0.0,1.0,0.0,0.0
+9.975352133559392,0.0482183513545736,0.5660837104214496,10.00000000000002,0.0015736624607596847,0.31729782170754367
+15.021899857204536,0.023109001221800096,0.541061424167431,15.128443053611093,0.0006480950273134919,0.20847475849103875
+20.062147834064536,0.0406648159648112,0.5374967866814861,20.205553097986094,0.000452860235044122,0.15502921850050788
+24.924468605496358,-0.04047324014767662,0.5146575311501209,25.16775431470035,6.940620839146646e-05,0.12259452810198132
+9.971782095506931,1.9821448683146543,0.5206511572266477,10.287478424823748,0.05861430948618236,0.07036494964262185
+15.00170010872385,2.0003864283110473,0.5236741185892617,15.245993376540827,0.02657730439557895,0.09933479864250763
+19.991716537639487,1.9940629519465154,0.5226444441451559,20.277923997037238,0.015108540596275507,0.09478988637993524
+24.946447973869596,2.0815930190993206,0.5306354765686239,25.20115925294013,0.010036251429787917,0.08505936469481987
+10.033694822745312,3.9724800521928056,0.5349578864544497,11.087694168363686,0.10279429366023954,-0.12501011715795404
+15.010712586144749,4.0046776414868095,0.5234972445048012,15.729582155835587,0.05010930398580602,-0.0008557723200034717
+19.9175798257299,4.053680042954521,0.5265397234296523,20.52466275843717,0.029584390559990882,0.035276591227371874
+24.98769016158626,3.991699950598091,0.5229000018897194,25.543297770996556,0.018800715817231053,0.04750751098144048
+10.018665105170687,6.004814533505462,0.5183921334245007,12.249438857228967,0.13207408248643182,-0.2742892277307502
+14.988626131330372,5.991226410357179,0.5248160422552801,16.53593823576699,0.06924423592936522,-0.08634227857486092
+20.014420271646646,6.006767110727591,0.5233060851224174,21.23271362632659,0.041402041787912916,-0.01770377839603589
+24.998338724667267,5.997352722087869,0.5235621854299422,26.009046544833613,0.027285850882345728,0.011507045054418165
+10.040118020822444,8.017131894913126,0.5076867575242261,13.8061261785273,0.14561700178565884,-0.3527538468214878
+14.96397914886416,7.974972375534203,0.5303378183744862,17.667338175004062,0.08318912494381935,-0.15372672981944802
+20.045725182938817,8.023486945646207,0.5201839069343577,22.126364299043573,0.05173969669894265,-0.06557547083017647
+25.004687466358227,8.00036398460779,0.5234938146870878,26.740089158520917,0.034867425244601645,-0.02199309906456302
+10.065138949829214,10.03244363616002,0.49375882493214895,15.701940360254637,0.14847998727328912,-0.39355037236614626
+15.05373212031198,10.026401491912143,0.5111826366036252,19.105461052226858,0.09205576730549936,-0.20458802229704312
+19.965550451103926,9.977668905006206,0.5278605653376056,23.14082140870299,0.06010674632014157,-0.10340577652521214
+25.04062496016141,9.956781577401756,0.5252395961316738,27.641194908523495,0.04115083532669924,-0.05054407239730677
+9.980172344087242,11.981944953180841,0.5354924711458446,17.764377273124804,0.14616069587267325,-0.40115138946106776
+15.031707905116134,12.011530784459552,0.5157261778129998,20.745000892047745,0.0970285785481706,-0.2379719980195133
+20.05384921212195,12.02621662711961,0.5153884987125119,24.513013940487117,0.06601543941341544,-0.13666530932375262
+25.04326204059146,12.019946808479768,0.5198699857547844,28.74306622689766,0.04671545692054678,-0.07827401225777673
+10.005005976167096,13.993516346269931,0.5249997047973469,20.063732124124442,0.14007166951362482,-0.39994549637994103
+15.013469777117386,13.998572375088138,0.5211760827701193,22.591299061495683,0.0989196134377572,-0.25909446951756165
+19.980150236409695,13.98233838451409,0.5278966095736082,25.971685915503254,0.07029833263412807,-0.15993299513197096
+25.009669110020404,14.000751236643762,0.5227555344229664,29.949071374991423,0.05106114063333748,-0.09952052168406796
+9.996712859814979,15.986637217372996,0.5301458018536311,22.47478825250167,0.1329741433122606,-0.38823042580907063
+15.02373126750475,16.00384009060484,0.5182833077580984,24.557463511123004,0.0989491582793761,-0.26836010532851323
+20.023756339113735,16.004847752803656,0.5197401980953318,27.669260302891157,0.07275720314277462,-0.178811991371391
+25.015003771679122,16.002919774604504,0.5219791758565742,31.36524983655211,0.05429827198598215,-0.11766440355003502
+10.078596822781892,18.025309925487992,0.49768408992179225,25.02580432036455,0.1252233187334947,-0.3747545825918585
+15.001968473293188,17.988033772017467,0.5262415135796346,26.67625473617623,0.09746306394892065,-0.27167606206451594
+20.026047062413117,18.00445752595148,0.5193568548054093,29.442158339897595,0.07417227896231118,-0.19015828496001386
+24.984711558393403,17.982744830235063,0.5266809346220684,32.855828700083094,0.05675308229799072,-0.13090299334069386
+9.999999973554228,8.906672256498078e-05,-0.00024912926939091307,9.993250237275609,1.9794429093204823e-06,-0.00016167063578544257
+14.999999988951053,0.00030609885737583985,-9.259737492950393e-05,14.939586274030715,4.066161982234725e-06,-5.3230908443270726e-05
+19.999999963637627,0.0008196433029304879,-0.00010277758318455454,19.985770355960977,6.0902800817987275e-06,-5.581407356116362e-05
+24.999999906323,0.001558015443394581,-0.0001252423879458675,24.925430653319882,7.508303551937611e-06,-5.98269885073166e-05
+9.93201732790474,1.9700581591656137,-0.006606314895513332,10.1625049701131,0.05795554613825405,-0.23594780459354886
+15.017121844754504,2.000131018972639,-0.001958259181754851,15.130494387563031,0.026367577418638183,-0.10529363184139814
+19.962697589600058,2.0003823634817484,0.0021983556339688626,20.055058569558643,0.014972854970102445,-0.0592998512022201
+24.990093248087035,2.0008914594513647,0.0003319006512292333,25.020899019312747,0.009609456446194334,-0.03808543941908436
+9.942924437331126,3.9434423219621073,-0.047789349898090805,10.916318098481405,0.10417074854184473,-0.42509733550937057
+14.976393375378994,3.9987876606083557,0.004653465622298736,15.69826778341493,0.04981535482126709,-0.20027162173052074
+19.954160472557877,4.000101578371634,0.0053292950039418585,20.459066225465484,0.02905576509783228,-0.11479451096219842
+25.06247590490118,3.9997579161047643,-0.00486183691237807,25.40723367563786,0.01874893916371208,-0.07533000027879669
+9.974854017566281,5.998183884411291,0.01394025812352817,12.27808815775426,0.13163310345287574,-0.5111693653344966
+14.99829771854157,5.999020207860274,0.0007330116466723879,16.57520987140955,0.06880393034208837,-0.27508456151767885
+19.98389776689381,5.999506195067484,0.002770060727207646,21.17690590277397,0.04131547230609369,-0.1652252863196287
+25.022089217041394,5.998166050230614,-0.002551136444779001,25.974625009044832,0.02718132258204399,-0.10978755041013998
+9.940106683734614,7.99448983539684,0.03735909486314526,13.864600506318645,0.14554135993596395,-0.5498471044599721
+15.015405965817797,7.996301502316838,-0.004430455799697253,17.779484729664652,0.08234534796805798,-0.3300198333333338
+19.965919061860355,7.998456498324741,0.00732927315681664,22.0665101267907,0.05178054118886435,-0.20507088323830897
+24.97580637673196,7.998036396987909,0.0034859866489540536,26.699711792661176,0.03478260921646504,-0.13959734880932403
+10.003237328881212,9.994037173180942,-0.002542633641336778,15.800576175296408,0.1482242831571022,-0.5606578442626601
+14.95848212484301,9.995827033229693,0.016804720248816185,19.19635868417634,0.09159937492256161,-0.3610497877526804
+20.018365340632464,9.997789133099982,-0.003880405312526758,23.259977677838524,0.05967179836565363,-0.23873172503708404
+25.034844162753302,9.996613275552045,-0.005490232481425661,27.647073656497884,0.04122997694830456,-0.16548182742762063
+10.041413516307436,11.988808245039152,-0.015743247245750158,18.0174427655263,0.14424296158815444,-0.5545987939832356
+15.0710608536628,11.993636485613393,-0.025235844052727163,20.92474299071291,0.0960774359909814,-0.38199459745149106
+20.061838597733104,11.995243972143648,-0.015325438311212025,24.63090823780847,0.06556771814265559,-0.2626353022718591
+24.90813949494271,11.995929681233529,0.01760171116909426,28.6986397040137,0.046810556161518815,-0.1847353186190147
+10.005191819464756,13.97797567430312,0.018961636911005275,20.358534835690133,0.13825179056925302,-0.5307789523538471
+14.978392340358946,13.991362718235834,0.012411272386128935,22.755419658274054,0.0984622955030996,-0.38447788120958937
+20.015767113356507,13.992558840024987,-0.002205036951612893,26.18420998778461,0.06961025144239422,-0.2786494668163888
+25.01318440442437,13.994258255793202,-0.0016239998449329995,30.09124393513656,0.05071043613803722,-0.20387658283659768
+10.038844117562423,15.966797017942504,0.016384527088525225,22.88736140380268,0.13044436631301143,-0.5070826347325453
+14.91898245890566,15.984279670640529,0.03784081306841358,24.796728185207627,0.09830913950807817,-0.38207974071854045
+19.999487117727806,15.99041117221354,0.0034823225688951354,27.881676426972927,0.07220430103629995,-0.2873083396987492
+25.056418472201756,15.995103453935709,-0.011257522827095023,31.50238694595278,0.05406499488342877,-0.21526296035737832
+10.076107447676621,17.952889979512353,0.017798231103724138,25.454959881832874,0.1231232463335769,-0.47600174850950705
+15.032725028551983,17.978015286760307,0.0020752804670070013,27.089888269358894,0.09590219542773218,-0.3801465515462427
+20.03544756240551,17.98685790169768,-0.005300968094156033,29.75070206477736,0.07340450527104486,-0.29182757725382324
+24.960019173190652,17.98909417109214,0.011594018486178026,33.0995680641525,0.05634561447882407,-0.22402297280749597