User/cryslu/fix/133

controls/sae_2025_ws/scripts/sync-src.sh

@@ -0,0 +1,85 @@
+#!/usr/bin/env bash
+# Syncs ROS2 source packages to the Pi over rsync.
+#
+# Usage:
+#   ./sync-src.sh <user@host>                                  # Deploy default packages
+#   ./sync-src.sh <user@host> --packages-select pkg_a pkg_b   # Deploy specific packages
+#   ./sync-src.sh <user@host> --password                       # Prompt for SSH password
+set -euo pipefail
+
+SCRIPT_DIR=$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)
+SRC_DIR="$SCRIPT_DIR/../src"
+
+# Packages copied in a normal deploy — no sim, no Gazebo, no bridges.
+DEPLOY_PACKAGES=(
+    actuator_msgs
+    payload
+    payload_interfaces
+    px4_msgs
+    uav
+    uav_interfaces
+)
+
+# --- Argument parsing ---
+if [[ $# -eq 0 || "$1" == --* ]]; then
+    echo "Usage: $0 <user@host> [--packages-select pkg...] [--password]" >&2
+    exit 1
+fi
+REMOTE="$1"
+shift
+
+PACKAGES=("${DEPLOY_PACKAGES[@]}")
+SSH_OPTS=()
+
+while [[ $# -gt 0 ]]; do
+    case "$1" in
+        --packages-select)
+            shift
+            PACKAGES=()
+            while [[ $# -gt 0 && "$1" != --* ]]; do
+                PACKAGES+=("$1")
+                shift
+            done
+            ;;
+        --password)
+            if ! command -v sshpass &>/dev/null; then
+                echo "sshpass is not installed. Install it with: sudo apt install sshpass" >&2
+                exit 1
+            fi
+            read -rsp "SSH password for $REMOTE: " SSH_PASSWORD
+            echo
+            SSH_OPTS=(-e "sshpass -p '$SSH_PASSWORD' ssh")
+            shift
+            ;;
+        *)
+            echo "Unknown argument: $1" >&2
+            exit 1
+            ;;
+    esac
+done
+
+if [[ ${#PACKAGES[@]} -eq 0 ]]; then
+    echo "No packages selected." >&2
+    exit 1
+fi
+
+# Build source list, verifying each package exists locally.
+SOURCES=()
+for pkg in "${PACKAGES[@]}"; do
+    pkg_path="$SRC_DIR/$pkg"
+    if [[ ! -d "$pkg_path" ]]; then
+        echo "Package not found: $pkg_path" >&2
+        exit 1
+    fi
+    SOURCES+=("$pkg_path")
+done
+
+echo "Syncing packages: ${PACKAGES[*]}"
+
+rsync -rv \
+    --no-times \
+    --no-perms --no-owner --no-group \
+    --delete \
+    "${SSH_OPTS[@]}" \
+    "${SOURCES[@]}" \
+    "$REMOTE:~/monorepo/controls/sae_2025_ws/src/"

controls/sae_2025_ws/src/uav/uav/ModeManager.py

@@ -101,6 +101,7 @@ def trigger_failsafe(self, request, response):
 
         self.uav.failsafe_trigger = True
         self.uav.failsafe = self.uav.failsafe_px4 or self.uav.failsafe_trigger
+        # self.uav.rtl()
 
         response.success = True
         response.message = "Failsafe triggered."
@@ -243,15 +244,11 @@ def spin_once(self) -> None:
         current_time = time()
         if self.uav.failsafe:
             if not self.uav.emergency_landing:
-                self.uav.hover()
-                self.get_logger().warn("Failsafe: Switching to AUTO_LOITER mode.")
+                self.uav.rtl()
+                self.get_logger().warn("Failsafe: Returning to Launch.")
                 self.uav.emergency_landing = True
-            if (
-                self.uav.nav_state == VehicleStatus.NAVIGATION_STATE_AUTO_LOITER
-                or self.uav.arm_state != VehicleStatus.ARMING_STATE_ARMED
-            ):
-                self.uav.land()  # Initiate the landing procedure.
-                self.get_logger().warn("Failsafe: Initiating landing.")
+                self.destroy_node()
+                rclpy.shutdown()
             return
         if self.servo_only:
             if self.active_mode is None:
@@ -310,7 +307,6 @@ def spin_once(self) -> None:
 
             if self.uav.local_position is None or self.uav.global_position is None:
                 return  # Wait for position data
-
             self.uav.publish_offboard_control_heartbeat_signal()
 
             # Start mission - TakeoffMode handles takeoff, heartbeat, and offboard engagement

controls/sae_2025_ws/src/uav/uav/UAV.py

@@ -148,6 +148,16 @@ def hover(self):
             },
         )
 
+    def rtl(self):
+        self._send_vehicle_command(
+            VehicleCommand.VEHICLE_CMD_DO_SET_MODE,
+            params={
+                "param1": 1.0,
+                "param2": PX4CustomMainMode.AUTO.value,
+                "param3": PX4CustomSubModeAuto.RTL.value,
+            },
+        )
+
     def disarm(self):
         """Send a disarm command to the UAV."""
         self._send_vehicle_command(

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

@@ -51,7 +51,6 @@ def on_enter(self):
 
         # Enable offboard mode
         self.publish_offboard_control_mode()
-
         # Arm the vehicle
         self.arm_vehicle()