Arjun edits

README.md

@@ -149,12 +149,25 @@ offsets, and MAVLink connection in a simulator:
 
 ```bash
 python -m scripts.multi_stage_gate_mission
+python -m scripts.single_gate_mission
 ```
 
 The mission approaches each detected gate at staged distances and then commands
-a pass-through target. Treat the default controller values as development
+a pass-through target. The single-gate mission instead takes 0.25 m
+receding-horizon steps, replans after every step, commits to the pass at 1 m,
+and lands after crossing. Treat the default controller values as development
 examples, not safe settings for an arbitrary vehicle.
 
+To isolate camera-offset effects during multi-stage simulator testing, set the
+three corrections to zero from the command line:
+
+```bash
+python -m scripts.multi_stage_gate_mission \
+  --camera-right-offset-m 0 \
+  --camera-down-offset-m 0 \
+  --camera-yaw-offset-deg 0
+```
+
 ## Model and calibration provenance
 
 The compiled model is intentionally excluded from the repository. Users must

navigation/__init__.py

@@ -13,4 +13,4 @@
     UDP_PORT,
     VehicleState,
 )
-from .missions import MultiStageGateMission
+from .missions import MultiStageGateMission, SingleGateMission

navigation/missions/__init__.py

@@ -1 +1,4 @@
 from .multi_stage_gate import MultiStageGateMission
+from .single_gate import SingleGateMission
+
+__all__ = ["MultiStageGateMission", "SingleGateMission"]

navigation/missions/multi_stage_gate.py

@@ -12,37 +12,37 @@ def run(self, nav: NavigationController):
             print(f"[*] Looking for Gate {gate_count + 1} of 8")
             print("==============================")
 
-            gate = self.observe_gate(nav, duration=1.0)
+            gate = self.observe_gate(nav, duration=3.0)
             if not gate:
                 nav.turn_around_180()
                 continue
 
             target_3m = self.build_standoff_target(nav, gate, standoff_m=3.0)
-            nav.move_to_target(target_3m, "3m Standoff")
+            nav.move_to_target(target_3m, "3m Standoff", max_speed_m_s= 0.15)
 
-            gate = self.observe_gate(nav, duration=1.0)
+            gate = self.observe_gate(nav, duration=6.0)
             if not gate:
                 print("[!] Lost gate at 3m. Restarting.")
                 continue
 
             target_2m = self.build_standoff_target(nav, gate, standoff_m=2.0)
-            nav.move_to_target(target_2m, "2m Standoff")
+            nav.move_to_target(target_2m, "2m Standoff", max_speed_m_s= 0.15)
 
-            gate = self.observe_gate(nav, duration=1.0)
+            gate = self.observe_gate(nav, duration=6.0)
             if not gate:
                 print("[!] Lost gate at 2m. Restarting.")
                 continue
 
             target_1m = self.build_standoff_target(nav, gate, standoff_m=1.0)
-            nav.move_to_target(target_1m, "1m Standoff")
+            nav.move_to_target(target_1m, "1m Standoff", max_speed_m_s= 0.15)
 
-            gate = self.observe_gate(nav, duration=0.5)
+            gate = self.observe_gate(nav, duration=6.0)
             if not gate:
                 print("[!] Lost gate right before pass. Restarting.")
                 continue
 
             pass_target = self.build_pass_through_target(nav, gate, pass_dist_m=1.5)
-            nav.move_to_target(pass_target, "Through The Gate!")
+            nav.move_to_target(pass_target, "Through The Gate!", max_speed_m_s= 0.15)
 
             gate_count += 1
             print(f"[*] Successfully navigated Gate {gate_count}!")

navigation/missions/single_gate.py

