diff --git a/rb_ws/src/buggy/launch/main.launch b/rb_ws/src/buggy/launch/main.launch
index bb2c61c0..a0a2c812 100644
--- a/rb_ws/src/buggy/launch/main.launch
+++ b/rb_ws/src/buggy/launch/main.launch
@@ -2,6 +2,7 @@
+
@@ -23,10 +24,8 @@
-
-
-
+
\ No newline at end of file
diff --git a/rb_ws/src/buggy/launch/sim_2d.launch b/rb_ws/src/buggy/launch/sim_2d.launch
index 2f3cef01..abaff00c 100644
--- a/rb_ws/src/buggy/launch/sim_2d.launch
+++ b/rb_ws/src/buggy/launch/sim_2d.launch
@@ -4,6 +4,7 @@
+
@@ -12,5 +13,5 @@
-
+
diff --git a/rb_ws/src/buggy/paths/buggycourse_safe_1.json b/rb_ws/src/buggy/paths/buggycourse_safe_1.json
new file mode 100644
index 00000000..e2022dc5
--- /dev/null
+++ b/rb_ws/src/buggy/paths/buggycourse_safe_1.json
@@ -0,0 +1,680 @@
+[
+ {
+ "key": "660f21fb-d1af-4352-a78e-e8a3aed41aee",
+ "lat": 40.441781330791706,
+ "lon": -79.94158212192264,
+ "active": false
+ },
+ {
+ "key": "65fb844f-98b9-4db4-959e-834408f188cc",
+ "lat": 40.44060811370255,
+ "lon": -79.94208769708543,
+ "active": false
+ },
+ {
+ "key": "8445d8e4-74f9-4f6b-ac44-1597a9688529",
+ "lat": 40.44053455688075,
+ "lon": -79.9421199313416,
+ "active": false
+ },
+ {
+ "key": "5ab671c4-8329-44a2-87a6-534f7ed822d8",
+ "lat": 40.44046112238632,
+ "lon": -79.94215679446907,
+ "active": false
+ },
+ {
+ "key": "92f92ae3-200d-4dce-820e-ef8ab7251061",
+ "lat": 40.44038431234247,
+ "lon": -79.94220226578663,
+ "active": false
+ },
+ {
+ "key": "2fce0aee-2a9f-48f6-a63e-cce562cd47fc",
+ "lat": 40.44028905295606,
+ "lon": -79.94226582261865,
+ "active": false
+ },
+ {
+ "key": "2b54add1-4b4e-47c4-970a-f493b0732764",
+ "lat": 40.44026982005531,
+ "lon": -79.9422891522463,
+ "active": false
+ },
+ {
+ "key": "c0cb492a-b102-4909-b95b-4a3c659eaca4",
+ "lat": 40.44025242283363,
+ "lon": -79.94231493393156,
+ "active": false
+ },
+ {
+ "key": "b75abe03-3c4b-4e39-8cac-231cd8024d66",
+ "lat": 40.44023946745282,
+ "lon": -79.94234412074486,
+ "active": false
+ },
+ {
+ "key": "b493928a-6273-432b-b128-6d57456d96be",
+ "lat": 40.44022318068476,
+ "lon": -79.94238644162438,
+ "active": false
+ },
+ {
+ "key": "98cdc52b-ae6c-4bee-944c-9f14c1f4877e",
+ "lat": 40.44021096560622,
+ "lon": -79.94242195224734,
+ "active": false
+ },
+ {
+ "key": "55f98766-ec27-4eb9-826a-6df806479158",
+ "lat": 40.44019726990932,
+ "lon": -79.94247789363976,
+ "active": false
+ },
+ {
+ "key": "ba653a6f-53fe-4c03-af28-8c48a5bc001c",
+ "lat": 40.44019078043659,
+ "lon": -79.9425236048339,
+ "active": false
+ },
+ {
+ "key": "9ca6c396-418d-460b-a9d5-d503b5283386",
+ "lat": 40.44018431451843,
+ "lon": -79.9425688592082,
+ "active": false
+ },
+ {
+ "key": "80f9464f-cb48-4cc6-b924-6a924309fe9d",
+ "lat": 40.44017987266922,
+ "lon": -79.94261263942838,
+ "active": false
+ },
+ {
+ "key": "0871c8ac-3c7b-4388-835b-a76d535c3ef4",
+ "lat": 40.44017209943271,
+ "lon": -79.9426797690989,
+ "active": false
+ },
+ {
+ "key": "4850ff5a-5ba5-4bd8-9124-54276c57af02",
+ "lat": 40.44016247542434,
+ "lon": -79.94275030389802,
+ "active": false
+ },
+ {
+ "key": "16a81f9f-33ee-4fa1-86a7-db216dd58f90",
+ "lat": 40.440149520026196,
+ "lon": -79.9428115962061,
+ "active": false
+ },
+ {
+ "key": "be2bdad0-fcf1-40e2-bc7a-57c9547add9f",
+ "lat": 40.44013767508839,
+ "lon": -79.94286705115157,
+ "active": false
+ },
+ {
+ "key": "394e3113-5b89-46a1-b9d0-237d1999ad4e",
+ "lat": 40.44012138829583,
+ "lon": -79.94292445188462,
+ "active": false
+ },
+ {
+ "key": "a09218a8-125a-42f8-9c91-03e7fb2130c2",
+ "lat": 40.440096217790924,
+ "lon": -79.94299060866157,
+ "active": false
+ },
+ {
+ "key": "87a091aa-0cbd-4b06-908a-b2dc8b4a6e71",
+ "lat": 40.44007141743073,
+ "lon": -79.94304898228845,
+ "active": false
+ },
+ {
+ "key": "c4e5cfee-5f49-4f7e-9138-dc71169927fe",
+ "lat": 40.440048837990226,
+ "lon": -79.94309908631813,
+ "active": false
+ },
+ {
+ "key": "9c9ec749-1f64-4bbb-bd42-afa4a255c5e2",
+ "lat": 40.440019225597815,
+ "lon": -79.94315064968849,
+ "active": false
+ },
+ {
+ "key": "94b38ed7-d3e9-4d9b-86cb-64c4482379fd",
+ "lat": 40.43998554148566,
+ "lon": -79.94320853686844,
+ "active": false
+ },
+ {
+ "key": "d690ac36-e9fa-4d76-ab5e-f4e44f6c95fa",
+ "lat": 40.43995333797838,
+ "lon": -79.94325474932302,
+ "active": false
+ },
+ {
+ "key": "e7805962-8a12-4408-9b98-34da0a8b4a23",
+ "lat": 40.439917803055586,
+ "lon": -79.94329609730869,
+ "active": false
+ },
+ {
+ "key": "e88cebd9-0cc7-4350-a024-ba02408fad2c",
+ "lat": 40.43988448904825,
+ "lon": -79.94332966214408,
+ "active": false
+ },
+ {
+ "key": "7042079e-7696-4e8f-9d38-3e636a8bf788",
+ "lat": 40.43983488816252,
+ "lon": -79.94337927972676,
+ "active": false
+ },
+ {
+ "key": "c2e38668-261a-4f68-98f8-1b8b827785cd",
+ "lat": 40.43959354601643,
+ "lon": -79.94360353174336,
+ "active": false
+ },
+ {
+ "key": "602cb8a9-49f7-4db6-86c7-5c9a4b315eb7",
+ "lat": 40.439478426988295,
+ "lon": -79.94371736031553,
+ "active": false
+ },
+ {
+ "key": "3f876251-a070-4227-bbe9-5dcdc49785d9",
+ "lat": 40.439302601725515,
+ "lon": -79.94389296764275,
+ "active": false
+ },
+ {
+ "key": "3a1abfdf-fb00-4662-b0e1-43350ae95ac9",
+ "lat": 40.43916157102462,
+ "lon": -79.94403598302821,
+ "active": false
+ },
+ {
+ "key": "a99422d4-e510-4ec1-8a02-43737f42a7ce",
+ "lat": 40.43911900264465,
+ "lon": -79.94407927680123,
+ "active": false
+ },
+ {
+ "key": "a308e8ca-312f-400c-a8f2-b30891521dbc",
+ "lat": 40.43908864952238,
+ "lon": -79.94411186874291,
+ "active": false
+ },
+ {
+ "key": "c94fad99-a51c-4dc6-85e6-d8feafdcce5c",
+ "lat": 40.43906347863029,
+ "lon": -79.94413716398121,
+ "active": false
+ },
+ {
+ "key": "234b9d99-9e70-4120-9794-6930afd305d5",
+ "lat": 40.43903534644546,
+ "lon": -79.94417316105108,
+ "active": false
+ },
+ {
+ "key": "aa31951f-e8f3-4d04-8640-e59e02825729",
+ "lat": 40.4390024021616,
+ "lon": -79.94421888705871,
+ "active": false
+ },
+ {
+ "key": "41b52f96-dde2-43fb-b4ed-cc1a35a2f50f",
+ "lat": 40.438968717539694,
+ "lon": -79.94427677423866,
+ "active": false
+ },
+ {
+ "key": "3fb694c8-91ad-4c78-881a-30d27a804ec3",
+ "lat": 40.438936143384055,
+ "lon": -79.94434147167493,
+ "active": false
+ },
+ {
+ "key": "9ae8f71b-458e-40c2-b363-cd76be6b73b3",
+ "lat": 40.43889986760059,
+ "lon": -79.94441638449607,
+ "active": false
+ },
+ {
+ "key": "e5081dee-73ba-44d8-9042-fe559152977d",
+ "lat": 40.43886951437957,
+ "lon": -79.94451367387417,
+ "active": false
+ },
+ {
+ "key": "2eca5c62-fa85-45e9-aa0d-7edf9ff204bc",
+ "lat": 40.43885655873202,
+ "lon": -79.94455258962527,
+ "active": false
+ },
+ {
+ "key": "f1c6ea5d-aa50-433e-a4e8-5defcca4ab1d",
+ "lat": 40.438830277268245,
+ "lon": -79.94466933687895,
+ "active": false
+ },
+ {
+ "key": "7ec16011-6c8e-43df-9b8e-8082c943c2f7",
+ "lat": 40.43878400706038,
+ "lon": -79.94487850904164,
+ "active": false
+ },
+ {
+ "key": "86e6e939-9d09-4b6d-aa6d-e6b06a5d8b38",
+ "lat": 40.438694798010715,
+ "lon": -79.9453445251622,
+ "active": false
+ },
+ {
+ "key": "029da21e-5461-4eec-ba3b-1990d0288ac3",
+ "lat": 40.43868517379034,
+ "lon": -79.94539316985127,
+ "active": false
+ },
+ {
+ "key": "9fb14587-704f-49b3-9996-f36931122dec",
+ "lat": 40.438678510868115,
+ "lon": -79.94544230098711,
+ "active": false
+ },
+ {
+ "key": "0ee9ffba-b608-4476-a972-d184e123b4a1",
+ "lat": 40.43867036729535,
+ "lon": -79.9454924050168,
+ "active": false
+ },
+ {
+ "key": "06625d0e-497d-42dc-b192-fdcc5441a0f3",
+ "lat": 40.438666665671285,
+ "lon": -79.94553667168377,
+ "active": false
+ },
+ {
+ "key": "513d73da-a671-41ea-99fe-9aed5cf967fc",
+ "lat": 40.43866333420924,
+ "lon": -79.94558191124459,
+ "active": false
+ },
+ {
+ "key": "040e2b63-15a7-4b8d-8494-19f7db7b4482",
+ "lat": 40.43866259388427,
+ "lon": -79.94564028487149,
+ "active": false
+ },
+ {
+ "key": "b9ab5da8-51ea-47c5-8015-191a28ece7c4",
+ "lat": 40.43866074307218,
+ "lon": -79.94571179256408,
+ "active": false
+ },
+ {
+ "key": "b883cdcd-fb53-4b62-af96-4f09cc7d4088",
+ "lat": 40.4386659253465,
+ "lon": -79.94576724750956,
+ "active": false
+ },
+ {
+ "key": "accfb48d-b34f-4ccb-a786-5a27d09e201a",
+ "lat": 40.43867110762042,
+ "lon": -79.94582270245503,
+ "active": false
+ },
+ {
+ "key": "25278c7a-eeb6-478b-881e-f31cbf7097b5",
+ "lat": 40.43867777054346,
+ "lon": -79.94588496765716,
+ "active": false
+ },
+ {
+ "key": "0b74d804-9787-483a-b5de-6e1f67752b49",
+ "lat": 40.438692206874364,
+ "lon": -79.94596571784085,
+ "active": false
+ },
+ {
+ "key": "1c8f69a7-ae51-4b0e-8395-cb6e4e3f9b20",
+ "lat": 40.438715897257055,
+ "lon": -79.94604938670605,
+ "active": false
+ },
+ {
+ "key": "60d92de9-ffd9-463d-8eb0-d4b718d98158",
+ "lat": 40.43874106827932,
+ "lon": -79.94611846216439,
+ "active": false
+ },
+ {
+ "key": "d5d01035-f9b3-46e8-83f7-595b37be772d",
+ "lat": 40.438771421558485,
+ "lon": -79.94619240209174,
+ "active": false
+ },
+ {
+ "key": "193117c4-4fc3-45a6-8166-d522c9439c2f",
+ "lat": 40.43880436595595,
+ "lon": -79.94625855886895,
+ "active": false
+ },
+ {
+ "key": "d014016c-357c-4b0b-b1a7-d179e060a528",
+ "lat": 40.438850265988016,
+ "lon": -79.94634563286229,
+ "active": false
+ },
+ {
+ "key": "b488f3e0-0ee2-4d12-9899-2cf049ed47c3",
+ "lat": 40.43889320469951,
+ "lon": -79.9464249237052,
+ "active": false
+ },
+ {
+ "key": "bb8d4780-be07-470b-a695-93e810fcc817",
+ "lat": 40.43894684191136,
+ "lon": -79.9465078485304,
+ "active": false
+ },
+ {
+ "key": "0155f015-17c0-4ff5-974e-cfd133f3ef1c",
+ "lat": 40.439008727581985,
+ "lon": -79.94657697943262,
+ "active": false
+ },
+ {
+ "key": "0bd80b9a-9de5-46bf-b4b3-615dca3ecaa8",
+ "lat": 40.43910691989441,
+ "lon": -79.94664824012304,
+ "active": false
+ },
+ {
+ "key": "94261baf-8b84-4778-92e7-c0c844be8a1f",
+ "lat": 40.43924294402823,
+ "lon": -79.94671050382154,
+ "active": false
+ },
+ {
+ "key": "e53f2ec0-f41c-4a81-935f-94f3936ed471",
+ "lat": 40.43941175481528,
+ "lon": -79.94676957589303,
+ "active": false
+ },
+ {
+ "key": "56413f33-59a2-44f6-8363-99dd9a9bee55",
+ "lat": 40.4395304641947,
+ "lon": -79.94681098413227,
+ "active": false
+ },
+ {
+ "key": "db632582-7d17-4a58-9407-6b66ea6b0167",
+ "lat": 40.439647790634375,
+ "lon": -79.94685608156028,
+ "active": false
+ },
+ {
+ "key": "feabaef4-45e3-45d6-b1a9-23daaf5d8dac",
+ "lat": 40.43973204818737,
+ "lon": -79.94690499359874,
+ "active": false
+ },
+ {
+ "key": "7382885a-7e77-449b-88fb-ae9b959fdf7a",
+ "lat": 40.43978400327805,
+ "lon": -79.94694965358384,
+ "active": false
+ },
+ {
+ "key": "e6f22436-7bc2-4c92-a477-bf08e22fdce1",
+ "lat": 40.43984262489535,
+ "lon": -79.94699491375209,
+ "active": false
+ },
+ {
+ "key": "47773aee-eb05-40c3-b0de-23072fdb013f",
+ "lat": 40.43991370353759,
+ "lon": -79.94706232251328,
+ "active": false
+ },
+ {
+ "key": "1a20275a-7e92-42d2-ada5-32fef7bc0510",
+ "lat": 40.43998698040972,
+ "lon": -79.94713936109747,
+ "active": false
+ },
+ {
+ "key": "a590dabf-a6b8-4d71-83ad-3f7079c3e451",
+ "lat": 40.44006465380737,
+ "lon": -79.94722699248705,
+ "active": false
+ },
+ {
+ "key": "10554684-6fb1-4abf-9072-4d02797a15f1",
+ "lat": 40.440136464981705,
+ "lon": -79.94732040177044,
+ "active": false
+ },
+ {
+ "key": "26073dca-898e-4683-b97b-b59d49b016c6",
+ "lat": 40.440338708284784,
+ "lon": -79.94761122242598,
+ "active": false
+ },
+ {
+ "key": "52cb3b60-b440-41ad-8e11-3716a0b16c8e",
+ "lat": 40.440455217737586,
+ "lon": -79.94777300345298,
+ "active": false
+ },
+ {
+ "key": "a791a486-b18f-42f9-887a-a2471dd1b0ac",
+ "lat": 40.440525562969825,
+ "lon": -79.94787411659468,
+ "active": false
+ },
+ {
+ "key": "0b0fed0b-3496-4772-8215-3859556a801c",
+ "lat": 40.44058711498772,
+ "lon": -79.94796752587807,
+ "active": false
+ },
+ {
+ "key": "9965f56f-63ad-496e-8b78-33cb7066050c",
+ "lat": 40.44064866694827,
+ "lon": -79.94801952692225,
+ "active": false
+ },
+ {
+ "key": "f90bdf83-013c-4ce0-9f96-27c1721b6187",
+ "lat": 40.44070802057209,
+ "lon": -79.94805226832051,
+ "active": false
+ },
+ {
+ "key": "3ea8ac0b-d137-4403-b917-1dd93dc13871",
+ "lat": 40.44076810690331,
+ "lon": -79.94806189814362,
+ "active": false
+ },
+ {
+ "key": "47857e72-567f-40d9-98e0-6db7ae33c7ff",
+ "lat": 40.44083552077191,
+ "lon": -79.94805612024973,
+ "active": false
+ },
+ {
+ "key": "a38da51a-284b-452f-b004-7c316cbff346",
+ "lat": 40.4408970725059,
+ "lon": -79.94803300867456,
+ "active": false
+ },
+ {
+ "key": "4d476f4a-851b-4d19-96c9-178bb344a3fd",
+ "lat": 40.44095715866797,
+ "lon": -79.9479906374532,
+ "active": false
+ },
+ {
+ "key": "00fce2df-0845-45b8-89e1-b527aee1e48a",
+ "lat": 40.44101871029077,
+ "lon": -79.94791937676275,
+ "active": false
+ },
+ {
+ "key": "4e33c28d-19f7-4dbe-ab45-d48927c2981f",
+ "lat": 40.44112789100693,
+ "lon": -79.94777396643487,
+ "active": false
+ },
+ {
+ "key": "3e31481d-4bfa-400e-9dff-2431ceed6948",
+ "lat": 40.441261985200526,
+ "lon": -79.94759388874432,
+ "active": false
+ },
+ {
+ "key": "c513f34a-1cb9-4524-bf46-e93fdaa76798",
+ "lat": 40.441448837319264,
+ "lon": -79.94734351334556,
+ "active": false
+ },
+ {
+ "key": "ae658aa8-291f-4f56-bf0d-8b7fb54a6ff7",
+ "lat": 40.441472285391725,
+ "lon": -79.94730114212432,
+ "active": false
+ },
+ {
+ "key": "2d927bc3-9078-4db1-8e71-ce308ad49ae4",
+ "lat": 40.44148694043265,
+ "lon": -79.94726454879682,
+ "active": false
+ },
+ {
+ "key": "b2007ab0-39fa-479d-9666-841957eaebc2",
+ "lat": 40.44149426795217,
+ "lon": -79.94722217757544,
+ "active": false
+ },
+ {
+ "key": "0e1af536-55e2-4545-a88c-95aa6c05daea",
+ "lat": 40.44149993154134,
+ "lon": -79.94716015451336,
+ "active": false
+ },
+ {
+ "key": "9c320bbe-4be0-4b0c-a260-c8b4d2807d15",
+ "lat": 40.4414976126352,
+ "lon": -79.94707953126428,
+ "active": false
+ },
+ {
+ "key": "0e0bc413-8f8a-437a-aac0-9243f3ce1545",
+ "lat": 40.441491363113876,
+ "lon": -79.94697960498323,
+ "active": false
+ },
+ {
+ "key": "fe40faaa-03fd-40d9-9a0e-4a218a2bef12",
+ "lat": 40.4414762601015,
+ "lon": -79.94688104755541,
+ "active": false
+ },
+ {
+ "key": "e3c89199-698a-4ba2-b838-e014e3d4acbe",
+ "lat": 40.441453345179745,
+ "lon": -79.94677290815545,
+ "active": false
+ },
+ {
+ "key": "fae72cb9-1502-45fb-bddc-510b9e062a58",
+ "lat": 40.441393453870006,
+ "lon": -79.94648065800483,
+ "active": false
+ },
+ {
+ "key": "926def9c-f4ba-47f5-8ce0-a65718879cf2",
+ "lat": 40.441338770453676,
+ "lon": -79.94622605131627,
+ "active": false
+ },
+ {
+ "key": "43a119fd-d669-4f8f-affb-912fb8a7e4b7",
+ "lat": 40.441277837451516,
+ "lon": -79.94593243231266,
+ "active": false
+ },
+ {
+ "key": "b97088c4-7edf-462f-a126-b4a297a52692",
+ "lat": 40.44119346858836,
+ "lon": -79.94551972308372,
+ "active": false
+ },
+ {
+ "key": "86ec3a84-e5a5-4144-a1ec-84b6cd06db0d",
+ "lat": 40.44110389165473,
+ "lon": -79.945109751561,
+ "active": false
+ },
+ {
+ "key": "339e7e7f-0d8e-47ca-a09e-7226ea4343c9",
+ "lat": 40.44102297737364,
+ "lon": -79.94473612702618,
+ "active": false
+ },
+ {
+ "key": "1dd76290-7043-4810-9c6c-539ecb894c95",
+ "lat": 40.440963606478164,
+ "lon": -79.9444589342603,
+ "active": false
+ },
+ {
+ "key": "ef5e1bf3-2421-472c-932f-34b456f5c35b",
+ "lat": 40.4409170855751,
+ "lon": -79.94424231957937,
+ "active": false
+ },
+ {
+ "key": "6949a6e5-2287-4dcb-892c-317cc56d08f9",
+ "lat": 40.440859967684034,
+ "lon": -79.94396067170872,
+ "active": false
+ },
+ {
+ "key": "b1499689-0cff-45b6-9233-b5949e7ba3fe",
+ "lat": 40.440801638242206,
+ "lon": -79.94367937238368,
+ "active": false
+ },
+ {
+ "key": "4d33c31f-00f1-4a41-a48c-e2a9d65d2d1c",
+ "lat": 40.44075684952912,
+ "lon": -79.94346035587725,
+ "active": false
+ },
+ {
+ "key": "db440b22-362a-4ea4-9394-5de35090141f",
+ "lat": 40.440688624804245,
+ "lon": -79.94315167948865,
+ "active": false
+ },
+ {
+ "key": "b2a57b14-4ad6-4a3f-b82f-a34a2f28ee17",
+ "lat": 40.44061935840987,
+ "lon": -79.94281015062408,
+ "active": false
+ },
+ {
+ "key": "722dc19b-3742-43f4-8320-f799ac2fbd73",
+ "lat": 40.44050634466599,
+ "lon": -79.94226945362436,
+ "active": false
+ }
+]
\ No newline at end of file
diff --git a/rb_ws/src/buggy/scripts/2d_sim/engine.py b/rb_ws/src/buggy/scripts/2d_sim/engine.py
index 045cbcea..5d67ce14 100755
--- a/rb_ws/src/buggy/scripts/2d_sim/engine.py
+++ b/rb_ws/src/buggy/scripts/2d_sim/engine.py
@@ -49,9 +49,13 @@ def __init__(self, heading: float):
"state/pose_navsat_noisy", NavSatFix, queue_size=1
)
+ # Start position for Start of Course
+ self.e_utm = Simulator.UTM_EAST_ZERO + 60
+ self.n_utm = Simulator.UTM_NORTH_ZERO + 150
+
# Start position for End of Hill 2
- self.e_utm = Simulator.UTM_EAST_ZERO - 3
- self.n_utm = Simulator.UTM_NORTH_ZERO - 10
+ # self.e_utm = Simulator.UTM_EAST_ZERO - 3
+ # self.n_utm = Simulator.UTM_NORTH_ZERO - 10
# Start position for Outdoor track
# self.e_utm = Simulator.UTM_EAST_ZERO + 110
@@ -63,7 +67,7 @@ def __init__(self, heading: float):
# self.n_utm = utm_coords[1]
self.heading = heading # degrees
- self.velocity = 5 # m/s
+ self.velocity = 15 # m/s
self.steering_angle = 0 # degrees
@@ -288,5 +292,5 @@ def loop(self):
if __name__ == "__main__":
rospy.init_node("sim_2d_engine")
- sim = Simulator(210)
+ sim = Simulator(-110)
sim.loop()
diff --git a/rb_ws/src/buggy/scripts/auton/autonsystem.py b/rb_ws/src/buggy/scripts/auton/autonsystem.py
index 90496ee4..0338ed09 100755
--- a/rb_ws/src/buggy/scripts/auton/autonsystem.py
+++ b/rb_ws/src/buggy/scripts/auton/autonsystem.py
@@ -4,7 +4,7 @@
import sys
# ROS Message Imports
-from std_msgs.msg import Float32, Float64
+from std_msgs.msg import Float32, Float64, Bool
from nav_msgs.msg import Odometry
import numpy as np
@@ -52,6 +52,9 @@ def __init__(self, trajectory, controller, brake_controller) -> None:
self.msg = None
rospy.Subscriber("nav/odom", Odometry, self.update_msg)
+ self.covariance_warning_publisher = rospy.Publisher(
+ "buggy/debug/is_high_covariance", Bool, queue_size=1
+ )
self.steer_publisher = rospy.Publisher(
"buggy/input/steering", Float64, queue_size=1
)
@@ -61,41 +64,42 @@ def __init__(self, trajectory, controller, brake_controller) -> None:
self.brake_debug_publisher = rospy.Publisher(
"auton/debug/brake", Float64, queue_size=1
)
-
self.heading_publisher = rospy.Publisher(
"auton/debug/heading", Float32, queue_size=1
)
- self.tick_caller()
+ self.distance_publisher = rospy.Publisher(
+ "auton/debug/distance", Float64, queue_size=1
+ )
+
+ self.auton_rate = 100
+ self.rosrate = rospy.Rate(self.auton_rate)
self.tick_caller()
def update_speed(self, msg):
with self.lock:
self.speed = msg.data
-
+
def update_msg(self, msg):
with self.lock:
self.msg = msg
def tick_caller(self):
- while(self.msg == None):
- rospy.sleep(0.001)
- while (True):
- self.tick()
+ while ((not rospy.is_shutdown()) and (self.msg == None)): # with no message, we wait
rospy.sleep(0.001)
-
- def update_msg(self, msg):
- with self.lock:
- self.msg = msg
- def tick_caller(self):
- while(self.msg == None):
+ # wait for covariance matrix to be better
+ while ((not rospy.is_shutdown()) and
+ (self.msg.pose.covariance[0] ** 2 + self.msg.pose.covariance[7] ** 2 > 1**2)):
+ # Covariance larger than one meter. We definitely can't trust the pose
rospy.sleep(0.001)
- while (True):
+ print("Waiting for Covariance to be better: ", rospy.get_rostime())
+ print("done checking covariance")
+ while (not rospy.is_shutdown()): # start the actual control loop
self.tick()
self.ticks += 1
- rospy.sleep(0.001)
+ self.rosrate.sleep()
def tick(self):
# Received an updated pose from the state estimator
@@ -104,10 +108,12 @@ def tick(self):
msg = self.msg
current_rospose = msg.pose.pose
- # Check if the pose covariance is a sane value. Otherwise ignore the message
+ # Check if the pose covariance is a sane value. Publish a warning if insane
if msg.pose.covariance[0] ** 2 + msg.pose.covariance[7] ** 2 > 1**2:
# Covariance larger than one meter. We definitely can't trust the pose
- return
+ self.covariance_warning_publisher.publish(Bool(True))
+ else:
+ self.covariance_warning_publisher.publish(Bool(False))
current_speed = np.sqrt(
msg.twist.twist.linear.x**2 + msg.twist.twist.linear.y**2
@@ -133,32 +139,42 @@ def tick(self):
self.heading_publisher.publish(Float32(pose.theta))
# Plot projected forward/back positions
- if (self.ticks % 5 == 0):
+ if (self.ticks % 50 == 0):
self.controller.plot_trajectory(
pose, self.trajectory, current_speed
)
-
-
+ distance_msg = Float64(self.trajectory.get_distance_from_index(
+ self.controller.current_traj_index))
+ self.distance_publisher.publish(distance_msg)
if __name__ == "__main__":
rospy.init_node("auton_system")
- arg = sys.argv[1]
- print("\n\nStarting Controller: " + str(arg) + "\n\n")
+ arg_ctrl = sys.argv[1]
+ arg_start_dist = sys.argv[2]
+ start_dist = float(arg_start_dist)
+
+ print("\n\nStarting Controller: " + str(arg_ctrl) + "\n\n")
+ print("\n\nStarting at distance: " + str(arg_start_dist) + "\n\n")
+
+ trajectory = Trajectory("/rb_ws/src/buggy/paths/frew_parkinglot_1.json")
+ # calculate starting index
+ start_index = trajectory.get_index_from_distance(start_dist)
+
# Add Controllers Here
ctrller = None
- if (arg == "stanley"):
- ctrller = StanleyController()
- elif (arg == "pure_pursuit"):
- ctrller = PurePursuitController()
- elif (arg == "mpc"):
- ctrller = ModelPredictiveController()
+ if (arg_ctrl == "stanley"):
+ ctrller = StanleyController(start_index)
+ elif (arg_ctrl == "pure_pursuit"):
+ ctrller = PurePursuitController(start_index)
+ elif (arg_ctrl == "mpc"):
+ ctrller = ModelPredictiveController(start_index)
if (ctrller == None):
raise Exception("Invalid Controller Argument")
-
+
auton_system = AutonSystem(
- Trajectory("/rb_ws/src/buggy/paths/buggycourse_safe.json"),
+ trajectory,
ctrller,
BrakeController()
)
diff --git a/rb_ws/src/buggy/scripts/auton/brake_controller.py b/rb_ws/src/buggy/scripts/auton/brake_controller.py
index 7dd6d75a..9fd85d76 100644
--- a/rb_ws/src/buggy/scripts/auton/brake_controller.py
+++ b/rb_ws/src/buggy/scripts/auton/brake_controller.py
@@ -6,9 +6,9 @@
class BrakeController:
MAX_LATERAL_ACCEL = 0.9 # in "g" based on regular vehicle
BRAKING_GAIN = 2.0
- def __init__(self):
+ def __init__(self, use_binary_braking = True):
# NOTE: Add more stuff here to keep track of PID stuff once I and D are implemented.
- pass
+ self.binary_braking = use_binary_braking
@staticmethod
def calculate_lateral_accel(linear_speed: float, steering_angle: float) -> float:
@@ -45,7 +45,10 @@ def compute_braking(self, speed: float, steering_angle: float) -> float:
Returns:
float: 0-1 (1 = full brake)
"""
- return self._compute_binary_braking(speed, steering_angle)
+ if self.binary_braking:
+ return self._compute_binary_braking(speed, steering_angle)
+ else:
+ return self._compute_modulated_braking(speed, steering_angle)
def _compute_binary_braking(self, speed: float, steering_angle: float) -> float:
"""Binary braking - using lateral acceleration
diff --git a/rb_ws/src/buggy/scripts/auton/controller.py b/rb_ws/src/buggy/scripts/auton/controller.py
index 98cdc303..8fc865ca 100755
--- a/rb_ws/src/buggy/scripts/auton/controller.py
+++ b/rb_ws/src/buggy/scripts/auton/controller.py
@@ -19,11 +19,14 @@ class Controller(ABC):
Example schemes include Pure Pursuit, Stanley, and LQR.
"""
- WHEELBASE = 1.3
+ NAND_WHEELBASE = 1.3
+ SS_WHEELBASE = 1.3
+
+ WHEELBASE = NAND_WHEELBASE
current_traj_index = 0
- def __init__(self) -> None:
+ def __init__(self, start_index) -> None:
self.trajectory_forward_1 = rospy.Publisher(
"auton/debug/forward1_navsat", NavSatFix, queue_size=1
)
@@ -39,6 +42,8 @@ def __init__(self) -> None:
# Make lists of publishers for easy iteration
self.forward_publishers = [self.trajectory_forward_1, self.trajectory_forward_2, self.trajectory_forward_3]
self.backward_publishers = [self.trajectory_backward_1]
+
+ self.current_traj_index = start_index
@abstractmethod
def compute_control(
@@ -70,14 +75,7 @@ def plot_trajectory(
"""
# Compute distance along path from current position
- traj_index = trajectory.get_closest_index_on_path(
- current_pose.x,
- current_pose.y,
- start_index=self.current_traj_index,
- end_index=self.current_traj_index + 10,
- )
-
- traj_dist = trajectory.get_distance_from_index(traj_index)
+ traj_dist = trajectory.get_distance_from_index(self.current_traj_index)
# Plot forward projections
for i in range(1, len(self.forward_publishers)+1):
diff --git a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
index 8b12fade..fa1ea314 100644
--- a/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
+++ b/rb_ws/src/buggy/scripts/auton/model_predictive_controller.py
@@ -87,9 +87,9 @@ class ModelPredictiveController(Controller):
state_cost_diag = np.diag(state_cost)
control_cost_diag = np.diag(control_cost)
- def __init__(self, ref_trajectory=None, ROS=False) -> None:
+ def __init__(self, start_index=0, ref_trajectory=None, ROS=False) -> None:
# instantiate parent
- super(ModelPredictiveController, self).__init__()
+ super(ModelPredictiveController, self).__init__(start_index)
# Internal variables
self.current_traj_index = 0 # Where in the trajectory we are currently
diff --git a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py
index 9d73a7ca..ab935c96 100755
--- a/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py
+++ b/rb_ws/src/buggy/scripts/auton/pure_pursuit_controller.py
@@ -21,8 +21,8 @@ class PurePursuitController(Controller):
MIN_LOOK_AHEAD_DIST = 0.5
MAX_LOOK_AHEAD_DIST = 10
- def __init__(self) -> None:
- super(PurePursuitController, self).__init__()
+ def __init__(self, start_index=0) -> None:
+ super(PurePursuitController, self).__init__(start_index)
self.debug_reference_pos_publisher = rospy.Publisher(
"auton/debug/reference_navsat", NavSatFix, queue_size=1
)
@@ -50,6 +50,7 @@ def compute_control(
if self.current_traj_index >= trajectory.get_num_points() - 1:
return 0
+ # 10 is a good number to search forward along the index
traj_index = trajectory.get_closest_index_on_path(
current_pose.x,
current_pose.y,
diff --git a/rb_ws/src/buggy/scripts/auton/stanley_controller.py b/rb_ws/src/buggy/scripts/auton/stanley_controller.py
index 1811909b..24a946d0 100644
--- a/rb_ws/src/buggy/scripts/auton/stanley_controller.py
+++ b/rb_ws/src/buggy/scripts/auton/stanley_controller.py
@@ -25,14 +25,11 @@ class StanleyController(Controller):
CROSS_TRACK_GAIN = 1
HEADING_GAIN = 0.75
- def __init__(self) -> None:
- super(StanleyController, self).__init__()
+ def __init__(self, start_index=0) -> None:
+ super(StanleyController, self).__init__(start_index)
self.debug_reference_pos_publisher = rospy.Publisher(
"auton/debug/reference_navsat", NavSatFix, queue_size=1
)
- self.debug_track_pos_publisher = rospy.Publisher(
- "auton/debug/track_navsat", NavSatFix, queue_size=1
- )
self.debug_error_publisher = rospy.Publisher(
"auton/debug/error", ROSPose, queue_size=1
)
@@ -59,6 +56,7 @@ def compute_control(
y = current_pose.y
# Assume current pose is rear of buggy, project it to center of front axle
+ # 10 is a good number to search forward along the index
front_x = x + StanleyController.WHEELBASE * np.cos(heading)
front_y = y + StanleyController.WHEELBASE * np.sin(heading)
diff --git a/rb_ws/src/buggy/scripts/auton/world.py b/rb_ws/src/buggy/scripts/auton/world.py
index b95112b3..3684a849 100755
--- a/rb_ws/src/buggy/scripts/auton/world.py
+++ b/rb_ws/src/buggy/scripts/auton/world.py
@@ -94,6 +94,8 @@ def world_to_gps_numpy(coords):
Returns:
numpy.ndarray [size: (N,2)]: array of lat, lon pairs
"""
+
+ # Pittsburgh is in UTM zone 17T.
utm_coords = utm.to_latlon(
coords[:, 0] + World.WORLD_EAST_ZERO,
coords[:, 1] + World.WORLD_NORTH_ZERO,