From 883dc950bb29240ba26b79ec11b5e1120a6112fa Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 12 Nov 2025 04:01:06 +0000 Subject: [PATCH 01/14] Hoop 1 --- controls/sae_2025_ws/src/uav/setup.py | 1 + .../src/uav/uav/cv/in_house/__init__.py | 0 .../src/uav/uav/cv/in_house/dlz.py | 0 .../src/uav/uav/cv/in_house/hoop.py | 133 ++++++++++++++++++ .../uav/uav/vision_nodes/HoopTrackingNode.py | 110 ++++++++++++++- .../src/uav/uav/vision_nodes/__init__.py | 3 +- .../uav_interfaces/src/srv/HoopTracking.srv | 8 ++ 7 files changed, 253 insertions(+), 2 deletions(-) create mode 100644 controls/sae_2025_ws/src/uav/uav/cv/in_house/__init__.py create mode 100644 controls/sae_2025_ws/src/uav/uav/cv/in_house/dlz.py create mode 100644 controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py create mode 100644 controls/sae_2025_ws/src/uav_interfaces/src/srv/HoopTracking.srv diff --git a/controls/sae_2025_ws/src/uav/setup.py b/controls/sae_2025_ws/src/uav/setup.py index 8ac0f71e..496dc4d4 100644 --- a/controls/sae_2025_ws/src/uav/setup.py +++ b/controls/sae_2025_ws/src/uav/setup.py @@ -36,6 +36,7 @@ 'flight = uav.flight:main', 'mission = uav.mission:main', 'payload_tracking_node = uav.vision_nodes.PayloadTrackingNode:main', + 'hoop_tracking_node = uav.vision_nodes.HoopTrackingNode:main', 'camera = uav.CameraNode:main', 'test_flight = uav.test_flight:main', 'vtol_testing = uav.vtol_testing:main', diff --git a/controls/sae_2025_ws/src/uav/uav/cv/in_house/__init__.py b/controls/sae_2025_ws/src/uav/uav/cv/in_house/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/controls/sae_2025_ws/src/uav/uav/cv/in_house/dlz.py b/controls/sae_2025_ws/src/uav/uav/cv/in_house/dlz.py new file mode 100644 index 00000000..e69de29b diff --git a/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py b/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py new file mode 100644 index 00000000..8a8079cd --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py @@ -0,0 +1,133 @@ +# TEAM 6 IN PROGRESS HOOP DETECTION +import cv2 +import numpy as np +from scipy.spatial import distance +from typing import Optional, Tuple +import os + +def detect_hoop( + image: np.ndarray, + uuid: str, + debug: bool = False, + save_vision: bool = False +) -> Optional[Tuple[int, int]]: + """ + Detect hoop in image using edge detection and ellipse fitting. + + Args: + image (np.ndarray): Input BGR image. + uuid (str): Unique identifier for saving debug images. + debug (bool): If True, display debug windows. + save_vision (bool): If True, save visualization images. + + Returns: + Optional[Tuple[int, int]]: A tuple (cx, cy) if detection is successful; + otherwise, None. + """ + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + h, w = gray.shape + blurred = cv2.GaussianBlur(gray, (9, 9), 2) + edges1 = cv2.Canny(blurred, 30, 100) + edges2 = cv2.Canny(blurred, 50, 150) + edges = cv2.bitwise_or(edges1, edges2) + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) + closed = cv2.morphologyEx(edges, cv2.MORPH_CLOSE, kernel, iterations=2) + dilated = cv2.dilate(closed, kernel, iterations=1) + + contours, hierarchy = cv2.findContours(dilated, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) + + hoop_candidates = [] + for i, contour in enumerate(contours): + area = cv2.contourArea(contour) + if area < 1000 or area > w * h * 0.5: + continue + + perimeter = cv2.arcLength(contour, True) + if perimeter < 100: + continue + + circularity = 4 * np.pi * area / (perimeter ** 2) if perimeter > 0 else 0 + if len(contour) >= 5: + try: + ellipse = cv2.fitEllipse(contour) + (x, y), (MA, ma), angle = ellipse + if x < 0 or x >= w or y < 0 or y >= h: + continue + if MA > 0: + aspect_ratio = ma / MA + else: + continue + + if 0.4 < aspect_ratio < 1.6 and circularity > 0.3: + center = (int(x), int(y)) + cam_pos = (w // 2, h) + dist = distance.euclidean(center, cam_pos) + size_score = (MA + ma) / 2 + + hoop_candidates.append({ + 'contour': contour, + 'ellipse': ellipse, + 'center': center, + 'distance': dist, + 'circularity': circularity, + 'aspect_ratio': aspect_ratio, + 'area': area, + 'size': size_score + }) + except: + continue + + if not hoop_candidates: + if debug or save_vision: + vis_image = image.copy() + large_contours = [c for c in contours if cv2.contourArea(c) > 500] + cv2.drawContours(vis_image, large_contours, -1, (0, 0, 255), 2) + cv2.putText(vis_image, "No Hoop Detected", (50, 50), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) + if debug: + cv2.namedWindow("Hoop Tracking", cv2.WINDOW_AUTOSIZE) + cv2.imshow("Hoop Tracking", vis_image) + cv2.imshow("Hoop Edges", dilated) + cv2.waitKey(1) + if save_vision: + import time + time_stamp = int(time.time()) + path = os.path.expanduser(f"~/vision_imgs/{uuid}") + cv2.imwrite(os.path.join(path, f"hoop_{time_stamp}.png"), vis_image) + cv2.imwrite(os.path.join(path, f"hoop_edges_{time_stamp}.png"), dilated) + return None + + # Sort by size and select the largest + hoop_candidates.sort(key=lambda x: x['size'], reverse=True) + closest_hoop = hoop_candidates[0] + center = closest_hoop['center'] + + if debug or save_vision: + vis_image = image.copy() + # Draw all candidates + for idx, hoop in enumerate(hoop_candidates): + color = (0, 255, 0) if idx == 0 else (255, 165, 0) + cv2.ellipse(vis_image, hoop['ellipse'], color, 2) + + # Highlight the selected hoop + cv2.ellipse(vis_image, closest_hoop['ellipse'], (0, 255, 0), 4) + cv2.circle(vis_image, center, 10, (0, 0, 255), -1) + cv2.line(vis_image, (center[0] - 30, center[1]), (center[0] + 30, center[1]), (0, 0, 255), 3) + cv2.line(vis_image, (center[0], center[1] - 30), (center[0], center[1] + 30), (0, 0, 255), 3) + text = f"Center: ({center[0]}, {center[1]})" + cv2.putText(vis_image, text, (center[0] + 40, center[1] - 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2) + + if debug: + cv2.namedWindow("Hoop Tracking", cv2.WINDOW_AUTOSIZE) + cv2.imshow("Hoop Tracking", vis_image) + cv2.imshow("Hoop Edges", dilated) + cv2.waitKey(1) + if save_vision: + import time + time_stamp = int(time.time()) + path = os.path.expanduser(f"~/vision_imgs/{uuid}") + cv2.imwrite(os.path.join(path, f"hoop_{time_stamp}.png"), vis_image) + cv2.imwrite(os.path.join(path, f"hoop_edges_{time_stamp}.png"), dilated) + + return center[0], center[1] \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py index 835a3b45..b89b76e8 100644 --- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py +++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py @@ -1 +1,109 @@ -# TO DO FELICIA AND BELLE \ No newline at end of file +#!/usr/bin/env python3 +# hoop_tracking_node.py +import cv2 +import numpy as np +from uav.cv.in_house.hoop import detect_hoop +from uav.cv.tracking import compute_3d_vector, rotate_image +from uav.vision_nodes import VisionNode +from uav_interfaces.srv import HoopTracking +import rclpy + + +class HoopTrackingNode(VisionNode): + """ + ROS node for hoop tracking with Kalman filtering. + """ + srv = HoopTracking + + def __init__(self): + super().__init__('hoop_tracking', self.__class__.srv) + + # Initialize Kalman filter + self.kalman = cv2.KalmanFilter(4, 2) + self._setup_kalman_filter() + + self.create_service(HoopTracking, self.service_name(), self.service_callback) + + def _setup_kalman_filter(self): + """Initialize Kalman filter matrices""" + dt = 1 + + # State transition matrix [x, y, vx, vy] + self.kalman.transitionMatrix = np.array([ + [1, 0, dt, 0], + [0, 1, 0, dt], + [0, 0, 1, 0], + [0, 0, 0, 1] + ], dtype=np.float32) + + # Measurement matrix [x, y] + self.kalman.measurementMatrix = np.array([ + [1, 0, 0, 0], + [0, 1, 0, 0] + ], dtype=np.float32) + + # Tune these covariances + self.kalman.processNoiseCov = np.eye(4, dtype=np.float32) * 1e-2 + self.kalman.measurementNoiseCov = np.eye(2, dtype=np.float32) * 1e-1 + self.kalman.errorCovPost = np.eye(4, dtype=np.float32) + + def service_callback(self, request: HoopTracking.Request, + response: HoopTracking.Response): + """Process tracking service request with Kalman filtering""" + image, camera_info = self.request_data(cam_image=True, cam_info=True) + image = self.convert_image_msg_to_frame(image) + image = rotate_image(image, -np.rad2deg(request.yaw)) + + # Predict next state + prediction = self.kalman.predict() + predicted_x, predicted_y = prediction[0, 0], prediction[1, 0] + + # Get raw detection + detection = detect_hoop( + image, + self.uuid, + self.debug, + self.save_vision + ) + + hoop_detected = False + if detection is not None: + cx, cy = detection + # Update Kalman filter with measurement + measurement = np.array([[np.float32(cx)], [np.float32(cy)]]) + corrected_state = self.kalman.correct(measurement) + x, y = corrected_state[0, 0], corrected_state[1, 0] + hoop_detected = True + else: + # Use prediction if no detection + x, y = predicted_x, predicted_y + # Fallback to image center if prediction is invalid + if x <= 0 or y <= 0: + h, w = image.shape[:2] + x, y = w / 2, h / 2 + + # Compute 3D direction vector + direction = compute_3d_vector(x, y, np.array(camera_info.k).reshape(3, 3), request.altitude) + + # Populate response + response.x = float(x) + response.y = float(y) + response.direction = direction + response.hoop_detected = hoop_detected + return response + + +def main(): + rclpy.init() + node = HoopTrackingNode() + try: + rclpy.spin(node) + except Exception as e: + print(e) + node.publish_failsafe() + + rclpy.shutdown() + + +if __name__ == "__main__": + main() 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..91f138d6 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 .HoopTrackingNode import HoopTrackingNode \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav_interfaces/src/srv/HoopTracking.srv b/controls/sae_2025_ws/src/uav_interfaces/src/srv/HoopTracking.srv new file mode 100644 index 00000000..b8d453d1 --- /dev/null +++ b/controls/sae_2025_ws/src/uav_interfaces/src/srv/HoopTracking.srv @@ -0,0 +1,8 @@ +float64 altitude +float64 yaw +--- +float64 x +float64 y +float64[3] direction +bool hoop_detected + From 9b0716c52cee5b5b72f62a2e1f27c3bf7b234ab6 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 12 Nov 2025 04:11:13 +0000 Subject: [PATCH 02/14] BLAH --- .../src/uav/launch/launch_params.yaml | 2 +- .../autonomous_modes/HoopNavigationMode.py | 167 ++++++++++++++++++ .../src/uav/uav/autonomous_modes/__init__.py | 3 +- .../uav/uav/missions/in_house_comp_hoop.yaml | 10 +- 4 files changed, 176 insertions(+), 6 deletions(-) create mode 100644 controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py 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 ca383cf2..2d18ad24 100644 --- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml +++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml @@ -1,5 +1,5 @@ # launch_params.yaml -mission_name: test_straight_course +mission_name: in_house_comp_hoop uav_debug: false vision_debug: false sim: true diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py new file mode 100644 index 00000000..db7d4d7e --- /dev/null +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py @@ -0,0 +1,167 @@ +import numpy as np +from uav import UAV +from uav.autonomous_modes import Mode +from rclpy.node import Node +from uav_interfaces.srv import HoopTracking +from uav.vision_nodes import HoopTrackingNode +from typing import Optional, Tuple +import cv2 + +class HoopNavigationMode(Mode): + """ + A mode for navigating through a hoop using vision tracking. + """ + + def __init__(self, node: Node, uav: UAV, approach_distance: float = 5.0, + pass_distance: float = 2.0, speed_factor: float = 1.5): + """ + Initialize the HoopNavigationMode. + + Args: + node (Node): ROS 2 node managing the UAV. + uav (UAV): The UAV instance to control. + approach_distance (float): Distance at which to start centering on hoop (meters). + pass_distance (float): Distance to travel past the hoop center (meters). + speed_factor (float): Speed multiplier for forward movement. + """ + super().__init__(node, uav) + + self.approach_distance = approach_distance + self.pass_distance = pass_distance + self.speed_factor = speed_factor + self.done = False + self.hoop_centered = False + self.passing_through = False + self.hoop_pass_target = None + self.initial_hoop_detection = False + + def on_enter(self) -> None: + """ + Called when this mode is activated. + """ + self.log("HoopNavigationMode activated") + self.done = False + self.hoop_centered = False + self.passing_through = False + self.hoop_pass_target = None + self.initial_hoop_detection = False + + def on_update(self, time_delta: float) -> None: + """ + Periodic logic for navigating through a hoop. + """ + # If UAV is unstable, skip the update + if self.uav.roll > 0.1 or self.uav.pitch > 0.1: + self.log("Roll or pitch detected. Waiting for stabilization.") + return + + # Get current altitude + current_altitude = -self.uav.get_local_position()[2] + + # Request hoop tracking + request = HoopTracking.Request() + request.altitude = current_altitude + request.yaw = float(self.uav.yaw) + response = self.send_request(HoopTrackingNode, request) + + # If no response received yet, exit early + if response is None: + return + + # If we're already passing through, just go straight + if self.passing_through: + if self.hoop_pass_target: + self.uav.publish_position_setpoint(self.hoop_pass_target) + if self.uav.distance_to_waypoint('LOCAL', self.hoop_pass_target) <= 0.3: + self.done = True + self.log("Successfully passed through hoop!") + return + + # Check if hoop is detected + if not response.hoop_detected: + self.log("No hoop detected. Holding position.") + # If we never detected a hoop, just hold position + if not self.initial_hoop_detection: + return + # If we lost the hoop but detected it before, move forward slowly + else: + direction = [0, 0, 0] + direction[2] = 0 + self.log("Lost hoop tracking. Moving forward slowly.") + # Move forward in body frame + forward_body = self.uav.uav_to_local((self.speed_factor * 0.5, 0, 0)) + self.uav.publish_position_setpoint(forward_body, relative=True) + return + + # Mark that we've detected the hoop at least once + self.initial_hoop_detection = True + + # Convert direction vector from camera frame to UAV frame + # response.direction is [x, y, z] in camera frame + # Camera frame: x right, y down, z forward + # UAV frame: x forward (North), y left (West), z up + direction = [-response.direction[1], response.direction[0], -response.direction[2]] + + # Apply camera offsets + camera_offsets = tuple(x / current_altitude for x in self.uav.camera_offsets) if current_altitude > 1 else self.uav.camera_offsets + direction = [x + y for x, y in zip(direction, self.uav.uav_to_local(camera_offsets))] + + # Normalize direction + direction_magnitude = np.linalg.norm(direction) + if direction_magnitude > 0: + direction_normalized = [d / direction_magnitude for d in direction] + else: + direction_normalized = [0, 0, 0] + + # Estimate distance to hoop (rough approximation based on altitude and direction) + estimated_distance = current_altitude * direction_magnitude + + self.log(f"Hoop detected. Direction: {direction}, Est. distance: {estimated_distance:.2f}m") + + # Check if hoop is centered (within threshold) + # Check x and y components of normalized direction + centering_threshold = 0.05 # Adjust as needed + is_centered = (abs(direction_normalized[0]) < centering_threshold and + abs(direction_normalized[1]) < centering_threshold) + + if is_centered: + self.log("Hoop centered! Moving forward to pass through.") + # Calculate target position to pass through hoop + current_pos = self.uav.get_local_position() + # Move forward in the direction of the hoop + forward_distance = self.pass_distance + forward_direction = self.uav.uav_to_local((forward_distance, 0, 0)) + self.hoop_pass_target = tuple(current_pos[i] + forward_direction[i] for i in range(3)) + self.passing_through = True + else: + # Not centered yet - adjust position to center on hoop + # Scale movement based on error + movement_scale = min(1.0, direction_magnitude) + adjusted_direction = [d * movement_scale for d in direction] + + # Slow down vertical movement for stability + adjusted_direction[2] *= 0.5 + + self.log(f"Centering on hoop. Adjusted direction: {adjusted_direction}") + self.uav.publish_position_setpoint(adjusted_direction, relative=True) + + def check_status(self) -> str: + """ + Check the status of the hoop navigation. + + Returns: + str: The status of the hoop navigation. + """ + if self.done: + return 'complete' + return 'continue' + + def log(self, message: str): + """ + Log a message with mode context. + + Args: + message (str): The message to log. + """ + self.node.get_logger().info(f"[HoopNavigationMode] {message}") + 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..3d458309 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 .HoopNavigationMode import HoopNavigationMode \ No newline at end of file diff --git a/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml b/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml index 95525152..46ddc139 100644 --- a/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml +++ b/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml @@ -3,12 +3,14 @@ start: params: coordinates: '[((-5, -10, -10), 1, "LOCAL"),]' transitions: - complete: payload + complete: hoop -payload: - class: uav.autonomous_modes.PayloadPickupMode +hoop: + class: uav.autonomous_modes.HoopNavigationMode params: - color: 'yellow' + approach_distance: 5.0 + pass_distance: 3.0 + speed_factor: 1.5 transitions: complete: land From 36afd2d9ba62b4234126483d61902dc49dba8e65 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 12 Nov 2025 04:15:45 +0000 Subject: [PATCH 03/14] fix --- sim/sae aero/gazebo harmonic/standalone_px4_cmd.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sim/sae aero/gazebo harmonic/standalone_px4_cmd.sh b/sim/sae aero/gazebo harmonic/standalone_px4_cmd.sh index 3efbe140..45c69433 100644 --- a/sim/sae aero/gazebo harmonic/standalone_px4_cmd.sh +++ b/sim/sae aero/gazebo harmonic/standalone_px4_cmd.sh @@ -1,3 +1,3 @@ # PX4_GZ_STANDALONE=1 make px4_sitl gz_x500_lawn # PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_SIM_MODEL=gz_x500 PX4_GZ_WORLD=lawn ./build/px4_sitl_default/bin/px4 -PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_SIM_MODEL=gz_x500_mono_cam_down PX4_GZ_WORLD=custom ./build/px4_sitl_default/bin/px4 \ No newline at end of file +PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART=4001 PX4_SIM_MODEL=gz_x500_mono_cam PX4_GZ_WORLD=custom ./build/px4_sitl_default/bin/px4 \ No newline at end of file From 8c4fc70fe97fda688e0ca9fd0651943d0093566a Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 12 Nov 2025 04:17:40 +0000 Subject: [PATCH 04/14] Hoops --- controls/sae_2025_ws/src/uav/launch/main.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b1bd568d..24ac963a 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -144,7 +144,7 @@ def launch_setup(context, *args, **kwargs): # Define the PX4 SITL process. if vehicle_type == 'quadcopter': autostart = 4001 - model = 'gz_x500_mono_cam_down' + model = 'gz_x500_mono_cam' elif vehicle_type == 'tiltrotor_vtol': autostart = 4020 model = 'gz_tiltrotor' From 24557172acb35c00c9773f6bdb2a611a6e45a441 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 12 Nov 2025 04:18:52 +0000 Subject: [PATCH 05/14] Hoops --- controls/sae_2025_ws/src/uav/launch/main.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 24ac963a..335450c5 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -9,8 +9,8 @@ from launch_ros.actions import Node from uav.utils import vehicle_map -GZ_CAMERA_TOPIC = '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/image' -GZ_CAMERA_INFO_TOPIC = '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/camera_info' +GZ_CAMERA_TOPIC = '/world/custom/model/x500_mono_cam_0/link/camera_link/sensor/imager/image' +GZ_CAMERA_INFO_TOPIC = '/world/custom/model/x500_mono_cam_0/link/camera_link/sensor/imager/camera_info' HARDCODE_PATH = False def find_folder(folder_name, search_path): @@ -167,7 +167,7 @@ def launch_setup(context, *args, **kwargs): gz_ros_bridge_camera = ExecuteProcess( cmd=['ros2', 'run', 'ros_gz_bridge', 'parameter_bridge', f'{GZ_CAMERA_TOPIC}@sensor_msgs/msg/Image[gz.msgs.Image', - '--ros-args', '--remap', '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/image:=/camera'], + '--ros-args', '--remap', '/world/custom/model/x500_mono_cam_0/link/camera_link/sensor/imager/image:=/camera'], output='screen', cwd=sae_ws_path, name='gz_ros_bridge_camera' @@ -176,7 +176,7 @@ def launch_setup(context, *args, **kwargs): gz_ros_bridge_camera_info = ExecuteProcess( cmd=['ros2', 'run', 'ros_gz_bridge', 'parameter_bridge', f'{GZ_CAMERA_INFO_TOPIC}@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo', - '--ros-args', '--remap', '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/camera_info:=/camera_info'], + '--ros-args', '--remap', '/world/custom/model/x500_mono_cam_0/link/camera_link/sensor/imager/camera_info:=/camera_info'], output='screen', cwd=sae_ws_path, name='gz_ros_bridge_camera_info' From eb3f73d12b7c02b10c37a3f1d5ace72963099941 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Wed, 12 Nov 2025 04:28:31 +0000 Subject: [PATCH 06/14] fix --- controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py b/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py index 8a8079cd..5e5cfec6 100644 --- a/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py +++ b/controls/sae_2025_ws/src/uav/uav/cv/in_house/hoop.py @@ -1,7 +1,6 @@ # TEAM 6 IN PROGRESS HOOP DETECTION import cv2 import numpy as np -from scipy.spatial import distance from typing import Optional, Tuple import os @@ -61,7 +60,8 @@ def detect_hoop( if 0.4 < aspect_ratio < 1.6 and circularity > 0.3: center = (int(x), int(y)) cam_pos = (w // 2, h) - dist = distance.euclidean(center, cam_pos) + # Calculate Euclidean distance without scipy + dist = np.sqrt((center[0] - cam_pos[0])**2 + (center[1] - cam_pos[1])**2) size_score = (MA + ma) / 2 hoop_candidates.append({ From ce4d6bf6f4e12bd70983d68fb0ca0e780e91d84a Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 00:59:44 +0000 Subject: [PATCH 07/14] Type fix --- controls/sae_2025_ws/src/uav/uav/ModeManager.py | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/controls/sae_2025_ws/src/uav/uav/ModeManager.py b/controls/sae_2025_ws/src/uav/uav/ModeManager.py index 5fdd76fe..b50f882a 100644 --- a/controls/sae_2025_ws/src/uav/uav/ModeManager.py +++ b/controls/sae_2025_ws/src/uav/uav/ModeManager.py @@ -74,10 +74,15 @@ def initialize_mode(self, mode_path: str, params: dict) -> Mode: if param.annotation in (str, inspect.Parameter.empty) or name in ('node', 'uav'): args[name] = param_value else: - try: - args[name] = ast.literal_eval(param_value) - except (ValueError, SyntaxError): - raise ValueError(f"Parameter '{name}' must be a valid literal for mode '{mode_path}'. Received: {param_value}") + # If param_value is already not a string (e.g., loaded from YAML as a native type), + # use it directly. Otherwise, parse it with ast.literal_eval. + if not isinstance(param_value, str): + args[name] = param_value + else: + try: + args[name] = ast.literal_eval(param_value) + except (ValueError, SyntaxError): + raise ValueError(f"Parameter '{name}' must be a valid literal for mode '{mode_path}'. Received: {param_value}") elif param.default != inspect.Parameter.empty: args[name] = param.default else: From e978b7fe32a2e802d88a15543ce6f32af56e8a55 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 01:13:36 +0000 Subject: [PATCH 08/14] in progress --- .../sae_2025_ws/src/uav/uav/ModeManager.py | 17 ++---- controls/sae_2025_ws/src/uav/uav/UAV.py | 12 +++- .../autonomous_modes/HoopNavigationMode.py | 23 ++++--- .../src/uav/uav/autonomous_modes/Mode.py | 51 +++++++++------- .../uav/uav/vision_nodes/HoopTrackingNode.py | 60 ++++++++++++++----- .../uav_interfaces/src/msg/HoopTracking.msg | 6 ++ .../src/msg/PayloadTracking.msg | 6 ++ .../src/uav_interfaces/src/msg/UAVState.msg | 4 ++ 8 files changed, 118 insertions(+), 61 deletions(-) create mode 100644 controls/sae_2025_ws/src/uav_interfaces/src/msg/HoopTracking.msg create mode 100644 controls/sae_2025_ws/src/uav_interfaces/src/msg/PayloadTracking.msg create mode 100644 controls/sae_2025_ws/src/uav_interfaces/src/msg/UAVState.msg diff --git a/controls/sae_2025_ws/src/uav/uav/ModeManager.py b/controls/sae_2025_ws/src/uav/uav/ModeManager.py index b50f882a..3788ab06 100644 --- a/controls/sae_2025_ws/src/uav/uav/ModeManager.py +++ b/controls/sae_2025_ws/src/uav/uav/ModeManager.py @@ -42,21 +42,14 @@ def get_active_mode(self) -> Mode: def setup_vision(self, vision_nodes: str) -> None: """ Setup the vision node for this mode. + Vision nodes now use pub/sub instead of services. Args: - mode (Mode): The mode to setup vision for. - vision (str): The comma-separated string of vision nodes to setup for this mode. + vision_nodes (str): The comma-separated string of vision nodes (kept for compatibility, but not used). """ - if vision_nodes.strip() == '': - return - module = importlib.import_module(VISION_NODE_PATH) - for vision_node in vision_nodes.split(','): - vision_class = getattr(module, vision_node) - if vision_class.service_name() not in self.uav.vision_clients: - client = self.create_client(vision_class.srv, vision_class.service_name()) - while not client.wait_for_service(timeout_sec=1.0): - self.get_logger().info(f"Service {vision_class.service_name()} not available, waiting again...") - self.uav.vision_clients[vision_class.service_name()] = client + # Vision nodes now publish data continuously via topics + # Modes subscribe to the topics they need directly + pass def initialize_mode(self, mode_path: str, params: dict) -> Mode: module_name, class_name = mode_path.rsplit('.', 1) diff --git a/controls/sae_2025_ws/src/uav/uav/UAV.py b/controls/sae_2025_ws/src/uav/uav/UAV.py index 6a28ed86..959a90e1 100755 --- a/controls/sae_2025_ws/src/uav/uav/UAV.py +++ b/controls/sae_2025_ws/src/uav/uav/UAV.py @@ -21,6 +21,7 @@ import math from collections import deque from std_msgs.msg import Bool +from uav_interfaces.msg import UAVState from uav.px4_modes import PX4CustomMainMode, PX4CustomSubModeAuto from uav.utils import R_earth @@ -34,7 +35,6 @@ def __init__(self, node: Node, takeoff_amount=5.0, DEBUG=False, camera_offsets=[ self.node = node self.DEBUG = DEBUG self.node.get_logger().info(f"Initializing UAV with DEBUG={DEBUG}") - self.vision_clients = {} # Initialize necessary parameters to handle PX4 flight failures self.flight_check = False @@ -423,6 +423,13 @@ def _attitude_callback(self, msg: VehicleAttitude): self.pitch = np.arcsin(2.0 * (w * y - z * x)) self.yaw = np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y**2 + z**2)) + + # Publish UAV state for vision nodes + if self.local_position is not None: + uav_state_msg = UAVState() + uav_state_msg.altitude = -self.local_position.z + uav_state_msg.yaw = float(self.yaw) + self.uav_state_publisher.publish(uav_state_msg) def _global_position_callback(self, msg: VehicleGlobalPosition): self.node.destroy_subscription(self.vehicle_gps_sub) # vehicle_gps_sub is available faster but more coarse @@ -467,6 +474,9 @@ def _initialize_publishers_and_subscribers(self): self.target_position_publisher = self.node.create_publisher( VehicleLocalPosition, '/fmu/in/target_position', 10 ) + self.uav_state_publisher = self.node.create_publisher( + UAVState, '/uav_state', 10 + ) # Subscribers self.status_sub = self.node.create_subscription( diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py index db7d4d7e..69211f59 100644 --- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py @@ -2,8 +2,7 @@ from uav import UAV from uav.autonomous_modes import Mode from rclpy.node import Node -from uav_interfaces.srv import HoopTracking -from uav.vision_nodes import HoopTrackingNode +from uav_interfaces.msg import HoopTracking from typing import Optional, Tuple import cv2 @@ -34,6 +33,9 @@ def __init__(self, node: Node, uav: UAV, approach_distance: float = 5.0, self.passing_through = False self.hoop_pass_target = None self.initial_hoop_detection = False + + # Subscribe to hoop tracking data + self.subscribe_to_vision('/vision/hoop_tracking', HoopTracking, 'hoop') def on_enter(self) -> None: """ @@ -58,14 +60,11 @@ def on_update(self, time_delta: float) -> None: # Get current altitude current_altitude = -self.uav.get_local_position()[2] - # Request hoop tracking - request = HoopTracking.Request() - request.altitude = current_altitude - request.yaw = float(self.uav.yaw) - response = self.send_request(HoopTrackingNode, request) + # Get latest hoop tracking data + tracking_data = self.get_vision_data('hoop') - # If no response received yet, exit early - if response is None: + # If no tracking data received yet, exit early + if tracking_data is None: return # If we're already passing through, just go straight @@ -78,7 +77,7 @@ def on_update(self, time_delta: float) -> None: return # Check if hoop is detected - if not response.hoop_detected: + if not tracking_data.hoop_detected: self.log("No hoop detected. Holding position.") # If we never detected a hoop, just hold position if not self.initial_hoop_detection: @@ -97,10 +96,10 @@ def on_update(self, time_delta: float) -> None: self.initial_hoop_detection = True # Convert direction vector from camera frame to UAV frame - # response.direction is [x, y, z] in camera frame + # tracking_data.direction is [x, y, z] in camera frame # Camera frame: x right, y down, z forward # UAV frame: x forward (North), y left (West), z up - direction = [-response.direction[1], response.direction[0], -response.direction[2]] + direction = [-tracking_data.direction[1], tracking_data.direction[0], -tracking_data.direction[2]] # Apply camera offsets camera_offsets = tuple(x / current_altitude for x in self.uav.camera_offsets) if current_altitude > 1 else self.uav.camera_offsets diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/Mode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/Mode.py index cb307e2d..a5eb9123 100644 --- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/Mode.py +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/Mode.py @@ -1,8 +1,6 @@ from abc import ABC, abstractmethod import rclpy from rclpy.node import Node -# from rclpy.type_support import SrvRequestT -from uav.vision_nodes import VisionNode from uav import UAV class Mode(ABC): @@ -22,8 +20,8 @@ def __init__(self, node: Node, uav: UAV): self.node = node self.active = False self.uav: UAV = uav - self.vision_clients = {} - self.sent_request = False + self.vision_subscriptions = {} + self.latest_vision_data = {} def on_enter(self) -> None: """ @@ -31,25 +29,36 @@ def on_enter(self) -> None: Should include any initialization required for the mode. """ pass - - def send_request(self, vision_node: VisionNode, request): + + def subscribe_to_vision(self, topic: str, msg_type, key: str = None): """ - Send a request to a service. - + Subscribe to a vision topic and store the latest message. + Args: - request (SrvRequestT): The request to send. - service_name (VIsionNode): The name of the service. - """ - if self.sent_request: - if self.future.done(): - response = self.future.result() - self.sent_request = False - assert type(response) == vision_node.srv.Response, f"Expected response type {vision_node.srv.Response}, got {type(response)}." - return response - else: - self.sent_request = True - client = self.uav.vision_clients[vision_node.service_name()] - self.future = client.call_async(request) + topic (str): The topic to subscribe to. + msg_type: The message type. + key (str): Optional key for storing the data. Defaults to topic name. + """ + if key is None: + key = topic + + def callback(msg): + self.latest_vision_data[key] = msg + + sub = self.node.create_subscription(msg_type, topic, callback, 10) + self.vision_subscriptions[key] = sub + + def get_vision_data(self, key: str): + """ + Get the latest vision data for a given key. + + Args: + key (str): The key for the vision data. + + Returns: + The latest message, or None if no data received yet. + """ + return self.latest_vision_data.get(key, None) def on_exit(self) -> None: """ diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py index b89b76e8..de59179c 100644 --- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py +++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py @@ -5,24 +5,43 @@ from uav.cv.in_house.hoop import detect_hoop from uav.cv.tracking import compute_3d_vector, rotate_image from uav.vision_nodes import VisionNode -from uav_interfaces.srv import HoopTracking +from uav_interfaces.msg import HoopTracking, UAVState import rclpy class HoopTrackingNode(VisionNode): """ ROS node for hoop tracking with Kalman filtering. + Publishes tracking data continuously. """ - srv = HoopTracking def __init__(self): - super().__init__('hoop_tracking', self.__class__.srv) + super().__init__('hoop_tracking', None) # Initialize Kalman filter self.kalman = cv2.KalmanFilter(4, 2) self._setup_kalman_filter() - self.create_service(HoopTracking, self.service_name(), self.service_callback) + # Create publisher for hoop tracking data + self.tracking_publisher = self.create_publisher( + HoopTracking, + '/vision/hoop_tracking', + 10 + ) + + # Subscribe to UAV state + self.uav_state_sub = self.create_subscription( + UAVState, + '/uav_state', + self.uav_state_callback, + 10 + ) + + self.current_altitude = 0.0 + self.current_yaw = 0.0 + + # Create timer to publish tracking data at regular intervals + self.timer = self.create_timer(0.1, self.publish_tracking_data) # 10 Hz def _setup_kalman_filter(self): """Initialize Kalman filter matrices""" @@ -47,12 +66,21 @@ def _setup_kalman_filter(self): self.kalman.measurementNoiseCov = np.eye(2, dtype=np.float32) * 1e-1 self.kalman.errorCovPost = np.eye(4, dtype=np.float32) - def service_callback(self, request: HoopTracking.Request, - response: HoopTracking.Response): - """Process tracking service request with Kalman filtering""" + def uav_state_callback(self, msg: UAVState): + """Update current UAV state from subscription""" + self.current_altitude = msg.altitude + self.current_yaw = msg.yaw + + def publish_tracking_data(self): + """Publish hoop tracking data at regular intervals""" image, camera_info = self.request_data(cam_image=True, cam_info=True) + + # Check if we have valid image and camera info + if image is None or camera_info is None: + return + image = self.convert_image_msg_to_frame(image) - image = rotate_image(image, -np.rad2deg(request.yaw)) + image = rotate_image(image, -np.rad2deg(self.current_yaw)) # Predict next state prediction = self.kalman.predict() @@ -83,14 +111,16 @@ def service_callback(self, request: HoopTracking.Request, x, y = w / 2, h / 2 # Compute 3D direction vector - direction = compute_3d_vector(x, y, np.array(camera_info.k).reshape(3, 3), request.altitude) + direction = compute_3d_vector(x, y, np.array(camera_info.k).reshape(3, 3), self.current_altitude) - # Populate response - response.x = float(x) - response.y = float(y) - response.direction = direction - response.hoop_detected = hoop_detected - return response + # Create and publish message + msg = HoopTracking() + msg.x = float(x) + msg.y = float(y) + msg.direction = direction + msg.hoop_detected = hoop_detected + + self.tracking_publisher.publish(msg) def main(): diff --git a/controls/sae_2025_ws/src/uav_interfaces/src/msg/HoopTracking.msg b/controls/sae_2025_ws/src/uav_interfaces/src/msg/HoopTracking.msg new file mode 100644 index 00000000..4142e9f3 --- /dev/null +++ b/controls/sae_2025_ws/src/uav_interfaces/src/msg/HoopTracking.msg @@ -0,0 +1,6 @@ +# Hoop tracking data published continuously +float64 x +float64 y +float64[3] direction +bool hoop_detected + diff --git a/controls/sae_2025_ws/src/uav_interfaces/src/msg/PayloadTracking.msg b/controls/sae_2025_ws/src/uav_interfaces/src/msg/PayloadTracking.msg new file mode 100644 index 00000000..597a0c6a --- /dev/null +++ b/controls/sae_2025_ws/src/uav_interfaces/src/msg/PayloadTracking.msg @@ -0,0 +1,6 @@ +# Payload tracking data published continuously +float64 x +float64 y +float64[3] direction +bool dlz_empty + diff --git a/controls/sae_2025_ws/src/uav_interfaces/src/msg/UAVState.msg b/controls/sae_2025_ws/src/uav_interfaces/src/msg/UAVState.msg new file mode 100644 index 00000000..40cfcc6c --- /dev/null +++ b/controls/sae_2025_ws/src/uav_interfaces/src/msg/UAVState.msg @@ -0,0 +1,4 @@ +# UAV state information for vision nodes +float64 altitude +float64 yaw + From b898f9201a22a681d4d6c9f0f6060b6899a92c53 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 01:19:58 +0000 Subject: [PATCH 09/14] Very good commit message --- controls/sae_2025_ws/src/uav/launch/main.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 335450c5..7e026246 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -157,7 +157,7 @@ def launch_setup(context, *args, **kwargs): else: raise ValueError(f"Invalid vehicle type: {vehicle_type}") px4_sitl = ExecuteProcess( - cmd=['bash', '-c', f'PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART={autostart} PX4_SIM_MODEL={model} PX4_GZ_WORLD=custom ./build/px4_sitl_default/bin/px4'], + cmd=['bash', '-c', f'PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART={autostart} PX4_SIM_MODEL={model} ./build/px4_sitl_default/bin/px4'], cwd=px4_path, output='screen', name='px4_sitl' From 7f2f50fe7990c7b5814f548f965d91156069fe43 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 01:36:25 +0000 Subject: [PATCH 10/14] revert attempt --- controls/sae_2025_ws/src/uav/launch/main.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7e026246..335450c5 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -157,7 +157,7 @@ def launch_setup(context, *args, **kwargs): else: raise ValueError(f"Invalid vehicle type: {vehicle_type}") px4_sitl = ExecuteProcess( - cmd=['bash', '-c', f'PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART={autostart} PX4_SIM_MODEL={model} ./build/px4_sitl_default/bin/px4'], + cmd=['bash', '-c', f'PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART={autostart} PX4_SIM_MODEL={model} PX4_GZ_WORLD=custom ./build/px4_sitl_default/bin/px4'], cwd=px4_path, output='screen', name='px4_sitl' From db95ebd7f04ca9a7a07209b0abeecebee3a5f283 Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 02:01:17 +0000 Subject: [PATCH 11/14] more lenient --- .../autonomous_modes/HoopNavigationMode.py | 50 ++++++++++++++----- .../uav/uav/missions/in_house_comp_hoop.yaml | 1 + 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py index 69211f59..be2f507d 100644 --- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py @@ -57,23 +57,30 @@ def on_update(self, time_delta: float) -> None: self.log("Roll or pitch detected. Waiting for stabilization.") return - # Get current altitude - current_altitude = -self.uav.get_local_position()[2] + # Get current altitude and position + current_pos = self.uav.get_local_position() + current_altitude = -current_pos[2] # Get latest hoop tracking data tracking_data = self.get_vision_data('hoop') # If no tracking data received yet, exit early if tracking_data is None: + self.log("No tracking data received yet") return + + # Debug: Log current state + self.log(f"Current Position: ({current_pos[0]:.2f}, {current_pos[1]:.2f}, {current_pos[2]:.2f}), Altitude: {current_altitude:.2f}m") # If we're already passing through, just go straight if self.passing_through: if self.hoop_pass_target: + dist_to_target = self.uav.distance_to_waypoint('LOCAL', self.hoop_pass_target) + self.log(f"Passing through... Distance to target: {dist_to_target:.2f}m") self.uav.publish_position_setpoint(self.hoop_pass_target) - if self.uav.distance_to_waypoint('LOCAL', self.hoop_pass_target) <= 0.3: + if dist_to_target <= 0.3: self.done = True - self.log("Successfully passed through hoop!") + self.log("✓✓✓ Successfully passed through hoop! ✓✓✓") return # Check if hoop is detected @@ -95,15 +102,25 @@ def on_update(self, time_delta: float) -> None: # Mark that we've detected the hoop at least once self.initial_hoop_detection = True + # Debug: Log raw camera data + self.log(f"Raw Camera Direction: [{tracking_data.direction[0]:.3f}, {tracking_data.direction[1]:.3f}, {tracking_data.direction[2]:.3f}]") + self.log(f"Pixel coords: x={tracking_data.x:.1f}, y={tracking_data.y:.1f}") + # Convert direction vector from camera frame to UAV frame # tracking_data.direction is [x, y, z] in camera frame # Camera frame: x right, y down, z forward # UAV frame: x forward (North), y left (West), z up - direction = [-tracking_data.direction[1], tracking_data.direction[0], -tracking_data.direction[2]] + direction_camera = tracking_data.direction + direction_uav_raw = [-direction_camera[1], direction_camera[0], -direction_camera[2]] + self.log(f"Direction after camera->UAV conversion: [{direction_uav_raw[0]:.3f}, {direction_uav_raw[1]:.3f}, {direction_uav_raw[2]:.3f}]") # Apply camera offsets camera_offsets = tuple(x / current_altitude for x in self.uav.camera_offsets) if current_altitude > 1 else self.uav.camera_offsets - direction = [x + y for x, y in zip(direction, self.uav.uav_to_local(camera_offsets))] + camera_offsets_rotated = self.uav.uav_to_local(camera_offsets) + self.log(f"Camera offsets (raw): {self.uav.camera_offsets}, scaled: {camera_offsets}, rotated: {camera_offsets_rotated}") + + direction = [x + y for x, y in zip(direction_uav_raw, camera_offsets_rotated)] + self.log(f"Direction after camera offset: [{direction[0]:.3f}, {direction[1]:.3f}, {direction[2]:.3f}]") # Normalize direction direction_magnitude = np.linalg.norm(direction) @@ -115,22 +132,26 @@ def on_update(self, time_delta: float) -> None: # Estimate distance to hoop (rough approximation based on altitude and direction) estimated_distance = current_altitude * direction_magnitude - self.log(f"Hoop detected. Direction: {direction}, Est. distance: {estimated_distance:.2f}m") + self.log(f"Direction magnitude: {direction_magnitude:.3f}, Normalized: [{direction_normalized[0]:.3f}, {direction_normalized[1]:.3f}, {direction_normalized[2]:.3f}]") + self.log(f"Estimated distance to hoop: {estimated_distance:.2f}m") # Check if hoop is centered (within threshold) # Check x and y components of normalized direction centering_threshold = 0.05 # Adjust as needed - is_centered = (abs(direction_normalized[0]) < centering_threshold and - abs(direction_normalized[1]) < centering_threshold) + x_error = abs(direction_normalized[0]) + y_error = abs(direction_normalized[1]) + is_centered = (x_error < centering_threshold and y_error < centering_threshold) + + self.log(f"Centering errors - X: {x_error:.3f}, Y: {y_error:.3f}, Threshold: {centering_threshold}, Centered: {is_centered}") if is_centered: - self.log("Hoop centered! Moving forward to pass through.") + self.log("✓ Hoop centered! Moving forward to pass through.") # Calculate target position to pass through hoop - current_pos = self.uav.get_local_position() # Move forward in the direction of the hoop forward_distance = self.pass_distance forward_direction = self.uav.uav_to_local((forward_distance, 0, 0)) self.hoop_pass_target = tuple(current_pos[i] + forward_direction[i] for i in range(3)) + self.log(f"Pass target set: ({self.hoop_pass_target[0]:.2f}, {self.hoop_pass_target[1]:.2f}, {self.hoop_pass_target[2]:.2f})") self.passing_through = True else: # Not centered yet - adjust position to center on hoop @@ -141,7 +162,12 @@ def on_update(self, time_delta: float) -> None: # Slow down vertical movement for stability adjusted_direction[2] *= 0.5 - self.log(f"Centering on hoop. Adjusted direction: {adjusted_direction}") + # Calculate target position + target_pos = tuple(current_pos[i] + adjusted_direction[i] for i in range(3)) + distance_to_move = np.linalg.norm(adjusted_direction) + + self.log(f"→ Centering adjustment: [{adjusted_direction[0]:.3f}, {adjusted_direction[1]:.3f}, {adjusted_direction[2]:.3f}] (magnitude: {distance_to_move:.3f}m)") + self.log(f"→ Target position: ({target_pos[0]:.2f}, {target_pos[1]:.2f}, {target_pos[2]:.2f})") self.uav.publish_position_setpoint(adjusted_direction, relative=True) def check_status(self) -> str: diff --git a/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml b/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml index 46ddc139..5389630c 100644 --- a/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml +++ b/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml @@ -2,6 +2,7 @@ start: class: uav.autonomous_modes.NavGPSMode params: coordinates: '[((-5, -10, -10), 1, "LOCAL"),]' + margin: 1.5 transitions: complete: hoop From 37f8899c5221e7b14b1f74e692527be2311627ba Mon Sep 17 00:00:00 2001 From: felicia-19 Date: Wed, 12 Nov 2025 21:07:00 -0500 Subject: [PATCH 12/14] edit coords for switch to hoopmode --- .../sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml b/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml index 5389630c..154b90c9 100644 --- a/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml +++ b/controls/sae_2025_ws/src/uav/uav/missions/in_house_comp_hoop.yaml @@ -1,7 +1,7 @@ start: class: uav.autonomous_modes.NavGPSMode params: - coordinates: '[((-5, -10, -10), 1, "LOCAL"),]' + coordinates: '[((-1, -3, -3), 1, "LOCAL"),]' margin: 1.5 transitions: complete: hoop From 4ce4be6b100bc6e915ccc0a43333e55e6bd6d0bc Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 02:09:21 +0000 Subject: [PATCH 13/14] HUGGEE CHANGE --- .../src/uav/uav/autonomous_modes/HoopNavigationMode.py | 1 + 1 file changed, 1 insertion(+) diff --git a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py index be2f507d..5760e567 100644 --- a/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py +++ b/controls/sae_2025_ws/src/uav/uav/autonomous_modes/HoopNavigationMode.py @@ -3,6 +3,7 @@ from uav.autonomous_modes import Mode from rclpy.node import Node from uav_interfaces.msg import HoopTracking +from uav.vision_nodes import HoopTrackingNode from typing import Optional, Tuple import cv2 From 7c0b26d467679f49e1b0859d8b28155f7b4f2cce Mon Sep 17 00:00:00 2001 From: Ubuntu Date: Thu, 13 Nov 2025 02:15:32 +0000 Subject: [PATCH 14/14] Add logs --- .../uav/uav/vision_nodes/HoopTrackingNode.py | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py b/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py index de59179c..86fba66b 100644 --- a/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py +++ b/controls/sae_2025_ws/src/uav/uav/vision_nodes/HoopTrackingNode.py @@ -18,9 +18,12 @@ class HoopTrackingNode(VisionNode): def __init__(self): super().__init__('hoop_tracking', None) + self.get_logger().info("HoopTrackingNode initializing...") + # Initialize Kalman filter self.kalman = cv2.KalmanFilter(4, 2) self._setup_kalman_filter() + self.get_logger().info("Kalman filter initialized") # Create publisher for hoop tracking data self.tracking_publisher = self.create_publisher( @@ -28,6 +31,7 @@ def __init__(self): '/vision/hoop_tracking', 10 ) + self.get_logger().info("Created publisher on /vision/hoop_tracking") # Subscribe to UAV state self.uav_state_sub = self.create_subscription( @@ -36,12 +40,15 @@ def __init__(self): self.uav_state_callback, 10 ) + self.get_logger().info("Subscribed to /uav_state") self.current_altitude = 0.0 self.current_yaw = 0.0 + self.uav_state_received = False # Create timer to publish tracking data at regular intervals self.timer = self.create_timer(0.1, self.publish_tracking_data) # 10 Hz + self.get_logger().info("HoopTrackingNode fully initialized and ready!") def _setup_kalman_filter(self): """Initialize Kalman filter matrices""" @@ -68,25 +75,37 @@ def _setup_kalman_filter(self): def uav_state_callback(self, msg: UAVState): """Update current UAV state from subscription""" + if not self.uav_state_received: + self.get_logger().info(f"First UAV state received! Altitude: {msg.altitude:.2f}, Yaw: {msg.yaw:.2f}") + self.uav_state_received = True self.current_altitude = msg.altitude self.current_yaw = msg.yaw def publish_tracking_data(self): """Publish hoop tracking data at regular intervals""" + self.get_logger().debug("publish_tracking_data() called") + image, camera_info = self.request_data(cam_image=True, cam_info=True) # Check if we have valid image and camera info if image is None or camera_info is None: + self.get_logger().warn(f"No camera data - image: {image is not None}, camera_info: {camera_info is not None}", throttle_duration_sec=2.0) return + + self.get_logger().debug(f"Got camera data - converting and processing...") image = self.convert_image_msg_to_frame(image) + self.get_logger().debug(f"Image shape: {image.shape}, Altitude: {self.current_altitude:.2f}, Yaw: {np.rad2deg(self.current_yaw):.1f}°") + image = rotate_image(image, -np.rad2deg(self.current_yaw)) # Predict next state prediction = self.kalman.predict() predicted_x, predicted_y = prediction[0, 0], prediction[1, 0] + self.get_logger().debug(f"Kalman prediction: ({predicted_x:.1f}, {predicted_y:.1f})") # Get raw detection + self.get_logger().debug("Running hoop detection...") detection = detect_hoop( image, self.uuid, @@ -97,21 +116,25 @@ def publish_tracking_data(self): hoop_detected = False if detection is not None: cx, cy = detection + self.get_logger().info(f"✓ Hoop detected at pixel ({cx:.1f}, {cy:.1f})") # Update Kalman filter with measurement measurement = np.array([[np.float32(cx)], [np.float32(cy)]]) corrected_state = self.kalman.correct(measurement) x, y = corrected_state[0, 0], corrected_state[1, 0] hoop_detected = True else: + self.get_logger().warn("✗ No hoop detected in frame, using prediction", throttle_duration_sec=1.0) # Use prediction if no detection x, y = predicted_x, predicted_y # Fallback to image center if prediction is invalid if x <= 0 or y <= 0: h, w = image.shape[:2] x, y = w / 2, h / 2 + self.get_logger().debug(f"Invalid prediction, using image center: ({x:.1f}, {y:.1f})") # Compute 3D direction vector direction = compute_3d_vector(x, y, np.array(camera_info.k).reshape(3, 3), self.current_altitude) + self.get_logger().debug(f"3D direction: [{direction[0]:.3f}, {direction[1]:.3f}, {direction[2]:.3f}]") # Create and publish message msg = HoopTracking() @@ -121,17 +144,22 @@ def publish_tracking_data(self): msg.hoop_detected = hoop_detected self.tracking_publisher.publish(msg) + self.get_logger().debug(f"Published tracking message - detected: {hoop_detected}, pos: ({x:.1f}, {y:.1f})") def main(): + print("Starting HoopTrackingNode main()...") rclpy.init() node = HoopTrackingNode() + node.get_logger().info("=== HoopTrackingNode spinning ===") try: rclpy.spin(node) except Exception as e: + node.get_logger().error(f"Exception in HoopTrackingNode: {e}") print(e) node.publish_failsafe() + node.get_logger().info("HoopTrackingNode shutting down") rclpy.shutdown()