@@ -0,0 +1,171 @@
+"""Receding-horizon mission for approaching and crossing one gate."""
+
+import math
+from typing import Tuple
+
+from ..navigation import (
+    GateDetection,
+    GateMission,
+    LocalTarget,
+    NavigationController,
+    VehicleState,
+)
+
+
+def plan_standoff_horizon(
+    distance_m: float,
+    commit_distance_m: float,
+    step_size_m: float,
+    horizon_steps: int,
+) -> Tuple[float, ...]:
+    """Return the next decreasing standoff distances in a short horizon.
+
+    Each planned move is no larger than ``step_size_m`` and the horizon never
+    asks the vehicle to move closer than ``commit_distance_m``.  The mission
+    executes only the first target before measuring the gate again.
+    """
+    if commit_distance_m < 0.0:
+        raise ValueError("commit_distance_m must be non-negative")
+    if step_size_m <= 0.0:
+        raise ValueError("step_size_m must be positive")
+    if horizon_steps <= 0:
+        raise ValueError("horizon_steps must be positive")
+
+    if distance_m <= commit_distance_m:
+        return ()
+
+    standoffs = []
+    predicted_distance = distance_m
+    for _ in range(horizon_steps):
+        predicted_distance = max(commit_distance_m, predicted_distance - step_size_m)
+        standoffs.append(predicted_distance)
+        if predicted_distance <= commit_distance_m:
+            break
+
+    return tuple(standoffs)
+
+
+def limit_target_step(
+    current: VehicleState,
+    target: LocalTarget,
+    max_step_m: float,
+) -> LocalTarget:
+    """Limit an approach target to a maximum 3D displacement."""
+    if max_step_m <= 0.0:
+        raise ValueError("max_step_m must be positive")
+
+    delta_n = target.n - current.n
+    delta_e = target.e - current.e
+    delta_d = target.d - current.d
+    distance = math.sqrt(delta_n**2 + delta_e**2 + delta_d**2)
+
+    if distance <= max_step_m:
+        return target
+
+    scale = max_step_m / distance
+    return LocalTarget(
+        n=current.n + delta_n * scale,
+        e=current.e + delta_e * scale,
+        d=current.d + delta_d * scale,
+        yaw_rad=target.yaw_rad,
+    )
+
+
+class SingleGateMission(GateMission):
+    """Approach one gate with small replanned moves, cross it, and land.
+
+    This is an MPC-style receding-horizon controller rather than a numerical
+    optimizer: it predicts a few safe standoff targets, applies the first one,
+    and uses a new vision observation to rebuild the horizon.
+    """
+
+    def __init__(
+        self,
+        *args,
+        step_size_m: float = 0.25,
+        horizon_steps: int = 2,
+        commit_distance_m: float = 1.0,
+        pass_distance_m: float = 1.5,
+        observation_duration_s: float = 0.5,
+        **kwargs,
+    ):
+        super().__init__(*args, **kwargs)
+
+        # Validate all planner settings at construction time.
+        plan_standoff_horizon(
+            commit_distance_m + step_size_m,
+            commit_distance_m,
+            step_size_m,
+            horizon_steps,
+        )
+        if pass_distance_m <= 0.0:
+            raise ValueError("pass_distance_m must be positive")
+        if observation_duration_s <= 0.0:
+            raise ValueError("observation_duration_s must be positive")
+
+        self.step_size_m = step_size_m
+        self.horizon_steps = horizon_steps
+        self.commit_distance_m = commit_distance_m
+        self.pass_distance_m = pass_distance_m
+        self.observation_duration_s = observation_duration_s
+
+    def _build_horizon(self, gate: GateDetection) -> Tuple[float, ...]:
+        return plan_standoff_horizon(
+            distance_m=gate.dist,
+            commit_distance_m=self.commit_distance_m,
+            step_size_m=self.step_size_m,
+            horizon_steps=self.horizon_steps,
+        )
+
+    def run(self, nav: NavigationController):
+        """Replan until 1 m away, then cross using the last visible gate pose."""
+        print("[*] Starting single-gate receding-horizon approach")
+
+        while nav.running:
+            gate = self.observe_gate(nav, duration=self.observation_duration_s)
+            if gate is None:
+                print("[!] Gate unavailable. Holding position and trying again.")
+                continue
+
+            horizon = self._build_horizon(gate)
+            if horizon:
+                horizon_text = ", ".join(f"{distance:.2f}m" for distance in horizon)
+                print(
+                    f"[*] Gate distance {gate.dist:.2f}m; "
+                    f"planned standoffs: {horizon_text}"
+                )
+
+                # Receding-horizon behavior: execute only the first prediction,
+                # then observe the gate and solve the short plan again.
+                next_standoff = horizon[0]
+                desired_target = self.build_standoff_target(
+                    nav,
+                    gate,
+                    standoff_m=next_standoff,
+                )
+                target = limit_target_step(
+                    current=nav.get_vehicle_snapshot(),
+                    target=desired_target,
+                    max_step_m=self.step_size_m,
+                )
+                nav.move_to_target(target, f"MPC standoff {next_standoff:.2f}m")
+                continue
+
+            # Do not depend on vision after committing to the crossing. The
+            # current detection is the final gate pose used for pass-through.
+            print(
+                f"[*] Within {self.commit_distance_m:.2f}m; "
+                "committing to gate pass"
+            )
+            pass_target = self.build_pass_through_target(
+                nav,
+                gate,
+                pass_dist_m=self.pass_distance_m,
+            )
+            crossed = nav.move_to_target(pass_target, "through the gate")
+            if not crossed:
+                print("[!] Pass-through move timed out; landing at current position.")
+
+            if nav.running:
+                nav.land()
+            return

