Merge branch 'sw/mpc_linear' of https://github.com/CMU-Robotics-Club/…

…RoboBuggy2

rb_ws/src/buggy/INS_params.yml

@@ -184,7 +184,7 @@ publish_nmea   : False
 # ****************************************************************** 
 # Kalman Filter Settings (only applicable for devices with a Kalman Filter) 
 # ****************************************************************** 
-filter_data_rate : 100
+filter_data_rate : 50
 
 # The speed at which the individual Filter publishers will publish at.
 #      If set to -1, will use filter_data_rate to determine the rate at which to stream and publish

rb_ws/src/buggy/launch/sim_2d.launch

@@ -5,6 +5,8 @@
 <launch>
     <arg name="controller"      default="stanley" />
     <node name="foxglove" pkg="foxglove_bridge" type="foxglove_bridge" />
+    <node name="mpc_calculator" pkg="buggy" type="model_predictive_controller.py" output="screen"/>
+
 
     <node name="sim_2d_engine" pkg="buggy" type="engine.py" output="screen"/>
     <!-- <node name="sim_2d_visualizer" pkg="buggy" type="grapher.py" output="screen"/> -->

rb_ws/src/buggy/paths/frew_parkinglot_0.json

@@ -0,0 +1,62 @@
+[
+  {
+    "key": "1ee53c54-f5c1-4952-8404-528db36d0100",
+    "lat": 40.44180244698272,
+    "lon": -79.94169866523342,
+    "active": false
+  },
+  {
+    "key": "e7cb45b5-7e68-40d5-aff7-076ff85b9399",
+    "lat": 40.44188614170883,
+    "lon": -79.94212639403665,
+    "active": false
+  },
+  {
+    "key": "d3121256-30a4-4765-8e93-f61ee669507b",
+    "lat": 40.441975960323575,
+    "lon": -79.94250451166205,
+    "active": false
+  },
+  {
+    "key": "c724f8c2-8262-40cc-ae55-3538d2abb5b7",
+    "lat": 40.44204638619858,
+    "lon": -79.94293760383579,
+    "active": false
+  },
+  {
+    "key": "0e30798b-f8b8-4785-aaa9-f82e83b098d4",
+    "lat": 40.44208415076806,
+    "lon": -79.94302475860762,
+    "active": false
+  },
+  {
+    "key": "af701c43-1b24-4805-a006-43cb52d41137",
+    "lat": 40.44214947321631,
+    "lon": -79.9431239809632,
+    "active": false
+  },
+  {
+    "key": "35cdb7f2-78b7-4616-9d88-546555f3a1fb",
+    "lat": 40.442277736370215,
+    "lon": -79.94322097021222,
+    "active": false
+  },
+  {
+    "key": "e2015ad9-e05a-4a33-bc25-00e617ac85ae",
+    "lat": 40.44238320458174,
+    "lon": -79.94327150522668,
+    "active": false
+  },
+  {
+    "key": "f7664107-b8aa-4875-acea-7d4ef06587ec",
+    "lat": 40.44258121217669,
+    "lon": -79.94333991503834,
+    "active": false
+  },
+  {
+    "key": "a3ff1702-360a-477e-b6a4-46da980f5d56",
+    "lat": 40.442740434355834,
+    "lon": -79.94339222842373,
+    "active": false
+  }
+]

rb_ws/src/buggy/paths/frew_parkinglot_1.json

@@ -0,0 +1,86 @@
+[
+  {
+    "key": "76525b18-71a0-490c-945f-af3157c3ce1e",
+    "lat": 40.44295382467282,
+    "lon": -79.94345933166863,
+    "active": false
+  },
+  {
+    "key": "16b6c16f-3458-4eb4-bbcb-dc6828c0a778",
+    "lat": 40.44283710141919,
+    "lon": -79.94341692048053,
+    "active": false
+  },
+  {
+    "key": "db8bfb7e-14a9-420e-bb6a-98bfbb6907c2",
+    "lat": 40.44265767896026,
+    "lon": -79.94336999645112,
+    "active": false
+  },
+  {
+    "key": "1a1775c5-d3a8-4eb5-b85b-53777d83079a",
+    "lat": 40.44252913720254,
+    "lon": -79.94332424552249,
+    "active": false
+  },
+  {
+    "key": "8b876b5d-341a-4d80-9e44-ad8596b1ac96",
+    "lat": 40.442414877656084,
+    "lon": -79.94328318699667,
+    "active": false
+  },
+  {
+    "key": "93b948e5-e962-4d89-8566-68a825416a19",
+    "lat": 40.44232561225035,
+    "lon": -79.94324095537026,
+    "active": false
+  },
+  {
+    "key": "3645dc94-9d24-4f1c-b0e1-ceb76c943779",
+    "lat": 40.44223545407017,
+    "lon": -79.94319520444166,
+    "active": false
+  },
+  {
+    "key": "baecbda3-8de5-4be1-a900-5ef9159568bc",
+    "lat": 40.44206227861279,
+    "lon": -79.94295823809365,
+    "active": false
+  },
+  {
+    "key": "fe3dbba5-eda8-4ae7-8ad6-d9cd559fc347",
+    "lat": 40.44202389430194,
+    "lon": -79.94282215840855,
+    "active": false
+  },
+  {
+    "key": "f0ef5b95-dd9d-4c69-ad13-4be8b5423408",
+    "lat": 40.44199265124159,
+    "lon": -79.94270250213366,
+    "active": false
+  },
+  {
+    "key": "52d1a5c6-81be-44ea-825e-2d85d4d9c233",
+    "lat": 40.44196408614528,
+    "lon": -79.94256524934785,
+    "active": false
+  },
+  {
+    "key": "e773b294-07fd-4428-96e8-10a4c9b589a7",
+    "lat": 40.441931950397326,
+    "lon": -79.94238693803621,
+    "active": false
+  },
+  {
+    "key": "df99e69a-2847-4fee-a4cc-d045f7e8910b",
+    "lat": 40.44190070729448,
+    "lon": -79.94224147354515,
+    "active": false
+  },
+  {
+    "key": "66ca6719-e079-4b9d-84df-af56602a3f6a",
+    "lat": 40.441885532067694,
+    "lon": -79.94212064416959,
+    "active": false
+  }
+]