User/jonkach/save vision

controls/sae_2025_ws/README.md

@@ -120,6 +120,26 @@ 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
+   ```
+
+7. **Missing `generate_parameter_library`:**
+
+    ```bash
+    sudo apt install ros-humble-generate-parameter-library
+    ```
 
 ---
 

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
+use_camera: true
 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.

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:

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',
         ],
     },

controls/sae_2025_ws/src/uav/uav/autonomous_modes/SaveImageMode.py

@@ -0,0 +1,113 @@
+import math
+from typing import List
+from uav.autonomous_modes import Mode
+from rclpy.node import Node
+from uav import UAV
+from uav.vision_nodes 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.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:
+        """
+        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 and not self.is_circling:
+            self.uav.publish_position_setpoint(self.target) # PX4 expects stream of setpoints
+            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):
+                    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:
+                if self.circle_cycle >= 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_cycle -= 1
+                    self.angle += 5
+
+                    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.")
+
+                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")
+
+    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"

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
+from .WaypointMission import WaypointMission
+from .SaveImageMode import SaveImageMode

controls/sae_2025_ws/src/uav/uav/missions/save_image.yaml

@@ -0,0 +1,9 @@
+start:
+  class: uav.autonomous_modes.SaveImageMode
+  params:
+    coordinates: '[((0, 0, -6), 10, "LOCAL"), ((0, 20, -3), 10, "LOCAL")]'
+  transitions:
+    complete: land
+
+land:
+  class: uav.autonomous_modes.LandingMode

controls/sae_2025_ws/src/uav/uav/vision_nodes/SaveImageNode.py

@@ -0,0 +1,54 @@
+# 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
+import time
+import os
+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_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_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):
+        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')
+            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()
+    node = SaveImageNode()
+    try:    
+        rclpy.spin(node)
+    except Exception as e:
+        print(e)
+        node.publish_failsafe()
+
+    rclpy.shutdown()

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
+from .PayloadTrackingNode import PayloadTrackingNode
+from .SaveImageNode import SaveImageNode