├── README.md ├── data └── test.xodr ├── opendriveparser ├── __init__.py ├── __pycache__ │ ├── __init__.cpython-38.pyc │ └── parser.cpython-38.pyc ├── elements │ ├── __init__.py │ ├── __pycache__ │ │ ├── __init__.cpython-38.pyc │ │ ├── junction.cpython-38.pyc │ │ ├── openDrive.cpython-38.pyc │ │ ├── road.cpython-38.pyc │ │ ├── roadElevationProfile.cpython-38.pyc │ │ ├── roadLanes.cpython-38.pyc │ │ ├── roadLateralProfile.cpython-38.pyc │ │ ├── roadLink.cpython-38.pyc │ │ ├── roadPlanView.cpython-38.pyc │ │ └── roadType.cpython-38.pyc │ ├── junction.py │ ├── openDrive.py │ ├── road.py │ ├── roadElevationProfile.py │ ├── roadLanes.py │ ├── roadLateralProfile.py │ ├── roadLink.py │ ├── roadPlanView.py │ └── roadType.py └── parser.py └── parse_and_visualize.py /README.md: -------------------------------------------------------------------------------- 1 | This is a project for parsing opendrive .xodr file and visualization using matplotlib. 2 | 3 | # Introductions 4 | 5 | - /opendriveparser: Copied from [https://github.com/fiefdx/pyopendriveparser](https://github.com/fiefdx/pyopendriveparser) for parsing opendrive .xodr files. Note that some bugs are fixed in this work. 6 | - /data: Consisting input demo data of .xodr file. 7 | - parse_and_visualize.py: Parsing and visualizing the .xodr file. 8 | 9 | # Installation 10 | 11 | Please create virtual environment and install required packages. 12 | 13 | ``` 14 | conda create -n opendrive python=3.8 15 | conda activate opendrive 16 | pip install lxml>=5.1.0 17 | pip install matplotlib 18 | pip install https://github.com/stefan-urban/pyeulerspiral/archive/master.zip 19 | ``` 20 | 21 | # Parameters and Run 22 | 23 | The parameters are given at the start of “parse_and_visualize.py” including: 24 | 25 | - XODR_FILE: input file path. 26 | - XXXX_COLOR: colors of different lane types. 27 | - STEP: Sample steps while ploting. 28 | 29 | Directly run the main file: 30 | 31 | ``` 32 | python parse_and_visualize.py 33 | ``` 34 | 35 | The results of visualization of the road network will be saved in “/data/” directory. 36 | 37 | If you find this demo useful, please consider star our work! 38 | -------------------------------------------------------------------------------- /data/test.xodr: -------------------------------------------------------------------------------- 1 | 2 | 3 |
4 |
5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 |
36 | 37 | 38 | 39 | 40 | 41 |
42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 |
130 | 131 | 132 | 133 | 134 | 135 |
136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 |
209 | 210 | 211 | 212 | 213 | 214 |
215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 |
303 | 304 | 305 | 306 | 307 | 308 |
309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 |
382 | 383 | 384 | 385 | 386 | 387 |
388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 |
476 | 477 | 478 | 479 | 480 | 481 |
482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 |
573 | 574 | 575 | 576 | 577 | 578 |
579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | 603 | 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 630 | 631 | 632 | 633 | 634 | 635 | 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | 649 | 650 | 651 |
652 | 653 | 654 | 655 | 656 | 657 |
658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 670 | 671 | 672 | 673 | 674 | 675 | 676 | 677 | 678 | 679 | 680 | 681 | 682 | 683 | 684 | 685 | 686 | 687 | 688 | 689 | 690 | 691 | 692 | 693 | 694 | 695 | 696 | 697 | 698 | 699 | 700 | 701 | 702 | 703 | 704 | 705 | 706 | 707 | 708 | 709 | 710 | 711 | 712 | 713 | 714 | 715 |
716 | 717 | 718 | 719 | 720 | 721 |
722 | 723 | 724 | 725 | 726 | 727 | 728 | 729 | 730 | 731 | 732 | 733 | 734 | 735 | 736 | 737 | 738 | 739 | 740 | 741 | 742 | 743 | 744 | 745 | 746 | 747 | 748 | 749 | 750 | 751 | 752 | 753 | 754 | 755 | 756 | 757 | 758 | 759 | 760 | 761 | 762 | 763 | 764 | 765 | 766 | 767 | 768 | 769 |
770 | 771 | 772 | 773 | 774 | 775 |
776 | 777 | 778 | 779 | 780 | 781 | 782 | 783 | 784 | 785 | 786 | 787 | 788 | 789 | 790 | 791 | 792 | 793 | 794 | 795 | 796 | 797 | 798 | 799 | 800 | 801 | 802 | 803 | 804 | 805 | 806 | 807 | 808 | 809 | 810 | 811 | 812 | 813 | 814 | 815 | 816 | 817 | 818 | 819 | 820 | 821 | 822 | 823 |
824 | 825 | 826 | 827 | 828 | 829 |
830 | 831 | 832 | 833 | 834 | 835 | 836 | 837 | 838 | 839 | 840 | 841 | 842 | 843 | 844 | 845 | 846 | 847 | 848 | 849 | 850 | 851 | 852 | 853 | 854 | 855 | 856 | 857 | 858 | 859 | 860 | 861 | 862 | 863 | 864 | 865 | 866 | 867 | 868 | 869 | 870 | 871 | 872 | 873 | 874 | 875 | 876 | 877 |
878 | 879 | 880 | 881 | 882 | 883 |
884 | 885 | 886 | 887 | 888 | 889 | 890 | 891 | 892 | 893 | 894 | 895 | 896 | 897 | 898 | 899 | 900 | 901 | 902 | 903 | 904 | 905 | 906 | 907 | 908 | 909 | 910 | 911 | 912 | 913 | 914 | 915 | 916 | 917 | 918 | 919 | 920 | 921 | 922 | 923 | 924 | 925 | 926 | 927 | 928 | 929 | 930 | 931 | 932 | 933 | 934 | 935 | 936 | 937 | 938 | 939 | 940 | 941 | 942 | 943 | 944 | 945 | 946 | 947 | 948 | 949 | 950 | 951 | 952 | 953 | 954 | 955 | 956 | 957 | 958 | 959 | 960 | 961 | 962 | 963 | 964 | 965 | 966 | 967 | 968 | 969 | 970 | 971 | 972 | 973 | 974 | 975 | 976 | 977 | 978 | 979 | 980 | 981 | 982 | 983 | 984 | 985 | 986 | 987 | 988 | 989 | 990 | 991 | 992 |
993 | 994 | 995 | 996 | 997 | 998 |
999 | 1000 | 1001 | 1002 | 1003 | 1004 | 1005 | 1006 | 1007 | 1008 | 1009 | 1010 | 1011 | 1012 | 1013 | 1014 | 1015 | 1016 | 1017 | 1018 | 1019 | 1020 | 1021 | 1022 | 1023 | 1024 | 1025 | 1026 | 1027 | 1028 | 1029 | 1030 | 1031 | 1032 | 1033 | 1034 | 1035 | 1036 | 1037 | 1038 | 1039 | 1040 | 1041 | 1042 | 1043 | 1044 | 1045 | 1046 | 1047 | 1048 | 1049 | 1050 | 1051 | 1052 | 1053 | 1054 | 1055 | 1056 | 1057 | 1058 | 1059 | 1060 | 1061 | 1062 | 1063 | 1064 | 1065 | 1066 | 1067 | 1068 | 1069 | 1070 | 1071 | 1072 | 1073 | 1074 | 1075 | 1076 | 1077 | 1078 | 1079 | 1080 | 1081 | 1082 | 1083 | 1084 | 1085 | 1086 | 1087 | 1088 | 1089 | 1090 | 1091 | 1092 | 1093 | 1094 | 1095 | 1096 | 1097 | 1098 | 1099 | 1100 | 1101 | 1102 | 1103 | 1104 | 1105 | 1106 | 1107 | 1108 | 1109 | 1110 | 1111 | 1112 | 1113 | 1114 | 1115 | 1116 | 1117 | 1118 | 1119 |
1120 | 1121 | 1122 | 1123 | 1124 | 1125 |
1126 | 1127 | 1128 | 1129 | 1130 | 1131 | 1132 | 1133 | 1134 | 1135 | 1136 | 1137 | 1138 | 1139 | 1140 | 1141 | 1142 | 1143 | 1144 | 1145 | 1146 | 1147 | 1148 | 1149 | 1150 | 1151 | 1152 | 1153 | 1154 | 1155 | 1156 | 1157 | 1158 | 1159 | 1160 | 1161 | 1162 | 1163 | 1164 | 1165 | 1166 | 1167 | 1168 | 1169 | 1170 | 1171 | 1172 | 1173 | 1174 | 1175 | 1176 | 1177 | 1178 | 1179 | 1180 | 1181 | 1182 | 1183 | 1184 | 1185 | 1186 | 1187 | 1188 | 1189 | 1190 | 1191 | 1192 | 1193 | 1194 | 1195 | 1196 |
1197 | 1198 | 1199 | 1200 | 1201 | 1202 |
1203 | 1204 | 1205 | 1206 | 1207 | 1208 | 1209 | 1210 | 1211 | 1212 | 1213 | 1214 | 1215 | 1216 | 1217 | 1218 | 1219 | 1220 | 1221 | 1222 | 1223 | 1224 | 1225 | 1226 | 1227 | 1228 | 1229 | 1230 | 1231 | 1232 | 1233 | 1234 | 1235 | 1236 | 1237 | 1238 | 1239 | 1240 | 1241 | 1242 | 1243 | 1244 | 1245 | 1246 | 1247 | 1248 | 1249 |
1250 | 1251 | 1252 | 1253 | 1254 | 1255 |
1256 | 1257 | 1258 | 1259 | 1260 | 1261 | 1262 | 1263 | 1264 | 1265 | 1266 | 1267 | 1268 | 1269 | 1270 | 1271 | 1272 | 1273 | 1274 | 1275 | 1276 | 1277 | 1278 | 1279 | 1280 | 1281 | 1282 | 1283 | 1284 | 1285 | 1286 | 1287 | 1288 | 1289 | 1290 | 1291 | 1292 | 1293 | 1294 | 1295 | 1296 | 1297 | 1298 | 1299 | 1300 | 1301 | 1302 |
1303 | 1304 | 1305 | 1306 | 1307 | 1308 |
1309 | 1310 | 1311 | 1312 | 1313 | 1314 | 1315 | 1316 | 1317 | 1318 | 1319 | 1320 | 1321 | 1322 | 1323 | 1324 | 1325 | 1326 | 1327 | 1328 | 1329 | 1330 | 1331 | 1332 | 1333 | 1334 | 1335 | 1336 | 1337 | 1338 | 1339 | 1340 | 1341 | 1342 | 1343 | 1344 | 1345 | 1346 | 1347 | 1348 | 1349 | 1350 | 1351 | 1352 | 1353 | 1354 | 1355 |
1356 | 1357 | 1358 | 1359 | 1360 | 1361 |
1362 | 1363 | 1364 | 1365 | 1366 | 1367 | 1368 | 1369 | 1370 | 1371 | 1372 | 1373 | 1374 | 1375 | 1376 | 1377 | 1378 | 1379 | 1380 | 1381 | 1382 | 1383 | 1384 | 1385 | 1386 | 1387 | 1388 | 1389 | 1390 | 1391 | 1392 | 1393 | 1394 | 1395 | 1396 | 1397 | 1398 | 1399 | 1400 | 1401 | 1402 | 1403 | 1404 | 1405 | 1406 | 1407 | 1408 | 1409 | 1410 | 1411 | 1412 | 1413 | 1414 | 1415 | 1416 | 1417 | 1418 | 1419 | 1420 | 1421 | 1422 | 1423 | 1424 | 1425 | 1426 | 1427 | 1428 | 1429 | 1430 | 1431 | 1432 | 1433 | 1434 | 1435 | 1436 | 1437 | 1438 | 1439 | 1440 | 1441 | 1442 | 1443 | 1444 | 1445 | 1446 | 1447 | 1448 | 1449 | 1450 | 1451 | 1452 | 1453 | 1454 | -------------------------------------------------------------------------------- /opendriveparser/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from opendriveparser.parser import parse_opendrive 3 | -------------------------------------------------------------------------------- /opendriveparser/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/__pycache__/parser.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/__pycache__/parser.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__init__.py -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/__init__.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/__init__.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/junction.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/junction.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/openDrive.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/openDrive.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/road.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/road.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/roadElevationProfile.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/roadElevationProfile.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/roadLanes.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/roadLanes.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/roadLateralProfile.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/roadLateralProfile.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/roadLink.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/roadLink.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/roadPlanView.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/roadPlanView.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/__pycache__/roadType.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/skx6/parse-and-visualize-xodr/6d771d7f408ff8e5bb752ea7ecb143d7f04adfdc/opendriveparser/elements/__pycache__/roadType.cpython-38.pyc -------------------------------------------------------------------------------- /opendriveparser/elements/junction.py: -------------------------------------------------------------------------------- 1 | 2 | class Junction(object): 3 | # TODO priority 4 | # TODO controller 5 | 6 | def __init__(self): 7 | self._id = None 8 | self._name = None 9 | self._connections = [] 10 | 11 | @property 12 | def id(self): 13 | return self._id 14 | 15 | @id.setter 16 | def id(self, value): 17 | self._id = int(value) 18 | 19 | @property 20 | def name(self): 21 | return self._name 22 | 23 | @name.setter 24 | def name(self, value): 25 | self._name = str(value) 26 | 27 | @property 28 | def connections(self): 29 | return self._connections 30 | 31 | def addConnection(self, connection): 32 | if not isinstance(connection, Connection): 33 | raise TypeError("Has to be of instance Connection") 34 | 35 | self._connections.append(connection) 36 | 37 | 38 | class Connection(object): 39 | 40 | def __init__(self): 41 | self._id = None 42 | self._incomingRoad = None 43 | self._connectingRoad = None 44 | self._contactPoint = None 45 | self._laneLinks = [] 46 | 47 | @property 48 | def id(self): 49 | return self._id 50 | 51 | @id.setter 52 | def id(self, value): 53 | self._id = int(value) 54 | 55 | @property 56 | def incomingRoad(self): 57 | return self._incomingRoad 58 | 59 | @incomingRoad.setter 60 | def incomingRoad(self, value): 61 | self._incomingRoad = int(value) 62 | 63 | @property 64 | def connectingRoad(self): 65 | return self._connectingRoad 66 | 67 | @connectingRoad.setter 68 | def connectingRoad(self, value): 69 | self._connectingRoad = int(value) 70 | 71 | @property 72 | def contactPoint(self): 73 | return self._contactPoint 74 | 75 | @contactPoint.setter 76 | def contactPoint(self, value): 77 | if value not in ["start", "end"]: 78 | raise AttributeError("Contact point can only be start or end.") 79 | 80 | self._contactPoint = value 81 | 82 | @property 83 | def laneLinks(self): 84 | return self._laneLinks 85 | 86 | def addLaneLink(self, laneLink): 87 | if not isinstance(laneLink, LaneLink): 88 | raise TypeError("Has to be of instance LaneLink") 89 | 90 | self._laneLinks.append(laneLink) 91 | 92 | 93 | class LaneLink(object): 94 | 95 | def __init__(self): 96 | self._from = None 97 | self._to = None 98 | 99 | def __str__(self): 100 | return str(self._from) + " > " + str(self._to) 101 | 102 | @property 103 | def fromId(self): 104 | return self._from 105 | 106 | @fromId.setter 107 | def fromId(self, value): 108 | self._from = int(value) 109 | 110 | @property 111 | def toId(self): 112 | return self._to 113 | 114 | @toId.setter 115 | def toId(self, value): 116 | self._to = int(value) 117 | -------------------------------------------------------------------------------- /opendriveparser/elements/openDrive.py: -------------------------------------------------------------------------------- 1 | 2 | class OpenDrive(object): 3 | 4 | def __init__(self): 5 | self._header = None 6 | self._roads = [] 7 | self._controllers = [] 8 | self._junctions = [] 9 | self._junctionGroups = [] 10 | self._stations = [] 11 | 12 | @property 13 | def header(self): 14 | return self._header 15 | 16 | @property 17 | def roads(self): 18 | return self._roads 19 | 20 | def getRoad(self, id): 21 | for road in self._roads: 22 | if road.id == id: 23 | return road 24 | 25 | return None 26 | 27 | @property 28 | def controllers(self): 29 | return self._controllers 30 | 31 | @property 32 | def junctions(self): 33 | return self._junctions 34 | 35 | @property 36 | def junctionGroups(self): 37 | return self._junctionGroups 38 | 39 | @property 40 | def stations(self): 41 | return self._stations 42 | 43 | 44 | class Header(object): 45 | 46 | def __init__(self): 47 | self._revMajor = None 48 | self._revMinor = None 49 | self._name = None 50 | self._version = None 51 | self._date = None 52 | self._north = None 53 | self._south = None 54 | self._east = None 55 | self._west = None 56 | self._vendor = None 57 | -------------------------------------------------------------------------------- /opendriveparser/elements/road.py: -------------------------------------------------------------------------------- 1 | 2 | from opendriveparser.elements.roadPlanView import PlanView 3 | from opendriveparser.elements.roadLink import Link 4 | from opendriveparser.elements.roadLanes import Lanes 5 | from opendriveparser.elements.roadElevationProfile import ElevationProfile 6 | from opendriveparser.elements.roadLateralProfile import LateralProfile 7 | 8 | class Road(object): 9 | 10 | def __init__(self): 11 | self._id = None 12 | self._name = None 13 | self._junction = None 14 | self._length = None 15 | 16 | self._header = None # TODO 17 | self._link = Link() 18 | self._types = [] 19 | self._planView = PlanView() 20 | self._elevationProfile = ElevationProfile() 21 | self._lateralProfile = LateralProfile() 22 | self._lanes = Lanes() 23 | 24 | @property 25 | def id(self): 26 | return self._id 27 | 28 | @id.setter 29 | def id(self, value): 30 | self._id = int(value) 31 | 32 | @property 33 | def name(self): 34 | return self._name 35 | 36 | @name.setter 37 | def name(self, value): 38 | self._name = str(value) 39 | 40 | @property 41 | def junction(self): 42 | return self._junction 43 | 44 | @junction.setter 45 | def junction(self, value): 46 | if not isinstance(value, int) and value is not None: 47 | raise TypeError("Property must be a int or NoneType") 48 | 49 | if value == -1: 50 | value = None 51 | 52 | self._junction = value 53 | 54 | @property 55 | def link(self): 56 | return self._link 57 | 58 | @property 59 | def types(self): 60 | return self._types 61 | 62 | @property 63 | def planView(self): 64 | return self._planView 65 | 66 | @property 67 | def elevationProfile(self): 68 | return self._elevationProfile 69 | 70 | @property 71 | def lateralProfile(self): 72 | return self._lateralProfile 73 | 74 | @property 75 | def lanes(self): 76 | return self._lanes 77 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadElevationProfile.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class ElevationProfile(object): 4 | 5 | def __init__(self): 6 | self._elevations = [] 7 | 8 | @property 9 | def elevations(self): 10 | return self._elevations 11 | 12 | 13 | class Elevation(object): 14 | 15 | def __init__(self): 16 | self._sPos = None 17 | self._a = None 18 | self._b = None 19 | self._c = None 20 | self._d = None 21 | 22 | @property 23 | def sPos(self): 24 | return self._sPos 25 | 26 | @sPos.setter 27 | def sPos(self, value): 28 | self._sPos = float(value) 29 | 30 | @property 31 | def a(self): 32 | return self._a 33 | 34 | @a.setter 35 | def a(self, value): 36 | self._a = float(value) 37 | 38 | @property 39 | def b(self): 40 | return self._b 41 | 42 | @b.setter 43 | def b(self, value): 44 | self._b = float(value) 45 | 46 | @property 47 | def c(self): 48 | return self._c 49 | 50 | @c.setter 51 | def c(self, value): 52 | self._c = float(value) 53 | 54 | @property 55 | def d(self): 56 | return self._d 57 | 58 | @d.setter 59 | def d(self, value): 60 | self._d = float(value) 61 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadLanes.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class Lanes(object): 4 | 5 | def __init__(self): 6 | self._laneOffsets = [] 7 | self._laneSections = [] 8 | 9 | @property 10 | def laneOffsets(self): 11 | self._laneOffsets.sort(key=lambda x: x.sPos) 12 | return self._laneOffsets 13 | 14 | @property 15 | def laneSections(self): 16 | self._laneSections.sort(key=lambda x: x.sPos) 17 | return self._laneSections 18 | 19 | def getLaneSection(self, laneSectionIdx): 20 | for laneSection in self.laneSections: 21 | if laneSection.idx == laneSectionIdx: 22 | return laneSection 23 | 24 | return None 25 | 26 | def getLastLaneSectionIdx(self): 27 | """ Returns the index of the last lane section of the road """ 28 | 29 | numLaneSections = len(self.laneSections) 30 | 31 | if numLaneSections > 1: 32 | return numLaneSections - 1 33 | 34 | return 0 35 | 36 | class LaneOffset(object): 37 | 38 | def __init__(self): 39 | self._sPos = None 40 | self._a = None 41 | self._b = None 42 | self._c = None 43 | self._d = None 44 | 45 | @property 46 | def sPos(self): 47 | return self._sPos 48 | 49 | @sPos.setter 50 | def sPos(self, value): 51 | self._sPos = float(value) 52 | 53 | @property 54 | def a(self): 55 | return self._a 56 | 57 | @a.setter 58 | def a(self, value): 59 | self._a = float(value) 60 | 61 | @property 62 | def b(self): 63 | return self._b 64 | 65 | @b.setter 66 | def b(self, value): 67 | self._b = float(value) 68 | 69 | @property 70 | def c(self): 71 | return self._c 72 | 73 | @c.setter 74 | def c(self, value): 75 | self._c = float(value) 76 | 77 | @property 78 | def d(self): 79 | return self._d 80 | 81 | @d.setter 82 | def d(self, value): 83 | self._d = float(value) 84 | 85 | @property 86 | def coeffs(self): 87 | """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ 88 | return [self._a, self._b, self._c, self._d] 89 | 90 | 91 | class LaneSection(object): 92 | 93 | def __init__(self): 94 | self._idx = None 95 | self._sPos = None 96 | self._singleSide = None 97 | self._leftLanes = LeftLanes() 98 | self._centerLanes = CenterLanes() 99 | self._rightLanes = RightLanes() 100 | 101 | @property 102 | def idx(self): 103 | return self._idx 104 | 105 | @idx.setter 106 | def idx(self, value): 107 | self._idx = int(value) 108 | 109 | @property 110 | def sPos(self): 111 | return self._sPos 112 | 113 | @sPos.setter 114 | def sPos(self, value): 115 | self._sPos = float(value) 116 | 117 | @property 118 | def length(self): 119 | return self._length 120 | 121 | @length.setter 122 | def length(self, value): 123 | self._length = float(value) 124 | 125 | @property 126 | def singleSide(self): 127 | return self._singleSide 128 | 129 | @singleSide.setter 130 | def singleSide(self, value): 131 | if value not in ["true", "false"] and value is not None: 132 | raise AttributeError("Value must be true or false.") 133 | 134 | self._singleSide = (value == "true") 135 | 136 | @property 137 | def leftLanes(self): 138 | """ Get list of sorted lanes always starting in the middle (lane id -1) """ 139 | return self._leftLanes.lanes 140 | 141 | @property 142 | def centerLanes(self): 143 | return self._centerLanes.lanes 144 | 145 | @property 146 | def rightLanes(self): 147 | """ Get list of sorted lanes always starting in the middle (lane id 1) """ 148 | return self._rightLanes.lanes 149 | 150 | @property 151 | def allLanes(self): 152 | """ Attention! lanes are not sorted by id """ 153 | return self._leftLanes.lanes + self._centerLanes.lanes + self._rightLanes.lanes 154 | 155 | def getLane(self, laneId): 156 | for lane in self.allLanes: 157 | if lane.id == laneId: 158 | return lane 159 | 160 | return None 161 | 162 | 163 | class LeftLanes(object): 164 | 165 | sort_direction = False 166 | 167 | def __init__(self): 168 | self._lanes = [] 169 | 170 | @property 171 | def lanes(self): 172 | self._lanes.sort(key=lambda x: x.id, reverse=self.sort_direction) 173 | return self._lanes 174 | 175 | class CenterLanes(LeftLanes): 176 | pass 177 | 178 | class RightLanes(LeftLanes): 179 | sort_direction = True 180 | 181 | 182 | class Lane(object): 183 | 184 | laneTypes = [ 185 | "none", "driving", "stop", "shoulder", "biking", "sidewalk", "border", 186 | "restricted", "parking", "bidirectional", "median", "special1", "special2", 187 | "special3", "roadWorks", "tram", "rail", "entry", "exit", "offRamp", "onRamp" 188 | ] 189 | 190 | def __init__(self): 191 | self._id = None 192 | self._type = None 193 | self._level = None 194 | self._link = LaneLink() 195 | self._widths = [] 196 | self._borders = [] 197 | 198 | @property 199 | def id(self): 200 | return self._id 201 | 202 | @id.setter 203 | def id(self, value): 204 | self._id = int(value) 205 | 206 | @property 207 | def type(self): 208 | return self._type 209 | 210 | @type.setter 211 | def type(self, value): 212 | if value not in self.laneTypes: 213 | raise Exception() 214 | 215 | self._type = str(value) 216 | 217 | @property 218 | def level(self): 219 | return self._level 220 | 221 | @level.setter 222 | def level(self, value): 223 | # Added for avoiding value errors. 224 | if value == 0 or value == "0": 225 | value = "true" 226 | elif value == 1 or value == "1": 227 | value = "false" 228 | 229 | if value not in ["true", "false"] and value is not None: 230 | raise AttributeError("Value must be true or false.") 231 | 232 | self._level = (value == "true") 233 | 234 | @property 235 | def link(self): 236 | return self._link 237 | 238 | @property 239 | def widths(self): 240 | self._widths.sort(key=lambda x: x.sOffset) 241 | return self._widths 242 | 243 | def getWidth(self, widthIdx): 244 | for width in self._widths: 245 | if width.idx == widthIdx: 246 | return width 247 | 248 | return None 249 | 250 | def getLastLaneWidthIdx(self): 251 | """ Returns the index of the last width sector of the lane """ 252 | 253 | numWidths = len(self._widths) 254 | 255 | if numWidths > 1: 256 | return numWidths - 1 257 | 258 | return 0 259 | 260 | @property 261 | def borders(self): 262 | return self._borders 263 | 264 | 265 | class LaneLink(object): 266 | 267 | def __init__(self): 268 | self._predecessor = None 269 | self._successor = None 270 | 271 | @property 272 | def predecessorId(self): 273 | return self._predecessor 274 | 275 | @predecessorId.setter 276 | def predecessorId(self, value): 277 | self._predecessor = int(value) 278 | 279 | @property 280 | def successorId(self): 281 | return self._successor 282 | 283 | @successorId.setter 284 | def successorId(self, value): 285 | self._successor = int(value) 286 | 287 | 288 | class LaneWidth(object): 289 | 290 | def __init__(self): 291 | self._idx = None 292 | self._sOffset = None 293 | self._a = None 294 | self._b = None 295 | self._c = None 296 | self._d = None 297 | 298 | @property 299 | def idx(self): 300 | return self._idx 301 | 302 | @idx.setter 303 | def idx(self, value): 304 | self._idx = int(value) 305 | 306 | @property 307 | def sOffset(self): 308 | return self._sOffset 309 | 310 | @sOffset.setter 311 | def sOffset(self, value): 312 | self._sOffset = float(value) 313 | 314 | @property 315 | def a(self): 316 | return self._a 317 | 318 | @a.setter 319 | def a(self, value): 320 | self._a = float(value) 321 | 322 | @property 323 | def b(self): 324 | return self._b 325 | 326 | @b.setter 327 | def b(self, value): 328 | self._b = float(value) 329 | 330 | @property 331 | def c(self): 332 | return self._c 333 | 334 | @c.setter 335 | def c(self, value): 336 | self._c = float(value) 337 | 338 | @property 339 | def d(self): 340 | return self._d 341 | 342 | @d.setter 343 | def d(self, value): 344 | self._d = float(value) 345 | 346 | @property 347 | def coeffs(self): 348 | """ Array of coefficients for usage with numpy.polynomial.polynomial.polyval """ 349 | return [self._a, self._b, self._c, self._d] 350 | 351 | class LaneBorder(LaneWidth): 352 | pass 353 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadLateralProfile.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class LateralProfile(object): 4 | 5 | def __init__(self): 6 | self._superelevations = [] 7 | self._crossfalls = [] 8 | self._shapes = [] 9 | 10 | @property 11 | def superelevations(self): 12 | return self._superelevations 13 | 14 | @superelevations.setter 15 | def superelevations(self, value): 16 | if not isinstance(value, list) or not all(isinstance(x, Superelevation) for x in value): 17 | raise TypeError("Value must be an instance of Superelevation.") 18 | 19 | self._superelevations = value 20 | 21 | 22 | @property 23 | def crossfalls(self): 24 | return self._crossfalls 25 | 26 | @crossfalls.setter 27 | def crossfalls(self, value): 28 | if not isinstance(value, list) or not all(isinstance(x, Crossfall) for x in value): 29 | raise TypeError("Value must be an instance of Crossfall.") 30 | 31 | self._crossfalls = value 32 | 33 | 34 | @property 35 | def shapes(self): 36 | return self._shapes 37 | 38 | @shapes.setter 39 | def shapes(self, value): 40 | if not isinstance(value, list) or not all(isinstance(x, Shape) for x in value): 41 | raise TypeError("Value must be a list of instances of Shape.") 42 | 43 | self._shapes = value 44 | 45 | 46 | 47 | class Superelevation(object): 48 | 49 | def __init__(self): 50 | self._sPos = None 51 | self._a = None 52 | self._b = None 53 | self._c = None 54 | self._d = None 55 | 56 | @property 57 | def sPos(self): 58 | return self._sPos 59 | 60 | @sPos.setter 61 | def sPos(self, value): 62 | self._sPos = float(value) 63 | 64 | @property 65 | def a(self): 66 | return self._a 67 | 68 | @a.setter 69 | def a(self, value): 70 | self._a = float(value) 71 | 72 | @property 73 | def b(self): 74 | return self._b 75 | 76 | @b.setter 77 | def b(self, value): 78 | self._b = float(value) 79 | 80 | @property 81 | def c(self): 82 | return self._c 83 | 84 | @c.setter 85 | def c(self, value): 86 | self._c = float(value) 87 | 88 | @property 89 | def d(self): 90 | return self._d 91 | 92 | @d.setter 93 | def d(self, value): 94 | self._d = float(value) 95 | 96 | 97 | class Crossfall(object): 98 | 99 | def __init__(self): 100 | self._side = None 101 | self._sPos = None 102 | self._a = None 103 | self._b = None 104 | self._c = None 105 | self._d = None 106 | 107 | @property 108 | def side(self): 109 | return self._side 110 | 111 | @side.setter 112 | def side(self, value): 113 | if value not in ["left", "right", "both"]: 114 | raise TypeError("Value must be string with content 'left', 'right' or 'both'.") 115 | 116 | self._side = value 117 | 118 | @property 119 | def sPos(self): 120 | return self._sPos 121 | 122 | @sPos.setter 123 | def sPos(self, value): 124 | self._sPos = float(value) 125 | 126 | @property 127 | def a(self): 128 | return self._a 129 | 130 | @a.setter 131 | def a(self, value): 132 | self._a = float(value) 133 | 134 | @property 135 | def b(self): 136 | return self._b 137 | 138 | @b.setter 139 | def b(self, value): 140 | self._b = float(value) 141 | 142 | @property 143 | def c(self): 144 | return self._c 145 | 146 | @c.setter 147 | def c(self, value): 148 | self._c = float(value) 149 | 150 | @property 151 | def d(self): 152 | return self._d 153 | 154 | @d.setter 155 | def d(self, value): 156 | self._d = float(value) 157 | 158 | 159 | class Shape(object): 160 | 161 | def __init__(self): 162 | self._sPos = None 163 | self._t = None 164 | self._a = None 165 | self._b = None 166 | self._c = None 167 | self._d = None 168 | 169 | @property 170 | def sPos(self): 171 | return self._sPos 172 | 173 | @sPos.setter 174 | def sPos(self, value): 175 | self._sPos = float(value) 176 | 177 | @property 178 | def t(self): 179 | return self._t 180 | 181 | @t.setter 182 | def t(self, value): 183 | self._t = float(value) 184 | 185 | @property 186 | def a(self): 187 | return self._a 188 | 189 | @a.setter 190 | def a(self, value): 191 | self._a = float(value) 192 | 193 | @property 194 | def b(self): 195 | return self._b 196 | 197 | @b.setter 198 | def b(self, value): 199 | self._b = float(value) 200 | 201 | @property 202 | def c(self): 203 | return self._c 204 | 205 | @c.setter 206 | def c(self, value): 207 | self._c = float(value) 208 | 209 | @property 210 | def d(self): 211 | return self._d 212 | 213 | @d.setter 214 | def d(self, value): 215 | self._d = float(value) 216 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadLink.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class Link(object): 4 | 5 | def __init__(self): 6 | self._id = None 7 | self._predecessor = None 8 | self._successor = None 9 | self._neighbors = [] 10 | 11 | def __str__(self): 12 | return " > link id " + str(self._id) + " | successor: " + str(self._successor) 13 | 14 | @property 15 | def id(self): 16 | return self._id 17 | 18 | @id.setter 19 | def id(self, value): 20 | self._id = int(value) 21 | 22 | @property 23 | def predecessor(self): 24 | return self._predecessor 25 | 26 | @predecessor.setter 27 | def predecessor(self, value): 28 | if not isinstance(value, Predecessor): 29 | raise TypeError("Value must be Predecessor") 30 | 31 | self._predecessor = value 32 | 33 | @property 34 | def successor(self): 35 | return self._successor 36 | 37 | @successor.setter 38 | def successor(self, value): 39 | if not isinstance(value, Successor): 40 | raise TypeError("Value must be Successor") 41 | 42 | self._successor = value 43 | 44 | @property 45 | def neighbors(self): 46 | return self._neighbors 47 | 48 | @neighbors.setter 49 | def neighbors(self, value): 50 | if not isinstance(value, list) or not all(isinstance(x, Neighbor) for x in value): 51 | raise TypeError("Value must be list of instances of Neighbor.") 52 | 53 | self._neighbors = value 54 | 55 | def addNeighbor(self, value): 56 | if not isinstance(value, Neighbor): 57 | raise TypeError("Value must be Neighbor") 58 | 59 | self._neighbors.append(value) 60 | 61 | 62 | class Predecessor(object): 63 | 64 | def __init__(self): 65 | self._elementType = None 66 | self._elementId = None 67 | self._contactPoint = None 68 | 69 | def __str__(self): 70 | return str(self._elementType) + " with id " + str(self._elementId) + " contact at " + str(self._contactPoint) 71 | 72 | @property 73 | def elementType(self): 74 | return self._elementType 75 | 76 | @elementType.setter 77 | def elementType(self, value): 78 | if value not in ["road", "junction"]: 79 | raise AttributeError("Value must be road or junction") 80 | 81 | self._elementType = value 82 | 83 | @property 84 | def elementId(self): 85 | return self._elementId 86 | 87 | @elementId.setter 88 | def elementId(self, value): 89 | self._elementId = int(value) 90 | 91 | @property 92 | def contactPoint(self): 93 | return self._contactPoint 94 | 95 | @contactPoint.setter 96 | def contactPoint(self, value): 97 | if value not in ["start", "end"] and value is not None: 98 | raise AttributeError("Value must be start or end") 99 | 100 | self._contactPoint = value 101 | 102 | class Successor(Predecessor): 103 | pass 104 | 105 | # def __init__(self): 106 | # self._elementType = None 107 | # self._elementId = None 108 | # self._contactPoint = None 109 | 110 | # @property 111 | # def elementType(self): 112 | # return self._elementType 113 | 114 | # @elementType.setter 115 | # def elementType(self, value): 116 | # if value not in ["road", "junction"]: 117 | # raise AttributeError("Value must be road or junction") 118 | 119 | # self._elementType = value 120 | 121 | # @property 122 | # def elementId(self): 123 | # return self._elementId 124 | 125 | # @elementId.setter 126 | # def elementId(self, value): 127 | # self._elementId = int(value) 128 | 129 | # @property 130 | # def contactPoint(self): 131 | # return self._contactPoint 132 | 133 | # @contactPoint.setter 134 | # def contactPoint(self, value): 135 | # if value not in ["start", "end"] and value is not None: 136 | # raise AttributeError("Value must be start or end") 137 | 138 | # self._contactPoint = value 139 | 140 | 141 | class Neighbor(object): 142 | 143 | def __init__(self): 144 | self._side = None 145 | self._elementId = None 146 | self._direction = None 147 | 148 | @property 149 | def side(self): 150 | return self._side 151 | 152 | @side.setter 153 | def side(self, value): 154 | if value not in ["left", "right"]: 155 | raise AttributeError("Value must be left or right") 156 | 157 | self._side = value 158 | 159 | @property 160 | def elementId(self): 161 | return self._elementId 162 | 163 | @elementId.setter 164 | def elementId(self, value): 165 | self._elementId = int(value) 166 | 167 | @property 168 | def direction(self): 169 | return self._direction 170 | 171 | @direction.setter 172 | def direction(self, value): 173 | if value not in ["same", "opposite"]: 174 | raise AttributeError("Value must be same or opposite") 175 | 176 | self._direction = value 177 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadPlanView.py: -------------------------------------------------------------------------------- 1 | 2 | import abc 3 | import numpy as np 4 | from eulerspiral import eulerspiral 5 | 6 | 7 | class PlanView(object): 8 | 9 | def __init__(self): 10 | self._geometries = [] 11 | 12 | def addLine(self, startPosition, heading, length): 13 | self._geometries.append(Line(startPosition, heading, length)) 14 | 15 | def addSpiral(self, startPosition, heading, length, curvStart, curvEnd): 16 | self._geometries.append(Spiral(startPosition, heading, length, curvStart, curvEnd)) 17 | 18 | def addArc(self, startPosition, heading, length, curvature): 19 | self._geometries.append(Arc(startPosition, heading, length, curvature)) 20 | 21 | def addParamPoly3(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): 22 | self._geometries.append(ParamPoly3(startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange)) 23 | 24 | def getLength(self): 25 | """ Get length of whole plan view """ 26 | 27 | length = 0 28 | 29 | for geometry in self._geometries: 30 | length += geometry.getLength() 31 | 32 | return length 33 | 34 | def calc(self, sPos): 35 | """ Calculate position and tangent at sPos """ 36 | 37 | for geometry in self._geometries: 38 | if geometry.getLength() < sPos and not np.isclose(geometry.getLength(), sPos): 39 | sPos -= geometry.getLength() 40 | continue 41 | return geometry.calcPosition(sPos) 42 | 43 | raise Exception("Tried to calculate a position outside of the borders of the trajectory by s=" + str(sPos)) 44 | 45 | class Geometry(object): 46 | __metaclass__ = abc.ABCMeta 47 | 48 | @abc.abstractmethod 49 | def getStartPosition(self): 50 | """ Returns the overall geometry length """ 51 | return 52 | 53 | @abc.abstractmethod 54 | def getLength(self): 55 | """ Returns the overall geometry length """ 56 | return 57 | 58 | @abc.abstractmethod 59 | def calcPosition(self, s): 60 | """ Calculates the position of the geometry as if the starting point is (0/0) """ 61 | return 62 | 63 | class Line(Geometry): 64 | 65 | def __init__(self, startPosition, heading, length): 66 | self.startPosition = np.array(startPosition) 67 | self.heading = heading 68 | self.length = length 69 | 70 | def getStartPosition(self): 71 | return self.startPosition 72 | 73 | def getLength(self): 74 | return self.length 75 | 76 | def calcPosition(self, s): 77 | pos = self.startPosition + np.array([s * np.cos(self.heading), s * np.sin(self.heading)]) 78 | tangent = self.heading 79 | 80 | return (pos, tangent) 81 | 82 | class Arc(Geometry): 83 | 84 | def __init__(self, startPosition, heading, length, curvature): 85 | self.startPosition = np.array(startPosition) 86 | self.heading = heading 87 | self.length = length 88 | self.curvature = curvature 89 | 90 | def getStartPosition(self): 91 | return self.startPosition 92 | 93 | def getLength(self): 94 | return self.length 95 | 96 | def calcPosition(self, s): 97 | c = self.curvature 98 | hdg = self.heading - np.pi / 2 99 | 100 | a = 2 / c * np.sin(s * c / 2) 101 | alpha = (np.pi - s * c) / 2 - hdg 102 | 103 | dx = -1 * a * np.cos(alpha) 104 | dy = a * np.sin(alpha) 105 | 106 | pos = self.startPosition + np.array([dx, dy]) 107 | tangent = self.heading + s * self.curvature 108 | 109 | return (pos, tangent) 110 | 111 | class Spiral(Geometry): 112 | 113 | def __init__(self, startPosition, heading, length, curvStart, curvEnd): 114 | self._startPosition = np.array(startPosition) 115 | self._heading = heading 116 | self._length = length 117 | self._curvStart = curvStart 118 | self._curvEnd = curvEnd 119 | 120 | self._spiral = eulerspiral.EulerSpiral.createFromLengthAndCurvature(self._length, self._curvStart, self._curvEnd) 121 | 122 | def getStartPosition(self): 123 | return self._startPosition 124 | 125 | def getLength(self): 126 | return self._length 127 | 128 | def calcPosition(self, s): 129 | (x, y, t) = self._spiral.calc(s, self._startPosition[0], self._startPosition[1], self._curvStart, self._heading) 130 | 131 | return (np.array([x, y]), t) 132 | 133 | class Poly3(Geometry): 134 | 135 | def __init__(self, startPosition, heading, length, a, b, c, d): 136 | self._startPosition = np.array(startPosition) 137 | self._heading = heading 138 | self._length = length 139 | self._a = a 140 | self._b = b 141 | self._c = c 142 | self._d = d 143 | 144 | raise NotImplementedError() 145 | 146 | def getStartPosition(self): 147 | return self._startPosition 148 | 149 | def getLength(self): 150 | return self._length 151 | 152 | def calcPosition(self, s): 153 | # TODO untested 154 | 155 | # Calculate new point in s/t coordinate system 156 | coeffs = [self._a, self._b, self._c, self._d] 157 | 158 | t = np.polynomial.polynomial.polyval(s, coeffs) 159 | 160 | # Rotate and translate 161 | srot = s * np.cos(self._heading) - t * np.sin(self._heading) 162 | trot = s * np.sin(self._heading) + t * np.cos(self._heading) 163 | 164 | # Derivate to get heading change 165 | dCoeffs = coeffs[1:] * np.array(np.arange(1, len(coeffs))) 166 | tangent = np.polynomial.polynomial.polyval(s, dCoeffs) 167 | 168 | return (self._startPosition + np.array([srot, trot]), self._heading + tangent) 169 | 170 | class ParamPoly3(Geometry): 171 | 172 | def __init__(self, startPosition, heading, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange): 173 | self._startPosition = np.array(startPosition) 174 | self._heading = heading 175 | self._length = length 176 | 177 | self._aU = aU 178 | self._bU = bU 179 | self._cU = cU 180 | self._dU = dU 181 | self._aV = aV 182 | self._bV = bV 183 | self._cV = cV 184 | self._dV = dV 185 | 186 | if pRange is None: 187 | self._pRange = 1.0 188 | else: 189 | self._pRange = pRange 190 | 191 | def getStartPosition(self): 192 | return self._startPosition 193 | 194 | def getLength(self): 195 | return self._length 196 | 197 | def calcPosition(self, s): 198 | 199 | # Position 200 | pos = (s / self._length) * self._pRange 201 | 202 | coeffsU = [self._aU, self._bU, self._cU, self._dU] 203 | coeffsV = [self._aV, self._bV, self._cV, self._dV] 204 | 205 | x = np.polynomial.polynomial.polyval(pos, coeffsU) 206 | y = np.polynomial.polynomial.polyval(pos, coeffsV) 207 | 208 | xrot = x * np.cos(self._heading) - y * np.sin(self._heading) 209 | yrot = x * np.sin(self._heading) + y * np.cos(self._heading) 210 | 211 | # Tangent is defined by derivation 212 | dCoeffsU = coeffsU[1:] * np.array(np.arange(1, len(coeffsU))) 213 | dCoeffsV = coeffsV[1:] * np.array(np.arange(1, len(coeffsV))) 214 | 215 | dx = np.polynomial.polynomial.polyval(pos, dCoeffsU) 216 | dy = np.polynomial.polynomial.polyval(pos, dCoeffsV) 217 | 218 | tangent = np.arctan2(dy, dx) 219 | 220 | 221 | return (self._startPosition + np.array([xrot, yrot]), self._heading + tangent) 222 | -------------------------------------------------------------------------------- /opendriveparser/elements/roadType.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class Type(object): 4 | 5 | allowedTypes = ["unknown", "rural", "motorway", "town", "lowSpeed", "pedestrian", "bicycle"] 6 | 7 | def __init__(self): 8 | self._sPos = None 9 | self._type = None 10 | self._speed = None 11 | 12 | @property 13 | def sPos(self): 14 | return self._sPos 15 | 16 | @sPos.setter 17 | def sPos(self, value): 18 | self._sPos = float(value) 19 | 20 | @property 21 | def type(self): 22 | return self._type 23 | 24 | @type.setter 25 | def type(self, value): 26 | if value not in self.allowedTypes: 27 | raise AttributeError("Type not allowed.") 28 | 29 | self._type = value 30 | 31 | @property 32 | def speed(self): 33 | return self._speed 34 | 35 | @speed.setter 36 | def speed(self, value): 37 | if not isinstance(value, Speed): 38 | raise TypeError("Value must be instance of Speed.") 39 | 40 | self._speed = value 41 | 42 | 43 | class Speed(object): 44 | 45 | def __init__(self): 46 | self._max = None 47 | self._unit = None 48 | 49 | @property 50 | def max(self): 51 | return self._max 52 | 53 | @max.setter 54 | def max(self, value): 55 | self._max = str(value) 56 | 57 | @property 58 | def unit(self): 59 | return self._unit 60 | 61 | @unit.setter 62 | def unit(self, value): 63 | # TODO validate input 64 | self._unit = str(value) 65 | -------------------------------------------------------------------------------- /opendriveparser/parser.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | from lxml import etree 4 | 5 | from opendriveparser.elements.openDrive import OpenDrive 6 | from opendriveparser.elements.road import Road 7 | from opendriveparser.elements.roadLink import Predecessor as RoadLinkPredecessor, Successor as RoadLinkSuccessor, Neighbor as RoadLinkNeighbor 8 | from opendriveparser.elements.roadType import Type as RoadType, Speed as RoadTypeSpeed 9 | from opendriveparser.elements.roadElevationProfile import Elevation as RoadElevationProfileElevation 10 | from opendriveparser.elements.roadLateralProfile import Superelevation as RoadLateralProfileSuperelevation, Crossfall as RoadLateralProfileCrossfall, Shape as RoadLateralProfileShape 11 | from opendriveparser.elements.roadLanes import LaneOffset as RoadLanesLaneOffset, Lane as RoadLaneSectionLane, LaneSection as RoadLanesSection, LaneWidth as RoadLaneSectionLaneWidth, LaneBorder as RoadLaneSectionLaneBorder 12 | from opendriveparser.elements.junction import Junction, Connection as JunctionConnection, LaneLink as JunctionConnectionLaneLink 13 | 14 | 15 | 16 | def parse_opendrive(rootNode): 17 | """ Tries to parse XML tree, return OpenDRIVE object """ 18 | 19 | # Only accept xml element 20 | if not etree.iselement(rootNode): 21 | raise TypeError("Argument rootNode is not a xml element") 22 | 23 | 24 | newOpenDrive = OpenDrive() 25 | 26 | # Header 27 | header = rootNode.find("header") 28 | 29 | if header is not None: 30 | 31 | # Reference 32 | if header.find("geoReference") is not None: 33 | pass 34 | 35 | # Junctions 36 | for junction in rootNode.findall("junction"): 37 | 38 | newJunction = Junction() 39 | 40 | newJunction.id = int(junction.get("id")) 41 | newJunction.name = str(junction.get("name")) 42 | 43 | for connection in junction.findall("connection"): 44 | 45 | newConnection = JunctionConnection() 46 | 47 | newConnection.id = connection.get("id") 48 | newConnection.incomingRoad = connection.get("incomingRoad") 49 | newConnection.connectingRoad = connection.get("connectingRoad") 50 | newConnection.contactPoint = connection.get("contactPoint") 51 | 52 | for laneLink in connection.findall("laneLink"): 53 | 54 | newLaneLink = JunctionConnectionLaneLink() 55 | 56 | newLaneLink.fromId = laneLink.get("from") 57 | newLaneLink.toId = laneLink.get("to") 58 | 59 | newConnection.addLaneLink(newLaneLink) 60 | 61 | newJunction.addConnection(newConnection) 62 | 63 | newOpenDrive.junctions.append(newJunction) 64 | 65 | 66 | 67 | # Load roads 68 | for road in rootNode.findall("road"): 69 | 70 | newRoad = Road() 71 | 72 | newRoad.id = int(road.get("id")) 73 | newRoad.name = road.get("name") 74 | newRoad.junction = int(road.get("junction")) if road.get("junction") != "-1" else None 75 | 76 | # TODO: Problems!!!! 77 | newRoad.length = float(road.get("length")) 78 | 79 | # Links 80 | if road.find("link") is not None: 81 | 82 | predecessor = road.find("link").find("predecessor") 83 | 84 | if predecessor is not None: 85 | 86 | newPredecessor = RoadLinkPredecessor() 87 | 88 | newPredecessor.elementType = predecessor.get("elementType") 89 | newPredecessor.elementId = predecessor.get("elementId") 90 | newPredecessor.contactPoint = predecessor.get("contactPoint") 91 | 92 | newRoad.link.predecessor = newPredecessor 93 | 94 | 95 | successor = road.find("link").find("successor") 96 | 97 | if successor is not None: 98 | 99 | newSuccessor = RoadLinkSuccessor() 100 | 101 | newSuccessor.elementType = successor.get("elementType") 102 | newSuccessor.elementId = successor.get("elementId") 103 | newSuccessor.contactPoint = successor.get("contactPoint") 104 | 105 | newRoad.link.successor = newSuccessor 106 | 107 | for neighbor in road.find("link").findall("neighbor"): 108 | 109 | newNeighbor = RoadLinkNeighbor() 110 | 111 | newNeighbor.side = neighbor.get("side") 112 | newNeighbor.elementId = neighbor.get("elementId") 113 | newNeighbor.direction = neighbor.get("direction") 114 | 115 | newRoad.link.neighbors.append(newNeighbor) 116 | 117 | 118 | # Type 119 | for roadType in road.findall("type"): 120 | 121 | newType = RoadType() 122 | 123 | newType.sPos = roadType.get("s") 124 | newType.type = roadType.get("type") 125 | 126 | if roadType.find("speed"): 127 | 128 | newSpeed = RoadTypeSpeed() 129 | 130 | newSpeed.max = roadType.find("speed").get("max") 131 | newSpeed.unit = roadType.find("speed").get("unit") 132 | 133 | newType.speed = newSpeed 134 | 135 | newRoad.types.append(newType) 136 | 137 | 138 | # Plan view 139 | for geometry in road.find("planView").findall("geometry"): 140 | 141 | startCoord = [float(geometry.get("x")), float(geometry.get("y"))] 142 | 143 | if geometry.find("line") is not None: 144 | newRoad.planView.addLine(startCoord, float(geometry.get("hdg")), float(geometry.get("length"))) 145 | 146 | elif geometry.find("spiral") is not None: 147 | newRoad.planView.addSpiral(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("spiral").get("curvStart")), float(geometry.find("spiral").get("curvEnd"))) 148 | 149 | elif geometry.find("arc") is not None: 150 | newRoad.planView.addArc(startCoord, float(geometry.get("hdg")), float(geometry.get("length")), float(geometry.find("arc").get("curvature"))) 151 | 152 | elif geometry.find("poly3") is not None: 153 | raise NotImplementedError() 154 | 155 | elif geometry.find("paramPoly3") is not None: 156 | if geometry.find("paramPoly3").get("pRange"): 157 | 158 | if geometry.find("paramPoly3").get("pRange") == "arcLength": 159 | pMax = float(geometry.get("length")) 160 | else: 161 | pMax = None 162 | else: 163 | pMax = None 164 | 165 | newRoad.planView.addParamPoly3( \ 166 | startCoord, \ 167 | float(geometry.get("hdg")), \ 168 | float(geometry.get("length")), \ 169 | float(geometry.find("paramPoly3").get("aU")), \ 170 | float(geometry.find("paramPoly3").get("bU")), \ 171 | float(geometry.find("paramPoly3").get("cU")), \ 172 | float(geometry.find("paramPoly3").get("dU")), \ 173 | float(geometry.find("paramPoly3").get("aV")), \ 174 | float(geometry.find("paramPoly3").get("bV")), \ 175 | float(geometry.find("paramPoly3").get("cV")), \ 176 | float(geometry.find("paramPoly3").get("dV")), \ 177 | pMax \ 178 | ) 179 | 180 | else: 181 | raise Exception("invalid xml") 182 | 183 | 184 | # Elevation profile 185 | if road.find("elevationProfile") is not None: 186 | 187 | for elevation in road.find("elevationProfile").findall("elevation"): 188 | 189 | newElevation = RoadElevationProfileElevation() 190 | 191 | newElevation.sPos = elevation.get("s") 192 | newElevation.a = elevation.get("a") 193 | newElevation.b = elevation.get("b") 194 | newElevation.c = elevation.get("c") 195 | newElevation.d = elevation.get("d") 196 | 197 | newRoad.elevationProfile.elevations.append(newElevation) 198 | 199 | 200 | # Lateral profile 201 | if road.find("lateralProfile") is not None: 202 | 203 | for superelevation in road.find("lateralProfile").findall("superelevation"): 204 | 205 | newSuperelevation = RoadLateralProfileSuperelevation() 206 | 207 | newSuperelevation.sPos = superelevation.get("s") 208 | newSuperelevation.a = superelevation.get("a") 209 | newSuperelevation.b = superelevation.get("b") 210 | newSuperelevation.c = superelevation.get("c") 211 | newSuperelevation.d = superelevation.get("d") 212 | 213 | newRoad.lateralProfile.superelevations.append(newSuperelevation) 214 | 215 | for crossfall in road.find("lateralProfile").findall("crossfall"): 216 | 217 | newCrossfall = RoadLateralProfileCrossfall() 218 | 219 | newCrossfall.side = crossfall.get("side") 220 | newCrossfall.sPos = crossfall.get("s") 221 | newCrossfall.a = crossfall.get("a") 222 | newCrossfall.b = crossfall.get("b") 223 | newCrossfall.c = crossfall.get("c") 224 | newCrossfall.d = crossfall.get("d") 225 | 226 | newRoad.lateralProfile.crossfalls.append(newCrossfall) 227 | 228 | for shape in road.find("lateralProfile").findall("shape"): 229 | 230 | newShape = RoadLateralProfileShape() 231 | 232 | newShape.sPos = shape.get("s") 233 | newShape.t = shape.get("t") 234 | newShape.a = shape.get("a") 235 | newShape.b = shape.get("b") 236 | newShape.c = shape.get("c") 237 | newShape.d = shape.get("d") 238 | 239 | newRoad.lateralProfile.shapes.append(newShape) 240 | 241 | 242 | # Lanes 243 | lanes = road.find("lanes") 244 | 245 | if lanes is None: 246 | raise Exception("Road must have lanes element") 247 | 248 | # Lane offset 249 | for laneOffset in lanes.findall("laneOffset"): 250 | 251 | newLaneOffset = RoadLanesLaneOffset() 252 | 253 | newLaneOffset.sPos = laneOffset.get("s") 254 | newLaneOffset.a = laneOffset.get("a") 255 | newLaneOffset.b = laneOffset.get("b") 256 | newLaneOffset.c = laneOffset.get("c") 257 | newLaneOffset.d = laneOffset.get("d") 258 | 259 | newRoad.lanes.laneOffsets.append(newLaneOffset) 260 | 261 | 262 | # Lane sections 263 | for laneSectionIdx, laneSection in enumerate(road.find("lanes").findall("laneSection")): 264 | 265 | newLaneSection = RoadLanesSection() 266 | 267 | # Manually enumerate lane sections for referencing purposes 268 | newLaneSection.idx = laneSectionIdx 269 | 270 | newLaneSection.sPos = laneSection.get("s") 271 | newLaneSection.singleSide = laneSection.get("singleSide") 272 | 273 | sides = dict( 274 | left=newLaneSection.leftLanes, 275 | center=newLaneSection.centerLanes, 276 | right=newLaneSection.rightLanes 277 | ) 278 | 279 | for sideTag, newSideLanes in sides.items(): 280 | 281 | side = laneSection.find(sideTag) 282 | 283 | # It is possible one side is not present 284 | if side is None: 285 | continue 286 | 287 | for lane in side.findall("lane"): 288 | 289 | newLane = RoadLaneSectionLane() 290 | 291 | newLane.id = lane.get("id") 292 | newLane.type = lane.get("type") 293 | newLane.level = lane.get("level") 294 | 295 | # Lane Links 296 | if lane.find("link") is not None: 297 | 298 | if lane.find("link").find("predecessor") is not None: 299 | newLane.link.predecessorId = lane.find("link").find("predecessor").get("id") 300 | 301 | if lane.find("link").find("successor") is not None: 302 | newLane.link.successorId = lane.find("link").find("successor").get("id") 303 | 304 | # Width 305 | for widthIdx, width in enumerate(lane.findall("width")): 306 | 307 | newWidth = RoadLaneSectionLaneWidth() 308 | 309 | newWidth.idx = widthIdx 310 | newWidth.sOffset = width.get("sOffset") 311 | newWidth.a = width.get("a") 312 | newWidth.b = width.get("b") 313 | newWidth.c = width.get("c") 314 | newWidth.d = width.get("d") 315 | 316 | newLane.widths.append(newWidth) 317 | 318 | # Border 319 | for borderIdx, border in enumerate(lane.findall("border")): 320 | 321 | newBorder = RoadLaneSectionLaneBorder() 322 | 323 | newBorder.idx = borderIdx 324 | newBorder.sPos = border.get("sOffset") 325 | newBorder.a = border.get("a") 326 | newBorder.b = border.get("b") 327 | newBorder.c = border.get("c") 328 | newBorder.d = border.get("d") 329 | 330 | newLane.borders.append(newBorder) 331 | 332 | # Road Marks 333 | # TODO 334 | 335 | # Material 336 | # TODO 337 | 338 | # Visiblility 339 | # TODO 340 | 341 | # Speed 342 | # TODO 343 | 344 | # Access 345 | # TODO 346 | 347 | # Lane Height 348 | # TODO 349 | 350 | # Rules 351 | # TODO 352 | 353 | newSideLanes.append(newLane) 354 | 355 | newRoad.lanes.laneSections.append(newLaneSection) 356 | 357 | 358 | # OpenDrive does not provide lane section lengths by itself, calculate them by ourselves 359 | for laneSection in newRoad.lanes.laneSections: 360 | 361 | # Last lane section in road 362 | if laneSection.idx + 1 >= len(newRoad.lanes.laneSections): 363 | laneSection.length = newRoad.planView.getLength() - laneSection.sPos 364 | 365 | # All but the last lane section end at the succeeding one 366 | else: 367 | laneSection.length = newRoad.lanes.laneSections[laneSection.idx + 1].sPos - laneSection.sPos 368 | 369 | # OpenDrive does not provide lane width lengths by itself, calculate them by ourselves 370 | for laneSection in newRoad.lanes.laneSections: 371 | for lane in laneSection.allLanes: 372 | widthsPoses = np.array([x.sOffset for x in lane.widths] + [laneSection.length]) 373 | widthsLengths = widthsPoses[1:] - widthsPoses[:-1] 374 | 375 | for widthIdx, width in enumerate(lane.widths): 376 | width.length = widthsLengths[widthIdx] 377 | 378 | # Objects 379 | # TODO 380 | 381 | # Signals 382 | # TODO 383 | 384 | newOpenDrive.roads.append(newRoad) 385 | 386 | return newOpenDrive 387 | -------------------------------------------------------------------------------- /parse_and_visualize.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | """ 4 | 5 | 6 | import os 7 | 8 | from lxml import etree 9 | from tqdm import tqdm 10 | 11 | from opendriveparser import parse_opendrive 12 | from math import pi, sin, cos 13 | 14 | 15 | # Prepare the input file. 16 | XODR_FILE = "data/test.xodr" 17 | 18 | 19 | def to_color(r, g, b): 20 | return '#{:02x}{:02x}{:02x}'.format(r, g, b) 21 | 22 | 23 | # Prepare the colors. 24 | DRIVING_COLOR = (135, 151, 154) 25 | TYPE_COLOR_DICT = { 26 | "shoulder": (136, 158, 131), 27 | "border": (84, 103, 80), 28 | "driving": DRIVING_COLOR, 29 | "stop": (128, 68, 59), 30 | "none": (236, 236, 236), 31 | "restricted": (165, 134, 88), 32 | "parking": DRIVING_COLOR, 33 | "median": (119, 155, 88), 34 | "biking": (108, 145, 125), 35 | "sidewalk": (106, 159, 170), 36 | "curb": (30, 49, 53), 37 | "exit": DRIVING_COLOR, 38 | "entry": DRIVING_COLOR, 39 | "onramp": DRIVING_COLOR, 40 | "offRamp": DRIVING_COLOR, 41 | "connectingRamp": DRIVING_COLOR, 42 | "onRamp": DRIVING_COLOR, 43 | "bidirectional": DRIVING_COLOR, 44 | } 45 | TYPE_COLOR_DICT = {k: to_color(*v) for k, v in TYPE_COLOR_DICT.items()} 46 | COLOR_CENTER_LANE = "#FFC500" 47 | COLOR_REFERECE_LINE = "#0000EE" 48 | 49 | # Prepare sample step. 50 | STEP = 0.1 51 | 52 | 53 | def load_xodr_and_parse(file=XODR_FILE): 54 | """ 55 | Load and parse .xodr file. 56 | :param file: 57 | :return: 58 | """ 59 | with open(file, 'r') as fh: 60 | parser = etree.XMLParser() 61 | root_node = etree.parse(fh, parser).getroot() 62 | road_network = parse_opendrive(root_node) 63 | return road_network 64 | 65 | 66 | def calculate_reference_points_of_one_geometry(geometry, length, step=0.01): 67 | """ 68 | Calculate the stepwise reference points with position(x, y), tangent and distance between the point and the start. 69 | :param geometry: 70 | :param length: 71 | :param step: 72 | :return: 73 | """ 74 | nums = int(length / step) 75 | res = [] 76 | for i in range(nums): 77 | s_ = step * i 78 | pos_, tangent_ = geometry.calcPosition(s_) 79 | x, y = pos_ 80 | one_point = { 81 | "position": (x, y), # The location of the reference point 82 | "tangent": tangent_, # Orientation of the reference point 83 | "s_geometry": s_, # The distance between the start point of the geometry and current point along the reference line 84 | } 85 | res.append(one_point) 86 | return res 87 | 88 | 89 | def get_geometry_length(geometry): 90 | """ 91 | Get the length of one geometry (or the length of the reference line of the geometry). 92 | :param geometry: 93 | :return: 94 | """ 95 | if hasattr(geometry, "length"): 96 | length = geometry.length 97 | elif hasattr(geometry, "_length"): 98 | length = geometry._length # Some geometry has the attribute "_length". 99 | else: 100 | raise AttributeError("No attribute length found!!!") 101 | return length 102 | 103 | 104 | def get_all_reference_points_of_one_road(geometries, step=0.01): 105 | """ 106 | Obtain the sampling point of the reference line of the road, including: 107 | the position of the point 108 | the direction of the reference line at the point 109 | the distance of the point along the reference line relative to the start of the road 110 | the distance of the point relative to the start of geometry along the reference line 111 | :param geometries: Geometries of one road. 112 | :param step: Calculate steps. 113 | :return: 114 | """ 115 | reference_points = [] 116 | s_start_road = 0 117 | for geometry_id, geometry in enumerate(geometries): 118 | geometry_length = get_geometry_length(geometry) 119 | 120 | # Calculate all the reference points of current geometry. 121 | pos_tangent_s_list = calculate_reference_points_of_one_geometry(geometry, geometry_length, step=step) 122 | 123 | # As for every reference points, add the distance start by road and its geometry index. 124 | pos_tangent_s_s_list = [{**point, 125 | "s_road": point["s_geometry"]+s_start_road, 126 | "index_geometry": geometry_id} 127 | for point in pos_tangent_s_list] 128 | reference_points.extend(pos_tangent_s_s_list) 129 | 130 | s_start_road += geometry_length 131 | return reference_points 132 | 133 | 134 | def get_width(widths, s): 135 | assert isinstance(widths, list), TypeError(type(widths)) 136 | widths.sort(key=lambda x: x.sOffset) 137 | current_width = None 138 | # EPS = 1e-5 139 | milestones = [width.sOffset for width in widths] + [float("inf")] 140 | 141 | control_mini_section = [(start, end) for (start, end) in zip(milestones[:-1], milestones[1:])] 142 | for width, start_end in zip(widths, control_mini_section): 143 | start, end = start_end 144 | if start <= s < end: 145 | ds = s - width.sOffset 146 | current_width = width.a + width.b * ds + width.c * ds ** 2 + width.d * ds ** 3 147 | return current_width 148 | 149 | 150 | def get_lane_offset(lane_offsets, section_s, length=float("inf")): 151 | 152 | assert isinstance(lane_offsets, list), TypeError(type(lane_offsets)) 153 | if not lane_offsets: 154 | return 0 155 | lane_offsets.sort(key=lambda x: x.sPos) 156 | current_offset = 0 157 | EPS = 1e-5 158 | milestones = [lane_offset.sPos for lane_offset in lane_offsets] + [length+EPS] 159 | 160 | control_mini_section = [(start, end) for (start, end) in zip(milestones[:-1], milestones[1:])] 161 | for offset_params, start_end in zip(lane_offsets, control_mini_section): 162 | start, end = start_end 163 | if start <= section_s < end: 164 | ds = section_s - offset_params.sPos 165 | current_offset = offset_params.a + offset_params.b * ds + offset_params.c * ds ** 2 + offset_params.d * ds ** 3 166 | return current_offset 167 | 168 | 169 | class LaneOffsetCalculate: 170 | 171 | def __init__(self, lane_offsets): 172 | lane_offsets = list(sorted(lane_offsets, key=lambda x: x.sPos)) 173 | lane_offsets_dict = dict() 174 | for lane_offset in lane_offsets: 175 | a = lane_offset.a 176 | b = lane_offset.b 177 | c = lane_offset.c 178 | d = lane_offset.d 179 | s_start = lane_offset.sPos 180 | lane_offsets_dict[s_start] = (a, b, c, d) 181 | self.lane_offsets_dict = lane_offsets_dict 182 | 183 | def calculate_offset(self, s): 184 | for s_start, (a, b, c, d) in reversed(self.lane_offsets_dict.items()): # e.g. 75, 25 185 | if s >= s_start: 186 | ds = s - s_start 187 | offset = a + b * ds + c * ds ** 2 + d * ds ** 3 188 | return offset 189 | return 0 190 | 191 | 192 | def calculate_area_of_one_left_lane(left_lane, points, most_left_points): 193 | inner_points = most_left_points[:] 194 | 195 | widths = left_lane.widths 196 | update_points = [] 197 | for reference_point, inner_point in zip(points, inner_points): 198 | 199 | tangent = reference_point["tangent"] 200 | s_lane_section = reference_point["s_lane_section"] 201 | lane_width = get_width(widths, s_lane_section) 202 | 203 | normal_left = tangent + pi / 2 204 | x_inner, y_inner = inner_point 205 | 206 | lane_width_offset = lane_width 207 | 208 | x_outer = x_inner + cos(normal_left) * lane_width_offset 209 | y_outer = y_inner + sin(normal_left) * lane_width_offset 210 | 211 | update_points.append((x_outer, y_outer)) 212 | 213 | outer_points = update_points[:] 214 | most_left_points = outer_points[:] 215 | 216 | current_ara = { 217 | "inner": inner_points, 218 | "outer": outer_points, 219 | } 220 | return current_ara, most_left_points 221 | 222 | 223 | def calculate_area_of_one_right_lane(right_lane, points, most_right_points): 224 | inner_points = most_right_points[:] 225 | 226 | widths = right_lane.widths 227 | update_points = [] 228 | for reference_point, inner_point in zip(points, inner_points): 229 | 230 | tangent = reference_point["tangent"] 231 | s_lane_section = reference_point["s_lane_section"] 232 | lane_width = get_width(widths, s_lane_section) 233 | 234 | normal_eight = tangent - pi / 2 235 | x_inner, y_inner = inner_point 236 | 237 | lane_width_offset = lane_width 238 | 239 | x_outer = x_inner + cos(normal_eight) * lane_width_offset 240 | y_outer = y_inner + sin(normal_eight) * lane_width_offset 241 | 242 | update_points.append((x_outer, y_outer)) 243 | 244 | outer_points = update_points[:] 245 | most_right_points = outer_points[:] 246 | 247 | current_ara = { 248 | "inner": inner_points, 249 | "outer": outer_points, 250 | } 251 | return current_ara, most_right_points 252 | 253 | 254 | def calculate_lane_area_within_one_lane_section(lane_section, points): 255 | """ 256 | Lane areas are represented by boundary lattice. Calculate boundary points of every lanes. 257 | :param lane_section: 258 | :param points: 259 | :return: 260 | """ 261 | 262 | all_lanes = lane_section.allLanes 263 | 264 | # Process the lane indexes. 265 | left_lanes = [lane for lane in all_lanes if int(lane.id) > 0] 266 | right_lanes = [lane for lane in all_lanes if int(lane.id) < 0] 267 | left_lanes.sort(key=lambda x: x.id) 268 | right_lanes.sort(reverse=True, key=lambda x: x.id) 269 | 270 | # Get the lane area of left lanes and the most left lane line. 271 | left_lanes_area = dict() 272 | most_left_points = [point["position_center_lane"] for point in points][:] 273 | for left_lane in left_lanes: 274 | current_area, most_left_points = calculate_area_of_one_left_lane(left_lane, points, most_left_points) 275 | left_lanes_area[left_lane.id] = current_area 276 | 277 | # Get the lane area of right lanes and the most right lane line. 278 | right_lanes_area = dict() 279 | most_right_points = [point["position_center_lane"] for point in points][:] 280 | for right_lane in right_lanes: 281 | current_area, most_right_points = calculate_area_of_one_right_lane(right_lane, points, most_right_points) 282 | right_lanes_area[right_lane.id] = current_area 283 | 284 | return left_lanes_area, right_lanes_area, most_left_points, most_right_points 285 | 286 | 287 | def calculate_points_of_reference_line_of_one_section(points): 288 | """ 289 | Calculate center lane points accoding to the reference points and offsets. 290 | :param points: Points on reference line including position and tangent. 291 | :return: Updated points. 292 | """ 293 | res = [] 294 | for point in points: 295 | tangent = point["tangent"] 296 | x, y = point["position"] # Points on reference line. 297 | normal = tangent + pi / 2 298 | lane_offset = point["lane_offset"] # Offset of center lane. 299 | 300 | x += cos(normal) * lane_offset 301 | y += sin(normal) * lane_offset 302 | 303 | point = { 304 | **point, 305 | "position_center_lane": (x, y), 306 | } 307 | res.append(point) 308 | return res 309 | 310 | 311 | def calculate_s_lane_section(reference_points, lane_sections): 312 | 313 | res = [] 314 | for point in reference_points: 315 | 316 | for lane_section in reversed(lane_sections): 317 | if point["s_road"] >= lane_section.sPos: 318 | res.append( 319 | { 320 | **point, 321 | "s_lane_section": point["s_road"] - lane_section.sPos, 322 | "index_lane_section": lane_section.idx, 323 | } 324 | ) 325 | break 326 | return res 327 | 328 | 329 | def uncompress_dict_list(dict_list: list): 330 | assert isinstance(dict_list, list), TypeError("Keys") 331 | if not dict_list: 332 | return dict() 333 | 334 | keys = set(dict_list[0].keys()) 335 | for dct in dict_list: 336 | cur = set(dct.keys()) 337 | assert keys == cur, "Inconsistency of dict keys! {} {}".format(keys, cur) 338 | 339 | res = dict() 340 | for sample in dict_list: 341 | for k, v in sample.items(): 342 | if k not in res: 343 | res[k] = [v] 344 | else: 345 | res[k].append(v) 346 | 347 | keys = list(sorted(list(keys))) 348 | res = {k: res[k] for k in keys} 349 | return res 350 | 351 | 352 | def get_lane_line(section_data: dict): 353 | """ 354 | 提取车道分界线 355 | :param section_data: 356 | :return: 357 | """ 358 | left_lanes_area = section_data["left_lanes_area"] 359 | right_lanes_area = section_data["right_lanes_area"] 360 | 361 | lane_line_left = dict() 362 | if left_lanes_area: 363 | indexes = list(left_lanes_area.keys()) # 默认是排好序的 364 | for index_inner, index_outer in zip(indexes, indexes[1:] + ["NAN"]): 365 | lane_line_left[(index_inner, index_outer)] = left_lanes_area[index_inner]["outer"] 366 | 367 | lane_line_right = dict() 368 | if right_lanes_area: 369 | indexes = list(right_lanes_area.keys()) # 默认是排好序的 370 | for index_inner, index_outer in zip(indexes, indexes[1:] + ["NAN"]): 371 | lane_line_right[(index_inner, index_outer)] = right_lanes_area[index_inner]["outer"] 372 | 373 | return {"lane_line_left": lane_line_left, "lane_line_right": lane_line_right} 374 | 375 | 376 | def get_lane_area_of_one_road(road, step=0.01): 377 | """ 378 | Get all corresponding positions of every lane section in one road. 379 | :param road: 380 | :param step: 381 | :return: A dictionary of dictionary: {(road id, lane section id): section data} 382 | Section data is a dictionary of position information. 383 | section_data = { 384 | "left_lanes_area": left_lanes_area, 385 | "right_lanes_area": right_lanes_area, 386 | "most_left_points": most_left_points, 387 | "most_right_points": most_right_points, 388 | "types": types, 389 | "reference_points": uncompressed_lane_section_data, 390 | } 391 | """ 392 | geometries = road.planView._geometries 393 | # Lane offset is the offset between center lane (width is 0) and the reference line. 394 | lane_offsets = road.lanes.laneOffsets 395 | lane_offset_calculate = LaneOffsetCalculate(lane_offsets=lane_offsets) 396 | lane_sections = road.lanes.laneSections 397 | lane_sections = list(sorted(lane_sections, key=lambda x: x.sPos)) # Sort the lane sections by start position. 398 | 399 | reference_points = get_all_reference_points_of_one_road(geometries, step=step) # Extract the reference points. 400 | 401 | # Calculate the offsets of center lane. 402 | reference_points = [{**point, "lane_offset": lane_offset_calculate.calculate_offset(point["s_road"])} 403 | for point in reference_points] 404 | 405 | # Calculate the points of center lane based on reference points and offsets. 406 | reference_points = calculate_points_of_reference_line_of_one_section(reference_points) 407 | 408 | # Calculate the distance of each point starting from the current section along the direction of the reference line. 409 | reference_points = calculate_s_lane_section(reference_points, lane_sections) 410 | 411 | total_areas = dict() 412 | for lane_section in lane_sections: 413 | section_start = lane_section.sPos # Start position of the section in current road. 414 | section_end = lane_section.sPos + lane_section.length # End position of the section in current road. 415 | 416 | # Filter out the points belonging to current lane section. 417 | current_reference_points = list(filter(lambda x: section_start <= x["s_road"] < section_end, reference_points)) 418 | 419 | # Calculate the boundary point of every lane in current lane section. 420 | area = calculate_lane_area_within_one_lane_section(lane_section, current_reference_points) 421 | left_lanes_area, right_lanes_area, most_left_points, most_right_points = area 422 | 423 | # Extract types and indexes. 424 | types = {lane.id: lane.type for lane in lane_section.allLanes if lane.id != 0} 425 | index = (road.id, lane_section.idx) 426 | 427 | # Convert dict list to list dict of the reference points information. 428 | uncompressed_lane_section_data = uncompress_dict_list(current_reference_points) 429 | 430 | # Integrate all the information of current lane section of current road. 431 | section_data = { 432 | "left_lanes_area": left_lanes_area, 433 | "right_lanes_area": right_lanes_area, 434 | "most_left_points": most_left_points, 435 | "most_right_points": most_right_points, 436 | "types": types, 437 | "reference_points": uncompressed_lane_section_data, # 这些是lane section的信息 438 | } 439 | 440 | # Get all lane lines with their left and right lanes. 441 | lane_line = get_lane_line(section_data) 442 | section_data.update(lane_line) 443 | 444 | total_areas[index] = section_data 445 | 446 | return total_areas 447 | 448 | 449 | def get_all_lanes(road_network, step=0.1): 450 | """ 451 | Get all lanes of one road network. 452 | :param road_network: Parsed road network. 453 | :param step: Step of calculation. 454 | :return: Dictionary with the following format: 455 | keys: (road id, lane section id) 456 | values: dict(left_lanes_area, right_lanes_area, most_left_points, most_right_points, types, reference_points) 457 | """ 458 | roads = road_network.roads 459 | total_areas_all_roads = dict() 460 | 461 | for road in tqdm(roads, desc="Calculating boundary points."): 462 | lanes_of_one_road = get_lane_area_of_one_road(road, step=step) 463 | total_areas_all_roads = {**total_areas_all_roads, **lanes_of_one_road} 464 | 465 | return total_areas_all_roads 466 | 467 | 468 | def rescale_color(hex_color, rate=0.5): 469 | """ 470 | Half the light of input color, e.g. white => grey. 471 | :param hex_color: e.g. #a55f13 472 | :param rate: Scale rate from 0 to 1. 473 | :return: 474 | """ 475 | 476 | r = int(hex_color[1:3], 16) 477 | g = int(hex_color[3:5], 16) 478 | b = int(hex_color[5:7], 16) 479 | 480 | # Half the colors. 481 | r = min(255, max(0, int(r * rate))) 482 | g = min(255, max(0, int(g * rate))) 483 | b = min(255, max(0, int(b * rate))) 484 | 485 | r = hex(r)[2:] 486 | g = hex(g)[2:] 487 | b = hex(b)[2:] 488 | 489 | r = r.rjust(2, "0") 490 | g = g.rjust(2, "0") 491 | b = b.rjust(2, "0") 492 | 493 | new_hex_color = '#{}{}{}'.format(r, g, b) 494 | return new_hex_color 495 | 496 | 497 | def plot_planes_of_roads(total_areas, save_folder): 498 | """ 499 | Plot the roads. 500 | :param total_areas: 501 | :param save_folder: 502 | :return: 503 | """ 504 | 505 | import matplotlib.pyplot as plt 506 | plt.cla() 507 | 508 | plt.figure(figsize=(160, 90)) 509 | area_select = 10 # select one from 10 boundary points for accelerating. 510 | 511 | all_types = set() 512 | 513 | # Plot lane area. 514 | for k, v in tqdm(total_areas.items(), desc="Ploting Roads"): 515 | left_lanes_area = v["left_lanes_area"] 516 | right_lanes_area = v["right_lanes_area"] 517 | 518 | types = v["types"] 519 | 520 | for left_lane_id, left_lane_area in left_lanes_area.items(): 521 | type_of_lane = types[left_lane_id] 522 | all_types.add(type_of_lane) 523 | lane_color = TYPE_COLOR_DICT[type_of_lane] 524 | inner_points = left_lane_area["inner"] 525 | outer_points = left_lane_area["outer"] 526 | 527 | points_of_one_road = inner_points + outer_points[::-1] 528 | xs = [i for i, _ in points_of_one_road] 529 | ys = [i for _, i in points_of_one_road] 530 | plt.fill(xs, ys, color=lane_color, label=type_of_lane) 531 | plt.scatter(xs[::area_select], ys[::area_select], color=rescale_color(lane_color, 0.5), s=1) 532 | 533 | for right_lane_id, right_lane_area in right_lanes_area.items(): 534 | type_of_lane = types[right_lane_id] 535 | all_types.add(type_of_lane) 536 | 537 | lane_color = TYPE_COLOR_DICT[type_of_lane] 538 | inner_points = right_lane_area["inner"] 539 | outer_points = right_lane_area["outer"] 540 | 541 | points_of_one_road = inner_points + outer_points[::-1] 542 | xs = [i for i, _ in points_of_one_road] 543 | ys = [i for _, i in points_of_one_road] 544 | plt.fill(xs, ys, color=lane_color, label=type_of_lane) 545 | plt.scatter(xs[::area_select], ys[::area_select], color=rescale_color(lane_color, 0.5), s=1) 546 | 547 | # Plot boundaries 548 | for k, v in tqdm(total_areas.items(), desc="Ploting Edges"): 549 | left_lanes_area = v["left_lanes_area"] 550 | right_lanes_area = v["right_lanes_area"] 551 | 552 | types = v["types"] 553 | for left_lane_id, left_lane_area in left_lanes_area.items(): 554 | type_of_lane = types[left_lane_id] 555 | all_types.add(type_of_lane) 556 | lane_color = TYPE_COLOR_DICT[type_of_lane] 557 | inner_points = left_lane_area["inner"] 558 | outer_points = left_lane_area["outer"] 559 | points_of_one_road = inner_points + outer_points[::-1] 560 | xs = [i for i, _ in points_of_one_road] 561 | ys = [i for _, i in points_of_one_road] 562 | plt.scatter(xs[::area_select], ys[::area_select], color=rescale_color(lane_color, 0.5), s=1) 563 | 564 | for right_lane_id, right_lane_area in right_lanes_area.items(): 565 | type_of_lane = types[right_lane_id] 566 | all_types.add(type_of_lane) 567 | lane_color = TYPE_COLOR_DICT[type_of_lane] 568 | inner_points = right_lane_area["inner"] 569 | outer_points = right_lane_area["outer"] 570 | points_of_one_road = inner_points + outer_points[::-1] 571 | xs = [i for i, _ in points_of_one_road] 572 | ys = [i for _, i in points_of_one_road] 573 | plt.scatter(xs[::area_select], ys[::area_select], color=rescale_color(lane_color, 0.5), s=1) 574 | 575 | # Plot center lane and reference line. 576 | saved_ceter_lanes = dict() 577 | for k, v in tqdm(total_areas.items(), desc="Ploting Reference and center"): 578 | 579 | reference_points = v["reference_points"] 580 | if not reference_points: 581 | continue 582 | position_reference_points = reference_points["position"] 583 | position_center_lane = reference_points["position_center_lane"] 584 | 585 | position_reference_points_xs = [x for x, y in position_reference_points] 586 | position_reference_points_ys = [y for x, y in position_reference_points] 587 | position_center_lane_xs = [x for x, y in position_center_lane] 588 | position_center_lane_ys = [y for x, y in position_center_lane] 589 | 590 | saved_ceter_lanes[k] = position_center_lane 591 | plt.scatter(position_reference_points_xs, position_reference_points_ys, color=COLOR_REFERECE_LINE, s=3) 592 | plt.scatter(position_center_lane_xs, position_center_lane_ys, color=COLOR_CENTER_LANE, s=2) 593 | 594 | import matplotlib.pyplot as plt 595 | from matplotlib.patches import Patch 596 | 597 | # Create legend. 598 | legend_dict = { 599 | # k: Patch(facecolor=v, edgecolor=v, alpha=0.3) for k, v in type_color_dict.items() if k in all_types 600 | k: Patch(facecolor=v, edgecolor=v, alpha=1.0) for k, v in TYPE_COLOR_DICT.items() if k in all_types 601 | } 602 | legend_dict.update({ 603 | "center_lane": Patch(facecolor=COLOR_CENTER_LANE, edgecolor=COLOR_CENTER_LANE, alpha=1.0), 604 | }) 605 | legend_dict.update({ 606 | "reference_line": Patch(facecolor=COLOR_REFERECE_LINE, edgecolor=COLOR_REFERECE_LINE, alpha=1.0), 607 | }) 608 | 609 | plt.legend(handles=legend_dict.values(), labels=legend_dict.keys(), fontsize=50) 610 | plt.xlabel("x") 611 | plt.ylabel("y") 612 | plt.axis("equal") 613 | 614 | os.makedirs(save_folder, exist_ok=True) 615 | save_pdf_file = os.path.join(save_folder, "lanes.pdf") 616 | plt.savefig(save_pdf_file) 617 | 618 | 619 | def process_one_file(file, step=0.1): 620 | """ 621 | Load one .xodr file and calculate the railing positions with other important messages. 622 | :param file: Input file. 623 | :param step: Step of calculation. 624 | :return: None 625 | """ 626 | 627 | assert os.path.exists(file), FileNotFoundError(file) 628 | d, ne = os.path.split(file) 629 | n, e = os.path.splitext(ne) 630 | save_folder = os.path.join(d, n) 631 | 632 | road_network = load_xodr_and_parse(file) 633 | total_areas = get_all_lanes(road_network, step=step) 634 | 635 | plot_planes_of_roads(total_areas, save_folder) 636 | 637 | 638 | def main(): 639 | process_one_file(file=XODR_FILE) 640 | 641 | 642 | if __name__ == "__main__": 643 | main() 644 | --------------------------------------------------------------------------------