From a756be4bf7d68d85e24755370a29fc27f71fe0dc Mon Sep 17 00:00:00 2001 From: default Date: Mon, 9 Feb 2026 03:39:49 +0000 Subject: [PATCH] Initial Commit --- README.md | 218 ++++++++++++++++++++++ activate_venv.sh | 11 ++ config/ardupilot_gps_denied.parm | 98 ++++++++++ config/geofence_params.yaml | 76 ++++++++ config/mavros_params.yaml | 72 +++++++ config/simulation_config.yaml | 86 +++++++++ config/uav_params.yaml | 105 +++++++++++ config/ugv_params.yaml | 110 +++++++++++ docs/architecture.md | 126 +++++++++++++ docs/gps_denied_navigation.md | 198 ++++++++++++++++++++ docs/setup_guide.md | 144 ++++++++++++++ docs/troubleshooting.md | 178 ++++++++++++++++++ docs/usage.md | 151 +++++++++++++++ launch/full_simulation.launch.py | 169 +++++++++++++++++ launch/uav_only.launch.py | 97 ++++++++++ launch/ugv_only.launch.py | 63 +++++++ launch/utils/launch_helpers.py | 21 +++ models/custom_ugv/model.config | 12 ++ models/custom_ugv/model.sdf | 159 ++++++++++++++++ models/iris_with_camera/model.config | 13 ++ models/iris_with_camera/model.sdf | 128 +++++++++++++ models/iris_with_sensors/model.config | 12 ++ models/iris_with_sensors/model.sdf | 88 +++++++++ package.xml | 49 +++++ requirements.txt | 58 ++++++ resource/uav_ugv_simulation | 0 scripts/kill_simulation.sh | 19 ++ scripts/run_simulation.sh | 53 ++++++ scripts/setup_gazebo_nvidia.sh | 25 +++ setup.py | 77 ++++++++ setup.sh | 93 +++++++++ src/__init__.py | 4 + src/control/__init__.py | 1 + src/control/mission_planner.py | 236 +++++++++++++++++++++++ src/control/uav_controller.py | 249 +++++++++++++++++++++++++ src/control/ugv_controller.py | 198 ++++++++++++++++++++ src/localization/__init__.py | 1 + src/localization/ekf_fusion.py | 202 ++++++++++++++++++++ src/localization/landmark_tracker.py | 165 ++++++++++++++++ src/localization/position_estimator.py | 188 +++++++++++++++++++ src/navigation/__init__.py | 1 + src/navigation/local_planner.py | 149 +++++++++++++++ src/navigation/obstacle_avoidance.py | 160 ++++++++++++++++ src/navigation/waypoint_follower.py | 183 ++++++++++++++++++ src/nodes/__init__.py | 1 + src/nodes/geofence_node.py | 7 + src/nodes/multi_vehicle_coord.py | 112 +++++++++++ src/nodes/vision_nav_node.py | 134 +++++++++++++ src/nodes/visual_odom_node.py | 7 + src/safety/__init__.py | 1 + src/safety/failsafe_handler.py | 154 +++++++++++++++ src/safety/geofence_monitor.py | 162 ++++++++++++++++ src/utils/__init__.py | 1 + src/utils/helpers.py | 111 +++++++++++ src/utils/transforms.py | 84 +++++++++ src/vision/__init__.py | 1 + src/vision/camera_processor.py | 227 ++++++++++++++++++++++ src/vision/object_detector.py | 132 +++++++++++++ src/vision/optical_flow.py | 128 +++++++++++++ src/vision/visual_odometry.py | 173 +++++++++++++++++ src/vision/visual_servoing.py | 135 ++++++++++++++ tests/__init__.py | 1 + tests/test_camera.py | 35 ++++ tests/test_installation.py | 47 +++++ tests/test_position_estimator.py | 71 +++++++ tests/test_uav_control.py | 40 ++++ tests/test_ugv_control.py | 41 ++++ tests/test_visual_odometry.py | 63 +++++++ worlds/empty_custom.world | 85 +++++++++ worlds/indoor_warehouse.world | 189 +++++++++++++++++++ worlds/urban_canyon.world | 117 ++++++++++++ 71 files changed, 6705 insertions(+) create mode 100644 README.md create mode 100755 activate_venv.sh create mode 100644 config/ardupilot_gps_denied.parm create mode 100644 config/geofence_params.yaml create mode 100644 config/mavros_params.yaml create mode 100644 config/simulation_config.yaml create mode 100644 config/uav_params.yaml create mode 100644 config/ugv_params.yaml create mode 100644 docs/architecture.md create mode 100644 docs/gps_denied_navigation.md create mode 100644 docs/setup_guide.md create mode 100644 docs/troubleshooting.md create mode 100644 docs/usage.md create mode 100644 launch/full_simulation.launch.py create mode 100644 launch/uav_only.launch.py create mode 100644 launch/ugv_only.launch.py create mode 100644 launch/utils/launch_helpers.py create mode 100644 models/custom_ugv/model.config create mode 100644 models/custom_ugv/model.sdf create mode 100644 models/iris_with_camera/model.config create mode 100644 models/iris_with_camera/model.sdf create mode 100644 models/iris_with_sensors/model.config create mode 100644 models/iris_with_sensors/model.sdf create mode 100644 package.xml create mode 100644 requirements.txt create mode 100644 resource/uav_ugv_simulation create mode 100755 scripts/kill_simulation.sh create mode 100755 scripts/run_simulation.sh create mode 100755 scripts/setup_gazebo_nvidia.sh create mode 100644 setup.py create mode 100755 setup.sh create mode 100644 src/__init__.py create mode 100644 src/control/__init__.py create mode 100644 src/control/mission_planner.py create mode 100644 src/control/uav_controller.py create mode 100644 src/control/ugv_controller.py create mode 100644 src/localization/__init__.py create mode 100644 src/localization/ekf_fusion.py create mode 100644 src/localization/landmark_tracker.py create mode 100644 src/localization/position_estimator.py create mode 100644 src/navigation/__init__.py create mode 100644 src/navigation/local_planner.py create mode 100644 src/navigation/obstacle_avoidance.py create mode 100644 src/navigation/waypoint_follower.py create mode 100644 src/nodes/__init__.py create mode 100644 src/nodes/geofence_node.py create mode 100644 src/nodes/multi_vehicle_coord.py create mode 100644 src/nodes/vision_nav_node.py create mode 100644 src/nodes/visual_odom_node.py create mode 100644 src/safety/__init__.py create mode 100644 src/safety/failsafe_handler.py create mode 100644 src/safety/geofence_monitor.py create mode 100644 src/utils/__init__.py create mode 100644 src/utils/helpers.py create mode 100644 src/utils/transforms.py create mode 100644 src/vision/__init__.py create mode 100644 src/vision/camera_processor.py create mode 100644 src/vision/object_detector.py create mode 100644 src/vision/optical_flow.py create mode 100644 src/vision/visual_odometry.py create mode 100644 src/vision/visual_servoing.py create mode 100644 tests/__init__.py create mode 100644 tests/test_camera.py create mode 100644 tests/test_installation.py create mode 100644 tests/test_position_estimator.py create mode 100644 tests/test_uav_control.py create mode 100644 tests/test_ugv_control.py create mode 100644 tests/test_visual_odometry.py create mode 100644 worlds/empty_custom.world create mode 100644 worlds/indoor_warehouse.world create mode 100644 worlds/urban_canyon.world diff --git a/README.md b/README.md new file mode 100644 index 0000000..44eb496 --- /dev/null +++ b/README.md @@ -0,0 +1,218 @@ +# UAV-UGV Gazebo SITL Simulation +## GPS-Denied Navigation with Geofencing + +A complete simulation environment for UAV (drone) and UGV (ground vehicle) development using **GPS-denied navigation** with vision-based localization, while maintaining GPS-based geofencing for safety. + +## šŸŽÆ Key Feature: GPS-Denied Navigation + +**Navigation Mode**: All vehicles navigate using **relative positioning** only: +- āœ… Visual odometry from cameras +- āœ… Optical flow sensors +- āœ… IMU integration +- āœ… Visual landmark tracking +- āœ… Local coordinate frames + +**GPS Usage**: GPS is ONLY used for: +- āœ… Geofencing (safety boundaries) +- āœ… Initial position reference (optional) +- āŒ NOT used for waypoint navigation +- āŒ NOT used for position control + +This simulates real-world GPS-denied environments like: +- Indoor spaces +- Urban canyons +- GPS-jammed areas +- Under bridges/tunnels + +## Features + +- šŸš **Iris quadcopter** with dual cameras (forward + downward) +- šŸš— **Ground vehicle (UGV)** with vision sensors +- šŸ“· **Visual odometry** - camera-based position estimation +- šŸ‘ļø **Optical flow** - velocity estimation from downward camera +- šŸ—ŗļø **Landmark navigation** - visual feature tracking +- šŸ›”ļø **GPS geofencing** - safety boundaries only +- šŸŽ® **Single command launch** - everything runs from one script +- šŸ–„ļø **NVIDIA GPU acceleration** support +- šŸ **Python virtual environment** for isolated dependencies +- šŸŒ **GPS-denied worlds** - indoor and urban environments + +## System Requirements + +- Ubuntu 22.04 LTS +- Python 3.10 +- ROS 2 Humble +- 8GB RAM minimum (16GB recommended) +- NVIDIA GPU recommended + +## Quick Start + +```bash +# 1. Clone repository +cd ~/ros2_ws/src +git clone uav_ugv_simulation +cd uav_ugv_simulation + +# 2. Run setup (installs everything) +bash setup.sh + +# 3. Restart terminal or reload bash +source ~/.bashrc + +# 4. Activate virtual environment and run +source activate_venv.sh +bash scripts/run_simulation.sh +``` + +## GPS-Denied Navigation Architecture + +``` +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ Vision Sensors ā”‚ +ā”‚ Forward Camera + Downward Camera + Optical Flow ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ Visual Odometry & Feature Tracking ā”‚ +ā”‚ Estimates relative position from camera motion ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ Position Estimator (EKF Fusion) ā”‚ +ā”‚ Fuses: Visual Odom + Optical Flow + IMU ā”‚ +ā”‚ Output: Local position estimate (relative) ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ Navigation Controller ā”‚ +ā”‚ Commands: "Move 5m forward, 3m right" ā”‚ +ā”‚ (Relative coordinates only, NO GPS waypoints) ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + + SEPARATE SAFETY LAYER (GPS-based): +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ Geofence Monitor ā”‚ +ā”‚ GPS position ā†’ Check against boundaries ā”‚ +ā”‚ If outside: Emergency RTL or hold ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ +``` + +## Navigation Modes + +### 1. **Vision-Only Mode** (Default) +- Uses camera for all position estimates +- Suitable for structured environments +- Requires good lighting and visual features + +### 2. **Optical Flow Mode** +- Uses downward camera for velocity +- Works well at low altitudes +- Good for hovering and slow flight + +### 3. **Hybrid Mode** +- Combines visual odometry + optical flow + IMU +- Most robust approach +- Recommended for complex missions + +## Geofencing Configuration + +Edit `config/geofence_params.yaml`: + +```yaml +geofence: + enabled: true + use_gps: true # GPS ONLY for geofence + + # Define boundaries (GPS coordinates) + fence_type: "polygon" # or "circle" + + # Polygon fence (lat/lon points) + polygon_points: + - {lat: 47.397742, lon: 8.545594} # Point 1 + - {lat: 47.398242, lon: 8.545594} # Point 2 + - {lat: 47.398242, lon: 8.546094} # Point 3 + - {lat: 47.397742, lon: 8.546094} # Point 4 + + # Or circle fence + center_lat: 47.397742 + center_lon: 8.545594 + radius_meters: 100 + + # Actions on breach + action: "RTL" # Return to launch + max_altitude: 50 # meters +``` + +## Example Mission (Relative Coordinates) + +```python +# Example: Navigate to visual landmark + +# Define mission in RELATIVE coordinates +mission_waypoints = [ + {"x": 0, "y": 0, "z": 5}, # Takeoff to 5m + {"x": 10, "y": 0, "z": 5}, # Move 10m forward + {"x": 10, "y": 5, "z": 5}, # Move 5m right + {"x": 0, "y": 5, "z": 5}, # Return to start (offset) + {"x": 0, "y": 0, "z": 5}, # Back to takeoff point + {"x": 0, "y": 0, "z": 0}, # Land +] + +# GPS is NEVER used for these waypoints +# Position estimated from visual odometry +``` + +## Project Structure + +- `launch/` - ROS 2 launch files +- `worlds/` - Gazebo world files (indoor, urban) +- `models/` - Robot models (Iris with cameras, UGV) +- `src/vision/` - Visual odometry, optical flow +- `src/localization/` - Position estimation, sensor fusion +- `src/navigation/` - Path planning (relative coordinates) +- `src/safety/` - Geofencing (GPS-based) +- `config/` - Configuration files + +## Documentation + +- [Setup Guide](docs/setup_guide.md) +- [Usage Guide](docs/usage.md) +- [Architecture Overview](docs/architecture.md) +- [GPS-Denied Navigation](docs/gps_denied_navigation.md) +- [Troubleshooting](docs/troubleshooting.md) + +## Key Differences from GPS Navigation + +| Aspect | GPS Navigation | This Project (GPS-Denied) | +|--------|---------------|---------------------------| +| Position Source | GPS satellites | Visual odometry + sensors | +| Waypoint Type | GPS coordinates | Relative coordinates (x,y,z) | +| Reference Frame | Global (lat/lon) | Local (relative to start) | +| Indoor Capability | āŒ No | āœ… Yes | +| Drift | Minimal | Accumulates over time | +| Geofencing | GPS-based | GPS-based (safety only) | +| Use Cases | Outdoor, open sky | Indoor, urban, GPS-jammed | + +## Running Different Scenarios + +```bash +# Indoor warehouse (no GPS available) +bash scripts/run_simulation.sh --world worlds/indoor_warehouse.world + +# Urban canyon (degraded GPS) +bash scripts/run_simulation.sh --world worlds/urban_canyon.world + +# Open outdoor (GPS available but not used for nav) +bash scripts/run_simulation.sh --world worlds/empty_custom.world +``` + +## License + +MIT License + +## Contributing + +Contributions welcome! Please ensure all navigation remains GPS-denied (except geofencing). diff --git a/activate_venv.sh b/activate_venv.sh new file mode 100755 index 0000000..d331027 --- /dev/null +++ b/activate_venv.sh @@ -0,0 +1,11 @@ +#!/bin/bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/venv/bin/activate" +source /opt/ros/humble/setup.bash +if [ -f "$SCRIPT_DIR/install/setup.bash" ]; then + source "$SCRIPT_DIR/install/setup.bash" +fi +export GAZEBO_MODEL_PATH="$SCRIPT_DIR/models:$GAZEBO_MODEL_PATH" +export GAZEBO_RESOURCE_PATH="$SCRIPT_DIR/worlds:$GAZEBO_RESOURCE_PATH" +echo "Environment activated." +echo "Run: bash scripts/run_simulation.sh" diff --git a/config/ardupilot_gps_denied.parm b/config/ardupilot_gps_denied.parm new file mode 100644 index 0000000..8cfeebc --- /dev/null +++ b/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 diff --git a/config/geofence_params.yaml b/config/geofence_params.yaml new file mode 100644 index 0000000..c1ccb70 --- /dev/null +++ b/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" diff --git a/config/mavros_params.yaml b/config/mavros_params.yaml new file mode 100644 index 0000000..d5a79dc --- /dev/null +++ b/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 diff --git a/config/simulation_config.yaml b/config/simulation_config.yaml new file mode 100644 index 0000000..1c5da43 --- /dev/null +++ b/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 diff --git a/config/uav_params.yaml b/config/uav_params.yaml new file mode 100644 index 0000000..60abd84 --- /dev/null +++ b/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" diff --git a/config/ugv_params.yaml b/config/ugv_params.yaml new file mode 100644 index 0000000..dd274f5 --- /dev/null +++ b/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" diff --git a/docs/architecture.md b/docs/architecture.md new file mode 100644 index 0000000..83df69c --- /dev/null +++ b/docs/architecture.md @@ -0,0 +1,126 @@ +# Architecture Overview + +## System Architecture + +``` +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ SIMULATION LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ Gazebo World ā”‚ ā”‚ ArduPilot SITL ā”‚ ā”‚ MAVROS ā”‚ ā”‚ +ā”‚ ā”‚ (Physics, Viz) ā”‚ ā”‚ (Autopilot) ā”‚ ā”‚ (ROS Bridge) ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ ā”‚ ā”‚ + ā–¼ ā–¼ ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ SENSOR LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ Forward ā”‚ ā”‚ Downward ā”‚ ā”‚ IMU ā”‚ ā”‚ Range- ā”‚ ā”‚ +ā”‚ ā”‚ Camera ā”‚ ā”‚ Camera ā”‚ ā”‚ ā”‚ ā”‚ finder ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ ā”‚ ā”‚ ā”‚ + ā–¼ ā–¼ ā–¼ ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ PERCEPTION LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ Visual Odometry ā”‚ ā”‚ Optical Flow ā”‚ ā”‚ Landmark ā”‚ ā”‚ +ā”‚ ā”‚ (ORB/SIFT/SURF) ā”‚ ā”‚ (Lucas-Kanade) ā”‚ ā”‚ Detection ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ ā”‚ ā”‚ + ā–¼ ā–¼ ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ LOCALIZATION LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ Position Estimator ā”‚ ā”‚ +ā”‚ ā”‚ (EKF Sensor Fusion)ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ NAVIGATION LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ Local Planner ā”‚ ā”‚ Obstacle ā”‚ ā”‚ Waypoint ā”‚ ā”‚ +ā”‚ ā”‚ ā”‚ ā”‚ Avoidance ā”‚ ā”‚ Follower ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ ā”‚ ā”‚ + ā–¼ ā–¼ ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ CONTROL LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ UAV Controller ā”‚ ā”‚ UGV Controller ā”‚ ā”‚ Mission Planner ā”‚ ā”‚ +ā”‚ ā”‚ ā”‚ ā”‚ ā”‚ ā”‚ ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¼ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ ā”‚ ā”‚ + ā–¼ ā–¼ ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ SAFETY LAYER ā”‚ +ā”‚ ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” ā”‚ +ā”‚ ā”‚ Geofence Monitor ā”‚ ā”‚ Failsafe Handler ā”‚ ā”‚ +ā”‚ ā”‚ (GPS-based only) ā”‚ ā”‚ (Vision loss, battery) ā”‚ ā”‚ +ā”‚ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ +``` + +## Module Descriptions + +### Vision Module (`src/vision/`) +- **camera_processor.py**: Image preprocessing, undistortion +- **visual_odometry.py**: Feature-based pose estimation +- **optical_flow.py**: Velocity from image flow +- **object_detector.py**: Landmark/obstacle detection +- **visual_servoing.py**: Vision-based control + +### Localization Module (`src/localization/`) +- **position_estimator.py**: Sensor fusion for position +- **ekf_fusion.py**: Extended Kalman Filter implementation +- **landmark_tracker.py**: Track known visual features + +### Navigation Module (`src/navigation/`) +- **local_planner.py**: Path planning in local frame +- **obstacle_avoidance.py**: Reactive obstacle avoidance +- **waypoint_follower.py**: Sequential waypoint navigation + +### Control Module (`src/control/`) +- **uav_controller.py**: Quadcopter flight control +- **ugv_controller.py**: Ground vehicle control +- **mission_planner.py**: Multi-vehicle coordination + +### Safety Module (`src/safety/`) +- **geofence_monitor.py**: GPS-based boundary checking +- **failsafe_handler.py**: Emergency procedures + +## Data Flow + +``` +Camera Image ā†’ Feature Detection ā†’ Feature Matching ā†’ +Essential Matrix ā†’ Relative Pose ā†’ EKF Update ā†’ +Local Position ā†’ Navigation Controller ā†’ +Velocity Commands ā†’ MAVROS ā†’ ArduPilot +``` + +## ROS 2 Topics + +### Sensor Topics +- `/uav/camera/forward/image_raw` +- `/uav/camera/downward/image_raw` +- `/uav/imu/data` +- `/uav/rangefinder/range` + +### Perception Topics +- `/uav/visual_odometry/pose` +- `/uav/optical_flow/velocity` + +### Control Topics +- `/uav/mavros/setpoint_position/local` +- `/uav/mavros/setpoint_velocity/cmd_vel` +- `/ugv/cmd_vel` + +### Safety Topics +- `/geofence/status` +- `/failsafe/active` + diff --git a/docs/gps_denied_navigation.md b/docs/gps_denied_navigation.md new file mode 100644 index 0000000..073b05b --- /dev/null +++ b/docs/gps_denied_navigation.md @@ -0,0 +1,198 @@ +# GPS-Denied Navigation + +## Overview + +This project implements **GPS-denied navigation** for UAV and UGV vehicles. Instead of relying on GPS for position estimation and waypoint navigation, the system uses vision-based sensors and local coordinate frames. + +## Why GPS-Denied? + +GPS-denied navigation is critical for scenarios where GPS is: + +1. **Unavailable**: Indoor environments, underground, tunnels +2. **Unreliable**: Urban canyons with multipath errors +3. **Jammed/Spoofed**: Electronic warfare, contested environments +4. **Degraded**: Under bridges, heavy foliage, near tall structures + +## Navigation Architecture + +``` +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ VISION SENSORS ā”‚ +ā”‚ ā€¢ Forward Camera (640x480, 30Hz) ā”‚ +ā”‚ ā€¢ Downward Camera (320x240, 30Hz) ā”‚ +ā”‚ ā€¢ Optional: Depth Camera, LiDAR ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ VISUAL ODOMETRY MODULE ā”‚ +ā”‚ ā€¢ Feature detection (ORB/SIFT/SURF) ā”‚ +ā”‚ ā€¢ Feature matching between frames ā”‚ +ā”‚ ā€¢ Essential matrix estimation ā”‚ +ā”‚ ā€¢ Relative pose computation ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ OPTICAL FLOW MODULE ā”‚ +ā”‚ ā€¢ Lucas-Kanade optical flow ā”‚ +ā”‚ ā€¢ Velocity estimation from flow vectors ā”‚ +ā”‚ ā€¢ Height compensation using rangefinder ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ POSITION ESTIMATOR (EKF) ā”‚ +ā”‚ Fuses: ā”‚ +ā”‚ ā€¢ Visual odometry (weight: 0.6) ā”‚ +ā”‚ ā€¢ Optical flow (weight: 0.3) ā”‚ +ā”‚ ā€¢ IMU integration (weight: 0.1) ā”‚ +ā”‚ Output: Local position/velocity estimate ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”¬ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ + ā”‚ + ā–¼ +ā”Œā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā” +ā”‚ NAVIGATION CONTROLLER ā”‚ +ā”‚ ā€¢ Waypoints in LOCAL frame (x, y, z meters) ā”‚ +ā”‚ ā€¢ Path planning using relative coordinates ā”‚ +ā”‚ ā€¢ Obstacle avoidance using depth/camera ā”‚ +ā””ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”€ā”˜ +``` + +## Coordinate Frames + +### LOCAL_NED Frame +- **Origin**: Vehicle starting position +- **X**: North (forward) +- **Y**: East (right) +- **Z**: Down + +### Body Frame +- **X**: Forward +- **Y**: Right +- **Z**: Down + +## GPS Usage: Geofencing Only + +While navigation is GPS-denied, GPS is still used for **geofencing** (safety boundaries): + +```yaml +geofence: + enabled: true + use_gps: true # GPS ONLY for geofence check + + polygon_points: + - {lat: 47.397742, lon: 8.545594} + - {lat: 47.398242, lon: 8.545594} + - {lat: 47.398242, lon: 8.546094} + - {lat: 47.397742, lon: 8.546094} + + action_on_breach: "RTL" # Return to LOCAL origin +``` + +## Example: Relative Waypoint Mission + +```python +# All waypoints are in LOCAL frame (meters from origin) +waypoints = [ + {"x": 0, "y": 0, "z": 5}, # Takeoff to 5m + {"x": 10, "y": 0, "z": 5}, # 10m forward + {"x": 10, "y": 10, "z": 5}, # 10m right + {"x": 0, "y": 0, "z": 5}, # Return to origin + {"x": 0, "y": 0, "z": 0}, # Land +] +``` + +## Sensor Fusion Details + +### Extended Kalman Filter State + +``` +State vector x = [px, py, pz, vx, vy, vz, ax, ay, az] + +Where: +- px, py, pz = position (meters) +- vx, vy, vz = velocity (m/s) +- ax, ay, az = acceleration (m/sĀ²) +``` + +### Measurement Sources + +| Source | Measures | Update Rate | Noise | +|-----------------|----------------|-------------|-------| +| Visual Odometry | Position | 30 Hz | 0.05m | +| Optical Flow | Velocity | 60 Hz | 0.1m/s| +| IMU | Acceleration | 200 Hz | 0.2m/sĀ²| +| Rangefinder | Altitude | 30 Hz | 0.02m | + +## Drift Mitigation + +GPS-denied navigation accumulates drift over time. Mitigation strategies: + +1. **Visual Landmarks**: Detect and track known markers (ArUco) +2. **Loop Closure**: Recognize previously visited locations +3. **Ground Truth Reset**: Periodic reset in simulation +4. **Multi-Sensor Fusion**: Reduce individual sensor drift + +## Configuration Parameters + +### Visual Odometry + +```yaml +visual_odometry: + method: "ORB" # or "SIFT", "SURF" + min_features: 100 + max_features: 500 + feature_quality: 0.01 +``` + +### Optical Flow + +```yaml +optical_flow: + method: "Lucas-Kanade" + window_size: 15 + max_level: 3 + min_altitude: 0.3 # meters +``` + +### Position Estimator + +```yaml +position_estimator: + fusion_method: "EKF" + weights: + visual_odometry: 0.6 + optical_flow: 0.3 + imu: 0.1 +``` + +## Testing GPS-Denied Navigation + +### Indoor Warehouse World +```bash +bash scripts/run_simulation.sh worlds/indoor_warehouse.world +``` + +### Urban Canyon World +```bash +bash scripts/run_simulation.sh worlds/urban_canyon.world +``` + +## Troubleshooting + +### High Position Drift +- Check camera exposure/focus +- Ensure sufficient visual features in environment +- Increase feature count in visual odometry +- Add visual markers to environment + +### Vision Loss +- The failsafe handler triggers after 5 seconds of no visual odometry +- Action: HOLD (hover in place) by default +- Configure with `action_on_vision_loss` parameter + +### Geofence Breach +- Vehicle returns to LOCAL origin (not GPS home) +- RTL uses the same local coordinate system + diff --git a/docs/setup_guide.md b/docs/setup_guide.md new file mode 100644 index 0000000..ff7d88c --- /dev/null +++ b/docs/setup_guide.md @@ -0,0 +1,144 @@ +# Setup Guide + +## System Requirements + +- **OS**: Ubuntu 22.04 LTS (Jammy Jellyfish) +- **Python**: 3.10.x (native to Ubuntu 22.04) +- **ROS 2**: Humble Hawksbill +- **Gazebo**: Classic 11 +- **RAM**: 8GB minimum, 16GB recommended +- **GPU**: NVIDIA GPU recommended for better performance + +## Installation Steps + +### 1. Install ROS 2 Humble + +```bash +# Add ROS 2 repository +sudo apt update && sudo apt install curl gnupg lsb-release +sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg +echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + +# Install ROS 2 Humble +sudo apt update +sudo apt install ros-humble-desktop + +# Source ROS 2 +echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc +source ~/.bashrc +``` + +### 2. Install Gazebo Classic + +```bash +sudo apt install gazebo11 libgazebo11-dev ros-humble-gazebo-ros-pkgs +``` + +### 3. Install ArduPilot SITL + +```bash +# Clone ArduPilot +cd ~ +git clone https://github.com/ArduPilot/ardupilot.git +cd ardupilot +git submodule update --init --recursive + +# Install dependencies +Tools/environment_install/install-prereqs-ubuntu.sh -y +. ~/.profile + +# Build SITL +./waf configure --board sitl +./waf copter +``` + +### 4. Install ardupilot_gazebo Plugin + +```bash +cd ~ +git clone https://github.com/ArduPilot/ardupilot_gazebo.git +cd ardupilot_gazebo +mkdir build && cd build +cmake .. +make -j4 +sudo make install + +# Add to environment +echo 'export GAZEBO_MODEL_PATH=$HOME/ardupilot_gazebo/models:$GAZEBO_MODEL_PATH' >> ~/.bashrc +echo 'export GAZEBO_RESOURCE_PATH=$HOME/ardupilot_gazebo/worlds:$GAZEBO_RESOURCE_PATH' >> ~/.bashrc +source ~/.bashrc +``` + +### 5. Setup This Project + +```bash +# Clone to ROS 2 workspace +cd ~/ros2_ws/src +git clone uav_ugv_simulation +cd uav_ugv_simulation + +# Run automated setup +bash setup.sh + +# Reload environment +source ~/.bashrc +``` + +## Verification + +### Test ROS 2 +```bash +ros2 topic list +``` + +### Test Gazebo +```bash +gazebo --verbose +``` + +### Test ArduPilot SITL +```bash +cd ~/ardupilot/ArduCopter +sim_vehicle.py -v ArduCopter -f gazebo-iris --console --map +``` + +### Test Python Environment +```bash +cd ~/ros2_ws/src/uav_ugv_simulation +source activate_venv.sh +python -c "import cv2; import numpy; import rclpy; print('OK')" +``` + +## Running the Simulation + +```bash +cd ~/ros2_ws/src/uav_ugv_simulation +source activate_venv.sh +bash scripts/run_simulation.sh +``` + +## NVIDIA GPU Setup (Optional) + +For better Gazebo performance with NVIDIA GPU: + +```bash +bash scripts/setup_gazebo_nvidia.sh +``` + +## Troubleshooting + +### "Package not found" error +```bash +cd ~/ros2_ws +colcon build --packages-select uav_ugv_simulation +source install/setup.bash +``` + +### Gazebo crashes +- Check GPU drivers: `nvidia-smi` +- Try software rendering: `export LIBGL_ALWAYS_SOFTWARE=1` + +### MAVROS connection failed +- Ensure ArduPilot SITL is running first +- Check port availability: `lsof -i :14550` + diff --git a/docs/troubleshooting.md b/docs/troubleshooting.md new file mode 100644 index 0000000..cf455c2 --- /dev/null +++ b/docs/troubleshooting.md @@ -0,0 +1,178 @@ +# Troubleshooting Guide + +## Common Issues + +### 1. Gazebo Won't Start + +**Symptoms**: Gazebo window doesn't open, or crashes immediately + +**Solutions**: +```bash +# Kill any existing Gazebo processes +killall gzserver gzclient + +# Check for port conflicts +lsof -i :11345 + +# Try software rendering +export LIBGL_ALWAYS_SOFTWARE=1 +gazebo +``` + +### 2. ArduPilot SITL Connection Failed + +**Symptoms**: MAVROS can't connect to ArduPilot + +**Solutions**: +```bash +# Check if SITL is running +ps aux | grep ArduCopter + +# Check port availability +lsof -i :14550 + +# Start SITL manually first +cd ~/ardupilot/ArduCopter +sim_vehicle.py -v ArduCopter -f gazebo-iris --console + +# Then start MAVROS +ros2 run mavros mavros_node --ros-args -p fcu_url:=udp://:14550@127.0.0.1:14555 +``` + +### 3. Visual Odometry Not Working + +**Symptoms**: No pose output, high drift + +**Solutions**: +- Ensure camera is publishing: + ```bash + ros2 topic hz /uav/camera/forward/image_raw + ``` +- Check for sufficient visual features in scene +- Adjust feature detection parameters in `config/uav_params.yaml` +- Add visual markers to the world + +### 4. Vehicle Won't Arm + +**Symptoms**: Arm command fails + +**Solutions**: +```bash +# Check MAVROS state +ros2 topic echo /uav/mavros/state + +# Check for prearm errors +ros2 topic echo /uav/mavros/statustext/recv + +# Force arm (use with caution) +ros2 service call /uav/mavros/cmd/arming mavros_msgs/srv/CommandBool "{value: true}" +``` + +### 5. Geofence Immediately Triggers + +**Symptoms**: Vehicle triggers geofence breach on startup + +**Solutions**: +- Check GPS coordinates in `config/geofence_params.yaml` +- Ensure Gazebo world has correct `spherical_coordinates` +- Verify MAVROS is receiving GPS: + ```bash + ros2 topic echo /uav/mavros/global_position/global + ``` + +### 6. High CPU/Memory Usage + +**Symptoms**: System becomes slow during simulation + +**Solutions**: +- Reduce camera resolution in model SDF files +- Lower Gazebo real-time factor +- Disable unnecessary visualizations +- Use headless mode: + ```bash + gazebo --headless + ``` + +### 7. ROS 2 Package Not Found + +**Symptoms**: `Package 'uav_ugv_simulation' not found` + +**Solutions**: +```bash +# Rebuild the package +cd ~/ros2_ws +colcon build --packages-select uav_ugv_simulation + +# Source the workspace +source install/setup.bash + +# Verify installation +ros2 pkg list | grep uav_ugv +``` + +### 8. Python Import Errors + +**Symptoms**: `ModuleNotFoundError: No module named 'xxx'` + +**Solutions**: +```bash +# Activate virtual environment +source activate_venv.sh + +# Reinstall requirements +pip install -r requirements.txt +``` + +### 9. Camera Images Are Black + +**Symptoms**: Camera topic publishes but images are empty + +**Solutions**: +- Check lighting in Gazebo world +- Verify camera plugin is loaded: + ```bash + ros2 topic info /uav/camera/forward/image_raw + ``` +- Check camera is within clip range + +### 10. Vehicle Drifts Excessively + +**Symptoms**: Position estimate diverges from actual position + +**Solutions**: +- Increase visual features in environment +- Add ArUco markers for landmark tracking +- Tune EKF covariances in `config/uav_params.yaml` +- Check optical flow height compensation + +## Debug Commands + +```bash +# View all ROS nodes +ros2 node list + +# Check topic connections +ros2 topic info /topic_name + +# View parameter values +ros2 param dump /node_name + +# Echo topic data +ros2 topic echo /topic_name --once + +# View TF tree +ros2 run tf2_tools view_frames +``` + +## Log Files + +- Gazebo: `~/.gazebo/log/` +- ROS 2: `~/.ros/log/` +- ArduPilot: Check console window + +## Getting Help + +1. Check ROS 2 logs: `ros2 launch --debug ...` +2. Enable verbose Gazebo: `gazebo --verbose` +3. Check ArduPilot parameters: Use MAVProxy console + diff --git a/docs/usage.md b/docs/usage.md new file mode 100644 index 0000000..73930a1 --- /dev/null +++ b/docs/usage.md @@ -0,0 +1,151 @@ +# Usage Guide + +## Quick Start + +```bash +# 1. Navigate to project +cd ~/ros2_ws/src/uav_ugv_simulation + +# 2. Activate environment +source activate_venv.sh + +# 3. Run simulation +bash scripts/run_simulation.sh +``` + +## Launch Options + +### Full Simulation (UAV + UGV) +```bash +ros2 launch uav_ugv_simulation full_simulation.launch.py +``` + +### UAV Only +```bash +ros2 launch uav_ugv_simulation uav_only.launch.py +``` + +### UGV Only +```bash +ros2 launch uav_ugv_simulation ugv_only.launch.py +``` + +### Different Worlds +```bash +# Indoor warehouse (GPS-denied) +bash scripts/run_simulation.sh worlds/indoor_warehouse.world + +# Urban canyon (GPS-degraded) +bash scripts/run_simulation.sh worlds/urban_canyon.world +``` + +## Interacting with Vehicles + +### UAV Commands +```bash +# Arm the vehicle +ros2 topic pub /uav/controller/command std_msgs/String "data: 'arm'" + +# Takeoff +ros2 topic pub /uav/controller/command std_msgs/String "data: 'takeoff'" + +# Go to waypoint (local coordinates) +ros2 topic pub /uav/setpoint_position geometry_msgs/PoseStamped \ + "{header: {frame_id: 'odom'}, pose: {position: {x: 10.0, y: 5.0, z: 5.0}}}" + +# Land +ros2 topic pub /uav/controller/command std_msgs/String "data: 'land'" +``` + +### UGV Commands +```bash +# Go to goal (local coordinates) +ros2 topic pub /ugv/goal_pose geometry_msgs/PoseStamped \ + "{header: {frame_id: 'odom'}, pose: {position: {x: 5.0, y: 3.0, z: 0.0}}}" + +# Stop +ros2 topic pub /ugv/controller/command std_msgs/String "data: 'stop'" +``` + +## Mission Planner + +### Load Demo Mission +```bash +ros2 topic pub /mission/command std_msgs/String "data: 'load'" +``` + +### Start Mission +```bash +ros2 topic pub /mission/command std_msgs/String "data: 'start'" +``` + +### Pause/Resume +```bash +ros2 topic pub /mission/command std_msgs/String "data: 'pause'" +ros2 topic pub /mission/command std_msgs/String "data: 'resume'" +``` + +## Monitoring + +### View Topics +```bash +# List all topics +ros2 topic list + +# UAV status +ros2 topic echo /uav/controller/status + +# Visual odometry pose +ros2 topic echo /uav/visual_odometry/pose + +# Geofence status +ros2 topic echo /geofence/status +``` + +### View Camera Feeds +```bash +# Using rqt_image_view +ros2 run rqt_image_view rqt_image_view + +# Select topics: +# /uav/camera/forward/image_raw +# /uav/camera/downward/image_raw +``` + +### Visualization +```bash +# RViz +rviz2 + +# Add displays for: +# - TF +# - Camera +# - Path +# - Marker (geofence) +``` + +## Configuration + +### Modify Parameters + +Edit YAML files in `config/` directory: + +- `uav_params.yaml` - UAV settings +- `geofence_params.yaml` - Safety boundaries +- `mavros_params.yaml` - MAVROS connection + +### Runtime Parameter Changes +```bash +ros2 param set /uav/visual_odom_node max_features 500 +``` + +## Stopping the Simulation + +```bash +# Graceful shutdown +Ctrl+C + +# Force kill all processes +bash scripts/kill_simulation.sh +``` + diff --git a/launch/full_simulation.launch.py b/launch/full_simulation.launch.py new file mode 100644 index 0000000..2e785e9 --- /dev/null +++ b/launch/full_simulation.launch.py @@ -0,0 +1,169 @@ +#!/usr/bin/env python3 +"""Full Simulation Launch - GPS-Denied Navigation.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction, IncludeLaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + + +def generate_launch_description(): + pkg_share = FindPackageShare('uav_ugv_simulation').find('uav_ugv_simulation') + + world_arg = DeclareLaunchArgument( + 'world', + default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'), + description='Path to world file' + ) + + use_nvidia_arg = DeclareLaunchArgument( + 'use_nvidia', default_value='true', description='Enable NVIDIA GPU' + ) + + headless_arg = DeclareLaunchArgument( + 'headless', default_value='false', description='Run without GUI' + ) + + use_ground_truth_arg = DeclareLaunchArgument( + 'use_ground_truth', default_value='true', description='Use Gazebo ground truth' + ) + + gazebo = ExecuteProcess( + cmd=['gazebo', '--verbose', LaunchConfiguration('world')], + output='screen', + additional_env={ + 'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'), + 'GAZEBO_RESOURCE_PATH': f"{pkg_share}/worlds:" + os.path.expanduser('~/ardupilot_gazebo/worlds') + } + ) + + ardupilot_uav = TimerAction( + period=3.0, + actions=[ + ExecuteProcess( + cmd=[ + 'sim_vehicle.py', + '-v', 'ArduCopter', + '-f', 'gazebo-iris', + '--model', 'JSON', + '--map', '--console', + '-I0', + '--out', '127.0.0.1:14550', + '--out', '127.0.0.1:14551', + '--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm') + ], + cwd=os.path.expanduser('~/ardupilot/ArduCopter'), + output='screen' + ) + ] + ) + + mavros_uav = TimerAction( + period=8.0, + actions=[ + Node( + package='mavros', + executable='mavros_node', + name='mavros_uav', + namespace='uav', + output='screen', + parameters=[ + os.path.join(pkg_share, 'config', 'mavros_params.yaml'), + { + 'fcu_url': 'udp://:14550@127.0.0.1:14555', + 'gcs_url': '', + 'target_system_id': 1, + 'target_component_id': 1, + 'fcu_protocol': 'v2.0' + } + ] + ) + ] + ) + + visual_odom_node = TimerAction( + period=10.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='visual_odom_node', + name='visual_odom_node', + namespace='uav', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')] + ) + ] + ) + + geofence_node = TimerAction( + period=12.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='geofence_node', + name='geofence_monitor', + namespace='uav', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'geofence_params.yaml')] + ) + ] + ) + + vision_nav_node = TimerAction( + period=14.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='vision_nav_node', + name='vision_nav_node', + namespace='uav', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')] + ) + ] + ) + + uav_controller = TimerAction( + period=15.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='uav_controller', + name='uav_controller', + namespace='uav', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')] + ) + ] + ) + + failsafe_handler = TimerAction( + period=16.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='failsafe_handler', + name='failsafe_handler', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')] + ) + ] + ) + + return LaunchDescription([ + world_arg, + use_nvidia_arg, + headless_arg, + use_ground_truth_arg, + gazebo, + ardupilot_uav, + mavros_uav, + visual_odom_node, + geofence_node, + vision_nav_node, + uav_controller, + failsafe_handler, + ]) diff --git a/launch/uav_only.launch.py b/launch/uav_only.launch.py new file mode 100644 index 0000000..2014b14 --- /dev/null +++ b/launch/uav_only.launch.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 +"""UAV-only Simulation Launch.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + + +def generate_launch_description(): + pkg_share = FindPackageShare('uav_ugv_simulation').find('uav_ugv_simulation') + + world_arg = DeclareLaunchArgument( + 'world', + default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'), + description='Path to world file' + ) + + gazebo = ExecuteProcess( + cmd=['gazebo', '--verbose', LaunchConfiguration('world')], + output='screen', + additional_env={ + 'GAZEBO_MODEL_PATH': f"{pkg_share}/models:" + os.path.expanduser('~/ardupilot_gazebo/models'), + } + ) + + ardupilot = TimerAction( + period=3.0, + actions=[ + ExecuteProcess( + cmd=[ + 'sim_vehicle.py', '-v', 'ArduCopter', '-f', 'gazebo-iris', + '--model', 'JSON', '--map', '--console', '-I0', + '--out', '127.0.0.1:14550', + '--add-param-file', os.path.join(pkg_share, 'config', 'ardupilot_gps_denied.parm') + ], + cwd=os.path.expanduser('~/ardupilot/ArduCopter'), + output='screen' + ) + ] + ) + + mavros = TimerAction( + period=8.0, + actions=[ + Node( + package='mavros', + executable='mavros_node', + name='mavros', + namespace='uav', + output='screen', + parameters=[ + os.path.join(pkg_share, 'config', 'mavros_params.yaml'), + {'fcu_url': 'udp://:14550@127.0.0.1:14555'} + ] + ) + ] + ) + + visual_odom = TimerAction( + period=10.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='visual_odom_node', + name='visual_odom_node', + namespace='uav', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')] + ) + ] + ) + + uav_controller = TimerAction( + period=12.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='uav_controller', + name='uav_controller', + namespace='uav', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'uav_params.yaml')] + ) + ] + ) + + return LaunchDescription([ + world_arg, + gazebo, + ardupilot, + mavros, + visual_odom, + uav_controller, + ]) diff --git a/launch/ugv_only.launch.py b/launch/ugv_only.launch.py new file mode 100644 index 0000000..9812aa2 --- /dev/null +++ b/launch/ugv_only.launch.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +"""UGV-only Simulation Launch.""" + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +import os + + +def generate_launch_description(): + pkg_share = FindPackageShare('uav_ugv_simulation').find('uav_ugv_simulation') + + world_arg = DeclareLaunchArgument( + 'world', + default_value=os.path.join(pkg_share, 'worlds', 'empty_custom.world'), + description='Path to world file' + ) + + gazebo = ExecuteProcess( + cmd=['gazebo', '--verbose', LaunchConfiguration('world')], + output='screen', + additional_env={ + 'GAZEBO_MODEL_PATH': f"{pkg_share}/models", + } + ) + + ugv_spawn = TimerAction( + period=3.0, + actions=[ + ExecuteProcess( + cmd=[ + 'ros2', 'run', 'gazebo_ros', 'spawn_entity.py', + '-entity', 'ugv', + '-file', os.path.join(pkg_share, 'models', 'custom_ugv', 'model.sdf'), + '-x', '5.0', '-y', '0.0', '-z', '0.1' + ], + output='screen' + ) + ] + ) + + ugv_controller = TimerAction( + period=5.0, + actions=[ + Node( + package='uav_ugv_simulation', + executable='ugv_controller', + name='ugv_controller', + namespace='ugv', + output='screen', + parameters=[os.path.join(pkg_share, 'config', 'ugv_params.yaml')] + ) + ] + ) + + return LaunchDescription([ + world_arg, + gazebo, + ugv_spawn, + ugv_controller, + ]) diff --git a/launch/utils/launch_helpers.py b/launch/utils/launch_helpers.py new file mode 100644 index 0000000..1c54cd9 --- /dev/null +++ b/launch/utils/launch_helpers.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +"""Launch file helper utilities.""" + +import os +from ament_index_python.packages import get_package_share_directory + + +def get_package_share_path(package_name): + return get_package_share_directory(package_name) + + +def get_config_path(package_name, config_file): + return os.path.join(get_package_share_path(package_name), 'config', config_file) + + +def get_world_path(package_name, world_file): + return os.path.join(get_package_share_path(package_name), 'worlds', world_file) + + +def get_model_path(package_name, model_name): + return os.path.join(get_package_share_path(package_name), 'models', model_name) diff --git a/models/custom_ugv/model.config b/models/custom_ugv/model.config new file mode 100644 index 0000000..99fa28f --- /dev/null +++ b/models/custom_ugv/model.config @@ -0,0 +1,12 @@ + + + Custom UGV + 1.0 + model.sdf + + UAV-UGV Simulation + + + Custom ground vehicle with cameras for GPS-denied navigation. + + diff --git a/models/custom_ugv/model.sdf b/models/custom_ugv/model.sdf new file mode 100644 index 0000000..f2b0398 --- /dev/null +++ b/models/custom_ugv/model.sdf @@ -0,0 +1,159 @@ + + + + false + + + 0 0 0.1 0 0 0 + + 5.0 + + 0.100 + 0.1500.1 + + + + 0.4 0.3 0.15 + + + 0.4 0.3 0.15 + 0.3 0.3 0.8 1 + + + + + 0 0.175 0.05 -1.5708 0 0 + + 0.5 + + 0.00100 + 0.00100.001 + + + + 0.050.04 + + 1.01.0 + + + + 0.050.04 + 0.1 0.1 0.1 1 + + + + + 0 -0.175 0.05 -1.5708 0 0 + + 0.5 + + 0.00100 + 0.00100.001 + + + + 0.050.04 + + 1.01.0 + + + + 0.050.04 + 0.1 0.1 0.1 1 + + + + + -0.15 0 0.025 0 0 0 + + 0.1 + + 0.000100 + 0.000100.0001 + + + + 0.025 + + 0.00.0 + + + + 0.025 + 0.1 0.1 0.1 1 + + + + + base_link + left_wheel + 0 0 1 + + + + base_link + right_wheel + 0 0 1 + + + + base_link + caster + + + + 0.22 0 0.15 0 0 0 + + 0.01 + + 0.0000100 + 0.0000100.00001 + + + + 0.02 0.04 0.02 + 0 0 0 1 + + + + 1.57 + 640480R8G8B8 + 0.150 + + 1 + 30 + + + /ugv + image_raw:=camera/forward/image_raw + camera_info:=camera/forward/camera_info + + forward_camera + camera_link + + + + + + base_link + camera_link + + + + /ugv + left_wheel_joint + right_wheel_joint + 0.35 + 0.1 + 5 + 2.0 + cmd_vel + odom + odom + base_link + true + true + true + + + diff --git a/models/iris_with_camera/model.config b/models/iris_with_camera/model.config new file mode 100644 index 0000000..a8792ad --- /dev/null +++ b/models/iris_with_camera/model.config @@ -0,0 +1,13 @@ + + + Iris with Camera + 1.0 + model.sdf + + UAV-UGV Simulation + developer@example.com + + + Iris quadcopter with forward and downward cameras for GPS-denied navigation. + + diff --git a/models/iris_with_camera/model.sdf b/models/iris_with_camera/model.sdf new file mode 100644 index 0000000..f7917bf --- /dev/null +++ b/models/iris_with_camera/model.sdf @@ -0,0 +1,128 @@ + + + + + model://iris + + + + 0.1 0 0 0 0 0 + + 0.01 + + 0.0000100 + 0.0000100.00001 + + + + 0.02 0.02 0.02 + 0 0 0 1 + + + + 1.57 + + 640 + 480 + R8G8B8 + + 0.1100 + + 1 + 30 + true + + + /uav + image_raw:=camera/forward/image_raw + camera_info:=camera/forward/camera_info + + forward_camera + forward_camera_link + + + + + + iris::base_link + forward_camera_link + + + + 0 0 -0.05 0 1.5708 0 + + 0.01 + + 0.0000100 + 0.0000100.00001 + + + + 0.02 0.02 0.02 + 0.3 0.3 0.3 1 + + + + 1.2 + + 320 + 240 + R8G8B8 + + 0.150 + + 1 + 30 + false + + + /uav + image_raw:=camera/downward/image_raw + camera_info:=camera/downward/camera_info + + downward_camera + downward_camera_link + + + + + + iris::base_link + downward_camera_link + + + + 0 0 -0.06 0 1.5708 0 + + 0.005 + + 0.00000100 + 0.00000100.000001 + + + + + + 1100 + + 0.1100.01 + + 1 + 30 + + + /uav + ~/out:=rangefinder/range + + sensor_msgs/Range + rangefinder_link + + + + + + iris::base_link + rangefinder_link + + + diff --git a/models/iris_with_sensors/model.config b/models/iris_with_sensors/model.config new file mode 100644 index 0000000..7a31d9c --- /dev/null +++ b/models/iris_with_sensors/model.config @@ -0,0 +1,12 @@ + + + Iris with Sensors + 1.0 + model.sdf + + UAV-UGV Simulation + + + Iris quadcopter with optical flow sensor for GPS-denied navigation. + + diff --git a/models/iris_with_sensors/model.sdf b/models/iris_with_sensors/model.sdf new file mode 100644 index 0000000..a24c375 --- /dev/null +++ b/models/iris_with_sensors/model.sdf @@ -0,0 +1,88 @@ + + + + + model://iris + + + + 0 0 -0.05 0 1.5708 0 + + 0.01 + + 0.0000100 + 0.0000100.00001 + + + + 0.010.02 + 0.2 0.2 0.2 1 + + + + 1.0 + + 160 + 120 + R8G8B8 + + 0.120 + + 1 + 60 + + + /uav + image_raw:=optical_flow/image_raw + + optical_flow + optical_flow_link + + + + + + iris::base_link + optical_flow_link + + + + 0 0 0 0 0 0 + + 0.005 + + 0.00000100 + 0.00000100.000001 + + + + + + 00.0002 + 00.0002 + 00.0002 + + + 00.017 + 00.017 + 00.017 + + + 1 + 200 + + + /uav + ~/out:=imu/data + + imu_link + + + + + + iris::base_link + imu_link + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..181bdea --- /dev/null +++ b/package.xml @@ -0,0 +1,49 @@ + + + + uav_ugv_simulation + 1.0.0 + UAV-UGV Gazebo SITL Simulation with GPS-Denied Navigation + Developer + MIT + + ament_python + + + rclpy + std_msgs + geometry_msgs + sensor_msgs + nav_msgs + visualization_msgs + + + tf2 + tf2_ros + tf2_geometry_msgs + + + cv_bridge + image_transport + + + mavros + mavros_msgs + + + gazebo_ros + gazebo_plugins + + + launch + launch_ros + + + ament_lint_auto + ament_lint_common + python3-pytest + + + ament_python + + diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..f653a94 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,58 @@ +# Core dependencies for Ubuntu 22.04 + Python 3.10 +# GPS-Denied Navigation Requirements + +numpy>=1.21.0,<1.27.0 +opencv-python>=4.5.0,<4.9.0 +opencv-contrib-python>=4.5.0,<4.9.0 + +# Visual odometry and feature tracking +# OpenCV's contrib has SIFT, SURF, ORB for feature detection + +# ROS 2 Python dependencies +empy>=3.3.4 +lark>=1.1.1 + +# Computer vision and scientific computing +scipy>=1.7.0,<1.12.0 +scikit-learn>=1.0.0,<1.4.0 +scikit-image>=0.19.0,<0.23.0 +matplotlib>=3.5.0,<3.9.0 +pillow>=9.0.0,<11.0.0 + +# MAVLink +pymavlink>=2.4.0 + +# Kalman filtering for sensor fusion +filterpy>=1.4.5 + +# Coordinate transformations +transforms3d>=0.4.1 +pyproj>=3.3.0 # For geofence coordinate transforms only + +# Geometry for geofencing +shapely>=2.0.0 + +# Utilities +pyyaml>=6.0 +pyserial>=3.5 +colorama>=0.4.4 + +# Development and testing +pytest>=7.0.0 +pytest-cov>=3.0.0 +black>=22.0.0 +flake8>=4.0.0 +pylint>=2.12.0 + +# Optional: SLAM libraries (uncomment if needed) +# opencv-contrib-python includes ORB-SLAM dependencies +# Add these for advanced SLAM: +# g2o-python>=0.0.1 # Graph optimization +# open3d>=0.15.0 # 3D processing + +# Optional: Deep Learning for visual features (uncomment if needed) +# torch>=1.10.0,<2.2.0 +# torchvision>=0.11.0,<0.17.0 + +# Optional: Advanced visual odometry (uncomment if needed) +# pykitti>=0.3.1 # For testing with KITTI dataset diff --git a/resource/uav_ugv_simulation b/resource/uav_ugv_simulation new file mode 100644 index 0000000..e69de29 diff --git a/scripts/kill_simulation.sh b/scripts/kill_simulation.sh new file mode 100755 index 0000000..f4f0d12 --- /dev/null +++ b/scripts/kill_simulation.sh @@ -0,0 +1,19 @@ +#!/bin/bash + +echo "Killing all simulation processes..." + +pkill -9 -f "gazebo" 2>/dev/null || true +pkill -9 -f "gzserver" 2>/dev/null || true +pkill -9 -f "gzclient" 2>/dev/null || true +pkill -9 -f "sim_vehicle.py" 2>/dev/null || true +pkill -9 -f "mavproxy" 2>/dev/null || true +pkill -9 -f "mavros" 2>/dev/null || true +pkill -9 -f "ArduCopter" 2>/dev/null || true +pkill -9 -f "ArduRover" 2>/dev/null || true +pkill -9 -f "ros2" 2>/dev/null || true + +sleep 1 + +echo "All processes killed." +echo "Remaining ROS nodes:" +ros2 node list 2>/dev/null || echo "No ROS nodes running" diff --git a/scripts/run_simulation.sh b/scripts/run_simulation.sh new file mode 100755 index 0000000..b93a4f1 --- /dev/null +++ b/scripts/run_simulation.sh @@ -0,0 +1,53 @@ +#!/bin/bash +set -e + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_DIR="$(dirname "$SCRIPT_DIR")" + +echo "==========================================" +echo "UAV-UGV Simulation - Run Script" +echo "GPS-Denied Navigation with Geofencing" +echo "==========================================" + +WORLD="${1:-$PROJECT_DIR/worlds/empty_custom.world}" + +if [ ! -f "$WORLD" ]; then + echo "World file not found: $WORLD" + echo "Using default world..." + WORLD="$PROJECT_DIR/worlds/empty_custom.world" +fi + +export GAZEBO_MODEL_PATH="$PROJECT_DIR/models:$GAZEBO_MODEL_PATH" +export GAZEBO_RESOURCE_PATH="$PROJECT_DIR/worlds:$GAZEBO_RESOURCE_PATH" + +if command -v nvidia-smi &> /dev/null; then + echo "NVIDIA GPU detected, enabling acceleration..." + export __NV_PRIME_RENDER_OFFLOAD=1 + export __GLX_VENDOR_LIBRARY_NAME=nvidia +fi + +cleanup() { + echo "" + echo "Shutting down simulation..." + pkill -f "gazebo" 2>/dev/null || true + pkill -f "sim_vehicle.py" 2>/dev/null || true + pkill -f "mavros" 2>/dev/null || true + echo "Cleanup complete." +} + +trap cleanup EXIT + +echo "Starting simulation with world: $WORLD" +echo "" + +if [ -f "$PROJECT_DIR/venv/bin/activate" ]; then + source "$PROJECT_DIR/venv/bin/activate" +fi + +source /opt/ros/humble/setup.bash 2>/dev/null || true + +if [ -f "$PROJECT_DIR/install/setup.bash" ]; then + source "$PROJECT_DIR/install/setup.bash" +fi + +ros2 launch uav_ugv_simulation full_simulation.launch.py world:="$WORLD" diff --git a/scripts/setup_gazebo_nvidia.sh b/scripts/setup_gazebo_nvidia.sh new file mode 100755 index 0000000..8257265 --- /dev/null +++ b/scripts/setup_gazebo_nvidia.sh @@ -0,0 +1,25 @@ +#!/bin/bash + +echo "Setting up NVIDIA GPU for Gazebo..." + +if ! command -v nvidia-smi &> /dev/null; then + echo "NVIDIA driver not found. Skipping GPU setup." + exit 0 +fi + +echo "NVIDIA GPU detected:" +nvidia-smi --query-gpu=name,driver_version --format=csv,noheader + +export __NV_PRIME_RENDER_OFFLOAD=1 +export __GLX_VENDOR_LIBRARY_NAME=nvidia +export VK_ICD_FILENAMES=/usr/share/vulkan/icd.d/nvidia_icd.json + +if [ -f /usr/lib/x86_64-linux-gnu/libEGL_nvidia.so.0 ]; then + export __EGL_VENDOR_LIBRARY_FILENAMES=/usr/share/glvnd/egl_vendor.d/10_nvidia.json +fi + +echo "NVIDIA GPU environment configured." +echo "" +echo "Add these to your ~/.bashrc for permanent setup:" +echo " export __NV_PRIME_RENDER_OFFLOAD=1" +echo " export __GLX_VENDOR_LIBRARY_NAME=nvidia" diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..49f31a8 --- /dev/null +++ b/setup.py @@ -0,0 +1,77 @@ +"""ROS 2 Python package setup for UAV-UGV Simulation.""" + +from setuptools import setup, find_packages +import os +from glob import glob + +package_name = 'uav_ugv_simulation' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['tests']), + data_files=[ + # Package resource index + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + # Package.xml + ('share/' + package_name, ['package.xml']), + # Launch files + (os.path.join('share', package_name, 'launch'), + glob('launch/*.py')), + (os.path.join('share', package_name, 'launch', 'utils'), + glob('launch/utils/*.py')), + # Config files + (os.path.join('share', package_name, 'config'), + glob('config/*.yaml') + glob('config/*.parm')), + # World files + (os.path.join('share', package_name, 'worlds'), + glob('worlds/*.world')), + # Model files + (os.path.join('share', package_name, 'models', 'iris_with_camera'), + glob('models/iris_with_camera/*')), + (os.path.join('share', package_name, 'models', 'iris_with_sensors'), + glob('models/iris_with_sensors/*')), + (os.path.join('share', package_name, 'models', 'custom_ugv'), + glob('models/custom_ugv/*')), + # Scripts + (os.path.join('share', package_name, 'scripts'), + glob('scripts/*.sh')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Developer', + maintainer_email='developer@example.com', + description='UAV-UGV Gazebo SITL Simulation with GPS-Denied Navigation', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + # Vision nodes + 'visual_odom_node = src.nodes.visual_odom_node:main', + 'camera_processor = src.vision.camera_processor:main', + 'optical_flow_node = src.vision.optical_flow:main', + + # Localization nodes + 'position_estimator = src.localization.position_estimator:main', + 'ekf_fusion_node = src.localization.ekf_fusion:main', + + # Navigation nodes + 'vision_nav_node = src.nodes.vision_nav_node:main', + 'local_planner = src.navigation.local_planner:main', + 'waypoint_follower = src.navigation.waypoint_follower:main', + + # Control nodes + 'uav_controller = src.control.uav_controller:main', + 'ugv_controller = src.control.ugv_controller:main', + 'mission_planner = src.control.mission_planner:main', + + # Safety nodes + 'geofence_node = src.nodes.geofence_node:main', + 'failsafe_handler = src.safety.failsafe_handler:main', + + # Coordination + 'multi_vehicle_coord = src.nodes.multi_vehicle_coord:main', + ], + }, +) diff --git a/setup.sh b/setup.sh new file mode 100755 index 0000000..cbae3b3 --- /dev/null +++ b/setup.sh @@ -0,0 +1,93 @@ +#!/bin/bash +set -e + +echo "==========================================" +echo "UAV-UGV Simulation Setup" +echo "Ubuntu 22.04 + Python 3.10 + ROS 2 Humble" +echo "GPS-Denied Navigation with Geofencing" +echo "==========================================" +echo "" + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$SCRIPT_DIR" + +echo "[1/7] Updating system packages..." +sudo apt-get update + +echo "[2/7] Installing system dependencies..." +sudo apt-get install -y \ + python3-pip \ + python3-venv \ + python3-opencv \ + libopencv-dev \ + ros-humble-mavros \ + ros-humble-mavros-extras \ + ros-humble-cv-bridge \ + ros-humble-image-transport \ + ros-humble-gazebo-ros-pkgs \ + ros-humble-tf2 \ + ros-humble-tf2-ros \ + ros-humble-tf2-geometry-msgs + +echo "[3/7] Installing MAVROS GeographicLib datasets..." +if [ ! -f /usr/share/GeographicLib/geoids/egm96-5.pgm ]; then + sudo /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh || true +fi + +echo "[4/7] Creating Python virtual environment..." +if [ ! -d "venv" ]; then + python3 -m venv venv +fi +source venv/bin/activate + +echo "[5/7] Installing Python dependencies..." +pip install --upgrade pip +pip install -r requirements.txt + +echo "[6/7] Building ROS 2 package..." +source /opt/ros/humble/setup.bash + +cd .. +if [ -d "$(dirname "$SCRIPT_DIR")/src" ]; then + cd "$(dirname "$SCRIPT_DIR")" +fi + +if [ -f "$SCRIPT_DIR/package.xml" ]; then + colcon build --packages-select uav_ugv_simulation --symlink-install 2>/dev/null || \ + echo "Colcon build skipped - run from ROS workspace" +fi + +cd "$SCRIPT_DIR" + +echo "[7/7] Making scripts executable..." +chmod +x scripts/*.sh + +cat > activate_venv.sh << 'EOF' +#!/bin/bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/venv/bin/activate" +source /opt/ros/humble/setup.bash +if [ -f "$SCRIPT_DIR/install/setup.bash" ]; then + source "$SCRIPT_DIR/install/setup.bash" +fi +export GAZEBO_MODEL_PATH="$SCRIPT_DIR/models:$GAZEBO_MODEL_PATH" +export GAZEBO_RESOURCE_PATH="$SCRIPT_DIR/worlds:$GAZEBO_RESOURCE_PATH" +echo "Environment activated." +echo "Run: bash scripts/run_simulation.sh" +EOF +chmod +x activate_venv.sh + +echo "" +echo "==========================================" +echo "Setup Complete!" +echo "==========================================" +echo "" +echo "Next steps:" +echo " 1. source ~/.bashrc" +echo " 2. source activate_venv.sh" +echo " 3. bash scripts/run_simulation.sh" +echo "" +echo "GPS-Denied Navigation:" +echo " - All navigation uses LOCAL coordinates" +echo " - GPS is ONLY used for geofencing" +echo "" diff --git a/src/__init__.py b/src/__init__.py new file mode 100644 index 0000000..847e6e8 --- /dev/null +++ b/src/__init__.py @@ -0,0 +1,4 @@ +"""UAV-UGV Simulation Package - GPS-Denied Navigation with Geofencing.""" + +__version__ = "1.0.0" +__author__ = "Developer" diff --git a/src/control/__init__.py b/src/control/__init__.py new file mode 100644 index 0000000..df7880f --- /dev/null +++ b/src/control/__init__.py @@ -0,0 +1 @@ +"""Control module for UAV and UGV controllers.""" diff --git a/src/control/mission_planner.py b/src/control/mission_planner.py new file mode 100644 index 0000000..49b7834 --- /dev/null +++ b/src/control/mission_planner.py @@ -0,0 +1,236 @@ +#!/usr/bin/env python3 +"""Mission Planner - Coordinates missions using local waypoints.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from std_msgs.msg import String, Bool +import yaml +from enum import Enum + + +class MissionState(Enum): + IDLE = 0 + LOADING = 1 + EXECUTING = 2 + PAUSED = 3 + COMPLETED = 4 + ABORTED = 5 + + +class MissionPlanner(Node): + + def __init__(self): + super().__init__('mission_planner') + + self.declare_parameter('mission_file', '') + + self.mission = [] + self.current_action_idx = 0 + self.state = MissionState.IDLE + + self.uav_goal_reached = False + self.ugv_goal_reached = False + + self.command_sub = self.create_subscription( + String, '/mission/command', self.command_callback, 10) + + self.uav_status_sub = self.create_subscription( + String, '/uav/controller/status', self.uav_status_callback, 10) + + self.ugv_status_sub = self.create_subscription( + String, '/ugv/controller/status', self.ugv_status_callback, 10) + + self.uav_reached_sub = self.create_subscription( + Bool, '/uav/waypoint_follower/waypoint_reached', self.uav_reached_callback, 10) + + self.ugv_reached_sub = self.create_subscription( + Bool, '/ugv/goal_reached', self.ugv_reached_callback, 10) + + self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10) + self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10) + self.uav_waypoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10) + self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10) + self.uav_path_pub = self.create_publisher(Path, '/waypoint_follower/set_path', 10) + self.status_pub = self.create_publisher(String, '/mission/status', 10) + + self.mission_timer = self.create_timer(0.5, self.mission_loop) + + self.get_logger().info('Mission Planner Started - LOCAL coordinates only') + + def command_callback(self, msg): + cmd = msg.data.lower().split(':') + action = cmd[0] + + if action == 'load': + if len(cmd) > 1: + self.load_mission_file(cmd[1]) + else: + self.load_demo_mission() + elif action == 'start': + self.start_mission() + elif action == 'pause': + self.state = MissionState.PAUSED + elif action == 'resume': + self.state = MissionState.EXECUTING + elif action == 'abort': + self.abort_mission() + elif action == 'clear': + self.mission = [] + self.current_action_idx = 0 + self.state = MissionState.IDLE + + def uav_status_callback(self, msg): + pass + + def ugv_status_callback(self, msg): + pass + + def uav_reached_callback(self, msg): + if msg.data: + self.uav_goal_reached = True + + def ugv_reached_callback(self, msg): + if msg.data: + self.ugv_goal_reached = True + + def load_demo_mission(self): + self.mission = [ + {'type': 'uav_command', 'command': 'arm'}, + {'type': 'uav_command', 'command': 'takeoff'}, + {'type': 'wait', 'duration': 3.0}, + {'type': 'uav_waypoint', 'x': 10.0, 'y': 0.0, 'z': 5.0}, + {'type': 'uav_waypoint', 'x': 10.0, 'y': 10.0, 'z': 5.0}, + {'type': 'ugv_goal', 'x': 5.0, 'y': 5.0}, + {'type': 'wait_for_vehicles'}, + {'type': 'uav_waypoint', 'x': 0.0, 'y': 0.0, 'z': 5.0}, + {'type': 'ugv_goal', 'x': 0.0, 'y': 0.0}, + {'type': 'wait_for_vehicles'}, + {'type': 'uav_command', 'command': 'land'}, + ] + self.current_action_idx = 0 + self.state = MissionState.IDLE + self.get_logger().info(f'Demo mission loaded: {len(self.mission)} actions') + + def load_mission_file(self, filepath): + try: + with open(filepath, 'r') as f: + data = yaml.safe_load(f) + self.mission = data.get('mission', []) + self.current_action_idx = 0 + self.state = MissionState.IDLE + self.get_logger().info(f'Mission loaded from {filepath}') + except Exception as e: + self.get_logger().error(f'Failed to load mission: {e}') + + def start_mission(self): + if len(self.mission) == 0: + self.get_logger().warn('No mission loaded') + return + + self.current_action_idx = 0 + self.state = MissionState.EXECUTING + self.uav_goal_reached = False + self.ugv_goal_reached = False + self.get_logger().info('Mission started') + + def abort_mission(self): + self.state = MissionState.ABORTED + + stop_msg = String() + stop_msg.data = 'hold' + self.uav_cmd_pub.publish(stop_msg) + + stop_msg.data = 'stop' + self.ugv_cmd_pub.publish(stop_msg) + + self.get_logger().warn('Mission aborted') + + def mission_loop(self): + status_msg = String() + status_msg.data = f'{self.state.name}|{self.current_action_idx}/{len(self.mission)}' + self.status_pub.publish(status_msg) + + if self.state != MissionState.EXECUTING: + return + + if self.current_action_idx >= len(self.mission): + self.state = MissionState.COMPLETED + self.get_logger().info('Mission completed!') + return + + action = self.mission[self.current_action_idx] + completed = self.execute_action(action) + + if completed: + self.current_action_idx += 1 + self.uav_goal_reached = False + self.ugv_goal_reached = False + + def execute_action(self, action): + action_type = action.get('type', '') + + if action_type == 'uav_command': + cmd_msg = String() + cmd_msg.data = action['command'] + self.uav_cmd_pub.publish(cmd_msg) + return True + + elif action_type == 'ugv_command': + cmd_msg = String() + cmd_msg.data = action['command'] + self.ugv_cmd_pub.publish(cmd_msg) + return True + + elif action_type == 'uav_waypoint': + pose = PoseStamped() + pose.header.stamp = self.get_clock().now().to_msg() + pose.header.frame_id = 'odom' + pose.pose.position.x = float(action['x']) + pose.pose.position.y = float(action['y']) + pose.pose.position.z = float(action['z']) + pose.pose.orientation.w = 1.0 + self.uav_waypoint_pub.publish(pose) + return self.uav_goal_reached + + elif action_type == 'ugv_goal': + pose = PoseStamped() + pose.header.stamp = self.get_clock().now().to_msg() + pose.header.frame_id = 'odom' + pose.pose.position.x = float(action['x']) + pose.pose.position.y = float(action['y']) + pose.pose.orientation.w = 1.0 + self.ugv_goal_pub.publish(pose) + return self.ugv_goal_reached + + elif action_type == 'wait': + if not hasattr(self, '_wait_start'): + self._wait_start = self.get_clock().now() + + elapsed = (self.get_clock().now() - self._wait_start).nanoseconds / 1e9 + if elapsed >= action['duration']: + delattr(self, '_wait_start') + return True + return False + + elif action_type == 'wait_for_vehicles': + return self.uav_goal_reached and self.ugv_goal_reached + + return True + + +def main(args=None): + rclpy.init(args=args) + node = MissionPlanner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/control/uav_controller.py b/src/control/uav_controller.py new file mode 100644 index 0000000..69e85e0 --- /dev/null +++ b/src/control/uav_controller.py @@ -0,0 +1,249 @@ +#!/usr/bin/env python3 +"""UAV Controller - Flight control using local position only.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, TwistStamped, Twist +from mavros_msgs.srv import CommandBool, SetMode, CommandTOL +from mavros_msgs.msg import State +from std_msgs.msg import String, Bool +import numpy as np +from enum import Enum + + +class FlightState(Enum): + DISARMED = 0 + ARMED = 1 + TAKING_OFF = 2 + HOVERING = 3 + NAVIGATING = 4 + LANDING = 5 + EMERGENCY = 6 + + +class UAVController(Node): + + def __init__(self): + super().__init__('uav_controller') + + self.declare_parameter('takeoff_altitude', 5.0) + self.declare_parameter('position_tolerance', 0.3) + self.declare_parameter('namespace', '/uav') + + self.takeoff_altitude = self.get_parameter('takeoff_altitude').value + self.position_tolerance = self.get_parameter('position_tolerance').value + ns = self.get_parameter('namespace').value + + self.state = FlightState.DISARMED + self.mavros_state = None + self.current_pose = None + self.target_pose = None + self.home_position = None + + self.state_sub = self.create_subscription( + State, f'{ns}/mavros/state', self.state_callback, 10) + + self.local_pose_sub = self.create_subscription( + PoseStamped, f'{ns}/mavros/local_position/pose', self.pose_callback, 10) + + self.cmd_sub = self.create_subscription( + String, f'{ns}/controller/command', self.command_callback, 10) + + self.setpoint_sub = self.create_subscription( + PoseStamped, f'{ns}/setpoint_position', self.setpoint_callback, 10) + + self.vel_sub = self.create_subscription( + Twist, f'{ns}/cmd_vel_safe', self.velocity_callback, 10) + + self.setpoint_pub = self.create_publisher( + PoseStamped, f'{ns}/mavros/setpoint_position/local', 10) + + self.vel_pub = self.create_publisher( + TwistStamped, f'{ns}/mavros/setpoint_velocity/cmd_vel', 10) + + self.status_pub = self.create_publisher( + String, f'{ns}/controller/status', 10) + + self.arming_client = self.create_client(CommandBool, f'{ns}/mavros/cmd/arming') + self.set_mode_client = self.create_client(SetMode, f'{ns}/mavros/set_mode') + self.takeoff_client = self.create_client(CommandTOL, f'{ns}/mavros/cmd/takeoff') + self.land_client = self.create_client(CommandTOL, f'{ns}/mavros/cmd/land') + + self.setpoint_timer = self.create_timer(0.05, self.publish_setpoint) + self.status_timer = self.create_timer(0.5, self.publish_status) + + self.get_logger().info('UAV Controller Started - GPS-denied mode') + + def state_callback(self, msg): + self.mavros_state = msg + if msg.armed and self.state == FlightState.DISARMED: + self.state = FlightState.ARMED + + def pose_callback(self, msg): + self.current_pose = msg + if self.home_position is None and self.mavros_state and self.mavros_state.armed: + self.home_position = np.array([ + msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z + ]) + self.get_logger().info(f'Home position set: {self.home_position}') + + def setpoint_callback(self, msg): + self.target_pose = msg + + def velocity_callback(self, msg): + if self.state not in [FlightState.NAVIGATING, FlightState.HOVERING]: + return + + vel_msg = TwistStamped() + vel_msg.header.stamp = self.get_clock().now().to_msg() + vel_msg.header.frame_id = 'base_link' + vel_msg.twist = msg + self.vel_pub.publish(vel_msg) + + def command_callback(self, msg): + cmd = msg.data.lower() + + if cmd == 'arm': + self.arm() + elif cmd == 'disarm': + self.disarm() + elif cmd == 'takeoff': + self.takeoff() + elif cmd == 'land': + self.land() + elif cmd == 'rtl': + self.return_to_launch() + elif cmd == 'hold': + self.hold_position() + elif cmd == 'guided': + self.set_mode('GUIDED') + + def arm(self): + if not self.arming_client.wait_for_service(timeout_sec=5.0): + self.get_logger().error('Arming service not available') + return False + + req = CommandBool.Request() + req.value = True + future = self.arming_client.call_async(req) + future.add_done_callback(self.arm_response) + return True + + def arm_response(self, future): + try: + result = future.result() + if result.success: + self.state = FlightState.ARMED + self.get_logger().info('Vehicle armed') + else: + self.get_logger().error('Arming failed') + except Exception as e: + self.get_logger().error(f'Arming error: {e}') + + def disarm(self): + req = CommandBool.Request() + req.value = False + future = self.arming_client.call_async(req) + self.state = FlightState.DISARMED + + def set_mode(self, mode): + if not self.set_mode_client.wait_for_service(timeout_sec=5.0): + return False + + req = SetMode.Request() + req.custom_mode = mode + future = self.set_mode_client.call_async(req) + return True + + def takeoff(self): + self.set_mode('GUIDED') + self.arm() + + if self.current_pose is not None: + self.target_pose = PoseStamped() + self.target_pose.header.frame_id = 'odom' + self.target_pose.pose.position.x = self.current_pose.pose.position.x + self.target_pose.pose.position.y = self.current_pose.pose.position.y + self.target_pose.pose.position.z = self.takeoff_altitude + self.target_pose.pose.orientation.w = 1.0 + + self.state = FlightState.TAKING_OFF + self.get_logger().info(f'Taking off to {self.takeoff_altitude}m') + + def land(self): + if self.current_pose is not None: + self.target_pose = PoseStamped() + self.target_pose.header.frame_id = 'odom' + self.target_pose.pose.position.x = self.current_pose.pose.position.x + self.target_pose.pose.position.y = self.current_pose.pose.position.y + self.target_pose.pose.position.z = 0.0 + self.target_pose.pose.orientation.w = 1.0 + + self.state = FlightState.LANDING + self.get_logger().info('Landing') + + def return_to_launch(self): + if self.home_position is not None: + self.target_pose = PoseStamped() + self.target_pose.header.frame_id = 'odom' + self.target_pose.pose.position.x = float(self.home_position[0]) + self.target_pose.pose.position.y = float(self.home_position[1]) + self.target_pose.pose.position.z = self.takeoff_altitude + self.target_pose.pose.orientation.w = 1.0 + + self.state = FlightState.NAVIGATING + self.get_logger().info('Returning to local home position') + + def hold_position(self): + if self.current_pose is not None: + self.target_pose = PoseStamped() + self.target_pose.header = self.current_pose.header + self.target_pose.pose = self.current_pose.pose + self.state = FlightState.HOVERING + + def publish_setpoint(self): + if self.target_pose is None: + return + + self.target_pose.header.stamp = self.get_clock().now().to_msg() + self.setpoint_pub.publish(self.target_pose) + + if self.current_pose is not None: + error = np.array([ + self.target_pose.pose.position.x - self.current_pose.pose.position.x, + self.target_pose.pose.position.y - self.current_pose.pose.position.y, + self.target_pose.pose.position.z - self.current_pose.pose.position.z + ]) + distance = np.linalg.norm(error) + + if self.state == FlightState.TAKING_OFF and distance < self.position_tolerance: + self.state = FlightState.HOVERING + self.get_logger().info('Takeoff complete') + elif self.state == FlightState.LANDING and self.current_pose.pose.position.z < 0.2: + self.state = FlightState.ARMED + self.get_logger().info('Landing complete') + + def publish_status(self): + status_msg = String() + armed = 'ARMED' if (self.mavros_state and self.mavros_state.armed) else 'DISARMED' + mode = self.mavros_state.mode if self.mavros_state else 'UNKNOWN' + status_msg.data = f'{self.state.name}|{armed}|{mode}' + self.status_pub.publish(status_msg) + + +def main(args=None): + rclpy.init(args=args) + node = UAVController() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/control/ugv_controller.py b/src/control/ugv_controller.py new file mode 100644 index 0000000..e0f20e2 --- /dev/null +++ b/src/control/ugv_controller.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +"""UGV Controller - Ground vehicle control using local position.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist, PoseStamped +from nav_msgs.msg import Odometry +from std_msgs.msg import String, Bool +import numpy as np +from enum import Enum + + +class UGVState(Enum): + IDLE = 0 + MOVING = 1 + ROTATING = 2 + STOPPED = 3 + + +class UGVController(Node): + + def __init__(self): + super().__init__('ugv_controller') + + self.declare_parameter('max_linear_velocity', 1.0) + self.declare_parameter('max_angular_velocity', 1.5) + self.declare_parameter('kp_linear', 0.8) + self.declare_parameter('kp_angular', 1.5) + self.declare_parameter('position_tolerance', 0.3) + self.declare_parameter('angle_tolerance', 0.1) + + self.max_linear_vel = self.get_parameter('max_linear_velocity').value + self.max_angular_vel = self.get_parameter('max_angular_velocity').value + self.kp_linear = self.get_parameter('kp_linear').value + self.kp_angular = self.get_parameter('kp_angular').value + self.position_tolerance = self.get_parameter('position_tolerance').value + self.angle_tolerance = self.get_parameter('angle_tolerance').value + + self.state = UGVState.IDLE + self.current_odom = None + self.target_pose = None + self.home_position = None + + self.odom_sub = self.create_subscription( + Odometry, '/ugv/odom', self.odom_callback, 10) + + self.goal_sub = self.create_subscription( + PoseStamped, '/ugv/goal_pose', self.goal_callback, 10) + + self.cmd_sub = self.create_subscription( + String, '/ugv/controller/command', self.command_callback, 10) + + self.vel_sub = self.create_subscription( + Twist, '/ugv/cmd_vel_safe', self.velocity_callback, 10) + + self.cmd_vel_pub = self.create_publisher(Twist, '/ugv/cmd_vel', 10) + self.status_pub = self.create_publisher(String, '/ugv/controller/status', 10) + self.goal_reached_pub = self.create_publisher(Bool, '/ugv/goal_reached', 10) + + self.control_timer = self.create_timer(0.05, self.control_loop) + self.status_timer = self.create_timer(0.5, self.publish_status) + + self.get_logger().info('UGV Controller Started - GPS-denied mode') + + def odom_callback(self, msg): + self.current_odom = msg + if self.home_position is None: + self.home_position = np.array([ + msg.pose.pose.position.x, + msg.pose.pose.position.y + ]) + self.get_logger().info(f'Home position set: {self.home_position}') + + def goal_callback(self, msg): + self.target_pose = msg + self.state = UGVState.MOVING + self.get_logger().info( + f'Goal received: [{msg.pose.position.x:.2f}, {msg.pose.position.y:.2f}]') + + def command_callback(self, msg): + cmd = msg.data.lower() + + if cmd == 'stop': + self.stop() + self.state = UGVState.STOPPED + elif cmd == 'resume': + self.state = UGVState.MOVING + elif cmd == 'rtl': + self.return_to_launch() + + def velocity_callback(self, msg): + if self.state == UGVState.MOVING: + self.cmd_vel_pub.publish(msg) + + def control_loop(self): + if self.state != UGVState.MOVING: + return + + if self.current_odom is None or self.target_pose is None: + return + + current_pos = np.array([ + self.current_odom.pose.pose.position.x, + self.current_odom.pose.pose.position.y + ]) + + target_pos = np.array([ + self.target_pose.pose.position.x, + self.target_pose.pose.position.y + ]) + + error = target_pos - current_pos + distance = np.linalg.norm(error) + + if distance < self.position_tolerance: + self.stop() + self.state = UGVState.IDLE + + reached_msg = Bool() + reached_msg.data = True + self.goal_reached_pub.publish(reached_msg) + + self.get_logger().info('Goal reached!') + return + + target_angle = np.arctan2(error[1], error[0]) + + quat = self.current_odom.pose.pose.orientation + current_yaw = np.arctan2( + 2.0 * (quat.w * quat.z + quat.x * quat.y), + 1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z) + ) + + angle_error = self.normalize_angle(target_angle - current_yaw) + + cmd = Twist() + + if abs(angle_error) > self.angle_tolerance: + cmd.angular.z = np.clip( + self.kp_angular * angle_error, + -self.max_angular_vel, + self.max_angular_vel + ) + + if abs(angle_error) < np.pi / 4: + speed = self.kp_linear * distance * (1.0 - abs(angle_error) / np.pi) + cmd.linear.x = np.clip(speed, 0.0, self.max_linear_vel) + else: + cmd.linear.x = np.clip( + self.kp_linear * distance, + 0.0, + self.max_linear_vel + ) + + self.cmd_vel_pub.publish(cmd) + + def stop(self): + cmd = Twist() + self.cmd_vel_pub.publish(cmd) + + def return_to_launch(self): + if self.home_position is not None: + self.target_pose = PoseStamped() + self.target_pose.header.frame_id = 'odom' + self.target_pose.pose.position.x = float(self.home_position[0]) + self.target_pose.pose.position.y = float(self.home_position[1]) + self.target_pose.pose.orientation.w = 1.0 + + self.state = UGVState.MOVING + self.get_logger().info('Returning to local home') + + def normalize_angle(self, angle): + while angle > np.pi: + angle -= 2 * np.pi + while angle < -np.pi: + angle += 2 * np.pi + return angle + + def publish_status(self): + status_msg = String() + status_msg.data = self.state.name + self.status_pub.publish(status_msg) + + +def main(args=None): + rclpy.init(args=args) + node = UGVController() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/localization/__init__.py b/src/localization/__init__.py new file mode 100644 index 0000000..bc24145 --- /dev/null +++ b/src/localization/__init__.py @@ -0,0 +1 @@ +"""Localization module for sensor fusion and position estimation.""" diff --git a/src/localization/ekf_fusion.py b/src/localization/ekf_fusion.py new file mode 100644 index 0000000..293ce7e --- /dev/null +++ b/src/localization/ekf_fusion.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +"""EKF Fusion - Extended Kalman Filter for sensor fusion.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +import numpy as np +from filterpy.kalman import ExtendedKalmanFilter + + +class EKFFusionNode(Node): + + def __init__(self): + super().__init__('ekf_fusion_node') + + self.declare_parameter('process_noise_pos', 0.1) + self.declare_parameter('process_noise_vel', 0.5) + self.declare_parameter('measurement_noise_vo', 0.05) + self.declare_parameter('measurement_noise_of', 0.1) + self.declare_parameter('update_rate', 50.0) + + self.ekf = ExtendedKalmanFilter(dim_x=9, dim_z=6) + + self.ekf.x = np.zeros(9) + + self.ekf.P = np.eye(9) * 1.0 + + process_noise_pos = self.get_parameter('process_noise_pos').value + process_noise_vel = self.get_parameter('process_noise_vel').value + + self.ekf.Q = np.diag([ + process_noise_pos, process_noise_pos, process_noise_pos, + process_noise_vel, process_noise_vel, process_noise_vel, + 0.1, 0.1, 0.1 + ]) + + meas_noise_vo = self.get_parameter('measurement_noise_vo').value + meas_noise_of = self.get_parameter('measurement_noise_of').value + + self.ekf.R = np.diag([ + meas_noise_vo, meas_noise_vo, meas_noise_vo, + meas_noise_of, meas_noise_of, meas_noise_of + ]) + + self.prev_time = None + self.orientation = np.array([0.0, 0.0, 0.0, 1.0]) + + self.vo_sub = self.create_subscription( + PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10) + + self.of_sub = self.create_subscription( + TwistStamped, '/uav/optical_flow/velocity', self.of_callback, 10) + + self.imu_sub = self.create_subscription( + Imu, '/uav/imu/data', self.imu_callback, 10) + + self.pose_pub = self.create_publisher( + PoseWithCovarianceStamped, '/uav/ekf/pose', 10) + + self.odom_pub = self.create_publisher( + Odometry, '/uav/ekf/odom', 10) + + update_rate = self.get_parameter('update_rate').value + self.timer = self.create_timer(1.0 / update_rate, self.predict_step) + + self.get_logger().info('EKF Fusion Node Started') + + def state_transition(self, x, dt): + F = np.eye(9) + F[0, 3] = dt + F[1, 4] = dt + F[2, 5] = dt + F[3, 6] = dt + F[4, 7] = dt + F[5, 8] = dt + return F @ x + + def jacobian_F(self, dt): + F = np.eye(9) + F[0, 3] = dt + F[1, 4] = dt + F[2, 5] = dt + F[3, 6] = dt + F[4, 7] = dt + F[5, 8] = dt + return F + + def predict_step(self): + current_time = self.get_clock().now() + + if self.prev_time is not None: + dt = (current_time.nanoseconds - self.prev_time.nanoseconds) / 1e9 + if dt <= 0 or dt > 1.0: + dt = 0.02 + else: + dt = 0.02 + + self.prev_time = current_time + + F = self.jacobian_F(dt) + self.ekf.F = F + self.ekf.predict() + + self.publish_state() + + def vo_callback(self, msg): + z = np.array([ + msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z, + 0.0, 0.0, 0.0 + ]) + + self.orientation = np.array([ + msg.pose.orientation.x, + msg.pose.orientation.y, + msg.pose.orientation.z, + msg.pose.orientation.w + ]) + + H = np.zeros((6, 9)) + H[0, 0] = 1.0 + H[1, 1] = 1.0 + H[2, 2] = 1.0 + + self.ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x) + + def of_callback(self, msg): + z = np.array([ + self.ekf.x[0], + self.ekf.x[1], + self.ekf.x[2], + msg.twist.linear.x, + msg.twist.linear.y, + 0.0 + ]) + + H = np.zeros((6, 9)) + H[0, 0] = 1.0 + H[1, 1] = 1.0 + H[2, 2] = 1.0 + H[3, 3] = 1.0 + H[4, 4] = 1.0 + + self.ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x) + + def imu_callback(self, msg): + accel = np.array([ + msg.linear_acceleration.x, + msg.linear_acceleration.y, + msg.linear_acceleration.z - 9.81 + ]) + + self.ekf.x[6:9] = accel + + def publish_state(self): + pose_msg = PoseWithCovarianceStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = 'odom' + + pose_msg.pose.pose.position.x = float(self.ekf.x[0]) + pose_msg.pose.pose.position.y = float(self.ekf.x[1]) + pose_msg.pose.pose.position.z = float(self.ekf.x[2]) + + pose_msg.pose.pose.orientation.x = float(self.orientation[0]) + pose_msg.pose.pose.orientation.y = float(self.orientation[1]) + pose_msg.pose.pose.orientation.z = float(self.orientation[2]) + pose_msg.pose.pose.orientation.w = float(self.orientation[3]) + + cov = self.ekf.P[:6, :6].flatten().tolist() + pose_msg.pose.covariance = cov + + self.pose_pub.publish(pose_msg) + + odom_msg = Odometry() + odom_msg.header = pose_msg.header + odom_msg.child_frame_id = 'base_link' + odom_msg.pose = pose_msg.pose + + odom_msg.twist.twist.linear.x = float(self.ekf.x[3]) + odom_msg.twist.twist.linear.y = float(self.ekf.x[4]) + odom_msg.twist.twist.linear.z = float(self.ekf.x[5]) + + self.odom_pub.publish(odom_msg) + + +def main(args=None): + rclpy.init(args=args) + node = EKFFusionNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/localization/landmark_tracker.py b/src/localization/landmark_tracker.py new file mode 100644 index 0000000..cf036fa --- /dev/null +++ b/src/localization/landmark_tracker.py @@ -0,0 +1,165 @@ +#!/usr/bin/env python3 +"""Landmark Tracker - Tracks visual landmarks for position correction.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseArray, PoseStamped, Point +from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import Float32MultiArray +import numpy as np + + +class LandmarkTracker(Node): + + def __init__(self): + super().__init__('landmark_tracker') + + self.declare_parameter('max_landmarks', 50) + self.declare_parameter('matching_threshold', 0.5) + self.declare_parameter('landmark_timeout', 5.0) + + self.max_landmarks = self.get_parameter('max_landmarks').value + self.matching_threshold = self.get_parameter('matching_threshold').value + self.landmark_timeout = self.get_parameter('landmark_timeout').value + + self.landmarks = {} + self.landmark_id_counter = 0 + + self.current_pose = None + + self.detection_sub = self.create_subscription( + PoseArray, '/uav/landmarks/poses', self.detection_callback, 10) + + self.marker_ids_sub = self.create_subscription( + Float32MultiArray, '/uav/landmarks/ids', self.marker_ids_callback, 10) + + self.pose_sub = self.create_subscription( + PoseStamped, '/uav/local_position/pose', self.pose_callback, 10) + + self.landmark_map_pub = self.create_publisher( + MarkerArray, '/landmarks/map', 10) + + self.position_correction_pub = self.create_publisher( + PoseStamped, '/landmarks/position_correction', 10) + + self.timer = self.create_timer(1.0, self.cleanup_landmarks) + self.viz_timer = self.create_timer(0.5, self.publish_landmark_map) + + self.get_logger().info('Landmark Tracker Started') + + def pose_callback(self, msg): + self.current_pose = msg + + def marker_ids_callback(self, msg): + pass + + def detection_callback(self, msg): + if self.current_pose is None: + return + + robot_pos = np.array([ + self.current_pose.pose.position.x, + self.current_pose.pose.position.y, + self.current_pose.pose.position.z + ]) + + for pose in msg.poses: + local_pos = np.array([pose.position.x, pose.position.y, pose.position.z]) + + global_pos = robot_pos + local_pos + + matched_id = self.match_landmark(global_pos) + + if matched_id is not None: + self.update_landmark(matched_id, global_pos) + else: + self.add_landmark(global_pos) + + def match_landmark(self, position): + for lm_id, lm_data in self.landmarks.items(): + distance = np.linalg.norm(position - lm_data['position']) + if distance < self.matching_threshold: + return lm_id + return None + + def add_landmark(self, position): + if len(self.landmarks) >= self.max_landmarks: + oldest_id = min(self.landmarks.keys(), + key=lambda k: self.landmarks[k]['last_seen']) + del self.landmarks[oldest_id] + + self.landmarks[self.landmark_id_counter] = { + 'position': position.copy(), + 'observations': 1, + 'last_seen': self.get_clock().now(), + 'covariance': np.eye(3) * 0.5 + } + self.landmark_id_counter += 1 + + def update_landmark(self, lm_id, position): + lm = self.landmarks[lm_id] + + alpha = 1.0 / (lm['observations'] + 1) + lm['position'] = (1 - alpha) * lm['position'] + alpha * position + lm['observations'] += 1 + lm['last_seen'] = self.get_clock().now() + lm['covariance'] *= 0.95 + + def cleanup_landmarks(self): + current_time = self.get_clock().now() + expired = [] + + for lm_id, lm_data in self.landmarks.items(): + dt = (current_time - lm_data['last_seen']).nanoseconds / 1e9 + if dt > self.landmark_timeout: + expired.append(lm_id) + + for lm_id in expired: + del self.landmarks[lm_id] + + def publish_landmark_map(self): + marker_array = MarkerArray() + + for lm_id, lm_data in self.landmarks.items(): + marker = Marker() + marker.header.stamp = self.get_clock().now().to_msg() + marker.header.frame_id = 'odom' + marker.ns = 'landmarks' + marker.id = lm_id + marker.type = Marker.SPHERE + marker.action = Marker.ADD + + marker.pose.position.x = float(lm_data['position'][0]) + marker.pose.position.y = float(lm_data['position'][1]) + marker.pose.position.z = float(lm_data['position'][2]) + marker.pose.orientation.w = 1.0 + + scale = 0.1 + 0.02 * min(lm_data['observations'], 20) + marker.scale.x = scale + marker.scale.y = scale + marker.scale.z = scale + + marker.color.r = 0.0 + marker.color.g = 0.8 + marker.color.b = 0.2 + marker.color.a = 0.8 + + marker_array.markers.append(marker) + + self.landmark_map_pub.publish(marker_array) + + +def main(args=None): + rclpy.init(args=args) + node = LandmarkTracker() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/localization/position_estimator.py b/src/localization/position_estimator.py new file mode 100644 index 0000000..7f8fe25 --- /dev/null +++ b/src/localization/position_estimator.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +"""Position Estimator - Fuses sensors for local position estimate.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +import numpy as np +from scipy.spatial.transform import Rotation + + +class PositionEstimator(Node): + + def __init__(self): + super().__init__('position_estimator') + + self.declare_parameter('fusion_method', 'weighted_average') + self.declare_parameter('vo_weight', 0.6) + self.declare_parameter('optical_flow_weight', 0.3) + self.declare_parameter('imu_weight', 0.1) + self.declare_parameter('update_rate', 50.0) + + self.fusion_method = self.get_parameter('fusion_method').value + self.vo_weight = self.get_parameter('vo_weight').value + self.of_weight = self.get_parameter('optical_flow_weight').value + self.imu_weight = self.get_parameter('imu_weight').value + + self.position = np.zeros(3) + self.velocity = np.zeros(3) + self.orientation = np.array([0.0, 0.0, 0.0, 1.0]) + + self.vo_pose = None + self.of_velocity = None + self.imu_data = None + + self.prev_time = None + + self.vo_sub = self.create_subscription( + PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10) + + self.of_sub = self.create_subscription( + TwistStamped, '/uav/optical_flow/velocity', self.of_callback, 10) + + self.imu_sub = self.create_subscription( + Imu, '/uav/imu/data', self.imu_callback, 10) + + self.pose_pub = self.create_publisher( + PoseWithCovarianceStamped, '/uav/local_position/pose', 10) + + self.odom_pub = self.create_publisher( + Odometry, '/uav/local_position/odom', 10) + + update_rate = self.get_parameter('update_rate').value + self.timer = self.create_timer(1.0 / update_rate, self.update_estimate) + + self.get_logger().info(f'Position Estimator Started - {self.fusion_method}') + + def vo_callback(self, msg): + self.vo_pose = msg + + def of_callback(self, msg): + self.of_velocity = msg + + def imu_callback(self, msg): + self.imu_data = msg + + def update_estimate(self): + current_time = self.get_clock().now() + + if self.prev_time is not None: + dt = (current_time.nanoseconds - self.prev_time.nanoseconds) / 1e9 + if dt <= 0: + dt = 0.02 + else: + dt = 0.02 + + self.prev_time = current_time + + if self.fusion_method == 'weighted_average': + self.weighted_average_fusion(dt) + else: + self.simple_fusion(dt) + + self.publish_estimate() + + def weighted_average_fusion(self, dt): + total_weight = 0.0 + weighted_pos = np.zeros(3) + weighted_vel = np.zeros(3) + + if self.vo_pose is not None: + vo_pos = np.array([ + self.vo_pose.pose.position.x, + self.vo_pose.pose.position.y, + self.vo_pose.pose.position.z + ]) + weighted_pos += self.vo_weight * vo_pos + total_weight += self.vo_weight + + self.orientation = np.array([ + self.vo_pose.pose.orientation.x, + self.vo_pose.pose.orientation.y, + self.vo_pose.pose.orientation.z, + self.vo_pose.pose.orientation.w + ]) + + if self.of_velocity is not None: + of_vel = np.array([ + self.of_velocity.twist.linear.x, + self.of_velocity.twist.linear.y, + 0.0 + ]) + weighted_vel += self.of_weight * of_vel + + if self.imu_data is not None: + imu_accel = np.array([ + self.imu_data.linear_acceleration.x, + self.imu_data.linear_acceleration.y, + self.imu_data.linear_acceleration.z - 9.81 + ]) + self.velocity += imu_accel * dt * self.imu_weight + + if total_weight > 0: + self.position = weighted_pos / total_weight + + if self.of_velocity is not None: + self.velocity = weighted_vel + + def simple_fusion(self, dt): + if self.vo_pose is not None: + self.position = np.array([ + self.vo_pose.pose.position.x, + self.vo_pose.pose.position.y, + self.vo_pose.pose.position.z + ]) + + def publish_estimate(self): + pose_msg = PoseWithCovarianceStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = 'odom' + + pose_msg.pose.pose.position.x = float(self.position[0]) + pose_msg.pose.pose.position.y = float(self.position[1]) + pose_msg.pose.pose.position.z = float(self.position[2]) + + pose_msg.pose.pose.orientation.x = float(self.orientation[0]) + pose_msg.pose.pose.orientation.y = float(self.orientation[1]) + pose_msg.pose.pose.orientation.z = float(self.orientation[2]) + pose_msg.pose.pose.orientation.w = float(self.orientation[3]) + + pose_msg.pose.covariance = [ + 0.1, 0, 0, 0, 0, 0, + 0, 0.1, 0, 0, 0, 0, + 0, 0, 0.1, 0, 0, 0, + 0, 0, 0, 0.05, 0, 0, + 0, 0, 0, 0, 0.05, 0, + 0, 0, 0, 0, 0, 0.05 + ] + + self.pose_pub.publish(pose_msg) + + odom_msg = Odometry() + odom_msg.header = pose_msg.header + odom_msg.child_frame_id = 'base_link' + odom_msg.pose = pose_msg.pose + + odom_msg.twist.twist.linear.x = float(self.velocity[0]) + odom_msg.twist.twist.linear.y = float(self.velocity[1]) + odom_msg.twist.twist.linear.z = float(self.velocity[2]) + + self.odom_pub.publish(odom_msg) + + +def main(args=None): + rclpy.init(args=args) + node = PositionEstimator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/navigation/__init__.py b/src/navigation/__init__.py new file mode 100644 index 0000000..20b8ecd --- /dev/null +++ b/src/navigation/__init__.py @@ -0,0 +1 @@ +"""Navigation module for local path planning and waypoint following.""" diff --git a/src/navigation/local_planner.py b/src/navigation/local_planner.py new file mode 100644 index 0000000..3db426f --- /dev/null +++ b/src/navigation/local_planner.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +"""Local Planner - Path planning using relative coordinates only.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, Twist, Point +from nav_msgs.msg import Path, Odometry +from visualization_msgs.msg import Marker +import numpy as np + + +class LocalPlanner(Node): + + def __init__(self): + super().__init__('local_planner') + + self.declare_parameter('max_velocity', 2.0) + self.declare_parameter('max_acceleration', 1.0) + self.declare_parameter('lookahead_distance', 1.0) + self.declare_parameter('goal_tolerance', 0.3) + + self.max_velocity = self.get_parameter('max_velocity').value + self.max_acceleration = self.get_parameter('max_acceleration').value + self.lookahead_distance = self.get_parameter('lookahead_distance').value + self.goal_tolerance = self.get_parameter('goal_tolerance').value + + self.current_pose = None + self.current_velocity = np.zeros(3) + self.path = [] + self.current_waypoint_idx = 0 + + self.odom_sub = self.create_subscription( + Odometry, '/uav/local_position/odom', self.odom_callback, 10) + + self.goal_sub = self.create_subscription( + PoseStamped, '/uav/goal_pose', self.goal_callback, 10) + + self.path_sub = self.create_subscription( + Path, '/uav/planned_path', self.path_callback, 10) + + self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10) + self.setpoint_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10) + self.path_viz_pub = self.create_publisher(Path, '/uav/local_path', 10) + + self.timer = self.create_timer(0.05, self.control_loop) + + self.get_logger().info('Local Planner Started - GPS-denied navigation') + + def odom_callback(self, msg): + self.current_pose = msg.pose.pose + self.current_velocity = np.array([ + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z + ]) + + def goal_callback(self, msg): + goal = np.array([ + msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z + ]) + self.path = [goal] + self.current_waypoint_idx = 0 + self.get_logger().info(f'New goal received: [{goal[0]:.2f}, {goal[1]:.2f}, {goal[2]:.2f}]') + + def path_callback(self, msg): + self.path = [] + for pose_stamped in msg.poses: + waypoint = np.array([ + pose_stamped.pose.position.x, + pose_stamped.pose.position.y, + pose_stamped.pose.position.z + ]) + self.path.append(waypoint) + self.current_waypoint_idx = 0 + self.get_logger().info(f'Path received with {len(self.path)} waypoints') + + def control_loop(self): + if self.current_pose is None or len(self.path) == 0: + return + + if self.current_waypoint_idx >= len(self.path): + self.stop() + return + + current_pos = np.array([ + self.current_pose.position.x, + self.current_pose.position.y, + self.current_pose.position.z + ]) + + target = self.path[self.current_waypoint_idx] + distance = np.linalg.norm(target - current_pos) + + if distance < self.goal_tolerance: + self.current_waypoint_idx += 1 + if self.current_waypoint_idx >= len(self.path): + self.get_logger().info('Path complete!') + self.stop() + return + target = self.path[self.current_waypoint_idx] + + direction = target - current_pos + distance = np.linalg.norm(direction) + + if distance > 0: + direction = direction / distance + + speed = min(self.max_velocity, distance * 0.5) + velocity = direction * speed + + self.publish_command(velocity, target) + + def publish_command(self, velocity, target): + cmd = Twist() + cmd.linear.x = float(velocity[0]) + cmd.linear.y = float(velocity[1]) + cmd.linear.z = float(velocity[2]) + self.cmd_vel_pub.publish(cmd) + + setpoint = PoseStamped() + setpoint.header.stamp = self.get_clock().now().to_msg() + setpoint.header.frame_id = 'odom' + setpoint.pose.position.x = float(target[0]) + setpoint.pose.position.y = float(target[1]) + setpoint.pose.position.z = float(target[2]) + setpoint.pose.orientation.w = 1.0 + self.setpoint_pub.publish(setpoint) + + def stop(self): + cmd = Twist() + self.cmd_vel_pub.publish(cmd) + + +def main(args=None): + rclpy.init(args=args) + node = LocalPlanner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/navigation/obstacle_avoidance.py b/src/navigation/obstacle_avoidance.py new file mode 100644 index 0000000..b301db0 --- /dev/null +++ b/src/navigation/obstacle_avoidance.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +"""Obstacle Avoidance - Vision-based obstacle detection and avoidance.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image, LaserScan +from geometry_msgs.msg import Twist, TwistStamped, Vector3 +from std_msgs.msg import Bool +from cv_bridge import CvBridge +import cv2 +import numpy as np + + +class ObstacleAvoidance(Node): + + def __init__(self): + super().__init__('obstacle_avoidance') + + self.bridge = CvBridge() + + self.declare_parameter('detection_range', 5.0) + self.declare_parameter('safety_margin', 1.0) + self.declare_parameter('avoidance_gain', 1.0) + self.declare_parameter('enable', True) + + self.detection_range = self.get_parameter('detection_range').value + self.safety_margin = self.get_parameter('safety_margin').value + self.avoidance_gain = self.get_parameter('avoidance_gain').value + self.enabled = self.get_parameter('enable').value + + self.obstacle_detected = False + self.obstacle_direction = np.zeros(3) + self.obstacle_distance = float('inf') + + self.depth_sub = self.create_subscription( + Image, '/uav/camera/depth/image_raw', self.depth_callback, 10) + + self.scan_sub = self.create_subscription( + LaserScan, '/uav/scan', self.scan_callback, 10) + + self.cmd_vel_sub = self.create_subscription( + Twist, '/uav/cmd_vel', self.cmd_vel_callback, 10) + + self.enable_sub = self.create_subscription( + Bool, '/obstacle_avoidance/enable', self.enable_callback, 10) + + self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel_safe', 10) + self.obstacle_pub = self.create_publisher(Bool, '/obstacle_avoidance/obstacle_detected', 10) + self.avoidance_vec_pub = self.create_publisher(Vector3, '/obstacle_avoidance/avoidance_vector', 10) + + self.current_cmd = Twist() + + self.timer = self.create_timer(0.05, self.avoidance_loop) + + self.get_logger().info('Obstacle Avoidance Node Started') + + def enable_callback(self, msg): + self.enabled = msg.data + + def depth_callback(self, msg): + try: + depth_image = self.bridge.imgmsg_to_cv2(msg, 'passthrough') + + height, width = depth_image.shape + center_region = depth_image[height//3:2*height//3, width//3:2*width//3] + + valid_depths = center_region[center_region > 0] + if len(valid_depths) > 0: + min_distance = np.min(valid_depths) + + if min_distance < self.detection_range: + self.obstacle_detected = True + self.obstacle_distance = min_distance + + left_region = depth_image[:, :width//3] + right_region = depth_image[:, 2*width//3:] + + left_clear = np.mean(left_region[left_region > 0]) if np.any(left_region > 0) else float('inf') + right_clear = np.mean(right_region[right_region > 0]) if np.any(right_region > 0) else float('inf') + + if left_clear > right_clear: + self.obstacle_direction = np.array([0.0, 1.0, 0.0]) + else: + self.obstacle_direction = np.array([0.0, -1.0, 0.0]) + else: + self.obstacle_detected = False + self.obstacle_distance = float('inf') + + except Exception as e: + self.get_logger().error(f'Depth processing error: {e}') + + def scan_callback(self, msg): + ranges = np.array(msg.ranges) + valid_ranges = ranges[np.isfinite(ranges)] + + if len(valid_ranges) > 0: + min_range = np.min(valid_ranges) + min_idx = np.argmin(ranges) + + if min_range < self.detection_range: + self.obstacle_detected = True + self.obstacle_distance = min_range + + angle = msg.angle_min + min_idx * msg.angle_increment + self.obstacle_direction = np.array([ + -np.cos(angle), + -np.sin(angle), + 0.0 + ]) + else: + self.obstacle_detected = False + + def cmd_vel_callback(self, msg): + self.current_cmd = msg + + def avoidance_loop(self): + obstacle_msg = Bool() + obstacle_msg.data = self.obstacle_detected + self.obstacle_pub.publish(obstacle_msg) + + if not self.enabled: + self.cmd_vel_pub.publish(self.current_cmd) + return + + safe_cmd = Twist() + + if self.obstacle_detected and self.obstacle_distance < self.safety_margin: + repulsion = self.avoidance_gain * (self.safety_margin / self.obstacle_distance) + avoidance = self.obstacle_direction * repulsion + + safe_cmd.linear.x = self.current_cmd.linear.x + float(avoidance[0]) + safe_cmd.linear.y = self.current_cmd.linear.y + float(avoidance[1]) + safe_cmd.linear.z = self.current_cmd.linear.z + float(avoidance[2]) + safe_cmd.angular = self.current_cmd.angular + + vec_msg = Vector3() + vec_msg.x = float(avoidance[0]) + vec_msg.y = float(avoidance[1]) + vec_msg.z = float(avoidance[2]) + self.avoidance_vec_pub.publish(vec_msg) + else: + safe_cmd = self.current_cmd + + self.cmd_vel_pub.publish(safe_cmd) + + +def main(args=None): + rclpy.init(args=args) + node = ObstacleAvoidance() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/navigation/waypoint_follower.py b/src/navigation/waypoint_follower.py new file mode 100644 index 0000000..1be97df --- /dev/null +++ b/src/navigation/waypoint_follower.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +"""Waypoint Follower - Follows relative waypoints without GPS.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, Twist +from nav_msgs.msg import Odometry, Path +from std_msgs.msg import Int32, String, Bool +import numpy as np +from enum import Enum + + +class WaypointState(Enum): + IDLE = 0 + NAVIGATING = 1 + REACHED = 2 + PAUSED = 3 + + +class WaypointFollower(Node): + + def __init__(self): + super().__init__('waypoint_follower') + + self.declare_parameter('waypoint_radius', 0.5) + self.declare_parameter('max_velocity', 2.0) + self.declare_parameter('kp_linear', 0.8) + self.declare_parameter('kp_angular', 1.0) + + self.waypoint_radius = self.get_parameter('waypoint_radius').value + self.max_velocity = self.get_parameter('max_velocity').value + self.kp_linear = self.get_parameter('kp_linear').value + self.kp_angular = self.get_parameter('kp_angular').value + + self.waypoints = [] + self.current_idx = 0 + self.state = WaypointState.IDLE + self.current_pose = None + + self.odom_sub = self.create_subscription( + Odometry, '/uav/local_position/odom', self.odom_callback, 10) + + self.waypoint_sub = self.create_subscription( + PoseStamped, '/waypoint_follower/add_waypoint', self.add_waypoint_callback, 10) + + self.path_sub = self.create_subscription( + Path, '/waypoint_follower/set_path', self.set_path_callback, 10) + + self.command_sub = self.create_subscription( + String, '/waypoint_follower/command', self.command_callback, 10) + + self.cmd_vel_pub = self.create_publisher(Twist, '/uav/cmd_vel', 10) + self.current_waypoint_pub = self.create_publisher(Int32, '/waypoint_follower/current_index', 10) + self.status_pub = self.create_publisher(String, '/waypoint_follower/status', 10) + self.reached_pub = self.create_publisher(Bool, '/waypoint_follower/waypoint_reached', 10) + + self.timer = self.create_timer(0.05, self.control_loop) + + self.get_logger().info('Waypoint Follower Started - Using LOCAL coordinates only') + + def odom_callback(self, msg): + self.current_pose = msg.pose.pose + + def add_waypoint_callback(self, msg): + waypoint = np.array([ + msg.pose.position.x, + msg.pose.position.y, + msg.pose.position.z + ]) + self.waypoints.append(waypoint) + self.get_logger().info(f'Added waypoint {len(self.waypoints)}: {waypoint}') + + def set_path_callback(self, msg): + self.waypoints = [] + for pose_stamped in msg.poses: + waypoint = np.array([ + pose_stamped.pose.position.x, + pose_stamped.pose.position.y, + pose_stamped.pose.position.z + ]) + self.waypoints.append(waypoint) + self.current_idx = 0 + self.get_logger().info(f'Set path with {len(self.waypoints)} waypoints') + + def command_callback(self, msg): + cmd = msg.data.lower() + + if cmd == 'start': + if len(self.waypoints) > 0: + self.state = WaypointState.NAVIGATING + self.get_logger().info('Starting waypoint navigation') + elif cmd == 'pause': + self.state = WaypointState.PAUSED + self.stop() + elif cmd == 'resume': + self.state = WaypointState.NAVIGATING + elif cmd == 'stop': + self.state = WaypointState.IDLE + self.stop() + elif cmd == 'clear': + self.waypoints = [] + self.current_idx = 0 + self.state = WaypointState.IDLE + elif cmd == 'next': + if self.current_idx < len(self.waypoints) - 1: + self.current_idx += 1 + elif cmd == 'prev': + if self.current_idx > 0: + self.current_idx -= 1 + + def control_loop(self): + self.publish_status() + + if self.state != WaypointState.NAVIGATING: + return + + if self.current_pose is None or len(self.waypoints) == 0: + return + + if self.current_idx >= len(self.waypoints): + self.state = WaypointState.IDLE + self.stop() + self.get_logger().info('All waypoints reached!') + return + + current_pos = np.array([ + self.current_pose.position.x, + self.current_pose.position.y, + self.current_pose.position.z + ]) + + target = self.waypoints[self.current_idx] + error = target - current_pos + distance = np.linalg.norm(error) + + if distance < self.waypoint_radius: + reached_msg = Bool() + reached_msg.data = True + self.reached_pub.publish(reached_msg) + + self.get_logger().info(f'Reached waypoint {self.current_idx + 1}/{len(self.waypoints)}') + self.current_idx += 1 + return + + direction = error / distance + speed = min(self.kp_linear * distance, self.max_velocity) + velocity = direction * speed + + cmd = Twist() + cmd.linear.x = float(velocity[0]) + cmd.linear.y = float(velocity[1]) + cmd.linear.z = float(velocity[2]) + + self.cmd_vel_pub.publish(cmd) + + def stop(self): + cmd = Twist() + self.cmd_vel_pub.publish(cmd) + + def publish_status(self): + idx_msg = Int32() + idx_msg.data = self.current_idx + self.current_waypoint_pub.publish(idx_msg) + + status_msg = String() + status_msg.data = f'{self.state.name}|{self.current_idx}/{len(self.waypoints)}' + self.status_pub.publish(status_msg) + + +def main(args=None): + rclpy.init(args=args) + node = WaypointFollower() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/nodes/__init__.py b/src/nodes/__init__.py new file mode 100644 index 0000000..2c805ab --- /dev/null +++ b/src/nodes/__init__.py @@ -0,0 +1 @@ +"""ROS 2 Node implementations.""" diff --git a/src/nodes/geofence_node.py b/src/nodes/geofence_node.py new file mode 100644 index 0000000..a41194e --- /dev/null +++ b/src/nodes/geofence_node.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 +"""Geofence Node - ROS 2 wrapper.""" + +from src.safety.geofence_monitor import GeofenceMonitor, main + +if __name__ == '__main__': + main() diff --git a/src/nodes/multi_vehicle_coord.py b/src/nodes/multi_vehicle_coord.py new file mode 100644 index 0000000..59f6c4b --- /dev/null +++ b/src/nodes/multi_vehicle_coord.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python3 +"""Multi-Vehicle Coordination Node.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, TwistStamped +from std_msgs.msg import String, Bool +import numpy as np + + +class MultiVehicleCoordinator(Node): + + def __init__(self): + super().__init__('multi_vehicle_coord') + + self.declare_parameter('min_separation', 3.0) + self.declare_parameter('coordination_mode', 'leader_follower') + + self.min_separation = self.get_parameter('min_separation').value + self.coord_mode = self.get_parameter('coordination_mode').value + + self.uav_pose = None + self.ugv_pose = None + + self.uav_pose_sub = self.create_subscription( + PoseStamped, '/uav/local_position/pose', self.uav_pose_callback, 10) + + self.ugv_pose_sub = self.create_subscription( + PoseStamped, '/ugv/local_position/pose', self.ugv_pose_callback, 10) + + self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10) + self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10) + self.uav_goal_pub = self.create_publisher(PoseStamped, '/uav/setpoint_position', 10) + self.ugv_goal_pub = self.create_publisher(PoseStamped, '/ugv/goal_pose', 10) + + self.collision_warning_pub = self.create_publisher(Bool, '/coordination/collision_warning', 10) + self.status_pub = self.create_publisher(String, '/coordination/status', 10) + + self.timer = self.create_timer(0.1, self.coordination_loop) + + self.get_logger().info(f'Multi-Vehicle Coordinator Started - Mode: {self.coord_mode}') + + def uav_pose_callback(self, msg): + self.uav_pose = msg + + def ugv_pose_callback(self, msg): + self.ugv_pose = msg + + def coordination_loop(self): + if self.uav_pose is None or self.ugv_pose is None: + return + + uav_pos = np.array([ + self.uav_pose.pose.position.x, + self.uav_pose.pose.position.y, + self.uav_pose.pose.position.z + ]) + + ugv_pos = np.array([ + self.ugv_pose.pose.position.x, + self.ugv_pose.pose.position.y, + 0.0 + ]) + + horizontal_distance = np.linalg.norm(uav_pos[:2] - ugv_pos[:2]) + + collision_warning = Bool() + collision_warning.data = horizontal_distance < self.min_separation + self.collision_warning_pub.publish(collision_warning) + + if collision_warning.data: + self.get_logger().warn(f'Vehicles too close: {horizontal_distance:.2f}m') + + status = String() + status.data = f'separation:{horizontal_distance:.2f}|mode:{self.coord_mode}' + self.status_pub.publish(status) + + if self.coord_mode == 'leader_follower': + self.leader_follower_control(uav_pos, ugv_pos) + elif self.coord_mode == 'formation': + self.formation_control(uav_pos, ugv_pos) + + def leader_follower_control(self, uav_pos, ugv_pos): + offset = np.array([0.0, 0.0, 5.0]) + target_uav_pos = ugv_pos + offset + + goal = PoseStamped() + goal.header.stamp = self.get_clock().now().to_msg() + goal.header.frame_id = 'odom' + goal.pose.position.x = float(target_uav_pos[0]) + goal.pose.position.y = float(target_uav_pos[1]) + goal.pose.position.z = float(target_uav_pos[2]) + goal.pose.orientation.w = 1.0 + + def formation_control(self, uav_pos, ugv_pos): + pass + + +def main(args=None): + rclpy.init(args=args) + node = MultiVehicleCoordinator() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/nodes/vision_nav_node.py b/src/nodes/vision_nav_node.py new file mode 100644 index 0000000..b69005a --- /dev/null +++ b/src/nodes/vision_nav_node.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 +"""Vision-Based Navigation Node.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from geometry_msgs.msg import PoseStamped, TwistStamped +from mavros_msgs.srv import CommandBool, SetMode +from mavros_msgs.msg import State +from cv_bridge import CvBridge +import numpy as np + + +class VisionNavNode(Node): + + def __init__(self): + super().__init__('vision_nav_node') + + self.bridge = CvBridge() + + self.current_image = None + self.current_local_pose = None + self.current_vo_pose = None + self.is_armed = False + self.current_mode = "" + + self.waypoints = [ + {'x': 0.0, 'y': 0.0, 'z': 5.0}, + {'x': 10.0, 'y': 0.0, 'z': 5.0}, + {'x': 10.0, 'y': 10.0, 'z': 5.0}, + {'x': 0.0, 'y': 10.0, 'z': 5.0}, + {'x': 0.0, 'y': 0.0, 'z': 5.0}, + {'x': 0.0, 'y': 0.0, 'z': 0.0}, + ] + + self.current_waypoint_idx = 0 + self.waypoint_tolerance = 0.5 + + self.image_sub = self.create_subscription( + Image, '/uav/camera/forward/image_raw', self.image_callback, 10) + + self.local_pose_sub = self.create_subscription( + PoseStamped, '/uav/mavros/local_position/pose', self.local_pose_callback, 10) + + self.vo_pose_sub = self.create_subscription( + PoseStamped, '/uav/visual_odometry/pose', self.vo_pose_callback, 10) + + self.state_sub = self.create_subscription( + State, '/uav/mavros/state', self.state_callback, 10) + + self.setpoint_local_pub = self.create_publisher( + PoseStamped, '/uav/mavros/setpoint_position/local', 10) + + self.setpoint_vel_pub = self.create_publisher( + TwistStamped, '/uav/mavros/setpoint_velocity/cmd_vel', 10) + + self.arming_client = self.create_client(CommandBool, '/uav/mavros/cmd/arming') + self.set_mode_client = self.create_client(SetMode, '/uav/mavros/set_mode') + + self.control_timer = self.create_timer(0.05, self.control_loop) + self.setpoint_timer = self.create_timer(0.1, self.publish_setpoint) + + self.get_logger().info('Vision Navigation Node Started - GPS-DENIED mode') + + def image_callback(self, msg): + try: + self.current_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + except Exception as e: + self.get_logger().error(f'Image conversion failed: {e}') + + def local_pose_callback(self, msg): + self.current_local_pose = msg + + def vo_pose_callback(self, msg): + self.current_vo_pose = msg + + def state_callback(self, msg): + self.is_armed = msg.armed + self.current_mode = msg.mode + + def publish_setpoint(self): + if self.current_waypoint_idx >= len(self.waypoints): + return + + waypoint = self.waypoints[self.current_waypoint_idx] + + setpoint = PoseStamped() + setpoint.header.stamp = self.get_clock().now().to_msg() + setpoint.header.frame_id = 'map' + + setpoint.pose.position.x = waypoint['x'] + setpoint.pose.position.y = waypoint['y'] + setpoint.pose.position.z = waypoint['z'] + setpoint.pose.orientation.w = 1.0 + + self.setpoint_local_pub.publish(setpoint) + + def control_loop(self): + if self.current_local_pose is None: + return + + if self.current_waypoint_idx < len(self.waypoints): + waypoint = self.waypoints[self.current_waypoint_idx] + + dx = waypoint['x'] - self.current_local_pose.pose.position.x + dy = waypoint['y'] - self.current_local_pose.pose.position.y + dz = waypoint['z'] - self.current_local_pose.pose.position.z + + distance = np.sqrt(dx**2 + dy**2 + dz**2) + + if distance < self.waypoint_tolerance: + self.get_logger().info( + f'Reached waypoint {self.current_waypoint_idx}: ' + f'[{waypoint["x"]}, {waypoint["y"]}, {waypoint["z"]}] (LOCAL coordinates)') + self.current_waypoint_idx += 1 + + if self.current_waypoint_idx >= len(self.waypoints): + self.get_logger().info('Mission complete!') + + +def main(args=None): + rclpy.init(args=args) + node = VisionNavNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/nodes/visual_odom_node.py b/src/nodes/visual_odom_node.py new file mode 100644 index 0000000..7f8bbe6 --- /dev/null +++ b/src/nodes/visual_odom_node.py @@ -0,0 +1,7 @@ +#!/usr/bin/env python3 +"""Visual Odometry Node - ROS 2 wrapper.""" + +from src.vision.visual_odometry import VisualOdometryNode, main + +if __name__ == '__main__': + main() diff --git a/src/safety/__init__.py b/src/safety/__init__.py new file mode 100644 index 0000000..be85e0f --- /dev/null +++ b/src/safety/__init__.py @@ -0,0 +1 @@ +"""Safety module for geofencing and failsafe handling.""" diff --git a/src/safety/failsafe_handler.py b/src/safety/failsafe_handler.py new file mode 100644 index 0000000..04d3ac6 --- /dev/null +++ b/src/safety/failsafe_handler.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python3 +"""Failsafe Handler - Emergency procedures for vision loss and other failures.""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, TwistStamped +from std_msgs.msg import String, Bool +from sensor_msgs.msg import BatteryState +from enum import Enum + + +class FailsafeState(Enum): + NORMAL = 0 + VISION_LOSS = 1 + LOW_BATTERY = 2 + GEOFENCE_BREACH = 3 + COMM_LOSS = 4 + EMERGENCY = 5 + + +class FailsafeHandler(Node): + + def __init__(self): + super().__init__('failsafe_handler') + + self.declare_parameter('vision_loss_timeout', 5.0) + self.declare_parameter('low_battery_threshold', 20.0) + self.declare_parameter('critical_battery_threshold', 10.0) + self.declare_parameter('action_on_vision_loss', 'HOLD') + self.declare_parameter('action_on_low_battery', 'RTL') + self.declare_parameter('action_on_critical_battery', 'LAND') + + self.vision_timeout = self.get_parameter('vision_loss_timeout').value + self.low_battery = self.get_parameter('low_battery_threshold').value + self.critical_battery = self.get_parameter('critical_battery_threshold').value + self.vision_loss_action = self.get_parameter('action_on_vision_loss').value + self.low_battery_action = self.get_parameter('action_on_low_battery').value + self.critical_battery_action = self.get_parameter('action_on_critical_battery').value + + self.state = FailsafeState.NORMAL + self.last_vo_time = None + self.battery_percent = 100.0 + self.geofence_ok = True + + self.vision_triggered = False + self.battery_triggered = False + + self.vo_pose_sub = self.create_subscription( + PoseStamped, '/uav/visual_odometry/pose', self.vo_callback, 10) + + self.battery_sub = self.create_subscription( + BatteryState, '/uav/battery', self.battery_callback, 10) + + self.geofence_sub = self.create_subscription( + Bool, '/geofence/status', self.geofence_callback, 10) + + self.geofence_action_sub = self.create_subscription( + String, '/geofence/action', self.geofence_action_callback, 10) + + self.uav_cmd_pub = self.create_publisher(String, '/uav/controller/command', 10) + self.ugv_cmd_pub = self.create_publisher(String, '/ugv/controller/command', 10) + self.failsafe_status_pub = self.create_publisher(String, '/failsafe/status', 10) + self.failsafe_active_pub = self.create_publisher(Bool, '/failsafe/active', 10) + + self.check_timer = self.create_timer(0.5, self.check_failsafes) + + self.get_logger().info('Failsafe Handler Started') + + def vo_callback(self, msg): + self.last_vo_time = self.get_clock().now() + + if self.state == FailsafeState.VISION_LOSS: + self.state = FailsafeState.NORMAL + self.vision_triggered = False + self.get_logger().info('Vision restored - returning to normal') + + def battery_callback(self, msg): + self.battery_percent = msg.percentage * 100.0 + + def geofence_callback(self, msg): + self.geofence_ok = msg.data + + def geofence_action_callback(self, msg): + self.state = FailsafeState.GEOFENCE_BREACH + self.execute_action(msg.data) + + def check_failsafes(self): + if self.last_vo_time is not None: + elapsed = (self.get_clock().now() - self.last_vo_time).nanoseconds / 1e9 + if elapsed > self.vision_timeout and not self.vision_triggered: + self.state = FailsafeState.VISION_LOSS + self.vision_triggered = True + self.get_logger().error(f'VISION LOSS - No VO for {elapsed:.1f}s') + self.execute_action(self.vision_loss_action) + + if self.battery_percent <= self.critical_battery and not self.battery_triggered: + self.state = FailsafeState.LOW_BATTERY + self.battery_triggered = True + self.get_logger().error(f'CRITICAL BATTERY: {self.battery_percent:.1f}%') + self.execute_action(self.critical_battery_action) + elif self.battery_percent <= self.low_battery and not self.battery_triggered: + self.state = FailsafeState.LOW_BATTERY + self.battery_triggered = True + self.get_logger().warn(f'LOW BATTERY: {self.battery_percent:.1f}%') + self.execute_action(self.low_battery_action) + + self.publish_status() + + def execute_action(self, action): + cmd_msg = String() + + if action == 'HOLD': + cmd_msg.data = 'hold' + elif action == 'RTL': + cmd_msg.data = 'rtl' + elif action == 'LAND': + cmd_msg.data = 'land' + elif action == 'STOP': + cmd_msg.data = 'stop' + else: + cmd_msg.data = 'hold' + + self.uav_cmd_pub.publish(cmd_msg) + + ugv_cmd = String() + ugv_cmd.data = 'stop' + self.ugv_cmd_pub.publish(ugv_cmd) + + self.get_logger().warn(f'Failsafe action executed: {action}') + + def publish_status(self): + status_msg = String() + status_msg.data = f'{self.state.name}|battery:{self.battery_percent:.1f}|geofence:{self.geofence_ok}' + self.failsafe_status_pub.publish(status_msg) + + active_msg = Bool() + active_msg.data = self.state != FailsafeState.NORMAL + self.failsafe_active_pub.publish(active_msg) + + +def main(args=None): + rclpy.init(args=args) + node = FailsafeHandler() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/safety/geofence_monitor.py b/src/safety/geofence_monitor.py new file mode 100644 index 0000000..ed8041a --- /dev/null +++ b/src/safety/geofence_monitor.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 +"""Geofence Monitor - GPS ONLY used for safety boundaries.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import Point +from std_msgs.msg import Bool, String +from visualization_msgs.msg import Marker +import numpy as np +from shapely.geometry import Point as ShapelyPoint, Polygon + + +class GeofenceMonitor(Node): + + def __init__(self): + super().__init__('geofence_monitor') + + self.declare_parameter('fence_enabled', True) + self.declare_parameter('fence_type', 'polygon') + self.declare_parameter('warning_distance', 10.0) + self.declare_parameter('breach_action', 'RTL') + self.declare_parameter('min_altitude', 0.0) + self.declare_parameter('max_altitude', 50.0) + self.declare_parameter('consecutive_breaches_required', 3) + + self.fence_enabled = self.get_parameter('fence_enabled').value + self.fence_type = self.get_parameter('fence_type').value + self.warning_distance = self.get_parameter('warning_distance').value + self.breach_action = self.get_parameter('breach_action').value + self.min_altitude = self.get_parameter('min_altitude').value + self.max_altitude = self.get_parameter('max_altitude').value + self.breaches_required = self.get_parameter('consecutive_breaches_required').value + + self.fence_points = [ + (47.397742, 8.545594), + (47.398242, 8.545594), + (47.398242, 8.546094), + (47.397742, 8.546094), + ] + self.fence_polygon = Polygon(self.fence_points) + + self.current_position = None + self.inside_fence = True + self.breach_count = 0 + + self.gps_sub = self.create_subscription( + NavSatFix, '/uav/mavros/global_position/global', self.gps_callback, 10) + + self.fence_status_pub = self.create_publisher(Bool, '/geofence/status', 10) + self.fence_action_pub = self.create_publisher(String, '/geofence/action', 10) + self.fence_viz_pub = self.create_publisher(Marker, '/geofence/visualization', 10) + self.warning_pub = self.create_publisher(String, '/geofence/warning', 10) + + self.viz_timer = self.create_timer(1.0, self.publish_visualization) + + self.get_logger().info('Geofence Monitor Started - GPS ONLY for safety boundaries') + self.get_logger().info(f'Fence type: {self.fence_type}, Action: {self.breach_action}') + + def gps_callback(self, msg): + if not self.fence_enabled: + return + + self.current_position = (msg.latitude, msg.longitude, msg.altitude) + + point = ShapelyPoint(msg.latitude, msg.longitude) + inside_horizontal = self.fence_polygon.contains(point) + inside_vertical = self.min_altitude <= msg.altitude <= self.max_altitude + + self.inside_fence = inside_horizontal and inside_vertical + + status_msg = Bool() + status_msg.data = self.inside_fence + self.fence_status_pub.publish(status_msg) + + if not self.inside_fence: + self.breach_count += 1 + + if not inside_horizontal: + distance = self.fence_polygon.exterior.distance(point) * 111000 + self.get_logger().warn(f'GEOFENCE: Outside boundary by {distance:.1f}m') + + if not inside_vertical: + if msg.altitude < self.min_altitude: + self.get_logger().warn(f'GEOFENCE: Below min altitude ({msg.altitude:.1f}m)') + else: + self.get_logger().warn(f'GEOFENCE: Above max altitude ({msg.altitude:.1f}m)') + + if self.breach_count >= self.breaches_required: + self.trigger_breach_action() + else: + self.breach_count = 0 + + if inside_horizontal: + distance_to_edge = self.fence_polygon.exterior.distance(point) * 111000 + if distance_to_edge < self.warning_distance: + warning_msg = String() + warning_msg.data = f'Approaching boundary: {distance_to_edge:.1f}m remaining' + self.warning_pub.publish(warning_msg) + + def trigger_breach_action(self): + action_msg = String() + action_msg.data = self.breach_action + self.fence_action_pub.publish(action_msg) + + self.get_logger().error(f'GEOFENCE BREACH ACTION: {self.breach_action}') + self.breach_count = 0 + + def publish_visualization(self): + if not self.fence_enabled: + return + + marker = Marker() + marker.header.stamp = self.get_clock().now().to_msg() + marker.header.frame_id = 'map' + marker.ns = 'geofence' + marker.id = 0 + marker.type = Marker.LINE_STRIP + marker.action = Marker.ADD + + marker.scale.x = 2.0 + + if self.inside_fence: + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + else: + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 0.8 + + for lat, lon in self.fence_points: + p = Point() + p.x = (lon - self.fence_points[0][1]) * 111000 + p.y = (lat - self.fence_points[0][0]) * 111000 + p.z = 0.0 + marker.points.append(p) + + p = Point() + p.x = 0.0 + p.y = 0.0 + p.z = 0.0 + marker.points.append(p) + + self.fence_viz_pub.publish(marker) + + +def main(args=None): + rclpy.init(args=args) + node = GeofenceMonitor() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/utils/__init__.py b/src/utils/__init__.py new file mode 100644 index 0000000..e40643e --- /dev/null +++ b/src/utils/__init__.py @@ -0,0 +1 @@ +"""Utility functions and helpers.""" diff --git a/src/utils/helpers.py b/src/utils/helpers.py new file mode 100644 index 0000000..8c3d7db --- /dev/null +++ b/src/utils/helpers.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python3 +"""Utility helper functions.""" + +import numpy as np +import yaml +from pathlib import Path + + +def load_yaml_config(filepath): + with open(filepath, 'r') as f: + return yaml.safe_load(f) + + +def save_yaml_config(data, filepath): + with open(filepath, 'w') as f: + yaml.dump(data, f, default_flow_style=False) + + +def clamp(value, min_val, max_val): + return max(min_val, min(value, max_val)) + + +def lerp(a, b, t): + return a + (b - a) * clamp(t, 0.0, 1.0) + + +def smooth_step(edge0, edge1, x): + t = clamp((x - edge0) / (edge1 - edge0), 0.0, 1.0) + return t * t * (3.0 - 2.0 * t) + + +def distance_2d(p1, p2): + return np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2) + + +def distance_3d(p1, p2): + return np.linalg.norm(np.array(p2) - np.array(p1)) + + +def moving_average(values, window_size): + if len(values) < window_size: + return np.mean(values) + return np.mean(values[-window_size:]) + + +class RateLimiter: + def __init__(self, max_rate): + self.max_rate = max_rate + self.last_value = 0.0 + + def apply(self, value, dt): + max_change = self.max_rate * dt + change = clamp(value - self.last_value, -max_change, max_change) + self.last_value += change + return self.last_value + + +class LowPassFilter: + def __init__(self, alpha=0.1): + self.alpha = alpha + self.value = None + + def apply(self, new_value): + if self.value is None: + self.value = new_value + else: + self.value = self.alpha * new_value + (1 - self.alpha) * self.value + return self.value + + def reset(self): + self.value = None + + +class PIDController: + def __init__(self, kp, ki, kd, output_limits=None): + self.kp = kp + self.ki = ki + self.kd = kd + self.output_limits = output_limits + + self.integral = 0.0 + self.last_error = 0.0 + self.last_time = None + + def compute(self, error, current_time): + if self.last_time is None: + dt = 0.0 + else: + dt = current_time - self.last_time + + self.integral += error * dt + + if dt > 0: + derivative = (error - self.last_error) / dt + else: + derivative = 0.0 + + output = self.kp * error + self.ki * self.integral + self.kd * derivative + + if self.output_limits: + output = clamp(output, self.output_limits[0], self.output_limits[1]) + + self.last_error = error + self.last_time = current_time + + return output + + def reset(self): + self.integral = 0.0 + self.last_error = 0.0 + self.last_time = None diff --git a/src/utils/transforms.py b/src/utils/transforms.py new file mode 100644 index 0000000..96f7081 --- /dev/null +++ b/src/utils/transforms.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +"""Coordinate transforms for local navigation.""" + +import numpy as np +from scipy.spatial.transform import Rotation + + +def quaternion_to_euler(quat): + r = Rotation.from_quat([quat[0], quat[1], quat[2], quat[3]]) + return r.as_euler('xyz') + + +def euler_to_quaternion(roll, pitch, yaw): + r = Rotation.from_euler('xyz', [roll, pitch, yaw]) + return r.as_quat() + + +def rotation_matrix_from_quaternion(quat): + r = Rotation.from_quat([quat[0], quat[1], quat[2], quat[3]]) + return r.as_matrix() + + +def transform_point(point, translation, rotation_quat): + R = rotation_matrix_from_quaternion(rotation_quat) + return R @ point + translation + + +def inverse_transform_point(point, translation, rotation_quat): + R = rotation_matrix_from_quaternion(rotation_quat) + return R.T @ (point - translation) + + +def body_to_world(body_vel, yaw): + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + + world_vel = np.zeros(3) + world_vel[0] = body_vel[0] * cos_yaw - body_vel[1] * sin_yaw + world_vel[1] = body_vel[0] * sin_yaw + body_vel[1] * cos_yaw + world_vel[2] = body_vel[2] + + return world_vel + + +def world_to_body(world_vel, yaw): + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + + body_vel = np.zeros(3) + body_vel[0] = world_vel[0] * cos_yaw + world_vel[1] * sin_yaw + body_vel[1] = -world_vel[0] * sin_yaw + world_vel[1] * cos_yaw + body_vel[2] = world_vel[2] + + return body_vel + + +def normalize_angle(angle): + while angle > np.pi: + angle -= 2 * np.pi + while angle < -np.pi: + angle += 2 * np.pi + return angle + + +def angle_difference(angle1, angle2): + diff = angle1 - angle2 + return normalize_angle(diff) + + +def create_transformation_matrix(translation, rotation_quat): + T = np.eye(4) + T[:3, :3] = rotation_matrix_from_quaternion(rotation_quat) + T[:3, 3] = translation + return T + + +def invert_transformation_matrix(T): + R = T[:3, :3] + t = T[:3, 3] + + T_inv = np.eye(4) + T_inv[:3, :3] = R.T + T_inv[:3, 3] = -R.T @ t + return T_inv diff --git a/src/vision/__init__.py b/src/vision/__init__.py new file mode 100644 index 0000000..103157c --- /dev/null +++ b/src/vision/__init__.py @@ -0,0 +1 @@ +"""Vision processing module for GPS-denied navigation.""" diff --git a/src/vision/camera_processor.py b/src/vision/camera_processor.py new file mode 100644 index 0000000..669ec93 --- /dev/null +++ b/src/vision/camera_processor.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python3 +""" +Camera Processor Node +Handles camera feed processing for GPS-denied navigation +""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image, CameraInfo +from std_msgs.msg import Header +from cv_bridge import CvBridge +import cv2 +import numpy as np + + +class CameraProcessor(Node): + """ + Processes camera feeds for visual odometry and obstacle detection. + Outputs processed images for downstream nodes. + """ + + def __init__(self): + super().__init__('camera_processor') + + self.bridge = CvBridge() + + # Camera parameters + self.camera_matrix = None + self.dist_coeffs = None + self.image_size = None + + # Image processing parameters + self.declare_parameter('undistort', True) + self.declare_parameter('grayscale_output', True) + self.declare_parameter('histogram_equalization', True) + self.declare_parameter('resize_factor', 1.0) + + self.undistort = self.get_parameter('undistort').value + self.grayscale_output = self.get_parameter('grayscale_output').value + self.histogram_eq = self.get_parameter('histogram_equalization').value + self.resize_factor = self.get_parameter('resize_factor').value + + # Undistort maps (computed once) + self.map1 = None + self.map2 = None + + # Subscribers - Forward camera + self.forward_image_sub = self.create_subscription( + Image, + '/uav/camera/forward/image_raw', + self.forward_image_callback, + 10 + ) + + self.forward_info_sub = self.create_subscription( + CameraInfo, + '/uav/camera/forward/camera_info', + self.camera_info_callback, + 10 + ) + + # Subscribers - Downward camera + self.downward_image_sub = self.create_subscription( + Image, + '/uav/camera/downward/image_raw', + self.downward_image_callback, + 10 + ) + + # Publishers - Processed images + self.forward_processed_pub = self.create_publisher( + Image, + '/uav/camera/forward/image_processed', + 10 + ) + + self.downward_processed_pub = self.create_publisher( + Image, + '/uav/camera/downward/image_processed', + 10 + ) + + # Debug visualization + self.debug_pub = self.create_publisher( + Image, + '/uav/camera/debug', + 10 + ) + + self.get_logger().info('Camera Processor Node Started') + + def camera_info_callback(self, msg: CameraInfo): + """Extract and store camera calibration parameters.""" + if self.camera_matrix is None: + self.camera_matrix = np.array(msg.k).reshape(3, 3) + self.dist_coeffs = np.array(msg.d) + self.image_size = (msg.width, msg.height) + + # Compute undistortion maps + if self.undistort and self.dist_coeffs is not None: + new_camera_matrix, _ = cv2.getOptimalNewCameraMatrix( + self.camera_matrix, + self.dist_coeffs, + self.image_size, + alpha=0 + ) + self.map1, self.map2 = cv2.initUndistortRectifyMap( + self.camera_matrix, + self.dist_coeffs, + None, + new_camera_matrix, + self.image_size, + cv2.CV_16SC2 + ) + + self.get_logger().info( + f'Camera calibration received: {self.image_size[0]}x{self.image_size[1]}' + ) + + def process_image(self, image: np.ndarray) -> np.ndarray: + """ + Apply image processing pipeline. + + Args: + image: Input BGR image + + Returns: + Processed image + """ + processed = image.copy() + + # 1. Undistort if calibration available + if self.undistort and self.map1 is not None: + processed = cv2.remap(processed, self.map1, self.map2, cv2.INTER_LINEAR) + + # 2. Resize if needed + if self.resize_factor != 1.0: + new_size = ( + int(processed.shape[1] * self.resize_factor), + int(processed.shape[0] * self.resize_factor) + ) + processed = cv2.resize(processed, new_size, interpolation=cv2.INTER_AREA) + + # 3. Convert to grayscale if requested + if self.grayscale_output: + if len(processed.shape) == 3: + processed = cv2.cvtColor(processed, cv2.COLOR_BGR2GRAY) + + # 4. Histogram equalization for better feature detection + if self.histogram_eq: + if len(processed.shape) == 2: + # CLAHE for better results than standard histogram equalization + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)) + processed = clahe.apply(processed) + else: + # For color images, apply to L channel in LAB + lab = cv2.cvtColor(processed, cv2.COLOR_BGR2LAB) + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)) + lab[:, :, 0] = clahe.apply(lab[:, :, 0]) + processed = cv2.cvtColor(lab, cv2.COLOR_LAB2BGR) + + return processed + + def forward_image_callback(self, msg: Image): + """Process forward camera images for visual odometry.""" + try: + # Convert ROS Image to OpenCV + cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + + # Process image + processed = self.process_image(cv_image) + + # Convert back to ROS Image + if len(processed.shape) == 2: + encoding = 'mono8' + else: + encoding = 'bgr8' + + processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding) + processed_msg.header = msg.header + + # Publish processed image + self.forward_processed_pub.publish(processed_msg) + + except Exception as e: + self.get_logger().error(f'Forward image processing error: {e}') + + def downward_image_callback(self, msg: Image): + """Process downward camera images for optical flow.""" + try: + # Convert ROS Image to OpenCV + cv_image = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + + # Process image + processed = self.process_image(cv_image) + + # Convert back to ROS Image + if len(processed.shape) == 2: + encoding = 'mono8' + else: + encoding = 'bgr8' + + processed_msg = self.bridge.cv2_to_imgmsg(processed, encoding) + processed_msg.header = msg.header + + # Publish processed image + self.downward_processed_pub.publish(processed_msg) + + except Exception as e: + self.get_logger().error(f'Downward image processing error: {e}') + + +def main(args=None): + rclpy.init(args=args) + node = CameraProcessor() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vision/object_detector.py b/src/vision/object_detector.py new file mode 100644 index 0000000..7afbd0d --- /dev/null +++ b/src/vision/object_detector.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +"""Object Detector - Visual landmark and obstacle detection.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from geometry_msgs.msg import PoseArray, Pose +from std_msgs.msg import Float32MultiArray +from cv_bridge import CvBridge +import cv2 +import numpy as np + + +class ObjectDetector(Node): + + def __init__(self): + super().__init__('object_detector') + + self.bridge = CvBridge() + + self.declare_parameter('detection_method', 'ArUco') + self.declare_parameter('marker_size', 0.15) + self.declare_parameter('camera_matrix', [500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0]) + + self.detection_method = self.get_parameter('detection_method').value + self.marker_size = self.get_parameter('marker_size').value + camera_matrix_flat = self.get_parameter('camera_matrix').value + self.camera_matrix = np.array(camera_matrix_flat).reshape(3, 3) + self.dist_coeffs = np.zeros(5) + + if self.detection_method == 'ArUco': + self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + self.aruco_params = cv2.aruco.DetectorParameters() + self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params) + + self.image_sub = self.create_subscription( + Image, '/uav/camera/forward/image_raw', self.image_callback, 10) + + self.landmarks_pub = self.create_publisher(PoseArray, '/uav/landmarks/poses', 10) + self.marker_ids_pub = self.create_publisher(Float32MultiArray, '/uav/landmarks/ids', 10) + self.debug_image_pub = self.create_publisher(Image, '/uav/landmarks/debug', 10) + + self.get_logger().info(f'Object Detector Started - {self.detection_method}') + + def image_callback(self, msg): + try: + frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + + if self.detection_method == 'ArUco': + self.detect_aruco(frame, msg.header) + else: + self.detect_orb_features(frame, msg.header) + + except Exception as e: + self.get_logger().error(f'Detection error: {e}') + + def detect_aruco(self, frame, header): + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + corners, ids, rejected = self.aruco_detector.detectMarkers(gray) + + pose_array = PoseArray() + pose_array.header = header + pose_array.header.frame_id = 'camera_link' + + id_array = Float32MultiArray() + + if ids is not None: + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, self.marker_size, self.camera_matrix, self.dist_coeffs) + + for i, marker_id in enumerate(ids.flatten()): + pose = Pose() + pose.position.x = float(tvecs[i][0][0]) + pose.position.y = float(tvecs[i][0][1]) + pose.position.z = float(tvecs[i][0][2]) + + rotation_matrix, _ = cv2.Rodrigues(rvecs[i]) + from scipy.spatial.transform import Rotation + r = Rotation.from_matrix(rotation_matrix) + quat = r.as_quat() + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + + pose_array.poses.append(pose) + id_array.data.append(float(marker_id)) + + debug_frame = cv2.aruco.drawDetectedMarkers(frame.copy(), corners, ids) + for i in range(len(rvecs)): + cv2.drawFrameAxes(debug_frame, self.camera_matrix, self.dist_coeffs, + rvecs[i], tvecs[i], self.marker_size * 0.5) + + debug_msg = self.bridge.cv2_to_imgmsg(debug_frame, 'bgr8') + debug_msg.header = header + self.debug_image_pub.publish(debug_msg) + + self.landmarks_pub.publish(pose_array) + self.marker_ids_pub.publish(id_array) + + def detect_orb_features(self, frame, header): + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + orb = cv2.ORB_create(nfeatures=500) + keypoints, descriptors = orb.detectAndCompute(gray, None) + + pose_array = PoseArray() + pose_array.header = header + + for kp in keypoints[:50]: + pose = Pose() + pose.position.x = float(kp.pt[0]) + pose.position.y = float(kp.pt[1]) + pose.position.z = 0.0 + pose_array.poses.append(pose) + + self.landmarks_pub.publish(pose_array) + + +def main(args=None): + rclpy.init(args=args) + node = ObjectDetector() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vision/optical_flow.py b/src/vision/optical_flow.py new file mode 100644 index 0000000..741dc38 --- /dev/null +++ b/src/vision/optical_flow.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 +"""Optical Flow - Velocity estimation from downward camera.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image, Range +from geometry_msgs.msg import TwistStamped +from cv_bridge import CvBridge +import cv2 +import numpy as np + + +class OpticalFlowNode(Node): + + def __init__(self): + super().__init__('optical_flow_node') + + self.bridge = CvBridge() + + self.prev_frame = None + self.prev_points = None + self.current_altitude = 1.0 + + self.declare_parameter('window_size', 15) + self.declare_parameter('max_level', 3) + self.declare_parameter('min_altitude', 0.3) + self.declare_parameter('max_altitude', 10.0) + self.declare_parameter('focal_length', 500.0) + + self.window_size = self.get_parameter('window_size').value + self.max_level = self.get_parameter('max_level').value + self.min_altitude = self.get_parameter('min_altitude').value + self.max_altitude = self.get_parameter('max_altitude').value + self.focal_length = self.get_parameter('focal_length').value + + self.lk_params = dict( + winSize=(self.window_size, self.window_size), + maxLevel=self.max_level, + criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03) + ) + + self.feature_params = dict( + maxCorners=100, + qualityLevel=0.3, + minDistance=7, + blockSize=7 + ) + + self.velocity = np.zeros(2) + self.prev_time = None + + self.image_sub = self.create_subscription( + Image, '/uav/camera/downward/image_raw', self.image_callback, 10) + + self.altitude_sub = self.create_subscription( + Range, '/uav/rangefinder/range', self.altitude_callback, 10) + + self.velocity_pub = self.create_publisher( + TwistStamped, '/uav/optical_flow/velocity', 10) + + self.get_logger().info('Optical Flow Node Started') + + def altitude_callback(self, msg): + if msg.range > self.min_altitude and msg.range < self.max_altitude: + self.current_altitude = msg.range + + def image_callback(self, msg): + try: + frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + current_time = self.get_clock().now() + + if self.prev_frame is not None and self.prev_points is not None: + new_points, status, error = cv2.calcOpticalFlowPyrLK( + self.prev_frame, gray, self.prev_points, None, **self.lk_params) + + if new_points is not None: + good_new = new_points[status == 1] + good_old = self.prev_points[status == 1] + + if len(good_new) > 10: + flow = good_new - good_old + avg_flow = np.mean(flow, axis=0) + + if self.prev_time is not None: + dt = (current_time - self.prev_time).nanoseconds / 1e9 + if dt > 0: + pixel_velocity = avg_flow / dt + + self.velocity[0] = pixel_velocity[0] * self.current_altitude / self.focal_length + self.velocity[1] = pixel_velocity[1] * self.current_altitude / self.focal_length + + self.publish_velocity() + + self.prev_points = cv2.goodFeaturesToTrack(gray, **self.feature_params) + self.prev_frame = gray + self.prev_time = current_time + + except Exception as e: + self.get_logger().error(f'Optical flow error: {e}') + + def publish_velocity(self): + vel_msg = TwistStamped() + vel_msg.header.stamp = self.get_clock().now().to_msg() + vel_msg.header.frame_id = 'base_link' + + vel_msg.twist.linear.x = float(self.velocity[0]) + vel_msg.twist.linear.y = float(self.velocity[1]) + vel_msg.twist.linear.z = 0.0 + + self.velocity_pub.publish(vel_msg) + + +def main(args=None): + rclpy.init(args=args) + node = OpticalFlowNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vision/visual_odometry.py b/src/vision/visual_odometry.py new file mode 100644 index 0000000..0f72cfa --- /dev/null +++ b/src/vision/visual_odometry.py @@ -0,0 +1,173 @@ +#!/usr/bin/env python3 +"""Visual Odometry - Camera-based position estimation without GPS.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image, CameraInfo +from geometry_msgs.msg import PoseStamped, TwistStamped +from cv_bridge import CvBridge +import cv2 +import numpy as np +from scipy.spatial.transform import Rotation + + +class VisualOdometryNode(Node): + + def __init__(self): + super().__init__('visual_odometry_node') + + self.bridge = CvBridge() + + self.prev_frame = None + self.prev_keypoints = None + self.prev_descriptors = None + + self.camera_matrix = None + self.dist_coeffs = None + + self.current_pose = np.eye(4) + self.position = np.zeros(3) + self.orientation = np.eye(3) + self.prev_time = None + self.velocity = np.zeros(3) + + self.detector_type = self.declare_parameter('detector_type', 'ORB').value + self.min_features = self.declare_parameter('min_features', 100).value + self.feature_quality = self.declare_parameter('feature_quality', 0.01).value + + if self.detector_type == 'ORB': + self.detector = cv2.ORB_create(nfeatures=500) + self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) + elif self.detector_type == 'SIFT': + self.detector = cv2.SIFT_create(nfeatures=500) + self.matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False) + else: + self.detector = cv2.ORB_create(nfeatures=500) + self.matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) + + self.image_sub = self.create_subscription( + Image, '/uav/camera/forward/image_raw', self.image_callback, 10) + + self.camera_info_sub = self.create_subscription( + CameraInfo, '/uav/camera/forward/camera_info', self.camera_info_callback, 10) + + self.pose_pub = self.create_publisher(PoseStamped, '/uav/visual_odometry/pose', 10) + self.velocity_pub = self.create_publisher(TwistStamped, '/uav/visual_odometry/velocity', 10) + + self.timer = self.create_timer(0.033, self.publish_pose) + + self.get_logger().info(f'Visual Odometry Node Started - {self.detector_type} detector') + + def camera_info_callback(self, msg): + if self.camera_matrix is None: + self.camera_matrix = np.array(msg.k).reshape(3, 3) + self.dist_coeffs = np.array(msg.d) + self.get_logger().info('Camera calibration received') + + def image_callback(self, msg): + try: + frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + + keypoints, descriptors = self.detector.detectAndCompute(gray, None) + + current_time = self.get_clock().now() + + if self.prev_frame is not None and len(keypoints) >= self.min_features: + matches = self.match_features(self.prev_descriptors, descriptors) + + if len(matches) >= self.min_features: + self.estimate_motion(self.prev_keypoints, keypoints, matches, current_time) + + self.prev_frame = gray + self.prev_keypoints = keypoints + self.prev_descriptors = descriptors + self.prev_time = current_time + + except Exception as e: + self.get_logger().error(f'Visual odometry error: {e}') + + def match_features(self, desc1, desc2): + if desc1 is None or desc2 is None: + return [] + + matches = self.matcher.knnMatch(desc1, desc2, k=2) + + good_matches = [] + for match_pair in matches: + if len(match_pair) == 2: + m, n = match_pair + if m.distance < 0.7 * n.distance: + good_matches.append(m) + + return good_matches + + def estimate_motion(self, prev_kp, curr_kp, matches, current_time): + if self.camera_matrix is None or len(matches) < 5: + return + + pts1 = np.float32([prev_kp[m.queryIdx].pt for m in matches]) + pts2 = np.float32([curr_kp[m.trainIdx].pt for m in matches]) + + E, mask = cv2.findEssentialMat( + pts1, pts2, self.camera_matrix, + method=cv2.RANSAC, prob=0.999, threshold=1.0) + + if E is None: + return + + _, R, t, mask = cv2.recoverPose(E, pts1, pts2, self.camera_matrix, mask=mask) + + scale = 1.0 + translation = scale * t.flatten() + + if self.prev_time is not None: + dt = (current_time - self.prev_time).nanoseconds / 1e9 + if dt > 0: + self.velocity = translation / dt + + self.position += self.orientation @ translation + self.orientation = R @ self.orientation + + def publish_pose(self): + pose_msg = PoseStamped() + pose_msg.header.stamp = self.get_clock().now().to_msg() + pose_msg.header.frame_id = 'odom' + + pose_msg.pose.position.x = float(self.position[0]) + pose_msg.pose.position.y = float(self.position[1]) + pose_msg.pose.position.z = float(self.position[2]) + + rotation = Rotation.from_matrix(self.orientation) + quat = rotation.as_quat() + pose_msg.pose.orientation.x = quat[0] + pose_msg.pose.orientation.y = quat[1] + pose_msg.pose.orientation.z = quat[2] + pose_msg.pose.orientation.w = quat[3] + + self.pose_pub.publish(pose_msg) + + vel_msg = TwistStamped() + vel_msg.header.stamp = self.get_clock().now().to_msg() + vel_msg.header.frame_id = 'odom' + vel_msg.twist.linear.x = float(self.velocity[0]) + vel_msg.twist.linear.y = float(self.velocity[1]) + vel_msg.twist.linear.z = float(self.velocity[2]) + + self.velocity_pub.publish(vel_msg) + + +def main(args=None): + rclpy.init(args=args) + node = VisualOdometryNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/vision/visual_servoing.py b/src/vision/visual_servoing.py new file mode 100644 index 0000000..5c8a27e --- /dev/null +++ b/src/vision/visual_servoing.py @@ -0,0 +1,135 @@ +#!/usr/bin/env python3 +"""Visual Servoing - Vision-based control for precision positioning.""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from geometry_msgs.msg import PoseStamped, TwistStamped, Pose +from std_msgs.msg import Bool +from cv_bridge import CvBridge +import cv2 +import numpy as np + + +class VisualServoing(Node): + + def __init__(self): + super().__init__('visual_servoing') + + self.bridge = CvBridge() + + self.declare_parameter('target_marker_id', 0) + self.declare_parameter('desired_distance', 1.0) + self.declare_parameter('kp_linear', 0.5) + self.declare_parameter('kp_angular', 0.3) + self.declare_parameter('max_velocity', 1.0) + + self.target_marker_id = self.get_parameter('target_marker_id').value + self.desired_distance = self.get_parameter('desired_distance').value + self.kp_linear = self.get_parameter('kp_linear').value + self.kp_angular = self.get_parameter('kp_angular').value + self.max_velocity = self.get_parameter('max_velocity').value + + self.enabled = False + self.target_pose = None + self.image_center = (320, 240) + + self.camera_matrix = np.array([ + [500.0, 0.0, 320.0], + [0.0, 500.0, 240.0], + [0.0, 0.0, 1.0] + ]) + self.dist_coeffs = np.zeros(5) + + self.aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + self.aruco_params = cv2.aruco.DetectorParameters() + self.aruco_detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.aruco_params) + + self.image_sub = self.create_subscription( + Image, '/uav/camera/forward/image_raw', self.image_callback, 10) + + self.enable_sub = self.create_subscription( + Bool, '/visual_servoing/enable', self.enable_callback, 10) + + self.target_sub = self.create_subscription( + Pose, '/visual_servoing/target', self.target_callback, 10) + + self.velocity_pub = self.create_publisher( + TwistStamped, '/uav/visual_servoing/cmd_vel', 10) + + self.status_pub = self.create_publisher( + Bool, '/visual_servoing/target_acquired', 10) + + self.get_logger().info('Visual Servoing Node Started') + + def enable_callback(self, msg): + self.enabled = msg.data + self.get_logger().info(f'Visual servoing {"enabled" if self.enabled else "disabled"}') + + def target_callback(self, msg): + self.target_pose = msg + + def image_callback(self, msg): + if not self.enabled: + return + + try: + frame = self.bridge.imgmsg_to_cv2(msg, 'bgr8') + self.image_center = (frame.shape[1] // 2, frame.shape[0] // 2) + + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + corners, ids, _ = self.aruco_detector.detectMarkers(gray) + + target_acquired = Bool() + target_acquired.data = False + + if ids is not None and self.target_marker_id in ids: + idx = np.where(ids == self.target_marker_id)[0][0] + target_corners = corners[idx] + + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + [target_corners], 0.15, self.camera_matrix, self.dist_coeffs) + + target_pos = tvecs[0][0] + self.compute_control(target_pos, target_corners) + + target_acquired.data = True + + self.status_pub.publish(target_acquired) + + except Exception as e: + self.get_logger().error(f'Visual servoing error: {e}') + + def compute_control(self, target_pos, corners): + marker_center = np.mean(corners[0], axis=0) + + error_x = self.image_center[0] - marker_center[0] + error_y = self.image_center[1] - marker_center[1] + error_z = target_pos[2] - self.desired_distance + + vel_msg = TwistStamped() + vel_msg.header.stamp = self.get_clock().now().to_msg() + vel_msg.header.frame_id = 'base_link' + + vel_msg.twist.linear.x = np.clip(self.kp_linear * error_z, -self.max_velocity, self.max_velocity) + vel_msg.twist.linear.y = np.clip(self.kp_linear * error_x / 100.0, -self.max_velocity, self.max_velocity) + vel_msg.twist.linear.z = np.clip(self.kp_linear * error_y / 100.0, -self.max_velocity, self.max_velocity) + vel_msg.twist.angular.z = np.clip(self.kp_angular * error_x / 100.0, -1.0, 1.0) + + self.velocity_pub.publish(vel_msg) + + +def main(args=None): + rclpy.init(args=args) + node = VisualServoing() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..46816dd --- /dev/null +++ b/tests/__init__.py @@ -0,0 +1 @@ +"""Tests package.""" diff --git a/tests/test_camera.py b/tests/test_camera.py new file mode 100644 index 0000000..dd80f82 --- /dev/null +++ b/tests/test_camera.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +"""Test camera processing.""" + +import pytest +import numpy as np +import cv2 + + +def test_image_processing(): + img = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8) + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + assert gray.shape == (480, 640) + + +def test_clahe(): + gray = np.random.randint(0, 255, (480, 640), dtype=np.uint8) + clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8)) + enhanced = clahe.apply(gray) + assert enhanced.shape == gray.shape + + +def test_undistort_maps(): + K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]], dtype=np.float64) + dist = np.zeros(5) + size = (640, 480) + + new_K, roi = cv2.getOptimalNewCameraMatrix(K, dist, size, alpha=0) + map1, map2 = cv2.initUndistortRectifyMap(K, dist, None, new_K, size, cv2.CV_16SC2) + + assert map1 is not None + assert map2 is not None + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/tests/test_installation.py b/tests/test_installation.py new file mode 100644 index 0000000..a9a1461 --- /dev/null +++ b/tests/test_installation.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +"""Test installation and imports.""" + +import pytest +import sys + + +def test_python_version(): + assert sys.version_info.major == 3 + assert sys.version_info.minor >= 10 + + +def test_numpy_import(): + import numpy as np + assert np.__version__ + + +def test_opencv_import(): + import cv2 + assert cv2.__version__ + + +def test_scipy_import(): + from scipy.spatial.transform import Rotation + r = Rotation.from_euler('xyz', [0, 0, 0]) + assert r is not None + + +def test_shapely_import(): + from shapely.geometry import Point, Polygon + p = Point(0, 0) + assert p is not None + + +def test_filterpy_import(): + from filterpy.kalman import ExtendedKalmanFilter + ekf = ExtendedKalmanFilter(dim_x=3, dim_z=2) + assert ekf is not None + + +def test_transforms3d_import(): + import transforms3d + assert transforms3d is not None + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/tests/test_position_estimator.py b/tests/test_position_estimator.py new file mode 100644 index 0000000..cf65bb2 --- /dev/null +++ b/tests/test_position_estimator.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""Test position estimator and EKF.""" + +import pytest +import numpy as np + + +def test_ekf_initialization(): + from filterpy.kalman import ExtendedKalmanFilter + + ekf = ExtendedKalmanFilter(dim_x=9, dim_z=6) + ekf.x = np.zeros(9) + ekf.P = np.eye(9) + ekf.Q = np.eye(9) * 0.1 + ekf.R = np.eye(6) * 0.1 + + assert ekf.x.shape == (9,) + assert ekf.P.shape == (9, 9) + + +def test_state_transition(): + dt = 0.02 + x = np.array([1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.0, 0.0, 0.0]) + + F = np.eye(9) + F[0, 3] = dt + F[1, 4] = dt + F[2, 5] = dt + + x_new = F @ x + + assert np.isclose(x_new[0], 1.0 + 0.1 * dt) + assert np.isclose(x_new[1], 2.0 + 0.2 * dt) + assert np.isclose(x_new[2], 3.0 + 0.3 * dt) + + +def test_measurement_update(): + from filterpy.kalman import ExtendedKalmanFilter + + ekf = ExtendedKalmanFilter(dim_x=6, dim_z=3) + ekf.x = np.zeros(6) + ekf.P = np.eye(6) + ekf.Q = np.eye(6) * 0.1 + ekf.R = np.eye(3) * 0.1 + ekf.F = np.eye(6) + + H = np.zeros((3, 6)) + H[0, 0] = 1.0 + H[1, 1] = 1.0 + H[2, 2] = 1.0 + + z = np.array([1.0, 2.0, 3.0]) + + ekf.update(z, HJacobian=lambda x: H, Hx=lambda x: H @ x) + + assert ekf.x[0] > 0 + + +def test_weighted_average(): + weights = [0.6, 0.3, 0.1] + values = [np.array([1, 0, 0]), np.array([0, 1, 0]), np.array([0, 0, 1])] + + total = sum([w * v for w, v in zip(weights, values)]) + + assert np.isclose(total[0], 0.6) + assert np.isclose(total[1], 0.3) + assert np.isclose(total[2], 0.1) + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/tests/test_uav_control.py b/tests/test_uav_control.py new file mode 100644 index 0000000..00c116e --- /dev/null +++ b/tests/test_uav_control.py @@ -0,0 +1,40 @@ +#!/usr/bin/env python3 +"""Test UAV control logic.""" + +import pytest +import numpy as np + + +def test_waypoint_distance(): + current = np.array([0, 0, 5]) + target = np.array([10, 0, 5]) + distance = np.linalg.norm(target - current) + assert np.isclose(distance, 10.0) + + +def test_velocity_command(): + current = np.array([0, 0, 5]) + target = np.array([10, 0, 5]) + + direction = target - current + distance = np.linalg.norm(direction) + direction = direction / distance + + max_vel = 2.0 + speed = min(max_vel, distance * 0.5) + velocity = direction * speed + + assert np.isclose(velocity[0], max_vel) + assert np.isclose(velocity[1], 0.0) + + +def test_position_hold(): + current = np.array([5.0, 3.0, 10.0]) + target = current.copy() + + error = np.linalg.norm(target - current) + assert np.isclose(error, 0.0) + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/tests/test_ugv_control.py b/tests/test_ugv_control.py new file mode 100644 index 0000000..5c01529 --- /dev/null +++ b/tests/test_ugv_control.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 +"""Test UGV control logic.""" + +import pytest +import numpy as np + + +def test_differential_drive(): + target_angle = np.pi / 4 + current_yaw = 0 + + angle_error = target_angle - current_yaw + kp_angular = 1.5 + max_angular = 1.0 + + angular_cmd = np.clip(kp_angular * angle_error, -max_angular, max_angular) + assert np.isclose(angular_cmd, 1.0) + + +def test_angle_normalization(): + def normalize(angle): + while angle > np.pi: + angle -= 2 * np.pi + while angle < -np.pi: + angle += 2 * np.pi + return angle + + assert np.isclose(normalize(3 * np.pi), np.pi) + assert np.isclose(normalize(-3 * np.pi), np.pi) + + +def test_goal_distance_2d(): + current = np.array([0, 0]) + target = np.array([3, 4]) + + distance = np.linalg.norm(target - current) + assert np.isclose(distance, 5.0) + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/tests/test_visual_odometry.py b/tests/test_visual_odometry.py new file mode 100644 index 0000000..bd36ec6 --- /dev/null +++ b/tests/test_visual_odometry.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python3 +"""Test visual odometry functionality.""" + +import pytest +import numpy as np +import cv2 + + +def test_feature_detector_orb(): + detector = cv2.ORB_create(nfeatures=500) + img = np.random.randint(0, 255, (480, 640), dtype=np.uint8) + keypoints, descriptors = detector.detectAndCompute(img, None) + assert keypoints is not None + + +def test_feature_detector_sift(): + detector = cv2.SIFT_create(nfeatures=500) + img = np.random.randint(0, 255, (480, 640), dtype=np.uint8) + keypoints, descriptors = detector.detectAndCompute(img, None) + assert keypoints is not None + + +def test_feature_matching(): + detector = cv2.ORB_create(nfeatures=100) + matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False) + + img1 = np.random.randint(0, 255, (240, 320), dtype=np.uint8) + img2 = np.random.randint(0, 255, (240, 320), dtype=np.uint8) + + kp1, desc1 = detector.detectAndCompute(img1, None) + kp2, desc2 = detector.detectAndCompute(img2, None) + + if desc1 is not None and desc2 is not None: + matches = matcher.knnMatch(desc1, desc2, k=2) + assert matches is not None + + +def test_essential_matrix(): + K = np.array([[500, 0, 320], [0, 500, 240], [0, 0, 1]], dtype=np.float64) + + pts1 = np.random.rand(10, 2) * 100 + 100 + pts2 = pts1 + np.random.rand(10, 2) * 5 + + pts1 = pts1.astype(np.float32) + pts2 = pts2.astype(np.float32) + + E, mask = cv2.findEssentialMat(pts1, pts2, K, method=cv2.RANSAC) + assert E is not None or mask is not None + + +def test_rotation_from_scipy(): + from scipy.spatial.transform import Rotation + + R_mat = np.eye(3) + r = Rotation.from_matrix(R_mat) + quat = r.as_quat() + + assert len(quat) == 4 + assert np.isclose(quat[3], 1.0) + + +if __name__ == '__main__': + pytest.main([__file__, '-v']) diff --git a/worlds/empty_custom.world b/worlds/empty_custom.world new file mode 100644 index 0000000..20751dd --- /dev/null +++ b/worlds/empty_custom.world @@ -0,0 +1,85 @@ + + + + + 0.001 + 1 + 1000 + + + + model://sun + + + + model://ground_plane + + + + true + + 5 0 0.01 0 0 0 + + + 0.5 0.5 0.01 + + + 1 0 0 1 + 1 0 0 1 + + + + + 10 5 0.01 0 0 0 + + + 0.5 0.5 0.01 + + + 0 1 0 1 + 0 1 0 1 + + + + + 5 10 0.01 0 0 0 + + + 0.5 0.5 0.01 + + + 0 0 1 1 + 0 0 1 1 + + + + + + + true + 0 0 0.01 0 0 0 + + + + + 0.3 + 0.02 + + + + 1 1 0 1 + 1 1 0 1 + + + + + + + EARTH_WGS84 + 47.397742 + 8.545594 + 488.0 + 0 + + + diff --git a/worlds/indoor_warehouse.world b/worlds/indoor_warehouse.world new file mode 100644 index 0000000..db47273 --- /dev/null +++ b/worlds/indoor_warehouse.world @@ -0,0 +1,189 @@ + + + + + 0.001 + 1 + 1000 + + + + 10 10 8 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 50 + 0.5 + 0.01 + 0.001 + + + + + 30 10 8 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 50 + 0.5 + 0.01 + 0.001 + + + + + true + 20 10 0 0 0 0 + + + + 50 30 0.1 + + + + + 50 30 0.1 + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + + + true + 20 10 10 0 0 0 + + + + 50 30 0.1 + + + 0.3 0.3 0.3 1 + + + + + + + true + 20 25 5 0 0 0 + + + 50 0.3 10 + + + 50 0.3 10 + 0.7 0.7 0.7 1 + + + + + + true + 20 -5 5 0 0 0 + + + 50 0.3 10 + + + 50 0.3 10 + 0.7 0.7 0.7 1 + + + + + + true + 45 10 5 0 0 0 + + + 0.3 30 10 + + + 0.3 30 10 + 0.7 0.7 0.7 1 + + + + + + true + -5 10 5 0 0 0 + + + 0.3 30 10 + + + 0.3 30 10 + 0.7 0.7 0.7 1 + + + + + + true + 10 5 1.5 0 0 0 + + + 8 1 3 + + + 8 1 3 + 0.6 0.4 0.2 1 + + + + + + true + 10 15 1.5 0 0 0 + + + 8 1 3 + + + 8 1 3 + 0.6 0.4 0.2 1 + + + + + + true + 30 5 1.5 0 0 0 + + + 8 1 3 + + + 8 1 3 + 0.6 0.4 0.2 1 + + + + + + true + 30 15 1.5 0 0 0 + + + 8 1 3 + + + 8 1 3 + 0.6 0.4 0.2 1 + + + + + + EARTH_WGS84 + 47.397742 + 8.545594 + 488.0 + + + diff --git a/worlds/urban_canyon.world b/worlds/urban_canyon.world new file mode 100644 index 0000000..5b7ae67 --- /dev/null +++ b/worlds/urban_canyon.world @@ -0,0 +1,117 @@ + + + + + 0.001 + 1 + 1000 + + + + model://sun + + + + model://ground_plane + + + + true + -15 0 15 0 0 0 + + + 10 20 30 + + + 10 20 30 + 0.5 0.5 0.6 1 + + + + + + true + 15 0 20 0 0 0 + + + 10 20 40 + + + 10 20 40 + 0.6 0.5 0.5 1 + + + + + + true + -15 30 12.5 0 0 0 + + + 10 20 25 + + + 10 20 25 + 0.5 0.6 0.5 1 + + + + + + true + 15 30 17.5 0 0 0 + + + 10 20 35 + + + 10 20 35 + 0.55 0.55 0.55 1 + + + + + + true + 0 15 0.01 0 0 0 + + + 12 50 0.02 + 0.2 0.2 0.2 1 + + + + + + true + + 0 5 0.02 0 0 0 + + 0.2 3 0.01 + 1 1 1 1 + + + + 0 15 0.02 0 0 0 + + 0.2 3 0.01 + 1 1 1 1 + + + + 0 25 0.02 0 0 0 + + 0.2 3 0.01 + 1 1 1 1 + + + + + + EARTH_WGS84 + 47.397742 + 8.545594 + 488.0 + + +