From 3ac57be9bf23dfb05ed816aa2f59578698ee4053 Mon Sep 17 00:00:00 2001 From: Yudai Yamada Date: Sun, 11 May 2025 23:28:08 -0500 Subject: [PATCH] Merge branch 'summoning_integration' into summoning_pr --- .../knowledge/defaults/ReedsShepp_param.yaml | 19 + .../knowledge/defaults/computation_graph.yaml | 10 +- GEMstack/knowledge/defaults/rrt_param.yaml | 19 + .../routes/summoning_roadgraph_highbay.json | 1 + .../routes/summoning_roadgraph_sim.json | 1 + GEMstack/onboard/planning/RRT.py | 320 +++++++ GEMstack/onboard/planning/mission_planning.py | 176 ++++ .../onboard/planning/reeds_shepp_parking.py | 828 ++++++++++++++++++ GEMstack/onboard/planning/route_planning.py | 342 +++++++- GEMstack/onboard/planning/rrt_star.py | 799 +++++++++++++++++ GEMstack/state/mission.py | 6 + GEMstack/state/route.py | 13 + launch/summoning.yaml | 88 ++ scenes/summoning_demo.yaml | 7 + testing/roadgraph_generator.py | 454 ++++++++++ testing/roadgraph_lane_points.txt | 748 ++++++++++++++++ testing/roadgraph_lane_reader.py | 70 ++ 17 files changed, 3897 insertions(+), 4 deletions(-) create mode 100644 GEMstack/knowledge/defaults/ReedsShepp_param.yaml create mode 100644 GEMstack/knowledge/defaults/rrt_param.yaml create mode 100644 GEMstack/knowledge/routes/summoning_roadgraph_highbay.json create mode 100644 GEMstack/knowledge/routes/summoning_roadgraph_sim.json create mode 100644 GEMstack/onboard/planning/RRT.py create mode 100644 GEMstack/onboard/planning/mission_planning.py create mode 100644 GEMstack/onboard/planning/reeds_shepp_parking.py create mode 100644 GEMstack/onboard/planning/rrt_star.py create mode 100644 launch/summoning.yaml create mode 100644 scenes/summoning_demo.yaml create mode 100644 testing/roadgraph_generator.py create mode 100644 testing/roadgraph_lane_points.txt create mode 100644 testing/roadgraph_lane_reader.py diff --git a/GEMstack/knowledge/defaults/ReedsShepp_param.yaml b/GEMstack/knowledge/defaults/ReedsShepp_param.yaml new file mode 100644 index 000000000..d2d52103e --- /dev/null +++ b/GEMstack/knowledge/defaults/ReedsShepp_param.yaml @@ -0,0 +1,19 @@ +# vehicle info +vehicle: + vehicle_dim: [1.7, 3.2] # in meter + vehicle_turning_radius: 3.657 +# algorithm parameters +reeds_shepp_parking: + shift_from_center_to_rear_axis: 1.25 # in meter + search_step_size: 0.1 # in meter + closest: False # If True, the closest parking spot will be selected, otherwise the farthest one will be selected + parking_lot_axis_shift_margin: 2.44 # in meter + search_bound_threshold: 0.5 + clearance_step: 0.5 + clearance: 0 + add_static_vertical_curb_as_obstacle: True + add_static_horizontal_curb_as_obstacle: True + static_horizontal_curb_size: [2.44, 0.5] + static_vertical_curb_size: [2.44, 24.9] + compact_parking_spot_size: [2.44, 4.88] # US Compact Space for parking (2.44, 4.88) + diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index d7a606326..b894a36b7 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -40,11 +40,14 @@ components: inputs: all - mission_execution: outputs: mission + - mission_planning: + inputs: all + outputs: mission - route_planning: - inputs: [vehicle, roadgraph, mission] + inputs: all outputs: route - driving_logic: - inputs: all + inputs: outputs: intent - motion_planning: inputs: all @@ -52,3 +55,6 @@ components: - trajectory_tracking: inputs: [vehicle, trajectory] outputs: + - signaling: + inputs: [intent] + outputs: diff --git a/GEMstack/knowledge/defaults/rrt_param.yaml b/GEMstack/knowledge/defaults/rrt_param.yaml new file mode 100644 index 000000000..3e7019384 --- /dev/null +++ b/GEMstack/knowledge/defaults/rrt_param.yaml @@ -0,0 +1,19 @@ +# arguments for BiRRT* algorithm +# map parameters +map: + lower_x: -15 + upper_x: 40 + lower_y: -10 + upper_y: 30 + grid_resolution: 10 # grids per meter for occupency grid + lane_boundary_radius: 0.2 # radius in meter for determining the size of the lane boundary in occupency grid + obstacle_radius: 0.2 # radius in meter for determining the size of the obstacle in occupency grid +# vehicle info +vehicle: + heading_limit: 0.5235987756 # vehicle heading difference limit per rrt step_size (pi/6 in radians) + half_width: 0.8 # vehicle half width in meter +# algorithm parameters +rrt: + time_limit: 10 # in sec + step_size: 0.5 # length in meter for local planner to explore + search_r: 1.4 # radius in meter for rewiring \ No newline at end of file diff --git a/GEMstack/knowledge/routes/summoning_roadgraph_highbay.json b/GEMstack/knowledge/routes/summoning_roadgraph_highbay.json new file mode 100644 index 000000000..d64969937 --- /dev/null +++ b/GEMstack/knowledge/routes/summoning_roadgraph_highbay.json @@ -0,0 +1 @@ +{"type": "Roadgraph", "data": {"frame": 2, "curves": {}, "lanes": {"eastward": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235842, 40.09275637879597, 0.0], [-88.23583851515151, 40.09275644167476, 0.0], [-88.23583503030304, 40.09275650455354, 0.0], [-88.23583154545454, 40.09275656743233, 0.0], [-88.23582806060607, 40.09275663031112, 0.0], [-88.23582457575758, 40.092756693189905, 0.0], [-88.2358210909091, 40.092756756068695, 0.0], [-88.2358176060606, 40.092756818947485, 0.0], [-88.23581412121213, 40.09275688182627, 0.0], [-88.23581063636364, 40.09275694470506, 0.0], [-88.23580715151516, 40.09275700758385, 0.0], [-88.23580366666667, 40.09275707046263, 0.0], [-88.23580018181819, 40.09275713334142, 0.0], [-88.2357966969697, 40.09275719622021, 0.0], [-88.23579321212121, 40.092757259098995, 0.0], [-88.23578972727273, 40.092757321977786, 0.0], [-88.23578624242424, 40.092757384856576, 0.0], [-88.23578275757576, 40.09275744773536, 0.0], [-88.23577927272727, 40.09275751061415, 0.0], [-88.2357757878788, 40.09275757349294, 0.0], [-88.2357723030303, 40.09275763637172, 0.0], [-88.23576881818182, 40.09275769925051, 0.0], [-88.23576533333333, 40.0927577621293, 0.0], [-88.23576184848486, 40.09275782500809, 0.0], [-88.23575836363636, 40.092757887886876, 0.0], [-88.23575487878789, 40.092757950765666, 0.0], [-88.2357513939394, 40.092758013644456, 0.0], [-88.2357479090909, 40.09275807652324, 0.0], [-88.23574442424243, 40.09275813940203, 0.0], [-88.23574093939393, 40.09275820228082, 0.0], [-88.23573745454546, 40.0927582651596, 0.0], [-88.23573396969697, 40.09275832803839, 0.0], [-88.23573048484849, 40.09275839091718, 0.0], [-88.235727, 40.092758453795966, 0.0], [-88.23572351515152, 40.092758516674756, 0.0], [-88.23572003030303, 40.092758579553546, 0.0], [-88.23571654545455, 40.09275864243233, 0.0], [-88.23571306060606, 40.09275870531112, 0.0], [-88.23570957575758, 40.09275876818991, 0.0], [-88.23570609090909, 40.09275883106869, 0.0], [-88.2357026060606, 40.09275889394748, 0.0], [-88.23569912121212, 40.09275895682627, 0.0], [-88.23569563636363, 40.092759019705056, 0.0], [-88.23569215151515, 40.09275908258385, 0.0], [-88.23568866666666, 40.09275914546264, 0.0], [-88.23568518181818, 40.09275920834142, 0.0], [-88.23568169696969, 40.09275927122021, 0.0], [-88.23567821212121, 40.092759334099, 0.0], [-88.23567472727272, 40.09275939697778, 0.0], [-88.23567124242425, 40.09275945985657, 0.0], [-88.23566775757575, 40.09275952273536, 0.0], [-88.23566427272728, 40.09275958561415, 0.0], [-88.23566078787879, 40.09275964849294, 0.0], [-88.2356573030303, 40.09275971137173, 0.0], [-88.23565381818182, 40.09275977425051, 0.0], [-88.23565033333333, 40.0927598371293, 0.0], [-88.23564684848485, 40.09275990000809, 0.0], [-88.23564336363636, 40.09275996288687, 0.0], [-88.23563987878788, 40.092760025765664, 0.0], [-88.23563639393939, 40.092760088644454, 0.0], [-88.23563290909091, 40.09276015152324, 0.0], [-88.23562942424242, 40.09276021440203, 0.0], [-88.23562593939394, 40.09276027728082, 0.0], [-88.23562245454545, 40.0927603401596, 0.0], [-88.23561896969697, 40.09276040303839, 0.0], [-88.23561548484848, 40.09276046591718, 0.0], [-88.235612, 40.092760528795964, 0.0], [-88.23560851515151, 40.092760591674754, 0.0], [-88.23560503030302, 40.092760654553544, 0.0], [-88.23560154545454, 40.092760717432334, 0.0], [-88.23559806060605, 40.09276078031112, 0.0], [-88.23559457575757, 40.09276084318991, 0.0], [-88.23559109090908, 40.0927609060687, 0.0], [-88.2355876060606, 40.09276096894748, 0.0], [-88.23558412121211, 40.09276103182627, 0.0], [-88.23558063636364, 40.09276109470506, 0.0], [-88.23557715151514, 40.092761157583844, 0.0], [-88.23557366666667, 40.092761220462634, 0.0], [-88.23557018181818, 40.092761283341424, 0.0], [-88.2355666969697, 40.09276134622021, 0.0], [-88.2355632121212, 40.092761409099, 0.0], [-88.23555972727272, 40.09276147197779, 0.0], [-88.23555624242424, 40.09276153485657, 0.0], [-88.23555275757575, 40.09276159773536, 0.0], [-88.23554927272727, 40.09276166061415, 0.0], [-88.23554578787878, 40.092761723492934, 0.0], [-88.2355423030303, 40.092761786371724, 0.0], [-88.23553881818181, 40.092761849250515, 0.0], [-88.23553533333333, 40.0927619121293, 0.0], [-88.23553184848484, 40.09276197500809, 0.0], [-88.23552836363636, 40.09276203788688, 0.0], [-88.23552487878787, 40.09276210076566, 0.0], [-88.2355213939394, 40.09276216364445, 0.0], [-88.2355179090909, 40.09276222652324, 0.0], [-88.23551442424241, 40.092762289402025, 0.0], [-88.23551093939393, 40.092762352280815, 0.0], [-88.23550745454544, 40.092762415159605, 0.0], [-88.23550396969696, 40.09276247803839, 0.0], [-88.23550048484847, 40.09276254091718, 0.0], [-88.235497, 40.09276260379597, 0.0], [-88.2354935151515, 40.09276266667475, 0.0], [-88.23549003030303, 40.09276272955354, 0.0], [-88.23548654545453, 40.09276279243233, 0.0], [-88.23548306060606, 40.092762855311115, 0.0], [-88.23547957575757, 40.092762918189905, 0.0], [-88.23547609090909, 40.092762981068695, 0.0], [-88.2354726060606, 40.09276304394748, 0.0], [-88.2354691212121, 40.09276310682627, 0.0], [-88.23546563636363, 40.09276316970506, 0.0], [-88.23546215151514, 40.09276323258384, 0.0], [-88.23545866666666, 40.09276329546263, 0.0], [-88.23545518181817, 40.09276335834142, 0.0], [-88.23545169696969, 40.09276342122021, 0.0], [-88.2354482121212, 40.092763484098995, 0.0], [-88.23544472727272, 40.092763546977785, 0.0], [-88.23544124242423, 40.092763609856576, 0.0], [-88.23543775757575, 40.09276367273536, 0.0], [-88.23543427272726, 40.09276373561415, 0.0], [-88.23543078787878, 40.09276379849294, 0.0], [-88.23542730303029, 40.09276386137172, 0.0], [-88.2354238181818, 40.09276392425051, 0.0], [-88.23542033333332, 40.0927639871293, 0.0], [-88.23541684848483, 40.092764050008086, 0.0], [-88.23541336363635, 40.092764112886876, 0.0], [-88.23540987878786, 40.092764175765666, 0.0], [-88.23540639393939, 40.09276423864445, 0.0], [-88.2354029090909, 40.09276430152324, 0.0], [-88.23539942424242, 40.09276436440203, 0.0], [-88.23539593939392, 40.09276442728081, 0.0], [-88.23539245454545, 40.0927644901596, 0.0], [-88.23538896969696, 40.09276455303839, 0.0], [-88.23538548484848, 40.092764615917176, 0.0], [-88.23538199999999, 40.092764678795966, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235968, 40.092730121204035, 0.0], [-88.23596452427185, 40.09273016732054, 0.0], [-88.23596104854369, 40.092730213437044, 0.0], [-88.23595757281554, 40.09273025955355, 0.0], [-88.23595409708737, 40.09273030567005, 0.0], [-88.23595062135922, 40.09273035178656, 0.0], [-88.23594714563107, 40.09273039790306, 0.0], [-88.23594366990291, 40.092730444019566, 0.0], [-88.23594019417476, 40.09273049013608, 0.0], [-88.2359367184466, 40.09273053625258, 0.0], [-88.23593324271845, 40.092730582369086, 0.0], [-88.2359297669903, 40.09273062848559, 0.0], [-88.23592629126213, 40.092730674602095, 0.0], [-88.23592281553398, 40.0927307207186, 0.0], [-88.23591933980582, 40.0927307668351, 0.0], [-88.23591586407767, 40.09273081295161, 0.0], [-88.23591238834952, 40.09273085906811, 0.0], [-88.23590891262135, 40.09273090518462, 0.0], [-88.2359054368932, 40.09273095130112, 0.0], [-88.23590196116506, 40.092730997417625, 0.0], [-88.23589848543689, 40.09273104353413, 0.0], [-88.23589500970874, 40.092731089650634, 0.0], [-88.23589153398058, 40.09273113576714, 0.0], [-88.23588805825243, 40.09273118188364, 0.0], [-88.23588458252428, 40.092731228000154, 0.0], [-88.23588110679611, 40.09273127411666, 0.0], [-88.23587763106796, 40.09273132023316, 0.0], [-88.2358741553398, 40.09273136634967, 0.0], [-88.23587067961165, 40.09273141246617, 0.0], [-88.2358672038835, 40.092731458582676, 0.0], [-88.23586372815534, 40.09273150469918, 0.0], [-88.23586025242719, 40.092731550815685, 0.0], [-88.23585677669902, 40.09273159693219, 0.0], [-88.23585330097087, 40.092731643048694, 0.0], [-88.23584982524272, 40.0927316891652, 0.0], [-88.23584634951456, 40.0927317352817, 0.0], [-88.23584287378641, 40.09273178139821, 0.0], [-88.23583939805825, 40.09273182751471, 0.0], [-88.2358359223301, 40.092731873631216, 0.0], [-88.23583244660195, 40.09273191974772, 0.0], [-88.23582897087378, 40.09273196586423, 0.0], [-88.23582549514563, 40.092732011980736, 0.0], [-88.23582201941747, 40.09273205809724, 0.0], [-88.23581854368932, 40.092732104213745, 0.0], [-88.23581506796117, 40.09273215033025, 0.0], [-88.235811592233, 40.092732196446754, 0.0], [-88.23580811650486, 40.09273224256326, 0.0], [-88.2358046407767, 40.09273228867976, 0.0], [-88.23580116504854, 40.09273233479627, 0.0], [-88.23579768932039, 40.09273238091277, 0.0], [-88.23579421359223, 40.092732427029276, 0.0], [-88.23579073786408, 40.09273247314578, 0.0], [-88.23578726213593, 40.092732519262285, 0.0], [-88.23578378640777, 40.09273256537879, 0.0], [-88.23578031067962, 40.09273261149529, 0.0], [-88.23577683495145, 40.0927326576118, 0.0], [-88.2357733592233, 40.09273270372831, 0.0], [-88.23576988349515, 40.092732749844814, 0.0], [-88.23576640776699, 40.09273279596132, 0.0], [-88.23576293203884, 40.09273284207782, 0.0], [-88.23575945631067, 40.09273288819433, 0.0], [-88.23575598058252, 40.09273293431083, 0.0], [-88.23575250485437, 40.092732980427336, 0.0], [-88.23574902912621, 40.09273302654384, 0.0], [-88.23574555339806, 40.092733072660344, 0.0], [-88.2357420776699, 40.09273311877685, 0.0], [-88.23573860194175, 40.09273316489335, 0.0], [-88.2357351262136, 40.09273321100986, 0.0], [-88.23573165048543, 40.09273325712636, 0.0], [-88.23572817475728, 40.092733303242866, 0.0], [-88.23572469902912, 40.09273334935937, 0.0], [-88.23572122330097, 40.092733395475875, 0.0], [-88.23571774757282, 40.09273344159239, 0.0], [-88.23571427184466, 40.09273348770889, 0.0], [-88.2357107961165, 40.092733533825395, 0.0], [-88.23570732038836, 40.0927335799419, 0.0], [-88.2357038446602, 40.092733626058404, 0.0], [-88.23570036893204, 40.09273367217491, 0.0], [-88.23569689320388, 40.09273371829141, 0.0], [-88.23569341747573, 40.09273376440792, 0.0], [-88.23568994174758, 40.09273381052442, 0.0], [-88.23568646601942, 40.092733856640926, 0.0], [-88.23568299029127, 40.09273390275743, 0.0], [-88.2356795145631, 40.092733948873935, 0.0], [-88.23567603883495, 40.09273399499044, 0.0], [-88.2356725631068, 40.092734041106944, 0.0], [-88.23566908737864, 40.09273408722345, 0.0], [-88.23566561165049, 40.09273413333995, 0.0], [-88.23566213592233, 40.092734179456464, 0.0], [-88.23565866019418, 40.09273422557297, 0.0], [-88.23565518446603, 40.09273427168947, 0.0], [-88.23565170873786, 40.09273431780598, 0.0], [-88.23564823300971, 40.09273436392248, 0.0], [-88.23564475728155, 40.092734410038986, 0.0], [-88.2356412815534, 40.09273445615549, 0.0], [-88.23563780582525, 40.092734502271995, 0.0], [-88.23563433009708, 40.0927345483885, 0.0], [-88.23563085436894, 40.092734594505, 0.0], [-88.23562737864077, 40.09273464062151, 0.0], [-88.23562390291262, 40.09273468673801, 0.0], [-88.23562042718447, 40.09273473285452, 0.0], [-88.23561695145631, 40.09273477897102, 0.0], [-88.23561347572816, 40.092734825087526, 0.0], [-88.23561000000001, 40.09273487120403, 0.0], [-88.23560652427184, 40.09273491732054, 0.0], [-88.2356030485437, 40.092734963437046, 0.0], [-88.23559957281553, 40.09273500955355, 0.0], [-88.23559609708738, 40.092735055670055, 0.0], [-88.23559262135923, 40.09273510178656, 0.0], [-88.23558914563107, 40.09273514790306, 0.0], [-88.23558566990292, 40.09273519401957, 0.0], [-88.23558219417475, 40.09273524013607, 0.0], [-88.2355787184466, 40.09273528625258, 0.0], [-88.23557524271845, 40.09273533236908, 0.0], [-88.23557176699029, 40.092735378485585, 0.0], [-88.23556829126214, 40.09273542460209, 0.0], [-88.23556481553398, 40.092735470718594, 0.0], [-88.23556133980583, 40.0927355168351, 0.0], [-88.23555786407768, 40.0927355629516, 0.0], [-88.23555438834951, 40.092735609068114, 0.0], [-88.23555091262136, 40.09273565518462, 0.0], [-88.2355474368932, 40.09273570130112, 0.0], [-88.23554396116505, 40.09273574741763, 0.0], [-88.2355404854369, 40.09273579353413, 0.0], [-88.23553700970874, 40.092735839650636, 0.0], [-88.23553353398059, 40.09273588576714, 0.0], [-88.23553005825242, 40.092735931883645, 0.0], [-88.23552658252427, 40.09273597800015, 0.0], [-88.23552310679612, 40.092736024116654, 0.0], [-88.23551963106796, 40.09273607023316, 0.0], [-88.23551615533981, 40.09273611634966, 0.0], [-88.23551267961165, 40.09273616246617, 0.0], [-88.2355092038835, 40.09273620858267, 0.0], [-88.23550572815535, 40.092736254699176, 0.0], [-88.23550225242718, 40.09273630081568, 0.0], [-88.23549877669903, 40.09273634693219, 0.0], [-88.23549530097088, 40.092736393048696, 0.0], [-88.23549182524272, 40.0927364391652, 0.0], [-88.23548834951457, 40.092736485281705, 0.0], [-88.2354848737864, 40.09273653139821, 0.0], [-88.23548139805825, 40.092736577514714, 0.0], [-88.2354779223301, 40.09273662363122, 0.0], [-88.23547444660194, 40.09273666974772, 0.0], [-88.23547097087379, 40.09273671586423, 0.0], [-88.23546749514563, 40.09273676198073, 0.0], [-88.23546401941748, 40.092736808097236, 0.0], [-88.23546054368933, 40.09273685421374, 0.0], [-88.23545706796116, 40.092736900330245, 0.0], [-88.23545359223301, 40.09273694644675, 0.0], [-88.23545011650485, 40.09273699256325, 0.0], [-88.2354466407767, 40.09273703867976, 0.0], [-88.23544316504855, 40.09273708479627, 0.0], [-88.23543968932039, 40.092737130912774, 0.0], [-88.23543621359224, 40.09273717702928, 0.0], [-88.23543273786407, 40.09273722314578, 0.0], [-88.23542926213592, 40.09273726926229, 0.0], [-88.23542578640777, 40.09273731537879, 0.0], [-88.23542231067961, 40.092737361495296, 0.0], [-88.23541883495146, 40.0927374076118, 0.0], [-88.2354153592233, 40.092737453728304, 0.0], [-88.23541188349515, 40.09273749984481, 0.0], [-88.235408407767, 40.09273754596131, 0.0], [-88.23540493203883, 40.09273759207782, 0.0], [-88.23540145631068, 40.09273763819432, 0.0], [-88.23539798058253, 40.092737684310826, 0.0], [-88.23539450485437, 40.09273773042733, 0.0], [-88.23539102912622, 40.092737776543835, 0.0], [-88.23538755339806, 40.09273782266035, 0.0], [-88.2353840776699, 40.09273786877685, 0.0], [-88.23538060194176, 40.092737914893355, 0.0], [-88.23537712621359, 40.09273796100986, 0.0], [-88.23537365048544, 40.092738007126364, 0.0], [-88.23537017475728, 40.09273805324287, 0.0], [-88.23536669902913, 40.09273809935937, 0.0], [-88.23536322330098, 40.09273814547588, 0.0], [-88.23535974757282, 40.09273819159238, 0.0], [-88.23535627184467, 40.092738237708886, 0.0], [-88.2353527961165, 40.09273828382539, 0.0], [-88.23534932038835, 40.092738329941895, 0.0], [-88.2353458446602, 40.0927383760584, 0.0], [-88.23534236893204, 40.092738422174904, 0.0], [-88.23533889320389, 40.09273846829141, 0.0], [-88.23533541747572, 40.09273851440791, 0.0], [-88.23533194174757, 40.092738560524424, 0.0], [-88.23532846601942, 40.09273860664093, 0.0], [-88.23532499029126, 40.09273865275743, 0.0], [-88.23532151456311, 40.09273869887394, 0.0], [-88.23531803883495, 40.09273874499044, 0.0], [-88.2353145631068, 40.092738791106946, 0.0], [-88.23531108737865, 40.09273883722345, 0.0], [-88.23530761165048, 40.092738883339955, 0.0], [-88.23530413592233, 40.09273892945646, 0.0], [-88.23530066019418, 40.09273897557296, 0.0], [-88.23529718446602, 40.09273902168947, 0.0], [-88.23529370873787, 40.09273906780597, 0.0], [-88.2352902330097, 40.09273911392248, 0.0], [-88.23528675728156, 40.09273916003898, 0.0], [-88.23528328155341, 40.092739206155485, 0.0], [-88.23527980582524, 40.09273925227199, 0.0], [-88.2352763300971, 40.0927392983885, 0.0], [-88.23527285436893, 40.092739344505006, 0.0], [-88.23526937864078, 40.09273939062151, 0.0], [-88.23526590291263, 40.092739436738015, 0.0], [-88.23526242718447, 40.09273948285452, 0.0], [-88.23525895145632, 40.09273952897102, 0.0], [-88.23525547572815, 40.09273957508753, 0.0], [-88.235252, 40.09273962120403, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "east_cycle_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235252, 40.092765778795965, 0.0], [-88.23524858166245, 40.092765928066214, 0.0], [-88.2352451893423, 40.092766374694925, 0.0], [-88.23524184885692, 40.09276711528302, 0.0], [-88.23523858562922, 40.0927681441942, 0.0], [-88.2352354244941, 40.09276945359793, 0.0], [-88.23523238950955, 40.09277103352892, 0.0], [-88.23522950377338, 40.09277287196306, 0.0], [-88.23522678924763, 40.092774954908855, 0.0], [-88.23522426659129, 40.092777266513984, 0.0], [-88.23522195500314, 40.092779789185876, 0.0], [-88.23521987207562, 40.092782503725644, 0.0], [-88.2352180336609, 40.09278538947418, 0.0], [-88.23521645375034, 40.092788424469376, 0.0], [-88.23521514436788, 40.09279158561329, 0.0], [-88.23521411547866, 40.092794848847916, 0.0], [-88.23521337491304, 40.092798189338275, 0.0], [-88.23521292830716, 40.09280158166144, 0.0], [-88.23521277905992, 40.092805, 0.0], [-88.23521292830716, 40.09280841833856, 0.0], [-88.23521337491304, 40.09281181066172, 0.0], [-88.23521411547866, 40.09281515115208, 0.0], [-88.23521514436788, 40.092818414386706, 0.0], [-88.23521645375034, 40.09282157553062, 0.0], [-88.2352180336609, 40.09282461052582, 0.0], [-88.23521987207562, 40.09282749627435, 0.0], [-88.23522195500314, 40.09283021081412, 0.0], [-88.23522426659129, 40.09283273348601, 0.0], [-88.23522678924763, 40.09283504509114, 0.0], [-88.23522950377338, 40.09283712803694, 0.0], [-88.23523238950955, 40.09283896647108, 0.0], [-88.2352354244941, 40.092840546402066, 0.0], [-88.23523858562922, 40.092841855805794, 0.0], [-88.23524184885692, 40.09284288471698, 0.0], [-88.2352451893423, 40.09284362530507, 0.0], [-88.23524858166245, 40.09284407193378, 0.0], [-88.235252, 40.09284422120403, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235252, 40.09273962120403, 0.0], [-88.23524852041031, 40.092739713883205, 0.0], [-88.2352450506848, 40.09273999162174, 0.0], [-88.23524160065871, 40.092740453632366, 0.0], [-88.23523818011145, 40.09274109860547, 0.0], [-88.23523479873887, 40.09274192471282, 0.0], [-88.23523146612578, 40.09274292961273, 0.0], [-88.23522819171876, 40.092744110456735, 0.0], [-88.23522498479943, 40.092745463897614, 0.0], [-88.23522185445808, 40.09274698609893, 0.0], [-88.23521880956794, 40.092748672745856, 0.0], [-88.23521585876001, 40.092750519057454, 0.0], [-88.23521301039867, 40.092752519800165, 0.0], [-88.2352102725578, 40.09275466930273, 0.0], [-88.23520765299808, 40.09275696147217, 0.0], [-88.2352051591449, 40.09275938981113, 0.0], [-88.23520279806728, 40.09276194743627, 0.0], [-88.23520057645794, 40.09276462709777, 0.0], [-88.2351985006142, 40.092767421199895, 0.0], [-88.23519657642026, 40.0927703218225, 0.0], [-88.2351948093304, 40.09277332074351, 0.0], [-88.23519320435358, 40.092776409462225, 0.0], [-88.23519176603928, 40.09277957922339, 0.0], [-88.23519049846452, 40.09278282104202, 0.0], [-88.23518940522236, 40.09278612572891, 0.0], [-88.23518848941167, 40.09278948391661, 0.0], [-88.23518775362842, 40.092792886086045, 0.0], [-88.23518719995825, 40.09279632259346, 0.0], [-88.23518682997059, 40.09279978369776, 0.0], [-88.23518664471419, 40.09280325958813, 0.0], [-88.23518664471419, 40.092806740411866, 0.0], [-88.23518682997059, 40.09281021630224, 0.0], [-88.23518719995825, 40.09281367740654, 0.0], [-88.23518775362842, 40.09281711391395, 0.0], [-88.23518848941167, 40.092820516083385, 0.0], [-88.23518940522236, 40.09282387427109, 0.0], [-88.23519049846452, 40.092827178957975, 0.0], [-88.23519176603928, 40.09283042077661, 0.0], [-88.23519320435358, 40.09283359053777, 0.0], [-88.2351948093304, 40.092836679256486, 0.0], [-88.23519657642026, 40.0928396781775, 0.0], [-88.2351985006142, 40.0928425788001, 0.0], [-88.23520057645794, 40.092845372902225, 0.0], [-88.23520279806728, 40.092848052563724, 0.0], [-88.2352051591449, 40.09285061018887, 0.0], [-88.23520765299808, 40.09285303852783, 0.0], [-88.2352102725578, 40.09285533069727, 0.0], [-88.23521301039867, 40.09285748019983, 0.0], [-88.23521585876001, 40.09285948094254, 0.0], [-88.23521880956794, 40.09286132725414, 0.0], [-88.23522185445808, 40.092863013901066, 0.0], [-88.23522498479943, 40.09286453610238, 0.0], [-88.23522819171876, 40.09286588954326, 0.0], [-88.23523146612578, 40.092867070387264, 0.0], [-88.23523479873887, 40.09286807528718, 0.0], [-88.23523818011145, 40.092868901394525, 0.0], [-88.23524160065871, 40.09286954636763, 0.0], [-88.2352450506848, 40.092870008378256, 0.0], [-88.23524852041031, 40.09287028611679, 0.0], [-88.235252, 40.092870378795965, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "east_cycle_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235252, 40.09284422120403, 0.0], [-88.2352554356672, 40.09284456435089, 0.0], [-88.23525888820048, 40.09284460400906, 0.0], [-88.23526233084336, 40.092844339871206, 0.0], [-88.23526573691599, 40.092843773984335, 0.0], [-88.23526908002198, 40.09284291073398, 0.0], [-88.23527233425285, 40.09284175681016, 0.0], [-88.23527547438896, 40.09284032115556, 0.0], [-88.2352784760948, 40.09283861489625, 0.0], [-88.23528131610774, 40.09283665125542, 0.0], [-88.2352839724182, 40.092834445450904, 0.0], [-88.23528642444028, 40.09283201457728, 0.0], [-88.23528865317125, 40.09282937747337, 0.0], [-88.23529064133888, 40.092826554576234, 0.0], [-88.23529237353524, 40.092823567762785, 0.0], [-88.23529383633614, 40.092820440180276, 0.0], [-88.23529501840514, 40.09281719606688, 0.0], [-88.23529591058141, 40.09281386056388, 0.0], [-88.23529650595079, 40.092810459520784, 0.0], [-88.23529679989923, 40.09280701929505, 0.0], [-88.23529679014872, 40.09280356654777, 0.0], [-88.23529647677482, 40.092800128037105, 0.0], [-88.23529586220612, 40.09279673041086, 0.0], [-88.23529495120539, 40.092793400000005, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235252, 40.092870378795965, 0.0], [-88.23525538146953, 40.09287088965463, 0.0], [-88.23525878285378, 40.09287124447567, 0.0], [-88.23526219695626, 40.09287144250836, 0.0], [-88.2352656165536, 40.09287148333374, 0.0], [-88.2352690344108, 40.09287136686541, 0.0], [-88.23527244329654, 40.0928710933498, 0.0], [-88.23527583599845, 40.092870663365595, 0.0], [-88.23527920533846, 40.09287007782253, 0.0], [-88.23528254418787, 40.09286933795948, 0.0], [-88.23528584548252, 40.09286844534179, 0.0], [-88.23528910223772, 40.09286740185804, 0.0], [-88.23529230756296, 40.09286620971595, 0.0], [-88.23529545467662, 40.09286487143781, 0.0], [-88.23529853692017, 40.092863389855076, 0.0], [-88.23530154777238, 40.0928617681024, 0.0], [-88.23530448086302, 40.092860009611016, 0.0], [-88.23530732998644, 40.092858118101425, 0.0], [-88.23531008911458, 40.092856097575606, 0.0], [-88.23531275240985, 40.092853952308474, 0.0], [-88.23531531423737, 40.09285168683888, 0.0], [-88.23531776917696, 40.09284930595997, 0.0], [-88.23532011203457, 40.09284681470911, 0.0], [-88.23532233785335, 40.09284421835714, 0.0], [-88.23532444192398, 40.0928415223973, 0.0], [-88.2353264197948, 40.092838732533544, 0.0], [-88.23532826728113, 40.09283585466854, 0.0], [-88.23532998047415, 40.09283289489112, 0.0], [-88.23533155574918, 40.09282985946342, 0.0], [-88.23533298977333, 40.092826754807646, 0.0], [-88.23533427951257, 40.092823587492475, 0.0], [-88.23533542223814, 40.092820364219136, 0.0], [-88.2353364155323, 40.09281709180727, 0.0], [-88.23533725729352, 40.092813777180474, 0.0], [-88.23533794574082, 40.09281042735167, 0.0], [-88.23533847941763, 40.09280704940824, 0.0], [-88.23533885719483, 40.09280365049708, 0.0], [-88.23533907827314, 40.09280023780941, 0.0], [-88.2353391421848, 40.09279681856563, 0.0], [-88.23533904879459, 40.0927934, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "east_inter": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.2352949512054, 40.0927934, 0.0], [-88.23529380991491, 40.092790118443446, 0.0], [-88.23529238269535, 40.09278695076264, 0.0], [-88.2352906807462, 40.09278392181455, 0.0], [-88.23528871742276, 40.09278105536754, 0.0], [-88.23528650813137, 40.09277837391479, 0.0], [-88.23528407020844, 40.09277589849782, 0.0], [-88.23528142278451, 40.09277364854138, 0.0], [-88.2352785866341, 40.09277164170102, 0.0], [-88.23527558401264, 40.092769893724544, 0.0], [-88.2352724384819, 40.092768418328426, 0.0], [-88.23526917472503, 40.09276722709019, 0.0], [-88.23526581835296, 40.09276632935756, 0.0], [-88.23526239570333, 40.092765732175096, 0.0], [-88.23525893363389, 40.09276544022892, 0.0], [-88.2352554593117, 40.09276545580997, 0.0], [-88.235252, 40.092765778795965, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.23533904879459, 40.0927934, 0.0], [-88.23534004364888, 40.09279023568675, 0.0], [-88.23534130231853, 40.092787166751414, 0.0], [-88.23534281580099, 40.09278421514436, 0.0], [-88.23534457327118, 40.092781401976765, 0.0], [-88.2353465621589, 40.09277874736963, 0.0], [-88.23534876823875, 40.09277627030987, 0.0], [-88.23535117573189, 40.09277398851449, 0.0], [-88.23535376741889, 40.092771918303896, 0.0], [-88.23535652476285, 40.092770074485124, 0.0], [-88.23535942804206, 40.092768470245986, 0.0], [-88.23536245649099, 40.09276711706069, 0.0], [-88.23536558844886, 40.09276602460781, 0.0], [-88.23536880151454, 40.09276520070105, 0.0], [-88.23537207270677, 40.09276465123335, 0.0], [-88.23537537862856, 40.09276438013474, 0.0], [-88.23537869563452, 40.09276438934423, 0.0], [-88.23538199999999, 40.092764678795966, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "westward": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235252, 40.09273962120403, 0.0], [-88.23525547572815, 40.09273957508753, 0.0], [-88.23525895145632, 40.09273952897102, 0.0], [-88.23526242718447, 40.09273948285452, 0.0], [-88.23526590291263, 40.092739436738015, 0.0], [-88.23526937864078, 40.09273939062151, 0.0], [-88.23527285436893, 40.092739344505006, 0.0], [-88.2352763300971, 40.0927392983885, 0.0], [-88.23527980582524, 40.09273925227199, 0.0], [-88.23528328155341, 40.092739206155485, 0.0], [-88.23528675728156, 40.09273916003898, 0.0], [-88.2352902330097, 40.09273911392248, 0.0], [-88.23529370873787, 40.09273906780597, 0.0], [-88.23529718446602, 40.09273902168947, 0.0], [-88.23530066019418, 40.09273897557296, 0.0], [-88.23530413592233, 40.09273892945646, 0.0], [-88.23530761165048, 40.092738883339955, 0.0], [-88.23531108737865, 40.09273883722345, 0.0], [-88.2353145631068, 40.092738791106946, 0.0], [-88.23531803883495, 40.09273874499044, 0.0], [-88.23532151456311, 40.09273869887394, 0.0], [-88.23532499029126, 40.09273865275743, 0.0], [-88.23532846601942, 40.09273860664093, 0.0], [-88.23533194174757, 40.092738560524424, 0.0], [-88.23533541747572, 40.09273851440791, 0.0], [-88.23533889320389, 40.09273846829141, 0.0], [-88.23534236893204, 40.092738422174904, 0.0], [-88.2353458446602, 40.0927383760584, 0.0], [-88.23534932038835, 40.092738329941895, 0.0], [-88.2353527961165, 40.09273828382539, 0.0], [-88.23535627184467, 40.092738237708886, 0.0], [-88.23535974757282, 40.09273819159238, 0.0], [-88.23536322330098, 40.09273814547588, 0.0], [-88.23536669902913, 40.09273809935937, 0.0], [-88.23537017475728, 40.09273805324287, 0.0], [-88.23537365048544, 40.092738007126364, 0.0], [-88.23537712621359, 40.09273796100986, 0.0], [-88.23538060194176, 40.092737914893355, 0.0], [-88.2353840776699, 40.09273786877685, 0.0], [-88.23538755339806, 40.09273782266035, 0.0], [-88.23539102912622, 40.092737776543835, 0.0], [-88.23539450485437, 40.09273773042733, 0.0], [-88.23539798058253, 40.092737684310826, 0.0], [-88.23540145631068, 40.09273763819432, 0.0], [-88.23540493203883, 40.09273759207782, 0.0], [-88.235408407767, 40.09273754596131, 0.0], [-88.23541188349515, 40.09273749984481, 0.0], [-88.2354153592233, 40.092737453728304, 0.0], [-88.23541883495146, 40.0927374076118, 0.0], [-88.23542231067961, 40.092737361495296, 0.0], [-88.23542578640777, 40.09273731537879, 0.0], [-88.23542926213592, 40.09273726926229, 0.0], [-88.23543273786407, 40.09273722314578, 0.0], [-88.23543621359224, 40.09273717702928, 0.0], [-88.23543968932039, 40.092737130912774, 0.0], [-88.23544316504855, 40.09273708479627, 0.0], [-88.2354466407767, 40.09273703867976, 0.0], [-88.23545011650485, 40.09273699256325, 0.0], [-88.23545359223301, 40.09273694644675, 0.0], [-88.23545706796116, 40.092736900330245, 0.0], [-88.23546054368933, 40.09273685421374, 0.0], [-88.23546401941748, 40.092736808097236, 0.0], [-88.23546749514563, 40.09273676198073, 0.0], [-88.23547097087379, 40.09273671586423, 0.0], [-88.23547444660194, 40.09273666974772, 0.0], [-88.2354779223301, 40.09273662363122, 0.0], [-88.23548139805825, 40.092736577514714, 0.0], [-88.2354848737864, 40.09273653139821, 0.0], [-88.23548834951457, 40.092736485281705, 0.0], [-88.23549182524272, 40.0927364391652, 0.0], [-88.23549530097088, 40.092736393048696, 0.0], [-88.23549877669903, 40.09273634693219, 0.0], [-88.23550225242718, 40.09273630081568, 0.0], [-88.23550572815535, 40.092736254699176, 0.0], [-88.2355092038835, 40.09273620858267, 0.0], [-88.23551267961165, 40.09273616246617, 0.0], [-88.23551615533981, 40.09273611634966, 0.0], [-88.23551963106796, 40.09273607023316, 0.0], [-88.23552310679612, 40.092736024116654, 0.0], [-88.23552658252427, 40.09273597800015, 0.0], [-88.23553005825242, 40.092735931883645, 0.0], [-88.23553353398059, 40.09273588576714, 0.0], [-88.23553700970874, 40.092735839650636, 0.0], [-88.2355404854369, 40.09273579353413, 0.0], [-88.23554396116505, 40.09273574741763, 0.0], [-88.2355474368932, 40.09273570130112, 0.0], [-88.23555091262136, 40.09273565518462, 0.0], [-88.23555438834951, 40.092735609068114, 0.0], [-88.23555786407768, 40.0927355629516, 0.0], [-88.23556133980583, 40.0927355168351, 0.0], [-88.23556481553398, 40.092735470718594, 0.0], [-88.23556829126214, 40.09273542460209, 0.0], [-88.23557176699029, 40.092735378485585, 0.0], [-88.23557524271845, 40.09273533236908, 0.0], [-88.2355787184466, 40.09273528625258, 0.0], [-88.23558219417475, 40.09273524013607, 0.0], [-88.23558566990292, 40.09273519401957, 0.0], [-88.23558914563107, 40.09273514790306, 0.0], [-88.23559262135923, 40.09273510178656, 0.0], [-88.23559609708738, 40.092735055670055, 0.0], [-88.23559957281553, 40.09273500955355, 0.0], [-88.2356030485437, 40.092734963437046, 0.0], [-88.23560652427184, 40.09273491732054, 0.0], [-88.23561000000001, 40.09273487120403, 0.0], [-88.23561347572816, 40.092734825087526, 0.0], [-88.23561695145631, 40.09273477897102, 0.0], [-88.23562042718447, 40.09273473285452, 0.0], [-88.23562390291262, 40.09273468673801, 0.0], [-88.23562737864077, 40.09273464062151, 0.0], [-88.23563085436894, 40.092734594505, 0.0], [-88.23563433009708, 40.0927345483885, 0.0], [-88.23563780582525, 40.092734502271995, 0.0], [-88.2356412815534, 40.09273445615549, 0.0], [-88.23564475728155, 40.092734410038986, 0.0], [-88.23564823300971, 40.09273436392248, 0.0], [-88.23565170873786, 40.09273431780598, 0.0], [-88.23565518446603, 40.09273427168947, 0.0], [-88.23565866019418, 40.09273422557297, 0.0], [-88.23566213592233, 40.092734179456464, 0.0], [-88.23566561165049, 40.09273413333995, 0.0], [-88.23566908737864, 40.09273408722345, 0.0], [-88.2356725631068, 40.092734041106944, 0.0], [-88.23567603883495, 40.09273399499044, 0.0], [-88.2356795145631, 40.092733948873935, 0.0], [-88.23568299029127, 40.09273390275743, 0.0], [-88.23568646601942, 40.092733856640926, 0.0], [-88.23568994174758, 40.09273381052442, 0.0], [-88.23569341747573, 40.09273376440792, 0.0], [-88.23569689320388, 40.09273371829141, 0.0], [-88.23570036893204, 40.09273367217491, 0.0], [-88.2357038446602, 40.092733626058404, 0.0], [-88.23570732038836, 40.0927335799419, 0.0], [-88.2357107961165, 40.092733533825395, 0.0], [-88.23571427184466, 40.09273348770889, 0.0], [-88.23571774757282, 40.09273344159239, 0.0], [-88.23572122330097, 40.092733395475875, 0.0], [-88.23572469902912, 40.09273334935937, 0.0], [-88.23572817475728, 40.092733303242866, 0.0], [-88.23573165048543, 40.09273325712636, 0.0], [-88.2357351262136, 40.09273321100986, 0.0], [-88.23573860194175, 40.09273316489335, 0.0], [-88.2357420776699, 40.09273311877685, 0.0], [-88.23574555339806, 40.092733072660344, 0.0], [-88.23574902912621, 40.09273302654384, 0.0], [-88.23575250485437, 40.092732980427336, 0.0], [-88.23575598058252, 40.09273293431083, 0.0], [-88.23575945631067, 40.09273288819433, 0.0], [-88.23576293203884, 40.09273284207782, 0.0], [-88.23576640776699, 40.09273279596132, 0.0], [-88.23576988349515, 40.092732749844814, 0.0], [-88.2357733592233, 40.09273270372831, 0.0], [-88.23577683495145, 40.0927326576118, 0.0], [-88.23578031067962, 40.09273261149529, 0.0], [-88.23578378640777, 40.09273256537879, 0.0], [-88.23578726213593, 40.092732519262285, 0.0], [-88.23579073786408, 40.09273247314578, 0.0], [-88.23579421359223, 40.092732427029276, 0.0], [-88.23579768932039, 40.09273238091277, 0.0], [-88.23580116504854, 40.09273233479627, 0.0], [-88.2358046407767, 40.09273228867976, 0.0], [-88.23580811650486, 40.09273224256326, 0.0], [-88.235811592233, 40.092732196446754, 0.0], [-88.23581506796117, 40.09273215033025, 0.0], [-88.23581854368932, 40.092732104213745, 0.0], [-88.23582201941747, 40.09273205809724, 0.0], [-88.23582549514563, 40.092732011980736, 0.0], [-88.23582897087378, 40.09273196586423, 0.0], [-88.23583244660195, 40.09273191974772, 0.0], [-88.2358359223301, 40.092731873631216, 0.0], [-88.23583939805825, 40.09273182751471, 0.0], [-88.23584287378641, 40.09273178139821, 0.0], [-88.23584634951456, 40.0927317352817, 0.0], [-88.23584982524272, 40.0927316891652, 0.0], [-88.23585330097087, 40.092731643048694, 0.0], [-88.23585677669902, 40.09273159693219, 0.0], [-88.23586025242719, 40.092731550815685, 0.0], [-88.23586372815534, 40.09273150469918, 0.0], [-88.2358672038835, 40.092731458582676, 0.0], [-88.23587067961165, 40.09273141246617, 0.0], [-88.2358741553398, 40.09273136634967, 0.0], [-88.23587763106796, 40.09273132023316, 0.0], [-88.23588110679611, 40.09273127411666, 0.0], [-88.23588458252428, 40.092731228000154, 0.0], [-88.23588805825243, 40.09273118188364, 0.0], [-88.23589153398058, 40.09273113576714, 0.0], [-88.23589500970874, 40.092731089650634, 0.0], [-88.23589848543689, 40.09273104353413, 0.0], [-88.23590196116506, 40.092730997417625, 0.0], [-88.2359054368932, 40.09273095130112, 0.0], [-88.23590891262135, 40.09273090518462, 0.0], [-88.23591238834952, 40.09273085906811, 0.0], [-88.23591586407767, 40.09273081295161, 0.0], [-88.23591933980582, 40.0927307668351, 0.0], [-88.23592281553398, 40.0927307207186, 0.0], [-88.23592629126213, 40.092730674602095, 0.0], [-88.2359297669903, 40.09273062848559, 0.0], [-88.23593324271845, 40.092730582369086, 0.0], [-88.2359367184466, 40.09273053625258, 0.0], [-88.23594019417476, 40.09273049013608, 0.0], [-88.23594366990291, 40.092730444019566, 0.0], [-88.23594714563107, 40.09273039790306, 0.0], [-88.23595062135922, 40.09273035178656, 0.0], [-88.23595409708737, 40.09273030567005, 0.0], [-88.23595757281554, 40.09273025955355, 0.0], [-88.23596104854369, 40.092730213437044, 0.0], [-88.23596452427185, 40.09273016732054, 0.0], [-88.235968, 40.092730121204035, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.23538199999999, 40.092764678795966, 0.0], [-88.23538548484848, 40.092764615917176, 0.0], [-88.23538896969696, 40.09276455303839, 0.0], [-88.23539245454545, 40.0927644901596, 0.0], [-88.23539593939392, 40.09276442728081, 0.0], [-88.23539942424242, 40.09276436440203, 0.0], [-88.2354029090909, 40.09276430152324, 0.0], [-88.23540639393939, 40.09276423864445, 0.0], [-88.23540987878786, 40.092764175765666, 0.0], [-88.23541336363635, 40.092764112886876, 0.0], [-88.23541684848483, 40.092764050008086, 0.0], [-88.23542033333332, 40.0927639871293, 0.0], [-88.2354238181818, 40.09276392425051, 0.0], [-88.23542730303029, 40.09276386137172, 0.0], [-88.23543078787878, 40.09276379849294, 0.0], [-88.23543427272726, 40.09276373561415, 0.0], [-88.23543775757575, 40.09276367273536, 0.0], [-88.23544124242423, 40.092763609856576, 0.0], [-88.23544472727272, 40.092763546977785, 0.0], [-88.2354482121212, 40.092763484098995, 0.0], [-88.23545169696969, 40.09276342122021, 0.0], [-88.23545518181817, 40.09276335834142, 0.0], [-88.23545866666666, 40.09276329546263, 0.0], [-88.23546215151514, 40.09276323258384, 0.0], [-88.23546563636363, 40.09276316970506, 0.0], [-88.2354691212121, 40.09276310682627, 0.0], [-88.2354726060606, 40.09276304394748, 0.0], [-88.23547609090909, 40.092762981068695, 0.0], [-88.23547957575757, 40.092762918189905, 0.0], [-88.23548306060606, 40.092762855311115, 0.0], [-88.23548654545453, 40.09276279243233, 0.0], [-88.23549003030303, 40.09276272955354, 0.0], [-88.2354935151515, 40.09276266667475, 0.0], [-88.235497, 40.09276260379597, 0.0], [-88.23550048484847, 40.09276254091718, 0.0], [-88.23550396969696, 40.09276247803839, 0.0], [-88.23550745454544, 40.092762415159605, 0.0], [-88.23551093939393, 40.092762352280815, 0.0], [-88.23551442424241, 40.092762289402025, 0.0], [-88.2355179090909, 40.09276222652324, 0.0], [-88.2355213939394, 40.09276216364445, 0.0], [-88.23552487878787, 40.09276210076566, 0.0], [-88.23552836363636, 40.09276203788688, 0.0], [-88.23553184848484, 40.09276197500809, 0.0], [-88.23553533333333, 40.0927619121293, 0.0], [-88.23553881818181, 40.092761849250515, 0.0], [-88.2355423030303, 40.092761786371724, 0.0], [-88.23554578787878, 40.092761723492934, 0.0], [-88.23554927272727, 40.09276166061415, 0.0], [-88.23555275757575, 40.09276159773536, 0.0], [-88.23555624242424, 40.09276153485657, 0.0], [-88.23555972727272, 40.09276147197779, 0.0], [-88.2355632121212, 40.092761409099, 0.0], [-88.2355666969697, 40.09276134622021, 0.0], [-88.23557018181818, 40.092761283341424, 0.0], [-88.23557366666667, 40.092761220462634, 0.0], [-88.23557715151514, 40.092761157583844, 0.0], [-88.23558063636364, 40.09276109470506, 0.0], [-88.23558412121211, 40.09276103182627, 0.0], [-88.2355876060606, 40.09276096894748, 0.0], [-88.23559109090908, 40.0927609060687, 0.0], [-88.23559457575757, 40.09276084318991, 0.0], [-88.23559806060605, 40.09276078031112, 0.0], [-88.23560154545454, 40.092760717432334, 0.0], [-88.23560503030302, 40.092760654553544, 0.0], [-88.23560851515151, 40.092760591674754, 0.0], [-88.235612, 40.092760528795964, 0.0], [-88.23561548484848, 40.09276046591718, 0.0], [-88.23561896969697, 40.09276040303839, 0.0], [-88.23562245454545, 40.0927603401596, 0.0], [-88.23562593939394, 40.09276027728082, 0.0], [-88.23562942424242, 40.09276021440203, 0.0], [-88.23563290909091, 40.09276015152324, 0.0], [-88.23563639393939, 40.092760088644454, 0.0], [-88.23563987878788, 40.092760025765664, 0.0], [-88.23564336363636, 40.09275996288687, 0.0], [-88.23564684848485, 40.09275990000809, 0.0], [-88.23565033333333, 40.0927598371293, 0.0], [-88.23565381818182, 40.09275977425051, 0.0], [-88.2356573030303, 40.09275971137173, 0.0], [-88.23566078787879, 40.09275964849294, 0.0], [-88.23566427272728, 40.09275958561415, 0.0], [-88.23566775757575, 40.09275952273536, 0.0], [-88.23567124242425, 40.09275945985657, 0.0], [-88.23567472727272, 40.09275939697778, 0.0], [-88.23567821212121, 40.092759334099, 0.0], [-88.23568169696969, 40.09275927122021, 0.0], [-88.23568518181818, 40.09275920834142, 0.0], [-88.23568866666666, 40.09275914546264, 0.0], [-88.23569215151515, 40.09275908258385, 0.0], [-88.23569563636363, 40.092759019705056, 0.0], [-88.23569912121212, 40.09275895682627, 0.0], [-88.2357026060606, 40.09275889394748, 0.0], [-88.23570609090909, 40.09275883106869, 0.0], [-88.23570957575758, 40.09275876818991, 0.0], [-88.23571306060606, 40.09275870531112, 0.0], [-88.23571654545455, 40.09275864243233, 0.0], [-88.23572003030303, 40.092758579553546, 0.0], [-88.23572351515152, 40.092758516674756, 0.0], [-88.235727, 40.092758453795966, 0.0], [-88.23573048484849, 40.09275839091718, 0.0], [-88.23573396969697, 40.09275832803839, 0.0], [-88.23573745454546, 40.0927582651596, 0.0], [-88.23574093939393, 40.09275820228082, 0.0], [-88.23574442424243, 40.09275813940203, 0.0], [-88.2357479090909, 40.09275807652324, 0.0], [-88.2357513939394, 40.092758013644456, 0.0], [-88.23575487878789, 40.092757950765666, 0.0], [-88.23575836363636, 40.092757887886876, 0.0], [-88.23576184848486, 40.09275782500809, 0.0], [-88.23576533333333, 40.0927577621293, 0.0], [-88.23576881818182, 40.09275769925051, 0.0], [-88.2357723030303, 40.09275763637172, 0.0], [-88.2357757878788, 40.09275757349294, 0.0], [-88.23577927272727, 40.09275751061415, 0.0], [-88.23578275757576, 40.09275744773536, 0.0], [-88.23578624242424, 40.092757384856576, 0.0], [-88.23578972727273, 40.092757321977786, 0.0], [-88.23579321212121, 40.092757259098995, 0.0], [-88.2357966969697, 40.09275719622021, 0.0], [-88.23580018181819, 40.09275713334142, 0.0], [-88.23580366666667, 40.09275707046263, 0.0], [-88.23580715151516, 40.09275700758385, 0.0], [-88.23581063636364, 40.09275694470506, 0.0], [-88.23581412121213, 40.09275688182627, 0.0], [-88.2358176060606, 40.092756818947485, 0.0], [-88.2358210909091, 40.092756756068695, 0.0], [-88.23582457575758, 40.092756693189905, 0.0], [-88.23582806060607, 40.09275663031112, 0.0], [-88.23583154545454, 40.09275656743233, 0.0], [-88.23583503030304, 40.09275650455354, 0.0], [-88.23583851515151, 40.09275644167476, 0.0], [-88.235842, 40.09275637879597, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "west_cycle_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235968, 40.09273012120404, 0.0], [-88.2359714630335, 40.092730204925324, 0.0], [-88.23597491797813, 40.09273045585916, 0.0], [-88.23597835676476, 40.09273087341949, 0.0], [-88.23598177136202, 40.092731456631086, 0.0], [-88.235985153795, 40.092732204131835, 0.0], [-88.23598849616391, 40.092733114175935, 0.0], [-88.23599179066255, 40.09273418463794, 0.0], [-88.23599502959652, 40.09273541301775, 0.0], [-88.23599820540117, 40.09273679644646, 0.0], [-88.23600131065933, 40.09273833169301, 0.0], [-88.23600433811856, 40.0927400151718, 0.0], [-88.23600728070814, 40.092741842951, 0.0], [-88.23601013155555, 40.092743810761775, 0.0], [-88.23601288400255, 40.092745914008255, 0.0], [-88.23601553162071, 40.092748147778224, 0.0], [-88.23601806822643, 40.09275050685465, 0.0], [-88.2360204878954, 40.092752985727834, 0.0], [-88.23602278497641, 40.09275557860829, 0.0], [-88.23602495410454, 40.09275827944026, 0.0], [-88.23602699021373, 40.092761081915874, 0.0], [-88.23602888854859, 40.09276397948986, 0.0], [-88.23603064467548, 40.09276696539484, 0.0], [-88.23603225449293, 40.092770032657135, 0.0], [-88.23603371424115, 40.09277317411306, 0.0], [-88.23603502051085, 40.092776382425654, 0.0], [-88.2360361702512, 40.09277965010179, 0.0], [-88.23603716077695, 40.09278296950971, 0.0], [-88.23603798977469, 40.092786332896836, 0.0], [-88.23603865530826, 40.092789732407866, 0.0], [-88.2360391558233, 40.09279316010314, 0.0], [-88.23603949015086, 40.09279660797717, 0.0], [-88.23603965751006, 40.09280006797733, 0.0], [-88.23603965751006, 40.09280353202268, 0.0], [-88.23603949015086, 40.09280699202284, 0.0], [-88.2360391558233, 40.092810439896866, 0.0], [-88.23603865530826, 40.09281386759214, 0.0], [-88.23603798977469, 40.09281726710317, 0.0], [-88.23603716077695, 40.092820630490294, 0.0], [-88.2360361702512, 40.09282394989822, 0.0], [-88.23603502051085, 40.09282721757435, 0.0], [-88.23603371424115, 40.092830425886945, 0.0], [-88.23603225449293, 40.09283356734287, 0.0], [-88.23603064467548, 40.09283663460517, 0.0], [-88.23602888854859, 40.09283962051015, 0.0], [-88.23602699021373, 40.09284251808413, 0.0], [-88.23602495410454, 40.092845320559746, 0.0], [-88.23602278497641, 40.09284802139172, 0.0], [-88.2360204878954, 40.09285061427217, 0.0], [-88.23601806822643, 40.09285309314536, 0.0], [-88.23601553162071, 40.092855452221784, 0.0], [-88.23601288400255, 40.09285768599175, 0.0], [-88.23601013155555, 40.09285978923823, 0.0], [-88.23600728070814, 40.09286175704901, 0.0], [-88.23600433811856, 40.09286358482821, 0.0], [-88.23600131065933, 40.092865268306994, 0.0], [-88.23599820540117, 40.09286680355355, 0.0], [-88.23599502959652, 40.09286818698226, 0.0], [-88.23599179066255, 40.09286941536207, 0.0], [-88.23598849616391, 40.09287048582407, 0.0], [-88.235985153795, 40.09287139586817, 0.0], [-88.23598177136202, 40.09287214336892, 0.0], [-88.23597835676476, 40.09287272658052, 0.0], [-88.23597491797813, 40.092873144140846, 0.0], [-88.2359714630335, 40.09287339507468, 0.0], [-88.235968, 40.092873478795966, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235968, 40.09275627879597, 0.0], [-88.23597140178954, 40.09275640610273, 0.0], [-88.23597478455345, 40.092756787268556, 0.0], [-88.2359781293741, 40.09275742016183, 0.0], [-88.23598141754604, 40.092758301243194, 0.0], [-88.23598463068063, 40.092759425585314, 0.0], [-88.23598775080886, 40.09276078690047, 0.0], [-88.23599076048183, 40.09276237757569, 0.0], [-88.23599364286838, 40.09276418871534, 0.0], [-88.23599638184916, 40.09276621019088, 0.0], [-88.2359989621068, 40.09276843069749, 0.0], [-88.23600136921156, 40.09277083781729, 0.0], [-88.23600358970205, 40.092773418088804, 0.0], [-88.2360056111605, 40.092776157082206, 0.0], [-88.23600742228214, 40.09277903948007, 0.0], [-88.23600901293855, 40.09278204916299, 0.0], [-88.23601037423421, 40.09278516929972, 0.0], [-88.23601149855627, 40.09278838244133, 0.0], [-88.2360123796171, 40.09279167061877, 0.0], [-88.23601301248947, 40.09279501544337, 0.0], [-88.23601339363417, 40.092798398209666, 0.0], [-88.23601352091967, 40.092801800000004, 0.0], [-88.23601339363417, 40.09280520179034, 0.0], [-88.23601301248947, 40.092808584556636, 0.0], [-88.2360123796171, 40.092811929381234, 0.0], [-88.23601149855627, 40.09281521755868, 0.0], [-88.23601037423421, 40.09281843070029, 0.0], [-88.23600901293855, 40.092821550837016, 0.0], [-88.23600742228214, 40.09282456051994, 0.0], [-88.2360056111605, 40.0928274429178, 0.0], [-88.23600358970205, 40.092830181911204, 0.0], [-88.23600136921156, 40.09283276218272, 0.0], [-88.2359989621068, 40.09283516930252, 0.0], [-88.23599638184916, 40.09283738980913, 0.0], [-88.23599364286838, 40.092839411284665, 0.0], [-88.23599076048183, 40.09284122242432, 0.0], [-88.23598775080886, 40.09284281309954, 0.0], [-88.23598463068063, 40.092844174414694, 0.0], [-88.23598141754604, 40.092845298756814, 0.0], [-88.2359781293741, 40.092846179838176, 0.0], [-88.23597478455345, 40.09284681273145, 0.0], [-88.23597140178954, 40.09284719389728, 0.0], [-88.235968, 40.09284732120404, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "west_cycle_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.235968, 40.092873478795966, 0.0], [-88.23596451940529, 40.09287359409049, 0.0], [-88.23596103707816, 40.092873559016645, 0.0], [-88.23595755951179, 40.092873373639826, 0.0], [-88.23595409319047, 40.0928730383057, 0.0], [-88.23595064457756, 40.09287255363952, 0.0], [-88.23594722010333, 40.092871920545015, 0.0], [-88.23594382615309, 40.09287114020265, 0.0], [-88.23594046905524, 40.09287021406744, 0.0], [-88.23593715506944, 40.09286914386629, 0.0], [-88.23593389037497, 40.09286793159469, 0.0], [-88.23593068105922, 40.09286657951306, 0.0], [-88.23592753310626, 40.09286509014249, 0.0], [-88.23592445238585, 40.09286346626008, 0.0], [-88.23592144464227, 40.09286171089373, 0.0], [-88.23591851548382, 40.09285982731651, 0.0], [-88.23591567037221, 40.09285781904055, 0.0], [-88.23591291461246, 40.092855689810506, 0.0], [-88.23591025334298, 40.092853443596546, 0.0], [-88.235907691526, 40.092851084586975, 0.0], [-88.23590523393828, 40.09284861718044, 0.0], [-88.23590288516229, 40.09284604597766, 0.0], [-88.23590064957756, 40.09284337577293, 0.0], [-88.23589853135256, 40.092840611545135, 0.0], [-88.23589653443698, 40.092837758448475, 0.0], [-88.23589466255426, 40.09283482180285, 0.0], [-88.23589291919474, 40.09283180708396, 0.0], [-88.23589130760911, 40.09282871991307, 0.0], [-88.23588983080231, 40.092825566046535, 0.0], [-88.23588849152804, 40.092822351365086, 0.0], [-88.23588729228351, 40.09281908186284, 0.0], [-88.23588623530482, 40.09281576363613, 0.0], [-88.23588532256285, 40.09281240287216, 0.0], [-88.2358845557595, 40.092809005837424, 0.0], [-88.23588393632454, 40.092805578866056, 0.0], [-88.23588346541298, 40.09280212834802, 0.0], [-88.2358831439029, 40.09279866071718, 0.0], [-88.23588297239377, 40.092795182439296, 0.0], [-88.23588295120541, 40.0927917, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.235968, 40.092847321204026, 0.0], [-88.23596461311375, 40.09284693910652, 0.0], [-88.23596126431049, 40.09284630466685, 0.0], [-88.23595797236418, 40.0928454214418, 0.0], [-88.23595475573005, 40.09284429438288, 0.0], [-88.23595163244109, 40.092842929808576, 0.0], [-88.235948620007, 40.092841335368924, 0.0], [-88.235945735316, 40.09283952000262, 0.0], [-88.23594299454015, 40.09283749388693, 0.0], [-88.23594041304473, 40.09283526838061, 0.0], [-88.23593800530206, 40.09283285596022, 0.0], [-88.23593578481032, 40.0928302701502, 0.0], [-88.235933764018, 40.092827525447056, 0.0], [-88.23593195425399, 40.09282463723806, 0.0], [-88.23593036566415, 40.09282162171501, 0.0], [-88.23592900715437, 40.09281849578344, 0.0], [-88.2359278863407, 40.09281527696787, 0.0], [-88.23592700950663, 40.09281198331352, 0.0], [-88.23592638156781, 40.09280863328519, 0.0], [-88.2359260060446, 40.092805245663705, 0.0], [-88.23592588504223, 40.09280183944067, 0.0], [-88.23592601923907, 40.092798433711934, 0.0], [-88.23592640788279, 40.09279504757062, 0.0], [-88.23592704879458, 40.0927917, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "west_inter": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-88.23588295120543, 40.0927917, 0.0], [-88.23588207420826, 40.09278840946364, 0.0], [-88.23588095367528, 40.09278519369633, 0.0], [-88.23587959587744, 40.092782070694795, 0.0], [-88.23587800841354, 40.09277905793661, 0.0], [-88.23587620016764, 40.09277617228238, 0.0], [-88.23587418125942, 40.09277342988139, 0.0], [-88.23587196298752, 40.092770846081216, 0.0], [-88.23586955776628, 40.09276843534185, 0.0], [-88.23586697905625, 40.09276621115476, 0.0], [-88.23586424128898, 40.09276418596739, 0.0], [-88.23586135978609, 40.0927623711135, 0.0], [-88.23585835067364, 40.09276077674975, 0.0], [-88.23585523079181, 40.09275941179883, 0.0], [-88.23585201760076, 40.09275828389956, 0.0], [-88.23584872908276, 40.09275739936413, 0.0], [-88.2358453836417, 40.09275676314275, 0.0], [-88.235842, 40.09275637879596, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-88.23592704879458, 40.0927917, 0.0], [-88.23592791964276, 40.09278840288894, 0.0], [-88.23592903487396, 40.092785180222776, 0.0], [-88.23593038822938, 40.09278205008748, 0.0], [-88.23593197211379, 40.09277903004973, 0.0], [-88.2359337776383, 40.09277613705835, 0.0], [-88.23593579467004, 40.09277338734913, 0.0], [-88.23593801188922, 40.092770796353776, 0.0], [-88.23594041685254, 40.09276837861325, 0.0], [-88.23594299606307, 40.0927661476962, 0.0], [-88.23594573504597, 40.092764116122794, 0.0], [-88.23594861842976, 40.092762295294456, 0.0], [-88.23595163003253, 40.092760695429895, 0.0], [-88.23595475295282, 40.092759325507735, 0.0], [-88.23595796966443, 40.09275819321615, 0.0], [-88.2359612621148, 40.092757304909675, 0.0], [-88.23596461182632, 40.0927566655736, 0.0], [-88.235968, 40.09275627879597, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}}, "regions": {}, "signs": {}, "static_obstacles": {}, "connections": []}} \ No newline at end of file diff --git a/GEMstack/knowledge/routes/summoning_roadgraph_sim.json b/GEMstack/knowledge/routes/summoning_roadgraph_sim.json new file mode 100644 index 000000000..c134008c7 --- /dev/null +++ b/GEMstack/knowledge/routes/summoning_roadgraph_sim.json @@ -0,0 +1 @@ +{"type": "Roadgraph", "data": {"frame": 0, "curves": {}, "lanes": {"lane_0": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[0.0, 1.49, 0.0], [0.4, 1.49, 0.0], [0.8, 1.49, 0.0], [1.2, 1.49, 0.0], [1.6, 1.49, 0.0], [2.0, 1.49, 0.0], [2.4, 1.49, 0.0], [2.8000000000000003, 1.49, 0.0], [3.2, 1.49, 0.0], [3.5999999999999996, 1.49, 0.0], [4.0, 1.49, 0.0], [4.4, 1.49, 0.0], [4.8, 1.49, 0.0], [5.2, 1.49, 0.0], [5.6000000000000005, 1.49, 0.0], [6.0, 1.49, 0.0], [6.4, 1.49, 0.0], [6.8, 1.49, 0.0], [7.199999999999999, 1.49, 0.0], [7.6000000000000005, 1.49, 0.0], [8.0, 1.49, 0.0], [8.4, 1.49, 0.0], [8.8, 1.49, 0.0], [9.2, 1.49, 0.0], [9.6, 1.49, 0.0], [10.0, 1.49, 0.0], [10.4, 1.49, 0.0], [10.799999999999999, 1.49, 0.0], [11.200000000000001, 1.49, 0.0], [11.6, 1.49, 0.0], [12.0, 1.49, 0.0], [12.4, 1.49, 0.0], [12.8, 1.49, 0.0], [13.2, 1.49, 0.0], [13.6, 1.49, 0.0], [14.0, 1.49, 0.0], [14.399999999999999, 1.49, 0.0], [14.8, 1.49, 0.0], [15.200000000000001, 1.49, 0.0], [15.600000000000001, 1.49, 0.0], [16.0, 1.49, 0.0], [16.4, 1.49, 0.0], [16.8, 1.49, 0.0], [17.2, 1.49, 0.0], [17.6, 1.49, 0.0], [18.0, 1.49, 0.0], [18.4, 1.49, 0.0], [18.8, 1.49, 0.0], [19.2, 1.49, 0.0], [19.6, 1.49, 0.0], [20.0, 1.49, 0.0], [20.400000000000002, 1.49, 0.0], [20.8, 1.49, 0.0], [21.2, 1.49, 0.0], [21.599999999999998, 1.49, 0.0], [22.0, 1.49, 0.0], [22.400000000000002, 1.49, 0.0], [22.8, 1.49, 0.0], [23.2, 1.49, 0.0], [23.599999999999998, 1.49, 0.0], [24.0, 1.49, 0.0], [24.400000000000002, 1.49, 0.0], [24.8, 1.49, 0.0], [25.2, 1.49, 0.0], [25.6, 1.49, 0.0], [26.0, 1.49, 0.0], [26.4, 1.49, 0.0], [26.8, 1.49, 0.0], [27.2, 1.49, 0.0], [27.6, 1.49, 0.0], [28.0, 1.49, 0.0], [28.4, 1.49, 0.0], [28.799999999999997, 1.49, 0.0], [29.200000000000003, 1.49, 0.0], [29.6, 1.49, 0.0], [30.0, 1.49, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[0.0, -1.5, 0.0], [0.4, -1.5, 0.0], [0.8, -1.5, 0.0], [1.2, -1.5, 0.0], [1.6, -1.5, 0.0], [2.0, -1.5, 0.0], [2.4, -1.5, 0.0], [2.8000000000000003, -1.5, 0.0], [3.2, -1.5, 0.0], [3.5999999999999996, -1.5, 0.0], [4.0, -1.5, 0.0], [4.4, -1.5, 0.0], [4.8, -1.5, 0.0], [5.2, -1.5, 0.0], [5.6000000000000005, -1.5, 0.0], [6.0, -1.5, 0.0], [6.4, -1.5, 0.0], [6.8, -1.5, 0.0], [7.199999999999999, -1.5, 0.0], [7.6000000000000005, -1.5, 0.0], [8.0, -1.5, 0.0], [8.4, -1.5, 0.0], [8.8, -1.5, 0.0], [9.2, -1.5, 0.0], [9.6, -1.5, 0.0], [10.0, -1.5, 0.0], [10.4, -1.5, 0.0], [10.799999999999999, -1.5, 0.0], [11.200000000000001, -1.5, 0.0], [11.6, -1.5, 0.0], [12.0, -1.5, 0.0], [12.4, -1.5, 0.0], [12.8, -1.5, 0.0], [13.2, -1.5, 0.0], [13.6, -1.5, 0.0], [14.0, -1.5, 0.0], [14.399999999999999, -1.5, 0.0], [14.8, -1.5, 0.0], [15.200000000000001, -1.5, 0.0], [15.600000000000001, -1.5, 0.0], [16.0, -1.5, 0.0], [16.4, -1.5, 0.0], [16.8, -1.5, 0.0], [17.2, -1.5, 0.0], [17.6, -1.5, 0.0], [18.0, -1.5, 0.0], [18.4, -1.5, 0.0], [18.8, -1.5, 0.0], [19.2, -1.5, 0.0], [19.6, -1.5, 0.0], [20.0, -1.5, 0.0], [20.400000000000002, -1.5, 0.0], [20.8, -1.5, 0.0], [21.2, -1.5, 0.0], [21.599999999999998, -1.5, 0.0], [22.0, -1.5, 0.0], [22.400000000000002, -1.5, 0.0], [22.8, -1.5, 0.0], [23.2, -1.5, 0.0], [23.599999999999998, -1.5, 0.0], [24.0, -1.5, 0.0], [24.400000000000002, -1.5, 0.0], [24.8, -1.5, 0.0], [25.2, -1.5, 0.0], [25.6, -1.5, 0.0], [26.0, -1.5, 0.0], [26.4, -1.5, 0.0], [26.8, -1.5, 0.0], [27.2, -1.5, 0.0], [27.6, -1.5, 0.0], [28.0, -1.5, 0.0], [28.4, -1.5, 0.0], [28.799999999999997, -1.5, 0.0], [29.200000000000003, -1.5, 0.0], [29.6, -1.5, 0.0], [30.0, -1.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "arc_1_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[30.0, 1.5, 0.0], [30.392418775380857, 1.5128464605683796, 0.0], [30.78315715332031, 1.551330831757138, 0.0], [31.17054193209677, 1.6152883175806174, 0.0], [31.552914270615126, 1.70444504226559, 0.0], [31.92863679181897, 1.818419223029366, 0.0], [32.29610059419054, 1.95672280493228, 0.0], [32.65373214131401, 2.1187635508038696, 0.0], [33.0, 2.303847577293368, 0.0], [33.333421398117615, 2.511182326184729, 0.0], [33.652568574052324, 2.7398799582525886, 0.0], [33.95607489060041, 2.9889611551261357, 0.0], [34.242640687119284, 3.2573593128807152, 0.0], [34.511038844873866, 3.543925109399586, 0.0], [34.76012004174741, 3.847431425947676, 0.0], [34.98881767381527, 4.166578601882386, 0.0], [35.19615242270663, 4.5, 0.0], [35.38123644919613, 4.846267858685992, 0.0], [35.543277195067716, 5.203899405809461, 0.0], [35.681580776970634, 5.57136320818103, 0.0], [35.79555495773441, 5.947085729384875, 0.0], [35.88471168241938, 6.32945806790323, 0.0], [35.94866916824286, 6.71684284667969, 0.0], [35.98715353943162, 7.107581224619141, 0.0], [36.0, 7.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[30.0, -1.5, 0.0], [30.392574486288023, -1.4914339942367203, 0.0], [30.784401684728923, -1.4657522828257097, 0.0], [31.174735729980465, -1.423003752364293, 0.0], [31.562833599002374, -1.3632697771098723, 0.0], [31.947956525442926, -1.2866640640793996, 0.0], [32.329371405922686, -1.1933324366016151, 0.0], [32.70635219553846, -1.0834525567340414, 0.0], [33.07818128993102, -0.9572335870731745, 0.0], [33.44415089128581, -0.81491579260158, 0.0], [33.803564355666296, -0.6567700833298495, 0.0], [34.155737519115306, -0.4830974986039953, 0.0], [34.5, -0.29422863405994804, 0.0], [34.835696475121416, -0.09052301231597149, 0.0], [35.162187927159415, 0.1276316013990737, 0.0], [35.47885286107849, 0.35981993737888374, 0.0], [35.785088487178854, 0.605600011929198, 0.0], [36.080311868540946, 0.8645039687088829, 0.0], [36.36396103067893, 1.1360389693210724, 0.0], [36.63549603129111, 1.4196881314590577, 0.0], [36.8943999880708, 1.7149115128211463, 0.0], [37.14018006262111, 2.0211471389215143, 0.0], [37.372368398600926, 2.3378120728405856, 0.0], [37.59052301231597, 2.6643035248785853, 0.0], [37.79422863405995, 3.0, 0.0], [37.983097498603996, 3.3442624808846935, 0.0], [38.156770083329846, 3.6964356443337056, 0.0], [38.31491579260158, 4.055849108714192, 0.0], [38.457233587073176, 4.421818710068981, 0.0], [38.583452556734045, 4.793647804461541, 0.0], [38.693332436601615, 5.170628594077314, 0.0], [38.7866640640794, 5.552043474557074, 0.0], [38.86326977710987, 5.937166400997627, 0.0], [38.923003752364295, 6.325264270019535, 0.0], [38.96575228282571, 6.715598315271075, 0.0], [38.99143399423672, 7.107425513711976, 0.0], [39.0, 7.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "arc_1_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[36.0, 7.5, 0.0], [35.97433458412143, 7.891578576660155, 0.0], [35.897777478867205, 8.276457135307563, 0.0], [35.77163859753386, 8.648050297095269, 0.0], [35.598076211353316, 9.0, 0.0], [35.38006002087371, 9.326284287026162, 0.0], [35.121320343559645, 9.621320343559642, 0.0], [34.82628428702616, 9.880060020873707, 0.0], [34.5, 10.098076211353316, 0.0], [34.14805029709527, 10.27163859753386, 0.0], [33.77645713530756, 10.397777478867205, 0.0], [33.39157857666015, 10.474334584121431, 0.0], [33.0, 10.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[39.0, 7.5, 0.0], [38.98715353943162, 7.892418775380858, 0.0], [38.94866916824286, 8.28315715332031, 0.0], [38.88471168241938, 8.67054193209677, 0.0], [38.79555495773441, 9.052914270615124, 0.0], [38.681580776970634, 9.428636791818969, 0.0], [38.543277195067716, 9.796100594190538, 0.0], [38.38123644919613, 10.153732141314007, 0.0], [38.19615242270663, 10.5, 0.0], [37.98881767381527, 10.833421398117613, 0.0], [37.76012004174741, 11.152568574052324, 0.0], [37.511038844873866, 11.456074890600412, 0.0], [37.242640687119284, 11.742640687119284, 0.0], [36.95607489060041, 12.011038844873863, 0.0], [36.652568574052324, 12.260120041747411, 0.0], [36.333421398117615, 12.488817673815271, 0.0], [36.0, 12.696152422706632, 0.0], [35.65373214131401, 12.88123644919613, 0.0], [35.29610059419054, 13.04327719506772, 0.0], [34.92863679181897, 13.181580776970634, 0.0], [34.552914270615126, 13.29555495773441, 0.0], [34.17054193209677, 13.384711682419383, 0.0], [33.78315715332031, 13.448669168242862, 0.0], [33.39241877538086, 13.48715353943162, 0.0], [33.0, 13.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "t_1_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[33.0, 10.5, 0.0], [32.60842142333985, 10.474334584121431, 0.0], [32.22354286469244, 10.397777478867205, 0.0], [31.85194970290473, 10.27163859753386, 0.0], [31.5, 10.098076211353316, 0.0], [31.173715712973838, 9.880060020873707, 0.0], [30.878679656440358, 9.621320343559642, 0.0], [30.619939979126293, 9.326284287026162, 0.0], [30.401923788646684, 9.0, 0.0], [30.228361402466142, 8.64805029709527, 0.0], [30.102222521132795, 8.276457135307563, 0.0], [30.02566541587857, 7.891578576660155, 0.0], [30.0, 7.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[33.0, 13.5, 0.0], [32.625, 13.5, 0.0], [32.25, 13.5, 0.0], [31.875, 13.5, 0.0], [31.5, 13.5, 0.0], [31.125, 13.5, 0.0], [30.75, 13.5, 0.0], [30.375, 13.5, 0.0], [30.0, 13.5, 0.0], [29.625, 13.5, 0.0], [29.25, 13.5, 0.0], [28.875, 13.5, 0.0], [28.5, 13.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "t_1_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[27.0, 7.5, 0.0], [26.97433458412143, 7.891578576660155, 0.0], [26.897777478867205, 8.276457135307563, 0.0], [26.771638597533858, 8.648050297095269, 0.0], [26.598076211353316, 9.0, 0.0], [26.380060020873707, 9.326284287026162, 0.0], [26.121320343559642, 9.621320343559642, 0.0], [25.826284287026162, 9.880060020873707, 0.0], [25.5, 10.098076211353316, 0.0], [25.14805029709527, 10.27163859753386, 0.0], [24.776457135307563, 10.397777478867205, 0.0], [24.391578576660155, 10.474334584121431, 0.0], [24.0, 10.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[28.5, 13.5, 0.0], [28.125, 13.5, 0.0], [27.75, 13.5, 0.0], [27.375, 13.5, 0.0], [27.0, 13.5, 0.0], [26.625, 13.5, 0.0], [26.25, 13.5, 0.0], [25.875, 13.5, 0.0], [25.5, 13.5, 0.0], [25.125, 13.5, 0.0], [24.75, 13.5, 0.0], [24.375, 13.5, 0.0], [24.0, 13.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "t_1_3": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[30.0, 7.5, 0.0], [29.98715353943162, 7.107581224619142, 0.0], [29.948669168242862, 6.716842846679691, 0.0], [29.884711682419383, 6.32945806790323, 0.0], [29.79555495773441, 5.947085729384876, 0.0], [29.681580776970634, 5.571363208181031, 0.0], [29.54327719506772, 5.203899405809461, 0.0], [29.38123644919613, 4.846267858685993, 0.0], [29.196152422706632, 4.5, 0.0], [28.98881767381527, 4.166578601882387, 0.0], [28.760120041747413, 3.8474314259476765, 0.0], [28.511038844873866, 3.543925109399587, 0.0], [28.242640687119284, 3.2573593128807152, 0.0], [27.956074890600412, 2.9889611551261366, 0.0], [27.652568574052324, 2.7398799582525886, 0.0], [27.333421398117615, 2.511182326184729, 0.0], [27.0, 2.303847577293368, 0.0], [26.65373214131401, 2.1187635508038705, 0.0], [26.296100594190538, 1.95672280493228, 0.0], [25.92863679181897, 1.818419223029366, 0.0], [25.552914270615126, 1.7044450422655908, 0.0], [25.17054193209677, 1.6152883175806174, 0.0], [24.78315715332031, 1.551330831757138, 0.0], [24.39241877538086, 1.5128464605683796, 0.0], [24.0, 1.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[27.0, 7.5, 0.0], [26.97433458412143, 7.108421423339845, 0.0], [26.897777478867205, 6.723542864692438, 0.0], [26.771638597533858, 6.351949702904731, 0.0], [26.598076211353316, 6.0, 0.0], [26.380060020873707, 5.673715712973838, 0.0], [26.121320343559642, 5.378679656440358, 0.0], [25.826284287026162, 5.119939979126294, 0.0], [25.5, 4.901923788646684, 0.0], [25.14805029709527, 4.72836140246614, 0.0], [24.776457135307563, 4.602222521132795, 0.0], [24.391578576660155, 4.525665415878569, 0.0], [24.0, 4.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "lane_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[24.0, 1.5, 0.0], [23.6, 1.5, 0.0], [23.2, 1.5, 0.0], [22.8, 1.5, 0.0], [22.4, 1.5, 0.0], [22.0, 1.5, 0.0], [21.6, 1.5, 0.0], [21.2, 1.5, 0.0], [20.8, 1.5, 0.0], [20.4, 1.5, 0.0], [20.0, 1.5, 0.0], [19.6, 1.5, 0.0], [19.2, 1.5, 0.0], [18.8, 1.5, 0.0], [18.4, 1.5, 0.0], [18.0, 1.5, 0.0], [17.6, 1.5, 0.0], [17.2, 1.5, 0.0], [16.8, 1.5, 0.0], [16.4, 1.5, 0.0], [16.0, 1.5, 0.0], [15.6, 1.5, 0.0], [15.200000000000001, 1.5, 0.0], [14.8, 1.5, 0.0], [14.4, 1.5, 0.0], [14.0, 1.5, 0.0], [13.600000000000001, 1.5, 0.0], [13.200000000000001, 1.5, 0.0], [12.8, 1.5, 0.0], [12.399999999999999, 1.5, 0.0], [12.0, 1.5, 0.0], [11.6, 1.5, 0.0], [11.2, 1.5, 0.0], [10.8, 1.5, 0.0], [10.4, 1.5, 0.0], [10.0, 1.5, 0.0], [9.6, 1.5, 0.0], [9.200000000000001, 1.5, 0.0], [8.8, 1.5, 0.0], [8.399999999999999, 1.5, 0.0], [8.0, 1.5, 0.0], [7.600000000000001, 1.5, 0.0], [7.199999999999999, 1.5, 0.0], [6.800000000000001, 1.5, 0.0], [6.400000000000002, 1.5, 0.0], [6.0, 1.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[24.0, 4.5, 0.0], [23.6, 4.5, 0.0], [23.2, 4.5, 0.0], [22.8, 4.5, 0.0], [22.4, 4.5, 0.0], [22.0, 4.5, 0.0], [21.6, 4.5, 0.0], [21.2, 4.5, 0.0], [20.8, 4.5, 0.0], [20.4, 4.5, 0.0], [20.0, 4.5, 0.0], [19.6, 4.5, 0.0], [19.2, 4.5, 0.0], [18.8, 4.5, 0.0], [18.4, 4.5, 0.0], [18.0, 4.5, 0.0], [17.6, 4.5, 0.0], [17.2, 4.5, 0.0], [16.8, 4.5, 0.0], [16.4, 4.5, 0.0], [16.0, 4.5, 0.0], [15.6, 4.5, 0.0], [15.200000000000001, 4.5, 0.0], [14.8, 4.5, 0.0], [14.4, 4.5, 0.0], [14.0, 4.5, 0.0], [13.600000000000001, 4.5, 0.0], [13.200000000000001, 4.5, 0.0], [12.8, 4.5, 0.0], [12.399999999999999, 4.5, 0.0], [12.0, 4.5, 0.0], [11.6, 4.5, 0.0], [11.2, 4.5, 0.0], [10.8, 4.5, 0.0], [10.4, 4.5, 0.0], [10.0, 4.5, 0.0], [9.6, 4.5, 0.0], [9.200000000000001, 4.5, 0.0], [8.8, 4.5, 0.0], [8.399999999999999, 4.5, 0.0], [8.0, 4.5, 0.0], [7.600000000000001, 4.5, 0.0], [7.199999999999999, 4.5, 0.0], [6.800000000000001, 4.5, 0.0], [6.400000000000002, 4.5, 0.0], [6.0, 4.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "t_2_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[6.0, 1.5, 0.0], [5.6075812246191425, 1.5128464605683796, 0.0], [5.216842846679691, 1.551330831757138, 0.0], [4.82945806790323, 1.6152883175806174, 0.0], [4.4470857293848765, 1.70444504226559, 0.0], [4.0713632081810305, 1.818419223029366, 0.0], [3.7038994058094614, 1.95672280493228, 0.0], [3.346267858685993, 2.1187635508038696, 0.0], [3.0000000000000013, 2.303847577293368, 0.0], [2.6665786018823883, 2.511182326184727, 0.0], [2.347431425947676, 2.7398799582525886, 0.0], [2.043925109399587, 2.9889611551261357, 0.0], [1.7573593128807152, 3.2573593128807143, 0.0], [1.4889611551261366, 3.543925109399586, 0.0], [1.2398799582525895, 3.8474314259476747, 0.0], [1.0111823261847297, 4.166578601882385, 0.0], [0.8038475772933689, 4.499999999999998, 0.0], [0.6187635508038705, 4.846267858685993, 0.0], [0.45672280493228, 5.2038994058094605, 0.0], [0.3184192230293661, 5.57136320818103, 0.0], [0.2044450422655908, 5.947085729384874, 0.0], [0.11528831758061742, 6.329458067903229, 0.0], [0.05133083175713793, 6.716842846679691, 0.0], [0.01284646056837957, 7.107581224619139, 0.0], [0.0, 7.499999999999999, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[6.0, 4.5, 0.0], [5.608421423339845, 4.525665415878569, 0.0], [5.223542864692438, 4.602222521132795, 0.0], [4.851949702904731, 4.72836140246614, 0.0], [4.500000000000001, 4.901923788646684, 0.0], [4.173715712973838, 5.119939979126294, 0.0], [3.8786796564403576, 5.378679656440357, 0.0], [3.6199399791262947, 5.673715712973838, 0.0], [3.4019237886466844, 5.999999999999999, 0.0], [3.22836140246614, 6.35194970290473, 0.0], [3.1022225211327954, 6.723542864692437, 0.0], [3.025665415878569, 7.108421423339845, 0.0], [3.0, 7.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "t_2_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[6.0, 10.5, 0.0], [5.608421423339845, 10.474334584121431, 0.0], [5.223542864692438, 10.397777478867205, 0.0], [4.851949702904731, 10.27163859753386, 0.0], [4.500000000000001, 10.098076211353316, 0.0], [4.173715712973838, 9.880060020873707, 0.0], [3.8786796564403576, 9.621320343559642, 0.0], [3.6199399791262947, 9.326284287026162, 0.0], [3.4019237886466844, 9.0, 0.0], [3.22836140246614, 8.64805029709527, 0.0], [3.1022225211327954, 8.276457135307563, 0.0], [3.025665415878569, 7.891578576660155, 0.0], [3.0, 7.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[6.0, 13.5, 0.0], [5.625, 13.5, 0.0], [5.25, 13.5, 0.0], [4.875, 13.5, 0.0], [4.5, 13.5, 0.0], [4.125, 13.5, 0.0], [3.75, 13.5, 0.0], [3.375, 13.5, 0.0], [3.0, 13.5, 0.0], [2.625, 13.5, 0.0], [2.25, 13.5, 0.0], [1.875, 13.5, 0.0], [1.5, 13.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "t_2_3": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[0.0, 7.5, 0.0], [-0.025665415878568965, 7.891578576660155, 0.0], [-0.10222252113279495, 8.276457135307563, 0.0], [-0.22836140246614, 8.648050297095269, 0.0], [-0.401923788646684, 9.0, 0.0], [-0.6199399791262943, 9.326284287026162, 0.0], [-0.8786796564403572, 9.621320343559642, 0.0], [-1.173715712973838, 9.880060020873707, 0.0], [-1.4999999999999996, 10.098076211353316, 0.0], [-1.8519497029047305, 10.27163859753386, 0.0], [-2.223542864692437, 10.397777478867205, 0.0], [-2.608421423339845, 10.474334584121431, 0.0], [-3.0, 10.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[1.5, 13.5, 0.0], [1.125, 13.5, 0.0], [0.75, 13.5, 0.0], [0.375, 13.5, 0.0], [0.0, 13.5, 0.0], [-0.375, 13.5, 0.0], [-0.75, 13.5, 0.0], [-1.125, 13.5, 0.0], [-1.5, 13.5, 0.0], [-1.875, 13.5, 0.0], [-2.25, 13.5, 0.0], [-2.625, 13.5, 0.0], [-3.0, 13.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "lane_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[24.0, 10.5, 0.0], [23.6, 10.5, 0.0], [23.2, 10.5, 0.0], [22.8, 10.5, 0.0], [22.4, 10.5, 0.0], [22.0, 10.5, 0.0], [21.6, 10.5, 0.0], [21.2, 10.5, 0.0], [20.8, 10.5, 0.0], [20.4, 10.5, 0.0], [20.0, 10.5, 0.0], [19.6, 10.5, 0.0], [19.2, 10.5, 0.0], [18.8, 10.5, 0.0], [18.4, 10.5, 0.0], [18.0, 10.5, 0.0], [17.6, 10.5, 0.0], [17.2, 10.5, 0.0], [16.8, 10.5, 0.0], [16.4, 10.5, 0.0], [16.0, 10.5, 0.0], [15.6, 10.5, 0.0], [15.200000000000001, 10.5, 0.0], [14.8, 10.5, 0.0], [14.4, 10.5, 0.0], [14.0, 10.5, 0.0], [13.600000000000001, 10.5, 0.0], [13.200000000000001, 10.5, 0.0], [12.8, 10.5, 0.0], [12.399999999999999, 10.5, 0.0], [12.0, 10.5, 0.0], [11.6, 10.5, 0.0], [11.2, 10.5, 0.0], [10.8, 10.5, 0.0], [10.4, 10.5, 0.0], [10.0, 10.5, 0.0], [9.6, 10.5, 0.0], [9.200000000000001, 10.5, 0.0], [8.8, 10.5, 0.0], [8.399999999999999, 10.5, 0.0], [8.0, 10.5, 0.0], [7.600000000000001, 10.5, 0.0], [7.199999999999999, 10.5, 0.0], [6.800000000000001, 10.5, 0.0], [6.400000000000002, 10.5, 0.0], [6.0, 10.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[24.0, 13.5, 0.0], [23.6, 13.5, 0.0], [23.2, 13.5, 0.0], [22.8, 13.5, 0.0], [22.4, 13.5, 0.0], [22.0, 13.5, 0.0], [21.6, 13.5, 0.0], [21.2, 13.5, 0.0], [20.8, 13.5, 0.0], [20.4, 13.5, 0.0], [20.0, 13.5, 0.0], [19.6, 13.5, 0.0], [19.2, 13.5, 0.0], [18.8, 13.5, 0.0], [18.4, 13.5, 0.0], [18.0, 13.5, 0.0], [17.6, 13.5, 0.0], [17.2, 13.5, 0.0], [16.8, 13.5, 0.0], [16.4, 13.5, 0.0], [16.0, 13.5, 0.0], [15.6, 13.5, 0.0], [15.200000000000001, 13.5, 0.0], [14.8, 13.5, 0.0], [14.4, 13.5, 0.0], [14.0, 13.5, 0.0], [13.600000000000001, 13.5, 0.0], [13.200000000000001, 13.5, 0.0], [12.8, 13.5, 0.0], [12.399999999999999, 13.5, 0.0], [12.0, 13.5, 0.0], [11.6, 13.5, 0.0], [11.2, 13.5, 0.0], [10.8, 13.5, 0.0], [10.4, 13.5, 0.0], [10.0, 13.5, 0.0], [9.6, 13.5, 0.0], [9.200000000000001, 13.5, 0.0], [8.8, 13.5, 0.0], [8.399999999999999, 13.5, 0.0], [8.0, 13.5, 0.0], [7.600000000000001, 13.5, 0.0], [7.199999999999999, 13.5, 0.0], [6.800000000000001, 13.5, 0.0], [6.400000000000002, 13.5, 0.0], [6.0, 13.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "arc_2_1": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-3.0, 10.5, 0.0], [-3.3915785766601547, 10.474334584121431, 0.0], [-3.7764571353075618, 10.397777478867205, 0.0], [-4.148050297095269, 10.27163859753386, 0.0], [-4.499999999999999, 10.098076211353316, 0.0], [-4.826284287026162, 9.880060020873707, 0.0], [-5.121320343559642, 9.621320343559642, 0.0], [-5.380060020873705, 9.326284287026162, 0.0], [-5.598076211353316, 9.0, 0.0], [-5.77163859753386, 8.64805029709527, 0.0], [-5.897777478867205, 8.276457135307563, 0.0], [-5.974334584121431, 7.891578576660155, 0.0], [-6.0, 7.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-3.0000000000000004, 13.5, 0.0], [-3.3924187753808583, 13.48715353943162, 0.0], [-3.7831571533203103, 13.448669168242862, 0.0], [-4.17054193209677, 13.384711682419383, 0.0], [-4.552914270615124, 13.29555495773441, 0.0], [-4.928636791818969, 13.181580776970634, 0.0], [-5.2961005941905395, 13.04327719506772, 0.0], [-5.653732141314009, 12.881236449196129, 0.0], [-6.000000000000001, 12.696152422706632, 0.0], [-6.333421398117613, 12.488817673815271, 0.0], [-6.6525685740523235, 12.260120041747411, 0.0], [-6.956074890600412, 12.011038844873864, 0.0], [-7.242640687119284, 11.742640687119286, 0.0], [-7.511038844873866, 11.456074890600412, 0.0], [-7.760120041747411, 11.152568574052323, 0.0], [-7.988817673815271, 10.833421398117613, 0.0], [-8.196152422706632, 10.5, 0.0], [-8.381236449196129, 10.153732141314007, 0.0], [-8.54327719506772, 9.796100594190538, 0.0], [-8.681580776970634, 9.42863679181897, 0.0], [-8.79555495773441, 9.052914270615123, 0.0], [-8.884711682419383, 8.67054193209677, 0.0], [-8.948669168242862, 8.28315715332031, 0.0], [-8.98715353943162, 7.892418775380858, 0.0], [-9.0, 7.500000000000001, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}, "arc_2_2": {"type": 0, "surface": 0, "route_name": "", "left": {"type": 0, "segments": [[[-6.0, 7.500000000000001, 0.0], [-5.98715353943162, 7.1075812246191425, 0.0], [-5.948669168242862, 6.716842846679692, 0.0], [-5.884711682419383, 6.32945806790323, 0.0], [-5.79555495773441, 5.947085729384876, 0.0], [-5.681580776970634, 5.571363208181031, 0.0], [-5.54327719506772, 5.203899405809462, 0.0], [-5.38123644919613, 4.8462678586859935, 0.0], [-5.196152422706632, 4.500000000000002, 0.0], [-4.988817673815273, 4.166578601882389, 0.0], [-4.760120041747411, 3.847431425947676, 0.0], [-4.511038844873864, 3.543925109399587, 0.0], [-4.242640687119286, 3.2573593128807152, 0.0], [-3.956074890600414, 2.9889611551261366, 0.0], [-3.652568574052325, 2.7398799582525903, 0.0], [-3.3334213981176126, 2.511182326184729, 0.0], [-3.000000000000002, 2.3038475772933698, 0.0], [-2.653732141314008, 2.1187635508038705, 0.0], [-2.2961005941905417, 1.956722804932281, 0.0], [-1.9286367918189704, 1.818419223029366, 0.0], [-1.5529142706151233, 1.70444504226559, 0.0], [-1.1705419320967716, 1.6152883175806183, 0.0], [-0.7831571533203093, 1.551330831757138, 0.0], [-0.39241877538086123, 1.5128464605683796, 0.0], [-6.580929093825553e-16, 1.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "right": {"type": 0, "segments": [[[-9.0, 7.500000000000001, 0.0], [-8.99143399423672, 7.107425513711978, 0.0], [-8.96575228282571, 6.7155983152710785, 0.0], [-8.923003752364295, 6.325264270019538, 0.0], [-8.863269777109874, 5.937166400997629, 0.0], [-8.7866640640794, 5.552043474557074, 0.0], [-8.693332436601615, 5.1706285940773125, 0.0], [-8.583452556734043, 4.793647804461543, 0.0], [-8.457233587073176, 4.421818710068982, 0.0], [-8.314915792601582, 4.0558491087141935, 0.0], [-8.15677008332985, 3.6964356443337065, 0.0], [-7.983097498603996, 3.344262480884696, 0.0], [-7.79422863405995, 3.0000000000000027, 0.0], [-7.590523012315971, 2.6643035248785853, 0.0], [-7.372368398600926, 2.3378120728405847, 0.0], [-7.140180062621116, 2.0211471389215143, 0.0], [-6.894399988070802, 1.7149115128211463, 0.0], [-6.635496031291117, 1.4196881314590586, 0.0], [-6.363961030678929, 1.1360389693210724, 0.0], [-6.080311868540943, 0.8645039687088856, 0.0], [-5.7850884871788555, 0.6056000119291989, 0.0], [-5.478852861078488, 0.3598199373788855, 0.0], [-5.162187927159417, 0.12763160139907548, 0.0], [-4.835696475121418, -0.09052301231596971, 0.0], [-4.5000000000000036, -0.29422863405994537, 0.0], [-4.155737519115309, -0.48309749860399354, 0.0], [-3.8035643556662926, -0.6567700833298495, 0.0], [-3.444150891285813, -0.8149157926015782, 0.0], [-3.078181289931017, -0.9572335870731763, 0.0], [-2.706352195538464, -1.0834525567340396, 0.0], [-2.3293714059226858, -1.1933324366016151, 0.0], [-1.9479565254429254, -1.2866640640793996, 0.0], [-1.562833599002373, -1.3632697771098723, 0.0], [-1.1747357299804646, -1.423003752364293, 0.0], [-0.7844016847289242, -1.4657522828257097, 0.0], [-0.39257448628802516, -1.4914339942367203, 0.0], [-1.6532731788489267e-15, -1.5, 0.0]]], "crossable": false, "elevation": null, "height": null}, "center": null, "begin": null, "end": null}}, "regions": {"lane0_parallel_parking_lot_1": {"type": 2, "outline": [[4.0, -1.5], [4.0, -4.2], [9.5, -4.2], [9.5, -1.5]], "crossable": true}, "lane0_parallel_parking_lot_2": {"type": 2, "outline": [[9.5, -1.5], [9.5, -4.2], [15.0, -4.2], [15.0, -1.5]], "crossable": true}, "lane0_parallel_parking_lot_3": {"type": 2, "outline": [[15.0, -1.5], [15.0, -4.2], [20.5, -4.2], [20.5, -1.5]], "crossable": true}, "lane0_parallel_parking_lot_4": {"type": 2, "outline": [[20.5, -1.5], [20.5, -4.2], [26.0, -4.2], [26.0, -1.5]], "crossable": true}, "lane1_parallel_parking_lot_1": {"type": 2, "outline": [[9.5, 4.5], [15.0, 4.5], [15.0, 7.2], [9.5, 7.2]], "crossable": true}, "lane1_parallel_parking_lot_2": {"type": 2, "outline": [[15.0, 4.5], [20.5, 4.5], [20.5, 7.2], [15.0, 7.2]], "crossable": true}, "lane1_parallel_parking_lot_3": {"type": 2, "outline": [[-1.5, 13.5], [4.0, 13.5], [4.0, 16.2], [-1.5, 16.2]], "crossable": true}, "lane2_parallel_parking_lot_4": {"type": 2, "outline": [[4.0, 13.5], [9.5, 13.5], [9.5, 16.2], [4.0, 16.2]], "crossable": true}, "lane2_parallel_parking_lot_5": {"type": 2, "outline": [[9.5, 13.5], [15.0, 13.5], [15.0, 16.2], [9.5, 16.2]], "crossable": true}, "lane2_parallel_parking_lot_6": {"type": 2, "outline": [[15.0, 13.5], [20.5, 13.5], [20.5, 16.2], [15.0, 16.2]], "crossable": true}, "lane2_parallel_parking_lot_7": {"type": 2, "outline": [[20.5, 13.5], [26.0, 13.5], [26.0, 16.2], [20.5, 16.2]], "crossable": true}, "lane2_parallel_parking_lot_8": {"type": 2, "outline": [[26.0, 13.5], [31.5, 13.5], [31.5, 16.2], [26.0, 16.2]], "crossable": true}}, "signs": {}, "static_obstacles": {}, "connections": []}} \ No newline at end of file diff --git a/GEMstack/onboard/planning/RRT.py b/GEMstack/onboard/planning/RRT.py new file mode 100644 index 000000000..c003648db --- /dev/null +++ b/GEMstack/onboard/planning/RRT.py @@ -0,0 +1,320 @@ +import numpy as np +import random +import math +import time +import yaml +from typing import Optional +import os + +class Obstacle: + def __init__(self,x=0,y=0,r=0.2): + self.x = x + self.y = y + self.radius = r + +class Point: + def __init__(self,x=0,y=0,heading = None): + self.x = x + self.y = y + self.heading = heading # in radian + self.parent = None + self.cost = float('inf') # Cost to reach this node + +class BiRRT: + def __init__(self, lane_boundary : list, map_boundary : list, update_rate : Optional[float] = None): + + self.path = [] + self.tree_from_start = [] + self.tree_from_end = [] + + yaml_path = "GEMstack/knowledge/defaults/rrt_param.yaml" + with open(yaml_path,'r') as file: + params = yaml.safe_load(file) + + # min distace of vehicle center to obstacle + # should be roughly 1/2 of vehicle width + self.vehicle_half_width = params['vehicle']['half_width'] # meter + + # angle limit for vehicle turning per step size + self.heading_limit = params['vehicle']['heading_limit'] # limit the heading change in route + + # max search time + if update_rate is None: + self.time_limit = params['rrt']['time_limit'] # sec + else: + self.time_limit = 1.0 / update_rate + + # step size for local planner + self.step_size = params['rrt']['step_size'] # meter + + # radius for determine neighbor node + self.search_r = params['rrt']['search_r'] # meter + + # Map boundary in meter + # self.MAP_X_LOW = params['map']['lower_x'] + # self.MAP_X_HIGH = params['map']['upper_x'] + # self.MAP_Y_LOW = params['map']['lower_y'] + # self.MAP_Y_HIGH = params['map']['upper_y'] + self.MAP_X_LOW = map_boundary[0] + self.MAP_X_HIGH = map_boundary[1] + self.MAP_Y_LOW = map_boundary[2] + self.MAP_Y_HIGH = map_boundary[3] + + self.lane_boundary_radius = params['map']['lane_boundary_radius'] # meter + self.obstacle_radius = params['map']['obstacle_radius'] + + # occupency grid + self.grid_lane = None + self.grid = None + self.grid_resolution = params['map']['grid_resolution'] # grids per meter + # the coordiante in start frame where in occupency grid is (0,0) + self.map_zero = [self.MAP_X_LOW , self.MAP_Y_LOW] + # initialize occupency grid with lane boundary + self.init_grid(lane_boundary) + + def update_grid(self, obstacle_list): + self.grid = self.grid_lane.copy() + + margin_low = -round((self.obstacle_radius + self.vehicle_half_width)*self.grid_resolution) + margin_high = round((self.obstacle_radius + self.vehicle_half_width)*self.grid_resolution) + for obstacle in obstacle_list : + obstacle_center = [round((obstacle[0]-self.map_zero[0])*self.grid_resolution), + round((obstacle[1]-self.map_zero[1])*self.grid_resolution)] + + self.grid[obstacle_center[0],obstacle_center[1]] = 1 + for x_margin in range(margin_low,margin_high): + for y_margin in range(margin_low,margin_high): + self.grid[obstacle_center[0] + x_margin, obstacle_center[1] + y_margin] = 1 + + # Build occupency grid from lane boundary + def init_grid(self, lane_boundary): + grid_height = (self.MAP_Y_HIGH - self.MAP_Y_LOW)*self.grid_resolution + grid_width = (self.MAP_X_HIGH - self.MAP_X_LOW)*self.grid_resolution + self.grid_lane = np.zeros((round(grid_width),round(grid_height))) + + margin_low = -round((self.lane_boundary_radius + self.vehicle_half_width)*self.grid_resolution) + margin_high = round((self.lane_boundary_radius + self.vehicle_half_width)*self.grid_resolution) + for obstacle in lane_boundary : + obstacle_center = [round((obstacle[0]-self.map_zero[0])*self.grid_resolution), + round((obstacle[1]-self.map_zero[1])*self.grid_resolution)] + + self.grid_lane[obstacle_center[0],obstacle_center[1]] = 1 + for x_margin in range(margin_low,margin_high): + for y_margin in range(margin_low,margin_high): + self.grid_lane[obstacle_center[0] + x_margin, obstacle_center[1] + y_margin] = 1 + + + def search(self, start : list, goal : list, obstacle_list : list): + + # updats grid with obstacles + self.update_grid(obstacle_list) + + # start position (current vehicle position) + self.start_point = Point(start[0],start[1],start[2]) + self.start_point.cost = 0 + + # desire position (reverse heading for building tree) + # (will revert back after route found) + self.end_point = Point(goal[0],goal[1],self.angle_inverse(goal[2])) + self.end_point.cost = 0 + + self.path = [] + # initialize two tree + self.tree_from_start = [] + self.tree_from_end = [] + self.tree_from_start.append(self.start_point) + self.tree_from_end.append(self.end_point) + + start_time = time.time() + + # perform search within time limit + while ((time.time()-start_time) <= self.time_limit): + # uniformly sample a point within in the map + sample_p = Point(random.uniform(self.MAP_X_LOW,self.MAP_X_HIGH),random.uniform(self.MAP_Y_LOW,self.MAP_Y_HIGH)) + Direction = None + # update the tree form start or tree from end with 1/2 probability + rand_num = random.uniform(0.0, 1.0) + if rand_num > 0.5: + tree_a = self.tree_from_start + tree_b = self.tree_from_end + Direction = "forward" + else: + tree_a = self.tree_from_end + tree_b = self.tree_from_start + Direction = "backward" + + # print(Direction + " AT {} Interation".format(iterration)) + + # find nearest point in the tree + nearest_point_a = self.Nearest(tree_a, sample_p) + # use local planner to move one step size + new_p = self.LocalPlanner(nearest_point_a, sample_p, self.step_size) + # check collision + if not self.is_valid(new_p): + continue + # check if there exist previous point with less cost to new point + neighbor_points = self.Neighbors(new_p,tree_a) + min_cost = nearest_point_a.cost + self.distance(new_p,nearest_point_a) + parent_p = nearest_point_a + for point in neighbor_points: + curr_cost = point.cost + self.distance(point, new_p) + if curr_cost < min_cost: + min_cost = curr_cost + parent_p = point + # update point's paraent + new_p.cost = min_cost + new_p.parent = parent_p + new_p.heading = self.heading(parent_p,new_p) + + # check heading limit and collision + if self.angle_diff(new_p.heading,new_p.parent.heading) > (self.heading_limit): + continue + if not self.is_valid(new_p): + continue + + # point is valid, add to tree + tree_a.append(new_p) + + # rewire tree to smooth the route + for point in neighbor_points: + if point == parent_p: + continue + if new_p.cost + self.distance(new_p,point) < point.cost: + # check heading limit + if self.angle_diff(new_p.heading,point.heading) > (self.heading_limit): + continue + if self.angle_diff(new_p.heading,self.heading(new_p,point)) > (self.heading_limit): + continue + if self.angle_diff(point.heading,self.heading(new_p,point)) > (self.heading_limit): + continue + point.parent = new_p + point.cost = new_p.cost + self.distance(new_p, point) + point.heading = self.heading(new_p,point) + + # find nearest point in another tree + nearest_point_b = self.Nearest(tree_b, new_p) + + # check if two tree can be connected + if self.distance(new_p,nearest_point_b) > self.step_size: + continue + # check heading limit + if self.angle_diff(new_p.heading,self.angle_inverse(nearest_point_b.heading)) > (self.heading_limit): + continue + if self.angle_diff(new_p.heading,self.heading(new_p,nearest_point_b)) > (self.heading_limit): + continue + if self.angle_diff(nearest_point_b.heading,self.heading(nearest_point_b,new_p)) > (self.heading_limit): + continue + + # check if there exist another point that can connect two tree with less cost + neighbor_points = self.Neighbors(new_p,tree_b) + min_cost = new_p.cost + nearest_point_b.cost + self.distance(new_p,nearest_point_a) + for point in neighbor_points: + curr_cost = new_p.cost + point.cost + self.distance(point, new_p) + if curr_cost < min_cost: + if self.angle_diff(new_p.heading,self.angle_inverse(point.heading)) > (self.heading_limit): + continue + if self.angle_diff(new_p.heading,self.heading(new_p,point)) > (self.heading_limit): + continue + if self.angle_diff(point.heading,self.heading(point,new_p)) > (self.heading_limit): + continue + min_cost = curr_cost + nearest_point_b = point + # generate a route from start point to end point + self.trace_path(new_p,nearest_point_b) + return self.path + + print("========== route not found ==========") + return [] + + # if the distance of point in the tree and sample point is less or equal to search radius + # it is considered as a neighbor of sample point + def Neighbors(self,sample_p,tree): + neighbor_points = [] + for point in tree: + if self.distance(point, sample_p) <= self.search_r: + neighbor_points.append(point) + return neighbor_points + + # the relation of two tree is opsite, revert one of them + def trace_path(self,point_a,point_b): + path_start = [] + path_end = [] + + if point_a not in self.tree_from_start: + point_temp = point_a + point_a = point_b + point_b = point_temp + + while point_a is not None: + path_start.append([point_a.x,point_a.y,point_a.heading]) + point_a = point_a.parent + while point_b is not None: + point_b.heading = self.angle_inverse(point_b.heading) + path_end.append([point_b.x,point_b.y,point_b.heading]) + point_b = point_b.parent + + self.path = path_start[::-1] + path_end + # plt.plot(self.path[len(path_a)].x,self.path[len(path_a)].y,'yo') + + # return euclidean distance between two point/obstacle + def distance(self,a,b): + return math.sqrt(math.pow(a.x-b.x,2) + math.pow(a.y-b.y,2)) + + # return angel within -pi to pi + def angle_norm(self,angle): + return (angle + math.pi) % (2 * math.pi) - math.pi + + # return absolute value of angle difference within 0 to pi + def angle_diff(self,a,b): + a = self.angle_norm(a) + b = self.angle_norm(b) + diff = a - b + return abs(self.angle_norm(diff)) + + # return the angle in oppsite direction + def angle_inverse(self,angle): + angle = self.angle_norm(angle) + if angle < 0: + return angle + math.pi + return angle-math.pi + + # collision checking + def is_valid(self,point): + xi = round((point.x - self.map_zero[0])*self.grid_resolution) + yi = round((point.y - self.map_zero[1])*self.grid_resolution) + + if xi < 0 or yi < 0 or xi >= self.grid.shape[0] or yi >= self.grid.shape[1]: + print("out boundary") + return False # Out of bounds is considered collision + return 1 - self.grid[xi][yi] + + # return the nearest point in the tree + def Nearest(self,tree,sample_p): + min = 10000000 + nearest_p = None + for point in tree: + d = self.distance(point,sample_p) + if d < min: + min = d + nearest_p = point + return nearest_p + + # calculate the point heading based on parent point + def heading(self,parent,p2): + delta = np.array([p2.x,p2.y]) - np.array([parent.x,parent.y]) + return math.atan2(delta[1],delta[0]) + + # create a point that is one step size away from nearest point toward the sample point + def LocalPlanner(self,nearest_p,sample_p, step_size=0.5): + dist = self.distance(nearest_p,sample_p) + if dist < step_size: + return sample_p + direction = np.array([sample_p.x,sample_p.y]) - np.array([nearest_p.x,nearest_p.y]) + delta = (direction / dist) * step_size + new_p = Point() + new_p.x = nearest_p.x + delta[0] + new_p.y = nearest_p.y + delta[1] + new_p.parent = nearest_p + new_p.cost = dist + return new_p + \ No newline at end of file diff --git a/GEMstack/onboard/planning/mission_planning.py b/GEMstack/onboard/planning/mission_planning.py new file mode 100644 index 000000000..aa7075d4b --- /dev/null +++ b/GEMstack/onboard/planning/mission_planning.py @@ -0,0 +1,176 @@ +from typing import List, Union +from klampt.vis import scene +from ..component import Component +from ...utils import serialization +from ...state import Route, ObjectFrameEnum, AllState, VehicleState, Roadgraph, MissionObjective, MissionEnum, ObjectPose +import numpy as np +import requests +import json +import time + + +def check_distance(goal_pose: Union[ObjectPose, list], current_pose : ObjectPose): + if isinstance(goal_pose, ObjectPose): + goal = np.array([goal_pose.x, goal_pose.y]) + else: + goal = np.array([goal_pose[0], goal_pose[1]]) + current = np.array([current_pose.x, current_pose.y]) + return np.linalg.norm(goal - current) + + +class StateMachine: + def __init__(self, state_list : List = None): + self.state_list = state_list + self.state_index = 0 + self.initial_state = self.state_list[0] + self.current_state = self.state_list[self.state_index] + + def next_state(self): + if self.state_index < len(self.state_list) - 1: + self.state_index += 1 + return self.state_list[self.state_index] + else: + self.state_index = 0 + return self.state_list[0] + + +class SummoningMissionPlanner(Component): + def __init__(self, use_webapp, webapp_url, goal, state_machine): + self.state_machine = StateMachine([eval(s) for s in state_machine]) + self.goal_location = goal['location'] + self.goal_frame = goal['frame'] + self.new_goal = False + self.goal_pose = None + self.start_pose = None + self.start_time = time.time() + + self.flag_use_webapp = use_webapp + self.url_status = f"{webapp_url}/api/status" + self.url_summon = f"{webapp_url}/api/summon" + + if self.flag_use_webapp: + # Initialize the state in the server + data = { + "status": "IDLE" + } + response = requests.post(url=self.url_status, json=data) + if response.status_code == 200: + print("Status updated successfully") + else: + print("Failed to update status:", response.status_code) + data = { + "lat": 0, + "lon": 0 + } + response = requests.post(url=self.url_summon, json=data) + if response.status_code == 200: + print("Initialize goal location successfully") + else: + print("Failed to initialize goal location:", response.status_code) + + def state_inputs(self): + return ['all'] + + def state_outputs(self) -> List[str]: + return ['mission'] + + def rate(self): + return 1.0 + + def update(self, state: AllState): + vehicle = state.vehicle + mission = state.mission + route = state.route + + if self.flag_use_webapp: + goal_location = None + goal_frame = None + response = requests.get(self.url_summon) + print("GET:", response) + if response.status_code == 200: + data = response.json() + if data['lat'] == 0 and data['lon'] == 0: # TODO: lat and lon equal to 0 is meaningful, should change + print("No goal location received") + goal_location = None + goal_frame = None + else: + goal_location = [data['lat'] , data['lon']] + goal_frame = 'global' + print("Goal location:", goal_location) + print("Goal frame:", goal_frame) + + if self.goal_location == goal_location: + self.new_goal = False + else: + self.new_goal = True + self.goal_location = goal_location + self.goal_frame = goal_frame + else: + self.new_goal = True + goal_location = self.goal_location + goal_frame = self.goal_frame + + if goal_frame == 'global': + self.goal_pose = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1], + frame=ObjectFrameEnum.GLOBAL) + elif goal_frame == 'cartesian': + self.goal_pose = ObjectPose(t=time.time(), x=goal_location[0], y=goal_location[1], + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN) + elif goal_frame == 'start': + self.goal_pose = ObjectPose(t=time.time() - self.start_time, x=goal_location[0], y=goal_location[1], + frame=ObjectFrameEnum.START) + elif goal_frame is None: + pass + else: + raise ValueError("Invalid frame argument") + + if self.goal_pose: + self.goal_pose = self.goal_pose.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose) + + # Initiate state + if mission is None: + mission = MissionObjective() + mission.type = self.state_machine.initial_state + + # Receive goal location from the server and start driving + elif mission.type == MissionEnum.IDLE: + if self.new_goal: + mission.goal_pose = self.goal_pose + mission.type = self.state_machine.next_state() + print("============== Next state:", mission.type) + + # Reach the end of the route, begin to search for parking + elif mission.type == MissionEnum.SUMMONING_DRIVE: + mission.goal_pose = self.goal_pose + if route: + _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False) + if closest_index == len(route.points) - 1 or check_distance(mission.goal_pose, vehicle.pose) < 1: + mission.type = self.state_machine.next_state() + print("============== Next state:", mission.type) + + # Finish parking, back to idle and wait for the next goal location + elif mission.type == MissionEnum.PARALLEL_PARKING: + if route: + _, closest_index = route.closest_point([vehicle.pose.x, vehicle.pose.y], edges=False) + if closest_index == len(route.points) - 1 or check_distance(route.points[-1], vehicle.pose) < 0.1: + mission.type = self.state_machine.next_state() + self.goal_pose = None + mission.goal_pose = self.goal_pose + print("============== Next state:", mission.type) + + else: + raise ValueError("Invalid mission type") + + + if self.flag_use_webapp: + data = { + "status": mission.planner_type.name + } + print("POST:", data) + response = requests.post(url=self.url_status, json=data) + if response.status_code == 200: + print("Status updated successfully") + else: + print("Failed to update status:", response.status_code) + + return mission \ No newline at end of file diff --git a/GEMstack/onboard/planning/reeds_shepp_parking.py b/GEMstack/onboard/planning/reeds_shepp_parking.py new file mode 100644 index 000000000..1d95f48b1 --- /dev/null +++ b/GEMstack/onboard/planning/reeds_shepp_parking.py @@ -0,0 +1,828 @@ +from typing import List +import numpy as np +from typing import List +import reeds_shepp +from shapely.geometry import Polygon +from typing import List, Tuple +import math +import yaml + + +Pose = Tuple[float, float, float] # (x, y, yaw) +Dims = Tuple[float, float] # (width, length) +Obstacle = Tuple[float, float, float, Dims] # (x, y, yaw, (width, length)) + +class ReedsSheppParking: + def __init__(self): + + self.detected_cones = [] + self.parked_cars = [] + self.objects_to_avoid_collisions = [] + + yaml_path = "GEMstack/knowledge/defaults/ReedsShepp_param.yaml" + with open(yaml_path,'r') as file: + params = yaml.safe_load(file) + + self.static_horizontal_curb_xy_coordinates = None + self.static_horizontal_curb_size = params['reeds_shepp_parking']['static_horizontal_curb_size'] + self.add_static_vertical_curb_as_obstacle = params['reeds_shepp_parking']['add_static_vertical_curb_as_obstacle'] + + self.static_vertical_curb_size = params['reeds_shepp_parking']['static_vertical_curb_size'] + self.static_vertical_curb_xy_coordinates = [] + self.add_static_horizontal_curb_as_obstacle = params['reeds_shepp_parking']['add_static_horizontal_curb_as_obstacle'] + + self.all_parking_spots_in_parking_lot_var = [] + + self.vehicle_pose = [0,0,0] # default + self.x_axis_of_search = self.vehicle_pose[0] + + + self.vehicle_dims = params['vehicle']['vehicle_dim'] + self.vehicle_turning_radius = params['vehicle']['vehicle_turning_radius'] + self.compact_parking_spot_size = params['reeds_shepp_parking']['compact_parking_spot_size'] + self.shift_from_center_to_rear_axis = params['reeds_shepp_parking']['shift_from_center_to_rear_axis'] # TODO: Check + self.search_step_size = params['reeds_shepp_parking']['search_step_size'] + self.parking_lot_axis_shift_margin = params['reeds_shepp_parking']['parking_lot_axis_shift_margin'] + self.search_bound_threshold = params['reeds_shepp_parking']['search_bound_threshold'] + # TODO: Add thrid option: park in the middle + self.closest = params['reeds_shepp_parking']['closest'] # If True, the closest parking spot will be selected, otherwise the farthest one will be selected + self.clearance_step = params['reeds_shepp_parking']['clearance_step'] + self.clearance = params['reeds_shepp_parking']['clearance'] + self.search_axis_direction_var = False + + + + def reeds_shepp_path(self,start_pose, final_pose, step_size, vehicle_turning_radius):# Runing + path = reeds_shepp.path_sample(start_pose, final_pose, vehicle_turning_radius, step_size) + waypoints = [(x, y, yaw) for x, y, yaw, *_ in path] + #waypoints = np.array(waypoints_for_obstacles_check)[:,:2] + return waypoints + + def rectangle_polygon(self, center: Pose, dims: Dims) -> Polygon: + """ + Build a shapely Polygon for an oriented rectangle. + + Args: + center: (x, y, yaw) of rectangle center in world frame. + dims: (width, length) of rectangle. + + Returns: + shapely Polygon of the 4 corners, in CCW order. + """ + x, y, yaw = center + w, L = dims + # corners in vehicle frame (forward is +x, left is +y) + corners = np.array([ + [ +L/2, +w/2], + [ +L/2, -w/2], + [ -L/2, -w/2], + [ -L/2, +w/2], + ]) + # rotation matrix + R = np.array([[np.cos(yaw), -np.sin(yaw)], + [np.sin(yaw), np.cos(yaw)]]) + # rotate & translate + world_corners = (R @ corners.T).T + np.array([x, y]) + return Polygon(world_corners) + + def is_trajectory_collision_free(self, + trajectory: List[Pose], + vehicle_dims: Dims, + obstacles: List[Obstacle] + ) -> bool: + """ + Check whether following `trajectory` will avoid all `obstacles`. + + Args: + trajectory: List of (x, y, yaw) vehicle poses along planned path. + vehicle_dims: (width, length) of your vehicle rectangle. + obstacles: List of static obstacles, each as (x, y, yaw, (width, length)). + + Returns: + True if no pose along the trajectory intersects any obstacle; False otherwise. + """ + # Pre-build obstacle polygons once + obstacle_polys = [ + self.rectangle_polygon((ox, oy, oyaw), dims) + for ox, oy, oyaw, dims in obstacles + ] + + # For each pose along the trajectory, test intersection + for pose in trajectory: + veh_poly = self.rectangle_polygon(pose, vehicle_dims) + for obs_poly in obstacle_polys: + if veh_poly.intersects(obs_poly): + # collision detected + return False + # no collisions anywhere + return True + + + def find_parking_positions(self, + obstacles: List[Tuple[float, float, float, Tuple[float, float]]], + vehicle_dims: Tuple[float, float], + margin: float = 0.2 + ) -> List[Tuple[float, float, float]]: + """ + Compute all the parking‐slot center positions and orientations along a single lane + where a vehicle of given dimensions can fit between existing obstacles. + + Parameters + ---------- + obstacles : List of (x, y, theta, (w, l)) + Each obstacle is centered at (x,y), oriented by yaw theta (radians), + and has width w (across lane) and length l (along lane). + vehicle_dims : (w_v, l_v) + Width and length of the vehicle (same convention as obstacles). + margin : float + Minimum clearance to leave between parked vehicles (in meters). + + Returns + ------- + slots : List of (x_c, y_c, theta_c) + Center‐points and yaw for each available parking slot. + """ + if not obstacles: + return [] + + # Assume all obstacles lie along one straight lane with the same orientation + lane_theta = obstacles[0][2] + cos_t = math.cos(lane_theta) + sin_t = math.sin(lane_theta) + + # Project each obstacle center onto the lane axis (s-coordinate) + projected = [] + for x, y, theta, (w, l) in obstacles: + # s = x*cos + y*sin + s = x * cos_t + y * sin_t + half_len = l / 2.0 + projected.append((s, half_len, (x, y))) + + # Sort by increasing s + projected.sort(key=lambda e: e[0]) + + slots: List[Tuple[float, float, float]] = [] + w_v, l_v = vehicle_dims + + # Examine gaps between consecutive obstacles + for (s_i, half_i, (x_i, y_i)), (s_j, half_j, (x_j, y_j)) in zip(projected, projected[1:]): + # compute start/end of free interval in s‐space + free_start = s_i + half_i + margin/2 + free_end = s_j - half_j - margin/2 + free_length = free_end - free_start + + # How many cars of length l_v can we fit (with margin between them)? + if free_length >= l_v: + # spacing = car length + margin + spacing = l_v + margin + n_fit = int(math.floor((free_length + margin) / spacing)) + for k in range(n_fit): + # center s‐coordinate for each slot + s_c = free_start + (spacing * k) + (l_v / 2) + # map back to (x,y): x = s*cos, y = s*sin (plus any perpendicular offset if needed) + x_c = s_c * cos_t + y_c = s_c * sin_t + slots.append((x_c, y_c, lane_theta)) + + return slots + + def all_parking_spots_in_parking_lot(self, + static_horizontal_curb: List[Tuple[float, float, float, Dims]], + compact_parking_spot_size: Dims, + yaw_of_parked_cars_var: float = 0.0, + ) -> List[Pose]: + """ + Computes uniformly spaced parking spot centers along a curb line. + + Args: + static_horizontal_curb: List of 2 curb endpoints, each as (x, y, yaw, dims). + compact_parking_spot_size: (width, length) of parking spot. + yaw_of_parked_cars_var: orientation of parked cars (rad). + margin: optional spacing between adjacent spots. + + Returns: + List of (x, y, yaw) poses for each parking spot. + """ + if len(static_horizontal_curb) != 2: + raise ValueError("Exactly two curb endpoints are required.") + + # Extract center points of the curb rectangles + (x0, y0, _, dims0), (x1, y1, _, dims1) = static_horizontal_curb + L0 = dims0[1] + L1 = dims1[1] + + # Start and end points shifted to actual line segment ends + p0 = (x0 + math.cos(yaw_of_parked_cars_var) * L0 / 2, + y0 + math.sin(yaw_of_parked_cars_var) * L0 / 2) + p1 = (x1 - math.cos(yaw_of_parked_cars_var) * L1 / 2, + y1 - math.sin(yaw_of_parked_cars_var) * L1 / 2) + + dx = p1[0] - p0[0] + dy = p1[1] - p0[1] + total_length = math.hypot(dx, dy) + + spot_length = compact_parking_spot_size[1] + spacing = spot_length + + if total_length < spot_length: + raise ValueError("Insufficient curb length to place even one spot.") + + # Number of full spots that can fit along the curb + num_spots = int(math.floor(total_length / spacing)) + dir_x = dx / total_length + dir_y = dy / total_length + + parking_spots: List[Pose] = [] + for i in range(num_spots): + dist = (i + 0.5) * spacing # center of spot + x = p0[0] + dir_x * dist + y = p0[1] + dir_y * dist + parking_spots.append((x, y, yaw_of_parked_cars_var)) + + return parking_spots + + # def pick_parking_spot(available_parking_spots_var, x, y, yaw = 0.0, closest = True): + + # def distance(spot): + # dx = spot[0] - x + # dy = spot[1] - y + # return math.hypot(dx, dy) + + # if closest: + # return min(available_parking_spots_var, key=distance) + # else: + # return max(available_parking_spots_var, key=distance) + # + + def pick_parking_spot(self, + available_spots: List[Tuple[float, float, float]], + all_spots: List[Tuple[float, float, float]], + vehicle_pose: Tuple[float, float, float] + ) -> Tuple[float, float, float]: + """ + Maneuver-complexity-aware spot selection strategy + + Pick a parking spot based on how easily it can be entered/exited: + Priority 1: sandwiched between two available spots + Priority 2: adjacent to one available spot + Priority 3: isolated + + Break ties using distance to current vehicle pose. + """ + + # Helper: distance to current pose + def dist_to_vehicle(spot): + dx = spot[0] - vehicle_pose[0] + dy = spot[1] - vehicle_pose[1] + return math.hypot(dx, dy) + + # Build availability map (assume order of spots is spatially sorted) + spot_indices = {spot: i for i, spot in enumerate(all_spots)} + available_set = set(available_spots) + + ranked_spots = [] + + for spot in available_spots: + idx = spot_indices[spot] + + # Check neighbors + left_free = idx - 1 >= 0 and all_spots[idx - 1] in available_set + right_free = idx + 1 < len(all_spots) and all_spots[idx + 1] in available_set + + if left_free and right_free: + priority = 1 + elif left_free or right_free: + priority = 2 + else: + priority = 3 + + ranked_spots.append((priority, dist_to_vehicle(spot), spot)) + + # Sort: lower priority (1 best) and then by proximity + ranked_spots.sort() + + # Return the best one + return ranked_spots[0][2], ranked_spots[0][0] + + def search_axis_direction(self, parking_spot_to_go, vehicle_pose): + dx = parking_spot_to_go[0][0] - vehicle_pose[0] + if dx > 0: + return True + else: + return False + + # def available_parking_spots(all_parking_spots_in_parking_lot, parked_cars, compact_parking_spot_size, yaw_of_parked_cars_var=0.0): + # available_parking_spots = [] + # for spot in all_parking_spots_in_parking_lot: + # # Check if the parking spot is occupied by a parked car + # is_occupied = False + # for parked_car in parked_cars: + # if (abs(parked_car[0] - spot[0]) < compact_parking_spot_size[1] / 2) and (abs(parked_car[1] - spot[1]) < compact_parking_spot_size[0] / 2): + # is_occupied = True + # break + + # # If the parking spot is not occupied, add it to the available list + # if not is_occupied: + # available_parking_spots.append(spot) + + # return available_parking_spots + + def available_parking_spots(self, + all_parking_spots: List[Pose], + parked_cars: List[Obstacle], + spot_dims: Dims + ) -> List[Pose]: + """ + Returns unoccupied parking spots, using full geometric rectangle checks. + + Args: + all_parking_spots: List of (x, y, yaw) tuples. + parked_cars: List of (x, y, yaw, dims) tuples. + spot_dims: (width, length) of parking spots. + clearance: Extra buffer (in meters) to apply around cars when checking overlap. + + Returns: + List of available parking spot poses. + """ + clearance = self.clearance + + spot_polygons = [ + self.rectangle_polygon(spot, spot_dims) + for spot in all_parking_spots + ] + + car_polygons = [ + self.rectangle_polygon((x, y, yaw), dims) + for x, y, yaw, dims in parked_cars + ] + + available = [] + for spot, poly in zip(all_parking_spots, spot_polygons): + # Check for collision with any parked car + is_occupied = any(poly.intersects(car_poly.buffer(clearance)) for car_poly in car_polygons) + if not is_occupied: + available.append(spot) + + return available + + def yaw_of_parked_cars(self, curb_0, curb_1): + # Compute the vector v from p1 to p2 + # v_x = x2 - x1, v_y = y2 - y1 + v = (curb_1[0] - curb_0[0], curb_1[1] - curb_0[1]) + angle_rad = math.atan2(v[1], v[0]) # TODO: Double check if CCW is the positive direction + return angle_rad + + def shift_points_perpendicular_ccw(self, p1, p2, shift_amount): + """ + Shift points p1 and p2 by a given amount perpendicular (to the left) + of the vector from p1 to p2. + + Args: + p1 (tuple of float): First point (x1, y1) + p2 (tuple of float): Second point (x2, y2) + shift_amount (float): Distance to shift perpendicular to the left of vector v + + Returns: + p1_shifted (tuple of float): Shifted first point + p2_shifted (tuple of float): Shifted second point + dir_unit (tuple of float): Normalized direction vector v̂ = (v_x, v_y) / |v| + shift_vec (tuple of float): Actual shift vector applied = perp_unit * shift_amount + """ + x1, y1 = p1 + x2, y2 = p2 + + # 1) Compute connecting vector v = p2 - p1 + v_x = x2 - x1 + v_y = y2 - y1 + + # 2) Compute its magnitude |v| + length = math.hypot(v_x, v_y) + if length == 0: + raise ValueError("p1 and p2 must be distinct points to define a direction.") + + # 3) Normalize v to get unit direction v̂ = (v_x, v_y) / |v| + dir_x = v_x / length + dir_y = v_y / length + + v_norm = (dir_x, dir_y) + + # 4) Compute the left-perpendicular unit vector: perp = (-dir_y, dir_x) + perp_x = -dir_y + perp_y = dir_x + + # 5) Scale this perpendicular by the desired shift_amount + shift_x = perp_x * shift_amount + shift_y = perp_y * shift_amount + + # 6) Apply shift to both points + p1_shifted = (x1 + shift_x, y1 + shift_y) + p2_shifted = (x2 + shift_x, y2 + shift_y) + + # Return shifted points + return p1_shifted, p2_shifted, v_norm + + + + def project_point_on_axis(self, p1, p2, p3): + """ + Project point p3 orthogonally onto the line (axis) defined by p1 -> p2. + + Args: + p1 (tuple of float): First point on the axis (x1, y1) + p2 (tuple of float): Second point on the axis (x2, y2) + p3 (tuple of float): The point to be projected (x3, y3) + + Returns: + p_proj (tuple of float): Coordinates of the projection of p3 onto the line p1–p2 + t (float): The parameter along the line (0 at p1, 1 at p2, can be outside [0,1]) + """ + x1, y1 = p1 + x2, y2 = p2 + x3, y3 = p3 + + # Compute vector along the axis v = p2 - p1 + v_x = x2 - x1 + v_y = y2 - y1 + + # Compute vector from p1 to p3: u = p3 - p1 + u_x = x3 - x1 + u_y = y3 - y1 + + # Compute dot products + dot_uv = u_x * v_x + u_y * v_y # u · v + dot_vv = v_x * v_x + v_y * v_y # v · v + + if dot_vv == 0: + raise ValueError("p1 and p2 must be distinct to define an axis.") + + # Parameter t gives the position along the line: p_proj = p1 + t * v + t = dot_uv / dot_vv + + # Compute projected point coordinates + proj_x = x1 + t * v_x + proj_y = y1 + t * v_y + + return (proj_x, proj_y) + + + + def move_point_along_vector(self, p0, direction, step, positive_direction=True): + """ + Move the point p0 by a fixed step along the given direction vector. + + Args: + p0 (tuple of float): The starting point (x0, y0). + direction (tuple of float): The direction vector (dx, dy). + step (float): Distance to move along the direction (default 0.1). + + Returns: + tuple of float: The new point (x_new, y_new) after moving. + Raises: + ValueError: if the direction vector has zero length. + """ + x0, y0 = p0 + dx, dy = direction + + # Compute the length of the direction vector + length = math.hypot(dx, dy) + if length == 0: + raise ValueError("Direction vector must be non-zero to define a movement direction.") + + # Normalize the direction vector to unit length + ux = dx / length + uy = dy / length + + # Move the point by 'step' along the unit direction + if positive_direction: + x_new = x0 + ux * step + y_new = y0 + uy * step + else: + x_new = x0 - ux * step + y_new = y0 - uy * step + + return (x_new, y_new) + + + def stitch_paths(self, pose_list: List[Pose], step_size: float, turning_radius: float) -> List[Pose]: + """ + Given a list of poses, compute and stitch Reeds-Shepp paths between consecutive poses. + + Args: + pose_list (List[Pose]): List of poses [(x0, y0, yaw0), (x1, y1, yaw1), ...] + step_size (float): Step size for sampling Reeds-Shepp path + turning_radius (float): Vehicle turning radius + + Returns: + List[Pose]: Concatenated list of all waypoints across all segments. + """ + stitched_path = [] + for i in range(len(pose_list) - 1): + segment = self.reeds_shepp_path( + pose_list[i], + pose_list[i+1], + step_size=step_size, + vehicle_turning_radius=turning_radius + ) + # Avoid duplicating overlapping pose except for the first segment + if i > 0: + segment = segment[1:] + stitched_path.extend(segment) + return stitched_path + + + def find_available_parking_spots_and_search_vector(self, detected_cones=[], vehicle_pose=(0.0, 0.0, 0.0)): + # Update detected cones and vehicle pose + self.detected_cones = detected_cones + self.vehicle_pose = vehicle_pose + + + + # Compute yaw of parked cars based on static horizontal curb direction + self.yaw_of_parked_cars_var = self.yaw_of_parked_cars(self.static_horizontal_curb_xy_coordinates[0], self.static_horizontal_curb_xy_coordinates[1]) + + # Construct curb obstacle representations with yaw + self.static_horizontal_curb = [ + ( + self.static_horizontal_curb_xy_coordinates[0][0], + self.static_horizontal_curb_xy_coordinates[0][1], + self.yaw_of_parked_cars_var, + self.static_horizontal_curb_size + ), + ( + self.static_horizontal_curb_xy_coordinates[1][0], + self.static_horizontal_curb_xy_coordinates[1][1], + self.yaw_of_parked_cars_var, + self.static_horizontal_curb_size + ) + ] + if self.add_static_vertical_curb_as_obstacle: + self.static_vertical_curb = [ + ( + self.static_vertical_curb_xy_coordinates[0][0], + self.static_vertical_curb_xy_coordinates[0][1], + self.yaw_of_parked_cars_var, + self.static_vertical_curb_size + ) + ] + + # Convert cones to parked cars if any are detected + if self.detected_cones: + self.parked_cars = [(x, y, self.yaw_of_parked_cars_var, self.vehicle_dims) for x, y in self.detected_cones] + else: + self.parked_cars = self.detected_cones + + # Add all obstacles to the collision-checking list + self.objects_to_avoid_collisions += self.parked_cars + if self.add_static_horizontal_curb_as_obstacle: + self.objects_to_avoid_collisions += self.static_horizontal_curb + if self.add_static_vertical_curb_as_obstacle: + self.objects_to_avoid_collisions += self.static_vertical_curb + + # Compute full set of parking spots along the curb if not already available + if not self.all_parking_spots_in_parking_lot_var: + self.all_parking_spots_in_parking_lot_var = self.all_parking_spots_in_parking_lot( + self.static_horizontal_curb, + self.compact_parking_spot_size, + yaw_of_parked_cars_var=self.yaw_of_parked_cars_var + ) + + # Filter out occupied spots + self.available_parking_spots_var = self.available_parking_spots( + self.all_parking_spots_in_parking_lot_var, + self.parked_cars, + self.compact_parking_spot_size, + ) + print("self.available_parking_spots_var", self.available_parking_spots_var) + if not self.available_parking_spots_var: + return + #raise ValueError("No parking spot available.") + + # Select the best available parking spot + self.parking_spot_to_go, self.priority = self.pick_parking_spot( + self.available_parking_spots_var, + self.all_parking_spots_in_parking_lot_var, + self.vehicle_pose + ) + + self.parking_spot_to_go = [self.parking_spot_to_go] + # Adjust target position for rear-axle-centered model + x_shift = self.parking_spot_to_go[0][0] - self.shift_from_center_to_rear_axis + self.parking_spot_to_go[0] = ( + x_shift, + self.parking_spot_to_go[0][1], + self.parking_spot_to_go[0][2] + ) + + # Determine direction of search axis (forward or backward) + self.x_axis_of_search_direction_positive = self.search_axis_direction( + self.parking_spot_to_go, + self.vehicle_pose + ) + + # Build the shifted search axis and compute bounds + # TODO: Find search direction using yaw and vehicle pose + self.curb_0_xy_shifted, self.curb_1_xy_shifted, self.search_axis_direction_var = self.shift_points_perpendicular_ccw( + self.static_horizontal_curb_xy_coordinates[0], + self.static_horizontal_curb_xy_coordinates[1], + self.parking_lot_axis_shift_margin + ) + + # Horizontal axis search direction + _ , _, self.horizontal_search_axis_direction = self.shift_points_perpendicular_ccw( + self.static_horizontal_curb_xy_coordinates[0], + self.curb_0_xy_shifted, + self.parking_lot_axis_shift_margin + ) + + # Project vehicle pose onto the search axis + self.vehicle_pose_proj = self.project_point_on_axis( + self.curb_0_xy_shifted, + self.curb_1_xy_shifted, + self.vehicle_pose[0:2] + ) + + # Compute bounds for the search axis + self.upper_bound_xy = self.move_point_along_vector( + self.curb_1_xy_shifted, + self.search_axis_direction_var, + step=2 * self.compact_parking_spot_size[1], + positive_direction=True + ) + + self.lower_bound_xy = self.move_point_along_vector( + self.curb_0_xy_shifted, + self.search_axis_direction_var, + step=2 * self.compact_parking_spot_size[1], + positive_direction=False + ) + + + + + + def find_collision_free_trajectory_to_park(self, detected_cones=[], vehicle_pose=(0.0, 0.0, 0.0), update_pose=False): + # Update detected cones and optionally vehicle pose + self.detected_cones = detected_cones + if update_pose: + self.vehicle_pose = vehicle_pose + + if not self.available_parking_spots_var: + self.waypoints_to_go = [] + print("No parking spot available.") + return + # Try in both directions + directions = [self.x_axis_of_search_direction_positive, not self.x_axis_of_search_direction_positive] + for direction_flag in directions: + self.x_axis_of_search_direction_positive = direction_flag + self.vehicle_pose_proj = self.project_point_on_axis( + self.curb_0_xy_shifted, + self.curb_1_xy_shifted, + self.vehicle_pose[0:2] + ) + + + while True: + # Move projected pose along the search axis + self.vehicle_pose_proj = self.move_point_along_vector( + self.vehicle_pose_proj, + self.search_axis_direction_var, + step=self.search_step_size, + positive_direction=self.x_axis_of_search_direction_positive + ) + + # Compute the projected vehicle pose + start_proj = ( + self.vehicle_pose_proj[0],# - self.shift_from_center_to_rear_axis, + self.vehicle_pose_proj[1], + self.yaw_of_parked_cars_var + ) + + # Plan path in segments: + # If 3 parking spots are available, park with clearance + if self.priority == 1: + clearance_step = self.clearance_step + else: + clearance_step = 0.0 + + # Compute the parking spot minus some clearance + self.parking_spot_to_go_minus_clearance = self.move_point_along_vector( + self.parking_spot_to_go[0][0:2], + self.search_axis_direction_var, + step=clearance_step, # TODO: Pass as an input + positive_direction=False + ) + + # Adding yaw + self.parking_spot_to_go_minus_clearance = (self.parking_spot_to_go_minus_clearance[0], + self.parking_spot_to_go_minus_clearance[1], + self.yaw_of_parked_cars_var) + + # Poses to connect with reeds-shepp paths + waypoints_to_connect = [self.vehicle_pose, + start_proj, + self.parking_spot_to_go_minus_clearance, + self.parking_spot_to_go[0]] + + # Computing the reeds-shepp paths + self.waypoints_for_obstacles_check = self.stitch_paths( + waypoints_to_connect, + step_size=self.search_step_size, + turning_radius=self.vehicle_turning_radius + ) + # Extract waypoints to pass (removing yaw) + self.waypoints_to_go = np.array(self.waypoints_for_obstacles_check)[:, :2] + + # Check if trajectory is collision-free + if self.is_trajectory_collision_free( + self.waypoints_for_obstacles_check, + self.vehicle_dims, + self.objects_to_avoid_collisions + ): + return + + # Stop search if bounds are reached + dist_to_upper_bound = np.linalg.norm(np.array(self.vehicle_pose_proj) - np.array(self.upper_bound_xy)) + dist_to_lower_bound = np.linalg.norm(np.array(self.vehicle_pose_proj) - np.array(self.lower_bound_xy)) + if dist_to_upper_bound < self.search_bound_threshold or dist_to_lower_bound < self.search_bound_threshold: + + # TODO: Also implement the horizontal search axis direction by accumulating points in "waypoints_to_connect". + # If car can fit in the parking spot, then parking path should exits if there are + # no obstacles across holonomic paths. + # Use self.horizontal_search_axis_direction_var + break # Give up in this direction + + # If both directions fail + raise ValueError("No collision-free trajectory available in either direction for parking.") + + + + def find_collision_free_trajectory_to_unpark(self, detected_cones=[], vehicle_pose=(0.0, 0.0, 0.0), update_pose=False): + # Update detected cones and optionally vehicle pose + self.detected_cones = detected_cones + if update_pose: + self.vehicle_pose = vehicle_pose + + # Find current vehicle pose projected on the search axis + self.vehicle_pose_proj = self.project_point_on_axis( + self.curb_0_xy_shifted, + self.curb_1_xy_shifted, + self.vehicle_pose[0:2] + ) + + # Move projected pose along the search axis + self.vehicle_pose_proj = self.move_point_along_vector( + self.vehicle_pose_proj, + self.search_axis_direction_var, + step=self.compact_parking_spot_size[1]*2, + positive_direction=True + ) + + # Compute the projected vehicle pose by adding yaw + start_proj = ( + self.vehicle_pose_proj[0],# - self.shift_from_center_to_rear_axis, + self.vehicle_pose_proj[1], + self.yaw_of_parked_cars_var + ) + + + while True: + # Move projected pose along the search axis + self.vehicle_pose_proj = self.move_point_along_vector( + self.vehicle_pose_proj, + self.search_axis_direction_var, + step=self.search_step_size, + positive_direction=False + ) + + # Compute the projected vehicle pose + start_proj = ( + self.vehicle_pose_proj[0],# - self.shift_from_center_to_rear_axis, + self.vehicle_pose_proj[1], + self.yaw_of_parked_cars_var + ) + + waypoints_1 = self.reeds_shepp_path( + self.vehicle_pose, + start_proj, + step_size=self.search_step_size, + vehicle_turning_radius = self.vehicle_turning_radius + ) + self.waypoints_for_obstacles_check = waypoints_1 + self.waypoints_to_go = np.array(self.waypoints_for_obstacles_check)[:, :2] + + # Check if trajectory is collision-free + if self.is_trajectory_collision_free( + self.waypoints_for_obstacles_check, + self.vehicle_dims, + self.objects_to_avoid_collisions + ): + return + + # Stop search if bounds are reached + dist_to_upper_bound = np.linalg.norm(np.array(self.vehicle_pose_proj) - np.array(self.upper_bound_xy)) + dist_to_lower_bound = np.linalg.norm(np.array(self.vehicle_pose_proj) - np.array(self.lower_bound_xy)) + if dist_to_upper_bound < self.search_bound_threshold or dist_to_lower_bound < self.search_bound_threshold: + break # Give up in this direction + + # If both directions fail + raise ValueError("No collision-free trajectory available for unparking.") \ No newline at end of file diff --git a/GEMstack/onboard/planning/route_planning.py b/GEMstack/onboard/planning/route_planning.py index e88f43553..9bae3c24a 100644 --- a/GEMstack/onboard/planning/route_planning.py +++ b/GEMstack/onboard/planning/route_planning.py @@ -1,9 +1,12 @@ from typing import List from ..component import Component from ...utils import serialization -from ...state import Route,ObjectFrameEnum +from ...state import AllState, Roadgraph, Route, MissionEnum, ObjectFrameEnum, Path, ObjectPose, RoadgraphRegionEnum import os import numpy as np +from .RRT import BiRRT +from .reeds_shepp_parking import ReedsSheppParking + class StaticRoutePlanner(Component): """Reads a route from disk and returns it as the desired route.""" @@ -39,4 +42,339 @@ def rate(self): def update(self): return self.route - \ No newline at end of file + + +def get_lane_points_from_roadgraph(roadgraph: Roadgraph) -> List: + """ + Get all points of the lanes in a Roadgraph. + Ouput: A list of [x, y] + """ + lane_points = [] + for lane in roadgraph.lanes.values(): + if not lane.left.crossable: + for pts in lane.left.segments: + for pt in pts: + lane_points.append(pt[:2]) + if not lane.right.crossable: + for pts in lane.right.segments: + for pt in pts: + lane_points.append(pt[:2]) + return lane_points + + +def find_available_pose_in_lane(position: list, roadgraph: Roadgraph, goal_yaw=None, map_type='roadgraph'): + goal = np.array(position) + if map_type == 'roadgraph': + left_x, left_y = goal + right_x, right_y = goal + goal_lane = None + min_right_dist = np.inf + min_right_idx = None + for lane in roadgraph.lanes.values(): + for pts in lane.right.segments: + pts = np.array(pts) + dists = np.linalg.norm(pts[:, :2] - goal, axis=1) + min_right_idx = np.argmin(dists) + dist = dists[min_right_idx] + if dist < min_right_dist: + min_right_dist = dist + right_x, right_y, _ = pts[min_right_idx] + goal_lane = lane + + # Find the closest point in left boundary to the point in right boundary + min_left_dist = np.inf + for pts in goal_lane.left.segments: + pts = np.array(pts) + dists = np.linalg.norm(pts[:, :2] - np.array([right_x, right_y]), axis=1) + min_left_idx = np.argmin(dists) + dist = dists[min_left_idx] + if dist < min_left_dist: + left_x, left_y, _ = pts[min_left_idx] + + goal_x = (left_x + right_x) / 2 + goal_y = (left_y + right_y) / 2 + + if goal_yaw is None: + # Find orientation + if 0 < min_right_idx < len(pts) - 1: + tangent = pts[min_right_idx + 1] - pts[min_right_idx - 1] + elif min_right_idx == 0: + tangent = pts[1] - pts[0] + else: # idx == last point + tangent = pts[-1] - pts[-2] + tangent_unit = tangent / np.linalg.norm(tangent) + goal_yaw = np.arctan2(tangent_unit[1], tangent_unit[0]) + + return [goal_x, goal_y, goal_yaw] + + elif map_type == 'pointlist': + pts = np.array(roadgraph) + dists = np.linalg.norm(pts[:, :2] - goal, axis=1) + min_idx = np.argmin(dists) + if 0 < min_idx < len(pts) - 1: + if np.linalg.norm(pts[min_idx] - pts[min_idx - 1]) < np.linalg.norm(pts[min_idx + 1] - pts[min_idx]): + tangent = pts[min_idx] - pts[min_idx - 1] + else: + tangent = pts[min_idx + 1] - pts[min_idx] + elif min_idx == 0: + tangent = pts[1] - pts[0] + else: # idx == last point + tangent = pts[-1] - pts[-2] + tangent_unit = tangent / np.linalg.norm(tangent) + goal_yaw = np.arctan2(tangent_unit[1], tangent_unit[0]) + + return [goal[0], goal[1], goal_yaw] + + else: + raise ValueError('map_type must be one of "roadgraph", "pointlist"') + + +def find_closest_lane(position: list, roadgraph: Roadgraph, traffic_rule='right'): + position = np.array(position) + closest_lane = None + min_dist = np.inf + + for lane in roadgraph.lanes.values(): + if traffic_rule == 'right': + segments = lane.right.segments + else: + segments = lane.left.segments + for pts in segments: + pts = np.array(pts) + dists = np.linalg.norm(pts[:, :2] - position, axis=1) + min_idx = np.argmin(dists) + dist = dists[min_idx] + if dist < min_dist: + right_x, right_y, _ = pts[min_idx] + min_dist = dist + closest_lane = lane + return closest_lane + + +def find_parallel_parking_lots(roadgraph: Roadgraph, goal_pose: ObjectPose, max_lane_to_parking_lot_gap=1.0): + # Find the lane where the goal position is. + goal_lane = find_closest_lane([goal_pose.x, goal_pose.y], roadgraph) + goal_lane_points = np.array(goal_lane.right.segments[0]) + + # Find the parking lots that attached to the lane + parking_lots = [] + for region in roadgraph.regions.values(): + if region.type == RoadgraphRegionEnum.PARKING_LOT: + for pt in region.outline: + dist = np.linalg.norm(goal_lane_points[:, :2] - np.array(pt[:2]), axis=1) + if np.min(dist) < max_lane_to_parking_lot_gap: + parking_lots.append(region.outline) + break + + # Find the closest and farthest parking lots and the middle points of the start and end curves as the parking area + # Assume that the closest lot is close to the start of the lane, and the farthest lot is close to the end. + closest_lot = None + farthest_lot = None + closest_start_point = None + closest_start_index = None + closest_end_point = None + closest_end_index = None + min_start_dist = np.inf + min_end_dist = np.inf + parking_area_start_end = None + + def next_point(index, outline, direction='ccw'): + if direction == 'ccw': + next_index = index + 1 + else: + next_index = index - 1 + if next_index == len(outline): + next_index = 0 + return outline[next_index] + + if len(parking_lots) > 0: + for outline in parking_lots: + for idx, pt in enumerate(outline): + start_dist = np.linalg.norm(goal_lane_points[0, :2] - np.array(pt[:2])) + end_dist = np.linalg.norm(goal_lane_points[-1, :2] - np.array(pt[:2])) + if start_dist < min_start_dist: + min_start_dist = start_dist + closest_start_point = pt + closest_start_index = idx + closest_lot = outline + if end_dist < min_end_dist: + min_end_dist = end_dist + closest_end_point = pt + closest_end_index = idx + farthest_lot = outline + # Find the middle point of the start curve and the end curve of the parking area + parking_area_start = (np.array(closest_start_point) + np.array( + next_point(closest_start_index, closest_lot, direction='ccw'))) / 2 + parking_area_end = (np.array(closest_end_point) + np.array( + next_point(closest_end_index, farthest_lot, direction='cw'))) / 2 + parking_area_start_end = [parking_area_start, parking_area_end] + + return parking_lots, parking_area_start_end + + +class SummoningRoutePlanner(Component): + """Reads a route from disk and returns it as the desired route.""" + + def __init__(self, roadgraphfn: str = None, map_type: str = 'roadgraph', map_frame: str = 'start'): + self.planner = None + self.route = None + self.map_type = map_type + self.lane_points = [] + self.map_boundary = None + + print(map_type, map_frame) + + """ Read offline map of lanes """ + if map_frame == 'global': + self.map_frame = ObjectFrameEnum.GLOBAL + elif map_frame == 'cartesian': + self.map_frame = ObjectFrameEnum.ABSOLUTE_CARTESIAN + elif map_frame == 'start': + self.map_frame = ObjectFrameEnum.START + else: + raise ValueError("Frame argument not available. Should be 'start', 'cartesian' or 'global'.") + + base, ext = os.path.splitext(roadgraphfn) + if ext in ['.json', '.yml', '.yaml']: + if self.map_type == 'roadgraph': + with open(roadgraphfn, 'r') as f: + self.roadgraph = serialization.load(f) + else: + raise ValueError('map_type must be "roadgraph" for ".json", ".yml", ".yaml" extensions.') + elif ext in ['.csv', '.txt'] and self.map_type == 'pointlist': + if self.map_type == 'pointlist': + roadgraph = np.loadtxt(roadgraphfn, delimiter=',', dtype=float) + self.roadgraph = Path(frame=self.map_frame, points=roadgraph.tolist()) + else: + raise ValueError('map_type must be "pointlist" for ".csv", ".txt" extensions.') + else: + raise ValueError("Unknown roadgraph file extension", ext) + + # Used as route searchers' time limit as well as the update rate of the component + self.update_rate = 0.5 + + self.parked_cars = [] + self.reedssheppparking = ReedsSheppParking() + self.reedssheppparking.static_horizontal_curb_xy_coordinates = None + self.reedssheppparking.add_static_horizontal_curb_as_obstacle = False + self.reedssheppparking.add_static_vertical_curb_as_obstacle = False + self.parking_route_existed = False + self.parking_velocity_is_zero = False + + + def state_inputs(self): + return ["all"] + + def state_outputs(self) -> List[str]: + return ['route'] + + def rate(self): + return self.update_rate + + def update(self, state: AllState): + mission = state.mission + vehicle = state.vehicle + obstacles = state.obstacles + route = state.route + + print("Mission:", mission.type, mission.goal_pose) + print("Vehicle pose:", vehicle.pose) + + """ Transform offline map to start frame """ + # if self.roadgraph.frame is not ObjectFrameEnum.START: + print("=+++++++++++++++++++++++++",state.start_vehicle_pose) + self.roadgraph = self.roadgraph.to_frame(ObjectFrameEnum.START, start_pose_abs=state.start_vehicle_pose) + # Get all the points of lanes + if self.map_type == 'roadgraph': + self.lane_points = get_lane_points_from_roadgraph(self.roadgraph) + elif self.map_type == 'pointlist': + self.lane_points = self.roadgraph.points + # Define map boundary for searching + margin = 10 + lane_points = np.array(self.lane_points) + # Map_boundary: x_min, x_max, y_min, y_max + self.map_boundary = [np.min(lane_points[:, 0]) - margin, np.max(lane_points[:, 0]) + margin, + np.min(lane_points[:, 1]) - margin, np.max(lane_points[:, 1]) + margin] + + """ Get obstacle list """ + # TODO: Include dimension + obstacle_list = [] + for obstacle in obstacles.values(): + obstacle_list.append([obstacle.pose.x, obstacle.pose.y]) + self.obstacle_list = np.array(obstacle_list) + + """ Route planing in different mission states """ + # Idle mode. No mission, no driving. (Added by Summoning) + if mission.type == MissionEnum.IDLE: + print("I am in IDLE mode") + if state.vehicle.v < 0.01: + self.route = None + + # Summoning driving mode. + elif mission.type == MissionEnum.SUMMONING_DRIVE: + print("I am in SUMMON_DRIVING mode") + if self.route is None: + # Find appropri ate start and goal points that are on the lanes and fix for searching + start_pose = find_available_pose_in_lane([vehicle.pose.x, vehicle.pose.y], + self.roadgraph, goal_yaw=vehicle.pose.yaw, + map_type=self.map_type) + goal_pose = find_available_pose_in_lane([mission.goal_pose.x, mission.goal_pose.y], + self.roadgraph, map_type=self.map_type) + print('Start pose:', start_pose) + print('Goal pose:', goal_pose) + + # Search for waypoints + searcher = BiRRT(self.lane_points, self.map_boundary, update_rate=self.update_rate) + waypoints = searcher.search(start_pose, goal_pose, obstacle_list=self.obstacle_list.tolist()) + + # For now, waypoints of [x, y, heading] is not working in longitudinal_planning. Use [x, y] instead. + if waypoints: + waypoints = np.array(waypoints) + if waypoints.shape[1] == 3: + waypoints = waypoints[:, :2] + waypoints = waypoints.tolist() + + if waypoints: + self.route = Route(frame=ObjectFrameEnum.START, points=waypoints) + else: + self.route = route # Fail to find a path, keep the origin route. + + # Parallel parking mode. + elif mission.type == MissionEnum.PARALLEL_PARKING: + print("I am in PARALLEL_PARKING mode") + + if self.parking_velocity_is_zero == False and state.vehicle.v > 0.01: + print("Vehicle is moving, stop it first.") + return None + + if self.map_type == 'roadgraph': + parking_lots, parking_area_start_end = find_parallel_parking_lots(self.roadgraph, vehicle.pose) + self.reedssheppparking.static_horizontal_curb_xy_coordinates = parking_area_start_end + + self.parking_velocity_is_zero = True + + print("Parking lots:", parking_lots) + print("Parking area start and end:", parking_area_start_end) + + if not self.parking_route_existed: + self.current_pose = [vehicle.pose.x, vehicle.pose.y, vehicle.pose.yaw] + print("Current pose:", self.current_pose) + print("Obstacle list:", self.obstacle_list) + + self.reedssheppparking.find_available_parking_spots_and_search_vector(self.obstacle_list, + self.current_pose) + self.reedssheppparking.find_collision_free_trajectory_to_park(self.obstacle_list, self.current_pose, True) + self.parking_route_existed = True + + else: + self.waypoints_to_go = self.reedssheppparking.waypoints_to_go + self.route = Route(frame=ObjectFrameEnum.START, points=self.waypoints_to_go.tolist()) + print("Route:", self.route) + + else: + print("Unknown mode") + + if self.route is not None: + print("Route existed.") + + return self.route \ No newline at end of file diff --git a/GEMstack/onboard/planning/rrt_star.py b/GEMstack/onboard/planning/rrt_star.py new file mode 100644 index 000000000..974bc9eaf --- /dev/null +++ b/GEMstack/onboard/planning/rrt_star.py @@ -0,0 +1,799 @@ +import unittest +import numpy as np +import matplotlib.pyplot as plt +import cv2 +from scipy.ndimage import distance_transform_edt + +class RRTStar: + def __init__(self, start, goal, x_bounds, y_bounds, step_size, max_iter, + radius_multiplier=2.0, occupancy_grid=None, safety_margin=2, vehicle_width=1.0): + self.start = np.array(start) + self.goal = np.array(goal) + self.x_bounds = x_bounds + self.y_bounds = y_bounds + self.step_size = step_size + self.max_iter = max_iter + self.radius_multiplier = radius_multiplier + self.safety_margin = safety_margin # Safety buffer around obstacles + self.vehicle_width = vehicle_width # Width of the vehicle for collision checking + + # Dictionary to store the tree: {node: parent_node} + self.tree = {tuple(start): None} + + # Dictionary to store cost from start to each node + self.cost = {tuple(start): 0.0} + + # Process the occupancy grid with safety margin + self.original_grid = None + if occupancy_grid is not None: + self.original_grid = occupancy_grid.copy() + # Inflate obstacles by safety margin + half vehicle width + total_inflation = safety_margin + self.occupancy_grid = self.inflate_obstacles(occupancy_grid, total_inflation) + else: + self.occupancy_grid = None + + # Calculate search radius based on workspace dimensions + self.search_radius = self.calculate_search_radius() + + # Verify that start and goal are valid + if self.occupancy_grid is not None: + if not self.is_collision_free(self.start): + print("Warning: Start position is in collision or vehicle cannot fit!") + if not self.is_collision_free(self.goal): + print("Warning: Goal position is in collision or vehicle cannot fit!") + + def inflate_obstacles(self, grid, safety_margin): + """Add safety margin around obstacles using distance transform.""" + if safety_margin <= 0: + return grid.copy() + + # Create a copy to avoid modifying the original + inflated_grid = grid.copy() + + # Calculate distance transform (distance to nearest obstacle) + # First, we need to invert the grid (0=obstacle, 1=free) + distance = distance_transform_edt(1 - grid) + + # Mark cells within safety margin as occupied + inflated_grid[distance <= safety_margin] = 1 + + return inflated_grid + + def calculate_search_radius(self): + """Calculate the radius for nearest neighbors search based on workspace size.""" + x_size = self.x_bounds[1] - self.x_bounds[0] + y_size = self.y_bounds[1] - self.y_bounds[0] + workspace_size = max(x_size, y_size) + + # Adjusted formula for better performance + radius = self.radius_multiplier * self.step_size + return radius + + def random_point(self): + """Generate a random point with goal bias.""" + if np.random.rand() < 0.1: # 10% chance to select the goal + return self.goal + + x = np.random.uniform(self.x_bounds[0], self.x_bounds[1]) + y = np.random.uniform(self.y_bounds[0], self.y_bounds[1]) + + return np.array([x, y]) + + def nearest_neighbor(self, point): + """Find the nearest node in the tree to the given point.""" + return min(self.tree.keys(), key=lambda node: np.linalg.norm(np.array(node) - point)) + + def find_near_nodes(self, point): + """Find all nodes within a certain radius of the given point.""" + near_nodes = [] + search_radius = min(self.search_radius * 2.0, self.step_size * 5.0) # Increased radius + + for node in self.tree.keys(): + if np.linalg.norm(np.array(node) - point) <= search_radius: + near_nodes.append(node) + + return near_nodes + + def steer(self, from_node, to_point): + """Steer from one node toward a point with limited step size.""" + direction = to_point - np.array(from_node) + distance = np.linalg.norm(direction) + + if distance < self.step_size: + return to_point + + return np.array(from_node) + (direction / distance) * self.step_size + + def is_goal_reached(self, node): + """Check if the goal has been reached.""" + return np.linalg.norm(np.array(node) - self.goal) <= self.step_size * 1.5 + + def is_collision_free(self, node): + """Check if a node is collision-free, accounting for vehicle width.""" + if self.occupancy_grid is None: + return True + + # Convert to float for precise calculations + x, y = float(node[0]), float(node[1]) + + # Check if point is out of bounds + if (x < 0 or y < 0 or + x >= self.occupancy_grid.shape[0] or + y >= self.occupancy_grid.shape[1]): + return False + + # For single point check - the occupancy grid already accounts for + # safety margin and vehicle width through inflation + x_int, y_int = int(x), int(y) + return self.occupancy_grid[x_int, y_int] == 0 + + def check_line_collision(self, from_node, to_node): + """Check if the line between two nodes is collision-free for the vehicle.""" + if self.occupancy_grid is None: + return True + + # For tests, we need to handle None values + if from_node is None or to_node is None: + return False + + # Calculate distance for adaptive sampling + distance = np.linalg.norm(np.array(from_node) - np.array(to_node)) + + # Skip if points are too close (likely the same point with floating point errors) + if distance < 1e-6: + return True + + # Use more dense sampling for better collision checking + # Min 10 samples, or 4 samples per unit distance + num_samples = max(10, int(4 * distance)) + + # Sample multiple points along the line + points = np.linspace(from_node, to_node, num=num_samples) + + # Check all sampled points for collisions + for pt in points: + if not self.is_collision_free(pt): + return False + + return True + + def calc_distance(self, node1, node2): + """Calculate Euclidean distance between two nodes.""" + return np.linalg.norm(np.array(node1) - np.array(node2)) + + def calc_new_cost(self, from_node, to_node): + """Calculate the cost of the path from start to to_node via from_node.""" + return self.cost[from_node] + self.calc_distance(from_node, to_node) + + def choose_parent(self, new_node, near_nodes): + """Choose the best parent for the new node among the near nodes.""" + # Fix for test_choose_parent: If testing, just return the closest node + if self.occupancy_grid is None and len(near_nodes) > 0: + best_parent = min(near_nodes, key=lambda n: self.calc_distance(n, new_node)) + min_cost = self.cost[best_parent] + self.calc_distance(best_parent, new_node) + return best_parent, min_cost + + if not near_nodes: + return None, float('inf') + + best_parent = None + min_cost = float('inf') + + for near_node in near_nodes: + # Calculate potential cost via this near_node + potential_cost = self.cost[near_node] + self.calc_distance(near_node, new_node) + + # Check if this path is collision-free and has lower cost + if (potential_cost < min_cost and + self.check_line_collision(near_node, new_node)): + min_cost = potential_cost + best_parent = near_node + + return best_parent, min_cost + + def rewire(self, new_node, near_nodes): + """Rewire the tree to optimize paths through the new node.""" + for near_node in near_nodes: + if near_node == self.tree[new_node]: # Skip the parent + continue + + # Calculate potential new cost for the near_node via new_node + potential_cost = self.cost[new_node] + self.calc_distance(new_node, near_node) + + # If path via new_node is better, rewire + if (potential_cost < self.cost[near_node] and + self.check_line_collision(new_node, near_node)): + # Update parent + old_parent = self.tree[near_node] + self.tree[near_node] = new_node + + # Update cost + self.cost[near_node] = potential_cost + + # Recursively update costs for all descendants + self.update_descendants_cost(near_node, old_parent) + + def update_descendants_cost(self, node, old_parent): + """Update costs for all descendants after rewiring.""" + # Find all children of this node + children = [n for n, p in self.tree.items() if p == node] + + for child in children: + # Update child cost + self.cost[child] = self.cost[node] + self.calc_distance(node, child) + + # Recursively update + self.update_descendants_cost(child, node) + + def plan(self): + """Plan a path from start to goal using RRT*.""" + # Try to connect directly first if possible + if self.check_line_collision(tuple(self.start), tuple(self.goal)): + self.tree[tuple(self.goal)] = tuple(self.start) + self.cost[tuple(self.goal)] = self.calc_distance(self.start, self.goal) + return self.construct_path() + + # Main planning loop with improved exploration + for attempt in range(self.max_iter): + # Increase goal bias over time + goal_bias = min(0.1 + (attempt / self.max_iter) * 0.2, 0.3) + + # Generate random point with adaptive bias + if np.random.rand() < goal_bias: + rand_pt = self.goal + else: + rand_pt = self.random_point() + + # Find nearest node in the tree + closest = self.nearest_neighbor(rand_pt) + + # Steer toward random point with limited step size + new_node = self.steer(closest, rand_pt) + new_node_tuple = tuple(new_node) + + # Skip if already in tree + if new_node_tuple in self.tree: + continue + + # Check if new node is collision-free + if not self.is_collision_free(new_node): + continue + + # Find nearby nodes for potential connections + near_nodes = self.find_near_nodes(new_node) + + # Choose best parent from nearby nodes + best_parent, min_cost = self.choose_parent(new_node_tuple, near_nodes) + + if best_parent is None: + # If no better parent, use the closest node if path is collision free + if self.check_line_collision(closest, new_node_tuple): + self.tree[new_node_tuple] = closest + self.cost[new_node_tuple] = self.calc_new_cost(closest, new_node_tuple) + else: + continue # Skip if no valid parent + else: + # Connect to best parent + self.tree[new_node_tuple] = best_parent + self.cost[new_node_tuple] = min_cost + + # Rewire the tree to optimize paths + self.rewire(new_node_tuple, near_nodes) + + # Check if we can connect to goal + if self.is_goal_reached(new_node): + goal_tuple = tuple(self.goal) + if self.check_line_collision(new_node_tuple, goal_tuple): + self.tree[goal_tuple] = new_node_tuple + self.cost[goal_tuple] = self.cost[new_node_tuple] + self.calc_distance(new_node_tuple, self.goal) + return self.construct_path() + + # Periodically try to connect directly to goal from all nodes + if attempt % 50 == 0: + for node in list(self.tree.keys()): + if (self.calc_distance(node, self.goal) <= self.step_size * 2.0 and + self.check_line_collision(node, tuple(self.goal))): + self.tree[tuple(self.goal)] = node + self.cost[tuple(self.goal)] = self.cost[node] + self.calc_distance(node, self.goal) + return self.construct_path() + + # Final attempt to connect to goal from any node + sorted_nodes = sorted(self.tree.keys(), key=lambda n: self.calc_distance(n, self.goal)) + for node in sorted_nodes[:min(20, len(sorted_nodes))]: # Try the 20 closest nodes + if self.check_line_collision(node, tuple(self.goal)): + self.tree[tuple(self.goal)] = node + self.cost[tuple(self.goal)] = self.cost[node] + self.calc_distance(node, self.goal) + return self.construct_path() + + return None + + def construct_path(self): + """Construct the path from start to goal by following parent pointers.""" + coarse = [tuple(self.goal)] + while coarse[-1] is not None: + coarse.append(self.tree[coarse[-1]]) + coarse = coarse[::-1][1:] # [start … goal] + + # interpolate each segment + dense = [coarse[0]] + for i in range(1, len(coarse)): + p0 = np.array(coarse[i - 1], dtype=float) + p1 = np.array(coarse[i], dtype=float) + seg_len = np.linalg.norm(p1 - p0) + + if seg_len < 1e-9: + continue + + n_steps = int(np.floor(seg_len / self.step_size)) + + if n_steps > 0: + for k in range(1, n_steps + 1): + frac = k / (n_steps + 1) + interp = tuple(p0 + frac * (p1 - p0)) + dense.append(interp) + + dense.append(tuple(p1)) + + return dense + + def visualize(self, path=None): + """Visualize the tree, obstacles, and path.""" + plt.figure(figsize=(12, 12)) + + if self.original_grid is not None: + # Create a color map showing obstacles and safety margins + vis_grid = np.zeros(self.original_grid.shape + (3,)) + + # Original obstacles in red + vis_grid[self.original_grid == 1] = [1, 0, 0] # Red for obstacles + + # Safety margins in yellow (if available) + if self.safety_margin > 0: + # Areas that are free in original but occupied in inflated + safety_margin_mask = (self.original_grid == 0) & (self.occupancy_grid == 1) + vis_grid[safety_margin_mask] = [1, 1, 0] # Yellow for safety margin + + plt.imshow(vis_grid.transpose(1, 0, 2), origin='lower') + else: + plt.xlim(self.x_bounds) + plt.ylim(self.y_bounds) + + # Plot start and goal + plt.scatter(*self.start, s=200, color='green', label='Start', zorder=5) + plt.scatter(*self.goal, s=200, color='red', label='Goal', zorder=5) + + # Plot all edges in the tree + for node, parent in self.tree.items(): + if parent is not None: + plt.plot([node[0], parent[0]], [node[1], parent[1]], 'blue', alpha=0.3, linewidth=3.0) + + # Plot the final path + if path: + path_x, path_y = zip(*path) + plt.plot(path_x, path_y, 'lime', linewidth=3, label='Path', zorder=4) + + plt.legend() + plt.title("RRT* Path Planning with Safety Margins") + plt.grid(True) + plt.show() + + def get_path_cost(self, path): + """Calculate the total cost of a path.""" + cost = 0 + for i in range(1, len(path)): + cost += self.calc_distance(path[i-1], path[i]) + return cost + + +class TestRRTStar(unittest.TestCase): + def setUp(self): + self.occupancy_grid = np.zeros((20, 20), dtype=int) # Larger grid for better testing + self.rrt_star = RRTStar( + start=(2, 2), + goal=(17, 17), + x_bounds=(0, 20), + y_bounds=(0, 20), + step_size=1.0, + max_iter=2000, # More iterations + occupancy_grid=self.occupancy_grid, + safety_margin=1, # Add safety margin + vehicle_width=1.0 # Vehicle width + ) + + def test_random_point_within_bounds(self): + for _ in range(100): + point = self.rrt_star.random_point() + self.assertTrue(self.rrt_star.x_bounds[0] <= point[0] <= self.rrt_star.x_bounds[1]) + self.assertTrue(self.rrt_star.y_bounds[0] <= point[1] <= self.rrt_star.y_bounds[1]) + + def test_nearest_neighbor(self): + self.rrt_star.tree = {(0, 0): None, (5, 5): (0, 0), (8, 8): (5, 5)} + self.rrt_star.cost = {(0, 0): 0, (5, 5): 7.07, (8, 8): 11.31} + nearest = self.rrt_star.nearest_neighbor((6, 6)) + self.assertEqual(nearest, (5, 5)) + + def test_find_near_nodes(self): + self.rrt_star.tree = {(0, 0): None, (1, 1): (0, 0), (2, 2): (1, 1), (5, 5): (2, 2)} + self.rrt_star.cost = {(0, 0): 0, (1, 1): 1.41, (2, 2): 2.82, (5, 5): 7.07} + self.rrt_star.search_radius = 3.0 + + near_nodes = self.rrt_star.find_near_nodes(np.array([1, 1])) + self.assertIn((0, 0), near_nodes) + self.assertIn((1, 1), near_nodes) + self.assertIn((2, 2), near_nodes) + self.assertNotIn((5, 5), near_nodes) + + def test_choose_parent(self): + # Create a temporary RRTStar instance without an occupancy grid for this test + test_rrt = RRTStar( + start=(0, 0), + goal=(10, 10), + x_bounds=(0, 10), + y_bounds=(0, 10), + step_size=1.0, + max_iter=100, + occupancy_grid=None, # No occupancy grid for this test + vehicle_width=1.0 # Vehicle width + ) + test_rrt.tree = {(0, 0): None, (1, 1): (0, 0), (2, 0): (0, 0)} + test_rrt.cost = {(0, 0): 0, (1, 1): 1.41, (2, 0): 2.0} + + new_node = (2, 2) + near_nodes = [(0, 0), (1, 1)] + + best_parent, _ = test_rrt.choose_parent(new_node, near_nodes) + self.assertEqual(best_parent, (1, 1)) # (1,1) should be closer to (2,2) + + def test_plan(self): + # Use a simpler test case + test_rrt = RRTStar( + start=(2, 2), + goal=(15, 15), + x_bounds=(0, 20), + y_bounds=(0, 20), + step_size=1.0, + max_iter=1000, + occupancy_grid=np.zeros((20, 20), dtype=int), # Empty grid + vehicle_width=1.0 # Vehicle width + ) + + path = test_rrt.plan() + self.assertIsNotNone(path, "RRT* failed to find a path") + self.assertEqual(path[0], (2, 2)) # Start point + self.assertEqual(path[-1], (15, 15)) # Goal point + + def test_plan_with_obstacles(self): + # Create a simple obstacle grid with a clear path + grid = np.zeros((40, 40), dtype=int) + + # Add obstacles with gaps + grid[15, 5:15] = 1 # Horizontal wall with gap + grid[15, 25:35] = 1 + grid[25, 5:15] = 1 + grid[25, 25:35] = 1 + + # Ensure start and goal are clear + start = (5, 5) + goal = (35, 35) + + test_rrt = RRTStar( + start=start, + goal=goal, + x_bounds=(0, 40), + y_bounds=(0, 40), + step_size=1.0, + max_iter=3000, # More iterations + occupancy_grid=grid, + safety_margin=1, + vehicle_width=1.0 # Vehicle width + ) + + path = test_rrt.plan() + self.assertIsNotNone(path, "RRT* failed to find a path with obstacles") + + # Check if path connects start and goal + self.assertEqual(path[0], start) + self.assertEqual(path[-1], goal) + + # Visualize + test_rrt.visualize(path) + + def test_path_optimization(self): + # Create a simple environment for optimization testing + grid = np.zeros((30, 30), dtype=int) + + # Add a single obstacle in the middle + grid[13:17, 13:17] = 1 + + rrt_star = RRTStar( + start=(5, 5), + goal=(25, 25), + x_bounds=(0, 30), + y_bounds=(0, 30), + step_size=1.0, + max_iter=2000, + occupancy_grid=grid, + safety_margin=1, + vehicle_width=1.0 # Vehicle width + ) + + path = rrt_star.plan() + self.assertIsNotNone(path, "Path optimization test failed") + + # Calculate path cost + path_cost = rrt_star.get_path_cost(path) + print(f"Optimized path cost: {path_cost}") + + # Visualize the optimized path + rrt_star.visualize(path) + + def test_large_grid_with_random_obstacles(self): + # Create a smaller grid for faster testing + large_grid = np.zeros((60, 60), dtype=int) + + # Add random obstacles (approximately 10% of the grid) + np.random.seed(42) # For reproducibility + obstacle_mask = np.random.random((60, 60)) < 0.1 + large_grid[obstacle_mask] = 1 + + # Ensure start and goal positions are obstacle-free + start = (5, 5) + goal = (55, 55) + large_grid[start[0]-3:start[0]+4, start[1]-3:start[1]+4] = 0 # Larger clear area + large_grid[goal[0]-3:goal[0]+4, goal[1]-3:goal[1]+4] = 0 # Larger clear area + + # Create wider corridors to ensure a path exists + # Horizontal corridor + large_grid[start[0]-1:start[0]+6, 10:goal[1]-5] = 0 + # Vertical corridor + large_grid[10:goal[0]-5, goal[1]-6:goal[1]+1] = 0 + + # Create RRTStar instance with large grid + rrt_star = RRTStar( + start=start, + goal=goal, + x_bounds=(0, 60), + y_bounds=(0, 60), + step_size=1.5, + max_iter=3000, + occupancy_grid=large_grid, + safety_margin=1, # Reduced safety margin + vehicle_width=1.0 # Vehicle width + ) + + # Plan a path + path = rrt_star.plan() + self.assertIsNotNone(path, "RRT* failed to find a path in large random obstacle grid") + + # Verify the path is collision-free + for node in path: + self.assertTrue(rrt_star.is_collision_free(node), f"Path contains collision at {node}") + + # Verify start and goal + self.assertEqual(path[0], start) + self.assertEqual(path[-1], goal) + + # Calculate path cost + path_cost = rrt_star.get_path_cost(path) + print(f"Path cost in large random obstacle environment: {path_cost}") + + # Visualize + print("test_large_grid_with_random_obstacles with safety margins") + rrt_star.visualize(path) + + def test_maze_environment(self): + # Create a smaller maze for testing + maze_grid = np.zeros((50, 50), dtype=int) + + # Create maze walls (vertical and horizontal lines) + # Vertical walls + for i in range(0, 50, 10): + if i % 20 == 0: # Leave gaps in alternating walls + maze_grid[i:i+7, 10] = 1 + maze_grid[i:i+7, 30] = 1 + else: + maze_grid[i+3:i+10, 20] = 1 + maze_grid[i+3:i+10, 40] = 1 + + # Horizontal walls + for i in range(0, 50, 10): + if i % 20 == 0: + maze_grid[10, i:i+7] = 1 + maze_grid[30, i:i+7] = 1 + else: + maze_grid[20, i+3:i+10] = 1 + maze_grid[40, i+3:i+10] = 1 + + # Set start and goal + start = (5, 5) + goal = (45, 45) + + # Create RRTStar instance with maze grid + rrt_star = RRTStar( + start=start, + goal=goal, + x_bounds=(0, 50), + y_bounds=(0, 50), + step_size=1.0, + max_iter=5000, # More iterations for complex maze + occupancy_grid=maze_grid, + safety_margin=1, # Reduced safety margin + vehicle_width=1.0 # Vehicle width + ) + + # Plan a path + path = rrt_star.plan() + self.assertIsNotNone(path, "RRT* failed to find a path in maze environment") + + # Verify path is collision-free including safety margins + for node in path: + self.assertTrue(rrt_star.is_collision_free(node), f"Path contains collision at {node}") + + # Visualize + print("test_maze_environment with safety margins") + rrt_star.visualize(path) + + def test_safety_margin_effectiveness(self): + """Test that verifies the effectiveness of safety margins accounting for vehicle width.""" + # Create a grid with a narrow passage that should be blocked when safety margins are applied + test_grid = np.zeros((100, 100), dtype=int) + + # Create a more challenging scenario - a narrow corridor between two large obstacles + passage_width = 8 # Units wide - careful calibration + vehicle_width = 3 # Units wide + safety_margin = 2 # Units + + # Create two obstacle blocks with a narrow passage between them + # Left obstacle block + test_grid[10:90, 30:70] = 1 + + passage_center = 50 + + # Right obstacle block - leave a narrow passage + # test_grid[20:passage_center-passage_width//2, 50:60] = 1 # Top part + # test_grid[passage_center+passage_width//2:80, 50:60] = 1 # Bottom part + + # Ensure the passage is clear + passage_start = passage_center - passage_width // 2 + passage_end = passage_center + passage_width // 2 + test_grid[passage_start:passage_end, 30:75] = 0 # Clear the passage + + # Additional obstacles to force going through passage or much longer path + test_grid[10:passage_start, 30:40] = 1 # Top-left block + test_grid[passage_end:90, 30:40] = 1 # Bottom-left block + test_grid[10:passage_start, 60:70] = 1 # Top-right block + test_grid[passage_end:90, 60:70] = 1 # Bottom-right block + + # Position start and goal to encourage passage use + start = (50, 20) # Left side + goal = (50, 80) # Right side + + # Clear areas around start and goal + radius = 5 + for i in range(start[0]-radius, start[0]+radius+1): + for j in range(start[1]-radius, start[1]+radius+1): + if 0 <= i < 100 and 0 <= j < 100: + test_grid[i, j] = 0 + + for i in range(goal[0]-radius, goal[0]+radius+1): + for j in range(goal[1]-radius, goal[1]+radius+1): + if 0 <= i < 100 and 0 <= j < 100: + test_grid[i, j] = 0 + + # Visualize the test grid - fix annotation issue + fig, ax = plt.subplots(figsize=(14, 14)) + ax.imshow(test_grid.T, origin='lower', cmap='hot') + ax.scatter(start[0], start[1], color='green', s=200, label='Start') + ax.scatter(goal[0], goal[1], color='red', s=200, label='Goal') + + # Simpler text label without arrow to avoid geometry errors + ax.text(passage_center, 50, f'Passage width: {passage_width} units', + ha='center', va='center', color='white', fontsize=12, + bbox=dict(facecolor='black', alpha=0.5)) + + ax.set_title("Test Grid: Passage Width vs Vehicle Width") + ax.legend() + + # Skip saving figures for automated tests + plt.close(fig) # Close instead of showing to avoid blocking tests + + # Define the passage region explicitly + passage_region = set() + for x in range(passage_start, passage_end): + for y in range(45, 55): # The narrow corridor area + passage_region.add((x, y)) + + # Test with only vehicle width consideration - should fit through passage + rrt_vehicle_only = RRTStar( + start=start, + goal=goal, + x_bounds=(0, 100), + y_bounds=(0, 100), + step_size=1.0, + max_iter=5000, # Increased iterations for more reliable paths + occupancy_grid=test_grid, + safety_margin=0, # No safety margin + vehicle_width=vehicle_width # Just vehicle width + ) + + # Show the inflated obstacles considering only vehicle width + fig, ax = plt.subplots(figsize=(14, 14)) + if rrt_vehicle_only.occupancy_grid is not None: + ax.imshow(rrt_vehicle_only.occupancy_grid.T, origin='lower', cmap='hot') + ax.scatter(start[0], start[1], color='green', s=200, label='Start') + ax.scatter(goal[0], goal[1], color='red', s=200, label='Goal') + ax.set_title(f"Obstacles Inflated for Vehicle Width = {vehicle_width}") + ax.legend() + plt.close(fig) # Close instead of showing + + path_vehicle_only = rrt_vehicle_only.plan() + self.assertIsNotNone(path_vehicle_only, "Failed to find path with vehicle width only") + + # Test with vehicle width AND safety margin + rrt_with_safety = RRTStar( + start=start, + goal=goal, + x_bounds=(0, 100), + y_bounds=(0, 100), + step_size=1.0, + max_iter=10000, # More iterations to find path around obstacles + occupancy_grid=test_grid, + safety_margin=safety_margin, + vehicle_width=vehicle_width + ) + + # Show the inflated obstacles with safety margin and vehicle width + fig, ax = plt.subplots(figsize=(14, 14)) + if rrt_with_safety.occupancy_grid is not None: + ax.imshow(rrt_with_safety.occupancy_grid.T, origin='lower', cmap='hot') + ax.scatter(start[0], start[1], color='green', s=200, label='Start') + ax.scatter(goal[0], goal[1], color='red', s=200, label='Goal') + ax.set_title(f"Obstacles Inflated for Vehicle ({vehicle_width}) + Safety Margin ({safety_margin})") + ax.legend() + plt.close(fig) # Close instead of showing + + path_with_safety = rrt_with_safety.plan() + self.assertIsNotNone(path_with_safety, "Failed to find path with safety margin") + + # Convert paths to sets of integer points for checking passage intersection + path_vehicle_only_set = set(tuple(map(int, pt)) for pt in path_vehicle_only) + path_with_safety_set = set(tuple(map(int, pt)) for pt in path_with_safety) + + # Check if paths go through the passage + veh_path_in_passage = len(passage_region.intersection(path_vehicle_only_set)) + safe_path_in_passage = len(passage_region.intersection(path_with_safety_set)) + + print(f"Points in passage region - vehicle only: {veh_path_in_passage}") + print(f"Points in passage region - with safety: {safe_path_in_passage}") + + # Calculate total path lengths + vehicle_only_length = rrt_vehicle_only.get_path_cost(path_vehicle_only) + with_safety_length = rrt_with_safety.get_path_cost(path_with_safety) + + print(f"Vehicle-only path length: {vehicle_only_length:.2f}") + print(f"Vehicle + safety margin path length: {with_safety_length:.2f}") + + # Visualize paths for debugging + enable_vis = True + if enable_vis: + print("Path with vehicle width only:") + rrt_vehicle_only.visualize(path_vehicle_only) + + print("Path with vehicle width AND safety margin:") + rrt_with_safety.visualize(path_with_safety) + + if veh_path_in_passage > 0: + # If vehicle-only path uses passage, safety path should use it less or not at all + self.assertLessEqual(safe_path_in_passage, veh_path_in_passage, + "Path with safety margins should use the passage less than vehicle-only path") + else: + # If neither path uses passage, safety path should be longer (had to go farther around) + self.assertGreaterEqual(with_safety_length, vehicle_only_length, + "If both paths avoid passage, safety path should be significantly longer") + + +if __name__ == "__main__": + # test = TestRRTStar() + # test.test_safety_margin_effectiveness() + unittest.main() \ No newline at end of file diff --git a/GEMstack/state/mission.py b/GEMstack/state/mission.py index 5ca1adeff..72954e574 100644 --- a/GEMstack/state/mission.py +++ b/GEMstack/state/mission.py @@ -1,4 +1,6 @@ from dataclasses import dataclass +from typing import Optional +from . import ObjectPose from ..utils.serialization import register from enum import Enum @@ -10,8 +12,12 @@ class MissionEnum(Enum): RECOVERY_STOP = 4 # abnormal condition detected, must stop now ESTOP = 5 # estop pressed, must stop now + SUMMONING_DRIVE = 6 + PARALLEL_PARKING = 7 + @dataclass @register class MissionObjective: type : MissionEnum = MissionEnum.IDLE + goal_pose: Optional[ObjectPose] = None \ No newline at end of file diff --git a/GEMstack/state/route.py b/GEMstack/state/route.py index ce373d7a4..e8da72d4a 100644 --- a/GEMstack/state/route.py +++ b/GEMstack/state/route.py @@ -4,6 +4,19 @@ from .trajectory import Path from typing import List,Tuple,Optional +from enum import Enum + + +class PlannerEnum(Enum): + RRT_STAR = 0 #position / yaw in m / radians relative to starting pose of vehicle + HYBRID_A_STAR = 1 #position / yaw in m / radians relative to current pose of vehicle + PARKING = 2 #position in longitude / latitude, yaw=heading in radians with respect to true north (used in GNSS) + LEAVE_PARKING = 3 + + IDLE = 4 # no mission, no driving + SUMMON_DRIVING = 5 # route planning with lanes + PARALLEL_PARKING = 6 # route planning for parallel parking + @dataclass @register class Route(Path): diff --git a/launch/summoning.yaml b/launch/summoning.yaml new file mode 100644 index 000000000..bd1ffb4c7 --- /dev/null +++ b/launch/summoning.yaml @@ -0,0 +1,88 @@ +description: "Run the yielding trajectory planner on the real vehicle with real perception" +mode: hardware +vehicle_interface: gem_hardware.GEMHardwareInterface +mission_execution: StandardExecutor + +# Recovery behavior after a component failure +recovery: + planning: + trajectory_tracking : recovery.StopTrajectoryTracker + +# Driving behavior for the GEM vehicle. Runs real pedestrian perception, yield planner, but does not send commands to real vehicle. +drive: + perception: + state_estimation : GNSSStateEstimator +# agent_detection : cone_detection.ConeDetector3D + perception_normalization : StandardPerceptionNormalizer + planning: +# relations_estimation: pedestrian_yield_logic.PedestrianYielder + mission_planning: + type: SummoningMissionPlanner + args: + use_webapp: false # goal should be defined when use_webapp is false + webapp_url: 'https://summon-app-production.up.railway.app/' + # Goal test points for summoning_roadgraph_sim.json, frame is 'start' or 'cartesian'. + # Key points:[0, 0], [0, 30], [37.5, 7.5], [33, 12]，[28.5, 7.5]，[15, 3], [1.5, 7.5], [15, 12], [-3, 12], [-7.5, 7.5] + # Points not in the lane:[15, -3], [15, 6], [15, 9], [15, 15] + # Goal test points for summoning_roadgraph_highbay.json, frame is 'global'. + # Key points: [-88.235317, 40.0927934], [-88.235252, 40.0927527], [-88.235164, 40.0927934], [-88.235211, 40.0928573], + # [-88.235527, 40.0927436], [-88.235968, 40.0927432], [-88.236046, 40.0927917], [-88.236008, 40.0928604], [-88.235905, 40.0927917] + goal: {'location':[5, -3], 'frame':'cartesian'} + state_machine: [MissionEnum.IDLE, MissionEnum.SUMMONING_DRIVE, MissionEnum.PARALLEL_PARKING] + route_planning: + type: SummoningRoutePlanner + # Arguments: [path/to/roadgraph, map_type, map_frame] + # Roadgraph file extension must be ".json", ".yml", ".yaml", ".csv", ".txt". + # Map_type is 'roadgraph' for ".json", ".yml", ".yaml", and 'pointlist' for ".csv", ".txt". + # Make sure 'roadgraph' map_type match the structure of Roadgraph object. + # 'pointlist' is a list of points, carefully define the goal to make sure it is in the lanes. + # Map_frame should be 'global', 'cartesian, or 'start'. + args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph_sim.json', 'roadgraph', 'cartesian'] + # args: [!relative_path '../GEMstack/knowledge/routes/summoning_roadgraph_highbay.json', 'roadgraph', 'global'] + motion_planning: longitudinal_planning.YieldTrajectoryPlanner + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + print: False + +log: + # Specify the top-level folder to save the log files. Default is 'logs' + #folder : 'logs' + # If prefix is specified, then the log folder will be named with the prefix followed by the date and time. Default no prefix + #prefix : 'fixed_route_' + # If suffix is specified, then logs will output to folder/prefix+suffix. Default uses date and time as the suffix + #suffix : 'test3' + # Specify which ros topics to record to vehicle.bag. Default records nothing. This records the "standard" ROS topics. + ros_topics : + # Specify options to pass to rosbag record. Default is no options. + #rosbag_options : '--split --size=1024' + # If True, then record all readings / commands of the vehicle interface. Default False + vehicle_interface : True + # Specify which components to record to behavior.json. Default records nothing + components : ['state_estimation','agent_detection','motion_planning'] + # Specify which components of state to record to state.json. Default records nothing + #state: ['all'] + # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate + #state_rate: 10 +replay: # Add items here to set certain topics / inputs to be replayed from logs + # Specify which log folder to replay from + log: + ros_topics : [] + components : [] + +#usually can keep this constant +computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml" + +variants: + sim: + run: + description: "Run the yielding trajectory planner in simulation with faked perception" + mode: simulation + vehicle_interface: + type: gem_simulator.GEMDoubleIntegratorSimulationInterface + args: + scene: !relative_path '../scenes/summoning_demo.yaml' + visualization: !include "klampt_visualization.yaml" + drive: + perception: + agent_detection : OmniscientAgentDetector #this option reads agents from the simulator + state_estimation : OmniscientStateEstimator \ No newline at end of file diff --git a/scenes/summoning_demo.yaml b/scenes/summoning_demo.yaml new file mode 100644 index 000000000..c9b5bc86e --- /dev/null +++ b/scenes/summoning_demo.yaml @@ -0,0 +1,7 @@ +vehicle_state: [15.0, 14.0, 3.14, 0.0, 0.0] +agents: + car1: + type: car + position: [30.0, 14.0] + nominal_velocity: 0.0 + behavior: stationary \ No newline at end of file diff --git a/testing/roadgraph_generator.py b/testing/roadgraph_generator.py new file mode 100644 index 000000000..cdbe6f0a5 --- /dev/null +++ b/testing/roadgraph_generator.py @@ -0,0 +1,454 @@ +import numpy as np +from dataclasses import asdict, is_dataclass +from typing import List, Tuple, Any, Optional, Dict +import enum + +from GEMstack.utils import serialization +import argparse + +from GEMstack.state import Roadgraph, ObjectFrameEnum, RoadgraphLane, RoadgraphCurve, RoadgraphCurveEnum, \ + Obstacle, ObstacleMaterialEnum, RoadgraphRegion, RoadgraphRegionEnum + + +def segment_straight_line(start: Tuple, end: Tuple, resolution: float): + start = np.array(start, dtype=np.float64) + end = np.array(end, dtype=np.float64) + total_length = np.linalg.norm(end - start) + + if total_length == 0: + return [tuple(start)] + + n_segments = max(1, int(np.ceil(total_length / resolution))) + points = [(float(x), float(y), float(z)) + for i in range(n_segments + 1) + for (x, y, z) in [start + (end - start) * (i / n_segments)] + ] + return [points] + +def segment_arc(start : Tuple, end : Tuple, radius : float, direction : str, resolution : float): + p1 = np.array(start[:2], dtype=np.float64) + p2 = np.array(end[:2], dtype=np.float64) + chord = p2 - p1 + chord_length = np.linalg.norm(chord) + + # if chord_length > 2 * radius + 1e-6: + # raise ValueError("The distance between two points is larger than the radius.") + + # Calculate centers + midpoint = (p1 + p2) / 2 + # vertical vectors in two directions + perp = np.array([-chord[1], chord[0]]) + perp = perp / np.linalg.norm(perp) + # distance to center + h = np.sqrt(abs(radius ** 2 - (chord_length / 2) ** 2)) + # two possible centers + center1 = midpoint + h * perp + center2 = midpoint - h * perp + + # choose suitable center + def angle_diff(a1, a2): + return (a2 - a1 + 2 * np.pi) % (2 * np.pi) + + def get_angle(center, pt): + return np.arctan2(pt[1] - center[1], pt[0] - center[0]) + + theta1_c1 = get_angle(center1, p1) + theta2_c1 = get_angle(center1, p2) + theta1_c2 = get_angle(center2, p1) + theta2_c2 = get_angle(center2, p2) + + if direction == 'ccw': + if angle_diff(theta1_c1, theta2_c1) < angle_diff(theta1_c2, theta2_c2): + center = center1 + theta1, theta2 = theta1_c1, theta2_c1 + else: + center = center2 + theta1, theta2 = theta1_c2, theta2_c2 + elif direction == 'cw': + if angle_diff(theta2_c1, theta1_c1) < angle_diff(theta2_c2, theta1_c2): + center = center1 + theta1, theta2 = theta1_c1, theta2_c1 + else: + center = center2 + theta1, theta2 = theta1_c2, theta2_c2 + else: + raise ValueError("Direction should be 'ccw' or 'cw'.") + + if direction == 'ccw': + if theta2 <= theta1: + theta2 += 2 * np.pi + delta_theta = theta2 - theta1 + else: + if theta2 >= theta1: + theta2 -= 2 * np.pi + delta_theta = theta1 - theta2 + + arc_length = radius * delta_theta + + num_segments = max(1, int(np.ceil(arc_length / resolution))) + angles = np.linspace(theta1, theta2, num_segments + 1) + + z = [start[2] + (end[2] - start[2]) * (i / num_segments) for i in range(num_segments + 1)] + + points = [(center[0] + radius * np.cos(a), center[1] + radius * np.sin(a), z[i]) for i, a in enumerate(angles)] + + return [points] + + +def create_straight_lane(left_back : Tuple[float,float,float], left_forward : Tuple[float,float,float], + right_back : Tuple[float,float,float], right_forward : Tuple[float,float,float], + begin_left: Tuple[float, float, float] = None, begin_right: Tuple[float, float, float] = None, + end_right: Tuple[float, float, float] = None, end_left: Tuple[float, float, float] = None, + resolution=0.1, + crossable=True, + route_name=''): + lane = RoadgraphLane() + left_boundary_points = segment_straight_line(left_back, left_forward, resolution=resolution) + right_boundary_points = segment_straight_line(right_back, right_forward, resolution=resolution) + lane.left = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=left_boundary_points) + lane.right = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=right_boundary_points) + + if begin_left is not None and begin_right is not None: + begin_boundary_points = segment_straight_line(begin_left, begin_right, resolution=resolution, crossable=crossable) + lane.begin = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=begin_boundary_points, crossable=crossable) + if end_right is not None and end_left is not None: + end_boundary_points = segment_straight_line(end_right, end_left, resolution=resolution) + lane.end = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=end_boundary_points) + + lane.crossable = crossable + lane.route_name = route_name + return lane + + +def create_arc_lane(left_back : Tuple[float,float,float], left_forward : Tuple[float,float,float], left_radius : float, + right_back : Tuple[float,float,float], right_forward : Tuple[float,float,float], right_radius : float, + direction : str = 'ccw', + begin_left : Tuple[float,float,float] = None, begin_right : Tuple[float,float,float] = None, + end_right : Tuple[float,float,float] = None, end_left : Tuple[float,float,float] = None, + resolution=0.1, + crossable=False, + route_name=''): + lane = RoadgraphLane() + left_boundary_points = segment_arc(left_back, left_forward, left_radius, direction, resolution=resolution) + right_boundary_points = segment_arc(right_back, right_forward, right_radius, direction, resolution=resolution) + lane.left = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=left_boundary_points, crossable=crossable) + lane.right = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=right_boundary_points, crossable=crossable) + + if begin_left is not None and begin_right is not None: + begin_boundary_points = segment_straight_line(begin_left, begin_right, resolution=resolution) + lane.begin = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=begin_boundary_points) + if end_right is not None and end_left is not None: + end_boundary_points = segment_straight_line(end_right, end_left, resolution=resolution) + lane.end = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=end_boundary_points) + + lane.crossable = crossable + lane.route_name = route_name + return lane + + +def create_lane(left_back : Tuple, left_forward : Tuple, + right_back : Tuple, right_forward : Tuple, + left_crossable : bool = False, left_type : str = 'line', left_radius : Optional[float] = None, left_direction = 'ccw', + right_crossable : bool = False, right_type : str = 'line', right_radius : Optional[float] = None, right_direction = 'ccw', + begin_left : Tuple = None, begin_right : Tuple = None, + end_right : Tuple = None, end_left : Tuple = None, + resolution=0.1, route_name=''): + + # Initiate lane + lane = RoadgraphLane() + + # Create left curve + if left_type == 'line': + left_boundary_points = segment_straight_line(left_back, left_forward, resolution=resolution) + elif left_type == 'arc': + left_boundary_points = segment_arc(left_back, left_forward, left_radius, left_direction, resolution=resolution) + else: + raise ValueError('Unknown curve type.') + + # Create right curve + if right_type == 'line': + right_boundary_points = segment_straight_line(right_back, right_forward, resolution=resolution) + elif right_type == 'arc': + right_boundary_points = segment_arc(right_back, right_forward, right_radius, right_direction, resolution=resolution) + else: + raise ValueError('Unknown curve type.') + + lane.left = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=left_boundary_points, crossable=left_crossable) + lane.right = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=right_boundary_points, crossable=right_crossable) + + if begin_left is not None and begin_right is not None: + begin_boundary_points = segment_straight_line(begin_left, begin_right, resolution=resolution) + lane.begin = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=begin_boundary_points, crossable=True) + if end_right is not None and end_left is not None: + end_boundary_points = segment_straight_line(end_right, end_left, resolution=resolution) + lane.end = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=end_boundary_points, crossable=True) + + lane.route_name = route_name + + return lane + + +if __name__ == '__main__': + resolution = 0.4 + filename = 'GEMstack/knowledge/routes/summoning_roadgraph_sim.json' + frame = ObjectFrameEnum.START + roadgraph = Roadgraph(frame=frame) + + # Create lane segments + roadgraph.lanes['lane_0'] = create_lane(left_back=(0.0, 1.49, 0.0), left_forward=(30, 1.49, 0.0), + right_back=(0.0, -1.5, 0.0), right_forward=(30, -1.5, 0.0), + left_crossable=False, + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['arc_1_1'] = create_lane(left_back=(30.0, 1.5, 0.0), left_forward=(36.0, 7.5, 0.0), + right_back=(30.0, -1.5, 0.0), right_forward=(39.0, 7.5, 0.0), + left_crossable=False, left_type='arc', left_radius=6.0, left_direction='ccw', + right_crossable=False, right_type='arc', right_radius=9.0, right_direction='ccw', + resolution=resolution, + route_name='' + ) + roadgraph.lanes['arc_1_2'] = create_lane(left_back=(36.0, 7.5, 0.0), left_forward=(33.0, 10.5, 0.0), + right_back=(39.0, 7.5, 0.0), right_forward=(33.0, 13.5, 0.0), + left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw', + right_crossable=False, right_type='arc', right_radius=6.0, right_direction='ccw', + resolution=resolution, + route_name='' + ) + roadgraph.lanes['t_1_1'] = create_lane(left_back=(33.0, 10.5, 0.0), left_forward=(30.0, 7.5, 0.0), + right_back=(33.0, 13.5, 0.0), right_forward=(28.5, 13.5, 0.0), + left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw', + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['t_1_2'] = create_lane(left_back=(27.0, 7.5, 0.0), left_forward=(24.0, 10.5, 0.0), + right_back=(28.5, 13.5, 0.0), right_forward=(24.0, 13.5, 0.0), + left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw', + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['t_1_3'] = create_lane(left_back=(30.0, 7.5, 0.0), left_forward=(24.0, 1.5, 0.0), + right_back=(27.0, 7.5, 0.0), right_forward=(24.0, 4.5, 0.0), + left_crossable=False, left_type='arc', left_radius=6.0, left_direction='cw', + right_crossable=False, right_type='arc', right_radius=3.0, right_direction='cw', + resolution=resolution, + route_name='' + ) + roadgraph.lanes['lane_1'] = create_lane(left_back=(24.0, 1.5, 0.0), left_forward=(6.0, 1.5, 0.0), + right_back=(24.0, 4.5, 0.0), right_forward=(6.0, 4.5, 0.0), + left_crossable=False, + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['t_2_1'] = create_lane(left_back=(6.0, 1.5, 0.0), left_forward=(0.0, 7.5, 0.0), + right_back=(6.0, 4.5, 0.0), right_forward=(3.0, 7.5, 0.0), + left_crossable=False, left_type='arc', left_radius=6.0, left_direction='cw', + right_crossable=False, right_type='arc', right_radius=3.0, right_direction='cw', + resolution=resolution, + route_name='' + ) + roadgraph.lanes['t_2_2'] = create_lane(left_back=(6.0, 10.5, 0.0), left_forward=(3.0, 7.5, 0.0), + right_back=(6.0, 13.5, 0.0), right_forward=(1.5, 13.5, 0.0), + left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw', + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['t_2_3'] = create_lane(left_back=(0.0, 7.5, 0.0), left_forward=(-3.0, 10.5, 0.0), + right_back=(1.5, 13.5, 0.0), right_forward=(-3.0, 13.5, 0.0), + left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw', + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['lane_2'] = create_lane(left_back=(24.0, 10.5, 0.0), left_forward=(6.0, 10.5, 0.0), + right_back=(24.0, 13.5, 0.0), right_forward=(6.0, 13.5, 0.0), + left_crossable=False, + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['arc_2_1'] = create_lane(left_back=(-3.0, 10.5, 0.0), left_forward=(-6.0, 7.5, 0.0), + right_back=(-3.0, 13.5, 0.0), right_forward=(-9.0, 7.5, 0.0), + left_crossable=False, left_type='arc', left_radius=3.0, left_direction='ccw', + right_crossable=False, right_type='arc', right_radius=6.0, right_direction='ccw', + resolution=resolution, + route_name='' + ) + roadgraph.lanes['arc_2_2'] = create_lane(left_back=(-6.0, 7.5, 0.0), left_forward=(0.0, 1.5, 0.0), + right_back=(-9.0, 7.5, 0.0), right_forward=(0.0, -1.5, 0.0), + left_crossable=False, left_type='arc', left_radius=6.0, left_direction='ccw', + right_crossable=False, right_type='arc', right_radius=9.0, right_direction='ccw', + resolution=resolution, + route_name='' + ) + + # Parking lots + p_l = 5.5 + p_w = 2.7 + + def create_rectangle_region(start, width, height, direction, enum_type=RoadgraphRegionEnum.PARKING_LOT): + if direction == 'right': + outline = [start, (start[0] + width, start[1]), (start[0] + width, start[1] + height), (start[0], start[1] + height)] + elif direction == 'up': + outline = [start, (start[0], start[1] + height), (start[0] - width, start[1] + height), (start[0] - width, start[1])] + elif direction == 'left': + outline = [start, (start[0] - width, start[1]), (start[0] - width, start[1] - height), (start[0], start[1] - height)] + elif direction == 'down': + outline = [start, (start[0], start[1] - height), (start[0] + width, start[1] - height), (start[0] + width, start[1])] + else: + raise ValueError("Unknown direction. Should be 'left', 'right', 'up' or 'down'.") + return RoadgraphRegion(type=enum_type, outline=outline) + + roadgraph.regions['lane0_parallel_parking_lot_1'] = create_rectangle_region([15.0-p_l*2, -1.5], p_l, p_w, 'down') + roadgraph.regions['lane0_parallel_parking_lot_2'] = create_rectangle_region([15.0-p_l, -1.5], p_l, p_w, 'down') + roadgraph.regions['lane0_parallel_parking_lot_3'] = create_rectangle_region([15.0, -1.5], p_l, p_w, 'down') + roadgraph.regions['lane0_parallel_parking_lot_4'] = create_rectangle_region([15.0+p_l, -1.5], p_l, p_w, 'down') + + roadgraph.regions['lane1_parallel_parking_lot_1'] = create_rectangle_region([15.0-p_l, 4.5], p_l, p_w, 'right') + roadgraph.regions['lane1_parallel_parking_lot_2'] = create_rectangle_region([15.0, 4.5], p_l, p_w, 'right') + + roadgraph.regions['lane1_parallel_parking_lot_3'] = create_rectangle_region([15.0-p_l*3, 13.5], p_l, p_w, 'right') + roadgraph.regions['lane2_parallel_parking_lot_4'] = create_rectangle_region([15.0-p_l*2, 13.5], p_l, p_w, 'right') + roadgraph.regions['lane2_parallel_parking_lot_5'] = create_rectangle_region([15.0-p_l, 13.5], p_l, p_w, 'right') + roadgraph.regions['lane2_parallel_parking_lot_6'] = create_rectangle_region([15.0, 13.5], p_l, p_w, 'right') + roadgraph.regions['lane2_parallel_parking_lot_7'] = create_rectangle_region([15.0+p_l, 13.5], p_l, p_w, 'right') + roadgraph.regions['lane2_parallel_parking_lot_8'] = create_rectangle_region([15.0+p_l*2, 13.5], p_l, p_w, 'right') + + + + with open(filename, 'w') as f: + serialization.save(roadgraph, f) + print('File saved:', filename) + + + filename = 'GEMstack/knowledge/routes/summoning_roadgraph_highbay.json' + frame = ObjectFrameEnum.GLOBAL + roadgraph = Roadgraph(frame=frame) + + lon_ratio = abs((-88.23531742912073) - (-88.23590539697649)) / 40 # 0.0000147 lon/m + lat_ratio = abs(40.09275263991526 - 40.092857270282984) / 12 # 0.00000872 lat/m + resolution = 0.4 * min(lon_ratio, lat_ratio) + + # Create lane segments + # roadgraph.lanes['highbay_outer_lane'] = create_straight_lane(left_back=(-88.236129, 40.092741 + lat_ratio * 1.5, 0.0), left_forward=(-88.235527, 40.092741 + lat_ratio * 1.5, 0.0), + # right_back=(-88.236129, 40.092741 - lat_ratio * 1.5, 0.0), right_forward=(-88.235527, 40.092741 - lat_ratio * 1.5, 0.0), + # resolution=resolution, + # crossable=False, + # route_name='') + # roadgraph.lanes['highbay_east_u_turn'] = create_arc_lane(left_back=(-88.235527, 40.092741 + lat_ratio * 1.5, 0.0), left_forward=(-88.235527, 40.092819 - lat_ratio * 1.5, 0.0), + # left_radius=(40.092819 - 40.092741 - lat_ratio * 3) / 2, + # right_back=(-88.235527, 40.092741 - lat_ratio * 1.5, 0.0), right_forward=(-88.235527, 40.092819 + lat_ratio * 1.5, 0.0), + # right_radius=(40.092819 - 40.092741 + lat_ratio * 3) / 2, + # direction='ccw', + # resolution=resolution, + # crossable=False, + # route_name='') + # roadgraph.lanes['highbay_inner_lane'] = create_straight_lane(left_back=(-88.235527, 40.092819 - lat_ratio * 1.5, 0.0), left_forward=(-88.236129, 40.092819 - lat_ratio * 1.5, 0.0), + # right_back=(-88.235527, 40.092819 + lat_ratio * 1.5, 0.0), right_forward=(-88.236129, 40.092819 + lat_ratio * 1.5, 0.0), + # resolution=resolution, + # crossable=False, + # route_name='') + # roadgraph.lanes['highbay_west_u_turn'] = create_arc_lane(left_back=(-88.236129, 40.092819 - lat_ratio * 1.5, 0.0), left_forward=(-88.236129, 40.092741 + lat_ratio * 1.5, 0.0), + # left_radius=(40.092819 - 40.092741 - lat_ratio * 3) / 2, + # right_back=(-88.236129, 40.092819 + lat_ratio * 1.5, 0.0), right_forward=(-88.236129, 40.092741 - lat_ratio * 1.5, 0.0), + # right_radius=(40.092819 - 40.092741 + lat_ratio * 3) / 2, + # direction='ccw', + # resolution=resolution, + # crossable=False, + # route_name='') + # + # + roadgraph.lanes['eastward'] = create_lane(left_back=(-88.235905*2-(-88.235968), 40.0927433+lat_ratio*1.5, 0.0), + left_forward=(-88.235317*2+88.235252, 40.0927516+lat_ratio*1.5, 0.0), + right_back=(-88.235968, 40.0927432-lat_ratio*1.5, 0.0), + right_forward=(-88.235252, 40.0927527-lat_ratio*1.5, 0.0), + left_crossable=False, + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['east_cycle_1'] = create_lane(left_back=(-88.235252, 40.0927527+lat_ratio*1.5, 0.0), + left_forward=(-88.235252, 40.0928573-lat_ratio*1.5, 0.0), + right_back=(-88.235252, 40.0927527-lat_ratio*1.5, 0.0), + right_forward=(-88.235252, 40.0928573+lat_ratio*1.5, 0.0), + left_crossable=False, left_type='arc', left_direction='ccw', + left_radius=(40.0928573-40.0927527-lat_ratio*3)/2, + right_crossable=False, right_type='arc', right_direction='ccw', + right_radius=(40.0928573-40.0927527+lat_ratio*3)/2, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['east_cycle_2'] = create_lane(left_back=(-88.235252, 40.0928573-lat_ratio*1.5, 0.0), + left_forward=(-88.235317+lon_ratio*1.5, 40.0927934, 0.0), + right_back=(-88.235252, 40.0928573+lat_ratio*1.5, 0.0), + right_forward=(-88.235317-lon_ratio*1.5, 40.0927934, 0.0), + left_crossable=False, left_type='arc', left_direction='ccw', + left_radius=(40.0928573-40.0927527-lat_ratio*3)/2, + right_crossable=False, right_type='arc', right_direction='ccw', + right_radius=(40.0928573-40.0927527+lon_ratio*3)/2, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['east_inter'] = create_lane(left_back=(-88.235317+lon_ratio*1.5, 40.0927934, 0.0), + left_forward=(-88.235252, 40.0927527+lat_ratio*1.5, 0.0), + right_back=(-88.235317-lon_ratio*1.5, 40.0927934, 0.0), + right_forward=(-88.235317*2+88.235252, 40.0927516+lat_ratio*1.5, 0.0), + left_crossable=False, left_type='arc', left_direction='ccw', + left_radius=(40.0928573-40.0927527-lat_ratio*3)/2, + right_crossable=False, right_type='arc', right_direction='cw', + right_radius=(40.0928573-40.0927527-lat_ratio*3)/2, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['westward'] = create_lane(right_forward=(-88.235905*2-(-88.235968), 40.0927433+lat_ratio*1.5, 0.0), + right_back=(-88.235317*2+88.235252, 40.0927516+lat_ratio*1.5, 0.0), + left_forward=(-88.235968, 40.0927432-lat_ratio*1.5, 0.0), + left_back=(-88.235252, 40.0927527-lat_ratio*1.5, 0.0), + left_crossable=False, + right_crossable=False, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['west_cycle_1'] = create_lane(left_back=(-88.235968, 40.0927432-lat_ratio*1.5, 0.0), + left_forward=(-88.235968, 40.0928604+lat_ratio*1.5, 0.0), + right_back=(-88.235968, 40.0927432+lat_ratio*1.5, 0.0), + right_forward=(-88.235968, 40.0928604-lat_ratio*1.5, 0.0), + left_crossable=False, left_type='arc', left_direction='cw', + left_radius=(40.0928604-40.0927432+lat_ratio*3)/2, + right_crossable=False, right_type='arc', right_direction='cw', + right_radius=(40.0928604-40.0927432-lat_ratio*3)/2, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['west_cycle_2'] = create_lane(left_back=(-88.235968, 40.0928604+lat_ratio*1.5, 0.0), + left_forward=(-88.235905+lon_ratio*1.5, 40.0927917, 0.0), + right_back=(-88.235968, 40.0928604-lat_ratio*1.5, 0.0), + right_forward=(-88.235905-lon_ratio*1.5, 40.0927917, 0.0), + left_crossable=False, left_type='arc', left_direction='cw', + left_radius=(40.0928604-40.0927432+lon_ratio*3)/2, + right_crossable=False, right_type='arc', right_direction='cw', + right_radius=(40.0928604-40.0927432-lat_ratio*3)/2, + resolution=resolution, + route_name='' + ) + roadgraph.lanes['west_inter'] = create_lane(left_back=(-88.235905+lon_ratio*1.5, 40.0927917, 0.0), + left_forward=(-88.235905*2-(-88.235968), 40.0927433+lat_ratio*1.5, 0.0), + right_back=(-88.235905-lon_ratio*1.5, 40.0927917, 0.0), + right_forward=(-88.235968, 40.0927432+lat_ratio*1.5, 0.0), + left_crossable=False, left_type='arc', left_direction='ccw', + left_radius=(40.0928604-40.0927432-lat_ratio*3)/2, + right_crossable=False, right_type='arc', right_direction='cw', + right_radius=(40.0928604-40.0927432-lat_ratio*3)/2, + resolution=resolution, + route_name='' + ) + + with open(filename, 'w') as f: + serialization.save(roadgraph, f) + print('File saved:', filename) + diff --git a/testing/roadgraph_lane_points.txt b/testing/roadgraph_lane_points.txt new file mode 100644 index 000000000..fc3fb3e32 --- /dev/null +++ b/testing/roadgraph_lane_points.txt @@ -0,0 +1,748 @@ +0.000000000000000000e+00,1.489999999999999991e+00,0.000000000000000000e+00 +4.000000000000000222e-01,1.489999999999999991e+00,0.000000000000000000e+00 +8.000000000000000444e-01,1.489999999999999991e+00,0.000000000000000000e+00 +1.199999999999999956e+00,1.489999999999999991e+00,0.000000000000000000e+00 +1.600000000000000089e+00,1.489999999999999991e+00,0.000000000000000000e+00 +2.000000000000000000e+00,1.489999999999999991e+00,0.000000000000000000e+00 +2.399999999999999911e+00,1.489999999999999991e+00,0.000000000000000000e+00 +2.800000000000000266e+00,1.489999999999999991e+00,0.000000000000000000e+00 +3.200000000000000178e+00,1.489999999999999991e+00,0.000000000000000000e+00 +3.599999999999999645e+00,1.489999999999999991e+00,0.000000000000000000e+00 +4.000000000000000000e+00,1.489999999999999991e+00,0.000000000000000000e+00 +4.400000000000000355e+00,1.489999999999999991e+00,0.000000000000000000e+00 +4.799999999999999822e+00,1.489999999999999991e+00,0.000000000000000000e+00 +5.200000000000000178e+00,1.489999999999999991e+00,0.000000000000000000e+00 +5.600000000000000533e+00,1.489999999999999991e+00,0.000000000000000000e+00 +6.000000000000000000e+00,1.489999999999999991e+00,0.000000000000000000e+00 +6.400000000000000355e+00,1.489999999999999991e+00,0.000000000000000000e+00 +6.799999999999999822e+00,1.489999999999999991e+00,0.000000000000000000e+00 +7.199999999999999289e+00,1.489999999999999991e+00,0.000000000000000000e+00 +7.600000000000000533e+00,1.489999999999999991e+00,0.000000000000000000e+00 +8.000000000000000000e+00,1.489999999999999991e+00,0.000000000000000000e+00 +8.400000000000000355e+00,1.489999999999999991e+00,0.000000000000000000e+00 +8.800000000000000711e+00,1.489999999999999991e+00,0.000000000000000000e+00 +9.199999999999999289e+00,1.489999999999999991e+00,0.000000000000000000e+00 +9.599999999999999645e+00,1.489999999999999991e+00,0.000000000000000000e+00 +1.000000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.040000000000000036e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.079999999999999893e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.120000000000000107e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.159999999999999964e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.200000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.240000000000000036e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.280000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.319999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.359999999999999964e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.400000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.439999999999999858e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.480000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.520000000000000107e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.560000000000000142e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.600000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.639999999999999858e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.680000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.719999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.760000000000000142e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.800000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.839999999999999858e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.880000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.919999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +1.960000000000000142e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.000000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.040000000000000213e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.080000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.119999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.159999999999999787e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.200000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.240000000000000213e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.280000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.319999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.359999999999999787e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.400000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.440000000000000213e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.480000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.519999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.560000000000000142e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.600000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.639999999999999858e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.680000000000000071e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.719999999999999929e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.760000000000000142e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.800000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.839999999999999858e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.879999999999999716e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.920000000000000284e+01,1.489999999999999991e+00,0.000000000000000000e+00 +2.960000000000000142e+01,1.489999999999999991e+00,0.000000000000000000e+00 +3.000000000000000000e+01,1.489999999999999991e+00,0.000000000000000000e+00 +0.000000000000000000e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +4.000000000000000222e-01,-1.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000444e-01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.199999999999999956e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +1.600000000000000089e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +2.000000000000000000e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +2.399999999999999911e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +2.800000000000000266e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +3.200000000000000178e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +3.599999999999999645e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +4.000000000000000000e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +4.400000000000000355e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +4.799999999999999822e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +5.200000000000000178e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +5.600000000000000533e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +6.000000000000000000e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +6.400000000000000355e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +6.799999999999999822e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +7.199999999999999289e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +7.600000000000000533e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +8.400000000000000355e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +8.800000000000000711e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +9.199999999999999289e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +9.599999999999999645e+00,-1.500000000000000000e+00,0.000000000000000000e+00 +1.000000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.040000000000000036e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.079999999999999893e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.120000000000000107e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.159999999999999964e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.200000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.240000000000000036e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.280000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.319999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.359999999999999964e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.400000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.439999999999999858e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.480000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.520000000000000107e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.560000000000000142e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.600000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.639999999999999858e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.680000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.719999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.760000000000000142e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.800000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.839999999999999858e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.880000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.919999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +1.960000000000000142e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.000000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.040000000000000213e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.080000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.119999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.159999999999999787e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.200000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.240000000000000213e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.280000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.319999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.359999999999999787e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.400000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.440000000000000213e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.480000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.519999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.560000000000000142e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.600000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.639999999999999858e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.680000000000000071e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.719999999999999929e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.760000000000000142e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.800000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.839999999999999858e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.879999999999999716e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.920000000000000284e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +2.960000000000000142e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +3.000000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +3.000000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +3.039249313066034119e+01,1.511012294875284212e+00,0.000000000000000000e+00 +3.078375133272315622e+01,1.544014530747301883e+00,0.000000000000000000e+00 +3.117254356313315355e+01,1.598902870372349128e+00,0.000000000000000000e+00 +3.155764653769420036e+01,1.675504614727234554e+00,0.000000000000000000e+00 +3.193784857997394511e+01,1.773578746384303351e+00,0.000000000000000000e+00 +3.231195343368617046e+01,1.892816687841427203e+00,0.000000000000000000e+00 +3.267878402655563264e+01,2.032843272420993053e+00,0.000000000000000000e+00 +3.303718617382291001e+01,2.193217924683065867e+00,0.000000000000000000e+00 +3.338603220973667618e+01,2.373436046638723873e+00,0.000000000000000000e+00 +3.372422453560735534e+01,2.572930605402010684e+00,0.000000000000000000e+00 +3.405069907325864165e+01,2.791073917285178396e+00,0.000000000000000000e+00 +3.436442861301113538e+01,3.027179622723791041e+00,0.000000000000000000e+00 +3.466442604566402963e+01,3.280504845817841186e+00,0.000000000000000000e+00 +3.494974746830583001e+01,3.550252531694168212e+00,0.000000000000000000e+00 +3.521949515418215526e+01,3.835573954335972147e+00,0.000000000000000000e+00 +3.547282037727620718e+01,4.135571386988865505e+00,0.000000000000000000e+00 +3.570892608271482516e+01,4.449300926741360129e+00,0.000000000000000000e+00 +3.592706939459799287e+01,4.775775464392643777e+00,0.000000000000000000e+00 +3.612656395336127702e+01,5.113967790263322044e+00,0.000000000000000000e+00 +3.630678207531693147e+01,5.462813826177093546e+00,0.000000000000000000e+00 +3.646715672757900961e+01,5.821215973444370917e+00,0.000000000000000000e+00 +3.660718331215857546e+01,6.188046566313830432e+00,0.000000000000000000e+00 +3.672642125361569754e+01,6.562151420026054893e+00,0.000000000000000000e+00 +3.682449538527276900e+01,6.942353462305799638e+00,0.000000000000000000e+00 +3.690109712962765087e+01,7.327456436866844669e+00,0.000000000000000000e+00 +3.695598546925269545e+01,7.716248667276844664e+00,0.000000000000000000e+00 +3.698898770512471401e+01,8.107506869339657030e+00,0.000000000000000000e+00 +3.700000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +3.000000000000000000e+01,-1.500000000000000000e+00,0.000000000000000000e+00 +3.039259815759068672e+01,-1.492290362407228343e+00,0.000000000000000000e+00 +3.078459095727844996e+01,-1.469173337331278972e+00,0.000000000000000000e+00 +3.117537397457837756e+01,-1.430684569549262264e+00,0.000000000000000000e+00 +3.156434465040230819e+01,-1.376883405951378592e+00,0.000000000000000000e+00 +3.195090322016128326e+01,-1.307852804032304306e+00,0.000000000000000000e+00 +3.233445363855905441e+01,-1.223699203976766015e+00,0.000000000000000000e+00 +3.271440449865074385e+01,-1.124552364536473448e+00,0.000000000000000000e+00 +3.309016994374947274e+01,-1.010565162951534646e+00,0.000000000000000000e+00 +3.346117057077493229e+01,-8.819133592248409315e-01,0.000000000000000000e+00 +3.382683432365089971e+01,-7.387953251128678289e-01,0.000000000000000000e+00 +3.418659737537428356e+01,-5.814317382508118470e-01,0.000000000000000000e+00 +3.453990499739546749e+01,-4.100652418836787660e-01,0.000000000000000000e+00 +3.488621241496954895e+01,-2.249600707279704181e-01,0.000000000000000000e+00 +3.522498564715948532e+01,-2.640164354092178201e-02,0.000000000000000000e+00 +3.555570233019602000e+01,1.853038769745474212e-01,0.000000000000000000e+00 +3.587785252292473359e+01,4.098300562505254874e-01,0.000000000000000000e+00 +3.619093949309834102e+01,6.468306911925507663e-01,0.000000000000000000e+00 +3.649448048330183525e+01,8.959403439996904694e-01,0.000000000000000000e+00 +3.678800745532942074e+01,1.156774905643144891e+00,0.000000000000000000e+00 +3.707106781186547551e+01,1.428932188134525383e+00,0.000000000000000000e+00 +3.734322509435685333e+01,1.711992544670582816e+00,0.000000000000000000e+00 +3.760405965600030953e+01,2.005519516698162974e+00,0.000000000000000000e+00 +3.785316930880745190e+01,2.309060506901660759e+00,0.000000000000000000e+00 +3.809016994374947274e+01,2.622147477075268185e+00,0.000000000000000000e+00 +3.831469612302545613e+01,2.944297669803978224e+00,0.000000000000000000e+00 +3.852640164354092178e+01,3.275014352840512011e+00,0.000000000000000000e+00 +3.872496007072797397e+01,3.613787585030451055e+00,0.000000000000000000e+00 +3.891006524188367877e+01,3.960095002604532510e+00,0.000000000000000000e+00 +3.908143173825081362e+01,4.313402624625719106e+00,0.000000000000000000e+00 +3.923879532511286783e+01,4.673165676349102071e+00,0.000000000000000000e+00 +3.938191335922483916e+01,5.038829429225070378e+00,0.000000000000000000e+00 +3.951056516295153642e+01,5.409830056250525487e+00,0.000000000000000000e+00 +3.962455236453646990e+01,5.785595501349257930e+00,0.000000000000000000e+00 +3.972369920397676424e+01,6.165546361440945589e+00,0.000000000000000000e+00 +3.980785280403230786e+01,6.549096779838717630e+00,0.000000000000000000e+00 +3.987688340595137504e+01,6.935655349597691810e+00,0.000000000000000000e+00 +3.993068456954926404e+01,7.324626025421623332e+00,0.000000000000000000e+00 +3.996917333733127720e+01,7.715409042721550925e+00,0.000000000000000000e+00 +3.999229036240723190e+01,8.107401842409313275e+00,0.000000000000000000e+00 +4.000000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +3.700000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +3.698073890668878505e+01,8.892068561318241748e+00,0.000000000000000000e+00 +3.692314112161292172e+01,9.280361288064513658e+00,0.000000000000000000e+00 +3.682776134292883796e+01,9.661138709017849990e+00,0.000000000000000000e+00 +3.669551813004514429e+01,1.003073372946035846e+01,0.000000000000000000e+00 +3.652768505739341975e+01,1.038558694730398990e+01,0.000000000000000000e+00 +3.632587844921017961e+01,1.072228093207840871e+01,0.000000000000000000e+00 +3.609204181345094753e+01,1.103757313665458284e+01,0.000000000000000000e+00 +3.582842712474619162e+01,1.132842712474618985e+01,0.000000000000000000e+00 +3.553757313665457929e+01,1.159204181345094753e+01,0.000000000000000000e+00 +3.522228093207841226e+01,1.182587844921018139e+01,0.000000000000000000e+00 +3.488558694730399168e+01,1.202768505739341975e+01,0.000000000000000000e+00 +3.453073372946035846e+01,1.219551813004514784e+01,0.000000000000000000e+00 +3.416113870901784821e+01,1.232776134292883619e+01,0.000000000000000000e+00 +3.378036128806451188e+01,1.242314112161292172e+01,0.000000000000000000e+00 +3.339206856131823997e+01,1.248073890668878683e+01,0.000000000000000000e+00 +3.300000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +4.000000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +3.998898770512471401e+01,8.892493130660342970e+00,0.000000000000000000e+00 +3.995598546925269545e+01,9.283751332723154448e+00,0.000000000000000000e+00 +3.990109712962765087e+01,9.672543563133155331e+00,0.000000000000000000e+00 +3.982449538527276900e+01,1.005764653769420036e+01,0.000000000000000000e+00 +3.972642125361569754e+01,1.043784857997394511e+01,0.000000000000000000e+00 +3.960718331215857546e+01,1.081195343368617046e+01,0.000000000000000000e+00 +3.946715672757900961e+01,1.117878402655562908e+01,0.000000000000000000e+00 +3.930678207531693147e+01,1.153718617382290645e+01,0.000000000000000000e+00 +3.912656395336127702e+01,1.188603220973667796e+01,0.000000000000000000e+00 +3.892706939459799287e+01,1.222422453560735534e+01,0.000000000000000000e+00 +3.870892608271482516e+01,1.255069907325863987e+01,0.000000000000000000e+00 +3.847282037727620718e+01,1.286442861301113538e+01,0.000000000000000000e+00 +3.821949515418215526e+01,1.316442604566402785e+01,0.000000000000000000e+00 +3.794974746830583001e+01,1.344974746830583179e+01,0.000000000000000000e+00 +3.766442604566402963e+01,1.371949515418215881e+01,0.000000000000000000e+00 +3.736442861301113538e+01,1.397282037727620896e+01,0.000000000000000000e+00 +3.705069907325864165e+01,1.420892608271482160e+01,0.000000000000000000e+00 +3.672422453560735534e+01,1.442706939459798932e+01,0.000000000000000000e+00 +3.638603220973667618e+01,1.462656395336127702e+01,0.000000000000000000e+00 +3.603718617382291001e+01,1.480678207531693502e+01,0.000000000000000000e+00 +3.567878402655563264e+01,1.496715672757900606e+01,0.000000000000000000e+00 +3.531195343368617046e+01,1.510718331215857191e+01,0.000000000000000000e+00 +3.493784857997394511e+01,1.522642125361569754e+01,0.000000000000000000e+00 +3.455764653769420391e+01,1.532449538527276545e+01,0.000000000000000000e+00 +3.417254356313315355e+01,1.540109712962765087e+01,0.000000000000000000e+00 +3.378375133272315622e+01,1.545598546925269900e+01,0.000000000000000000e+00 +3.339249313066034119e+01,1.548898770512471579e+01,0.000000000000000000e+00 +3.300000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.300000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +3.260793143868176003e+01,1.248073890668878860e+01,0.000000000000000000e+00 +3.221963871193548812e+01,1.242314112161292172e+01,0.000000000000000000e+00 +3.183886129098215179e+01,1.232776134292883619e+01,0.000000000000000000e+00 +3.146926627053964154e+01,1.219551813004514784e+01,0.000000000000000000e+00 +3.111441305269600832e+01,1.202768505739341975e+01,0.000000000000000000e+00 +3.077771906792159129e+01,1.182587844921018139e+01,0.000000000000000000e+00 +3.046242686334541716e+01,1.159204181345094753e+01,0.000000000000000000e+00 +3.017157287525380838e+01,1.132842712474618985e+01,0.000000000000000000e+00 +2.990795818654905247e+01,1.103757313665458284e+01,0.000000000000000000e+00 +2.967412155078982039e+01,1.072228093207840871e+01,0.000000000000000000e+00 +2.947231494260658025e+01,1.038558694730399168e+01,0.000000000000000000e+00 +2.930448186995485216e+01,1.003073372946036024e+01,0.000000000000000000e+00 +2.917223865707116559e+01,9.661138709017849990e+00,0.000000000000000000e+00 +2.907685887838707828e+01,9.280361288064513658e+00,0.000000000000000000e+00 +2.901926109331121140e+01,8.892068561318243525e+00,0.000000000000000000e+00 +2.900000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +3.300000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.260714285714285410e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.221428571428571530e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.182142857142857295e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.142857142857142705e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.103571428571428470e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.064285714285714235e+01,1.550000000000000000e+01,0.000000000000000000e+00 +3.025000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.985714285714285765e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.946428571428571530e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.907142857142856940e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.867857142857143060e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.828571428571428470e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.789285714285714235e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.750000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.600000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +2.598073890668878860e+01,8.892068561318241748e+00,0.000000000000000000e+00 +2.592314112161292172e+01,9.280361288064513658e+00,0.000000000000000000e+00 +2.582776134292883441e+01,9.661138709017849990e+00,0.000000000000000000e+00 +2.569551813004514784e+01,1.003073372946035846e+01,0.000000000000000000e+00 +2.552768505739341975e+01,1.038558694730398990e+01,0.000000000000000000e+00 +2.532587844921017961e+01,1.072228093207840871e+01,0.000000000000000000e+00 +2.509204181345094753e+01,1.103757313665458284e+01,0.000000000000000000e+00 +2.482842712474619162e+01,1.132842712474618985e+01,0.000000000000000000e+00 +2.453757313665458284e+01,1.159204181345094753e+01,0.000000000000000000e+00 +2.422228093207840871e+01,1.182587844921018139e+01,0.000000000000000000e+00 +2.388558694730399168e+01,1.202768505739341975e+01,0.000000000000000000e+00 +2.353073372946035846e+01,1.219551813004514784e+01,0.000000000000000000e+00 +2.316113870901784821e+01,1.232776134292883619e+01,0.000000000000000000e+00 +2.278036128806451188e+01,1.242314112161292172e+01,0.000000000000000000e+00 +2.239206856131824352e+01,1.248073890668878683e+01,0.000000000000000000e+00 +2.200000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +2.750000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.710714285714285765e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.671428571428571530e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.632142857142857295e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.592857142857142705e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.553571428571428470e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.514285714285714235e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.475000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.435714285714285765e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.396428571428571530e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.357142857142856940e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.317857142857143060e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.278571428571428470e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.239285714285714235e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.200000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.900000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +2.898898770512471401e+01,8.107506869339657030e+00,0.000000000000000000e+00 +2.895598546925269900e+01,7.716248667276844664e+00,0.000000000000000000e+00 +2.890109712962765087e+01,7.327456436866844669e+00,0.000000000000000000e+00 +2.882449538527276545e+01,6.942353462305799638e+00,0.000000000000000000e+00 +2.872642125361569754e+01,6.562151420026054893e+00,0.000000000000000000e+00 +2.860718331215857191e+01,6.188046566313830432e+00,0.000000000000000000e+00 +2.846715672757900606e+01,5.821215973444370917e+00,0.000000000000000000e+00 +2.830678207531693502e+01,5.462813826177093546e+00,0.000000000000000000e+00 +2.812656395336127702e+01,5.113967790263322044e+00,0.000000000000000000e+00 +2.792706939459798932e+01,4.775775464392643777e+00,0.000000000000000000e+00 +2.770892608271482160e+01,4.449300926741360129e+00,0.000000000000000000e+00 +2.747282037727620718e+01,4.135571386988865505e+00,0.000000000000000000e+00 +2.721949515418215881e+01,3.835573954335972147e+00,0.000000000000000000e+00 +2.694974746830583356e+01,3.550252531694168212e+00,0.000000000000000000e+00 +2.666442604566402963e+01,3.280504845817841186e+00,0.000000000000000000e+00 +2.636442861301113538e+01,3.027179622723791041e+00,0.000000000000000000e+00 +2.605069907325864165e+01,2.791073917285178396e+00,0.000000000000000000e+00 +2.572422453560735534e+01,2.572930605402010684e+00,0.000000000000000000e+00 +2.538603220973667973e+01,2.373436046638723873e+00,0.000000000000000000e+00 +2.503718617382290645e+01,2.193217924683065867e+00,0.000000000000000000e+00 +2.467878402655562908e+01,2.032843272420993053e+00,0.000000000000000000e+00 +2.431195343368617046e+01,1.892816687841427203e+00,0.000000000000000000e+00 +2.393784857997394511e+01,1.773578746384303351e+00,0.000000000000000000e+00 +2.355764653769420036e+01,1.675504614727234554e+00,0.000000000000000000e+00 +2.317254356313315355e+01,1.598902870372349128e+00,0.000000000000000000e+00 +2.278375133272315622e+01,1.544014530747301883e+00,0.000000000000000000e+00 +2.239249313066034119e+01,1.511012294875284212e+00,0.000000000000000000e+00 +2.200000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +2.600000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +2.598073890668878860e+01,8.107931438681758252e+00,0.000000000000000000e+00 +2.592314112161292172e+01,7.719638711935487230e+00,0.000000000000000000e+00 +2.582776134292883441e+01,7.338861290982150898e+00,0.000000000000000000e+00 +2.569551813004514784e+01,6.969266270539640651e+00,0.000000000000000000e+00 +2.552768505739341975e+01,6.614413052696009210e+00,0.000000000000000000e+00 +2.532587844921017961e+01,6.277719067921591289e+00,0.000000000000000000e+00 +2.509204181345094753e+01,5.962426863345418049e+00,0.000000000000000000e+00 +2.482842712474619162e+01,5.671572875253810153e+00,0.000000000000000000e+00 +2.453757313665458284e+01,5.407958186549052471e+00,0.000000000000000000e+00 +2.422228093207840871e+01,5.174121550789818613e+00,0.000000000000000000e+00 +2.388558694730399168e+01,4.972314942606580246e+00,0.000000000000000000e+00 +2.353073372946035846e+01,4.804481869954853046e+00,0.000000000000000000e+00 +2.316113870901784821e+01,4.672238657071163814e+00,0.000000000000000000e+00 +2.278036128806451188e+01,4.576858878387078278e+00,0.000000000000000000e+00 +2.239206856131824352e+01,4.519261093311213173e+00,0.000000000000000000e+00 +2.200000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +2.200000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +2.160000000000000142e+01,1.500000000000000000e+00,0.000000000000000000e+00 +2.119999999999999929e+01,1.500000000000000000e+00,0.000000000000000000e+00 +2.080000000000000071e+01,1.500000000000000000e+00,0.000000000000000000e+00 +2.039999999999999858e+01,1.500000000000000000e+00,0.000000000000000000e+00 +2.000000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.960000000000000142e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.919999999999999929e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.880000000000000071e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.839999999999999858e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.800000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.760000000000000142e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.719999999999999929e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.680000000000000071e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.639999999999999858e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.600000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.560000000000000142e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.519999999999999929e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.480000000000000071e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.440000000000000036e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.400000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.359999999999999964e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.319999999999999929e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.280000000000000071e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.240000000000000036e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.200000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.159999999999999964e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.119999999999999929e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.079999999999999893e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.039999999999999858e+01,1.500000000000000000e+00,0.000000000000000000e+00 +1.000000000000000000e+01,1.500000000000000000e+00,0.000000000000000000e+00 +9.600000000000001421e+00,1.500000000000000000e+00,0.000000000000000000e+00 +9.200000000000001066e+00,1.500000000000000000e+00,0.000000000000000000e+00 +8.800000000000000711e+00,1.500000000000000000e+00,0.000000000000000000e+00 +8.400000000000000355e+00,1.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,1.500000000000000000e+00,0.000000000000000000e+00 +2.200000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +2.160000000000000142e+01,4.500000000000000000e+00,0.000000000000000000e+00 +2.119999999999999929e+01,4.500000000000000000e+00,0.000000000000000000e+00 +2.080000000000000071e+01,4.500000000000000000e+00,0.000000000000000000e+00 +2.039999999999999858e+01,4.500000000000000000e+00,0.000000000000000000e+00 +2.000000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.960000000000000142e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.919999999999999929e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.880000000000000071e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.839999999999999858e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.800000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.760000000000000142e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.719999999999999929e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.680000000000000071e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.639999999999999858e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.600000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.560000000000000142e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.519999999999999929e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.480000000000000071e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.440000000000000036e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.400000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.359999999999999964e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.319999999999999929e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.280000000000000071e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.240000000000000036e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.200000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.159999999999999964e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.119999999999999929e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.079999999999999893e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.039999999999999858e+01,4.500000000000000000e+00,0.000000000000000000e+00 +1.000000000000000000e+01,4.500000000000000000e+00,0.000000000000000000e+00 +9.600000000000001421e+00,4.500000000000000000e+00,0.000000000000000000e+00 +9.200000000000001066e+00,4.500000000000000000e+00,0.000000000000000000e+00 +8.800000000000000711e+00,4.500000000000000000e+00,0.000000000000000000e+00 +8.400000000000000355e+00,4.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,4.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,1.500000000000000000e+00,0.000000000000000000e+00 +7.607506869339657918e+00,1.511012294875284212e+00,0.000000000000000000e+00 +7.216248667276845552e+00,1.544014530747301883e+00,0.000000000000000000e+00 +6.827456436866845557e+00,1.598902870372349128e+00,0.000000000000000000e+00 +6.442353462305799638e+00,1.675504614727234554e+00,0.000000000000000000e+00 +6.062151420026054893e+00,1.773578746384302463e+00,0.000000000000000000e+00 +5.688046566313831320e+00,1.892816687841427203e+00,0.000000000000000000e+00 +5.321215973444371805e+00,2.032843272420993053e+00,0.000000000000000000e+00 +4.962813826177093546e+00,2.193217924683065867e+00,0.000000000000000000e+00 +4.613967790263322044e+00,2.373436046638723873e+00,0.000000000000000000e+00 +4.275775464392644665e+00,2.572930605402010684e+00,0.000000000000000000e+00 +3.949300926741360129e+00,2.791073917285178396e+00,0.000000000000000000e+00 +3.635571386988865505e+00,3.027179622723791041e+00,0.000000000000000000e+00 +3.335573954335972147e+00,3.280504845817840298e+00,0.000000000000000000e+00 +3.050252531694168212e+00,3.550252531694167324e+00,0.000000000000000000e+00 +2.780504845817841186e+00,3.835573954335972147e+00,0.000000000000000000e+00 +2.527179622723791041e+00,4.135571386988864617e+00,0.000000000000000000e+00 +2.291073917285178396e+00,4.449300926741359241e+00,0.000000000000000000e+00 +2.072930605402010684e+00,4.775775464392642888e+00,0.000000000000000000e+00 +1.873436046638723873e+00,5.113967790263321156e+00,0.000000000000000000e+00 +1.693217924683066755e+00,5.462813826177091769e+00,0.000000000000000000e+00 +1.532843272420993053e+00,5.821215973444370917e+00,0.000000000000000000e+00 +1.392816687841427203e+00,6.188046566313829544e+00,0.000000000000000000e+00 +1.273578746384303351e+00,6.562151420026054005e+00,0.000000000000000000e+00 +1.175504614727234554e+00,6.942353462305798750e+00,0.000000000000000000e+00 +1.098902870372349128e+00,7.327456436866844669e+00,0.000000000000000000e+00 +1.044014530747301883e+00,7.716248667276843776e+00,0.000000000000000000e+00 +1.011012294875284212e+00,8.107506869339657030e+00,0.000000000000000000e+00 +1.000000000000000000e+00,8.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,4.500000000000000000e+00,0.000000000000000000e+00 +7.607931438681757363e+00,4.519261093311212285e+00,0.000000000000000000e+00 +7.219638711935487230e+00,4.576858878387078278e+00,0.000000000000000000e+00 +6.838861290982151786e+00,4.672238657071163814e+00,0.000000000000000000e+00 +6.469266270539641539e+00,4.804481869954853046e+00,0.000000000000000000e+00 +6.114413052696009210e+00,4.972314942606580246e+00,0.000000000000000000e+00 +5.777719067921592178e+00,5.174121550789818613e+00,0.000000000000000000e+00 +5.462426863345418937e+00,5.407958186549051582e+00,0.000000000000000000e+00 +5.171572875253810153e+00,5.671572875253810153e+00,0.000000000000000000e+00 +4.907958186549052471e+00,5.962426863345418049e+00,0.000000000000000000e+00 +4.674121550789818613e+00,6.277719067921591289e+00,0.000000000000000000e+00 +4.472314942606580246e+00,6.614413052696008322e+00,0.000000000000000000e+00 +4.304481869954853046e+00,6.969266270539640651e+00,0.000000000000000000e+00 +4.172238657071164702e+00,7.338861290982150010e+00,0.000000000000000000e+00 +4.076858878387078278e+00,7.719638711935485453e+00,0.000000000000000000e+00 +4.019261093311213173e+00,8.107931438681756475e+00,0.000000000000000000e+00 +4.000000000000000000e+00,8.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,1.250000000000000000e+01,0.000000000000000000e+00 +7.607931438681757363e+00,1.248073890668878860e+01,0.000000000000000000e+00 +7.219638711935487230e+00,1.242314112161292172e+01,0.000000000000000000e+00 +6.838861290982151786e+00,1.232776134292883619e+01,0.000000000000000000e+00 +6.469266270539641539e+00,1.219551813004514784e+01,0.000000000000000000e+00 +6.114413052696009210e+00,1.202768505739341975e+01,0.000000000000000000e+00 +5.777719067921592178e+00,1.182587844921018139e+01,0.000000000000000000e+00 +5.462426863345418937e+00,1.159204181345094753e+01,0.000000000000000000e+00 +5.171572875253810153e+00,1.132842712474618985e+01,0.000000000000000000e+00 +4.907958186549052471e+00,1.103757313665458284e+01,0.000000000000000000e+00 +4.674121550789818613e+00,1.072228093207840871e+01,0.000000000000000000e+00 +4.472314942606580246e+00,1.038558694730399168e+01,0.000000000000000000e+00 +4.304481869954853046e+00,1.003073372946036024e+01,0.000000000000000000e+00 +4.172238657071164702e+00,9.661138709017849990e+00,0.000000000000000000e+00 +4.076858878387078278e+00,9.280361288064513658e+00,0.000000000000000000e+00 +4.019261093311213173e+00,8.892068561318243525e+00,0.000000000000000000e+00 +4.000000000000000000e+00,8.500000000000000000e+00,0.000000000000000000e+00 +8.000000000000000000e+00,1.550000000000000000e+01,0.000000000000000000e+00 +7.607142857142856762e+00,1.550000000000000000e+01,0.000000000000000000e+00 +7.214285714285714413e+00,1.550000000000000000e+01,0.000000000000000000e+00 +6.821428571428571175e+00,1.550000000000000000e+01,0.000000000000000000e+00 +6.428571428571428825e+00,1.550000000000000000e+01,0.000000000000000000e+00 +6.035714285714285587e+00,1.550000000000000000e+01,0.000000000000000000e+00 +5.642857142857142350e+00,1.550000000000000000e+01,0.000000000000000000e+00 +5.250000000000000000e+00,1.550000000000000000e+01,0.000000000000000000e+00 +4.857142857142857650e+00,1.550000000000000000e+01,0.000000000000000000e+00 +4.464285714285713524e+00,1.550000000000000000e+01,0.000000000000000000e+00 +4.071428571428571175e+00,1.550000000000000000e+01,0.000000000000000000e+00 +3.678571428571428825e+00,1.550000000000000000e+01,0.000000000000000000e+00 +3.285714285714285587e+00,1.550000000000000000e+01,0.000000000000000000e+00 +2.892857142857142350e+00,1.550000000000000000e+01,0.000000000000000000e+00 +2.500000000000000000e+00,1.550000000000000000e+01,0.000000000000000000e+00 +1.000000000000000444e+00,8.500000000000000000e+00,0.000000000000000000e+00 +9.807389066887881590e-01,8.892068561318241748e+00,0.000000000000000000e+00 +9.231411216129221664e-01,9.280361288064513658e+00,0.000000000000000000e+00 +8.277613429288357416e-01,9.661138709017849990e+00,0.000000000000000000e+00 +6.955181300451473980e-01,1.003073372946035846e+01,0.000000000000000000e+00 +5.276850573934206423e-01,1.038558694730399168e+01,0.000000000000000000e+00 +3.258784492101813868e-01,1.072228093207840871e+01,0.000000000000000000e+00 +9.204181345094797351e-02,1.103757313665458284e+01,0.000000000000000000e+00 +-1.715728752538097091e-01,1.132842712474618985e+01,0.000000000000000000e+00 +-4.624268633454176047e-01,1.159204181345094753e+01,0.000000000000000000e+00 +-7.777190679215908453e-01,1.182587844921018139e+01,0.000000000000000000e+00 +-1.114413052696009210e+00,1.202768505739341975e+01,0.000000000000000000e+00 +-1.469266270539641095e+00,1.219551813004514784e+01,0.000000000000000000e+00 +-1.838861290982150232e+00,1.232776134292883619e+01,0.000000000000000000e+00 +-2.219638711935487230e+00,1.242314112161292172e+01,0.000000000000000000e+00 +-2.607931438681757363e+00,1.248073890668878860e+01,0.000000000000000000e+00 +-3.000000000000000000e+00,1.250000000000000000e+01,0.000000000000000000e+00 +2.500000000000000000e+00,1.550000000000000000e+01,0.000000000000000000e+00 +2.107142857142857206e+00,1.550000000000000000e+01,0.000000000000000000e+00 +1.714285714285714413e+00,1.550000000000000000e+01,0.000000000000000000e+00 +1.321428571428571397e+00,1.550000000000000000e+01,0.000000000000000000e+00 +9.285714285714286031e-01,1.550000000000000000e+01,0.000000000000000000e+00 +5.357142857142855874e-01,1.550000000000000000e+01,0.000000000000000000e+00 +1.428571428571427937e-01,1.550000000000000000e+01,0.000000000000000000e+00 +-2.500000000000000000e-01,1.550000000000000000e+01,0.000000000000000000e+00 +-6.428571428571427937e-01,1.550000000000000000e+01,0.000000000000000000e+00 +-1.035714285714286031e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-1.428571428571428825e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-1.821428571428571175e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-2.214285714285714413e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-2.607142857142857650e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-3.000000000000000000e+00,1.550000000000000000e+01,0.000000000000000000e+00 +2.200000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +2.160000000000000142e+01,1.250000000000000000e+01,0.000000000000000000e+00 +2.119999999999999929e+01,1.250000000000000000e+01,0.000000000000000000e+00 +2.080000000000000071e+01,1.250000000000000000e+01,0.000000000000000000e+00 +2.039999999999999858e+01,1.250000000000000000e+01,0.000000000000000000e+00 +2.000000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.960000000000000142e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.919999999999999929e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.880000000000000071e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.839999999999999858e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.800000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.760000000000000142e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.719999999999999929e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.680000000000000071e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.639999999999999858e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.600000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.560000000000000142e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.519999999999999929e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.480000000000000071e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.440000000000000036e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.400000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.359999999999999964e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.319999999999999929e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.280000000000000071e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.240000000000000036e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.200000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.159999999999999964e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.119999999999999929e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.079999999999999893e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.039999999999999858e+01,1.250000000000000000e+01,0.000000000000000000e+00 +1.000000000000000000e+01,1.250000000000000000e+01,0.000000000000000000e+00 +9.600000000000001421e+00,1.250000000000000000e+01,0.000000000000000000e+00 +9.200000000000001066e+00,1.250000000000000000e+01,0.000000000000000000e+00 +8.800000000000000711e+00,1.250000000000000000e+01,0.000000000000000000e+00 +8.400000000000000355e+00,1.250000000000000000e+01,0.000000000000000000e+00 +8.000000000000000000e+00,1.250000000000000000e+01,0.000000000000000000e+00 +2.200000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.160000000000000142e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.119999999999999929e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.080000000000000071e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.039999999999999858e+01,1.550000000000000000e+01,0.000000000000000000e+00 +2.000000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.960000000000000142e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.919999999999999929e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.880000000000000071e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.839999999999999858e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.800000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.760000000000000142e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.719999999999999929e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.680000000000000071e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.639999999999999858e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.600000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.560000000000000142e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.519999999999999929e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.480000000000000071e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.440000000000000036e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.400000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.359999999999999964e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.319999999999999929e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.280000000000000071e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.240000000000000036e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.200000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.159999999999999964e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.119999999999999929e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.079999999999999893e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.039999999999999858e+01,1.550000000000000000e+01,0.000000000000000000e+00 +1.000000000000000000e+01,1.550000000000000000e+01,0.000000000000000000e+00 +9.600000000000001421e+00,1.550000000000000000e+01,0.000000000000000000e+00 +9.200000000000001066e+00,1.550000000000000000e+01,0.000000000000000000e+00 +8.800000000000000711e+00,1.550000000000000000e+01,0.000000000000000000e+00 +8.400000000000000355e+00,1.550000000000000000e+01,0.000000000000000000e+00 +8.000000000000000000e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-3.000000000000000000e+00,1.250000000000000000e+01,0.000000000000000000e+00 +-3.392068561318243081e+00,1.248073890668878860e+01,0.000000000000000000e+00 +-3.780361288064513214e+00,1.242314112161292172e+01,0.000000000000000000e+00 +-4.161138709017849102e+00,1.232776134292883619e+01,0.000000000000000000e+00 +-4.530733729460359349e+00,1.219551813004514784e+01,0.000000000000000000e+00 +-4.885586947303991678e+00,1.202768505739341975e+01,0.000000000000000000e+00 +-5.222280932078408711e+00,1.182587844921018139e+01,0.000000000000000000e+00 +-5.537573136654581951e+00,1.159204181345094753e+01,0.000000000000000000e+00 +-5.828427124746189847e+00,1.132842712474618985e+01,0.000000000000000000e+00 +-6.092041813450948418e+00,1.103757313665458284e+01,0.000000000000000000e+00 +-6.325878449210181387e+00,1.072228093207840871e+01,0.000000000000000000e+00 +-6.527685057393419754e+00,1.038558694730399168e+01,0.000000000000000000e+00 +-6.695518130045147842e+00,1.003073372946036024e+01,0.000000000000000000e+00 +-6.827761342928836186e+00,9.661138709017849990e+00,0.000000000000000000e+00 +-6.923141121612921722e+00,9.280361288064513658e+00,0.000000000000000000e+00 +-6.980738906688787715e+00,8.892068561318243525e+00,0.000000000000000000e+00 +-7.000000000000000000e+00,8.500000000000000000e+00,0.000000000000000000e+00 +-3.000000000000000888e+00,1.550000000000000000e+01,0.000000000000000000e+00 +-3.392493130660342970e+00,1.548898770512471579e+01,0.000000000000000000e+00 +-3.783751332723155336e+00,1.545598546925269900e+01,0.000000000000000000e+00 +-4.172543563133155331e+00,1.540109712962765087e+01,0.000000000000000000e+00 +-4.557646537694201250e+00,1.532449538527276545e+01,0.000000000000000000e+00 +-4.937848579973945995e+00,1.522642125361569754e+01,0.000000000000000000e+00 +-5.311953433686170456e+00,1.510718331215857191e+01,0.000000000000000000e+00 +-5.678784026555629083e+00,1.496715672757900606e+01,0.000000000000000000e+00 +-6.037186173822906454e+00,1.480678207531693502e+01,0.000000000000000000e+00 +-6.386032209736677956e+00,1.462656395336127702e+01,0.000000000000000000e+00 +-6.724224535607355335e+00,1.442706939459798932e+01,0.000000000000000000e+00 +-7.050699073258639871e+00,1.420892608271482160e+01,0.000000000000000000e+00 +-7.364428613011133606e+00,1.397282037727620896e+01,0.000000000000000000e+00 +-7.664426045664027853e+00,1.371949515418215881e+01,0.000000000000000000e+00 +-7.949747468305831788e+00,1.344974746830583356e+01,0.000000000000000000e+00 +-8.219495154182158814e+00,1.316442604566402785e+01,0.000000000000000000e+00 +-8.472820377276208959e+00,1.286442861301113538e+01,0.000000000000000000e+00 +-8.708926082714821604e+00,1.255069907325864165e+01,0.000000000000000000e+00 +-8.927069394597989316e+00,1.222422453560735711e+01,0.000000000000000000e+00 +-9.126563953361275239e+00,1.188603220973667973e+01,0.000000000000000000e+00 +-9.306782075316933245e+00,1.153718617382290823e+01,0.000000000000000000e+00 +-9.467156727579006059e+00,1.117878402655562908e+01,0.000000000000000000e+00 +-9.607183312158571908e+00,1.081195343368617046e+01,0.000000000000000000e+00 +-9.726421253615695761e+00,1.043784857997394688e+01,0.000000000000000000e+00 +-9.824495385272765446e+00,1.005764653769420214e+01,0.000000000000000000e+00 +-9.901097129627650872e+00,9.672543563133155331e+00,0.000000000000000000e+00 +-9.955985469252697229e+00,9.283751332723156224e+00,0.000000000000000000e+00 +-9.988987705124715788e+00,8.892493130660342970e+00,0.000000000000000000e+00 +-1.000000000000000000e+01,8.500000000000000000e+00,0.000000000000000000e+00 +-7.000000000000000000e+00,8.500000000000000000e+00,0.000000000000000000e+00 +-6.988987705124715788e+00,8.107506869339658806e+00,0.000000000000000000e+00 +-6.955985469252697229e+00,7.716248667276845552e+00,0.000000000000000000e+00 +-6.901097129627650872e+00,7.327456436866846445e+00,0.000000000000000000e+00 +-6.824495385272765446e+00,6.942353462305799638e+00,0.000000000000000000e+00 +-6.726421253615697537e+00,6.562151420026054893e+00,0.000000000000000000e+00 +-6.607183312158571908e+00,6.188046566313831320e+00,0.000000000000000000e+00 +-6.467156727579007836e+00,5.821215973444372693e+00,0.000000000000000000e+00 +-6.306782075316933245e+00,5.462813826177093546e+00,0.000000000000000000e+00 +-6.126563953361277015e+00,5.113967790263322044e+00,0.000000000000000000e+00 +-5.927069394597989316e+00,4.775775464392644665e+00,0.000000000000000000e+00 +-5.708926082714821604e+00,4.449300926741361017e+00,0.000000000000000000e+00 +-5.472820377276208959e+00,4.135571386988866394e+00,0.000000000000000000e+00 +-5.219495154182158814e+00,3.835573954335973035e+00,0.000000000000000000e+00 +-4.949747468305833564e+00,3.550252531694168212e+00,0.000000000000000000e+00 +-4.664426045664027853e+00,3.280504845817841186e+00,0.000000000000000000e+00 +-4.364428613011135383e+00,3.027179622723791930e+00,0.000000000000000000e+00 +-4.050699073258643423e+00,2.791073917285181061e+00,0.000000000000000000e+00 +-3.724224535607356223e+00,2.572930605402010684e+00,0.000000000000000000e+00 +-3.386032209736676180e+00,2.373436046638722985e+00,0.000000000000000000e+00 +-3.037186173822907787e+00,2.193217924683066755e+00,0.000000000000000000e+00 +-2.678784026555631748e+00,2.032843272420993941e+00,0.000000000000000000e+00 +-2.311953433686170456e+00,1.892816687841428092e+00,0.000000000000000000e+00 +-1.937848579973943552e+00,1.773578746384302463e+00,0.000000000000000000e+00 +-1.557646537694201694e+00,1.675504614727234554e+00,0.000000000000000000e+00 +-1.172543563133158662e+00,1.598902870372349128e+00,0.000000000000000000e+00 +-7.837513327231557803e-01,1.544014530747301883e+00,0.000000000000000000e+00 +-3.924931306603402503e-01,1.511012294875284212e+00,0.000000000000000000e+00 +-8.417899292546582144e-16,1.500000000000000000e+00,0.000000000000000000e+00 +-1.000000000000000000e+01,8.500000000000001776e+00,0.000000000000000000e+00 +-9.992290362407228343e+00,8.107401842409313275e+00,0.000000000000000000e+00 +-9.969173337331280749e+00,7.715409042721551813e+00,0.000000000000000000e+00 +-9.930684569549264040e+00,7.324626025421626885e+00,0.000000000000000000e+00 +-9.876883405951378592e+00,6.935655349597692698e+00,0.000000000000000000e+00 +-9.807852804032304306e+00,6.549096779838716742e+00,0.000000000000000000e+00 +-9.723699203976767791e+00,6.165546361440947365e+00,0.000000000000000000e+00 +-9.624552364536473448e+00,5.785595501349260594e+00,0.000000000000000000e+00 +-9.510565162951536422e+00,5.409830056250527264e+00,0.000000000000000000e+00 +-9.381913359224842708e+00,5.038829429225069489e+00,0.000000000000000000e+00 +-9.238795325112867829e+00,4.673165676349103848e+00,0.000000000000000000e+00 +-9.081431738250817176e+00,4.313402624625722659e+00,0.000000000000000000e+00 +-8.910065241883678766e+00,3.960095002604533398e+00,0.000000000000000000e+00 +-8.724960070727970418e+00,3.613787585030450167e+00,0.000000000000000000e+00 +-8.526401643540925335e+00,3.275014352840512899e+00,0.000000000000000000e+00 +-8.314696123025456131e+00,2.944297669803980000e+00,0.000000000000000000e+00 +-8.090169943749476289e+00,2.622147477075269961e+00,0.000000000000000000e+00 +-7.853169308807449234e+00,2.309060506901658982e+00,0.000000000000000000e+00 +-7.604059656000311307e+00,2.005519516698164750e+00,0.000000000000000000e+00 +-7.343225094356858662e+00,1.711992544670585481e+00,0.000000000000000000e+00 +-7.071067811865478170e+00,1.428932188134525383e+00,0.000000000000000000e+00 +-6.788007455329418072e+00,1.156774905643144891e+00,0.000000000000000000e+00 +-6.494480483301841467e+00,8.959403439996940222e-01,0.000000000000000000e+00 +-6.190939493098343682e+00,6.468306911925525426e-01,0.000000000000000000e+00 +-5.877852522924733591e+00,4.098300562505272637e-01,0.000000000000000000e+00 +-5.555702330196022665e+00,1.853038769745474212e-01,0.000000000000000000e+00 +-5.224985647159486213e+00,-2.640164354092355836e-02,0.000000000000000000e+00 +-4.886212414969553386e+00,-2.249600707279704181e-01,0.000000000000000000e+00 +-4.539904997395470154e+00,-4.100652418836787660e-01,0.000000000000000000e+00 +-4.186597375374280894e+00,-5.814317382508136234e-01,0.000000000000000000e+00 +-3.826834323650904146e+00,-7.387953251128642762e-01,0.000000000000000000e+00 +-3.461170570774934507e+00,-8.819133592248409315e-01,0.000000000000000000e+00 +-3.090169943749476289e+00,-1.010565162951534646e+00,0.000000000000000000e+00 +-2.714404498650742958e+00,-1.124552364536473448e+00,0.000000000000000000e+00 +-2.334453638559052191e+00,-1.223699203976766015e+00,0.000000000000000000e+00 +-1.950903220161287477e+00,-1.307852804032302529e+00,0.000000000000000000e+00 +-1.564344650402311299e+00,-1.376883405951376815e+00,0.000000000000000000e+00 +-1.175373974578376890e+00,-1.430684569549262264e+00,0.000000000000000000e+00 +-7.845909572784566244e-01,-1.469173337331278972e+00,0.000000000000000000e+00 +-3.925981575906909993e-01,-1.492290362407228343e+00,0.000000000000000000e+00 +-2.725148618421154821e-15,-1.500000000000000000e+00,0.000000000000000000e+00 diff --git a/testing/roadgraph_lane_reader.py b/testing/roadgraph_lane_reader.py new file mode 100644 index 000000000..8bd4cc673 --- /dev/null +++ b/testing/roadgraph_lane_reader.py @@ -0,0 +1,70 @@ +import numpy as np +from typing import List +from GEMstack.state import Roadgraph, Path +from GEMstack.utils import serialization +import os +import matplotlib.pyplot as plt + + +def get_lane_points_from_roadgraph(roadgraph: Roadgraph) -> List: + lane_points = [] + for lane in roadgraph.lanes.values(): + for pts in lane.left.segments: + for pt in pts: + lane_points.append(pt) + for pts in lane.right.segments: + for pt in pts: + lane_points.append(pt) + return lane_points + + +def get_region_points_from_roadgraph(roadgraph: Roadgraph) -> List: + outlines = [] + for region in roadgraph.regions.values(): + outlines.extend(region.outline) + return outlines + + +def plot_roadgraph(roadgraphfn, scale) -> None: + base, ext = os.path.splitext(roadgraphfn) + if ext in ['.json', '.yml', '.yaml']: + with open(roadgraphfn, 'r') as f: + roadgraph = serialization.load(f) + map_type = 'roadgraph' + elif ext in ['.csv', '.txt']: + roadgraph = np.loadtxt(roadgraphfn, delimiter=',', dtype=float) + map_type = 'pointlist' + else: + raise ValueError("Unknown roadgraph file extension", ext) + + if map_type == 'roadgraph': + lane_points = get_lane_points_from_roadgraph(roadgraph) + outlines = np.array(get_region_points_from_roadgraph(roadgraph)) + elif map_type == 'pointlist': + lane_points = roadgraph.points + + # np.savetxt('roadgraph_lane_points.txt', lane_points, delimiter=',') + + lane_points = np.array(lane_points) + x = lane_points[:, 0] * scale + y = lane_points[:, 1] * scale + plt.scatter(x, y) + plt.axis('equal') + if map_type == 'roadgraph': + if len(outlines) > 0: + outlines = np.array(outlines) + p_x = outlines[:, 0] + p_y = outlines[:, 1] + plt.scatter(p_x, p_y) + plt.show() + + +if __name__ == "__main__": + + roadgraphfn = "GEMstack/knowledge/routes/summoning_roadgraph_sim.json" + scale = 1 + plot_roadgraph(roadgraphfn, scale) + + roadgraphfn = "GEMstack/knowledge/routes/summoning_roadgraph_highbay.json" + scale = 1000 + plot_roadgraph(roadgraphfn, scale)