From 848833c5989dfcfcb41df467113fb8a2c8c5c986 Mon Sep 17 00:00:00 2001
From: JonKach <jon.kachappilly@gmail.com>
Date: Sat, 24 Jan 2026 19:42:44 +0000
Subject: [PATCH 1/7] Save Vision Mode

---
 .../uav/uav/autonomous_modes/SaveImageMode.py | 75 +++++++++++++++++++
 .../src/uav/uav/autonomous_modes/__init__.py  |  3 +-
 .../src/uav/uav/missions/save_image.yaml      |  9 +++
 .../src/uav/uav/vision_nodes/SaveImageNode.py | 51 +++++++++++++
 .../src/uav/uav/vision_nodes/__init__.py      |  3 +-
 5 files changed, 139 insertions(+), 2 deletions(-)
 create mode 100644 controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
 create mode 100644 controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
 create mode 100644 controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py

diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
new file mode 100644
index 00000000..5cee40c5
--- /dev/null
+++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
@@ -0,0 +1,75 @@
+from typing import List
+from uav.autonomous_modes import Mode
+from rclpy.node import Node
+from uav import UAV
+from uav.vision_nodes.SaveImageNode import SaveImageNode
+class SaveImageMode(Mode):
+    """
+    A mode for navigating to a GPS coordinate
+    """
+
+    def __init__(self, node: Node, uav: UAV, coordinates: List[tuple[tuple[float, float, float], float, str]], margin: float = 0.5):
+        """
+        Initialize the NavigateToGPSMode.
+
+        Args:
+            node (Node): ROS 2 node managing the UAV.
+            uav (UAV): The UAV instance to control.
+            coordinates (List[tuple[tuple[float, float, float], float, str]]): The coordinates to navigate to (x/y/z or lon/lat/alt, wait time, GPS/LOCAL).
+                                                                               Local are NED coordinates, relative to the starting position (https://docs.px4.io/main/en/ros2/user_guide.html#ros-2-px4-frame-conventions).
+            margin (float): The margin of error for the GPS coordinate.
+        """
+        super().__init__(node, uav)
+        self.coordinates = coordinates
+        self.goal = None
+        self.margin = margin
+        self.index = -1
+
+    def on_update(self, time_delta: float) -> None:
+        """
+        Periodic logic for setting gps coord.
+        """
+        dist = 0
+        if self.index != -1:
+            dist = self.uav.distance_to_waypoint(self.coordinate_system, self.goal)
+            self.log(f"Distance to waypoint: {dist}, current position: {self.uav.get_local_position()}")
+            
+        if dist >= self.margin:
+            self.uav.publish_position_setpoint(self.target) # PX4 expects stream of setpoints
+        elif self.goal is None or dist < self.margin:
+            if self.index == -1 or self.wait_time <= 0:
+                self.index += 1
+                if self.index >= len(self.coordinates):
+                    return
+                self.goal, self.wait_time, self.coordinate_system = self.coordinates[self.index]
+                self.target = self.get_local_target()
+                self.uav.publish_position_setpoint(self.target)
+            else:
+                self.wait_time -= time_delta
+                self.log(f"Holding - waiting for {self.wait_time} more seconds")
+
+    def get_local_target(self) -> tuple[float, float, float]:
+        """
+        Get the local target of the UAV.
+
+        Returns:
+            tuple[float, float, float]: The local target of the UAV.
+        """
+        if self.coordinate_system == "GPS":
+            return self.uav.gps_to_local(self.goal)
+        elif self.coordinate_system == "LOCAL":
+            return tuple(float(x) for x in self.goal)
+        else: 
+            raise ValueError(f"Invalid coordinate system {self.coordinate_system}")
+
+    def check_status(self) -> str:
+        """
+        Check the status of the mode.
+        
+        Returns:
+            str: The status of the mode.
+        """
+        if self.index >= len(self.coordinates):
+            return "complete"
+        else:
+            return "continue"
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py
index 6dabab0c..6195d293 100644
--- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py
+++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/__init__.py
@@ -5,4 +5,5 @@
 from .NavGPSMode import NavGPSMode
 from .TransitionMode import TransitionMode
 from .ServoDropoffMode import ServoDropoffMode
