Save mapping_move_to photo info to JSON #29

flight/camera.py

@@ -4,11 +4,12 @@
 # Not worth to deal with this with the time crunch we are in
 
 import asyncio
-import math
+from datetime import datetime
 import json
 import logging
+import math
 import os
-from datetime import datetime
+from typing import TypedDict
 
 import dronekit
 import gphoto2
@@ -19,6 +20,22 @@
 WAYPOINT_TOLERANCE: int = 1  # in meters
 
 
+class PhotoInfo(TypedDict):
+    """Basic information about a photo."""
+
+    focal_length: float
+    """The focal length, in millimeters."""
+
+    rotation_deg: tuple[float, float, float]
+    """The roll, pitch, and yaw, in degrees."""
+
+    drone_coordinates: tuple[float, float]
+    """The latitude and longitude, in degrees"""
+
+    altitude_f: float
+    """The altitude, in meters."""
+
+
 class Camera:
     """
     Initialize a new Camera object to control the Sony RX100-VII camera on the drone
@@ -52,7 +69,6 @@ class Camera:
     )
         Move the drone to the specified latitude, longitude, and altitude.
         Takes photos along the way.
-
     odlc_move_to(
         drone: dronekit.Vehicle,
         latitude: float,
@@ -63,6 +79,8 @@ class Camera:
     )
         Move the drone to the specified latitude, longitude, and altitude.
         Takes a photo at the end if take_photos is True.
+    _get_photo_information(drone: dronekit.Vehicle)
+        Get the current camera information to associate with a photo.
     """
 
     def __init__(self) -> None:
@@ -147,8 +165,19 @@ async def mapping_move_to(
         heading : float, default 0
             The yaw in which the camera should point, in degrees (0 is north, 90 is west).
         """
+        info: dict[str, PhotoInfo] = {}
+
+        drone.gimbal.rotate(
+            -drone.attitude.pitch - 90, 0, heading  # pitch is relative to the drone
+        )
+        await asyncio.sleep(1.0)
+
+        photo_info: PhotoInfo = await self._get_photo_info(drone)
+        file_path: str
+        _, file_path = await self.capture_photo(f"{os.getcwd()}/mapping_images/")
+        point: dict[str, PhotoInfo] = {file_path: photo_info}
+        info.update(point)
 
-        await self.capture_photo(f"{os.getcwd()}/mapping_images/")
         goto_task: asyncio.Task[None] = asyncio.ensure_future(
             move_to(
                 drone,
@@ -185,11 +214,33 @@ async def mapping_move_to(
 
             if distance >= next_interval_count * interval:
                 next_interval_count += 1
-                await self.capture_photo(f"{os.getcwd()}/mapping_images/")
+                photo_info = await self._get_photo_info(drone)
+                _, file_path = await self.capture_photo(f"{os.getcwd()}/mapping_images/")
+                point = {file_path: photo_info}
+                info.update(point)
 
             await asyncio.sleep(0.25)
 
-        await self.capture_photo(f"{os.getcwd()}/mapping_images/")
+        drone.gimbal.rotate(
+            -drone.attitude.pitch - 90, 0, heading  # pitch is relative to the drone
+        )
+        await asyncio.sleep(1.0)
+
+        photo_info = await self._get_photo_info(drone)
+        _, file_path = await self.capture_photo(f"{os.getcwd()}/mapping_images/")
+        point = {file_path: photo_info}
+        info.update(point)
+
+        current_photos: dict[str, PhotoInfo] = {}
+        if os.path.exists("flight/data/mapping_photos.json"):
+            with open("flight/data/mapping_photos.json", "r", encoding="utf8") as current_data:
+                try:
+                    current_photos = json.load(current_data)
+                except json.JSONDecodeError:
+                    pass
+
+        with open("flight/data/mapping_photos.json", "w", encoding="ascii") as camera:
+            json.dump(current_photos | info, camera)
 
     async def odlc_move_to(
         self,
@@ -220,7 +271,7 @@ async def odlc_move_to(
         heading : float, default 0
             The yaw in which the camera should point, in degrees (0 is north, 90 is west).
         """
-        info: dict[str, dict[str, int | list[int | float] | float]] = {}
+        info: dict[str, PhotoInfo] = {}
 
         await move_to(
             drone,
@@ -244,30 +295,13 @@ async def odlc_move_to(
         if not take_photos:
             return
 
+        photo_info: PhotoInfo = await self._get_photo_info(drone)
         file_path: str
         _, file_path = await self.capture_photo()
-
-        attitude: dronekit.Attitude = drone.attitude
-        roll_deg: float = math.degrees(attitude.roll)
-        pitch_deg: float = math.degrees(attitude.pitch)
-        yaw_deg: float = math.degrees(attitude.yaw)
-
-        point: dict[str, dict[str, int | list[int | float] | float]] = {
-            file_path: {
-                "focal_length": 24,
-                "rotation_deg": [
-                    roll_deg,
-                    pitch_deg,
-                    yaw_deg,
-                ],
-                "drone_coordinates": [latitude, longitude],
-                "altitude_f": drone.location.global_relative_frame.alt,
-            }
-        }
-
+        point: dict[str, PhotoInfo] = {file_path: photo_info}
         info.update(point)
 
-        current_photos: dict[str, dict[str, int | list[int | float] | float]] = {}
+        current_photos: dict[str, PhotoInfo] = {}
         if os.path.exists("flight/data/camera.json"):
             with open("flight/data/camera.json", "r", encoding="utf8") as current_data:
                 try:
@@ -277,3 +311,35 @@ async def odlc_move_to(
 
         with open("flight/data/camera.json", "w", encoding="ascii") as camera:
             json.dump(current_photos | info, camera)
+
+    async def _get_photo_info(self, drone: dronekit.Vehicle) -> PhotoInfo:
+        """
+        Gets the current camera information based on a drone's position.
+
+        Parameters
+        ----------
+        drone : dronekit.Vehicle
+            The drone whose position will be stored in the result.
+
+        Returns
+        -------
+        PhotoInfo
+            Camera information to be associated with a photo.
+        """
+        location: dronekit.LocationGlobalRelative = drone.location.global_relative_frame
+
+        attitude: dronekit.Attitude = drone.attitude
+        roll_deg: float = math.degrees(attitude.roll)
+        pitch_deg: float = math.degrees(attitude.pitch)
+        yaw_deg: float = math.degrees(attitude.yaw)
+
+        return {
+            "focal_length": 24,
+            "rotation_deg": (
+                roll_deg,
+                pitch_deg,
+                yaw_deg,
+            ),
+            "drone_coordinates": (location.lat, location.lon),
+            "altitude_f": location.alt,
+        }