Initial Commit

config/ardupilot_gps_denied.parm

@@ -0,0 +1,98 @@
+# ArduPilot Parameters for GPS-Denied Navigation
+# GPS ONLY used for geofencing, NOT navigation
+
+# ====================
+# GPS Configuration
+# ====================
+# Disable GPS for navigation
+GPS_TYPE 0
+GPS_TYPE2 0
+
+# ====================
+# Geofence Settings
+# ====================
+# Enable geofence (still uses GPS for boundaries)
+FENCE_ENABLE 1
+FENCE_TYPE 7          # All fence types (circle + polygon + altitude)
+FENCE_ACTION 1        # RTL on breach
+FENCE_ALT_MAX 50      # Maximum altitude (meters)
+FENCE_RADIUS 100      # Circular fence radius (meters)
+FENCE_MARGIN 2        # Margin inside fence for warning
+
+# ====================
+# EKF Configuration
+# ====================
+# Use EKF3 with external navigation
+AHRS_EKF_TYPE 3
+EK3_ENABLE 1
+EK2_ENABLE 0
+
+# EKF3 Source Configuration
+# Source 1: External Nav (Visual Odometry)
+EK3_SRC1_POSXY 6      # External nav for XY position
+EK3_SRC1_POSZ 1       # Barometer for Z position
+EK3_SRC1_VELXY 6      # External nav for XY velocity
+EK3_SRC1_VELZ 0       # None for Z velocity
+EK3_SRC1_YAW 6        # External nav for yaw
+
+# EKF3 Position Innovation Gate
+EK3_POS_I_GATE 300    # Larger gate for visual odometry
+
+# ====================
+# Vision Position Input
+# ====================
+VISO_TYPE 1           # MAVLink vision position
+VISO_POS_X 0.1        # Camera X offset from CG (meters)
+VISO_POS_Y 0.0        # Camera Y offset from CG (meters)
+VISO_POS_Z -0.05      # Camera Z offset from CG (meters)
+VISO_ORIENT 0         # Camera orientation (0 = forward)
+VISO_DELAY_MS 50      # Vision position delay (ms)
+
+# ====================
+# Optical Flow
+# ====================
+FLOW_TYPE 1           # Enable optical flow
+FLOW_FXSCALER 200     # X scale factor
+FLOW_FYSCALER 200     # Y scale factor
+FLOW_POS_X 0.0        # Flow sensor X offset
+FLOW_POS_Y 0.0        # Flow sensor Y offset
+FLOW_POS_Z 0.0        # Flow sensor Z offset
+
+# ====================
+# Rangefinder (for altitude in GPS-denied)
+# ====================
+RNGFND1_TYPE 1        # Analog rangefinder
+RNGFND1_MIN_CM 10     # Minimum range (cm)
+RNGFND1_MAX_CM 1000   # Maximum range (cm)
+RNGFND1_ORIENT 25     # Downward orientation
+
+# ====================
+# Failsafe Settings
+# ====================
+FS_EKF_ACTION 1       # Land on EKF failsafe
+FS_EKF_THRESH 0.8     # EKF failsafe threshold
+FS_VIBE_ENABLE 0      # Disable vibration failsafe (optical flow sensitive)
+
+# ====================
+# Flight Modes
+# ====================
+# Disable GPS-dependent modes
+# Allowed: STABILIZE, ALT_HOLD, LOITER (with optical flow), GUIDED
+FLTMODE1 0            # Stabilize
+FLTMODE2 2            # Alt Hold
+FLTMODE3 5            # Loiter (optical flow based)
+FLTMODE4 4            # Guided
+FLTMODE5 6            # RTL (returns to local origin)
+FLTMODE6 9            # Land
+
+# ====================
+# Arming Checks
+# ====================
+# Relax arming checks for GPS-denied operation
+ARMING_CHECK 14      # Skip GPS check (bit 2 = 4)
+
+# ====================
+# Logging
+# ====================
+LOG_BITMASK 176126    # Log all relevant data
+LOG_DISARMED 0        # Don't log when disarmed

config/geofence_params.yaml

