Test circle planner on drone simulation; fix ordering to wait until p…

…osition_controller starts moving before collecting data
misko · Mar 8, 2024 · 53bcb2d · 53bcb2d
1 parent b8bd0ca
commit 53bcb2d
Show file tree

Hide file tree

Showing 5 changed files with 24 additions and 6 deletions.
diff --git a/spf/data_collector.py b/spf/data_collector.py
@@ -273,6 +273,8 @@ def radios_to_online(self):
         self.collector_thread = threading.Thread(
             target=self.run_collector_thread, daemon=True
         )
+
+    def start(self):
         self.collector_thread.start()
 
     def run_collector_thread(self):

diff --git a/spf/grbl_radio_collection.py b/spf/grbl_radio_collection.py
@@ -108,6 +108,7 @@
         time.sleep(5)  # easy poll this
     logging.info("DRONE IS READY!!! LETS GOOO!!!")
 
+    data_collector.start()
     while data_collector.is_collecting():
         time.sleep(5)
 

diff --git a/spf/mavlink/mavlink_controller.py b/spf/mavlink/mavlink_controller.py
@@ -303,7 +303,7 @@ def run_planner(self):
         # drone.request_home()
         logging.info("Planer main loop")
         while not self.drone_ready:
-            # logging.info("wait for drone ready")
+            logging.info("wait for drone ready")
             self.single_operation_mode_on()
             self.turn_off_hardware_safety()
             self.arm()

diff --git a/spf/mavlink_radio_collection.py b/spf/mavlink_radio_collection.py
@@ -152,8 +152,10 @@
     data_collector.radios_to_online()  # blocking
 
     while not drone.has_planner_started_moving():
-        # logging.info(f"waiting for drone to start moving {time.time()}")
+        logging.info(f"waiting for drone to start moving {time.time()}")
         time.sleep(5)  # easy poll this
+
+    data_collector.start()
     logging.info("DRONE IS READY!!! LETS GOOO!!!")
 
     while data_collector.is_collecting():

diff --git a/spf/notebooks/dynamics_planners.ipynb b/spf/notebooks/dynamics_planners.ipynb
@@ -17,7 +17,7 @@
     "import matplotlib.pyplot as plt\n",
     "from spf.grbl.grbl_interactive import BouncePlanner\n",
     "from spf.grbl.grbl_interactive import home_bounding_box\n",
-    "from spf.gps.boundaries import franklin_boundary, crissy_boundary_convex"
+    "from spf.gps.boundaries import franklin_boundary, crissy_boundary_convex, franklin_safe"
    ]
   },
   {
@@ -27,11 +27,13 @@
    "outputs": [],
    "source": [
     "import matplotlib.pyplot as plt\n",
-    "from spf.grbl.grbl_interactive import home_bounding_box\n",
+    "from spf.grbl.grbl_interactive import CirclePlanner, home_bounding_box\n",
     "\n",
     "# GPS\n",
     "\n",
-    "boundary = crissy_boundary_convex - crissy_boundary_convex.mean(axis=0)\n",
+    "boundary = (\n",
+    "    franklin_safe  # crissy_boundary_convex #- crissy_boundary_convex.mean(axis=0)\n",
+    ")\n",
     "bp = BouncePlanner(\n",
     "    dynamics=Dynamics(\n",
     "        bounding_box=boundary,\n",
@@ -42,15 +44,26 @@
     "    step_size=0.1,\n",
     "    seed=123,\n",
     ")\n",
+    "bp = CirclePlanner(\n",
+    "    dynamics=Dynamics(\n",
+    "        bounding_box=boundary,\n",
+    "        bounds_radius=0.000001,\n",
+    "    ),\n",
+    "    start_point=boundary.mean(axis=0),\n",
+    "    step_size=0.0002,\n",
+    "    circle_diameter=0.0003,\n",
+    "    circle_center=boundary.mean(axis=0),\n",
+    ")\n",
     "\n",
     "p = bp.yield_points()\n",
-    "z = np.array([next(p) for x in range(10)])\n",
+    "z = np.array([next(p) for x in range(30)])\n",
     "\n",
     "fig, axs = plt.subplots(1, 1, figsize=(8, 6))\n",
     "axs.plot(z[:, 0], z[:, 1])  # , s=1)\n",
     "axs.scatter(boundary[:, 0], boundary[:, 1])\n",
     "\n",
     "axs.scatter(z[-1, 0], z[-1, 1])\n",
+    "axs.set_aspect(\"equal\")\n",
     "\n",
     "z"
    ]