June 11 Flight Test Changes (#223)

Co-authored-by: Eric Pedley <[email protected]>

log_analysis/graph_pose.py

@@ -19,7 +19,7 @@
 img_timestamps = []
 img_pixel_diff = []
 
-dir_name= Path("/home/ericp/uavf_2024/flight_logs/perception_logs_607/06-07 10:39")
+dir_name= Path("/home/forge/ws/src/libuavf_2024/flight_logs/06-11 15:01")
 
 
 for fname in tqdm(sorted(os.listdir(dir_name / 'poses'))):
@@ -39,32 +39,6 @@
     y_rotations.append(euler[1])
     z_rotations.append(euler[2])
 
-alt_timestamps = []
-alt_msl = []
-alt_local = []
-alt_rel = []
-alt_terrain = []
-
-for fname in tqdm(sorted(os.listdir(dir_name / 'altitudes'))):
-    alt = json.load(open(dir_name / 'altitudes' / fname))
-    timestamp = float(fname[:-5])
-    msl = alt['amsl']
-    local = alt['local']
-    rel = alt['relative']
-    terrain = alt['terrain']
-
-    alt_timestamps.append(timestamp)
-    alt_msl.append(msl)
-    alt_local.append(local)
-    alt_rel.append(rel)
-    alt_terrain.append(terrain)
-
-for (y_name, y_arr) in zip(["msl", "local", "relative", "terrain"], [alt_msl, alt_local, alt_rel, alt_terrain]):
-    plt.figure()
-    plt.plot(alt_timestamps, y_arr, label=y_name)
-    plt.title(f"{y_name} altitude vs time")
-    plt.savefig(f"{y_name}_vs_time.png")
-
 for (y_name, y_arr) in zip(["x_positions", "y_positions", "z_positions", "x_rotations", "y_rotations", "z_rotations"], [x_positions, y_positions, z_positions, x_rotations, y_rotations, z_rotations]):
     plt.figure() 
     plt.plot(pose_timestamps, y_arr, label= y_name)

log_analysis/plot_timestamps.py

@@ -10,7 +10,7 @@ def get_images(images_dir: Path):
 
     first = None
 
-    for path in tqdm(sorted(images_dir.glob("*.jpg"))):
+    for path in tqdm(sorted(images_dir.glob("*.json"))):
         timestamp = float(path.stem)
         if first is None:
             first = timestamp
@@ -22,15 +22,16 @@ def get_images(images_dir: Path):
 minute = 43
 
 plt.title("Timestamp vs index")
-cam_path = Path(f"/home/ericp/uavf_2024/flight_logs/perception_logs_608/06-08 01:{minute}/camera")
+logs_path = Path("/media/forge/SANDISK/logs/06-11 21h01m/")
+cam_path = logs_path / "camera"
 timestamps, imgs = get_images(cam_path)
 plt.scatter(range(len(timestamps)), timestamps, s=1)
 
-img_process_path = Path(f"/home/ericp/uavf_2024/flight_logs/perception_logs_608/06-08 01:{minute}/image_processor")
-for folder in sorted(img_process_path.glob("img_*")):
-    data = json.load(open(folder / "data.json"))
-    timestamp = data['image_time']
-    plt.plot([0, len(timestamps)], [timestamp, timestamp], color='red', linewidth=1)
+# img_process_path = logs_path / "image_processor"
+# for folder in sorted(img_process_path.glob("img_*")):
+#     data = json.load(open(folder / "data.json"))
+#     timestamp = data['image_time']
+#     plt.plot([0, len(timestamps)], [timestamp, timestamp], color='red', linewidth=1)
 
 plt.legend()
 plt.savefig("timestamps_vs_index.png")

scripts/imaging_node.py

@@ -59,7 +59,7 @@ def to_json(self):
 )
 
 
-class PoseProvider(RosLoggingProvider[PoseStamped, PoseDatum]):
+class PoseProvider(RosLoggingProvider[PoseStamped, PoseDatum]):        
     def _subscribe_to_topic(self, action: Callable[[PoseStamped], Any]) -> None:
         self.node.create_subscription(
             PoseStamped,
@@ -87,11 +87,14 @@ def format_data(self, message: PoseStamped) -> PoseDatum:
             time_seconds = message.header.stamp.sec + message.header.stamp.nanosec / 1e9
         )
 
-    def _interpolate_from_buffer(self, time_seconds: float) -> PoseDatum | None:
+    def _interpolate_from_buffer(self, time_seconds: float, wait: bool = False) -> PoseDatum | None:
         """
         Returns the pose datum interpolated between the two data points before and after the time given.
         
-        If this interpolation is not possible, returns None.
+        If this interpolation is not possible because the pose is too old, returns the oldest pose.
+        If this interpolation is not possible because the poses are not old enough, wait until enough data is available if wait is enabled.
+            Otherwise, return the newest pose
+        If there is no pose available, wait if enabled. Otherwise, return None.
         """
         if self._buffer.count == 0:
             return None
@@ -103,6 +106,15 @@ def _interpolate_from_buffer(self, time_seconds: float) -> PoseDatum | None:
         if closest_idx == 0:
             return data[0]
 
+        # Poll every 100ms
+        while closest_idx == len(data):
+            if not wait:
+                return data[closest_idx - 1]
+
+            time.sleep(0.1)
+            data = self._buffer.get_all_reversed()
+            closest_idx = bisect_left([d.time_seconds for d in data], time_seconds)
+
         pt_before = data[closest_idx - 1]
         pt_after = data[closest_idx]
 
@@ -134,7 +146,7 @@ def get_interpolated(self, time_seconds: float) -> tuple[PoseDatum, bool]:
 
         '''
         for _ in range(50):
-            interp_pose = self._interpolate_from_buffer(time_seconds)
+            interp_pose = self._interpolate_from_buffer(time_seconds, True)
             if interp_pose is not None:
                 return (interp_pose, True)
             else:
@@ -147,7 +159,7 @@ class ImagingNode(Node):
     def __init__(self) -> None:
         # Initialize the node
         super().__init__('imaging_node') # type: ignore
-        self.logs_path = Path(f'logs/{time.strftime("%m-%d %H:%M")}')
+        self.logs_path = Path(f'/media/forge/SANDISK/logs/{time.strftime("%m-%d %Hh%Mm")}')
 
         self.camera = Camera(self.logs_path / "camera")
         self.zoom_level = 3
@@ -161,7 +173,7 @@ def __init__(self) -> None:
         self.log(f"Setting up imaging node ROS connections")
 
         # Subscriptions ----
-        self.pose_provider = PoseProvider(self, self.logs_path / "poses")
+        self.pose_provider = PoseProvider(self, self.logs_path / "poses", 128)
         self.pose_provider.subscribe(self.cam_auto_point)
 
         # Only start the recording once (when the first pose comes in)