@@ -0,0 +1,76 @@
+# Geofence Configuration
+# GPS is ONLY used for safety boundaries, NOT navigation
+
+/**:
+  ros__parameters:
+    
+    geofence:
+      enabled: true
+      
+      # Fence type
+      type: "polygon"  # Options: "polygon", "circle", "cylinder"
+      
+      # Polygon fence (GPS coordinates for safety boundary)
+      polygon_fence:
+        points:
+          - latitude: 47.397742
+            longitude: 8.545594
+          - latitude: 47.398242
+            longitude: 8.545594
+          - latitude: 47.398242
+            longitude: 8.546094
+          - latitude: 47.397742
+            longitude: 8.546094
+        
+        # Close the polygon automatically
+        auto_close: true
+      
+      # Circular fence (alternative to polygon)
+      circle_fence:
+        center_latitude: 47.397742
+        center_longitude: 8.545594
+        radius_meters: 100
+        
+      # Cylinder fence (circle + altitude limits)
+      cylinder_fence:
+        center_latitude: 47.397742
+        center_longitude: 8.545594
+        radius_meters: 100
+        min_altitude: 0  # meters AGL
+        max_altitude: 50  # meters AGL
+      
+      # Altitude limits (applies to all fence types)
+      altitude_limits:
+        min_altitude: 0.0  # meters AGL
+        max_altitude: 50.0  # meters AGL
+        enable_floor: true
+        enable_ceiling: true
+      
+      # Fence actions
+      actions:
+        on_breach: "RTL"  # Options: "RTL", "LAND", "HOLD", "ALERT_ONLY"
+        warning_distance: 10.0  # meters from fence
+        warning_action: "ALERT"  # Options: "ALERT", "SLOW_DOWN"
+        
+      # Monitoring
+      check_rate: 10  # Hz
+      consecutive_breaches_required: 3  # Require 3 consecutive breaches before action
+      
+      # Visualization
+      publish_markers: true
+      marker_topic: "/geofence/markers"
+      marker_color:
+        safe: {r: 0.0, g: 1.0, b: 0.0, a: 0.5}
+        warning: {r: 1.0, g: 1.0, b: 0.0, a: 0.7}
+        breach: {r: 1.0, g: 0.0, b: 0.0, a: 0.9}
+      
+      # Multi-vehicle support
+      vehicles:
+        uav:
+          enabled: true
+          namespace: "/uav"
+          gps_topic: "/uav/mavros/global_position/global"
+        ugv:
+          enabled: true
+          namespace: "/ugv"
+          gps_topic: "/ugv/gps/fix"

config/mavros_params.yaml

@@ -0,0 +1,72 @@
+# MAVROS Configuration for GPS-Denied Navigation
+# GPS is DISABLED for navigation, ENABLED only for geofencing
+
+/**:
+  ros__parameters:
+    use_sim_time: true
+    
+    # Connection
+    fcu_protocol: "v2.0"
+    
+    # System
+    system_id: 1
+    component_id: 1
+    
+    # Time sync
+    time:
+      time_ref_source: "fcu"
+      timesync_mode: MAVLINK
+      timesync_avg_alpha: 0.6
+    
+    # GPS Settings - ONLY for geofencing
+    gps:
+      # Disable GPS for navigation
+      use_gps_for_navigation: false
+      
+      # Enable GPS only for geofence
+      enable_gps_geofence: true
+    
+    # Local position (PRIMARY navigation source)
+    local_position:
+      frame_id: "map"
+      tf:
+        send: true
+        frame_id: "map"
+        child_frame_id: "base_link"
+      
+      # Use local odometry, NOT GPS
+      use_vision: true  # Will use our visual odometry
+    
+    # Vision pose input (from visual odometry)
+    vision_pose:
+      tf:
+        listen: false
+        frame_id: "odom"
+        child_frame_id: "base_link"
+    
+    # Vision speed input (from optical flow)
+    vision_speed:
+      listen_twist: true
+    
+    # Setpoint configuration
+    setpoint_raw:
+      thrust_scaling: 1.0
+    
+    setpoint_position:
+      tf:
+        listen: false
+      # Use LOCAL_NED frame (relative positioning)
+      mav_frame: 1  # MAV_FRAME_LOCAL_NED
+      
+    setpoint_velocity:
+      mav_frame: 8  # MAV_FRAME_BODY_NED (body-relative)
+    
+    # Disable GPS-based modes
+    # Only allow: MANUAL, STABILIZE, ALT_HOLD, GUIDED (local), LOITER (local)
+    allowed_modes:
+      - "MANUAL"
+      - "STABILIZE"
+      - "ALT_HOLD"
+      - "GUIDED"
+      - "LOITER"
+      - "RTL"  # Returns to LOCAL origin, not GPS home

config/simulation_config.yaml

@@ -0,0 +1,86 @@
+# General Simulation Configuration
+
+/**:
+  ros__parameters:
+    
+    simulation:
+      # Use Gazebo ground truth for "perfect" position estimation
+      # In real world, this would come from visual odometry
+      use_gazebo_ground_truth: true  # Set false for realistic VO errors
+      
+      # Add noise to simulate real sensors
+      add_sensor_noise: true
+      
+      # Noise parameters
+      noise:
+        position_stddev: 0.05  # meters
+        velocity_stddev: 0.1  # m/s
+        imu_gyro_stddev: 0.0002  # rad/s
+        imu_accel_stddev: 0.017  # m/s^2
+        
+      # GPS simulation (for geofence only)
+      gps:
+        enabled: true  # Only for geofencing
+        update_rate: 5  # Hz
+        noise_stddev: 2.0  # meters (realistic GPS error)
+        
+      # Real-time factor (1.0 = real-time)
+      real_time_factor: 1.0
+      
+      # Physics settings
+      physics:
+        engine: "ode"
+        max_step_size: 0.001
+        real_time_update_rate: 1000
+        
+    # World configuration
+    world:
+      name: "empty_custom"
+      gravity: 9.81
+      
+      # Lighting (affects vision)
+      lighting:
+        ambient_level: 0.4
+        shadows: true
+        sun_angle: 45.0  # degrees
+        
+      # Ground plane
+      ground:
+        size: [100.0, 100.0]  # meters
+        texture: "ground_plane"
+        friction: 1.0
+        
+    # Initial positions (LOCAL coordinates)
+    initial_positions:
+      uav:
+        x: 0.0
+        y: 0.0
+        z: 0.2  # Start slightly above ground
+        roll: 0.0
+        pitch: 0.0
+        yaw: 0.0
+        
+      ugv:
+        x: 5.0
+        y: 0.0
+        z: 0.0
+        roll: 0.0
+        pitch: 0.0
+        yaw: 1.57  # 90 degrees (facing +Y)
+        
+    # Visualization
+    visualization:
+      show_trajectory: true
+      trajectory_length: 1000  # points
+      show_local_frame: true
+      show_velocity_vector: true
+      
+    # Data logging
+    logging:
+      enabled: true
+      log_directory: "~/.ros/uav_ugv_logs"
+      log_topics:
+        - "/uav/visual_odometry/pose"
+        - "/uav/mavros/local_position/pose"
+        - "/geofence/status"
+      bag_recording: false

config/uav_params.yaml

@@ -0,0 +1,105 @@
+# UAV Configuration - GPS-Denied Navigation
+/**:
+  ros__parameters:
+    
+    # Vision parameters for navigation
+    vision:
+      # Forward camera (for visual odometry)
+      forward_camera:
+        enabled: true
+        topic: "/uav/camera/forward/image_raw"
+        info_topic: "/uav/camera/forward/camera_info"
+        frame_rate: 30
+        
+      # Downward camera (for optical flow)
+      downward_camera:
+        enabled: true
+        topic: "/uav/camera/downward/image_raw"
+        info_topic: "/uav/camera/downward/camera_info"
+        frame_rate: 30
+        
+      # Visual odometry settings
+      visual_odometry:
+        enabled: true
+        method: "ORB"  # Options: ORB, SIFT, SURF
+        min_features: 100
+        max_features: 500
+        feature_quality: 0.01
+        min_distance: 10
+        
+      # Optical flow settings
+      optical_flow:
+        enabled: true
+        method: "Lucas-Kanade"
+        window_size: 15
+        max_level: 3
+        min_altitude: 0.3  # meters
+        max_altitude: 10.0  # meters
+        
+      # Landmark detection
+      landmarks:
+        enabled: true
+        detection_method: "ArUco"  # Options: ArUco, AprilTag, ORB
+        marker_size: 0.15  # meters
+        
+    # Position estimation (sensor fusion)
+    position_estimator:
+      fusion_method: "EKF"  # Extended Kalman Filter
+      
+      # Sensor weights (trust levels)
+      weights:
+        visual_odometry: 0.6
+        optical_flow: 0.3
+        imu: 0.1
+        
+      # Update rates
+      update_rate: 50  # Hz
+      
+      # Covariance matrices
+      process_noise:
+        position: 0.1
+        velocity: 0.5
+        
+      measurement_noise:
+        visual_odom: 0.05
+        optical_flow: 0.1
+        imu: 0.2
+    
+    # Navigation parameters (RELATIVE coordinates only)
+    navigation:
+      frame: "LOCAL_NED"  # Never use GPS frame
+      
+      # Control parameters
+      max_velocity: 2.0  # m/s
+      max_acceleration: 1.0  # m/s^2
+      max_climb_rate: 1.0  # m/s
+      
+      # Waypoint following
+      waypoint_radius: 0.5  # meters
+      position_hold_radius: 0.2  # meters
+      
+      # Obstacle avoidance (vision-based)
+      obstacle_avoidance:
+        enabled: true
+        detection_range: 5.0  # meters
+        safety_margin: 1.0  # meters
+    
+    # Mission parameters
+    mission:
+      takeoff_altitude: 5.0  # meters (relative)
+      loiter_radius: 2.0  # meters
+      rtl_altitude: 10.0  # meters (relative)
+      
+      # Home position is LOCAL (0,0,0), not GPS
+      home_mode: "local"  # Never "gps"
+    
+    # Safety (geofencing uses GPS, but navigation doesn't)
+    safety:
+      geofence:
+        enabled: true
+        use_gps: true  # ONLY for geofence
+        action_on_breach: "RTL"  # Return to LOCAL origin
+        
+      failsafe:
+        vision_loss_timeout: 5.0  # seconds
+        action_on_vision_loss: "HOLD"  # or "RTL" or "LAND"

config/ugv_params.yaml

@@ -0,0 +1,110 @@
+# UGV Configuration - GPS-Denied Navigation
+/**:
+  ros__parameters:
+    
+    # Vehicle physical parameters
+    vehicle:
+      wheelbase: 0.3  # meters
+      track_width: 0.25  # meters
+      wheel_radius: 0.05  # meters
+      max_speed: 1.0  # m/s
+      max_turn_rate: 1.5  # rad/s
+    
+    # Vision parameters for navigation
+    vision:
+      # Forward camera (for visual odometry and obstacle detection)
+      forward_camera:
+        enabled: true
+        topic: "/ugv/camera/forward/image_raw"
+        info_topic: "/ugv/camera/forward/camera_info"
+        frame_rate: 30
+        fov_horizontal: 90  # degrees
+        fov_vertical: 60  # degrees
+        
+      # Visual odometry settings
+      visual_odometry:
+        enabled: true
+        method: "ORB"  # Options: ORB, SIFT
+        min_features: 150
+        max_features: 600
+        feature_quality: 0.02
+        min_distance: 8
+        
+      # Wheel odometry (primary odometry for ground vehicle)
+      wheel_odometry:
+        enabled: true
+        topic: "/ugv/odom"
+        covariance_scale: 0.05
+        
+      # Obstacle detection
+      obstacle_detection:
+        enabled: true
+        method: "depth_camera"  # or "stereo"
+        min_distance: 0.3  # meters
+        max_distance: 10.0  # meters
+        
+    # Position estimation (sensor fusion)
+    position_estimator:
+      fusion_method: "EKF"
+      
+      # Sensor weights (trust levels)
+      weights:
+        visual_odometry: 0.3
+        wheel_odometry: 0.5
+        imu: 0.2
+        
+      # Update rates
+      update_rate: 50  # Hz
+      
+      # Covariance matrices
+      process_noise:
+        position: 0.05
+        velocity: 0.2
+        heading: 0.1
+        
+      measurement_noise:
+        visual_odom: 0.08
+        wheel_odom: 0.03
+        imu: 0.15
+    
+    # Navigation parameters (RELATIVE coordinates only)
+    navigation:
+      frame: "LOCAL_NED"  # Never use GPS frame
+      
+      # Control parameters
+      max_linear_velocity: 1.0  # m/s
+      max_angular_velocity: 1.5  # rad/s
+      linear_acceleration: 0.5  # m/s^2
+      angular_acceleration: 1.0  # rad/s^2
+      
+      # Path following
+      lookahead_distance: 0.5  # meters
+      waypoint_radius: 0.3  # meters
+      
+      # Obstacle avoidance
+      obstacle_avoidance:
+        enabled: true
+        detection_range: 3.0  # meters
+        safety_margin: 0.5  # meters
+        avoidance_method: "VFH"  # Vector Field Histogram
+    
+    # Mission parameters
+    mission:
+      default_speed: 0.5  # m/s
+      turn_in_place_threshold: 0.5  # radians
+      
+      # Home position is LOCAL (0,0,0), not GPS
+      home_mode: "local"
+    
+    # Safety
+    safety:
+      geofence:
+        enabled: true
+        use_gps: true  # ONLY for geofence
+        action_on_breach: "STOP"
+        
+      failsafe:
+        vision_loss_timeout: 3.0  # seconds
+        action_on_vision_loss: "STOP"
+        collision_distance: 0.2  # meters
+        action_on_collision: "STOP"