From 955ad60ee3cab7911fd15e7db6318233674b60f7 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Fri, 30 Sep 2022 09:14:27 +0900 Subject: [PATCH] Remove pandas dependency (#725) * Remove pandas dependency * Remove pandas dependency --- .../lookup_table_generator.py | 110 +++++++++++++++++ .../lookuptable_generator.py | 114 ------------------ .../StateLatticePlanner/lookup_table.csv | 82 +++++++++++++ .../StateLatticePlanner/lookuptable.csv | 25 ---- .../state_lattice_planner.py | 62 +++++----- README.md | 2 - docs/getting_started_main.rst | 2 - requirements/environment.yml | 1 - requirements/requirements.txt | 1 - 9 files changed, 226 insertions(+), 173 deletions(-) create mode 100644 PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py delete mode 100644 PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py create mode 100644 PathPlanning/StateLatticePlanner/lookup_table.csv delete mode 100644 PathPlanning/StateLatticePlanner/lookuptable.csv diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py new file mode 100644 index 00000000..6cb226ad --- /dev/null +++ b/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() diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py deleted file mode 100644 index c57a05da..00000000 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py +++ /dev/null @@ -1,114 +0,0 @@ -""" - -Lookup Table generation for model predictive trajectory generator - -author: Atsushi Sakai - -""" -from matplotlib import pyplot as plt -import numpy as np -import math -import model_predictive_trajectory_generator as planner -import motion_model -import pandas as pd - - -def calc_states_list(): - maxyaw = 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(-maxyaw, maxyaw, maxyaw) - - states = [] - for iyaw in yaw: - for iy in y: - for ix in x: - states.append([ix, iy, iyaw]) - print("nstate:", len(states)) - - return states - - -def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable): - mind = float("inf") - minid = -1 - - for (i, table) in enumerate(lookuptable): - - 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 lookuptable[minid] - - -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 - - # x, y, yaw, s, km, kf - lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] - - for state in states: - bestp = search_nearest_one_from_lookuptable( - state[0], state[1], state[2], lookuptable) - - target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) - init_p = np.array( - [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1) - - x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) - - if x is not None: - print("find good path") - lookuptable.append( - [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) - - print("finish lookup table generation") - - save_lookup_table("lookuptable.csv", lookuptable) - - for table in lookuptable: - xc, yc, yawc = motion_model.generate_trajectory( - table[3], table[4], table[5], k0) - plt.plot(xc, yc, "-r") - xc, yc, yawc = motion_model.generate_trajectory( - table[3], -table[4], -table[5], k0) - plt.plot(xc, yc, "-r") - - plt.grid(True) - plt.axis("equal") - plt.show() - - print("Done") - - -def main(): - generate_lookup_table() - - -if __name__ == '__main__': - main() diff --git a/PathPlanning/StateLatticePlanner/lookup_table.csv b/PathPlanning/StateLatticePlanner/lookup_table.csv new file mode 100644 index 00000000..2c48a11a --- /dev/null +++ b/PathPlanning/StateLatticePlanner/lookup_table.csv @@ -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 diff --git a/PathPlanning/StateLatticePlanner/lookuptable.csv b/PathPlanning/StateLatticePlanner/lookuptable.csv deleted file mode 100644 index 85eff9b9..00000000 --- a/PathPlanning/StateLatticePlanner/lookuptable.csv +++ /dev/null @@ -1,25 +0,0 @@ -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 diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py index fb80e1f5..e26fb828 100644 --- a/PathPlanning/StateLatticePlanner/state_lattice_planner.py +++ b/PathPlanning/StateLatticePlanner/state_lattice_planner.py @@ -4,9 +4,9 @@ State lattice planner with model predictive trajectory generator author: Atsushi Sakai (@Atsushi_twi) -- lookuptable.csv is generated with this script: +- plookuptable.csv is generated with this script: https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning -/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py +/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py Ref: @@ -21,27 +21,25 @@ import os from matplotlib import pyplot as plt import numpy as np import math -import pandas as pd import pathlib sys.path.append(str(pathlib.Path(__file__).parent.parent)) import ModelPredictiveTrajectoryGenerator.trajectory_generator as planner import ModelPredictiveTrajectoryGenerator.motion_model as motion_model -table_path = os.path.dirname(os.path.abspath(__file__)) + "/lookuptable.csv" +TABLE_PATH = os.path.dirname(os.path.abspath(__file__)) + "/lookup_table.csv" show_animation = True -def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table): +def search_nearest_one_from_lookup_table(t_x, t_y, t_yaw, 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] + dx = t_x - table[0] + dy = t_y - table[1] + dyaw = t_yaw - table[2] d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2) if d <= mind: minid = i @@ -50,24 +48,22 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table): return lookup_table[minid] -def get_lookup_table(): - data = pd.read_csv(table_path) - - return np.array(data) +def get_lookup_table(table_path): + return np.loadtxt(table_path, delimiter=',', skiprows=1) def generate_path(target_states, k0): # x, y, yaw, s, km, kf - lookup_table = get_lookup_table() + lookup_table = get_lookup_table(TABLE_PATH) result = [] for state in target_states: - bestp = search_nearest_one_from_lookuptable( + bestp = 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( - [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1) + [np.hypot(state[0], state[1]), bestp[4], bestp[5]]).reshape(3, 1) x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) @@ -82,18 +78,28 @@ def generate_path(target_states, k0): def calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max): """ - calc uniform state - :param nxy: number of position sampling - :param nh: number of heading sampleing - :param d: distance of terminal state - :param a_min: position sampling min angle - :param a_max: position sampling max angle - :param p_min: heading sampling min angle - :param p_max: heading sampling max angle - :return: states list + Parameters + ---------- + nxy : + number of position sampling + nh : + number of heading sampleing + d : + distance of terminal state + a_min : + position sampling min angle + a_max : + position sampling max angle + p_min : + heading sampling min angle + p_max : + heading sampling max angle + + Returns + ------- + """ - angle_samples = [i / (nxy - 1) for i in range(nxy)] states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh) @@ -312,11 +318,11 @@ def lane_state_sampling_test1(): plt.close("all") for table in result: - xc, yc, yawc = motion_model.generate_trajectory( + x_c, y_c, yaw_c = motion_model.generate_trajectory( table[3], table[4], table[5], k0) if show_animation: - plt.plot(xc, yc, "-r") + plt.plot(x_c, y_c, "-r") if show_animation: plt.grid(True) diff --git a/README.md b/README.md index 2d0ee8d6..28b932ba 100644 --- a/README.md +++ b/README.md @@ -103,8 +103,6 @@ For running each sample code: - [Matplotlib](https://matplotlib.org/) -- [pandas](https://pandas.pydata.org/) - - [cvxpy](https://www.cvxpy.org/) For development: diff --git a/docs/getting_started_main.rst b/docs/getting_started_main.rst index 4a191266..c2be9326 100644 --- a/docs/getting_started_main.rst +++ b/docs/getting_started_main.rst @@ -30,7 +30,6 @@ Requirements - `NumPy`_ - `SciPy`_ - `Matplotlib`_ -- `pandas`_ - `cvxpy`_ For development: @@ -45,7 +44,6 @@ For development: .. _`NumPy`: https://numpy.org/ .. _`SciPy`: https://scipy.org/ .. _`Matplotlib`: https://matplotlib.org/ -.. _`pandas`: https://pandas.pydata.org/ .. _`cvxpy`: https://www.cvxpy.org/ diff --git a/requirements/environment.yml b/requirements/environment.yml index 109fc577..202c2693 100644 --- a/requirements/environment.yml +++ b/requirements/environment.yml @@ -6,6 +6,5 @@ dependencies: - pip - scipy - numpy - - pandas - cvxpy - matplotlib diff --git a/requirements/requirements.txt b/requirements/requirements.txt index 61a73fd2..42225b5f 100644 --- a/requirements/requirements.txt +++ b/requirements/requirements.txt @@ -1,6 +1,5 @@ numpy == 1.23.3 scipy == 1.9.1 -pandas == 1.4.4 matplotlib == 3.5.3 cvxpy == 1.2.1 pytest == 7.1.3 # For unit test