-from .WaypointMission import WaypointMission
\ No newline at end of file
+from .WaypointMission import WaypointMission
+from .SaveImageMode import SaveImageMode
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
new file mode 100644
index 00000000..f774177a
--- /dev/null
+++ b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
@@ -0,0 +1,9 @@
+start:
+  class: uav.autonomous_modes.SaveImageMode
+  params:
+    coordinates: '[((5, 3, -10), 5, "LOCAL"), ((10, 1, -1), 3, "LOCAL")]'
+  transitions:
+    complete: land
+
+land:
+  class: uav.autonomous_modes.LandingMode
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
new file mode 100644
index 00000000..05dd4158
--- /dev/null
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
@@ -0,0 +1,51 @@
+# payload_tracking_node.py
+import cv2
+import numpy as np
+from uav.vision_nodes.VisionNode import VisionNode
+import rclpy
+from cv_bridge import CvBridge
+from uav.utils import pink, green, blue, yellow
+from geometry_msgs.msg import Vector3
+from std_msgs.msg import String, Float64MultiArray
+
+from std_srvs.srv import Empty
+
+from px4_msgs.msg import VehicleLocalPosition
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+
+class SaveImageNode(VisionNode):
+    """
+    ROS node for hoop tracking with OpenCV Algorithm.
+    """
+    srv = Empty
+    def __init__(self):
+        super().__init__('save_vision', display=False, use_service=False)
+        self.bridge = CvBridge()
+
+        # add an empty service server for ModeManager
+        self.create_service(self.srv, self.service_name(), self.service_callback)
+        self.get_logger().info(f"save_vision empty service '{self.service_name()}' started.")        
+
+
+    def service_callback(self, request, response):
+        self.get_logger().info("Empty service called (placeholder).")
+        return response
+    def image_callback(self, msg):
+        # self.get_logger().info("Received image for hoop tracking.")
+        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        import time
+        import os
+        time = int(time.time())
+        path = os.path.expanduser(f"~/vision_imgs/{image}")
+        cv2.imwrite(os.path.join(path, f"payload_{time}.png"), image)
+
+def main():
+    rclpy.init()
+    node = SaveImageNode()
+    try:    
+        rclpy.spin(node)
+    except Exception as e:
+        print(e)
+        node.publish_failsafe()
+
+    rclpy.shutdown()
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py
index 41b97014..97f08684 100644
--- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/__init__.py
@@ -1,2 +1,3 @@
 from .VisionNode import VisionNode # make sure to import the parent class FIRST (to avoid circular imports)
-from .PayloadTrackingNode import PayloadTrackingNode
\ No newline at end of file
+from .PayloadTrackingNode import PayloadTrackingNode
+from .SaveImageNode import SaveImageNode
\ No newline at end of file

From 82c498168746df4902aa76806c0dd366dc383f70 Mon Sep 17 00:00:00 2001
From: JonKach <jon.kachappilly@gmail.com>
Date: Sat, 24 Jan 2026 20:51:54 +0000
Subject: [PATCH 2/7] save image worksgit add .

---
 controls/sae_2025_ws/src/uav/setup.py         |  1 +
 .../uav/uav/autonomous_modes/SaveImageMode.py |  2 +-
 .../src/uav/uav/vision_nodes/SaveImageNode.py | 27 ++++++++++---------
 3 files changed, 17 insertions(+), 13 deletions(-)

diff --git a/controls/sae_2025_ws/src/uav/setup.py b/controls/sae_2025_ws/src/uav/setup.py
index c41a7a62..6522acab 100644
--- a/controls/sae_2025_ws/src/uav/setup.py
+++ b/controls/sae_2025_ws/src/uav/setup.py
@@ -35,6 +35,7 @@
             'global_position_offboard_control = uav.global_position_offboard_control:main',
             'mission = uav.mission:main',
             'payload_tracking_node = uav.vision_nodes.PayloadTrackingNode:main',
