diff --git a/configuration_files/Maneuverlist_scp3.xml b/configuration_files/Maneuverlist_scp3.xml new file mode 100644 index 0000000..76830c1 --- /dev/null +++ b/configuration_files/Maneuverlist_scp3.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/configuration_files/maps/scp3_2018_litd.pickle b/configuration_files/maps/scp3_2018_litd.pickle new file mode 100644 index 0000000..0de7a2c Binary files /dev/null and b/configuration_files/maps/scp3_2018_litd.pickle differ diff --git a/configuration_files/roadSigns_scp3.xml b/configuration_files/roadSigns_scp3.xml new file mode 100755 index 0000000..07b457b --- /dev/null +++ b/configuration_files/roadSigns_scp3.xml @@ -0,0 +1,4 @@ + + + + diff --git a/data/scaledMap.png b/data/scaledMap.png index 7f2ddd2..0d31401 100755 Binary files a/data/scaledMap.png and b/data/scaledMap.png differ diff --git a/data/scaledMap_qualification.png b/data/scaledMap_qualification.png new file mode 100755 index 0000000..7f2ddd2 Binary files /dev/null and b/data/scaledMap_qualification.png differ diff --git a/src/aadcUserPython/litdrive/our_map_generator.ipynb b/src/aadcUserPython/litdrive/our_map_generator.ipynb new file mode 100644 index 0000000..5be371d --- /dev/null +++ b/src/aadcUserPython/litdrive/our_map_generator.ipynb @@ -0,0 +1,531 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#Jupyter Notebook settings\n", + "%load_ext autoreload\n", + "%autoreload 2" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#All imports\n", + "import numpy as np\n", + "import matplotlib.image as mpimg\n", + "import scipy as sp\n", + "import matplotlib.pyplot as plt\n", + "from lxml import etree\n", + "from litdrive.roads.road_list import *\n", + "from litdrive.roads.road_access import *\n", + "from litdrive.selfdriving.enums import ManeuverState" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "def rot_poly(poly_x, poly_y, ang, center = np.array([0, 0])):\n", + " poly = np.array([poly_x.coefficients, poly_y.coefficients])\n", + " poly[:,3] = poly[:,3]- center\n", + " rot = np.array([[np.cos(ang), -np.sin(ang)], [np.sin(ang), np.cos(ang)]])\n", + " result = rot@poly\n", + " result[:,3] = result[:,3] + center\n", + " return np.poly1d(result[0]), np.poly1d(result[1])\n", + "\n", + "rl=RoadList()\n", + "#OUTER TRACK\n", + "\n", + "#straight 1m Road at the lower end left\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(1, 1.0, 1.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 1, {ManeuverState.NEXT: 10}, {ManeuverState.STRAIGHT: 2, ManeuverState.LEFT: 210})\n", + "\n", + "\n", + "#crossing straight 1m Road at the lower end middle\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(2, 2.0, 1.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 2, {ManeuverState.NEXT: 1}, {ManeuverState.NEXT:3})\n", + "\n", + "#straight 1m Road at the lower end right\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(3, 3.0, 1.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 3, {ManeuverState.STRAIGHT: 2, ManeuverState.LEFT: 209}, {ManeuverState.NEXT: 4})\n", + "\n", + "x_list=(4.0, 4.38, 4.59, 4.7, 4.74, 4.75)\n", + "y_list=(0.3, 0.35, 0.51, 0.67, 0.86, 1.0)\n", + "p_list=(0.0, 0.25, 0.4, 0.6, 0.8, 1.0)\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(4, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 4, {ManeuverState.NEXT:3}, {ManeuverState.NEXT: 5})\n", + "\n", + "#right 1m road up\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(5, 4.75, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 5, {ManeuverState.NEXT: 4}, {ManeuverState.NEXT: 6})\n", + "\n", + "x_list=list()\n", + "y_list=list()\n", + "pnts=[[4.75, 2.0], [4.7290274386327091, 2.3487627864425709], [4.6478002771878728, 2.6450030223002079], [4.514014364219908, 2.9173529165564229], [4.3085574264476758, 3.184924742492353], [4.1174346936362971, 3.3664913386631632], [3.8211944577786596, 3.5337237298731194], [3.5631787684832981, 3.6340631645990933], [3.295606942547368, 3.6961780527627912], [3.0, 3.70]]\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(6, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 6, {ManeuverState.NEXT: 5}, {ManeuverState.STRAIGHT: 7, ManeuverState.LEFT: 207})\n", + " \n", + "#top 1m road\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(7, 3.0, -1.0, 0.0, 0.0, 3.7, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 7, {ManeuverState.NEXT: 6}, {ManeuverState.NEXT: 8})\n", + "\n", + "x_list=list()\n", + "y_list=list()\n", + "#pnts=[[2.0, 3.7], [1.7284005334940624, 3.7105122577236447], [1.4226041609958564, 3.6149508913179553], [1.0690271052948059, 3.4572746367485681], [0.84923596256172018, 3.2900422455386114], [0.67244743471119472, 3.0654730344852412], [0.55777379502436764, 2.9030187115955695], [0.43354401869697134, 2.6306688173393544], [0.36187299389270433, 2.4204338112468378], [0.32, 2.0]]\n", + "#pnts=[[2.0015877302666132, 3.7010620851411034], [1.6842686449939821, 3.6680609002727498], [1.3009471899846443, 3.5665187929855078], [1.0039365261694617, 3.3786658945041106], [0.71454152040082264, 3.1375033896969109], [0.50638020046197674, 2.8354156205173666], [0.39468388244601071, 2.5612519308418134], [0.34391282880238983, 2.3429364001742434], [0.33122006539148463, 2.0053088934441643]]\n", + "#pnts=[[2.0, 3.7], [1.9119249655027499, 3.7], [1.7594982227963982, 3.6960685736109493], [1.5745870923001679, 3.6585865876995514], [1.1822756397608687, 3.5086586440539591], [0.82744617313296764, 3.2437859436134135], [0.5475806783278625, 2.9189420657146306], [0.38016114125695144, 2.499143823506973], [0.34018035628479359, 2.2317723240056671], [0.33268395910251403, 2.0818443803600752], [0.33, 2.0]]\n", + "pnts=[[2.0, 3.70], [1.7155187171008421, 3.67], [1.3573101926521409, 3.6100675835120244], [1.0410985296905286, 3.4544321556481057], [0.79158776438488132, 3.2518590590633232], [0.59148507141698625, 3.0221115226927768], [0.49513933035836999, 2.8442424622768696], [0.38150076398154054, 2.5107379739970446], [0.34938551696200182, 2.2315823652887459], [0.33, 2.0]]\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(8, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 8, {ManeuverState.STRAIGHT:7, ManeuverState.LEFT: 208}, {ManeuverState.NEXT:9})\n", + "\n", + "#left 1m road\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(9, 0.32, 0.0, 0.0, 0.0, 2.0, -1.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 9, {ManeuverState.NEXT: 8}, {ManeuverState.NEXT: 10})\n", + "\n", + "x_list=list()\n", + "y_list=list()\n", + "pnts=[[0.32, 1.00], [0.34753878893185097, 0.8818958121152386], [0.38576333549412667, 0.70510728426471314], [0.43832208701725583, 0.55220909801561013], [0.50999311182152296, 0.46620386825048965], [0.61511061486778129, 0.37542057016508479], [0.72978425455460838, 0.34197409192309347], [0.85879209920228916, 0.32286181864195562], [1.0, 0.30]]\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(10, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 10, {ManeuverState.NEXT: 9}, {ManeuverState.NEXT: 1})\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "#INNER TRACK\n", + "\n", + "\n", + "#straight 1m Road at the lower end right\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(101, 3.9, -0.8, 0.0, 0.0, 0.77, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 101, {ManeuverState.NEXT:110}, {ManeuverState.STRAIGHT: 102, ManeuverState.RIGHT: 205})\n", + "\n", + "\n", + "#crossing straight 1m Road at the lower end middle\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(102, 3.1, -1.2, 0.0, 0.0, 0.77, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 102, {ManeuverState.NEXT: 101}, {ManeuverState.NEXT: 103})\n", + "\n", + "#straight 1m Road at the lower end left\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(103, 1.9, -0.8, 0.0, 0.0, 0.77, 0.0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 103, {ManeuverState.STRAIGHT: 102, ManeuverState.RIGHT: 206}, {ManeuverState.NEXT: 104})\n", + "\n", + "x_list=list()\n", + "y_list=list()\n", + "pnts=[[4.3, 1.1], [4.2973033997099233, 1.0298203945189759], [4.283329587417235, 0.98091205149456595], [4.2501417832220998, 0.90405608388477865], [4.1994867136611029, 0.85165428778719654], [4.1453381910269353, 0.80623939783595866], [4.0754691295634924, 0.78353195286033972], [4.0038533415634632, 0.76257123442130681], [3.9, 0.77]]\n", + "#pnts=pnts[::-1]\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(110, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 110, {ManeuverState.NEXT: 109}, {ManeuverState.NEXT: 101})\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(109, 4.3, 0.0, 0.0, 0.0, 2.0, -0.9, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 109, {ManeuverState.NEXT:108}, {ManeuverState.NEXT: 110})\n", + "\n", + "pnts=[[3.1, 3.25], [3.287041989867256, 3.2394385642063894], [3.4640535078782202, 3.2020417646266082], [3.6435581458611699, 3.0973307258032206], [3.8455008635919885, 2.9826472070918917], [4.0000743018550846, 2.8355531287447526], [4.1197440605103841, 2.6784865705096714], [4.1845651797820045, 2.5214200122745902], [4.2568656589695815, 2.3543809741515678], [4.3117142983532606, 2.1848488160565593], [4.3, 2.0]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(108, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 108, {ManeuverState.RIGHT: 201, ManeuverState.STRAIGHT: 107}, {ManeuverState.NEXT: 109})\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(107, 1.9, 1.2, 0.0, 0.0, 3.25, 0, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 107, {ManeuverState.NEXT: 106}, {ManeuverState.NEXT: 108})\n", + "\n", + "pnts=[[0.80, 2.0], [0.81343811496569374, 2.1630845587555259], [0.83602953011047276, 2.3287549364839051], [0.86615141697017806, 2.4567729556376525], [0.92137487621297121, 2.5697300313615474], [1.0268014802219396, 2.745441038043162], [1.1648601283289224, 2.8709488999586004], [1.2552257889080383, 2.9813958184441867], [1.3857539653000945, 3.0818021079765376], [1.5589548147434003, 3.1847185547471977], [1.7221150352334704, 3.222370913321829], [1.9, 3.25]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(106, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 106, {ManeuverState.NEXT: 105}, {ManeuverState.RIGHT: 202, ManeuverState.STRAIGHT: 107})\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(105, 0.8, 0.0, 0.0, 0.0, 1.1, 0.9, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 105, {ManeuverState.NEXT: 104}, {ManeuverState.NEXT: 106})\n", + "\n", + "pnts=[[1.10, 0.77], [1.0282104028248913, 0.78617928865230358], [0.96561548635285666, 0.82127037818965631], [0.92293713421283308, 0.84498057382300273], [0.87361992729547244, 0.89619459639103116], [0.85654858643946297, 0.92654364680171453], [0.83758042993278581, 0.97775766936974295], [0.82050908907677633, 1.0232812449857682], [0.81102501082343781, 1.0773404910297981], [0.80, 1.10]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(104, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 104, {ManeuverState.NEXT:103}, {ManeuverState.NEXT:105})\n", + "\n", + "\n", + "\n", + "#CENTER\n", + "\n", + "pnts=[[2.78, 2.8], [2.7870896221965635, 2.9380825549471581], [2.7932911871382675, 3.0290388407588171], [2.8098286936494783, 3.0693490128798935], [2.8377357358871462, 3.1199951265704762], [2.869777154752617, 3.1778763993597137], [2.9131881093445453, 3.208884224068234], [2.9700357879768324, 3.2295561072072472], [3.0248162782952179, 3.236791266305902], [3.1, 3.25]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(201, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 201, {ManeuverState.NEXT: 204}, {ManeuverState.NEXT: 108})\n", + "\n", + "pnts=[[1.9044216655101069, 3.2504637439369368], [1.9851783078828462, 3.2453525640399281], [2.0782017820084069, 3.2310412603283032], [2.1364692328343073, 3.2105965407402679], [2.1967811556190115, 3.1860628772346256], [2.2386928307744838, 3.1594847417701799], [2.2642487302595278, 3.1329066063057338], [2.2898046297445722, 3.098150583006074], [2.3051381694355983, 3.0633945597064138], [2.3184272371678212, 3.0265940644479503], [2.3306940689206428, 2.8037466209383659]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(202, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 202, {ManeuverState.NEXT: 106}, {ManeuverState.NEXT: 203})\n", + "\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(203, 2.33, 0.0, 0.0, 0.0, 2.8, -1.6, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 203, {ManeuverState.LEFT: 207, ManeuverState.RIGHT: 202}, {ManeuverState.RIGHT: 206, ManeuverState.LEFT: 209})\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(204, 2.78, 0.0, 0.0, 0.0, 1.2, 1.6, 0.0, 0.0)\n", + "rl.addLaneElement(lep3, 204, {ManeuverState.RIGHT: 205, ManeuverState.LEFT: 210}, {ManeuverState.RIGHT: 201, ManeuverState.LEFT: 208})\n", + "\n", + "\n", + "pnts=[[3.20, 0.75], [3.0608925513048062, 0.77752102888161634], [2.9573691945145426, 0.83695110407602702], [2.8672655321230165, 0.92322056806791353], [2.8289235481266228, 0.99607033766106201], [2.8059183577287863, 1.0765885040534895], [2.78, 1.20]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(205, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 205, {ManeuverState.NEXT:101}, {ManeuverState.NEXT:204})\n", + "\n", + "pnts=[[2.33, 1.20], [2.3334444188638952, 1.1263251356413668], [2.325633576324976, 1.0104643046473982], [2.3035028557980382, 0.95058117851568391], [2.2748630998220007, 0.88809443820433009], [2.2227908162292058, 0.83992757588099498], [2.1629076900974917, 0.80868420572531785], [2.0861010717981192, 0.79045890646783978], [1.9910691542412686, 0.7787426426594608], [1.9, 0.77]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(206, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 206, {ManeuverState.NEXT:203}, {ManeuverState.NEXT:103})\n", + "\n", + "pnts=[[3.1084387622989493, 3.705958616743392], [2.8068133697017954, 3.6556877179772], [2.523468303928711, 3.493449817413579], [2.3817957710421687, 3.2946512632018186], [2.3543752808060634, 3.082142463872005], [2.3360949539819935, 2.8010824389519295]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(207, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 207, {ManeuverState.NEXT:6}, {ManeuverState.NEXT:203})\n", + "\n", + "\n", + "pnts=[[2.80, 2.80], [2.7971599993576506, 3.0322451521394345], [2.7163786119291258, 3.279341160744335], [2.571447299189713, 3.533564938828223], [2.374245676937725, 3.6642407126096606], [2.1580366694084376, 3.7022554831642607], [1.9, 3.73]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(208, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 208, {ManeuverState.NEXT: 204}, {ManeuverState.NEXT: 8})\n", + "\n", + "pnts=[[2.33, 1.20], [2.339351874764004, 1.0508332521777168], [2.3568012776415252, 0.8688609078835661], [2.3991783989155056, 0.6395258986361431], [2.4565121512273613, 0.47251018537986755], [2.5836435150493027, 0.3752920836336773], [2.7631230875038075, 0.33291496235969686], [3.0099074996287523, 0.3154655594821756]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(209, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 209, {ManeuverState.NEXT:203}, {ManeuverState.NEXT:3})\n", + "\n", + "pnts=[[2.010659377215461, 0.3036215591342494], [2.184582145777422, 0.3112946224531594], [2.4071009820258134, 0.3317561246369196], [2.6066006283174747, 0.40337138228008007], [2.7089081392362755, 0.5235827076096707], [2.7805233968794356, 0.6872747250797517], [2.793311835744286, 0.9788511311983334], [2.803542586836166, 1.2013699674467249]]\n", + "x_list=list()\n", + "y_list=list()\n", + "p_list=np.linspace(0.0, 1.0, len(pnts))\n", + "for pnt in pnts:\n", + " x_list.append(pnt[0])\n", + " y_list.append(pnt[1])\n", + "\n", + "px=np.polyfit(p_list, x_list, 3)\n", + "py=np.polyfit(p_list, y_list, 3)\n", + "\n", + "lep3=LaneElementPoly3.fromLITDLaneFormat(210, px[3], px[2], px[1], px[0], py[3], py[2], py[1], py[0])\n", + "rl.addLaneElement(lep3, 210, {ManeuverState.NEXT:1}, {ManeuverState.NEXT:204})\n", + "\n", + "for key,l in rl.lanes.items():\n", + " l.x_poly[0]-=0.05\n", + " #l.y_poly[0]-=0.05" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": { + "scrolled": false + }, + "outputs": [], + "source": [ + "#Plot the track on the picture of the map.\n", + "\n", + "%matplotlib notebook\n", + "\n", + "#pixel per meter\n", + "pic_ppm_x=150.0\n", + "pic_ppm_y=150.0\n", + "pic_offset_x=0.975\n", + "pic_offset_y=0.975\n", + "img_mat=mpimg.imread(\"/home/aadc/share/adtf/data/scaledMap.png\")\n", + "img_mat=np.flipud(img_mat)\n", + "\n", + "plt.figure(figsize=(5*4,4*4))\n", + "\n", + "plt.imshow(img_mat)\n", + "\n", + "plt.axis([0, 5*pic_ppm, 0, 4*pic_ppm])\n", + "\n", + "roads_len=0.0\n", + "\n", + "#for key,l in rl.roads.items():\n", + "# if(l is not None):\n", + "# print(l)\n", + "\n", + "for key,l in rl.lanes.items():\n", + " if(l is not None):\n", + " pts=l.getPixelPointList(pic_ppm_y, pic_ppm_y,10)\n", + " roads_len+=l.calcArcLength()\n", + " x=pts[0]\n", + " y=pts[1]\n", + " plt.plot(x,y, '-')\n", + " plt.quiver(x[:-1], y[:-1], x[1:]-x[:-1], y[1:]-y[:-1], scale_units='xy', angles='xy', scale=1)\n", + " \n", + " \n", + " if(True):\n", + " \n", + " #if(l.is_junction):\n", + " #if(key==60 or key==178 or key==64):\n", + " \n", + " #for the merge lane\n", + " #if(l.road_id==36 or l.road_id==110 or l.road_id==158 or l.road_id==171 or l.road_id==172 or l.road_id==173):\n", + " #plt.text(x[len(x)//2],y[len(y)//2],str(key), color=\"blue\", size=\"large\")\n", + " p1d_dx, p1d_dy= l.calcPolyDerivate()\n", + " dy=p1d_dy(0.5)\n", + " dx=p1d_dx(0.5)\n", + " angle=np.arctan2(dy,dx)-np.pi/2.0\n", + " dx=0.2*np.cos(angle)\n", + " dy=0.2*np.sin(angle)\n", + " plt.annotate(str(key), xy=(x[len(x)//2], y[len(y)//2]), xytext=(x[len(x)//2]+dx*pic_ppm, y[len(y)//2]+dy*pic_ppm), arrowprops=dict(facecolor='red', shrink=0.05), color=\"red\")\n", + " #for i in range(0,len(pts)-1):\n", + " # plt.arrow(pts[0][i],pts[1][i],pts[0][i+1]-pts[0][i],pts[1][i+1]-pts[1][i],,head_width=0.02, head_length=0.02, fc='black', ec='black')\n", + "\n", + "print(\"Length is {}\".format(roads_len))\n", + "#plt.text(xs[1]+0.2,ys[1]+0.2,s)#+' '+str(degs[3]*180/np.pi))\n", + "#plt.plot(xs,ys,'g.')\n", + "\n", + "#plt.savefig(\"./our_map_lanes.jpg\")\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "print(\"struct array_of_fuckts[]={\", end=\"\")\n", + "for key,lep3 in sorted(rl.lanes.items(), key=lambda kv: kv[0]):\n", + " print(\"{\", end=\"\")\n", + " print(\"{}, {}, {}, {}, {}, {}, {}, {}, {}, 0.0, 1.0, false\".format(key, lep3.x_poly[0], lep3.x_poly[1], lep3.x_poly[2], lep3.x_poly[3], lep3.y_poly[0], lep3.y_poly[1], lep3.y_poly[2], lep3.y_poly[3]), end=\"\")\n", + " print(\"}, \", end=\"\")\n", + "print(\"};\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#Check, that the lane successors and predecessors are not further appart then 4cm\n", + "\n", + "print(\"Checking {} elements, against {} successors and {} predecessors.\".format(len(rl.lanes), len(rl.successors), len(rl.predecessors)))\n", + "for key,l in rl.lanes.items():\n", + " if(l is not None):\n", + " x_1=l.x_poly(1.0)\n", + " y_1=l.y_poly(1.0)\n", + " x_0=l.x_poly(0.0)\n", + " y_0=l.y_poly(0.0)\n", + " suc_dict=rl.successors[key]\n", + " if(suc_dict is not None):\n", + " for key_2, suc_id in suc_dict.items():\n", + " s=rl.lanes[suc_id]\n", + " xs_0=s.x_poly(0.0)\n", + " ys_0=s.y_poly(0.0)\n", + " dx=abs(x_1-xs_0)\n", + " dy=abs(y_1-ys_0)\n", + " delta=np.sqrt(dx**2+dy**2)\n", + " if(delta>0.15):\n", + " print(\"ERROR: HIGH Difference between lane {} and successor {} is dx={} and dy={}!\".format(key,suc_id,dx,dy))\n", + " elif(delta>0.04):\n", + " print(\"WARNING: Difference between lane {} and successor {} is dx={} and dy={}!\".format(key,suc_id,dx,dy))\n", + " \n", + " else:\n", + " print(\"WARNING: lane {} (road {}) has no successor!\".format(key, l.road_id))\n", + " pre_dict=rl.predecessors[key]\n", + " if(pre_dict is not None):\n", + " for key_2, pre_id in pre_dict.items():\n", + " p=rl.lanes[pre_id]\n", + " xs_1=p.x_poly(1.0)\n", + " ys_1=p.y_poly(1.0)\n", + " dx=abs(x_0-xs_1)\n", + " dy=abs(y_0-ys_1)\n", + " delta=np.sqrt(dx**2+dy**2)\n", + " if(delta>0.15):\n", + " print(\"ERROR: HIGH Difference between lane {} and predecessor {} is dx={} and dy={}!\".format(key,pre_id,dx,dy))\n", + " if(delta>0.04):\n", + " print(\"WARNING: Difference between lane {} and predecessor {} is dx={} and dy={}!\".format(key,pre_id,dx,dy))\n", + " else:\n", + " print(\"WARNING: lane {} (road {}) has no predecessor!\".format(key, l.road_id))\n", + " \n", + "print(\"Done!\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#Should stop after 1 because of no suitable decision!\n", + "getLaneListByDecisions(rl, 1, [RoadDecisions.RIGHT])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "#Should stop after 3 loops becaus of wron decisions\n", + "getLaneListByDecisions(rl, 1, [RoadDecisions.LEFT, RoadDecisions.STRAIGHT])" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "rl.saveToFile(\"our_map.pickle\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.6.6" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}