navigation/navigation.py

@@ -265,12 +265,25 @@ def send_velocity_and_yaw_target(self, vn: float, ve: float, vd: float, yaw_rad:
             0,
         )
 
-    def move_to_target(self, final_target: LocalTarget, label: str) -> bool:
-        """Drive toward a local target using a simple proportional velocity loop."""
+    def move_to_target(
+        self,
+        final_target: LocalTarget,
+        label: str,
+        *,
+        max_speed_m_s: float = MAX_FLIGHT_SPEED_M_S,
+        timeout_s: float = MOVE_TIMEOUT,
+    ) -> bool:
+        """Drive to a local target with per-move speed and timeout limits."""
+        if not math.isfinite(max_speed_m_s) or max_speed_m_s <= 0.0:
+            raise ValueError("max_speed_m_s must be a positive finite number")
+        if not math.isfinite(timeout_s) or timeout_s <= 0.0:
+            raise ValueError("timeout_s must be a positive finite number")
+
         print(
             f"[*] Moving to {label}: "
             f"N={final_target.n:.2f}, E={final_target.e:.2f}, "
-            f"D={final_target.d:.2f}, Yaw={rad_to_deg(final_target.yaw_rad):.1f} deg"
+            f"D={final_target.d:.2f}, Yaw={rad_to_deg(final_target.yaw_rad):.1f} deg, "
+            f"MaxSpeed={max_speed_m_s:.2f}m/s"
         )
 
         start_time = time.time()
@@ -289,7 +302,7 @@ def move_to_target(self, final_target: LocalTarget, label: str) -> bool:
                 self.send_velocity_and_yaw_target(0.0, 0.0, 0.0, final_target.yaw_rad)
                 return True
 
-            if time.time() - start_time > MOVE_TIMEOUT:
+            if time.time() - start_time > timeout_s:
                 print(f"[!] Timeout moving to {label}. Proceeding anyway.")
                 self.send_velocity_and_yaw_target(0.0, 0.0, 0.0, final_target.yaw_rad)
                 return False
@@ -299,10 +312,10 @@ def move_to_target(self, final_target: LocalTarget, label: str) -> bool:
             vd = KP_POS * err_d
 
             cmd_speed = math.sqrt(vn**2 + ve**2 + vd**2)
-            if cmd_speed > MAX_FLIGHT_SPEED_M_S:
-                vn = (vn / cmd_speed) * MAX_FLIGHT_SPEED_M_S
-                ve = (ve / cmd_speed) * MAX_FLIGHT_SPEED_M_S
-                vd = (vd / cmd_speed) * MAX_FLIGHT_SPEED_M_S
+            if cmd_speed > max_speed_m_s:
+                vn = (vn / cmd_speed) * max_speed_m_s
+                ve = (ve / cmd_speed) * max_speed_m_s
+                vd = (vd / cmd_speed) * max_speed_m_s
 
             self.send_velocity_and_yaw_target(vn, ve, vd, final_target.yaw_rad)
             time.sleep(period)