+            'save_image_node = uav.vision_nodes.SaveImageNode:main',
             'camera = uav.CameraNode:main',
         ],
     },
diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
index 5cee40c5..d940a5c9 100644
--- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
+++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
@@ -2,7 +2,7 @@
 from uav.autonomous_modes import Mode
 from rclpy.node import Node
 from uav import UAV
-from uav.vision_nodes.SaveImageNode import SaveImageNode
+from uav.vision_nodes import SaveImageNode
 class SaveImageMode(Mode):
     """
     A mode for navigating to a GPS coordinate
diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
index 05dd4158..822d649f 100644
--- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
@@ -7,7 +7,8 @@
 from uav.utils import pink, green, blue, yellow
 from geometry_msgs.msg import Vector3
 from std_msgs.msg import String, Float64MultiArray
-
+import time
+import os
 from std_srvs.srv import Empty
 
 from px4_msgs.msg import VehicleLocalPosition
@@ -19,25 +20,27 @@ class SaveImageNode(VisionNode):
     """
     srv = Empty
     def __init__(self):
-        super().__init__('save_vision', display=False, use_service=False)
+        super().__init__('save_image_node', display=False, use_service=False)
         self.bridge = CvBridge()
-
+        self.begin_time = time.time()
+        self.time_elapsed = self.begin_time
         # add an empty service server for ModeManager
         self.create_service(self.srv, self.service_name(), self.service_callback)
-        self.get_logger().info(f"save_vision empty service '{self.service_name()}' started.")        
-
+        self.get_logger().info(f"save_image empty service '{self.service_name()}' started.")        
 
     def service_callback(self, request, response):
         self.get_logger().info("Empty service called (placeholder).")
         return response
     def image_callback(self, msg):
-        # self.get_logger().info("Received image for hoop tracking.")
-        image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
-        import time
-        import os
-        time = int(time.time())
-        path = os.path.expanduser(f"~/vision_imgs/{image}")
-        cv2.imwrite(os.path.join(path, f"payload_{time}.png"), image)
+        if(time.time() - self.time_elapsed > 5):
+            self.time_elapsed = time.time()
+            self.get_logger().info("Received image for save")
+            image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+            image_time = int(time.time())
+            os.system(f"mkdir -p ~/saved_imgs/{int(self.begin_time)}")
+            path = os.path.expanduser(f"~/saved_imgs/{int(self.begin_time)}")
+            self.get_logger().info(os.path.join(path, f"payload_{image_time}.png"))
+            cv2.imwrite(os.path.join(path, f"payload_{image_time}.png"), image)
 
 def main():
     rclpy.init()

From 4e92701b3fa68986e18244d8d498849f15f224c0 Mon Sep 17 00:00:00 2001
From: JonKach <jon.kachappilly@gmail.com>
Date: Sat, 24 Jan 2026 21:22:55 +0000
Subject: [PATCH 3/7] updated mission

---
 controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml      | 2 +-
 controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
index f774177a..96dcb424 100644
--- a/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
+++ b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
@@ -1,7 +1,7 @@
 start:
   class: uav.autonomous_modes.SaveImageMode
   params:
-    coordinates: '[((5, 3, -10), 5, "LOCAL"), ((10, 1, -1), 3, "LOCAL")]'
+    coordinates: '[((0, 0, -6), 10, "LOCAL"), ((0, 20, -3), 10, "LOCAL"), ((5, 5, -6), 10, "LOCAL")]'
   transitions:
     complete: land
 
diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
index 822d649f..0d53ce7c 100644
--- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
@@ -32,7 +32,7 @@ def service_callback(self, request, response):
         self.get_logger().info("Empty service called (placeholder).")
         return response
     def image_callback(self, msg):
-        if(time.time() - self.time_elapsed > 5):
+        if(time.time() - self.time_elapsed > 2):
             self.time_elapsed = time.time()
             self.get_logger().info("Received image for save")
             image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')

From 2e7b1f0fa9c9c5c7b8c1500e266794c15b92dea5 Mon Sep 17 00:00:00 2001
From: 0frankie <frankiel06@proton.me>
Date: Mon, 2 Feb 2026 00:04:03 +0000
Subject: [PATCH 4/7] Added revolving in circle ImageMode

