From 4e0c648e0542d454ebd1ef87b3eff3b892a7a409 Mon Sep 17 00:00:00 2001 From: Falko Schindler Date: Wed, 7 Sep 2022 16:22:19 +0200 Subject: [PATCH] add new path planning example --- rosys/pathplanning/demos/20220825-1.py | 612 +++++++++++++++++++++++++ 1 file changed, 612 insertions(+) create mode 100644 rosys/pathplanning/demos/20220825-1.py diff --git a/rosys/pathplanning/demos/20220825-1.py b/rosys/pathplanning/demos/20220825-1.py new file mode 100644 index 000000000..1839543ca --- /dev/null +++ b/rosys/pathplanning/demos/20220825-1.py @@ -0,0 +1,612 @@ +from rosys.geometry import Point, Pose +from rosys.pathplanning.area import Area +from rosys.pathplanning.obstacle import Obstacle +from rosys.pathplanning.planner_process import PlannerSearchCommand + +cmd = PlannerSearchCommand( + deadline=1661432050.9137967, + areas=[ + Area( + id='08d833cb-1f99-41aa-b6c7-fc0e8708a503', type='sand', color='SandyBrown', + outline=[Point(x=1.146224578376175, y=4.425387875450483), + Point(x=0.9706821610039387, y=7.839654798812025), + Point(x=8.949579050727243, y=7.155696569844053), + Point(x=9.807293172045306, y=16.537138256393195), + Point(x=2.2880630975356544, y=20.1152618193402), + Point(x=4.057213764602112, y=23.013797094452606), + Point(x=11.978148009530912, y=23.07176391525105), + Point(x=11.190226738196095, y=16.466462300485624), + Point(x=15.669612048124954, y=14.799233554057755), + Point(x=15.421032122977978, y=6.816139438734554), + Point(x=14.71775326964555, y=-1.3674614413210051), + Point(x=8.681343324808676, y=-1.300644784444895), + Point(x=5.281971106705159, y=-1.942215582873056), + Point(x=2.908936084850697, y=-2.427618946652103), + Point(x=2.7764313226174298, y=-1.0071191861923434), + Point(x=1.0713325290973081, y=-1.4278499911979645), + Point(x=1.096691342953639, y=-0.5967977570602154), + Point(x=0.5999284008922644, y=-0.6343399894289158)], + ), + Area(id='5b5b3ca0-b02d-484a-98db-2a89d2399808', type=None, color='green', outline=[]), + Area( + id='37ae7616-47d0-41fe-82e7-2c888f91eb9e', type=None, color='green', + outline=[Point(x=-0.34751041891048484, y=3.738231702799276), + Point(x=-2.7404637310952373, y=3.8338209724950847), + Point(x=-1.8114199369050352, y=-0.7062530567593677), + Point(x=-1.0277733370784377, y=-0.8153694202638235), + Point(x=-0.7721728650746034, y=-2.0865464362782475), + Point(x=0.4731271878982568, y=-2.0975134173396532), + Point(x=0.6514150551715424, y=-0.9440938388186013), + Point(x=0.8362914160530819, y=1.5379847461661211), + Point(x=0.4206204747119669, y=1.572518674706907), + Point(x=0.4920333776300071, y=2.456979477085203), + Point(x=0.24028086112249092, y=3.0187857986090156)], + ), + ], + obstacles=[ + Obstacle( + id='59a877c0-464d-433c-9263-8d4bb30d7229', + outline=[Point(x=6.296365326510804, y=4.868990084517678), + Point(x=6.161066301474255, y=5.004289109554227), + Point(x=5.96972458529171, y=5.004289109554227), + Point(x=5.834425560255161, y=4.868990084517678), + Point(x=5.834425560255161, y=4.677648368335133), + Point(x=5.96972458529171, y=4.542349343298584), + Point(x=6.161066301474255, y=4.542349343298584), + Point(x=6.296365326510804, y=4.677648368335133)]), + Obstacle( + id='7a3b65ff-0f8d-430b-bc77-0bace75d211c', + outline=[Point(x=6.312039106779131, y=4.35402188610765), + Point(x=6.176740081742582, y=4.4893209111441985), + Point(x=5.9853983655600365, y=4.4893209111441985), + Point(x=5.850099340523488, y=4.35402188610765), + Point(x=5.850099340523488, y=4.162680169925104), + Point(x=5.9853983655600365, y=4.0273811448885555), + Point(x=6.176740081742582, y=4.0273811448885555), + Point(x=6.312039106779131, y=4.162680169925104)]), + Obstacle( + id='5c148848-82c9-4a3b-99fa-b6fde57a5e5f', + outline=[Point(x=6.220304762715677, y=3.8775019022153776), + Point(x=6.085005737679128, y=4.012800927251927), + Point(x=5.8936640214965825, y=4.012800927251927), + Point(x=5.758364996460034, y=3.8775019022153776), + Point(x=5.758364996460034, y=3.686160186032833), + Point(x=5.8936640214965825, y=3.5508611609962837), + Point(x=6.085005737679128, y=3.5508611609962837), + Point(x=6.220304762715677, y=3.686160186032833)]), + Obstacle( + id='c020a17a-5eee-4aae-ab16-fee2b22d8dc9', + outline=[Point(x=7.239568635188525, y=1.3822239721007157), + Point(x=7.104269610151976, y=1.517522997137265), + Point(x=6.912927893969431, y=1.517522997137265), + Point(x=6.777628868932882, y=1.3822239721007157), + Point(x=6.777628868932882, y=1.1908822559181709), + Point(x=6.912927893969431, y=1.0555832308816218), + Point(x=7.104269610151976, y=1.0555832308816215), + Point(x=7.239568635188525, y=1.1908822559181707)]), + Obstacle( + id='a756a137-4559-489e-809d-bc1698c0a83c', + outline=[Point(x=7.868385671293735, y=0.7692628499774048), + Point(x=7.733086646257187, y=0.9045618750139541), + Point(x=7.541744930074641, y=0.9045618750139541), + Point(x=7.406445905038092, y=0.7692628499774049), + Point(x=7.406445905038092, y=0.57792113379486), + Point(x=7.541744930074641, y=0.44262210875831076), + Point(x=7.733086646257187, y=0.44262210875831076), + Point(x=7.868385671293735, y=0.5779211337948598)]), + Obstacle( + id='b2a3b8f6-acbb-49e2-97c1-03963fc7eb8c', + outline=[Point(x=8.615150763026485, y=1.0718548712997849), + Point(x=8.479851737989936, y=1.2071538963363342), + Point(x=8.28851002180739, y=1.2071538963363342), + Point(x=8.153210996770841, y=1.0718548712997849), + Point(x=8.153210996770841, y=0.88051315511724), + Point(x=8.28851002180739, y=0.7452141300806908), + Point(x=8.479851737989936, y=0.7452141300806908), + Point(x=8.615150763026485, y=0.8805131551172398)]), + Obstacle( + id='3dc84c62-76c5-46dc-99cf-1a578438e910', + outline=[Point(x=9.484664022515245, y=1.0257425515756826), + Point(x=9.349364997478697, y=1.161041576612232), + Point(x=9.15802328129615, y=1.161041576612232), + Point(x=9.022724256259602, y=1.0257425515756826), + Point(x=9.022724256259602, y=0.8344008353931378), + Point(x=9.15802328129615, y=0.6991018103565886), + Point(x=9.349364997478697, y=0.6991018103565886), + Point(x=9.484664022515245, y=0.8344008353931376)]), + Obstacle( + id='cb72ee36-cb09-4470-b7ae-efef6018b77b', + outline=[Point(x=10.158502697158694, y=1.1307606188254966), + Point(x=10.023203672122145, y=1.266059643862046), + Point(x=9.8318619559396, y=1.266059643862046), + Point(x=9.69656293090305, y=1.1307606188254966), + Point(x=9.69656293090305, y=0.9394189026429518), + Point(x=9.8318619559396, y=0.8041198776064026), + Point(x=10.023203672122145, y=0.8041198776064026), + Point(x=10.158502697158694, y=0.9394189026429516)]), + Obstacle( + id='9c1471c2-a0c4-4a3a-a513-2134002cf1bc', + outline=[Point(x=11.001813393718066, y=1.1409882665990039), + Point(x=10.866514368681518, y=1.2762872916355532), + Point(x=10.675172652498972, y=1.2762872916355532), + Point(x=10.539873627462423, y=1.1409882665990039), + Point(x=10.539873627462423, y=0.9496465504164591), + Point(x=10.675172652498972, y=0.8143475253799098), + Point(x=10.866514368681518, y=0.8143475253799098), + Point(x=11.001813393718066, y=0.9496465504164588)]), + Obstacle( + id='6f64ee05-1cdc-42f3-81c9-c63be0753020', + outline=[Point(x=10.942595806405645, y=1.7009169963824444), + Point(x=10.807296781369097, y=1.8362160214189938), + Point(x=10.61595506518655, y=1.8362160214189938), + Point(x=10.480656040150002, y=1.7009169963824444), + Point(x=10.480656040150002, y=1.5095752801998996), + Point(x=10.61595506518655, y=1.3742762551633505), + Point(x=10.807296781369097, y=1.3742762551633503), + Point(x=10.942595806405645, y=1.5095752801998994)]), + Obstacle( + id='63a1dc26-5cb9-4d75-8b96-2e0d03772eb3', + outline=[Point(x=10.947455945675285, y=2.2289303488955245), + Point(x=10.812156920638737, y=2.3642293739320737), + Point(x=10.620815204456191, y=2.3642293739320737), + Point(x=10.485516179419642, y=2.2289303488955245), + Point(x=10.485516179419642, y=2.0375886327129797), + Point(x=10.620815204456191, y=1.9022896076764306), + Point(x=10.812156920638737, y=1.9022896076764304), + Point(x=10.947455945675285, y=2.0375886327129797)]), + Obstacle( + id='21e92972-1bbe-4892-8a8d-cbb46906db6f', + outline=[Point(x=10.536854055351622, y=3.046559164978579), + Point(x=10.401555030315073, y=3.181858190015128), + Point(x=10.210213314132528, y=3.181858190015128), + Point(x=10.074914289095979, y=3.046559164978579), + Point(x=10.074914289095979, y=2.8552174487960342), + Point(x=10.210213314132528, y=2.719918423759485), + Point(x=10.401555030315073, y=2.719918423759485), + Point(x=10.536854055351622, y=2.8552174487960342)]), + Obstacle( + id='b10e962d-08d5-4ad1-b41d-bccdb52c5635', + outline=[Point(x=10.574602109806525, y=3.5612047256929804), + Point(x=10.439303084769977, y=3.6965037507295295), + Point(x=10.247961368587431, y=3.6965037507295295), + Point(x=10.112662343550882, y=3.5612047256929804), + Point(x=10.112662343550882, y=3.3698630095104356), + Point(x=10.247961368587431, y=3.2345639844738865), + Point(x=10.439303084769977, y=3.2345639844738865), + Point(x=10.574602109806525, y=3.3698630095104356)]), + Obstacle( + id='db0ebfe0-00e2-46a0-a988-0e3677116dac', + outline=[Point(x=10.591106319533957, y=4.262009404905639), + Point(x=10.455807294497408, y=4.397308429942187), + Point(x=10.264465578314862, y=4.397308429942187), + Point(x=10.129166553278314, y=4.262009404905639), + Point(x=10.129166553278314, y=4.070667688723093), + Point(x=10.264465578314862, y=3.9353686636865444), + Point(x=10.455807294497408, y=3.9353686636865444), + Point(x=10.591106319533957, y=4.070667688723093)]), + Obstacle( + id='e96587e5-0bf0-4952-b151-4cf1b131d7c8', + outline=[Point(x=10.494749407840228, y=4.7216876575639555), + Point(x=10.35945038280368, y=4.856986682600504), + Point(x=10.168108666621134, y=4.856986682600504), + Point(x=10.032809641584585, y=4.7216876575639555), + Point(x=10.032809641584585, y=4.53034594138141), + Point(x=10.168108666621134, y=4.395046916344861), + Point(x=10.35945038280368, y=4.395046916344861), + Point(x=10.494749407840228, y=4.53034594138141)]), + Obstacle( + id='d7af1894-234a-455a-ab79-d2480bc4a434', + outline=[Point(x=10.2880845089587, y=5.3073211971275445), + Point(x=10.152785483922152, y=5.442620222164093), + Point(x=9.961443767739606, y=5.442620222164093), + Point(x=9.826144742703057, y=5.3073211971275445), + Point(x=9.826144742703057, y=5.115979480944999), + Point(x=9.961443767739606, y=4.98068045590845), + Point(x=10.152785483922152, y=4.98068045590845), + Point(x=10.2880845089587, y=5.115979480944999)]), + Obstacle( + id='b9a2f9de-870c-4dd9-a57e-c08a00797316', + outline=[Point(x=9.949141042075288, y=5.6177005236814335), + Point(x=9.81384201703874, y=5.752999548717982), + Point(x=9.622500300856194, y=5.752999548717982), + Point(x=9.487201275819645, y=5.6177005236814335), + Point(x=9.487201275819645, y=5.426358807498888), + Point(x=9.622500300856194, y=5.291059782462339), + Point(x=9.81384201703874, y=5.291059782462339), + Point(x=9.949141042075288, y=5.426358807498888)]), + Obstacle( + id='69441b1f-95a6-40f1-a7b3-35be577711af', + outline=[Point(x=9.420715313278754, y=5.540229732780903), + Point(x=9.285416288242205, y=5.675528757817451), + Point(x=9.09407457205966, y=5.675528757817451), + Point(x=8.95877554702311, y=5.540229732780903), + Point(x=8.95877554702311, y=5.348888016598357), + Point(x=9.09407457205966, y=5.213588991561808), + Point(x=9.285416288242205, y=5.213588991561808), + Point(x=9.420715313278754, y=5.348888016598357)]), + Obstacle( + id='015abc9e-66f1-459d-9b18-a4576ee3ca35', + outline=[Point(x=8.768024750970715, y=5.584179654536951), + Point(x=8.632725725934167, y=5.719478679573499), + Point(x=8.441384009751621, y=5.719478679573499), + Point(x=8.306084984715072, y=5.584179654536951), + Point(x=8.306084984715072, y=5.392837938354405), + Point(x=8.441384009751621, y=5.257538913317856), + Point(x=8.632725725934167, y=5.257538913317856), + Point(x=8.768024750970715, y=5.392837938354405)]), + Obstacle( + id='e29c92b2-4428-4090-ad24-888c66accf20', + outline=[Point(x=8.031963212988098, y=5.511973201797639), + Point(x=7.896664187951549, y=5.647272226834188), + Point(x=7.705322471769003, y=5.647272226834188), + Point(x=7.5700234467324545, y=5.511973201797639), + Point(x=7.5700234467324545, y=5.320631485615094), + Point(x=7.705322471769003, y=5.185332460578545), + Point(x=7.896664187951549, y=5.185332460578545), + Point(x=8.031963212988098, y=5.320631485615094)]), + Obstacle( + id='f55a12d1-9386-467b-b71b-9e688b075607', + outline=[Point(x=7.4972415535701895, y=5.570043868296177), + Point(x=7.361942528533641, y=5.705342893332726), + Point(x=7.170600812351095, y=5.705342893332726), + Point(x=7.0353017873145465, y=5.570043868296177), + Point(x=7.0353017873145465, y=5.378702152113632), + Point(x=7.170600812351095, y=5.243403127077083), + Point(x=7.361942528533641, y=5.243403127077083), + Point(x=7.4972415535701895, y=5.378702152113632)]), + Obstacle( + id='edf471a5-3b0b-4aff-9d1d-682ca45604e8', + outline=[Point(x=6.95419431227138, y=5.571142769009535), + Point(x=6.818895287234831, y=5.706441794046084), + Point(x=6.627553571052285, y=5.706441794046084), + Point(x=6.492254546015737, y=5.571142769009535), + Point(x=6.492254546015737, y=5.3798010528269895), + Point(x=6.627553571052285, y=5.244502027790441), + Point(x=6.818895287234831, y=5.244502027790441), + Point(x=6.95419431227138, y=5.3798010528269895)]), + Obstacle( + id='b29bc768-36ea-4d49-8e20-6f8d058fa3fa', + outline=[Point(x=9.791935486897035, y=3.0877973398581946), + Point(x=9.250739386750839, y=3.6289934400043915), + Point(x=8.48537252202066, y=3.6289934400043915), + Point(x=7.944176421874462, y=3.087797339858195), + Point(x=7.944176421874462, y=2.3224304751280154), + Point(x=8.48537252202066, y=1.7812343749818185), + Point(x=9.250739386750839, y=1.7812343749818185), + Point(x=9.791935486897035, y=2.3224304751280145)]), + Obstacle( + id='932e8fd2-0f7e-4fe1-9d7d-976c89fda59f', + outline=[Point(x=8.039692632073018, y=4.023215022994727), + Point(x=7.498496531926821, y=4.564411123140925), + Point(x=6.733129667196642, y=4.564411123140925), + Point(x=6.1919335670504445, y=4.023215022994727), + Point(x=6.1919335670504445, y=3.257848158264548), + Point(x=6.733129667196641, y=2.7166520581183513), + Point(x=7.498496531926821, y=2.7166520581183513), + Point(x=8.039692632073018, y=3.2578481582645473)]), + Obstacle( + id='265f554b-c62d-4831-bf54-b43652897962', + outline=[Point(x=9.654827779336205, y=4.965743917622191), + Point(x=9.113631679190009, y=5.506940017768389), + Point(x=8.34826481445983, y=5.506940017768389), + Point(x=7.807068714313632, y=4.965743917622191), + Point(x=7.807068714313632, y=4.200377052892012), + Point(x=8.34826481445983, y=3.6591809527458152), + Point(x=9.113631679190009, y=3.6591809527458152), + Point(x=9.654827779336205, y=4.200377052892011)]), + Obstacle( + id='e5b95fd7-e30f-4ded-b972-b766fd398b7f', + outline=[Point(x=8.396538673919764, y=2.1600134940843), + Point(x=7.855342573773567, y=2.7012095942304972), + Point(x=7.0899757090433875, y=2.7012095942304972), + Point(x=6.54877960889719, y=2.1600134940843003), + Point(x=6.54877960889719, y=1.3946466293541206), + Point(x=7.089975709043387, y=0.8534505292079237), + Point(x=7.855342573773567, y=0.8534505292079236), + Point(x=8.396538673919764, y=1.3946466293541198)]), + Obstacle( + id='961187a9-9d4a-4236-ba92-ca3d66a124d7', + outline=[Point(x=10.926036061918742, y=2.2143764700150665), + Point(x=10.384839961772546, y=2.755572570161264), + Point(x=9.619473097042366, y=2.755572570161264), + Point(x=9.07827699689617, y=2.214376470015067), + Point(x=9.07827699689617, y=1.4490096052848873), + Point(x=9.619473097042366, y=0.9078135051386904), + Point(x=10.384839961772546, y=0.9078135051386903), + Point(x=10.926036061918742, y=1.4490096052848864)]), + Obstacle( + id='32f2178b-7c00-4c35-b18f-e45fefb9aaba', + outline=[Point(x=8.081746147787522, y=5.031827907609519), + Point(x=7.540550047641325, y=5.573024007755716), + Point(x=6.775183182911146, y=5.573024007755716), + Point(x=6.233987082764949, y=5.031827907609519), + Point(x=6.233987082764949, y=4.26646104287934), + Point(x=6.775183182911145, y=3.725264942733143), + Point(x=7.540550047641325, y=3.725264942733143), + Point(x=8.081746147787522, y=4.266461042879339)]), + Obstacle( + id='9d58a290-7fa1-4d12-b97b-c4b2b3de5440', + outline=[Point(x=6.314404170571617, y=5.297584630313086), + Point(x=6.179105145535068, y=5.4328836553496345), + Point(x=5.987763429352523, y=5.4328836553496345), + Point(x=5.852464404315974, y=5.297584630313086), + Point(x=5.852464404315974, y=5.10624291413054), + Point(x=5.987763429352523, y=4.970943889093991), + Point(x=6.179105145535068, y=4.970943889093991), + Point(x=6.314404170571617, y=5.10624291413054)]), + Obstacle( + id='66221228-1dc4-4781-a192-bfd3e0818c8a', + outline=[Point(x=6.550659932738968, y=5.573284335110351), + Point(x=6.41536090770242, y=5.7085833601469), + Point(x=6.224019191519874, y=5.7085833601469), + Point(x=6.088720166483325, y=5.573284335110351), + Point(x=6.088720166483325, y=5.3819426189278055), + Point(x=6.224019191519874, y=5.246643593891257), + Point(x=6.41536090770242, y=5.246643593891257), + Point(x=6.550659932738968, y=5.3819426189278055)]), + Obstacle( + id='715cd333-5cb7-4237-8208-cbb6fe89c592', + outline=[Point(x=6.026676904542507, y=4.626362412034945), + Point(x=5.891377879505958, y=4.761661437071494), + Point(x=5.7000361633234125, y=4.761661437071494), + Point(x=5.564737138286864, y=4.626362412034945), + Point(x=5.564737138286864, y=4.435020695852399), + Point(x=5.7000361633234125, y=4.299721670815851), + Point(x=5.891377879505958, y=4.299721670815851), + Point(x=6.026676904542507, y=4.435020695852399)]), + Obstacle( + id='ac8ec050-f96b-49e5-9ed0-3191080e398f', + outline=[Point(x=6.102573569865851, y=5.028131479910531), + Point(x=5.967274544829302, y=5.16343050494708), + Point(x=5.775932828646757, y=5.16343050494708), + Point(x=5.640633803610208, y=5.028131479910531), + Point(x=5.640633803610208, y=4.836789763727985), + Point(x=5.775932828646757, y=4.701490738691437), + Point(x=5.967274544829302, y=4.701490738691437), + Point(x=6.102573569865851, y=4.836789763727985)]), + Obstacle( + id='acf071ac-8338-43cc-8200-c34eef13f1de', + outline=[Point(x=5.961978178496638, y=4.124091238782518), + Point(x=5.8266791534600895, y=4.259390263819067), + Point(x=5.635337437277544, y=4.259390263819067), + Point(x=5.500038412240995, y=4.124091238782518), + Point(x=5.500038412240995, y=3.932749522599973), + Point(x=5.635337437277544, y=3.797450497563424), + Point(x=5.8266791534600895, y=3.797450497563424), + Point(x=5.961978178496638, y=3.932749522599973)]), + Obstacle( + id='3683fc8a-96c4-4f2e-bcb7-9fd1b07a7a56', + outline=[Point(x=5.85634492688675, y=3.5317551115567047), + Point(x=5.721045901850201, y=3.667054136593254), + Point(x=5.529704185667655, y=3.667054136593254), + Point(x=5.3944051606311065, y=3.5317551115567047), + Point(x=5.3944051606311065, y=3.34041339537416), + Point(x=5.529704185667655, y=3.205114370337611), + Point(x=5.721045901850201, y=3.205114370337611), + Point(x=5.85634492688675, y=3.34041339537416)]), + Obstacle( + id='4781039e-9c78-4db6-8b26-4f8b9fcb43f3', + outline=[Point(x=5.834262627222849, y=3.06144621712586), + Point(x=5.6989636021863, y=3.196745242162409), + Point(x=5.507621886003754, y=3.196745242162409), + Point(x=5.372322860967206, y=3.06144621712586), + Point(x=5.372322860967206, y=2.8701045009433153), + Point(x=5.507621886003754, y=2.734805475906766), + Point(x=5.6989636021863, y=2.734805475906766), + Point(x=5.834262627222849, y=2.8701045009433153)]), + Obstacle( + id='5ec6dc1e-c10c-4e1b-b535-933acd608eee', + outline=[Point(x=6.04485557645574, y=2.597657357867583), + Point(x=5.9095565514191915, y=2.732956382904132), + Point(x=5.718214835236646, y=2.732956382904132), + Point(x=5.582915810200097, y=2.597657357867583), + Point(x=5.582915810200097, y=2.406315641685038), + Point(x=5.718214835236646, y=2.271016616648489), + Point(x=5.9095565514191915, y=2.271016616648489), + Point(x=6.04485557645574, y=2.406315641685038)]), + Obstacle( + id='f3580100-7a58-43a3-9aa5-f22b0a7edb30', + outline=[Point(x=6.313379134834419, y=2.259537902064299), + Point(x=6.17808010979787, y=2.3948369271008483), + Point(x=5.986738393615324, y=2.3948369271008483), + Point(x=5.851439368578776, y=2.259537902064299), + Point(x=5.851439368578776, y=2.0681961858817544), + Point(x=5.986738393615324, y=1.9328971608452052), + Point(x=6.17808010979787, y=1.932897160845205), + Point(x=6.313379134834419, y=2.0681961858817544)]), + Obstacle( + id='d99c6e33-bca3-45d7-a86f-8b31937c008e', + outline=[Point(x=6.58366844562663, y=1.8555107024286004), + Point(x=6.4483694205900814, y=1.9908097274651497), + Point(x=6.257027704407536, y=1.9908097274651497), + Point(x=6.121728679370987, y=1.8555107024286004), + Point(x=6.121728679370987, y=1.6641689862460556), + Point(x=6.257027704407536, y=1.5288699612095065), + Point(x=6.4483694205900814, y=1.5288699612095062), + Point(x=6.58366844562663, y=1.6641689862460554)]), + Obstacle( + id='de3663e6-46f6-466f-b9e4-612b563a3378', + outline=[Point(x=6.718832695440436, y=2.3729283150510176), + Point(x=6.583533670403887, y=2.5082273400875668), + Point(x=6.3921919542213415, y=2.5082273400875668), + Point(x=6.256892929184793, y=2.3729283150510176), + Point(x=6.256892929184793, y=2.181586598868473), + Point(x=6.3921919542213415, y=2.0462875738319237), + Point(x=6.583533670403887, y=2.0462875738319237), + Point(x=6.718832695440436, y=2.181586598868473)]), + Obstacle( + id='109a76a6-796a-4f08-87af-170d0a1c0dbe', + outline=[Point(x=6.486445445933696, y=2.827635230122303), + Point(x=6.351146420897147, y=2.962934255158852), + Point(x=6.159804704714602, y=2.962934255158852), + Point(x=6.024505679678053, y=2.827635230122303), + Point(x=6.024505679678053, y=2.636293513939758), + Point(x=6.159804704714602, y=2.500994488903209), + Point(x=6.351146420897147, y=2.500994488903209), + Point(x=6.486445445933696, y=2.636293513939758)]), + Obstacle( + id='38c06854-eced-4794-bc67-e222e36e7ecd', + outline=[Point(x=6.289385383465441, y=3.1063917477555996), + Point(x=6.154086358428892, y=3.2416907727921487), + Point(x=5.962744642246347, y=3.2416907727921487), + Point(x=5.827445617209798, y=3.1063917477555996), + Point(x=5.827445617209798, y=2.9150500315730548), + Point(x=5.962744642246347, y=2.7797510065365056), + Point(x=6.154086358428892, y=2.7797510065365056), + Point(x=6.289385383465441, y=2.9150500315730548)]), + Obstacle( + id='761ad946-33bc-46ee-8c6e-c5c32ac96fad', + outline=[Point(x=6.202834320589403, y=3.4358025093896694), + Point(x=6.0675352955528545, y=3.5711015344262185), + Point(x=5.876193579370309, y=3.5711015344262185), + Point(x=5.74089455433376, y=3.4358025093896694), + Point(x=5.74089455433376, y=3.2444607932071245), + Point(x=5.876193579370309, y=3.1091617681705754), + Point(x=6.0675352955528545, y=3.1091617681705754), + Point(x=6.202834320589403, y=3.2444607932071245)]), + Obstacle( + id='c3f13ca1-3316-4d00-a4ef-c8ce2ac4dc51', + outline=[Point(x=9.113972771621876, y=1.027953938024135), + Point(x=8.978673746585327, y=1.1632529630606843), + Point(x=8.787332030402782, y=1.1632529630606843), + Point(x=8.652033005366233, y=1.027953938024135), + Point(x=8.652033005366233, y=0.8366122218415901), + Point(x=8.787332030402782, y=0.7013131968050409), + Point(x=8.978673746585327, y=0.7013131968050409), + Point(x=9.113972771621876, y=0.8366122218415899)]), + Obstacle( + id='56f7a20e-197e-41eb-a2d9-6c776892e08f', + outline=[Point(x=5.659502232810734, y=2.713610589631258), + Point(x=5.524203207774185, y=2.8489096146678072), + Point(x=5.33286149159164, y=2.8489096146678072), + Point(x=5.197562466555091, y=2.713610589631258), + Point(x=5.197562466555091, y=2.5222688734487133), + Point(x=5.33286149159164, y=2.386969848412164), + Point(x=5.524203207774185, y=2.386969848412164), + Point(x=5.659502232810734, y=2.5222688734487133)]), + Obstacle( + id='74243bd4-1c80-41b1-aea7-38b5a750d185', + outline=[Point(x=5.481404586782492, y=2.9394621914655206), + Point(x=5.346105561745944, y=3.0747612165020697), + Point(x=5.154763845563398, y=3.0747612165020697), + Point(x=5.019464820526849, y=2.9394621914655206), + Point(x=5.019464820526849, y=2.7481204752829758), + Point(x=5.154763845563398, y=2.6128214502464266), + Point(x=5.346105561745944, y=2.6128214502464266), + Point(x=5.481404586782492, y=2.7481204752829758)]), + Obstacle( + id='ee2d3837-2798-4eef-adfa-90e7c49fef41', + outline=[Point(x=5.565018933025964, y=3.3868224662070947), + Point(x=5.429719907989416, y=3.522121491243644), + Point(x=5.23837819180687, y=3.522121491243644), + Point(x=5.103079166770321, y=3.3868224662070947), + Point(x=5.103079166770321, y=3.19548075002455), + Point(x=5.23837819180687, y=3.060181724988001), + Point(x=5.429719907989416, y=3.060181724988001), + Point(x=5.565018933025964, y=3.19548075002455)]), + Obstacle( + id='dfe929e1-1580-46d1-beb2-fe627ef9b3fe', + outline=[Point(x=5.877213551991315, y=2.2690118305608697), + Point(x=5.7419145269547665, y=2.404310855597419), + Point(x=5.550572810772221, y=2.404310855597419), + Point(x=5.415273785735672, y=2.2690118305608697), + Point(x=5.415273785735672, y=2.077670114378325), + Point(x=5.550572810772221, y=1.9423710893417758), + Point(x=5.7419145269547665, y=1.9423710893417756), + Point(x=5.877213551991315, y=2.077670114378325)]), + Obstacle( + id='a65cdb76-4af7-4284-981f-c30164bd91fa', + outline=[Point(x=6.14405946938137, y=1.889121800844437), + Point(x=6.008760444344821, y=2.0244208258809864), + Point(x=5.817418728162275, y=2.0244208258809864), + Point(x=5.682119703125727, y=1.889121800844437), + Point(x=5.682119703125727, y=1.6977800846618922), + Point(x=5.817418728162275, y=1.562481059625343), + Point(x=6.008760444344821, y=1.562481059625343), + Point(x=6.14405946938137, y=1.697780084661892)]), + Obstacle( + id='db07e97c-8123-4572-8cea-901f588b8878', + outline=[Point(x=5.581434479995497, y=3.6824658224270017), + Point(x=5.446135454958949, y=3.817764847463551), + Point(x=5.254793738776403, y=3.817764847463551), + Point(x=5.119494713739854, y=3.6824658224270017), + Point(x=5.119494713739854, y=3.491124106244457), + Point(x=5.254793738776403, y=3.355825081207908), + Point(x=5.446135454958949, y=3.355825081207908), + Point(x=5.581434479995497, y=3.491124106244457)]), + Obstacle( + id='57bf7897-1e0e-4163-afcd-fafcba9dacf2', + outline=[Point(x=5.80917096313334, y=3.8596274791038887), + Point(x=5.6738719380967915, y=3.994926504140438), + Point(x=5.482530221914246, y=3.994926504140438), + Point(x=5.347231196877697, y=3.8596274791038887), + Point(x=5.347231196877697, y=3.668285762921344), + Point(x=5.482530221914246, y=3.532986737884795), + Point(x=5.6738719380967915, y=3.532986737884795), + Point(x=5.80917096313334, y=3.668285762921344)]), + Obstacle( + id='7e5c52e1-89f3-4aca-bb4a-1e6778064493', + outline=[Point(x=9.201846898496907, y=1.5577594142132916), + Point(x=9.066547873460358, y=1.693058439249841), + Point(x=8.875206157277812, y=1.693058439249841), + Point(x=8.739907132241264, y=1.5577594142132916), + Point(x=8.739907132241264, y=1.3664176980307468), + Point(x=8.875206157277812, y=1.2311186729941976), + Point(x=9.066547873460358, y=1.2311186729941974), + Point(x=9.201846898496907, y=1.3664176980307465)]), + Obstacle( + id='84e4aa8d-91b5-44f3-831c-052028146c32', + outline=[Point(x=8.755295215410008, y=1.487278838024226), + Point(x=8.61999619037346, y=1.6225778630607752), + Point(x=8.428654474190914, y=1.6225778630607752), + Point(x=8.293355449154365, y=1.487278838024226), + Point(x=8.293355449154365, y=1.295937121841681), + Point(x=8.428654474190914, y=1.160638096805132), + Point(x=8.61999619037346, y=1.1606380968051317), + Point(x=8.755295215410008, y=1.2959371218416809)]), + Obstacle( + id='830a6a5f-b0c7-4944-b1fd-897507db7fc9', + outline=[Point(x=5.693492261392962, y=4.425820831193372), + Point(x=5.558193236356414, y=4.56111985622992), + Point(x=5.366851520173868, y=4.56111985622992), + Point(x=5.231552495137319, y=4.425820831193372), + Point(x=5.231552495137319, y=4.234479115010826), + Point(x=5.366851520173868, y=4.099180089974277), + Point(x=5.558193236356414, y=4.099180089974277), + Point(x=5.693492261392962, y=4.234479115010826)]), + Obstacle( + id='61d7c75b-1f53-43ed-bf7c-da08904d151b', + outline=[Point(x=5.821303806691985, y=4.931706957901849), + Point(x=5.686004781655436, y=5.067005982938397), + Point(x=5.49466306547289, y=5.067005982938397), + Point(x=5.359364040436342, y=4.931706957901849), + Point(x=5.359364040436342, y=4.740365241719303), + Point(x=5.49466306547289, y=4.605066216682754), + Point(x=5.686004781655436, y=4.605066216682754), + Point(x=5.821303806691985, y=4.740365241719303)]), + Obstacle( + id='d6b2b8c8-4487-41c1-912d-c2cd33f8f61e', + outline=[Point(x=5.953070246536902, y=5.3858675918024685), + Point(x=5.817771221500354, y=5.521166616839017), + Point(x=5.626429505317808, y=5.521166616839017), + Point(x=5.491130480281259, y=5.3858675918024685), + Point(x=5.491130480281259, y=5.194525875619923), + Point(x=5.626429505317808, y=5.059226850583374), + Point(x=5.817771221500354, y=5.059226850583374), + Point(x=5.953070246536902, y=5.194525875619923)]), + Obstacle( + id='88aff2f9-f23b-4cf4-9f55-1d5be8063b39', + outline=[Point(x=6.27612635718415, y=5.701379811739512), + Point(x=6.140827332147602, y=5.836678836776061), + Point(x=5.949485615965056, y=5.836678836776061), + Point(x=5.814186590928507, y=5.701379811739512), + Point(x=5.814186590928507, y=5.5100380955569666), + Point(x=5.949485615965056, y=5.374739070520418), + Point(x=6.140827332147602, y=5.374739070520418), + Point(x=6.27612635718415, y=5.5100380955569666)]), + ], + start=Pose(x=3.963393516663482, y=5.864180086200984, yaw=2.806318498078692, time=1661432040.8997905), + goal=Pose(x=7.728188290118309, y=6.05330238815289, yaw=0.34906585039886606, time=0), +) + +robot_outline = [(-0.22, -0.36), (1.07, -0.36), (1.17, 0), (1.07, 0.36), (-0.22, 0.36)]