@@ -364,9 +377,16 @@ def __init__(
         self,
         udp_ip: str = UDP_IP,
         udp_port: int = UDP_PORT,
+        *,
+        cam_offset_right_m: float = CAM_OFFSET_RIGHT_M,
+        cam_offset_down_m: float = CAM_OFFSET_DOWN_M,
+        cam_yaw_offset_deg: float = CAM_YAW_OFFSET_DEG,
     ):
         self.udp_ip = udp_ip
         self.udp_port = udp_port
+        self.cam_offset_right_m = cam_offset_right_m
+        self.cam_offset_down_m = cam_offset_down_m
+        self.cam_yaw_offset_deg = cam_yaw_offset_deg
 
         self._running = threading.Event()
         self._latest_detection: Optional[GateDetection] = None
@@ -487,15 +507,15 @@ def observe_gate(self, nav: NavigationController, duration: float = 1.0) -> Opti
 
     def detection_to_gate_local(self, det: GateDetection, state: VehicleState):
         """Convert a camera-relative gate detection into local NED gate pose."""
-        corrected_right = det.right + CAM_OFFSET_RIGHT_M
-        corrected_down = det.down + CAM_OFFSET_DOWN_M
+        corrected_right = det.right + self.cam_offset_right_m
+        corrected_down = det.down + self.cam_offset_down_m
 
         dn, de, dd = body_to_local(det.forward, corrected_right, corrected_down, state.yaw_rad)
         gate_n = state.n + dn
         gate_e = state.e + de
         gate_d = state.d + dd
 
-        corrected_yaw_deg = det.yaw_deg + CAM_YAW_OFFSET_DEG
+        corrected_yaw_deg = det.yaw_deg + self.cam_yaw_offset_deg
         gate_yaw = wrap_pi(state.yaw_rad + deg_to_rad(corrected_yaw_deg))
 
         return gate_n, gate_e, gate_d, gate_yaw

scripts/multi_stage_gate_mission.py

@@ -1,11 +1,46 @@
-import sys
+import argparse
 
-from navigation import MAVLINK_CONN, MultiStageGateMission, NavigationController
+from navigation import (
+    CAM_OFFSET_DOWN_M,
+    CAM_OFFSET_RIGHT_M,
+    CAM_YAW_OFFSET_DEG,
+    MAVLINK_CONN,
+    MultiStageGateMission,
+    NavigationController,
+)
+
+
+def parse_args():
+    parser = argparse.ArgumentParser(description="Run the multi-stage gate mission")
+    parser.add_argument(
+        "--camera-right-offset-m",
+        type=float,
+        default=CAM_OFFSET_RIGHT_M,
+        help=f"camera right offset in meters (default: {CAM_OFFSET_RIGHT_M})",
+    )
+    parser.add_argument(
+        "--camera-down-offset-m",
+        type=float,
+        default=CAM_OFFSET_DOWN_M,
+        help=f"camera down offset in meters (default: {CAM_OFFSET_DOWN_M})",
+    )
+    parser.add_argument(
+        "--camera-yaw-offset-deg",
+        type=float,
+        default=CAM_YAW_OFFSET_DEG,
+        help=f"camera yaw offset in degrees (default: {CAM_YAW_OFFSET_DEG})",
+    )
+    return parser.parse_args()
 
 
 if __name__ == "__main__":
+    args = parse_args()
     nav = NavigationController(MAVLINK_CONN)
-    mission = MultiStageGateMission()
+    mission = MultiStageGateMission(
+        cam_offset_right_m=args.camera_right_offset_m,
+        cam_offset_down_m=args.camera_down_offset_m,
+        cam_yaw_offset_deg=args.camera_yaw_offset_deg,
+    )
     try:
         print("[*] Starting Multi-Stage Gate Mission")
         nav.run_mission(mission)