---
 .../uav/uav/autonomous_modes/SaveImageMode.py | 43 +++++++++++++++++--
 1 file changed, 39 insertions(+), 4 deletions(-)

diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
index d940a5c9..8da33e74 100644
--- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
+++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
@@ -1,3 +1,4 @@
+import math
 from typing import List
 from uav.autonomous_modes import Mode
 from rclpy.node import Node
@@ -23,7 +24,12 @@ def __init__(self, node: Node, uav: UAV, coordinates: List[tuple[tuple[float, fl
         self.coordinates = coordinates
         self.goal = None
         self.margin = margin
+        self.circle_time = 5
+        self.circle_pause_time = 2
+        self.circle_radius = 0
+        self.is_circling = False
         self.index = -1
+        self.angle = 0
 
     def on_update(self, time_delta: float) -> None:
         """
@@ -34,9 +40,10 @@ def on_update(self, time_delta: float) -> None:
             dist = self.uav.distance_to_waypoint(self.coordinate_system, self.goal)
             self.log(f"Distance to waypoint: {dist}, current position: {self.uav.get_local_position()}")
             
-        if dist >= self.margin:
+        if dist >= self.margin and not self.is_circling:
             self.uav.publish_position_setpoint(self.target) # PX4 expects stream of setpoints
-        elif self.goal is None or dist < self.margin:
+            self.circle_radius = -self.target[2] / 3
+        elif self.goal is None or dist < self.margin + self.circle_radius:
             if self.index == -1 or self.wait_time <= 0:
                 self.index += 1
                 if self.index >= len(self.coordinates):
@@ -45,8 +52,36 @@ def on_update(self, time_delta: float) -> None:
                 self.target = self.get_local_target()
                 self.uav.publish_position_setpoint(self.target)
             else:
-                self.wait_time -= time_delta
-                self.log(f"Holding - waiting for {self.wait_time} more seconds")
+                if self.circle_time >= 0:
+                    self.is_circling = True
+                    circle_target = list(self.target)
+                    angle_in_radians = self.angle * math.pi / 180
+                    circle_target[0] += self.circle_radius * math.cos(angle_in_radians)
+                    circle_target[1] += self.circle_radius * math.sin(angle_in_radians)
+
+                    self.uav.publish_position_setpoint(tuple(circle_target))
+
+                    if self.circle_pause_time > 0:
+                        self.circle_pause_time -= time_delta
+                        self.log(f"Angle: {self.angle}")
+                        self.log(f"Pausing to take photos for: {self.circle_pause_time} more seconds")
+                        return
+
+                    self.circle_time -= time_delta
+                    self.angle += 5
+
+                    if self.angle % 15 == 0:
+                        self.circle_pause_time = 2
+                    else:
+                        # self.log(f"Angle: {self.angle}")
+                        self.log(f"Holding - circling for {self.circle_time} more seconds")
+
+                elif self.wait_time >= 0:
+                    self.wait_time -= time_delta
+                    # self.log(f"Holding - waiting for {self.wait_time} more seconds")
+                else:
+                    self.circle_time = 5
+                    self.is_circling = False
 
     def get_local_target(self) -> tuple[float, float, float]:
         """

From caded1e2046debae435271181d451f6ce5177e1f Mon Sep 17 00:00:00 2001
From: 0frankie <frankiel06@proton.me>
Date: Mon, 2 Feb 2026 00:05:31 +0000
Subject: [PATCH 5/7] changed timing for SaveImageNode

---
 .../sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py   | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
index 0d53ce7c..06a42c43 100644
--- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
@@ -32,14 +32,14 @@ def service_callback(self, request, response):
         self.get_logger().info("Empty service called (placeholder).")
         return response
     def image_callback(self, msg):
-        if(time.time() - self.time_elapsed > 2):
+        if(time.time() - self.time_elapsed > 0.5):
             self.time_elapsed = time.time()
-            self.get_logger().info("Received image for save")
+            # self.get_logger().info("Received image for save")
             image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
             image_time = int(time.time())
             os.system(f"mkdir -p ~/saved_imgs/{int(self.begin_time)}")
             path = os.path.expanduser(f"~/saved_imgs/{int(self.begin_time)}")
-            self.get_logger().info(os.path.join(path, f"payload_{image_time}.png"))
+            # self.get_logger().info(os.path.join(path, f"payload_{image_time}.png"))
             cv2.imwrite(os.path.join(path, f"payload_{image_time}.png"), image)
 
 def main():

From 01041ab43771fee6053f1c6bb45f87cd6ba5463c Mon Sep 17 00:00:00 2001
From: 0frankie <frankiel06@proton.me>
Date: Sat, 7 Feb 2026 19:56:28 +0000
Subject: [PATCH 6/7] attempt to fix circle into cycles

---
 controls/sae_2025_ws/README.md                   | 13 +++++++++++++
 .../src/uav/launch/launch_params.yaml            |  8 ++++----
 .../uav/uav/autonomous_modes/SaveImageMode.py    | 16 +++++++++-------
 .../src/uav/uav/missions/save_image.yaml         |  2 +-
 .../src/uav/uav/vision_nodes/SaveImageNode.py    |  6 +++---
 5 files changed, 30 insertions(+), 15 deletions(-)

diff --git a/controls/sae_2025_ws/README.md b/controls/sae_2025_ws/README.md
index 670d9220..7b827695 100644
--- a/controls/sae_2025_ws/README.md
+++ b/controls/sae_2025_ws/README.md
@@ -120,7 +120,20 @@ You might run into the following issues during the build process. Here are solut
     cd ~/{path_to_monorepo}/controls/sae_2025_ws/src
     git clone git@github.com:rudislabs/actuator_msgs.git
     ```
+5. **Missing `tf_transformations`**:
+   
+   ```bash
+   sudo apt install tf_transformations
+   ```
 
+6. **`numpy` version is wrong:**
+
+   You might see one of two errors:
+   - `AttributeError: module 'numpy' has no attribute `**
+   - `AttributeError: `np.maximum_sctype` was removed in the NumPy 2.0 release`
+   ```bash
+   pip install numpy==1.21.5
+   ```
 ---
 
 ## Launching Components
diff --git a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
index b00b8a5f..1cb7a142 100644
--- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
+++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
@@ -1,16 +1,16 @@
 # launch_params.yaml
-mission_name: basic_waypoints
+mission_name: save_image
 uav_debug: false
-vision_debug: false
+vision_debug: true
 run_mission: true
-airframe: standard_vtol # Enter a preset below or a valid airframe ID (integer), either custom or from PX4
+airframe: quadcopter # Enter a preset below or a valid airframe ID (integer), either custom or from PX4
                      # quadcopter, tiltrotor_vtol, fixed_wing, standard_vtol, quadtailsitter
                      # Other useful IDs: 4014 for mono cam down
                      # IDs available for simulation can be found in PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
                      # These IDs load an airframe file and the corresponding model file in PX4-Autopilot/Tools/simulation/gz/models
 use_camera: false
 custom_airframe_model: '' # Leave blank if using default PX4 model or enter file name of custom model
-save_vision: false
+save_vision: true
 servo_only: false
 camera_offsets: [0, 0, 0] # Should denote the position of the camera relative to the payload mechanism, in meters
                           # In NED frame: x is forward, y is right, and z is down.
diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
index 8da33e74..6842d01e 100644
--- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
+++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
@@ -24,12 +24,13 @@ def __init__(self, node: Node, uav: UAV, coordinates: List[tuple[tuple[float, fl
         self.coordinates = coordinates
         self.goal = None
         self.margin = margin
-        self.circle_time = 5
         self.circle_pause_time = 2
         self.circle_radius = 0
         self.is_circling = False
         self.index = -1
         self.angle = 0
+        self.angle_increment = 15
+        self.circle_cycle = 360 / self.angle_increment
 
     def on_update(self, time_delta: float) -> None:
         """
@@ -52,7 +53,7 @@ def on_update(self, time_delta: float) -> None:
                 self.target = self.get_local_target()
                 self.uav.publish_position_setpoint(self.target)
             else:
-                if self.circle_time >= 0:
+                if self.circle_cycle >= 0:
                     self.is_circling = True
                     circle_target = list(self.target)
                     angle_in_radians = self.angle * math.pi / 180
@@ -67,21 +68,22 @@ def on_update(self, time_delta: float) -> None:
                         self.log(f"Pausing to take photos for: {self.circle_pause_time} more seconds")
                         return
 
-                    self.circle_time -= time_delta
+                    self.circle_cycle -= 1
                     self.angle += 5
 
                     if self.angle % 15 == 0:
                         self.circle_pause_time = 2
                     else:
                         # self.log(f"Angle: {self.angle}")
-                        self.log(f"Holding - circling for {self.circle_time} more seconds")
+                        self.log(f"Holding - circling for {self.circle_cycle} more cycles.")
 
                 elif self.wait_time >= 0:
                     self.wait_time -= time_delta
+                    if self.wait_time <= 0:
+                        self.angle = 0
+                        self.is_circling = False
+                        self.circle_cycle = 360 / self.angle_increment
                     # self.log(f"Holding - waiting for {self.wait_time} more seconds")
-                else:
-                    self.circle_time = 5
-                    self.is_circling = False
 
     def get_local_target(self) -> tuple[float, float, float]:
         """
diff --git a/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
index 96dcb424..ef4d8757 100644
--- a/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
+++ b/controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml
@@ -1,7 +1,7 @@
 start:
   class: uav.autonomous_modes.SaveImageMode
   params:
-    coordinates: '[((0, 0, -6), 10, "LOCAL"), ((0, 20, -3), 10, "LOCAL"), ((5, 5, -6), 10, "LOCAL")]'
+    coordinates: '[((0, 0, -6), 10, "LOCAL"), ((0, 20, -3), 10, "LOCAL")]'
   transitions:
     complete: land
 
diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
index 06a42c43..1f4051bb 100644
--- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
@@ -32,14 +32,14 @@ def service_callback(self, request, response):
         self.get_logger().info("Empty service called (placeholder).")
         return response
     def image_callback(self, msg):
-        if(time.time() - self.time_elapsed > 0.5):
+        if time.time() - self.time_elapsed > 0.5:
             self.time_elapsed = time.time()
-            # self.get_logger().info("Received image for save")
+            self.get_logger().info("Received image for save")
             image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
             image_time = int(time.time())
             os.system(f"mkdir -p ~/saved_imgs/{int(self.begin_time)}")
             path = os.path.expanduser(f"~/saved_imgs/{int(self.begin_time)}")
-            # self.get_logger().info(os.path.join(path, f"payload_{image_time}.png"))
+            self.get_logger().info(os.path.join(path, f"payload_{image_time}.png"))
             cv2.imwrite(os.path.join(path, f"payload_{image_time}.png"), image)
 
 def main():

From 511fc4834f45bc154327abb8d79c9f4613aa8ba7 Mon Sep 17 00:00:00 2001
From: JonKach <jon.kachappilly@gmail.com>
Date: Sat, 7 Feb 2026 20:26:14 +0000
Subject: [PATCH 7/7] Fixed camera node

---
 controls/sae_2025_ws/README.md                           | 7 +++++++
 controls/sae_2025_ws/src/uav/launch/launch_params.yaml   | 2 +-
 controls/sae_2025_ws/src/uav/launch/main.launch.py       | 2 ++
 .../src/uav/uav/autonomous_modes/SaveImageMode.py        | 9 +++++----
 .../src/uav/uav/vision_nodes/SaveImageNode.py            | 2 +-
 5 files changed, 16 insertions(+), 6 deletions(-)

diff --git a/controls/sae_2025_ws/README.md b/controls/sae_2025_ws/README.md
index 7b827695..05e59670 100644
--- a/controls/sae_2025_ws/README.md
+++ b/controls/sae_2025_ws/README.md
@@ -134,6 +134,13 @@ You might run into the following issues during the build process. Here are solut
    ```bash
    pip install numpy==1.21.5
    ```
+
+7. **Missing `generate_parameter_library`:**
+
+    ```bash
+    sudo apt install ros-humble-generate-parameter-library
+    ```
+
 ---
 
 ## Launching Components
diff --git a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
index 1cb7a142..894a1d9b 100644
--- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
+++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
@@ -8,7 +8,7 @@ airframe: quadcopter # Enter a preset below or a valid airframe ID (integer), ei
                      # Other useful IDs: 4014 for mono cam down
                      # IDs available for simulation can be found in PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
                      # These IDs load an airframe file and the corresponding model file in PX4-Autopilot/Tools/simulation/gz/models
-use_camera: false
+use_camera: true
 custom_airframe_model: '' # Leave blank if using default PX4 model or enter file name of custom model
 save_vision: true
 servo_only: false
diff --git a/controls/sae_2025_ws/src/uav/launch/main.launch.py b/controls/sae_2025_ws/src/uav/launch/main.launch.py
index c8a6b887..5df86700 100644
--- a/controls/sae_2025_ws/src/uav/launch/main.launch.py
+++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py
@@ -67,6 +67,7 @@ def launch_setup(context, *args, **kwargs):
         ))
 
         for node in extract_vision_nodes(YAML_PATH):
+            print(f"Adding vision node: {node}")
             vision_nodes.append(node)
             # Convert CamelCase node names to snake_case executable names.
             s1 = re.sub('(.)([A-Z][a-z]+)', r'\1_\2', node)
@@ -78,6 +79,7 @@ def launch_setup(context, *args, **kwargs):
                 output='screen',
                 parameters=[{'debug': vision_debug_bool, 'sim': sim_bool, 'save_vision': save_vision_bool}],
             ))
+        print(f"Final vision nodes to launch: {vision_nodes}")
         
         # Clear vision node actions if none are found.
         if len(vision_nodes) == 0:
diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
index 6842d01e..c14480ad 100644
--- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
+++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py
@@ -39,7 +39,7 @@ def on_update(self, time_delta: float) -> None:
         dist = 0
         if self.index != -1:
             dist = self.uav.distance_to_waypoint(self.coordinate_system, self.goal)
-            self.log(f"Distance to waypoint: {dist}, current position: {self.uav.get_local_position()}")
+            # self.log(f"Distance to waypoint: {dist}, current position: {self.uav.get_local_position()}")
             
         if dist >= self.margin and not self.is_circling:
             self.uav.publish_position_setpoint(self.target) # PX4 expects stream of setpoints
@@ -64,8 +64,8 @@ def on_update(self, time_delta: float) -> None:
 
                     if self.circle_pause_time > 0:
                         self.circle_pause_time -= time_delta
-                        self.log(f"Angle: {self.angle}")
-                        self.log(f"Pausing to take photos for: {self.circle_pause_time} more seconds")
+                        # self.log(f"Angle: {self.angle}")
+                        # self.log(f"Pausing to take photos for: {self.circle_pause_time} more seconds")
                         return
 
                     self.circle_cycle -= 1
@@ -74,8 +74,9 @@ def on_update(self, time_delta: float) -> None:
                     if self.angle % 15 == 0:
                         self.circle_pause_time = 2
                     else:
+                        pass
                         # self.log(f"Angle: {self.angle}")
-                        self.log(f"Holding - circling for {self.circle_cycle} more cycles.")
+                        # self.log(f"Holding - circling for {self.circle_cycle} more cycles.")
 
                 elif self.wait_time >= 0:
                     self.wait_time -= time_delta
diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
index 1f4051bb..d4c2f7cc 100644
--- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
+++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py
@@ -32,7 +32,7 @@ def service_callback(self, request, response):
         self.get_logger().info("Empty service called (placeholder).")
         return response
     def image_callback(self, msg):
-        if time.time() - self.time_elapsed > 0.5:
+        if time.time() - self.time_elapsed > 2:
             self.time_elapsed = time.time()
             self.get_logger().info("Received image for save")
             image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')