diff --git a/.airstack/README.md b/.airstack/README.md
new file mode 100644
index 00000000..21532bcf
--- /dev/null
+++ b/.airstack/README.md
@@ -0,0 +1,131 @@
+# AirStack CLI Tool
+
+The `airstack` command-line tool provides a unified interface for common development tasks in the AirStack project, including setup, installation, and container management.
+
+## Installation
+
+The `airstack` tool is included in the AirStack repository. To set it up:
+
+1. Clone the AirStack repository:
+   ```bash
+   git clone https://github.com/castacks/AirStack.git
+   cd AirStack
+   ```
+
+2. Run the setup command:
+   ```bash
+   ./airstack setup
+   ```
+
+This will add the `airstack` command to your PATH by modifying your shell profile (`.bashrc` or `.zshrc`).
+
+## Basic Usage
+
+```bash
+airstack <command> [options]
+```
+
+To see all available commands:
+```bash
+airstack commands
+```
+
+To get help for a specific command:
+```bash
+airstack help <command>
+```
+
+## Core Commands
+
+- `install`: Install dependencies (Docker Engine, Docker Compose, etc.)
+- `setup`: Configure AirStack settings and add to shell profile
+- `up [service]`: Start services using Docker Compose
+- `stop [service]`: Stop services
+- `connect [container]`: Connect to a running container (supports partial name matching)
+- `status`: Show status of all containers
+- `logs [container]`: View logs for a container (supports partial name matching)
+
+### Container Name Matching
+
+The `connect` and `logs` commands support partial name matching for container names. This means you can:
+
+1. Use just a portion of the container name (e.g., `airstack connect web` will match `airstack_web_1`)
+2. If multiple containers match, you'll be shown a list and prompted to select one
+3. The matching is case-insensitive and supports fuzzy matching
+
+Example:
+```bash
+$ airstack connect web
+[WARN] Multiple containers match 'web'. Please be more specific or select from the list below:
+NUM     CONTAINER NAME  IMAGE   STATUS
+1       airstack_web_1  nginx:latest    Up 2 hours
+2       airstack_webapi_1       node:14 Up 2 hours
+
+Options:
+  1. Enter a number to select a container
+  2. Type 'q' to quit
+  3. Press Ctrl+C to cancel and try again with a more specific name
+
+Your selection: 1
+[INFO] Connecting to container: airstack_web_1
+[INFO] Tip: Next time, you can directly use 'airstack connect airstack_web_1' for this container
+```
+
+## Development Commands
+
+- `test`: Run tests
+- `docs`: Build documentation
+- `lint`: Lint code
+- `format`: Format code
+
+## Extending the Tool
+
+The `airstack` tool is designed to be easily extensible. You can add new commands by creating module files in the `.airstack/modules/` directory.
+
+### Creating a New Module
+
+1. Create a new `.sh` file in the `.airstack/modules/` directory:
+   ```bash
+   touch .airstack/modules/mymodule.sh
+   ```
+
+2. Add your command functions and register them:
+   ```bash
+   #!/usr/bin/env bash
+
+   # Function to implement your command
+   function cmd_mymodule_mycommand {
+       log_info "Running my command..."
+       # Your command implementation here
+   }
+
+   # Register commands from this module
+   function register_mymodule_commands {
+       COMMANDS["mycommand"]="cmd_mymodule_mycommand"
+       COMMAND_HELP["mycommand"]="Description of my command"
+   }
+   ```
+
+3. Make the module executable:
+   ```bash
+   chmod +x .airstack/modules/mymodule.sh
+   ```
+
+Your new command will be automatically loaded and available as `airstack mycommand`.
+
+### Available Helper Functions
+
+When creating modules, you can use these helper functions:
+
+- `log_info "message"`: Print an info message
+- `log_warn "message"`: Print a warning message
+- `log_error "message"`: Print an error message
+- `check_docker`: Check if Docker is installed and running
+
+## Configuration
+
+The `airstack` tool stores its configuration in `~/.airstack.conf`. This file is created when you run `airstack setup`.
+
+## License
+
+This tool is part of the AirStack project and is subject to the same license terms.
\ No newline at end of file
diff --git a/.airstack/modules/config.sh b/.airstack/modules/config.sh
new file mode 100755
index 00000000..0850e3bf
--- /dev/null
+++ b/.airstack/modules/config.sh
@@ -0,0 +1,110 @@
+#!/usr/bin/env bash
+
+# config.sh - Configuration-related commands for AirStack
+# This module provides commands for configuring the AirStack environment
+
+# Helper function for confirmation prompts
+function confirm_no {
+    read -r -p "${1:-Are you sure? [y/N]} " response
+    case "$response" in
+        [yY][eE][sS]|[yY]) 
+            true
+            ;;
+        *)
+            false
+            ;;
+    esac
+}
+
+# Function to configure Isaac Sim settings
+function cmd_config_isaac_sim {
+    log_info "Configuring Isaac Sim settings..."
+    
+    # Generate user.config.json for IsaacSim settings
+    local USER_CONFIG_JSON_SOURCE="${PROJECT_ROOT}/simulation/isaac-sim/docker/user_TEMPLATE.config.json"
+    local USER_CONFIG_JSON_DESTINATION="${PROJECT_ROOT}/simulation/isaac-sim/docker/user.config.json"
+    
+    log_info "Generating Default IsaacSim Config ($USER_CONFIG_JSON_DESTINATION)"
+    
+    if [ -d "$USER_CONFIG_JSON_DESTINATION" ] && [ $(ls -A "$USER_CONFIG_JSON_DESTINATION" | wc -l) == 0 ]; then
+        # delete an empty directory with the same name as $USER_CONFIG_JSON_DESTINATION which gets created when
+        # docker compose up is run before this script. Doing this will create a directory name user.config.json because
+        # it is being mounted as a volume but it doesn't exist yet.
+        rm -rf "$USER_CONFIG_JSON_DESTINATION"
+    fi
+    
+    if [ -f "$USER_CONFIG_JSON_DESTINATION" ]; then
+        log_warn "The file $USER_CONFIG_JSON_DESTINATION already exists."
+        confirm_no "Do you want to reset it to the default? [y/N]" && cp "$USER_CONFIG_JSON_SOURCE" "$USER_CONFIG_JSON_DESTINATION"
+    else
+        cp "$USER_CONFIG_JSON_SOURCE" "$USER_CONFIG_JSON_DESTINATION"
+    fi
+    
+    log_info "Isaac Sim configuration complete"
+}
+
+# Function to configure AirLab Omniverse Nucleus Server Login
+function cmd_config_nucleus {
+    log_info "Configuring AirLab Nucleus Login..."
+    
+    # AirLab Omniverse Nucleus Server Login Config
+    local OMNI_PASS_SOURCE="${PROJECT_ROOT}/simulation/isaac-sim/docker/omni_pass_TEMPLATE.env"
+    local OMNI_PASS_DESTINATION="${PROJECT_ROOT}/simulation/isaac-sim/docker/omni_pass.env"
+    
+    log_info "Configure AirLab Nucleus Login ($OMNI_PASS_DESTINATION)"
+    
+    log_info "Go to https://airlab-storage.andrew.cmu.edu:8443/omni/web3/, log in, then right click on the cloud and click the \"API Tokens\" window to generate an API token and paste it here. Leave this blank if you want to skip this step: "
+    if [ -f "$OMNI_PASS_DESTINATION" ]; then
+        log_warn "The file $OMNI_PASS_DESTINATION already exists, leave it blank to skip."
+    fi
+    read -r -p "API Token: " API_TOKEN
+    
+    if [ ! -z "${API_TOKEN}" ]; then
+        sed "s/PASTE-YOUR-API-TOKEN/$API_TOKEN/g" "$OMNI_PASS_SOURCE" > "$OMNI_PASS_DESTINATION"
+        log_info "Nucleus login configuration complete"
+    else
+        log_info "Skipping Nucleus login configuration"
+    fi
+}
+
+# Function to set up Git Hooks
+function cmd_config_git_hooks {
+    log_info "Setting up Git Hooks..."
+    
+    # Git Hooks
+    local HOOKS_SOURCE="${PROJECT_ROOT}/git-hooks/docker-versioning/update-docker-image-tag.pre-commit"
+    local HOOKS_DESTINATION="${PROJECT_ROOT}/.git/hooks/pre-commit"
+    
+    if [ -f "$HOOKS_SOURCE" ]; then
+        cp "$HOOKS_SOURCE" "$HOOKS_DESTINATION"
+        chmod +x "$HOOKS_DESTINATION"
+        log_info "Git hooks setup complete"
+    else
+        log_error "Git hooks source file not found: $HOOKS_SOURCE"
+    fi
+}
+
+# Function to run all configuration tasks
+function cmd_config_all {
+    log_info "Running all configuration tasks..."
+    
+    cmd_config_isaac_sim
+    cmd_config_nucleus
+    cmd_config_git_hooks
+    
+    log_info "All configuration tasks complete"
+}
+
+# Register commands from this module
+function register_config_commands {
+    COMMANDS["config"]="cmd_config_all"
+    COMMANDS["config:isaac-sim"]="cmd_config_isaac_sim"
+    COMMANDS["config:nucleus"]="cmd_config_nucleus"
+    COMMANDS["config:git-hooks"]="cmd_config_git_hooks"
+    
+    # Add command help
+    COMMAND_HELP["config"]="Run all configuration tasks"
+    COMMAND_HELP["config:isaac-sim"]="Configure Isaac Sim settings"
+    COMMAND_HELP["config:nucleus"]="Configure AirLab Nucleus login"
+    COMMAND_HELP["config:git-hooks"]="Set up Git hooks"
+}
\ No newline at end of file
diff --git a/.airstack/modules/dev.sh b/.airstack/modules/dev.sh
new file mode 100644
index 00000000..89599111
--- /dev/null
+++ b/.airstack/modules/dev.sh
@@ -0,0 +1,88 @@
+#!/usr/bin/env bash
+
+# dev.sh - Development-related commands for AirStack
+# This module provides commands for development tasks
+
+# Function to run tests
+function cmd_dev_test {
+    log_info "Running tests..."
+    
+    local test_path="$PROJECT_ROOT/tests"
+    local test_filter=""
+    
+    # Parse arguments
+    for arg in "$@"; do
+        if [[ "$arg" == --path=* ]]; then
+            test_path="${arg#--path=}"
+        elif [[ "$arg" == --filter=* ]]; then
+            test_filter="${arg#--filter=}"
+        fi
+    done
+    
+    if [ -n "$test_filter" ]; then
+        log_info "Running tests matching '$test_filter' in $test_path"
+        # Add your test command here, e.g.:
+        # pytest "$test_path" -k "$test_filter"
+        echo "Test command would run here with filter: $test_filter"
+    else
+        log_info "Running all tests in $test_path"
+        # Add your test command here, e.g.:
+        # pytest "$test_path"
+        echo "Test command would run here"
+    fi
+}
+
+# Function to build documentation
+function cmd_dev_docs {
+    log_info "Building documentation..."
+    
+    # Check if mkdocs is installed
+    if ! command -v mkdocs &> /dev/null; then
+        log_warn "mkdocs not found. Installing..."
+        pip install mkdocs mkdocs-material
+    fi
+    
+    # Build documentation
+    cd "$PROJECT_ROOT"
+    mkdocs build
+    
+    # Serve documentation if requested
+    if [[ "$1" == "serve" ]]; then
+        log_info "Serving documentation at http://localhost:8000"
+        mkdocs serve
+    fi
+}
+
+# Function to lint code
+function cmd_dev_lint {
+    log_info "Linting code..."
+    
+    # Add your linting commands here
+    # For example:
+    # flake8 "$PROJECT_ROOT"
+    echo "Linting command would run here"
+}
+
+# Function to format code
+function cmd_dev_format {
+    log_info "Formatting code..."
+    
+    # Add your formatting commands here
+    # For example:
+    # black "$PROJECT_ROOT"
+    echo "Formatting command would run here"
+}
+
+# Register commands from this module
+function register_dev_commands {
+    COMMANDS["test"]="cmd_dev_test"
+    COMMANDS["docs"]="cmd_dev_docs"
+    COMMANDS["lint"]="cmd_dev_lint"
+    COMMANDS["format"]="cmd_dev_format"
+    
+    # Add command help
+    COMMAND_HELP["test"]="Run tests (options: --path=PATH, --filter=PATTERN)"
+    COMMAND_HELP["docs"]="Build documentation (options: serve)"
+    COMMAND_HELP["lint"]="Lint code"
+    COMMAND_HELP["format"]="Format code"
+}
\ No newline at end of file
diff --git a/.airstack/modules/wintak.sh b/.airstack/modules/wintak.sh
new file mode 100755
index 00000000..acd826df
--- /dev/null
+++ b/.airstack/modules/wintak.sh
@@ -0,0 +1,162 @@
+#!/usr/bin/env bash
+
+# wintak.sh - WINTAK-related commands for AirStack
+# This module provides commands for installing and managing WINTAK
+
+# Function to install WINTAK
+function cmd_wintak_install {
+    log_info "Installing WINTAK..."
+    
+    # Create log directory
+    local LOG_DIR="$HOME/wintak_setup_logs"
+    mkdir -p "$LOG_DIR"
+    local LOG_FILE="$LOG_DIR/setup_$(date +%Y%m%d_%H%M%S).log"
+    
+    # Log function
+    function wintak_log {
+        echo "[$(date '+%Y-%m-%d %H:%M:%S')] $1" | tee -a "$LOG_FILE"
+    }
+    
+    wintak_log "Starting WinTAK VirtualBox setup"
+    wintak_log "Setting up Ground Control Station"
+    
+    # Create vmware directory
+    wintak_log "Creating vmware directory..."
+    sudo mkdir -p "$HOME/vmware"
+    sudo chown -R $USER:$USER "$HOME/vmware"
+    sudo chmod -R 755 "$HOME/vmware"
+    
+    # Create ROS config directories if they don't exist
+    local GCS_DIR="$PROJECT_ROOT/ground_control_station"
+    mkdir -p "$GCS_DIR/ros_ws/src/ros2tak_tools/config"
+    mkdir -p "$GCS_DIR/ros_ws/src/ros2tak_tools/creds"
+    
+    # Single rsync command to get all necessary files from airlab-storage
+    wintak_log "Copying all required files from airlab-storage to $HOME/vmware/..."
+    wintak_log "This may take some time depending on your network connection..."
+    
+    # Prompt for ANDREWID
+    read -p "Please enter your Andrew ID: " ANDREWID
+    
+    # Check if ANDREWID is provided
+    if [ -z "$ANDREWID" ]; then
+        log_error "Error: Andrew ID cannot be empty"
+        return 1
+    fi
+    
+    # Set ANDREWID as environment variable
+    export ANDREWID
+    sudo rsync --progress -avz ${ANDREWID}@airlab-storage.andrew.cmu.edu:/volume4/dsta/engineering/atak/setup/ "$HOME/vmware/" 2>&1 | tee -a "$LOG_FILE"
+    
+    # Copy config and creds to ROS workspace
+    wintak_log "Copying config and creds to ROS workspace..."
+    sudo cp -R "$HOME/vmware/config/"* "$GCS_DIR/ros_ws/src/ros2tak_tools/config/"
+    sudo cp -R "$HOME/vmware/creds/"* "$GCS_DIR/ros_ws/src/ros2tak_tools/creds/"
+    
+    # Set secure permissions on creds directory
+    wintak_log "Setting secure permissions on credentials directory..."
+    # Go to the creds directory
+    pushd "$GCS_DIR/ros_ws/src/ros2tak_tools/creds/" > /dev/null
+    # Set restrictive permissions on directories (700: rwx------)
+    sudo find . -type d -exec chmod 700 {} \; 2>&1 | tee -a "$LOG_FILE"
+    # Set restrictive permissions on files (600: rw-------)
+    sudo find . -type f -exec chmod 600 {} \; 2>&1 | tee -a "$LOG_FILE"
+    # Ensure proper ownership
+    sudo chown -R $USER:$USER . 2>&1 | tee -a "$LOG_FILE"
+    wintak_log "Credentials directory secured with restricted permissions"
+    # Return to original directory
+    popd > /dev/null
+    
+    # Install VirtualBox with apt logs
+    wintak_log "Installing VirtualBox..."
+    (sudo dpkg -i "$HOME/vmware/virtualbox-7.1_7.1.6-167084~Ubuntu~jammy_amd64.deb" 2>&1 | tee -a "$LOG_FILE")
+    (sudo apt-get install -f -y 2>&1 | tee -a "$LOG_FILE")
+    
+    # Configure VirtualBox
+    wintak_log "Configuring VirtualBox kernel modules..."
+    (sudo /sbin/vboxconfig 2>&1 | tee -a "$LOG_FILE")
+    
+    # Import WinTAK VM
+    wintak_log "Importing WinTAK virtual machine..."
+    (VBoxManage import "$HOME/vmware/Windows11.ova" --vsys 0 --vmname "WinTAK" 2>&1 | tee -a "$LOG_FILE")
+    
+    # Start WinTAK VM
+    wintak_log "Setup complete! Starting WinTAK..."
+    (VBoxManage startvm "WinTAK" 2>&1 | tee -a "$LOG_FILE")
+    
+    wintak_log "WinTAK setup completed successfully"
+    
+    log_info "========================================================="
+    log_info "  WinTAK is now running in VirtualBox"
+    log_info "  To start WinTAK in the future, simply run:"
+    log_info "  airstack wintak:start"
+    log_info "  Setup logs are available at: $LOG_FILE"
+    log_warn "  NOTE: WinTAK will ask you to reset the VM password on first boot. Just choose your own memorable password."
+    log_info "========================================================="
+    
+    return 0
+}
+
+# Function to start WINTAK
+function cmd_wintak_start {
+    log_info "Starting WinTAK..."
+    
+    # Check if VirtualBox is installed
+    if ! command -v VBoxManage &> /dev/null; then
+        log_error "VirtualBox is not installed. Run 'airstack install --with-wintak' first."
+        return 1
+    fi
+    
+    # Check if WinTAK VM exists
+    if ! VBoxManage list vms | grep -q "\"WinTAK\""; then
+        log_error "WinTAK VM not found. Run 'airstack install --with-wintak' first."
+        return 1
+    fi
+    
+    # Start WinTAK VM
+    VBoxManage startvm "WinTAK"
+    
+    log_info "WinTAK started successfully"
+    return 0
+}
+
+# Function to stop WINTAK
+function cmd_wintak_stop {
+    log_info "Stopping WinTAK..."
+    
+    # Check if VirtualBox is installed
+    if ! command -v VBoxManage &> /dev/null; then
+        log_error "VirtualBox is not installed."
+        return 1
+    fi
+    
+    # Check if WinTAK VM exists
+    if ! VBoxManage list vms | grep -q "\"WinTAK\""; then
+        log_error "WinTAK VM not found."
+        return 1
+    fi
+    
+    # Check if WinTAK VM is running
+    if ! VBoxManage list runningvms | grep -q "\"WinTAK\""; then
+        log_warn "WinTAK VM is not running."
+        return 0
+    fi
+    
+    # Stop WinTAK VM
+    VBoxManage controlvm "WinTAK" acpipowerbutton
+    
+    log_info "WinTAK shutdown initiated"
+    return 0
+}
+
+# Register commands from this module
+function register_wintak_commands {
+    COMMANDS["wintak:install"]="cmd_wintak_install"
+    COMMANDS["wintak:start"]="cmd_wintak_start"
+    COMMANDS["wintak:stop"]="cmd_wintak_stop"
+    
+    # Add command help
+    COMMAND_HELP["wintak:install"]="Install WinTAK VirtualBox environment"
+    COMMAND_HELP["wintak:start"]="Start WinTAK virtual machine"
+    COMMAND_HELP["wintak:stop"]="Stop WinTAK virtual machine"
+}
\ No newline at end of file
diff --git a/.env b/.env
index 4a95be83..65b287be 100644
--- a/.env
+++ b/.env
@@ -1,9 +1,10 @@
 # This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml
 PROJECT_NAME="airstack"
-PROJECT_VERSION="0.13.0"
+# auto-generated from git commit hash
+DOCKER_IMAGE_TAG="7c21230"
 # can replace with your docker hub username
 PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
-DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/fire_academy.scene.usd"
+DEFAULT_ISAAC_SCENE="omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/AFCA/fire_academy_faro_with_sky.scene.usd"
 PLAY_SIM_ON_START="true"
 # the file under robot/docker/ that contains the robot's environment variables
-ROBOT_ENV_FILE_NAME="robot.env"
+ROBOT_ENV_FILE_NAME="robot.env"
\ No newline at end of file
diff --git a/.gitmodules b/.gitmodules
index f0f65c11..0a92be86 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -10,3 +10,6 @@
 [submodule "robot/ros_ws/src/autonomy/2_perception/macvo/macvo/src"]
 	path = robot/ros_ws/src/autonomy/2_perception/macvo/macvo/src
 	url = https://github.com/MAC-VO/MAC-VO.git
+[submodule "robot/ros_ws/src/autonomy/2_perception/macvo2"]
+	path = robot/ros_ws/src/autonomy/2_perception/macvo2
+	url = git@github.com:castacks/MAC-VO-ROS2.git
diff --git a/README.md b/README.md
deleted file mode 120000
index 0e01b430..00000000
--- a/README.md
+++ /dev/null
@@ -1 +0,0 @@
-docs/README.md
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 00000000..c5a1fc71
--- /dev/null
+++ b/README.md
@@ -0,0 +1,161 @@
+# AirStack: Democratizing Intelligent Mobile Robotics
+
+<div align="center">
+  <img src="docs/assets/logo_horizontal_color.png" alt="AirStack Logo" width="400"/>
+</div>
+
+AirStack is a comprehensive, modular autonomy stack for embodied AI and robotics developed by the [AirLab](https://theairlab.org) at Carnegie Mellon University's Robotics Institute. It provides a complete framework for developing, testing, and deploying autonomous mobile systems in both simulated and real-world environments.
+
+[![GitHub](https://img.shields.io/github/license/castacks/AirStack)](https://github.com/castacks/AirStack/blob/main/LICENSE)
+[![Documentation](https://img.shields.io/badge/docs-mkdocs-blue)](https://docs.theairlab.org/docs/)
+
+## 🚀 Features
+
+- **Modular Architecture**: Easily swap out components to customize for your specific needs
+- **ROS 2 Integration**: Built on ROS 2 for robust inter-process communication
+- **Simulation Support**: Integrated with NVIDIA Isaac Sim for high-fidelity simulation
+- **Multi-Robot Capability**: Control and coordinate multiple robots simultaneously
+- **Ground Control Station**: Monitor and control robots through an intuitive interface
+- **Comprehensive Autonomy Stack**:
+  - Robot Interface Layer
+  - Sensor Integration
+  - Perception Systems
+  - Local Planning & Control
+  - Global Planning
+  - Behavior Management
+
+## 📋 System Requirements
+
+- **Docker**: With NVIDIA Container Toolkit support
+- **NVIDIA GPU**: RTX 3070 or better (for local Isaac Sim)
+- **Storage**: At least 25GB free space for Docker images
+- **OS**: Ubuntu 22.04 recommended
+
+## 🔧 Quick Start
+
+### 1. Clone the Repository
+
+```bash
+git clone --recursive -j8 git@github.com:castacks/AirStack.git
+cd AirStack
+```
+
+### 2. Install Docker with NVIDIA Support
+
+Follow [NVIDIA's instructions](https://docs.nvidia.com/ai-enterprise/deployment/vmware/latest/docker.html) for installing Docker with NVIDIA GPU support. Make sure `docker-compose-plugin` is also installed.
+
+### 3. Configure the Repository
+
+```bash
+./configure.sh
+```
+
+Follow the prompts to complete the initial configuration.
+
+### 4. Get the Docker Images
+
+#### Option 1: Pull from AirLab Registry (Preferred)
+
+```bash
+docker login airlab-storage.andrew.cmu.edu:5001
+# Enter your andrew id (without @andrew.cmu.edu)
+# Enter your andrew password
+
+# Pull the images in the docker compose file
+docker compose pull
+```
+
+#### Option 2: Build Docker Images From Scratch
+
+```bash
+# Download the Ascent Spirit SITL software package
+bash simulation/isaac-sim/installation/download_sitl.bash
+
+# Build the images locally (requires NVIDIA NGC access)
+docker compose build
+```
+
+### 5. Launch the System
+
+```bash
+xhost +  # allow docker access to X-Server
+
+# Start docker compose services
+docker compose up -d
+# For multiple robots: docker compose up -d --scale robot=3
+```
+
+This will automatically launch and play the Isaac Sim scene specified in the `.env` file.
+
+### 6. Control the Robot
+
+Find the RQT GUI window:
+1. Click "Arm and Takeoff"
+2. Click "Global Plan" in the trajectory window
+
+You can also switch to "Fixed Trajectory" mode and click "Publish" to follow a predefined trajectory.
+
+### 7. Shutdown
+
+```bash
+docker compose down
+```
+
+## 🏗️ System Architecture
+
+AirStack follows a layered architecture approach:
+
+```
+Robot
+├── Interface Layer: Communication with robot controllers
+├── Sensors Layer: Data acquisition from various sensors
+├── Perception Layer: State estimation and environment understanding
+├── Local Layer: 
+│   ├── World Model: Local environment representation
+│   ├── Planning: Trajectory generation and obstacle avoidance
+│   └── Controls: Trajectory following
+├── Global Layer:
+│   ├── World Model: Global environment mapping
+│   └── Planning: Mission-level path planning
+└── Behavior Layer: High-level decision making
+```
+
+## 📁 Repository Structure
+
+- `robot/`: Contains the ROS 2 workspace for the robot autonomy stack
+- `ground_control_station/`: Software for monitoring and controlling robots
+- `simulation/`: Integration with Isaac Sim and simulation environments
+- `docs/`: Comprehensive documentation
+- `common/`: Shared libraries and utilities
+- `tests/`: Testing infrastructure
+
+## 🧪 Development
+
+AirStack is designed with modularity in mind, making it straightforward to extend or replace components. The development workflow is centered around Docker containers for consistent environments.
+
+For detailed development guidelines, see the [Developer Guide](https://docs.theairlab.org/docs/development/).
+
+## 📚 Documentation
+
+Comprehensive documentation is available at [https://docs.theairlab.org/docs/](https://docs.theairlab.org/docs/)
+
+The documentation covers:
+- Getting started guides
+- Development workflows
+- Component descriptions
+- API references
+- Simulation setup
+- Real-world deployment
+
+## 🤝 Contributing
+
+We welcome contributions to AirStack! Please see our [Contributing Guidelines](https://docs.theairlab.org/docs/development/contributing/) for more information.
+
+## 📄 License
+
+AirStack is licensed under the Apache 2.0 or MIT license (to be finalized).
+
+
+## 📧 Contact
+
+For questions or support, please contact the AirLab team at [theairlab.org](https://theairlab.org).
\ No newline at end of file
diff --git a/airstack.sh b/airstack.sh
new file mode 100755
index 00000000..68e0bd17
--- /dev/null
+++ b/airstack.sh
@@ -0,0 +1,793 @@
+#!/usr/bin/env bash
+
+# airstack - A convenience tool for AirStack development
+# 
+# This script provides a unified interface for common development tasks
+# in the AirStack project, including setup, installation, and container management.
+
+set -e
+
+# Script directory
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+PROJECT_ROOT="$SCRIPT_DIR"
+
+# Directory for command modules
+MODULES_DIR="$PROJECT_ROOT/.airstack/modules"
+
+# Create modules directory if it doesn't exist
+mkdir -p "$MODULES_DIR"
+
+# Color codes for output formatting
+RED='\033[0;31m'
+GREEN='\033[0;32m'
+YELLOW='\033[0;33m'
+BLUE='\033[0;34m'
+NC='\033[0m' # No Color
+
+# Print usage information
+function print_usage {
+    echo -e "${BLUE}AirStack Development Tool${NC}"
+    echo ""
+    echo "Usage: airstack <command> [options]"
+    echo ""
+    echo "Available commands:"
+    
+    # Sort commands alphabetically
+    local sorted_ommands=($(echo "${!COMMANDS[@]}" | tr ' ' '\n' | sort))
+    
+    # Calculate the longest command name for padding
+    local max_len=0
+    for cmd in "${sorted_commands[@]}"; do
+        if [ ${#cmd} -gt $max_len ]; then
+            max_len=${#cmd}
+        fi
+    done
+    
+    # Add 4 spaces of padding
+    max_len=$((max_len + 4))
+    
+    # Print each command with its help text
+    for cmd in "${sorted_commands[@]}"; do
+        # Skip hidden commands (those starting with _)
+        if [[ "$cmd" == _* ]]; then
+            continue
+        fi
+        
+        # Get help text or use a default
+        local help_text="${COMMAND_HELP[$cmd]:-No description available}"
+        
+        # Calculate padding
+        local padding=$((max_len - ${#cmd}))
+        local pad=$(printf '%*s' "$padding" '')
+        
+        echo "  $cmd$pad$help_text"
+    done
+    
+    echo ""
+    echo "For more information on a command, run: airstack help <command>"
+    echo "To see all available commands, run: airstack commands"
+    echo ""
+    echo "Note: The airstack command will automatically use the airstack.sh script from the"
+    echo "current directory or nearest parent directory containing an AirStack repository."
+}
+
+# Print command-specific help
+function print_command_help {
+    local command="$1"
+    
+    # Check if command exists
+    if [[ -z "${COMMANDS[$command]}" ]]; then
+        log_error "Unknown command: $command"
+        print_usage
+        return 1
+    fi
+    
+    # Get help text
+    local help_text="${COMMAND_HELP[$command]:-No description available}"
+    
+    # Print command-specific help
+    echo -e "${BLUE}airstack $command${NC} - $help_text"
+    echo ""
+    
+    # Command-specific usage and options
+    case "$command" in
+        install)
+            echo "Usage: airstack install [options]"
+            echo ""
+            echo "Options:"
+            echo "  --force       Force reinstallation of components"
+            echo "  --no-docker   Skip Docker installation"
+            echo "  --with-wintak Install WinTAK VirtualBox environment"
+            ;;
+        setup)
+            echo "Usage: airstack setup [options]"
+            echo ""
+            echo "Options:"
+            echo "  --no-shell    Don't modify shell configuration"
+            echo "  --no-config   Skip configuration tasks (Isaac Sim, Nucleus, Git hooks)"
+            echo ""
+            echo "This command adds an 'airstack' function to your shell profile that will"
+            echo "automatically find and use the airstack.sh script from the current directory"
+            echo "or nearest parent directory containing an AirStack repository."
+            ;;
+        up)
+            echo "Usage: airstack up [service_name] [options]"
+            echo ""
+            echo "Options:"
+            echo "  --build       Build images before starting containers"
+            echo "  --recreate    Recreate containers even if their configuration and image haven't changed"
+            ;;
+        connect)
+            echo "Usage: airstack connect [container_name] [options]"
+            echo ""
+            echo "Options:"
+            echo "  --command=CMD  Run specific command instead of shell (default: bash)"
+            ;;
+        down)
+            echo "Usage: airstack down [service_name]"
+            echo ""
+            echo "If no service name is provided, all services will be shutdown."
+            ;;
+        status)
+            echo "Usage: airstack status"
+            echo ""
+            echo "Shows the status of all running containers."
+            ;;
+        logs)
+            echo "Usage: airstack logs [container_name] [options]"
+            echo ""
+            echo "View logs for the specified container."
+            echo ""
+            echo "Options:"
+            echo "  --no-follow    Don't follow log output"
+            echo "  --tail=N       Show only the last N lines (default: all)"
+            ;;
+        test)
+            echo "Usage: airstack test [options]"
+            echo ""
+            echo "Options:"
+            echo "  --path=PATH    Path to test directory"
+            echo "  --filter=PATTERN  Filter tests by pattern"
+            ;;
+        docs)
+            echo "Usage: airstack docs [serve]"
+            echo ""
+            echo "Build documentation. If 'serve' is specified, also start a local server."
+            ;;
+        *)
+            # For commands without specific help, just show the general help
+            echo "Usage: airstack $command [options]"
+            echo ""
+            echo "For more information, check the documentation or source code."
+            ;;
+    esac
+}
+
+# Log messages with different severity levels
+function log_info {
+    echo -e "${GREEN}[INFO]${NC} $1"
+}
+
+function log_warn {
+    echo -e "${YELLOW}[WARN]${NC} $1"
+}
+
+function log_error {
+    echo -e "${RED}[ERROR]${NC} $1" >&2
+}
+
+# Check if Docker is installed and running
+function check_docker {
+    if ! command -v docker &> /dev/null; then
+        log_error "Docker is not installed. Run 'airstack install' first."
+        exit 1
+    fi
+
+    if ! docker info &> /dev/null; then
+        log_error "Docker daemon is not running."
+        exit 1
+    fi
+}
+
+# Find container by partial name using regex
+function find_container {
+    local search_term="$1"
+    local containers
+    
+    # Get list of running containers
+    containers=$(docker ps --format "{{.Names}}\t{{.Image}}\t{{.Status}}")
+    
+    # Find matches using grep
+    matches=$(echo "$containers" | grep -i "$search_term" || true)
+    match_count=$(echo "$matches" | grep -v "^$" | wc -l)
+    
+    if [ "$match_count" -eq 0 ]; then
+        # Try a more flexible search if exact match fails
+        log_warn "No exact matches for '$search_term', trying fuzzy search..."
+        matches=$(echo "$containers" | grep -i ".*$search_term.*" || true)
+        match_count=$(echo "$matches" | grep -v "^$" | wc -l)
+        
+        if [ "$match_count" -eq 0 ]; then
+            log_error "No running containers match '$search_term'"
+            
+            # Show available containers as a suggestion
+            available=$(docker ps --format "{{.Names}}")
+            if [ -n "$available" ]; then
+                log_info "Available containers:"
+                echo "$available"
+            fi
+            
+            return 1
+        fi
+    fi
+    
+    if [ "$match_count" -eq 1 ]; then
+        # Extract just the container name (first column)
+        container_name=$(echo "$matches" | awk '{print $1}')
+        echo "$container_name"
+        return 0
+    else
+        log_warn "Multiple containers match '$search_term'. Please be more specific or select from the list below:"
+        # Format the output as a table with numbers
+        echo -e "${BLUE}NUM\tCONTAINER NAME\tIMAGE\tSTATUS${NC}"
+        echo "$matches" | awk '{print NR "\t" $0}'
+        echo ""
+        echo "Options:"
+        echo "  1. Enter a number to select a container"
+        echo "  2. Type 'q' to quit"
+        echo "  3. Press Ctrl+C to cancel and try again with a more specific name"
+        echo ""
+        read -p "Your selection: " selection
+        
+        if [ "$selection" = "q" ]; then
+            log_info "Operation cancelled"
+            return 1
+        elif [[ "$selection" =~ ^[0-9]+$ ]] && [ "$selection" -gt 0 ] && [ "$selection" -le "$match_count" ]; then
+            # Extract just the container name from the selected line
+            container_name=$(echo "$matches" | sed -n "${selection}p" | awk '{print $1}')
+            echo "$container_name"
+            
+            # Provide a tip for future use
+            log_info "Tip: Next time, you can directly use 'airstack connect $container_name' for this container"
+            return 0
+        else
+            log_error "Invalid selection. Please enter a number between 1 and $match_count, or 'q' to quit."
+            
+            # Give the user another chance to select
+            echo ""
+            read -p "Try again (or 'q' to quit): " selection
+            
+            if [ "$selection" = "q" ]; then
+                log_info "Operation cancelled"
+                return 1
+            elif [[ "$selection" =~ ^[0-9]+$ ]] && [ "$selection" -gt 0 ] && [ "$selection" -le "$match_count" ]; then
+                container_name=$(echo "$matches" | sed -n "${selection}p" | awk '{print $1}')
+                echo "$container_name"
+                return 0
+            else
+                log_error "Invalid selection again. Please try the command again with a more specific container name."
+                return 1
+            fi
+        fi
+    fi
+}
+
+# Command implementations
+function cmd_install {
+    log_info "Installing dependencies..."
+    
+    # Check for --force flag
+    local force=false
+    # Check for --no-docker flag
+    local install_docker=true
+    # Check for --with-wintak flag
+    local install_wintak=false
+    
+    for arg in "$@"; do
+        if [ "$arg" == "--force" ]; then
+            force=true
+        elif [ "$arg" == "--no-docker" ]; then
+            install_docker=false
+        elif [ "$arg" == "--with-wintak" ]; then
+            install_wintak=true
+        fi
+    done
+    
+    # Install Docker if needed
+    if [ "$install_docker" = true ]; then
+        if command -v docker &> /dev/null && [ "$force" = false ]; then
+            log_info "Docker is already installed"
+        else
+            log_info "Installing Docker..."
+            
+            # Detect OS
+            if [ -f /etc/os-release ]; then
+                . /etc/os-release
+                OS=$ID
+            else
+                OS=$(uname -s)
+            fi
+            
+            case "$OS" in
+                ubuntu|debian)
+                    log_info "Detected Debian/Ubuntu system"
+                    
+                    # Update package index
+                    log_info "Updating package index..."
+                    sudo apt-get update
+                    
+                    # Install prerequisites
+                    log_info "Installing prerequisites..."
+                    sudo apt-get install -y apt-transport-https ca-certificates curl gnupg lsb-release
+                    
+                    # Add Docker's official GPG key
+                    log_info "Adding Docker's GPG key..."
+                    curl -fsSL https://download.docker.com/linux/$OS/gpg | sudo gpg --dearmor -o /usr/share/keyrings/docker-archive-keyring.gpg
+                    
+                    # Set up the stable repository
+                    log_info "Setting up Docker repository..."
+                    echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/docker-archive-keyring.gpg] https://download.docker.com/linux/$OS $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
+                    
+                    # Update package index again
+                    sudo apt-get update
+                    
+                    # Install Docker Engine
+                    log_info "Installing Docker Engine..."
+                    sudo apt-get install -y docker-ce docker-ce-cli containerd.io docker-compose-plugin
+                    ;;
+                    
+                fedora|centos|rhel)
+                    log_info "Detected Fedora/CentOS/RHEL system"
+                    
+                    # Install prerequisites
+                    log_info "Installing prerequisites..."
+                    sudo dnf -y install dnf-plugins-core
+                    
+                    # Add Docker repository
+                    log_info "Setting up Docker repository..."
+                    sudo dnf config-manager --add-repo https://download.docker.com/linux/$OS/docker-ce.repo
+                    
+                    # Install Docker Engine
+                    log_info "Installing Docker Engine..."
+                    sudo dnf -y install docker-ce docker-ce-cli containerd.io docker-compose-plugin
+                    ;;
+                    
+                darwin)
+                    log_info "Detected macOS system"
+                    log_info "Please install Docker Desktop for Mac from https://www.docker.com/products/docker-desktop"
+                    log_info "After installation, run 'airstack setup' to complete the configuration"
+                    ;;
+                    
+                *)
+                    log_error "Unsupported operating system: $OS"
+                    log_info "Please install Docker manually from https://docs.docker.com/engine/install/"
+                    ;;
+            esac
+            
+            # Start Docker service if not running
+            if systemctl status docker &>/dev/null; then
+                log_info "Starting Docker service..."
+                sudo systemctl enable --now docker
+                
+                # Add current user to docker group
+                log_info "Adding user to docker group..."
+                sudo usermod -aG docker "$USER"
+                log_info "You may need to log out and back in for group changes to take effect"
+            fi
+            
+            log_info "Docker installation complete"
+        fi
+        
+        # Install Docker Compose if needed
+        if ! command -v docker compose &> /dev/null && [ "$force" = false ]; then
+            log_info "Installing Docker Compose..."
+            
+            # Docker Compose is now included with Docker Engine as a plugin
+            # For older Docker versions, we'll install the standalone version
+            if docker --help | grep -q "compose"; then
+                log_info "Docker Compose plugin is already installed"
+            else
+                log_info "Installing Docker Compose standalone version..."
+                COMPOSE_VERSION=$(curl -s https://api.github.com/repos/docker/compose/releases/latest | grep 'tag_name' | cut -d\" -f4)
+                sudo curl -L "https://github.com/docker/compose/releases/download/${COMPOSE_VERSION}/docker-compose-$(uname -s)-$(uname -m)" -o /usr/local/bin/docker-compose
+                sudo chmod +x /usr/local/bin/docker-compose
+                log_info "Docker Compose installation complete"
+            fi
+        fi
+    fi
+    
+    # Install WINTAK if requested
+    if [ "$install_wintak" = true ]; then
+        # Check if the wintak module is available
+        if declare -f "cmd_wintak_install" > /dev/null; then
+            log_info "Installing WINTAK..."
+            cmd_wintak_install
+        else
+            log_error "WINTAK module not loaded. Cannot install WINTAK."
+            log_info "Please make sure the wintak.sh module is in the .airstack/modules directory."
+        fi
+    fi
+    
+    log_info "Installation complete!"
+}
+
+function cmd_setup {
+    log_info "Setting up AirStack environment..."
+    
+    # Check for --no-shell flag
+    local modify_shell=true
+    local skip_config=false
+    
+    for arg in "$@"; do
+        if [ "$arg" == "--no-shell" ]; then
+            modify_shell=false
+        elif [ "$arg" == "--no-config" ]; then
+            skip_config=true
+        fi
+    done
+    
+    # Add to shell profile if requested
+    if [ "$modify_shell" = true ]; then
+        local shell_profile=""
+        
+        if [ -f "$HOME/.zshrc" ]; then
+            shell_profile="$HOME/.zshrc"
+        elif [ -f "$HOME/.bashrc" ]; then
+            shell_profile="$HOME/.bashrc"
+        else
+            log_warn "Could not determine shell profile. Please add the airstack function to your shell profile manually."
+            echo "You can find the function definition in the setup section of $PROJECT_ROOT/airstack.sh"
+            return
+        fi
+        
+        if grep -q "# AirStack function" "$shell_profile"; then
+            log_info "'airstack' function already registered"
+        else
+            log_info "Adding 'airstack' function in $shell_profile"
+            echo "" >> "$shell_profile"
+            echo "# AirStack function" >> "$shell_profile"
+            echo 'function airstack() {' >> "$shell_profile"
+            echo '    # Start from the current directory and look for airstack.sh' >> "$shell_profile"
+            echo '    local current_dir="$(pwd)"' >> "$shell_profile"
+            echo '    local found_scripts=()' >> "$shell_profile"
+            echo '    local script_path=""' >> "$shell_profile"
+            echo '' >> "$shell_profile"
+            echo '    # First check if there is an airstack.sh in the current directory or any parent directory' >> "$shell_profile"
+            echo '    while [[ "$current_dir" != "" ]]; do' >> "$shell_profile"
+            echo '        if [[ -f "$current_dir/airstack.sh" ]]; then' >> "$shell_profile"
+            echo '            found_scripts+=("$current_dir/airstack.sh")' >> "$shell_profile"
+            echo '            # We found one, but keep looking to check for ambiguities' >> "$shell_profile"
+            echo '        fi' >> "$shell_profile"
+            echo '        # Stop if we reach the root directory' >> "$shell_profile"
+            echo '        if [[ "$current_dir" == "/" ]]; then' >> "$shell_profile"
+            echo '            break' >> "$shell_profile"
+            echo '        fi' >> "$shell_profile"
+            echo '        # Move up one directory' >> "$shell_profile"
+            echo '        current_dir="$(dirname "$current_dir")"' >> "$shell_profile"
+            echo '    done' >> "$shell_profile"
+            echo '' >> "$shell_profile"
+            echo '    # Check how many scripts we found' >> "$shell_profile"
+            echo '    if [[ ${#found_scripts[@]} -eq 0 ]]; then' >> "$shell_profile"
+            echo '        echo -e "\033[0;31m[ERROR]\033[0m No airstack.sh script found in the current directory or any parent directory."' >> "$shell_profile"
+            echo '        return 1' >> "$shell_profile"
+            echo '    elif [[ ${#found_scripts[@]} -gt 1 ]]; then' >> "$shell_profile"
+            echo '        echo -e "\033[0;31m[ERROR]\033[0m Multiple airstack.sh scripts found in the directory hierarchy:"' >> "$shell_profile"
+            echo '        for script in "${found_scripts[@]}"; do' >> "$shell_profile"
+            echo '            echo "  - $script"' >> "$shell_profile"
+            echo '        done' >> "$shell_profile"
+            echo '        echo "Please cd to the specific AirStack repository you want to use."' >> "$shell_profile"
+            echo '        return 1' >> "$shell_profile"
+            echo '    else' >> "$shell_profile"
+            echo '        # We found exactly one script, use it' >> "$shell_profile"
+            echo '        script_path="${found_scripts[@]:0:1}"' >> "$shell_profile"
+            echo '        "$script_path" "$@"' >> "$shell_profile"
+            echo '    fi' >> "$shell_profile"
+            echo '}' >> "$shell_profile"
+            echo "Added to $shell_profile. Please restart your shell or run 'source $shell_profile'. Then you'll be able to use the 'airstack' command from any sub-directory."
+        fi
+    fi
+    
+    # Run configuration tasks if not skipped
+    if [ "$skip_config" = false ]; then
+        # Check if the config module is available
+        if declare -f "cmd_config_all" > /dev/null; then
+            log_info "Running configuration tasks..."
+            cmd_config_all
+        else
+            log_warn "Configuration module not loaded. Skipping configuration tasks."
+        fi
+    fi
+    
+    log_info "Setup complete!"
+}
+
+function cmd_up {
+    check_docker
+    
+    # Build docker-compose command
+    local cmd="docker compose -f $PROJECT_ROOT/docker-compose.yaml up $@ -d"
+    
+    eval "$cmd"
+    log_info "Services brought up successfully"
+}
+
+function cmd_down {
+    check_docker
+    
+    local services=("$@")
+    
+    # Build docker-compose command
+    local cmd="docker compose -f $PROJECT_ROOT/docker-compose.yaml --profile '*' down"
+    
+    # Add services if specified
+    if [ ${#services[@]} -gt 0 ]; then
+        cmd="docker compose -f $PROJECT_ROOT/docker-compose.yaml down ${services[*]}"
+    fi
+    
+    log_info "Shutting down services: ${services[*]:-all}"
+    eval "$cmd"
+    log_info "Services shutdown successfully"
+}
+
+function cmd_connect {
+    check_docker
+    
+    if [ $# -eq 0 ]; then
+        log_error "Container name required"
+        print_command_help "connect"
+        
+        # Show available containers as a helpful suggestion
+        local available=$(docker ps --format "{{.Names}}")
+        if [ -n "$available" ]; then
+            log_info "Available containers:"
+            echo "$available"
+        fi
+        
+        exit 1
+    fi
+    
+    local container_pattern="$1"
+    shift
+    
+    # Default command is bash, but can be overridden
+    local command="bash"
+    for arg in "$@"; do
+        if [[ "$arg" == --command=* ]]; then
+            command="${arg#--command=}"
+        fi
+    done
+    
+    # Find container by pattern
+    local container
+    container=$(find_container "$container_pattern")
+    
+    if [ $? -eq 0 ]; then
+        log_info "Connecting to container: $container"
+        
+        # Check if the command exists in the container
+        if ! docker exec "$container" which "$command" &> /dev/null; then
+            log_warn "Command '$command' not found in container. Falling back to /bin/sh"
+            command="sh"
+        fi
+        
+        # Connect to the container
+        docker exec -it "$container" "$command"
+        
+        # Check exit status
+        local exit_status=$?
+        if [ $exit_status -ne 0 ]; then
+            log_warn "Command exited with status $exit_status"
+        fi
+    else
+        log_error "Failed to connect to container. Please try again with a more specific name."
+    fi
+}
+
+function cmd_status {
+    check_docker
+    
+    log_info "AirStack container status:"
+    docker ps --format "table {{.Names}}\t{{.Status}}\t{{.Ports}}"
+}
+
+function cmd_logs {
+    check_docker
+    
+    if [ $# -eq 0 ]; then
+        log_error "Container name required"
+        
+        # Show available containers as a helpful suggestion
+        local available=$(docker ps --format "{{.Names}}")
+        if [ -n "$available" ]; then
+            log_info "Available containers:"
+            echo "$available"
+        fi
+        
+        exit 1
+    fi
+    
+    local container_pattern="$1"
+    shift
+    
+    # Parse options
+    local follow=true
+    local tail="all"
+    
+    for arg in "$@"; do
+        if [ "$arg" == "--no-follow" ]; then
+            follow=false
+        elif [[ "$arg" == --tail=* ]]; then
+            tail="${arg#--tail=}"
+        fi
+    done
+    
+    # Find container by pattern
+    local container
+    container=$(find_container "$container_pattern")
+    
+    if [ $? -eq 0 ]; then
+        log_info "Showing logs for container: $container"
+        
+        # Build the logs command
+        local cmd="docker logs"
+        
+        if [ "$follow" = true ]; then
+            cmd="$cmd -f"
+        fi
+        
+        if [ "$tail" != "all" ]; then
+            cmd="$cmd --tail $tail"
+        fi
+        
+        # Execute the command
+        eval "$cmd $container"
+    else
+        log_error "Failed to find container. Please try again with a more specific name."
+    fi
+}
+
+# Function to load external command modules
+function load_command_modules {
+    # Skip if modules directory doesn't exist
+    if [ ! -d "$MODULES_DIR" ]; then
+        return
+    fi
+    
+    # Load all .sh files in the modules directory
+    for module in "$MODULES_DIR"/*.sh; do
+        if [ -f "$module" ]; then
+            # Source the module file
+            source "$module"
+            
+            # Extract module name from filename
+            module_name=$(basename "$module" .sh)
+            
+            # Register the module's commands if it has a register_commands function
+            if declare -f "register_${module_name}_commands" > /dev/null; then
+                "register_${module_name}_commands"
+            fi
+        fi
+    done
+}
+
+# Arrays to store available commands and their help text
+declare -A COMMANDS
+declare -A COMMAND_HELP
+
+# Register built-in commands
+function register_builtin_commands {
+    COMMANDS["install"]="cmd_install"
+    COMMANDS["setup"]="cmd_setup"
+    COMMANDS["up"]="cmd_up"
+    COMMANDS["down"]="cmd_down"
+    COMMANDS["connect"]="cmd_connect"
+    COMMANDS["status"]="cmd_status"
+    COMMANDS["logs"]="cmd_logs"
+    COMMANDS["help"]="cmd_help"
+    
+    # Register help text for built-in commands
+    COMMAND_HELP["install"]="Install dependencies (Docker Engine, Docker Compose, etc.)"
+    COMMAND_HELP["setup"]="Configure AirStack settings and add to shell profile"
+    COMMAND_HELP["up"]="Start services using Docker Compose"
+    COMMAND_HELP["down"]="down services"
+    COMMAND_HELP["connect"]="Connect to a running container (supports partial name matching)"
+    COMMAND_HELP["status"]="Show status of all containers"
+    COMMAND_HELP["logs"]="View logs for a container (supports partial name matching)"
+    COMMAND_HELP["help"]="Show help information"
+}
+
+# Help command implementation
+function cmd_help {
+    if [ $# -eq 0 ]; then
+        print_usage
+    else
+        print_command_help "$1"
+    fi
+}
+
+# Function to list all available commands
+function list_commands {
+    echo -e "${BLUE}All Available AirStack Commands:${NC}"
+    echo ""
+    
+    # Sort commands alphabetically
+    local sorted_commands=($(echo "${!COMMANDS[@]}" | tr ' ' '\n' | sort))
+    
+    # Calculate the longest command name for padding
+    local max_len=0
+    for cmd in "${sorted_commands[@]}"; do
+        if [ ${#cmd} -gt $max_len ]; then
+            max_len=${#cmd}
+        fi
+    done
+    
+    # Add 4 spaces of padding
+    max_len=$((max_len + 4))
+    
+    # Group commands by module (based on prefix)
+    declare -A modules
+    
+    for cmd in "${sorted_commands[@]}"; do
+        # Skip hidden commands (those starting with _)
+        if [[ "$cmd" == _* ]]; then
+            continue
+        fi
+        
+        # Determine module based on command name prefix
+        local module="core"
+        if [[ "$cmd" == *_* ]]; then
+            module="${cmd%%_*}"
+        fi
+        
+        # Add command to its module group
+        modules["$module"]+="$cmd "
+    done
+    
+    # Print commands by module
+    for module in $(echo "${!modules[@]}" | tr ' ' '\n' | sort); do
+        echo -e "${YELLOW}$module:${NC}"
+        
+        for cmd in ${modules["$module"]}; do
+            # Get help text or use a default
+            local help_text="${COMMAND_HELP[$cmd]:-No description available}"
+            
+            # Calculate padding
+            local padding=$((max_len - ${#cmd}))
+            local pad=$(printf '%*s' "$padding" '')
+            
+            echo "  $cmd$pad$help_text"
+        done
+        
+        echo ""
+    done
+}
+
+# Register built-in commands
+register_builtin_commands
+
+# Load external command modules
+load_command_modules
+
+# Main command dispatcher
+if [ $# -eq 0 ]; then
+    print_usage
+    exit 0
+fi
+
+command="$1"
+shift
+
+# Check if command exists
+if [[ -n "${COMMANDS[$command]}" ]]; then
+    # Execute the command function
+    ${COMMANDS[$command]} "$@"
+elif [ "$command" == "commands" ]; then
+    # Special case for listing all commands
+    list_commands
+else
+    log_error "Unknown command: $command"
+    print_usage
+    exit 1
+fi
+
+exit 0
\ No newline at end of file
diff --git a/common/ros_packages/straps_msgs b/common/ros_packages/straps_msgs
new file mode 160000
index 00000000..29098763
--- /dev/null
+++ b/common/ros_packages/straps_msgs
@@ -0,0 +1 @@
+Subproject commit 290987632f7486bca099f5552f49f4f9dd100112
diff --git a/configure.sh b/configure.sh
deleted file mode 100755
index a07af52c..00000000
--- a/configure.sh
+++ /dev/null
@@ -1,58 +0,0 @@
-#!/bin/bash
-
-# setup
-
-SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
-BOLDCYAN="\e[1;96m"
-YELLOW="\e[;33m"
-ENDCOLOR="\e[0m"
-
-confirm_no() { #https://stackoverflow.com/questions/3231804/in-bash-how-to-add-are-you-sure-y-n-to-any-command-or-alias
-    read -r -p "${1:-Are you sure? [y/N]} " response
-    case "$response" in
-        [yY][eE][sS]|[yY]) 
-            true
-            ;;
-        *)
-            false
-            ;;
-    esac
-}
-
-
-# Generate user.config.json
-USER_CONFIG_JSON_SOURCE=${SCRIPT_DIR}/simulation/isaac-sim/docker/user_TEMPLATE.config.json
-USER_CONFIG_JSON_DESTINATION=${SCRIPT_DIR}/simulation/isaac-sim/docker/user.config.json
-
-echo -e "${BOLDCYAN}1. Generating Default IsaacSim Config ($USER_CONFIG_JSON_DESTINATION)${ENDCOLOR}"
-
-if [ -d $USER_CONFIG_JSON_DESTINATION ] && [ $(ls -A $USER_CONFIG_JSON_DESTINATION | wc -l) == 0 ]; then
-    # delete an empty directory with the same name as $USER_CONFIG_JSON_DESTINATION which gets created when
-    # docker compose up is run before this script. Doing this will create a directory name user.config.json because
-    # it is being mounted as a volume but it doesn't exist yet.
-    rm -rf $USER_CONFIG_JSON_DESTINATION
-fi
-
-if [ -f $USER_CONFIG_JSON_DESTINATION ]; then
-    echo -e "${YELLOW}WARNING: The file $USER_CONFIG_JSON_DESTINATION already exists.${ENDCOLOR}"
-    confirm_no "Do you want to reset it to the default? [y/N]" && cp $USER_CONFIG_JSON_SOURCE $USER_CONFIG_JSON_DESTINATION
-else
-    cp $USER_CONFIG_JSON_SOURCE $USER_CONFIG_JSON_DESTINATION
-fi
-
-
-# AirLab Nucleus Login Config
-OMNI_PASS_SOURCE=${SCRIPT_DIR}/simulation/isaac-sim/docker/omni_pass_TEMPLATE.env
-OMNI_PASS_DESTINATION=${SCRIPT_DIR}/simulation/isaac-sim/docker/omni_pass.env
-
-echo -e "${BOLDCYAN}2. Configure AirLab Nucleus Login ($OMNI_PASS_DESTINATION)${ENDCOLOR}"
-
-echo "Go to https://airlab-storage.andrew.cmu.edu:8443/omni/web3/, log in, then right click on the cloud and click the \"API Tokens\" window to generate an API token and paste it here. Leave this blank if you want to skip this step: "
-if [ -f $OMNI_PASS_DESTINATION ]; then
-    echo -e "${YELLOW}WARNING: The file $USER_CONFIG_JSON_DESTINATION already exists, leave it blank to skip.${ENDCOLOR}"
-fi
-read -r -p "API Token: " API_TOKEN
-
-if [ ! -z "${API_TOKEN}" ]; then
-    sed "s/PASTE-YOUR-API-TOKEN/$API_TOKEN/g" $OMNI_PASS_SOURCE > $OMNI_PASS_DESTINATION
-fi
diff --git a/docker-compose.yaml b/docker-compose.yaml
index 4f7aa45a..8e119883 100644
--- a/docker-compose.yaml
+++ b/docker-compose.yaml
@@ -1,4 +1,8 @@
 # top-level docker-compose file for the autonomy stack
+# This file includes all component docker-compose files
+# Each component has its own docker-compose.yaml file
+# Components are organized by functionality
+# This allows for modular development and deployment
 include: 
   - simulation/isaac-sim/docker/docker-compose.yaml
   - robot/docker/docker-compose.yaml
diff --git a/docs/assets/logo_horizontal_color.png b/docs/assets/logo_horizontal_color.png
new file mode 100644
index 00000000..ccc1c401
Binary files /dev/null and b/docs/assets/logo_horizontal_color.png differ
diff --git a/docs/development/airstack-cli/architecture.md b/docs/development/airstack-cli/architecture.md
new file mode 100644
index 00000000..ed5e54ef
--- /dev/null
+++ b/docs/development/airstack-cli/architecture.md
@@ -0,0 +1,215 @@
+# AirStack CLI Architecture
+
+This document describes the architecture of the AirStack CLI tool, explaining how it's organized and how the different components work together.
+
+## Overview
+
+The AirStack CLI tool is built as a modular Bash application with the following components:
+
+- **Main Script (`airstack.sh`)**: The entry point that handles command dispatching and provides core utilities
+- **Command Modules**: Separate scripts that implement specific functionality
+- **Command Registry**: A system for registering and discovering commands
+- **Help System**: A system for providing help text for commands
+
+## Directory Structure
+
+```
+AirStack/
+├── airstack.sh              # Main script
+├── .airstack/               # AirStack configuration directory
+│   └── modules/             # Command modules directory
+│       ├── config.sh        # Configuration module
+│       ├── wintak.sh        # WinTAK module
+│       └── ...              # Other modules
+```
+
+## Main Script
+
+The main script (`airstack.sh`) serves as the entry point for the AirStack CLI tool. It:
+
+1. Sets up the environment
+2. Loads command modules
+3. Registers built-in commands
+4. Dispatches commands to the appropriate handler
+
+### Command Dispatching
+
+The main script uses a command dispatcher pattern:
+
+```bash
+# Main command dispatcher
+if [ $# -eq 0 ]; then
+    print_usage
+    exit 0
+fi
+
+command="$1"
+shift
+
+# Check if command exists
+if [[ -n "${COMMANDS[$command]}" ]]; then
+    # Execute the command function
+    ${COMMANDS[$command]} "$@"
+elif [ "$command" == "commands" ]; then
+    # Special case for listing all commands
+    list_commands
+else
+    log_error "Unknown command: $command"
+    print_usage
+    exit 1
+fi
+```
+
+This pattern allows for dynamic command registration and execution.
+
+## Command Registry
+
+Commands are registered in associative arrays:
+
+```bash
+# Arrays to store available commands and their help text
+declare -A COMMANDS
+declare -A COMMAND_HELP
+```
+
+Each module registers its commands by adding entries to these arrays:
+
+```bash
+function register_mymodule_commands {
+    COMMANDS["mymodule:action"]="cmd_mymodule_action"
+    COMMAND_HELP["mymodule:action"]="Description of your command"
+}
+```
+
+## Module Loading
+
+Modules are loaded dynamically from the `.airstack/modules/` directory:
+
+```bash
+function load_command_modules {
+    # Skip if modules directory doesn't exist
+    if [ ! -d "$MODULES_DIR" ]; then
+        return
+    fi
+    
+    # Load all .sh files in the modules directory
+    for module in "$MODULES_DIR"/*.sh; do
+        if [ -f "$module" ]; then
+            # Source the module file
+            source "$module"
+            
+            # Extract module name from filename
+            module_name=$(basename "$module" .sh)
+            
+            # Register the module's commands if it has a register_commands function
+            if declare -f "register_${module_name}_commands" > /dev/null; then
+                "register_${module_name}_commands"
+            fi
+        fi
+    done
+}
+```
+
+This allows for easy extension of the AirStack CLI without modifying the core script.
+
+## Command Implementation
+
+Commands are implemented as Bash functions with a consistent naming pattern:
+
+```bash
+function cmd_<module>_<command> {
+    # Command implementation
+}
+```
+
+For example:
+
+```bash
+function cmd_wintak_install {
+    log_info "Installing WINTAK..."
+    # Implementation details...
+    return 0
+}
+```
+
+## Help System
+
+The help system provides information about available commands:
+
+```bash
+function print_command_help {
+    local command="$1"
+    
+    if [[ -n "${COMMAND_HELP[$command]}" ]]; then
+        echo -e "airstack $command - ${COMMAND_HELP[$command]}"
+        echo ""
+        
+        # Command-specific usage and options
+        case "$command" in
+            install)
+                echo "Usage: airstack install [options]"
+                echo ""
+                echo "Options:"
+                echo "  --force       Force reinstallation of components"
+                echo "  --no-docker   Skip Docker installation"
+                echo "  --with-wintak Install WinTAK VirtualBox environment"
+                ;;
+            # Other commands...
+        esac
+    else
+        log_error "Unknown command: $command"
+        print_usage
+    fi
+}
+```
+
+## Logging System
+
+The AirStack CLI includes a simple logging system for consistent output formatting:
+
+```bash
+function log_info {
+    echo -e "${GREEN}[INFO]${NC} $1"
+}
+
+function log_warn {
+    echo -e "${YELLOW}[WARN]${NC} $1"
+}
+
+function log_error {
+    echo -e "${RED}[ERROR]${NC} $1"
+}
+
+function log_debug {
+    echo -e "${BLUE}[DEBUG]${NC} $1"
+}
+```
+
+## Command Execution Flow
+
+When a user runs a command, the following sequence occurs:
+
+1. The main script parses the command and arguments
+2. It looks up the command in the `COMMANDS` array
+3. If found, it executes the corresponding function
+4. If not found, it displays an error message and usage information
+
+## Design Principles
+
+The AirStack CLI tool follows several design principles:
+
+1. **Modularity**: Functionality is separated into modules
+2. **Extensibility**: New commands can be added without modifying the core script
+3. **Consistency**: Commands follow a consistent naming and implementation pattern
+4. **Discoverability**: Help text and command listing make it easy to discover functionality
+5. **Robustness**: Error handling and logging provide clear feedback
+
+## Future Enhancements
+
+Potential future enhancements for the AirStack CLI architecture include:
+
+1. **Command Aliases**: Allow commands to have multiple names
+2. **Command Grouping**: Group related commands for better organization
+3. **Tab Completion**: Add support for tab completion in shells
+4. **Plugin System**: Allow for more dynamic loading of plugins
+5. **Configuration System**: Add support for user-specific configuration
\ No newline at end of file
diff --git a/docs/development/airstack-cli/extending.md b/docs/development/airstack-cli/extending.md
new file mode 100644
index 00000000..1afc247b
--- /dev/null
+++ b/docs/development/airstack-cli/extending.md
@@ -0,0 +1,198 @@
+# Extending the AirStack CLI
+
+The AirStack CLI tool is designed to be easily extensible through modules. This guide explains how to create new modules and add commands to the AirStack CLI.
+
+## Module System Overview
+
+The AirStack CLI uses a modular architecture where:
+
+- Each module is a separate Bash script in the `.airstack/modules/` directory
+- Modules can define their own commands and functions
+- Modules are automatically loaded when the AirStack CLI starts
+- Each module registers its commands with the main script
+
+## Creating a New Module
+
+To create a new module:
+
+1. Create a new Bash script in the `.airstack/modules/` directory
+2. Define your command functions
+3. Create a registration function to register your commands
+4. Make the script executable
+
+### Module Template
+
+Here's a template for creating a new module:
+
+```bash
+#!/usr/bin/env bash
+
+# mymodule.sh - Description of your module
+# This module provides commands for...
+
+# Function to implement your command
+function cmd_mymodule_action {
+    log_info "Performing action..."
+    
+    # Your command implementation here
+    
+    log_info "Action completed!"
+    return 0
+}
+
+# Register commands from this module
+function register_mymodule_commands {
+    COMMANDS["mymodule:action"]="cmd_mymodule_action"
+    
+    # Add command help
+    COMMAND_HELP["mymodule:action"]="Description of your command"
+}
+```
+
+Save this file as `.airstack/modules/mymodule.sh` and make it executable:
+
+```bash
+chmod +x .airstack/modules/mymodule.sh
+```
+
+## Module Naming Conventions
+
+To ensure consistency and avoid conflicts:
+
+- Module filenames should use lowercase and end with `.sh`
+- Command functions should be prefixed with `cmd_`
+- Registration functions should be named `register_<modulename>_commands`
+- Command names should use the format `<module>:<command>`
+
+## Available Utilities
+
+When creating modules, you can use several utility functions provided by the main script:
+
+### Logging Functions
+
+```bash
+log_info "Informational message"    # Green text
+log_warn "Warning message"          # Yellow text
+log_error "Error message"           # Red text
+log_debug "Debug message"           # Blue text
+```
+
+### Environment Variables
+
+```bash
+$PROJECT_ROOT    # Root directory of the AirStack project
+$SCRIPT_DIR      # Directory containing the main script
+$MODULES_DIR     # Directory containing the modules
+```
+
+## Example: Creating a Custom Module
+
+Let's create a simple module that provides a command to check system resources:
+
+```bash
+#!/usr/bin/env bash
+
+# sysinfo.sh - System information commands for AirStack
+# This module provides commands for checking system resources
+
+# Function to check system resources
+function cmd_sysinfo_resources {
+    log_info "Checking system resources..."
+    
+    echo "CPU Usage:"
+    top -bn1 | grep "Cpu(s)" | sed "s/.*, *\([0-9.]*\)%* id.*/\1/" | awk '{print 100 - $1"%"}'
+    
+    echo "Memory Usage:"
+    free -m | awk 'NR==2{printf "%.2f%%\n", $3*100/$2}'
+    
+    echo "Disk Usage:"
+    df -h | grep -E '^/dev/' | awk '{print $1 " " $5}'
+    
+    log_info "Resource check completed!"
+    return 0
+}
+
+# Register commands from this module
+function register_sysinfo_commands {
+    COMMANDS["sysinfo:resources"]="cmd_sysinfo_resources"
+    
+    # Add command help
+    COMMAND_HELP["sysinfo:resources"]="Check system resources (CPU, memory, disk)"
+}
+```
+
+Save this file as `.airstack/modules/sysinfo.sh` and make it executable:
+
+```bash
+chmod +x .airstack/modules/sysinfo.sh
+```
+
+Now you can use your new command:
+
+```bash
+./airstack.sh sysinfo:resources
+```
+
+## Best Practices for Module Development
+
+When developing modules for the AirStack CLI, follow these best practices:
+
+1. **Keep modules focused**: Each module should have a specific purpose
+2. **Use descriptive names**: Command names should clearly indicate their function
+3. **Provide helpful error messages**: Use the logging functions to provide clear feedback
+4. **Include help text**: Always register help text for your commands
+5. **Handle errors gracefully**: Check for errors and return appropriate exit codes
+6. **Use consistent formatting**: Follow the style of the existing modules
+7. **Document your module**: Include comments explaining what your module does
+
+## Real-World Example: WinTAK Module
+
+The WinTAK module provides commands for installing and managing the WinTAK virtual machine:
+
+```bash
+#!/usr/bin/env bash
+
+# wintak.sh - WINTAK-related commands for AirStack
+# This module provides commands for installing and managing WINTAK
+
+# Function to install WINTAK
+function cmd_wintak_install {
+    log_info "Installing WINTAK..."
+    
+    # Implementation details...
+    
+    return 0
+}
+
+# Function to start WINTAK
+function cmd_wintak_start {
+    log_info "Starting WinTAK..."
+    
+    # Implementation details...
+    
+    return 0
+}
+
+# Function to stop WINTAK
+function cmd_wintak_stop {
+    log_info "Stopping WinTAK..."
+    
+    # Implementation details...
+    
+    return 0
+}
+
+# Register commands from this module
+function register_wintak_commands {
+    COMMANDS["wintak:install"]="cmd_wintak_install"
+    COMMANDS["wintak:start"]="cmd_wintak_start"
+    COMMANDS["wintak:stop"]="cmd_wintak_stop"
+    
+    # Add command help
+    COMMAND_HELP["wintak:install"]="Install WinTAK VirtualBox environment"
+    COMMAND_HELP["wintak:start"]="Start WinTAK virtual machine"
+    COMMAND_HELP["wintak:stop"]="Stop WinTAK virtual machine"
+}
+```
+
+This module provides three commands for managing WinTAK: `wintak:install`, `wintak:start`, and `wintak:stop`.
\ No newline at end of file
diff --git a/docs/development/airstack-cli/index.md b/docs/development/airstack-cli/index.md
new file mode 100644
index 00000000..78242a4f
--- /dev/null
+++ b/docs/development/airstack-cli/index.md
@@ -0,0 +1,134 @@
+# AirStack CLI Tool
+
+The AirStack CLI tool (`airstack.sh`) provides a unified interface for common development tasks in the AirStack project, including setup, installation, and container management. It simplifies the development workflow and ensures consistency across different environments.
+
+## Overview
+
+The AirStack CLI tool is designed to be:
+
+- **Modular**: Commands are organized into modules that can be easily extended
+- **Consistent**: Provides a unified interface for all AirStack-related tasks
+- **Helpful**: Includes detailed help text and error messages
+- **Extensible**: New commands can be added without modifying the core script
+
+## Basic Usage
+
+```bash
+./airstack.sh <command> [options]
+```
+
+To see all available commands:
+
+```bash
+./airstack.sh commands
+```
+
+To get help for a specific command:
+
+```bash
+./airstack.sh help <command>
+```
+
+## Core Commands
+
+The AirStack CLI includes several built-in commands:
+
+| Command | Description |
+|---------|-------------|
+| `install` | Install dependencies (Docker Engine, Docker Compose, etc.) |
+| `setup` | Configure AirStack settings and add to shell profile |
+| `up` | Start services using Docker Compose |
+| `down` | Stop services |
+| `connect` | Connect to a running container (supports partial name matching) |
+| `status` | Show status of all containers |
+| `logs` | View logs for a container (supports partial name matching) |
+| `help` | Show help information |
+
+### Installation
+
+The `install` command sets up the necessary dependencies for AirStack development:
+
+```bash
+./airstack.sh install [options]
+```
+
+Options:
+- `--force`: Force reinstallation of components
+- `--no-docker`: Skip Docker installation
+- `--with-wintak`: Install WinTAK VirtualBox environment
+
+### Setup
+
+The `setup` command configures your environment for AirStack development:
+
+```bash
+./airstack.sh setup [options]
+```
+
+Options:
+- `--no-shell`: Skip adding to shell profile
+- `--no-config`: Skip configuration tasks
+
+### Container Management
+
+The AirStack CLI provides several commands for managing Docker containers:
+
+```bash
+# Start services
+./airstack.sh up [service...]
+
+# Stop services
+./airstack.sh down [service...]
+
+# Show container status
+./airstack.sh status
+
+# Connect to a container shell
+./airstack.sh connect <container_name>
+
+# View container logs
+./airstack.sh logs <container_name>
+```
+
+## Module-Specific Commands
+
+In addition to the core commands, the AirStack CLI includes several module-specific commands:
+
+### Configuration Commands
+
+```bash
+# Run all configuration tasks
+./airstack.sh config
+
+# Configure Isaac Sim
+./airstack.sh config:isaac-sim
+
+# Configure Nucleus
+./airstack.sh config:nucleus
+
+# Configure Git hooks
+./airstack.sh config:git-hooks
+```
+
+### WinTAK Commands
+
+```bash
+# Install WinTAK
+./airstack.sh wintak:install
+
+# Start WinTAK VM
+./airstack.sh wintak:start
+
+# Stop WinTAK VM
+./airstack.sh wintak:stop
+```
+
+## Adding to Shell Profile
+
+For convenience, you can add the AirStack CLI to your shell profile to make it available from any directory:
+
+```bash
+./airstack.sh setup
+```
+
+This will add the necessary aliases to your shell profile (`~/.bashrc`, `~/.zshrc`, etc.) so you can use the `airstack` command from any directory.
\ No newline at end of file
diff --git a/docs/docker/docker-compose.yaml b/docs/docker/docker-compose.yaml
index c8c8ae3a..764e0744 100644
--- a/docs/docker/docker-compose.yaml
+++ b/docs/docker/docker-compose.yaml
@@ -5,7 +5,8 @@ services:
       - ""
       - sitl
       - hitl
-    image: &image_tag ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_mkdocs
+      - dev
+    image: &image_tag ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_mkdocs
     build:
       dockerfile: ./Dockerfile
       tags:
diff --git a/docs/getting_started.md b/docs/getting_started.md
index 251f457e..1fcb6dc0 100644
--- a/docs/getting_started.md
+++ b/docs/getting_started.md
@@ -6,20 +6,21 @@ By the end of this tutorial, you will have the autonomy stack running on your ma
 
 You need at least 25GB free to install the Docker image.
 
-Have an NVIDIA GPU >= RTX 3070 to run Isaac Sim locally.
+Check the hardware requirements for the NVIDIA Isaac Sim [here](https://docs.isaacsim.omniverse.nvidia.com/latest/installation/requirements.html).
+A GPU of GeForce RTX 4080 or higher is recommended for the best performance.
 
-## Setup
-
-### Clone
-
-```
+## Clone
+```bash
 git clone --recursive -j8 git@github.com:castacks/AirStack.git
+cd AirStack
 ```
 
-### Docker
+## Install and Setup
 
-Follow [NVIDIA's instructions](https://docs.nvidia.com/ai-enterprise/deployment/vmware/latest/docker.html) for installing Docker to be compatible with NVIDIA GPUs, including adding the NVIDIA Container Toolkit.
-Make sure `docker-compose-plugin` is also installed with Docker.
+```bash
+./airstack.sh install  # installs docker and docker-compose
+./airstack.sh setup  # this lets you use the `airstack` command
+```
 
 ## Configure
 
@@ -75,13 +76,7 @@ docker compose push
 ## Launch
 
 ```bash
-xhost +  # allow docker access to X-Server
-
-# Make sure you are in the AirStack directory.
-
-# Start docker compose services. This launches Isaac Sim and the robots.
-#  You can append `--scale robot=[NUM_ROBOTS]` for more robots, default is 1
-docker compose up -d
+airstack up # This will launch the robot, ground control station, and isaac sim
 ```
 
 This will automatically launch and play the Isaac scene specified under `AirStack/.env` (default is the Fire Academy).
@@ -99,5 +94,5 @@ You can also switch to `Fixed Trajectory` mode and hit `Publish` on the bottom r
 To shutdown and remove docker containers:
 
 ```bash
-docker compose down
+airstack down # This will stop and remove the docker containers
 ```
diff --git a/docs/ground_control_station/asset/TAK_ROS_Arch.png b/docs/ground_control_station/asset/TAK_ROS_Arch.png
new file mode 100644
index 00000000..b4e7b050
Binary files /dev/null and b/docs/ground_control_station/asset/TAK_ROS_Arch.png differ
diff --git a/docs/ground_control_station/asset/aerolens.ai/find.png b/docs/ground_control_station/asset/aerolens.ai/find.png
new file mode 100644
index 00000000..ed19d334
Binary files /dev/null and b/docs/ground_control_station/asset/aerolens.ai/find.png differ
diff --git a/docs/ground_control_station/asset/aerolens.ai/help.png b/docs/ground_control_station/asset/aerolens.ai/help.png
new file mode 100644
index 00000000..73d21516
Binary files /dev/null and b/docs/ground_control_station/asset/aerolens.ai/help.png differ
diff --git a/docs/ground_control_station/asset/drone_australia_zoomed_in.png b/docs/ground_control_station/asset/drone_australia_zoomed_in.png
new file mode 100644
index 00000000..a7f058e6
Binary files /dev/null and b/docs/ground_control_station/asset/drone_australia_zoomed_in.png differ
diff --git a/docs/ground_control_station/asset/drone_australia_zoomed_out.png b/docs/ground_control_station/asset/drone_australia_zoomed_out.png
new file mode 100644
index 00000000..e0835d9b
Binary files /dev/null and b/docs/ground_control_station/asset/drone_australia_zoomed_out.png differ
diff --git a/docs/ground_control_station/casualty_assessment/casualty_assessment.md b/docs/ground_control_station/casualty_assessment/casualty_assessment.md
new file mode 100644
index 00000000..6affaeca
--- /dev/null
+++ b/docs/ground_control_station/casualty_assessment/casualty_assessment.md
@@ -0,0 +1,87 @@
+# ROS2 CASEVAC Agent
+
+## Overview
+ROS2 CASEVAC Agent is a service that bridges ROS2 casualty information to TAK (Tactical Assault Kit) systems using MQTT as the transport layer. The agent subscribes to ROS topics containing casualty metadata and images, converts this information into CoT (Cursor on Target) format, and publishes it to an MQTT broker for TAK systems to consume.
+
+## Features
+- Subscribes to ROS casualty metadata and image topics
+- Converts casualty information into standard CoT format with ZMIST fields
+  - Z: Zap Number - Unique casualty identifier
+  - M: Mechanism of Injury
+  - I: Injuries Sustained
+  - S: Signs and Symptoms
+  - T: Treatments Rendered
+- Tracks multiple casualties simultaneously
+- Handles various injury types and severity levels
+- Transmits data over MQTT to TAK systems
+
+## Installation
+
+### Prerequisites
+- ROS2 (tested with Foxy/Humble)
+- Python 3.8+
+- paho-mqtt
+- PyTAK
+
+### Dependencies
+```bash
+pip install paho-mqtt pytak pyyaml
+```
+
+## Configuration
+Create a YAML configuration file with the following structure:
+
+```yaml
+project:
+  name: your_project_name
+
+services:
+  host: your_host_ip
+  mediator:
+    ros2casevac_agent:
+      topic_name: to_tak  # MQTT topic name for CoT messages
+      ros_casualty_meta_topic_name: '/casualty/meta'  # ROS topic for casualty metadata
+      ros_casualty_image_topic_name: '/casualty/image'  # ROS topic for casualty images
+
+mqtt:
+  host: mqtt_broker_ip
+  port: mqtt_broker_port
+  username: mqtt_username
+  password: mqtt_password
+```
+
+## Usage
+Run the agent with a configuration file:
+
+```bash
+ros2 run ros2tak_tools ros2casevac_agent --config path/to/your/config.yaml
+```
+
+## Message Types
+The agent expects the following ROS message types:
+- `airstack_msgs/CasualtyMeta`: Contains casualty metadata including:
+  - GPS coordinates
+  - Trauma assessments (head, torso, extremities)
+  - Vital signs (heart rate, respiratory rate)
+  - Critical conditions (hemorrhage, respiratory distress)
+  - Alertness indicators (ocular, verbal, motor)
+
+## How It Works
+1. The agent subscribes to the ROS topic for casualty metadata
+2. When new data is received, it updates an internal casualty tracking object
+3. If GPS data is available, it generates a CoT event in XML format
+4. The CoT event is published to the configured MQTT topic
+5. TAK systems subscribed to the MQTT topic receive and display the casualty information
+
+## Customization
+The code supports several enum types for different injury categories:
+- `TraumaType`: Different body regions (head, torso, extremities)
+- `TraumaSeverity`: Levels of trauma (normal, wound, amputation)
+- `OcularAlertness`: Eye response states
+- `AlertnessLevel`: Verbal and motor response states
+- `VitalType`: Types of vital signs
+- `ConditionType`: Critical conditions
+- `ConditionStatus`: Presence/absence of conditions
+
+## Author
+Aditya Rauniyar (rauniyar@cmu.edu)
\ No newline at end of file
diff --git a/docs/ground_control_station/command_center/command_center.md b/docs/ground_control_station/command_center/command_center.md
new file mode 100644
index 00000000..cf118439
--- /dev/null
+++ b/docs/ground_control_station/command_center/command_center.md
@@ -0,0 +1,44 @@
+# Aerolens.ai - Talk to your robots
+
+This configuration file defines the parameters for the `chat2ros_agent`, which connects to an MQTT topic, filters messages, and routes queries through ROS topics.
+```yaml
+chat2ros_agent:
+  mqtt_subcribe_topic: aerolens-ai  # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server.
+  ros_query_text_topic: '/query/text'  # ROS Topic name to publish the chat queries.
+  ros_query_response_topic: '/query/response'  # ROS Topic name to publish the chat responses.
+  filter_name: <project>-operator
+```
+
+
+## Parameters
+
+- **`mqtt_subscribe_topic`** (`aerolens-ai`):  
+  The MQTT topic where messages containing COT data are received.
+
+- **`ros_query_text_topic`** (`/query/text`):  
+  The ROS topic where filtered queries are published.
+
+- **`ros_query_response_topic`** (`/query/response`):  
+  The ROS topic where responses to the queries are published.
+
+- **`filter_name`** (`dsta-operator`):  
+  A filter applied to incoming messages, selecting only those where `filter_messages.name` matches this value.
+
+## Workflow
+
+1. The service subscribes to the MQTT topic (`aerolens-ai`).
+2. It filters messages based on `filter_name` (`<project>-operator`).
+3. The extracted query is published to `<robot_name>/query/text`.
+4. The response to the query is expected on `<robot_name>/query/response`.
+
+This enables seamless integration between an MQTT-based message broker and ROS for structured communication.
+
+## Commands:
+
+![Command Center](../asset/aerolens.ai/help.png)
+
+`help` - Display the help message.
+
+![Command Center](../asset/aerolens.ai/find.png)
+
+`robot <robot_name> find <object>` - Find an object using the robot.
\ No newline at end of file
diff --git a/docs/ground_control_station/index.md b/docs/ground_control_station/index.md
index eb984863..bfa8f71e 100644
--- a/docs/ground_control_station/index.md
+++ b/docs/ground_control_station/index.md
@@ -9,31 +9,52 @@ Requirements:
 - Ubuntu 22.04
 
 ## Setup 
-WinTAK is setup as auto start on boot and connects to Airlabs TAK-Server. Its run on Windows 11 VirtualBox Virtual Machine.
+WinTAK is setup as auto start on boot and connects to Airlabs TAK-Server. It runs on a Windows 11 VirtualBox Virtual Machine.
 
 ![Setup](asset/setup.png)
 
-1. Set your `ANDREWID` as an environment variable.
+### Installation using AirStack CLI
+
+The recommended way to install WinTAK is using the AirStack CLI tool:
+
 ```bash
-export ANDREWID=<your_andrew_id>
+# From the AirStack root directory
+./airstack.sh install --with-wintak
 ```
-2. Run the following commands to setup WinTAK running over Windows VirtualBox.
-```bash
-sudo mkdir -p "$HOME/vmware"
-sudo chown -R $USER:$USER "$HOME/vmware"
-sudo chmod -R 755 "$HOME/vmware"
-sudo rsync --progress -avz ${ANDREWID}@airlab-storage.andrew.cmu.edu:/volume4/dsta/atak/setup/ "$HOME/vmware"
-sudo dpkg -i "$HOME/vmware/virtualbox-7.1_7.1.6-167084~Ubuntu~jammy_amd64.deb"
-sudo apt-get install -f
-sudo /sbin/vboxconfig
-VBoxManage import "$HOME/vmware/Windows11.ova" --vsys 0 --vmname "WinTAK"
 
+This will:
+
+1. Download the necessary files from airlab-storage
+2. Install VirtualBox
+3. Import the WinTAK virtual machine
+4. Configure the necessary credentials and settings
+
+### Alternative Manual Installation
+
+Alternatively, you can run the setup script directly:
+
+```bash
+# Move to the directory:
+cd ground_control_station/installation
+# Execute the script
+./setup_ground_control_station.sh
 ```
-3. Run WinTAK using the following commands.
+
+### Starting and Stopping WinTAK
+
+Once installed, you can start and stop WinTAK using the AirStack CLI:
+
 ```bash
-VBoxManage startvm "WinTAK"
+# Start WinTAK
+./airstack.sh wintak:start
+
+# Stop WinTAK
+./airstack.sh wintak:stop
 ```
 
-NOTE: If it asks to reset the password, please reset to current password.
+**NOTE:** If it asks to reset the password on first boot, please choose your own memorable password.
+
+![WinTAK](asset/WinTAK_on_windows_virtualbox_vm.png)
 
-![WinTAK](asset/WinTAK_on_windows_virtualbox_vm.png)
\ No newline at end of file
+## Know more about TAK using the youtube link below:
+[![Video Title](https://img.youtube.com/vi/fiBt0wEiKh8/0.jpg)](https://www.youtube.com/watch?v=fiBt0wEiKh8&t=1s)
\ No newline at end of file
diff --git a/docs/ground_control_station/usage/user_interface.md b/docs/ground_control_station/usage/user_interface.md
index e69de29b..ad99141a 100644
--- a/docs/ground_control_station/usage/user_interface.md
+++ b/docs/ground_control_station/usage/user_interface.md
@@ -0,0 +1,34 @@
+## The TAK Architecture looks like follows:
+![TAK Architecture](../asset/TAK_ROS_Arch.png)
+
+## Note: Please check out the config file at [config.yaml](../../../ground_control_station/ros_ws/src/ros2tak_tools/config/demo_config.yaml) to understand further on how things are setup.
+
+## Learn how to use the TAK features:
+### 1. Sending Robot Query from the TAK Chat.
+[![Watch the video](https://img.youtube.com/vi/6YCHd70mCUY/0.jpg)](https://www.youtube.com/watch?v=6YCHd70mCUY&list=PLpJxwrRy4QbtVD3XxVzg3CAsm-A279d28&index=2)
+
+### 2. Displaying automated Casevac icons from the casualty inspections:
+[![Watch the video](https://img.youtube.com/vi/J577FWRaipg/0.jpg)](https://www.youtube.com/watch?v=J577FWRaipg&list=PLpJxwrRy4QbtVD3XxVzg3CAsm-A279d28&index=1)
+
+# Debugging tips: 
+
+launch just the ground-control-station container:
+```bash
+docker compose --profile deploy up ground-control-station
+````
+
+## 1. Running docker in interactive mode:
+```bash
+docker exec -it ground-control-station /bin/bash
+```
+
+## 2. Checking if the messages are being received by the MQTT broker:
+
+```bash
+mosquitto_sub -h localhost -t to_tak -u airlab # Default topic for sending messages to TAK
+mosquitto_sub -h localhost -t healthcheck -u airlab # Default topic for sending healthcheck messages
+```
+
+
+
+
diff --git a/docs/README.md b/docs/index.md
similarity index 100%
rename from docs/README.md
rename to docs/index.md
diff --git a/docs/overrides/home.html b/docs/overrides/home.html
index 63af074c..694a193c 100644
--- a/docs/overrides/home.html
+++ b/docs/overrides/home.html
@@ -18,7 +18,7 @@
       <div class="mdx-hero">
         <div class="mdx-hero__content">
           <h1>Welcome to AirStack</h1>
-          <p>A comprehensive autonomy stack boilerplate to accelerate your robotics development</p>
+          <p>A Boilerplate to Democratize Autonomous Intelligent Robotics</p>
           <a href="{{ 'docs/getting_started/' | url }}" class="md-button md-button--primary">
             Get Started
           </a>
diff --git a/docs/real_world/HITL/index.md b/docs/real_world/HITL/index.md
new file mode 100644
index 00000000..bf6057c7
--- /dev/null
+++ b/docs/real_world/HITL/index.md
@@ -0,0 +1,25 @@
+# Hardware-In-The-Loop Simulation
+We configure a multi-machine HITL simulation, where a powerful desktop computer runs Isaac Simulator and rendering, and one/multiple jetson compute boards run robot-specific programs (planning, mapping, etc.).
+## Requirement
+A desktop computer configured according to [here](/docs/getting_started). One/multiple ORIN AGX/NX configured according to [here](/docs/real_world/installation/).
+
+## Communication
+All machines should connect to the same network. In our test, all machines are connected to the same router with ethernet cables. Ensure that all machines are able to `ping` others' IP addresses.
+
+### Run
+On the desktop computer, under your Airstack folder, run
+```
+docker compose up isaac-sim-hitl
+```
+You should see the isaac simulator being launched.
+On the Jetson computer, run
+```
+docker compose up robot_l4t
+```
+Once the scene is played in the Isaac simulator, the rviz GUI on the Jetson should start displaying sensor data, which means the connection is successful. 
+
+Screen record of desktop computer:
+<iframe src="https://drive.google.com/file/d/1sNkEattgDyBAI9xFPVQsXn8sRi6gYQSG/view?usp=sharing" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
+
+Screen record of Jetson computer:
+<iframe src="https://drive.google.com/file/d/19S8Yceq8t2FPubN8Mly003Up1AbLzCA8/view?usp=sharing" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
\ No newline at end of file
diff --git a/docs/real_world/data_offloading/index.md b/docs/real_world/data_offloading/index.md
new file mode 100644
index 00000000..dfc63f64
--- /dev/null
+++ b/docs/real_world/data_offloading/index.md
@@ -0,0 +1,80 @@
+# Data Offloading
+
+We have a tool to automatically offload and sync your robot data to the AirLab internal storage server.
+
+## Setup Storage Tools Server Locally
+
+### Clone and install
+
+``` bash
+git clone https://github.com/castacks/storage_tools_server
+cd storage_tools_server
+python -m venv venv
+. venv/bin/activate
+pip install -r requirements.txt
+```
+
+### Configure
+
+Edit the `config/config.yaml` file to match your configuration.
+
+### REQUIRED UPDATES
+
+- `upload_dir` is the location for uploads.  This must be readable and writeable by the user running the Server.
+- `volume_root` sets the prefix for all entries in the `volume_map`.  This must be readable and writeable by the user running the Server.
+- `volume_map` is a mapping from project name to `volume_root/{path}`.  All projects must have a mapping.
+
+### Set Environment and Run
+
+- `CONFIG` is the full path to the `config.yaml` in use.  By default, the app will use `$PWD/config/config.yaml`
+- `PORT` is the same port as define in the optional setup. The default is 8091.
+
+``` bash
+export CONFIG=$PWD/config/config.yaml
+export PORT=8091
+
+gunicorn -k gevent -w 1 -b "0.0.0.0:${PORT}" --timeout 120 "server.app:app"
+```
+
+Open a web browser to http://localhost:8091 (or the PORT you set). The default user is `admin` and the default password is `NodeNodeDevices`.
+
+### Create an API Key for your robot
+
+- Log into the Server
+- Go to Configure -> Keys
+- Enter a name for the device key in the "Add a new key name" field.
+- Click "Generate Key"
+
+## Set up Storage Tools Device on your Robot
+
+### Install Requirements
+
+- [Docker Compose](https://docs.docker.com/compose/install/standalone/)
+
+### Clone Device Repo
+
+```bash
+cd /opt 
+git clone https://github.com/castacks/storage_tools_device
+cd stroage_tools_device
+```
+
+### Update the config.yaml
+
+Update `config/config.yaml` to match your environment.  Things you should update:
+
+- `API_KEY_TOKEN`.  The api key that your admin gave you, or the key that you set up in the [Server Setup](#create-an-api-key-for-your-robot)
+- `watch`.  The list of directories that have your robot's files.
+
+Update the `env.sh` to match your system.
+
+- `CONFIG_FILE`.  If you have multiple config files, make sure `CONFIG_FILE` points to the one you want to use.
+- `DATA_DIR`. This is the top level data directory that all of the `watch` dirs share.  For example, if you `watch` directories are `/mnt/data/processor_1` and `/mnt/data/processor_2`, set the `DATA_DIR` to `/mnt/data`.  
+
+### Build and Run
+
+``` bash
+cd /opt/storage_tools_device
+. env.sh
+docker compose up --build
+```
diff --git a/docs/real_world/installation/index.md b/docs/real_world/installation/index.md
index f6491851..aa2a01c7 100644
--- a/docs/real_world/installation/index.md
+++ b/docs/real_world/installation/index.md
@@ -1 +1,29 @@
-# Installation on Hardware
\ No newline at end of file
+# Installation on ORIN AGX/NX
+
+We have tested installation and running robot container on Jetson ORIN AGX/NX and Ubuntu 22.04.
+
+## Setup
+Ensure you have docker installed.
+### Clone
+
+```
+git clone --recursive -j8 git@github.com:castacks/AirStack.git
+```
+Checkout to the correct branch:
+```
+git checkout jkeller/jetson_36.4
+```
+## Configure
+
+Run `./configure.sh` and follow the instructions in the prompts to do an initial configuration of the repo.
+
+Pull the correct image:
+```
+docker compose pull robot_l4t
+```
+
+## Run
+```
+docker compose up robot_l4t
+```
+You should be able to see the rviz GUI being launched.
\ No newline at end of file
diff --git a/docs/robot/autonomy/1_sensors/gimbal.md b/docs/robot/autonomy/1_sensors/gimbal.md
new file mode 100644
index 00000000..e009674b
--- /dev/null
+++ b/docs/robot/autonomy/1_sensors/gimbal.md
@@ -0,0 +1,33 @@
+
+# **Gimbal Extension**
+
+## **Overview**  
+The **Gimbal Extension** provides an easy way to integrate a controllable gimbal into an existing drone model within the scene. This extension is designed to facilitate the attachment and operation of a camera-equipped gimbal, allowing for real-time adjustments to pitch and yaw angles via ROS 2 messages.
+
+
+## **Installation and Activation**  
+To enable the **Gimbal Extension**, follow these steps:
+
+1. Open the **Extensions** window by navigating to:  
+   **Window** → **Extensions**
+2. Under the **THIRD PARTIES** section, go to the **User** tab.
+3. Locate the **Gimbal Extension** and turn it on.
+4. Once enabled, a new **Gimbal Extension** window should appear.
+
+## **Adding a Gimbal to a Drone**  
+To attach a gimbal to an existing UAV model:
+
+1. Copy the **prim path** of the UAV to which you want to add the gimbal.
+2. In the **Gimbal Extension** window, paste the copied path into the **Robot Prim Path** text box.
+3. Set the **Robot Index** based on the `DOMAIN_ID` of the drone.  
+   - The `DOMAIN_ID` should match the identifier used for the robot to ensure proper communication.
+
+For a step-by-step demonstration, refer to the video tutorial below:
+
+<iframe src="https://drive.google.com/file/d/1pN0Pxe4nYQL1qs40oZTDqsOXsMrApLPM/preview" width="840" height="480" allow="autoplay" allowfullscreen="allowfullscreen"></iframe>
+
+## **Gimbal Camera Image Topic**
+Once the gimbal is successfully added, the camera image feed from the gimbal will be published on the following ROS 2 topic: `/robot_<ID>/gimbal/rgb`.
+
+## **Controlling the Gimbal**
+The gimbal pitch and yaw angles can be controled by the ros2 messages `/robot_<ID>/gimbal/desired_gimbal_pitch` and `/robot_<ID>/gimbal/desired_gimbal_yaw` of type `std_msgs/msg/Float64`, respectively.
\ No newline at end of file
diff --git a/git-hooks/README.md b/git-hooks/README.md
new file mode 100644
index 00000000..b046c0f1
--- /dev/null
+++ b/git-hooks/README.md
@@ -0,0 +1,33 @@
+# Git Hooks
+
+This directory contains git hooks used in the AirStack repository.
+
+## Available Hooks
+
+### Docker Versioning Hook
+
+The `update-docker-image-tag.pre-commit` hook automatically updates the `DOCKER_IMAGE_TAG` in the `.env` file with the current git commit hash whenever Docker-related files (Dockerfile or docker-compose.yaml) are modified. It also adds a comment above the variable indicating that the value is auto-generated from the git commit hash.
+
+This ensures that Docker images are always tagged with the exact commit they were built from, eliminating version conflicts between parallel branches.
+
+### Installation
+
+To install the hooks:
+
+1. Copy the hook to your local .git/hooks directory:
+   ```bash
+   cp git-hooks/docker-versioning/update-docker-image-tag.pre-commit .git/hooks/pre-commit
+   ```
+
+2. Make sure the hook file is executable:
+   ```bash
+   chmod +x .git/hooks/pre-commit
+   ```
+
+## How the Docker Versioning Hook Works
+
+1. When you commit changes, the hook checks if any Dockerfile or docker-compose.yaml files are being committed
+2. If Docker-related files are detected, it updates the DOCKER_IMAGE_TAG in the .env file with the current git commit hash and adds a comment above the variable
+3. The modified .env file is automatically added to the commit
+
+This approach eliminates version conflicts between parallel branches by ensuring Docker images are tagged with the exact commit they were built from.
\ No newline at end of file
diff --git a/git-hooks/docker-versioning/README.md b/git-hooks/docker-versioning/README.md
new file mode 100644
index 00000000..bbfe7521
--- /dev/null
+++ b/git-hooks/docker-versioning/README.md
@@ -0,0 +1,35 @@
+# Docker Versioning Git Hook
+
+This directory contains a git hook that automatically updates the Docker image tag with the current git commit hash.
+
+## Hook: update-docker-image-tag.pre-commit
+
+This pre-commit hook automatically updates the `DOCKER_IMAGE_TAG` in the `.env` file with the current git commit hash whenever Docker-related files (Dockerfile or docker-compose.yaml) are modified.
+
+### Features
+
+- Automatically updates `DOCKER_IMAGE_TAG` with the git commit hash
+- Adds a comment above the variable indicating it's auto-generated
+- Only triggers when Docker-related files are modified
+- Automatically stages the modified .env file for commit
+
+### Installation
+
+To install the hook:
+
+1. Copy the hook to your local .git/hooks directory:
+   ```bash
+   cp update-docker-image-tag.pre-commit ../../.git/hooks/pre-commit
+   ```
+
+2. Make sure the hook file is executable:
+   ```bash
+   chmod +x ../../.git/hooks/pre-commit
+   ```
+
+### Benefits
+
+- Eliminates version conflicts between parallel branches
+- Ensures Docker images are tagged with the exact commit they were built from
+- Simplifies tracking which version of the code is running in Docker containers
+- Provides a consistent and automated versioning system for Docker images
\ No newline at end of file
diff --git a/git-hooks/docker-versioning/update-docker-image-tag.pre-commit b/git-hooks/docker-versioning/update-docker-image-tag.pre-commit
new file mode 100755
index 00000000..3165c91f
--- /dev/null
+++ b/git-hooks/docker-versioning/update-docker-image-tag.pre-commit
@@ -0,0 +1,40 @@
+#!/bin/bash
+
+# Pre-commit hook to update DOCKER_IMAGE_TAG in .env file with git commit hash
+# when Dockerfile or docker-compose.yaml files are modified
+
+# Check if any Dockerfile or docker-compose.yaml files are staged for commit
+DOCKER_FILES_CHANGED=$(git diff --cached --name-only | grep -E 'Dockerfile|docker-compose\.yaml$')
+
+if [ -n "$DOCKER_FILES_CHANGED" ]; then
+    echo "Docker-related files changed. Updating DOCKER_IMAGE_TAG in .env file..."
+    
+    # Get the current commit hash (short version)
+    COMMIT_HASH=$(git rev-parse --short HEAD)
+    
+    # Update the DOCKER_IMAGE_TAG in .env file
+    if [ -f ".env" ]; then
+        # Check if DOCKER_IMAGE_TAG line exists
+        if grep -q "^DOCKER_IMAGE_TAG=" .env; then
+            # Replace the existing DOCKER_IMAGE_TAG line and ensure comment is above it
+            # First, remove any existing auto-generated comment
+            sed -i '/^# auto-generated from git commit hash$/d' .env
+            # Add the comment above the DOCKER_IMAGE_TAG line
+            sed -i '/^DOCKER_IMAGE_TAG=/i\# auto-generated from git commit hash' .env
+            # Update the DOCKER_IMAGE_TAG value
+            sed -i "s/^DOCKER_IMAGE_TAG=.*$/DOCKER_IMAGE_TAG=\"$COMMIT_HASH\"/" .env
+            echo "Updated DOCKER_IMAGE_TAG to $COMMIT_HASH in .env file"
+            
+            # Stage the modified .env file for commit
+            git add .env
+        else
+            echo "Error: DOCKER_IMAGE_TAG line not found in .env file"
+            exit 1
+        fi
+    else
+        echo "Error: .env file not found"
+        exit 1
+    fi
+fi
+
+exit 0
\ No newline at end of file
diff --git a/ground_control_station/docker/.bash_history b/ground_control_station/docker/.bash_history
index 929c6870..c656cdd8 100644
--- a/ground_control_station/docker/.bash_history
+++ b/ground_control_station/docker/.bash_history
@@ -5,4 +5,6 @@ ros2 launch gcs_bringup gcs.launch.xml
 cws
 bws
 sws
-bws --packages-select gcs_bringup
\ No newline at end of file
+bws --packages-select gcs_bringup
+mosquitto_sub -h localhost -t healthcheck -u airlab
+mosquitto_sub -h localhost -t to_tak -u airlab
diff --git a/ground_control_station/docker/.env b/ground_control_station/docker/.env
index e15dfc4c..a12c0fc4 100644
--- a/ground_control_station/docker/.env
+++ b/ground_control_station/docker/.env
@@ -1,5 +1,6 @@
-PROJECT_NAME= GCS # Enter the project name
-
+PROJECT_NAME="airstack"
+PROJECT_VERSION="0.12.0"
+PROJECT_DOCKER_REGISTRY="airlab-storage.andrew.cmu.edu:5001/shared"
 
 # ROS --------------------------------------------------------
 ROS_WS_DIR=/root/ros_ws
diff --git a/ground_control_station/docker/Dockerfile.gcs b/ground_control_station/docker/Dockerfile.gcs
index 8b57b92e..12090ca4 100644
--- a/ground_control_station/docker/Dockerfile.gcs
+++ b/ground_control_station/docker/Dockerfile.gcs
@@ -5,7 +5,7 @@ WORKDIR /root/ros_ws
 
 # Install system dependencies
 RUN apt-get update && apt-get install -y \
-    vim nano emacs wget curl tree \
+    vim nano emacs wget curl tree sshpass \
     iperf3 iftop iputils-ping net-tools htop \
     cmake build-essential less htop jq python3-pip tmux gdb \
     ros-dev-tools ros-humble-mavros ros-humble-tf2* ros-humble-stereo-image-proc \
@@ -16,26 +16,21 @@ RUN apt-get update && apt-get install -y \
     libgstreamer-plugins-bad1.0-dev gstreamer1.0-plugins-base \
     gstreamer1.0-plugins-good gstreamer1.0-plugins-bad \
     gstreamer1.0-plugins-ugly gstreamer1.0-libav gstreamer1.0-tools \
-    gstreamer1.0-x gstreamer1.0-alsa openssh-server
+    gstreamer1.0-x gstreamer1.0-alsa openssh-server \
+    xterm gnome-terminal libcanberra-gtk-module libcanberra-gtk3-module dbus-x11 \
+    && rm -rf /var/lib/apt/lists/*
 
 # Install Python dependencies
 RUN pip3 install empy future lxml matplotlib numpy pkgconfig psutil pygments \
-    wheel pymavlink pyyaml requests setuptools six toml scipy pytak paho-mqtt
+    wheel pymavlink pyyaml requests setuptools six toml scipy pytak paho-mqtt sphinx
 
 # Configure SSH
 RUN mkdir /var/run/sshd && echo 'root:airstack' | chpasswd && \
     sed -i 's/#PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config && \
     sed -i 's/#PasswordAuthentication yes/PasswordAuthentication yes/' /etc/ssh/sshd_config && \
     sed 's@session\s*required\s*pam_loginuid.so@session optional pam_loginuid.so@g' -i /etc/pam.d/sshd
-
+# open port 22 for ssh
 EXPOSE 22
 
-# Build ROS2 workspace
-# COPY ground_control_station/ros_ws/src/ros2tak_tools /root/ros_ws/src/ros2tak_tools
-# COPY common/ros_packages/airstack_msgs /root/ros_ws/src/airstack_msgs
-# RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \
-#                   colcon build --symlink-install --packages-select airstack_msgs && \
-#                   colcon build --symlink-install"
-
 # Cleanup
 RUN apt purge git -y && apt autoremove -y && apt clean -y && rm -rf /var/lib/apt/lists/*
diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml
index 88b3f5ee..4181aeb4 100644
--- a/ground_control_station/docker/docker-compose.yaml
+++ b/ground_control_station/docker/docker-compose.yaml
@@ -1,30 +1,35 @@
 services:
-  ground-control-station:
+  gcs:
     profiles:
       - ""
       - sitl
     extends:
       file: ./ground-control-station-base-docker-compose.yaml
-      service: ground-control-station-base
-    container_name: ground-control-station
+      service: gcs-base
     networks:
       - airstack_network
     ports:
       - 2222:22 # for ssh
-  
-  ground-control-station-real:
+
+  # -------------------------------------------------------------------------------------------------------------------
+  # dev container for developer debugging
+  gcs-dev:
+    profiles: !override
+      - dev
+    extends: gcs
+    command: "sleep infinity"
+
+  # ===================================================================================================================
+  # for running on a real world laptop on the local network, set network mode to host
+  gcs-real:
     profiles:
       - hitl
       - deploy
     extends:
       file: ./ground-control-station-base-docker-compose.yaml
-      service: ground-control-station-base
-    container_name: ground-control-station-real
+      service: gcs-base
     network_mode: host
     volumes:
-     - $HOME/bags/:/bags
-     - ../../robot/ros_ws/src/robot_bringup/rviz/:/bags/rviz
-     - ../../plot:/plot
-
-# include: 
-  # - ./tak-docker-compose.yaml
\ No newline at end of file
+      - $HOME/bags/:/bags
+      - ../../robot/ros_ws/src/robot_bringup/rviz/:/bags/rviz
+      - ../../plot:/plot
diff --git a/ground_control_station/docker/ground-control-station-base-docker-compose.yaml b/ground_control_station/docker/ground-control-station-base-docker-compose.yaml
index c6783cf4..92c4beeb 100644
--- a/ground_control_station/docker/ground-control-station-base-docker-compose.yaml
+++ b/ground_control_station/docker/ground-control-station-base-docker-compose.yaml
@@ -1,7 +1,7 @@
 # docker compose file
 services:
-  ground-control-station-base:
-    image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_gcs
+  gcs-base:
+    image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_gcs
     build:
       context: ../
       dockerfile: docker/Dockerfile.gcs
@@ -12,8 +12,7 @@ services:
       bash -c "ssh service restart;
       tmux new -d -s gcs_bringup
       && tmux send-keys -t gcs_bringup 
-      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
-      ros2 launch gcs_bringup gcs.launch.xml' ENTER
+      'bws && sws; ros2 launch gcs_bringup gcs.launch.xml' ENTER
       && sleep infinity"
     # Interactive shell
     stdin_open: true # docker run -i
diff --git a/ground_control_station/docker/tak-docker-compose.yaml b/ground_control_station/docker/tak-docker-compose.yaml
deleted file mode 100644
index af108a91..00000000
--- a/ground_control_station/docker/tak-docker-compose.yaml
+++ /dev/null
@@ -1,181 +0,0 @@
-services:
-  ####################### ROS2TAK TOOLS ######################
-  ############### MQTT for the GCS
-  mqtt:
-    container_name: "mqtt"
-    image: eclipse-mosquitto:2.0.20
-    restart: always
-    volumes:
-      - ../ros_ws/src/ros2tak_tools/mosquitto/config:/mosquitto/config
-      - ../ros_ws/src/ros2tak_tools/mosquitto/data:/mosquitto/data
-      - ../ros_ws/src/ros2tak_tools/mosquitto/log:/mosquitto/log
-    env_file:
-      - .env
-    ports:
-      - "1883:1883"
-    healthcheck:
-      test:
-        [
-          "CMD",
-          "mosquitto_pub",
-          "-h",
-          "localhost",
-          "-t",
-          "healthcheck",
-          "-m",
-          "ping",
-          "-u",
-          "${MQTT_USERNAME}",
-          "-P",
-          "${MQTT_PASSWORD}",
-        ]
-      interval: 5s
-      timeout: 3s
-      retries: 2
-      start_period: 5s
-    #    network_mode: host
-    networks:
-      - airstack_network
-  ################## ROS2COT_AGENT
-  ros2cot_agent:
-    image: &gcs_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_gcs
-    build:
-      context: ../
-      dockerfile: docker/Dockerfile.ros2tak_tools
-      tags:
-        - *ros2cot_agent_image
-      args:
-        - ROS_WS_DIR=${ROS_WS_DIR}
-    container_name: "${PROJECT_NAME}_ros2cot_agent"
-    stdin_open: true
-    tty: true
-    restart: unless-stopped
-    environment:
-      - ROS_DOMAIN_ID=${ROS_DOMAIN_ID}
-    depends_on:
-      mqtt:
-        condition: service_healthy
-    #    network_mode: host
-    networks:
-      - airstack_network
-    command:
-      [
-        "/bin/bash",
-        "-c",
-        "source /opt/ros/humble/setup.bash && source $ROS_WS_DIR/install/setup.bash && ./install/ros2tak_tools/bin/ros2cot_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME",
-      ]
-
-  # ################### TAK_PUBLISHER
-  tak_publisher:
-    image: *gcs_image
-    build:
-      context: ../
-      dockerfile: docker/Dockerfile.tak_publisher
-      args:
-        - ROS_WS_DIR=${ROS_WS_DIR}
-      tags:
-        - *gcs_image
-    container_name: "${PROJECT_NAME}-tak_publisher"
-    stdin_open: true
-    tty: true
-    restart: unless-stopped
-    depends_on:
-      mqtt:
-        condition: service_healthy
-    #    network_mode: host
-    networks:
-      - airstack_network
-    volumes:
-      - ../ros_ws/src/ros2tak_tools/:${ROS_WS_DIR}/src/ros2tak_tools/
-    command:
-      [
-        "python3",
-        "$TAK_PUBLISHER_FILEPATH",
-        "--config",
-        "$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME",
-      ]
-  ################### TAK_SUBSCRIBER
-  tak_subscriber:
-    image: *gcs_image
-    build:
-      context: ../
-      dockerfile: docker/Dockerfile.tak_subscriber
-      args:
-        - ROS_WS_DIR=${ROS_WS_DIR}
-      tags:
-        - *gcs_image
-    container_name: "${PROJECT_NAME}-tak_subscriber"
-    stdin_open: true
-    tty: true
-    restart: unless-stopped
-    depends_on:
-      mqtt:
-        condition: service_healthy
-    networks:
-      - airstack_network
-    volumes:
-      - ../ros_ws/src/ros2tak_tools/:${ROS_WS_DIR}/src/ros2tak_tools/
-    command:
-      [
-        "python3",
-        "$TAK_SUBSCRIBER_FILEPATH",
-        "--config",
-        "$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME",
-      ]
-
-  ################## ROS2COT_AGENT
-  cot2planner_agent:
-    image: *gcs_image
-    build:
-      context: ../../
-      dockerfile: ground_control_station/docker/Dockerfile.TAK
-      args:
-        - ROS_WS_DIR=${ROS_WS_DIR}
-      tags:
-        - *cot2planner_agent_image
-    container_name: "${PROJECT_NAME}-cot2planner_agent"
-    stdin_open: true
-    tty: true
-    restart: unless-stopped
-    depends_on:
-      mqtt:
-        condition: service_healthy
-    networks:
-      - airstack_network
-    command:
-      [
-        "/bin/bash",
-        "-c",
-        "source /opt/ros/humble/setup.bash && source $ROS_WS_DIR/install/setup.bash && ./install/ros2tak_tools/bin/cot2planner_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME",
-      ]
-
-  # ################## GCS_AI_AGENT
-  gcs_ai_agent:
-    image: *gcs_image
-    build:
-      context: ../../
-      dockerfile: ground_control_station/docker/Dockerfile.TAK
-      args:
-        - ROS_WS_DIR=${ROS_WS_DIR}
-      tags:
-        - *gcs_image
-    container_name: "${PROJECT_NAME}-gcs_ai_agent"
-    stdin_open: true
-    tty: true
-    restart: unless-stopped
-    environment:
-      - ROS_DOMAIN_ID=${ROS_DOMAIN_ID}
-      - AI_AGENT_NAME=${AI_AGENT_NAME}
-      - PROJECT_NAME=${PROJECT_NAME}
-    depends_on:
-      mqtt:
-        condition: service_healthy
-    #    network_mode: host
-    networks:
-      - airstack_network
-    command:
-      [
-        "/bin/bash",
-        "-c",
-        "source /opt/ros/humble/setup.bash && source $ROS_WS_DIR/install/setup.bash && ./install/ros2tak_tools/bin/chat2ros_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME",
-      ]
\ No newline at end of file
diff --git a/ground_control_station/installation/setup_ground_control_station.sh b/ground_control_station/installation/setup_ground_control_station.sh
new file mode 100755
index 00000000..51d4bede
--- /dev/null
+++ b/ground_control_station/installation/setup_ground_control_station.sh
@@ -0,0 +1,100 @@
+#!/bin/bash
+
+# Exit on error
+set -e
+# get the path of this script
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+YELLOW="\e[;33m"
+ENDCOLOR="\e[0m"
+
+# Display banner
+echo "========================================================="
+echo "        WinTAK VirtualBox Setup Script"
+echo "========================================================="
+
+# Create log directory
+LOG_DIR="$HOME/wintak_setup_logs"
+mkdir -p "$LOG_DIR"
+LOG_FILE="$LOG_DIR/setup_$(date +%Y%m%d_%H%M%S).log"
+
+# Log function
+log() {
+  echo "[$(date '+%Y-%m-%d %H:%M:%S')] $1" | tee -a "$LOG_FILE"
+}
+
+log "Starting WinTAK VirtualBox setup"
+
+log "Setting up Ground Control Station"
+
+# Create vmware directory
+log "Creating vmware directory..."
+sudo mkdir -p "$HOME/vmware"
+sudo chown -R $USER:$USER "$HOME/vmware"
+sudo chmod -R 755 "$HOME/vmware"
+
+# Create ROS config directories if they don't exist
+mkdir -p "$SCRIPT_DIR/../ros_ws/src/ros2tak_tools/config"
+mkdir -p "$SCRIPT_DIR/../ros_ws/src/ros2tak_tools/creds"
+
+# Single rsync command to get all necessary files from airlab-storage
+log "Copying all required files from airlab-storage to $HOME/vmware/..."
+log "This may take some time depending on your network connection..."
+
+# Prompt for ANDREWID
+read -p "Please enter your Andrew ID: " ANDREWID
+
+# Check if ANDREWID is provided
+if [ -z "$ANDREWID" ]; then
+    log "Error: Andrew ID cannot be empty"
+    exit 1
+fi
+
+# Set ANDREWID as environment variable
+export ANDREWID
+sudo rsync --progress -avz ${ANDREWID}@airlab-storage.andrew.cmu.edu:/volume4/dsta/engineering/atak/setup/ "$HOME/vmware/" 2>&1 | tee -a "$LOG_FILE"
+
+# Copy config and creds to ROS workspace
+log "Copying config and creds to ROS workspace..."
+sudo cp -R "$HOME/vmware/config/"* "$SCRIPT_DIR/../ros_ws/src/ros2tak_tools/config/"
+sudo cp -R "$HOME/vmware/creds/"* "$SCRIPT_DIR/../ros_ws/src/ros2tak_tools/creds/"
+
+# Set secure permissions on creds directory
+log "Setting secure permissions on credentials directory..."
+# Go to the creds directory
+pushd "$SCRIPT_DIR/../ros_ws/src/ros2tak_tools/creds/" > /dev/null
+# Set restrictive permissions on directories (700: rwx------)
+sudo find . -type d -exec chmod 700 {} \; 2>&1 | tee -a "$LOG_FILE"
+# Set restrictive permissions on files (600: rw-------)
+sudo find . -type f -exec chmod 600 {} \; 2>&1 | tee -a "$LOG_FILE"
+# Ensure proper ownership
+sudo chown -R $USER:$USER . 2>&1 | tee -a "$LOG_FILE"
+log "Credentials directory secured with restricted permissions"
+# Return to original directory
+popd > /dev/null
+
+# Install VirtualBox with apt logs
+log "Installing VirtualBox..."
+(sudo dpkg -i "$HOME/vmware/virtualbox-7.1_7.1.6-167084~Ubuntu~jammy_amd64.deb" 2>&1 | tee -a "$LOG_FILE")
+(sudo apt-get install -f -y 2>&1 | tee -a "$LOG_FILE")
+
+# Configure VirtualBox
+log "Configuring VirtualBox kernel modules..."
+(sudo /sbin/vboxconfig 2>&1 | tee -a "$LOG_FILE")
+
+# Import WinTAK VM
+log "Importing WinTAK virtual machine..."
+(VBoxManage import "$HOME/vmware/Windows11.ova" --vsys 0 --vmname "WinTAK" 2>&1 | tee -a "$LOG_FILE")
+
+# Start WinTAK VM
+log "Setup complete! Starting WinTAK..."
+(VBoxManage startvm "WinTAK" 2>&1 | tee -a "$LOG_FILE")
+
+log "WinTAK setup completed successfully"
+
+echo    "========================================================="
+echo    "  WinTAK is now running in VirtualBox"
+echo    "  To start WinTAK in the future, simply run:"
+echo    "  VBoxManage startvm \"WinTAK\""
+echo    "  Setup logs are available at: $LOG_FILE"
+echo -3 "  ${BOLDYELLOW} NOTE: WinTAK will ask you to reset the VM password on first boot. Just choose your own memorable password.${ENDCOLOR}"
+echo    "========================================================="
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml b/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml
index d4887648..32e920e8 100644
--- a/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml
+++ b/ground_control_station/ros_ws/src/gcs_bringup/config/domain_bridge.yaml
@@ -12,6 +12,7 @@ topics:
     type: behavior_tree_msgs/msg/BehaviorTreeCommands
     from_domain: 0
     to_domain: 3
+    
   /robot_1/fixed_trajectory_generator/fixed_trajectory_command:
     type: airstack_msgs/msg/FixedTrajectory
     from_domain: 0
@@ -23,4 +24,29 @@ topics:
   /robot_3/fixed_trajectory_generator/fixed_trajectory_command:
     type: airstack_msgs/msg/FixedTrajectory
     from_domain: 0
+    to_domain: 3
+    
+  /robot_1/bag_record/bag_recording_status:
+    type: std_msgs/msg/Bool
+    from_domain: 1
+    to_domain: 0
+  /robot_1/bag_record/set_recording_status:
+    type: std_msgs/msg/Bool
+    from_domain: 0
+    to_domain: 1
+  /robot_2/bag_record/bag_recording_status:
+    type: std_msgs/msg/Bool
+    from_domain: 2
+    to_domain: 0
+  /robot_2/bag_record/set_recording_status:
+    type: std_msgs/msg/Bool
+    from_domain: 0
+    to_domain: 2
+  /robot_3/bag_record/bag_recording_status:
+    type: std_msgs/msg/Bool
+    from_domain: 3
+    to_domain: 0
+  /robot_3/bag_record/set_recording_status:
+    type: std_msgs/msg/Bool
+    from_domain: 0
     to_domain: 3
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective
index df6f852d..47027615 100644
--- a/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective
+++ b/ground_control_station/ros_ws/src/gcs_bringup/config/gcs.perspective
@@ -4,14 +4,14 @@
     "mainwindow": {
       "keys": {
         "geometry": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000640000006400000776000003ab000000640000008900000776000003ab00000000000000000a00000000640000008900000776000003ab')",
+          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000014b00000069000008710000045c0000014b0000008e000008710000045c00000000000000000a000000014b0000008e000008710000045c')",
           "type": "repr(QByteArray.hex)",
-          "pretty-print": "     d d v   d   v        d   v  "
+          "pretty-print": "     K i q \\ K   q \\      K   q \\"
         },
         "state": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000713000002f9fc0100000002fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f01000000000000038f000002cd00fffffffb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f01000003950000037e0000007e00ffffff000007130000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000727000003a5fc0100000002fb0000006a007200710074005f00670072006f0075006e0064005f0063006f006e00740072006f006c005f00730074006100740069006f006e005f005f00470072006f0075006e00640043006f006e00740072006f006c00530074006100740069006f006e005f005f0031005f005f01000000000000039c000002cd00fffffffc000003a200000385000000d000fffffffa000000010200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000ffffffff0000004a00fffffffb0000006a007200710074005f0061006900720073007400610063006b005f0063006f006e00740072006f006c005f00700061006e0065006c005f005f0041006900720073007400610063006b0043006f006e00740072006f006c00500061006e0065006c005f005f0031005f005f0100000000ffffffff0000036300ffffff000007270000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
           "type": "repr(QByteArray.hex)",
-          "pretty-print": "                 jrqt_ground_control_station__GroundControlStation__1__          Brqt_behavior_tree__PyConsole__1__                            6MinimizedDockWidgetsToolbar        "
+          "pretty-print": "                 jrqt_ground_control_station__GroundControlStation__1__                                                               J                                                               c   '                                                      "
         }
       },
       "groups": {
@@ -29,18 +29,49 @@
     "pluginmanager": {
       "keys": {
         "running-plugins": {
-          "repr": "{'rqt_behavior_tree/PyConsole': [1], 'rqt_ground_control_station/GroundControlStation': [1]}",
+          "repr": "{'rqt_airstack_control_panel/AirstackControlPanel': [1], 'rqt_behavior_tree/PyConsole': [1], 'rqt_ground_control_station/GroundControlStation': [1]}",
           "type": "repr"
         }
       },
       "groups": {
+        "plugin__rqt_airstack_control_panel__AirstackControlPanel__1": {
+          "keys": {},
+          "groups": {
+            "dock_widget__": {
+              "keys": {
+                "dock_widget_title": {
+                  "repr": "'Control Panel'",
+                  "type": "repr"
+                },
+                "dockable": {
+                  "repr": "True",
+                  "type": "repr"
+                },
+                "parent": {
+                  "repr": "None",
+                  "type": "repr"
+                }
+              },
+              "groups": {}
+            },
+            "plugin": {
+              "keys": {
+                "info_dcts": {
+                  "repr": "[{'enable_display': True, 'excluded_services': [], 'hostname': '172.17.0.1', 'name': 'Localhost', 'namespace': 'none', 'password': '\\U000f01de\\U000f01db\\U000f01dc\\U000f01da', 'path': '~/airstack', 'username': 'john'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.11', 'name': 'NX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.12', 'name': 'AGX 1', 'namespace': 'robot_1', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.21', 'name': 'NX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.22', 'name': 'AGX 2', 'namespace': 'robot_2', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.31', 'name': 'NX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}, {'enable_display': False, 'excluded_services': ['docs', 'ground-control-station', 'ground-control-station-real', 'isaac-sim', 'robot'], 'hostname': '10.4.1.32', 'name': 'AGX 3', 'namespace': 'robot_3', 'password': '\\U000f01c4\\U000f01d5\\U000f01c7\\U000f01c7\\U000f01d9\\U000f01d1\\U000f0186\\U000f0180', 'path': '~/airstack', 'username': 'airlab'}]",
+                  "type": "repr"
+                }
+              },
+              "groups": {}
+            }
+          }
+        },
         "plugin__rqt_behavior_tree__PyConsole__1": {
           "keys": {},
           "groups": {
             "dock_widget__": {
               "keys": {
                 "dock_widget_title": {
-                  "repr": "''",
+                  "repr": "'Behavior Tree'",
                   "type": "repr"
                 },
                 "dockable": {
diff --git a/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml b/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml
index a10e3ce8..1814019d 100644
--- a/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml
+++ b/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml
@@ -18,5 +18,9 @@
     <arg name="config" value="$(find-pkg-share domain_bridge)/examples/example_bridge_config.yaml" />
   </include>
 -->
+
+    <!-- Include the TAK launch file -->
+  <include file="$(find-pkg-share gcs_bringup)/launch/tak.launch.xml">
+  </include>
   
 </launch>
diff --git a/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml b/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml
index 139fff7d..8e5f95de 100644
--- a/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml
+++ b/ground_control_station/ros_ws/src/gcs_bringup/launch/tak.launch.xml
@@ -1,28 +1,46 @@
 <launch>
 
-  <node pkg="ros2tak_tools" exec="ros2cot_agent"
-	args="--config $(find-pkg-share ros2tak_tools)/config/config.yaml"
-	output="screen" />
-  
-  <node pkg="ros2tak_tools" exec="cot2planner_agent"
-	args="--config $(find-pkg-share ros2tak_tools)/config/config.yaml"
-	output="screen" />
-
-  <!--
-  <node pkg="ros2tak_tools" exec="tak_subscriber"
-args="- -config $(find-pkg-share ros2tak_tools)/config/config.yaml"
-output="screen" />
-  -->
-
-  <!--
-  <node pkg="ros2tak_tools" exec="tak_publisher"
-	args="- -config $(find-pkg-share ros2tak_tools)/config/config.yaml"
-	output="screen" />
--->
+  <!-- Define paths using substitutions -->
+  <let name="config_file_path" value="$(find-pkg-share ros2tak_tools)/config/config.yaml"/>
+  <let name="creds_path" value="$(find-pkg-share ros2tak_tools)/creds"/>
+  <let name="tak_publisher" value="$(find-pkg-share ros2tak_tools)/scripts/tak_publisher.py"/>
+  <let name="tak_subscriber" value="$(find-pkg-share ros2tak_tools)/scripts/tak_subscriber.py"/>
 
+  <!-- Launch ros2cot_agent node -->
+  <node pkg="ros2tak_tools" exec="ros2cot_agent" name="ros2cot_agent" output="screen">
+    <param name="config_file_path" value="$(var config_file_path)"/>
+    <param name="creds_path" value="$(var creds_path)"/>
+  </node>
+
+  <!-- Launch ros2cot_agent node -->
+  <node pkg="ros2tak_tools" exec="chat2ros_agent" name="chat2ros_agent" output="screen">
+    <param name="config_file_path" value="$(var config_file_path)"/>
+    <param name="creds_path" value="$(var creds_path)"/>
+  </node>
+
+  <!-- Launch ros2casevac_agent node -->
+  <node pkg="ros2tak_tools" exec="ros2casevac_agent" name="ros2casevac_agent" output="screen">
+    <param name="config_file_path" value="$(var config_file_path)"/>
+    <param name="creds_path" value="$(var creds_path)"/>
+  </node>
+
+  <!--   Launch tak_publisher python executable-->
+  <executable cmd="python3 $(var tak_publisher)
+                    --config_file_path $(var config_file_path)
+                    --creds_path $(var creds_path)"
+          respawn="true" respawn_delay="5" />
+
+  <!--   Launch tak_subscriber python executable-->
+  <executable cmd="python3 $(var tak_subscriber)
+                    --config_file_path $(var config_file_path)
+                    --creds_path $(var creds_path)"
+          respawn="true" respawn_delay="5" />
+
+
+  <!--   Launch mosquitto broker-->
   <executable cmd="mosquitto" />
 
   <executable cmd="mosquitto_pub -h localhost -t healthcheck -m ping -u airlab"
 	      respawn="true" respawn_delay="5" />
-  
+
 </launch>
diff --git a/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz b/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz
index afd359ab..fdf6395d 100644
--- a/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz
+++ b/ground_control_station/ros_ws/src/gcs_bringup/rviz/gcs.rviz
@@ -6,8 +6,11 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
-      Splitter Ratio: 0.5
-    Tree Height: 725
+        - /robot 11
+        - /robot 21
+        - /robot 31
+      Splitter Ratio: 0.6411764621734619
+    Tree Height: 752
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -26,6 +29,11 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: ""
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
 Visualization Manager:
   Class: ""
   Displays:
@@ -101,6 +109,20 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: /robot_1/odometry_conversion/odometry
           Value: true
+        - Class: rviz_default_plugins/Image
+          Enabled: true
+          Max Value: 1
+          Median window: 5
+          Min Value: 0
+          Name: Robot 1 Image Left
+          Normalize Range: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/sensors/front_stereo/left/image_rect
+          Value: true
       Enabled: true
       Name: robot 1
     - Class: rviz_common/Group
@@ -109,7 +131,7 @@ Visualization Manager:
           Enabled: true
           Name: Marker
           Namespaces:
-            "": true
+            {}
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -261,33 +283,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 19.87488555908203
+      Distance: 16.989816665649414
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 1.4299687147140503
-        Y: -2.0854718685150146
-        Z: 3.9860916137695312
+        X: -0.5873028635978699
+        Y: -0.47332054376602173
+        Z: -0.03102908842265606
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7503980398178101
+      Pitch: 0.6553983688354492
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 4.923583030700684
+      Yaw: 3.5985805988311768
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 1043
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000003000000000000015600000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0056006900650077007300000002df000000d5000000a000ffffff00000001000001b900000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003b000003790000002800fffffffa000000000100000002fb000000240052006f0062006f00740020003100200049006d0061006700650020004c0065006600740100000000ffffffff000000a600fffffffb0000000a0056006900650077007300000006710000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004650000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Robot 1 Image Left:
+    collapsed: false
   Selection:
     collapsed: false
   Time:
@@ -296,6 +320,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1850
-  X: 70
-  Y: 27
+  Width: 1920
+  X: 0
+  Y: 155
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/config/.gitignore b/ground_control_station/ros_ws/src/ros2tak_tools/config/.gitignore
new file mode 100644
index 00000000..5b6b0720
--- /dev/null
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/config/.gitignore
@@ -0,0 +1 @@
+config.yaml
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml b/ground_control_station/ros_ws/src/ros2tak_tools/config/demo_config.yaml
similarity index 92%
rename from ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml
rename to ground_control_station/ros_ws/src/ros2tak_tools/config/demo_config.yaml
index f44a4e10..2ec3da59 100644
--- a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/config/demo_config.yaml
@@ -12,28 +12,21 @@
 
 
 project: 
-  name: Airstack # Name of the project. This will be used to name the services.
+  name: Airstack # Name of the project. This will be used to name the services and set the UUID of all the COT messages.
 
-robot:
-  count: 6  # Number of robots to subscribe to (e.g., 1 for robot1, 2 for robot2 and robot3, etc.)
-  gps_topicname: '/iphone{n}/gps'  # Topic name pattern for GPS data. Use {n} as a placeholder for the robot number.
+gps_streaming:
+  - name: 'drone1'
+    type: 'uav'  # Type of the robot (e.g., uav, quadruped, offroad)
+    topicname: '/robot_1/interface/mavros/global_position/raw/fix'
+    frequency: 1  # Frequency at which the GPS data should be published to TAK server in Hz.
 
 trackers:
   - name: 'base' # Note stable GPS reading 
     ip: '10.223.132.129'
     input_port: 2947
-  - name: 'target1'
-    ip: '10.223.132.61'
-    input_port: 2947
   - name: 'target2' # Name of the tracker. This can be the target or the robot streaming GPS data.
     ip: '10.223.118.110' # IP address of the tracker. (Testing using Doodle labs mesh rider)
     input_port: 2947 # Port of the Radio link to receive GPS data.
-  - name: 'target3' # NOTE: Stable GPS reading 
-    ip: '10.223.118.94'
-    input_port: 2947
-  - name: 'drone2'
-    ip: '10.4.1.20'
-    input_port: 2947
 
 
 tak_server:
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/.gitignore b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/.gitignore
deleted file mode 100644
index c4903234..00000000
--- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/.gitignore
+++ /dev/null
@@ -1 +0,0 @@
-creds/
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/__init__.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/__init__.py
old mode 100644
new mode 100755
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/chat2ros_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/chat2ros_agent.py
new file mode 100755
index 00000000..95fd2535
--- /dev/null
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/chat2ros_agent.py
@@ -0,0 +1,263 @@
+import os
+import xml.etree.ElementTree as ET
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import paho.mqtt.client as mqtt
+from tak_helper.create_cot_msgs import create_chat_COT, create_gps_COT, create_polygon_COT
+from tak_helper.logger import setup_logger
+from airstack_msgs.msg import TextQueryResponse
+import yaml
+import uuid
+import logging
+import sys
+from threading import Lock
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+
+HELP_MESSAGE = (
+    "Welcome to Fleet Control!\n\n"
+    "Available commands:\n"
+    "1. **robot {robot_name} find {area}**\n"
+    "   - Instruct a robot to locate a specified area.\n"
+    "     - `robot_name` should be a single word.\n"
+    "2. **help**\n"
+    "   - Display this help message."
+)
+
+# Shape of the CoT message:
+POLYGON = "u-d-f"
+
+
+class AiAgent(Node):
+    def __init__(self):
+        super().__init__('gcs_ai_agent')
+
+        # Initialize a basic logger before loading the config
+        self.logger = logging.getLogger("GCSAIAgent")
+
+        # Read config from config filename from the ros2 parameters
+        self.declare_parameter("config_file_path", "")
+        self.config_filepath = self.get_parameter("config_file_path").get_parameter_value().string_value
+
+        # Read credentials directory (for compatibility with ros2_cot_agent)
+        self.declare_parameter("creds_dir", "")
+        self.creds_dir = self.get_parameter("creds_dir").get_parameter_value().string_value
+
+        self.get_logger().info(f"Loading configuration from {self.config_filepath}")
+        self.get_logger().info(f"Credentials directory: {self.creds_dir}")
+
+        # Load the configuration
+        try:
+            with open(self.config_filepath, 'r') as file:
+                config = yaml.safe_load(file)
+
+            # Setup logger based on config
+            log_level = config.get('logging', {}).get('level', 'INFO')
+            self.logger = setup_logger(self, log_level)
+            self.logger.info(f"Logger configured with level: {log_level}")
+
+        except Exception as e:
+            self.get_logger().error(f"Failed to load configuration: {e}")
+            sys.exit(1)
+
+        # Read environment variables for configuration
+        self.project_name = config.get("project", {}).get("name", os.getenv("PROJECT_NAME", "airlab"))
+        self.ai_agent_name = os.getenv("AI_AGENT_NAME", "aerolens.ai")
+
+        # MQTT Configurations
+        mqtt_config = config['mqtt']
+        self.mqtt_broker = mqtt_config.get('host', "localhost")
+        self.mqtt_port = int(mqtt_config['port'])
+        self.mqtt_username = mqtt_config['username']
+        self.mqtt_pwd = mqtt_config['password']
+        self.mqtt2tak_topic = config['services']['publisher']['tak_publisher']['topic_name']
+        self.mqtt_subscribe_topic = config["services"]["mediator"]["chat2ros_agent"]["mqtt_subcribe_topic"]
+
+        # ROS Configurations
+        self.ros_robot_query_txt_topic = config["services"]["mediator"]["chat2ros_agent"]["ros_query_text_topic"]
+        self.ros_robot_query_response_topic = config["services"]["mediator"]["chat2ros_agent"][
+            "ros_query_response_topic"]
+        self.robot_publisher = {}
+
+        # Set up MQTT client
+        self.mqtt_client = mqtt.Client()
+        self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd)
+        self.mqtt_client.on_message = self._on_mqtt_message
+
+        # Create a QoS profile for ROS subscriptions
+        self.qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,
+            durability=DurabilityPolicy.VOLATILE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        # Connect to MQTT broker and subscribe to topic
+        try:
+            self.logger.info(f"Connecting to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
+            self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port, keepalive=65535)
+            self.mqtt_client.subscribe(self.mqtt_subscribe_topic)
+            self.mqtt_client.loop_start()
+            self.logger.info(
+                f"Connected and subscribed to MQTT topic '{self.mqtt_subscribe_topic}' on broker {self.mqtt_broker}:{self.mqtt_port}")
+        except Exception as e:
+            self.logger.error(f"Failed to connect or subscribe to MQTT: {e}")
+            self.logger.error(f"Exception type: {type(e)}")
+
+    def _get_robot_text_query(self, query):
+        return f"{query}"
+
+    def _on_mqtt_message(self, client, userdata, msg):
+        # Parse the XML message
+        try:
+            root = ET.fromstring(msg.payload.decode('utf-8'))
+            remarks_tag = root.find(".//remarks")
+            remarks = remarks_tag.text.lower()
+            self.logger.info(f"Received message: {remarks}")
+            self.process_remarks(remarks)
+        # Capture NoneType error
+        except AttributeError as e:
+            self.logger.warning(f"Failed to parse message: {e}")
+        except Exception as e:
+            self.logger.error(f"Failed to process message: {e}")
+            self.logger.error(f"Exception type: {type(e)}")
+
+    def process_remarks(self, remarks):
+        """Process the remarks and act accordingly."""
+        if remarks.startswith("robot"):
+            # Example: "robot {robot_name} find {area}"
+            parts = remarks.split(" ")
+            if len(parts) >= 4 and parts[2] == "find":
+                robot_name = parts[1]
+                area = " ".join(parts[3:])
+                self.publish_txt_query_to_robot(robot_name, area)
+            else:
+                self.logger.warning(f"Invalid robot command format: {remarks}")
+                help_cot_message = create_chat_COT(uuid=str(uuid.uuid4()), callsign=self.ai_agent_name,
+                                                   message=HELP_MESSAGE)
+                self.send_message_to_TAK(cot_message=help_cot_message)
+        elif remarks.lower() == "help":
+            help_cot_message = create_chat_COT(uuid=str(uuid.uuid4()), callsign=self.ai_agent_name,
+                                               message=HELP_MESSAGE)
+            self.send_message_to_TAK(cot_message=help_cot_message)
+        else:
+            self.logger.info(f"Unrecognized command: {remarks}")
+
+    def publish_txt_query_to_robot(self, robot_name, area):
+        """Publish the area to the ROS robot's query topic."""
+        message = self._get_robot_text_query(area)
+        msg = String()
+        msg.data = message
+
+        request_topic_name = f"/{robot_name}{self.ros_robot_query_txt_topic}"
+        response_topic_name = f"/{robot_name}{self.ros_robot_query_response_topic}"
+
+        if robot_name not in self.robot_publisher:
+            self.robot_publisher[robot_name] = self.create_publisher(String, request_topic_name, 10)
+        self.robot_publisher[robot_name].publish(msg)
+        self.logger.info(f"Sent command to {request_topic_name} to find {area}")
+
+        # Send a chat message
+        message = f"Sent command to {robot_name} to find '{area}'"
+        cot_message = create_chat_COT(uuid=str(uuid.uuid4()), callsign=self.ai_agent_name, message=message)
+        self.send_message_to_TAK(cot_message=cot_message)
+
+        # Create a subscriber to listen for the response
+        self.create_subscription(
+            TextQueryResponse,
+            response_topic_name,
+            lambda msg: self._on_robot_response(msg, robot_name),
+            self.qos_profile
+        )
+
+    def _on_robot_response(self, msg, robot_name):
+        """Callback for processing robot response and sending CoT messages."""
+        log_prefix = f"GCSAIAgent.{robot_name}"
+        robot_logger = logging.getLogger(log_prefix)
+
+        # Extracting the header information
+        header = msg.header
+        robot_logger.info(f"Header information: seq={header.seq}, stamp={header.stamp}, frame_id={header.frame_id}")
+
+        # Extract the tag name
+        tag_name = msg.tag_name
+        robot_logger.info(f"Tag name: {tag_name}")
+
+        # Extracting the geofence data (which is an array of NavSatFix)
+        geofence_data = msg.geofence
+        geofence_info = ""
+
+        # Create a list of GPS points
+        gps_points = []
+
+        for i, gps_fix in enumerate(geofence_data):
+            # Create a list of GPS points with {"lat": str, "lon": str, "hae": str}
+            gps_point = (gps_fix.latitude, gps_fix.longitude)
+            gps_points.append(gps_point)
+            geofence_info += f"Point {i}: lat={gps_fix.latitude}, lon={gps_fix.longitude}\n"
+
+        polygon_cot_message = create_polygon_COT(uuid=tag_name, callsign=self.ai_agent_name, gps_coordinates=gps_points)
+        robot_logger.info(f"Geofence data:\n{geofence_info}")
+
+        # Send the polygon CoT message
+        self.send_message_to_TAK(polygon_cot_message)
+
+        # Send confirmation chat message
+        cot_chat_message = create_chat_COT(
+            uuid=str(uuid.uuid4()),
+            callsign=self.ai_agent_name,
+            message=f"Response received! Please check the geofence data for {tag_name} near {robot_name}"
+        )
+        self.send_message_to_TAK(cot_chat_message)
+
+    def send_message_to_TAK(self, cot_message):
+        """Send a message to the TAK topic."""
+        mqtt_logger = logging.getLogger("GCSAIAgent.MQTT")
+        try:
+            self.mqtt_client.publish(self.mqtt2tak_topic, cot_message)
+            mqtt_logger.debug(f"Sent message to topic {self.mqtt2tak_topic}")
+        except Exception as e:
+            mqtt_logger.error(f"Failed to send message: {e}")
+            mqtt_logger.error(f"Exception type: {type(e)}")
+
+    def destroy_node(self):
+        """Stop MQTT loop and destroy the node."""
+        self.logger.info("Shutting down GCS AI Agent")
+        self.mqtt_client.loop_stop()
+        self.mqtt_client.disconnect()  # Explicit disconnect
+        self.logger.info("MQTT client disconnected")
+        super().destroy_node()
+
+
+def main(args=None):
+    # Basic logger setup before config is loaded
+    logging.basicConfig(
+        level=logging.INFO,
+        format='%(asctime)s - %(levelname)s - %(name)s - %(message)s'
+    )
+    startup_logger = logging.getLogger("GCSAIAgent.startup")
+    startup_logger.info("Starting GCS AI Agent")
+
+    rclpy.init(args=args)
+
+    try:
+        # Create the AI agent node
+        startup_logger.info("Creating AI Agent node")
+        ai_agent = AiAgent()
+
+        # Spin the node
+        startup_logger.info("Node initialized successfully, starting spin")
+        rclpy.spin(ai_agent)
+    except Exception as e:
+        startup_logger.critical(f"Fatal error occurred: {e}")
+    finally:
+        # Shutdown and cleanup
+        startup_logger.info("Shutting down node")
+        if 'ai_agent' in locals():
+            ai_agent.destroy_node()
+        rclpy.shutdown()
+        startup_logger.info("Node has shut down cleanly")
+
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py
old mode 100644
new mode 100755
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2casevac_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2casevac_agent.py
index 2a58cc56..b9eb356f 100755
--- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2casevac_agent.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2casevac_agent.py
@@ -7,6 +7,9 @@
 
 Author: Aditya Rauniyar (rauniyar@cmu.edu)
 
+Usage:
+    1. Run the script with the following command, specifying the path to the config file:
+       ros2 run your_package ros2casevac_agent --ros-args -p config_file_path:=config.yaml -p creds_dir:=/path/to/creds
 """
 
 import rclpy
@@ -14,557 +17,12 @@
 import paho.mqtt.client as mqtt
 import argparse
 import yaml
+import logging
+import sys
 from straps_msgs.msg import CasualtyMeta, Injury, Critical, Vitals
-from xml.etree.ElementTree import Element, SubElement, tostring
-from datetime import datetime
+from tak_helper.Casualty import CasualtyCOT, create_casualty_id
 import pytak
-from enum import Enum
-
-
-"""
-Enums to represent the different types of injuries and vitals.
-"""
-
-
-class TraumaType(Enum):
-    TRAUMA_HEAD = Injury.TRAUMA_HEAD
-    TRAUMA_TORSO = Injury.TRAUMA_TORSO
-    TRAUMA_LOWER_EXT = Injury.TRAUMA_LOWER_EXT
-    TRAUMA_UPPER_EXT = Injury.TRAUMA_UPPER_EXT
-    ALERTNESS_OCULAR = Injury.ALERTNESS_OCULAR
-    ALERTNESS_VERBAL = Injury.ALERTNESS_VERBAL
-    ALERTNESS_MOTOR = Injury.ALERTNESS_MOTOR
-
-
-class TraumaSeverity(Enum):
-    NORMAL = Injury.TRAUMA_NORMAL
-    WOUND = Injury.TRAUMA_WOUND
-    AMPUTATION = Injury.TRAUMA_AMPUTATION
-
-
-class OcularAlertness(Enum):
-    OPEN = Injury.OCULAR_OPEN
-    CLOSED = Injury.OCULAR_CLOSED
-    NOT_TESTABLE = Injury.OCULAR_NOT_TESTABLE
-
-
-class AlertnessLevel(Enum):
-    NORMAL = Injury.ALERTNESS_NORMAL
-    ABNORMAL = Injury.ALERTNESS_ABNORMAL
-    ABSENT = Injury.ALERTNESS_ABSENT
-    NOT_TESTABLE = Injury.ALERTNESS_NOT_TESTABLE
-
-
-class VitalType(Enum):
-    HEART_RATE = Vitals.HEART_RATE
-    RESPIRATORY_RATE = Vitals.RESPIRATORY_RATE
-
-
-class ConditionType(Enum):
-    SEVERE_HEMORRHAGE = Critical.SEVERE_HEMORRHAGE
-    RESPIRATORY_DISTRESS = Critical.RESPIRATORY_DISTRESS
-
-
-class ConditionStatus(Enum):
-    ABSENT = Critical.ABSENT
-    PRESENT = Critical.PRESENT
-
-
-"""
-Functions to validate the type and value of the injury.
-"""
-
-
-def is_valid_type_injury_value(trauma_type, value):
-    """Validates that the value matches the type based on the rules."""
-    if trauma_type in [TraumaType.TRAUMA_HEAD, TraumaType.TRAUMA_TORSO]:
-        # TRAUMA_HEAD and TRAUMA_TORSO should have values NORMAL or WOUND
-        return value in [TraumaSeverity.NORMAL, TraumaSeverity.WOUND]
-
-    elif trauma_type in [TraumaType.TRAUMA_LOWER_EXT, TraumaType.TRAUMA_UPPER_EXT]:
-        # TRAUMA_LOWER_EXT and TRAUMA_UPPER_EXT should have values NORMAL, WOUND, or AMPUTATION
-        return value in [
-            TraumaSeverity.NORMAL,
-            TraumaSeverity.WOUND,
-            TraumaSeverity.AMPUTATION,
-        ]
-
-    elif trauma_type == TraumaType.ALERTNESS_OCULAR:
-        # ALERTNESS_OCULAR should have values OPEN, CLOSED, or NOT_TESTABLE
-        return value in [
-            OcularAlertness.OPEN,
-            OcularAlertness.CLOSED,
-            OcularAlertness.NOT_TESTABLE,
-        ]
-
-    elif trauma_type in [TraumaType.ALERTNESS_VERBAL, TraumaType.ALERTNESS_MOTOR]:
-        # ALERTNESS_VERBAL and ALERTNESS_MOTOR should have values NORMAL, ABNORMAL, ABSENT, or NOT_TESTABLE
-        return value in [
-            AlertnessLevel.NORMAL,
-            AlertnessLevel.ABNORMAL,
-            AlertnessLevel.ABSENT,
-            AlertnessLevel.NOT_TESTABLE,
-        ]
-
-    return False
-
-
-# Function to create a unique casualty ID
-def create_casualty_id(casualty_id: int):
-    return f"casualty-{casualty_id}"
-
-
-"""
-Classes to represent a Casualty object with all the necessary information for triage.
-"""
-
-
-class GPSCOT:
-    """
-    GPS class to store the GPS coordinates of the casualty.
-    """
-
-    # Define the types
-    status: bool
-    latitude: float
-    longitude: float
-    altitude: float
-
-    def __init__(self):
-        self.status = False
-        self.latitude = 0.0
-        self.longitude = 0.0
-        self.altitude = 0.0
-
-    def set_gps(self, latitude, longitude, altitude):
-        self.latitude = latitude
-        self.longitude = longitude
-        self.altitude = altitude
-        self.status = True
-
-
-class StatusCOT:
-    """
-    Base class to store the status information.
-    """
-
-    name: str
-    status: bool  # True if diagnosis has been made, False otherwise
-    system: str
-    confidence: float
-
-    def __init__(self, name="", system="", confidence=0.0):
-        self.name = name
-        self.status = False
-        self.system = system
-        self.confidence = confidence
-
-    def __str__(self):
-        return f"{self.name}({int(self.confidence * 100)}%)"
-
-
-class VitalsCOT(StatusCOT):
-    """
-    Derived class to store vitals information.
-    """
-
-    vitalsType: VitalType
-    value: float
-    time_ago: float
-
-    def __init__(
-        self,
-        vitals_name="",
-        system="",
-        type_=None,
-        value=None,
-        time_ago=None,
-        confidence=0.0,
-    ):
-        super().__init__(vitals_name, system, confidence)
-        self.vitalsType = type_
-        self.value = value
-        self.time_ago = time_ago
-
-    def set_vitals(self, system, type_, value, time_ago, confidence):
-        if type_ not in [VitalType.HEART_RATE, VitalType.RESPIRATORY_RATE]:
-            raise ValueError("Type must be either HEART_RATE or RESPIRATORY_RATE")
-        self.system = system
-        self.vitalsType = type_
-        self.value = value
-        self.time_ago = time_ago
-        self.confidence = confidence
-        self.status = True
-
-    def __str__(self):
-        return (
-            f"{self.name}({self.value}, {int(self.confidence * 100)}%)"
-        )
-
-
-class InjuryCOT(StatusCOT):
-    """
-    Derived class to store injury information.
-    """
-
-    injuryType: TraumaType
-    
-
-    def __init__(
-        self, injury_name="", system="", type_=None, value=None, confidence=0.0
-    ):
-        super().__init__(injury_name, system, confidence)
-        self.injuryType = type_
-        self.value = value
-
-    def set_status(self, system, type_, value, confidence):
-        if not is_valid_type_injury_value(type_, value):
-            raise ValueError(f"Invalid value for type {type_}: {value}")
-        self.system = system
-        self.injuryType = TraumaType(value=type_)
-        
-        # update the value based on type
-        if type_ == TraumaType.TRAUMA_HEAD or type_ == TraumaType.TRAUMA_TORSO or type_ == TraumaType.TRAUMA_LOWER_EXT or type_ == TraumaType.TRAUMA_UPPER_EXT:
-            self.value = TraumaSeverity(value=value)
-        elif type_ == TraumaType.ALERTNESS_OCULAR:
-            self.value = OcularAlertness(value=value)
-        elif type_ == TraumaType.ALERTNESS_VERBAL or type_==TraumaType.ALERTNESS_MOTOR:
-            self.value = AlertnessLevel(value=value)
-        
-        self.confidence = confidence
-        self.status = True
-    
-    def __str__(self):
-        return (
-            f"{self.name}({self.value.name}, {int(self.confidence * 100)}%)"
-        )
-
-
-class CriticalCOT(StatusCOT):
-    """
-    Derived class to store critical condition information.
-    """
-
-    criticalType: ConditionType
-    value: ConditionStatus
-
-    def __init__(
-        self, critical_name="", system="", type_=None, value=None, confidence=0.0
-    ):
-        super().__init__(critical_name, system, confidence)
-        self.criticalType = type_
-        self.value = value
-
-    def set_status(self, system, type_, value, confidence):
-        self.system = system
-        self.criticalType = type_
-        self.value = value
-        self.confidence = confidence
-        self.status = True
-
-
-class CasualtyCOT:
-    """
-    Casualty class to store all the information of a casualty.
-    """
-
-    # Define the class types
-    gps: GPSCOT
-    stamp: str
-    casualty_id: str
-    # Critical conditions
-    severe_hemorrhage: CriticalCOT
-    respiratory_distress: CriticalCOT
-    # Vitals
-    heart_rate: VitalsCOT
-    respiratory_rate: VitalsCOT
-    # Injuries
-    trauma_head: InjuryCOT
-    trauma_torso: InjuryCOT
-    trauma_lower_ext: InjuryCOT
-    trauma_upper_ext: InjuryCOT
-    # Alertness
-    alertness_ocular: InjuryCOT
-    alertness_verbal: InjuryCOT
-    alertness_motor: InjuryCOT
-
-    def __init__(self, casualty_id: int):
-        self.stamp = pytak.cot_time()
-
-        self.casualty_id = create_casualty_id(casualty_id)
-
-        self.gps = GPSCOT()
-
-        self.severe_hemorrhage = CriticalCOT("Severe Hemorrhage")
-        self.respiratory_distress = CriticalCOT("Respiratory Distress")
-
-        self.heart_rate = VitalsCOT("Heart Rate")
-        self.respiratory_rate = VitalsCOT("Respiratory Rate")
-
-        self.trauma_head = InjuryCOT("Trauma Head")
-        self.trauma_torso = InjuryCOT("Trauma Torso")
-        self.trauma_lower_ext = InjuryCOT("Trauma Lower Extremity")
-        self.trauma_upper_ext = InjuryCOT("Trauma Upper Extremity")
-
-        self.alertness_ocular = InjuryCOT("Alertness Ocular")
-        self.alertness_verbal = InjuryCOT("Alertness Verbal")
-        self.alertness_motor = InjuryCOT("Alertness Motor")
-
-    # ZMIST Report Fields:
-    # Z: Zap Number – A unique identifier for the casualty.
-    # M: Mechanism of Injury – Describes how the injury occurred (e.g., explosion, gunshot wound).
-    # I: Injuries Sustained – Specifies the injuries observed (e.g., right leg amputation).
-    # S: Signs and Symptoms – Details vital signs and symptoms (e.g., massive hemorrhage, no radial pulse).
-    # T: Treatments Rendered – Lists the medical interventions provided (e.g., tourniquet applied, pain medication administered).
-
-    def get_zap_num(self):
-        return f"{self.casualty_id}"
-
-    def get_mechanism(self):
-        return "Unknown"
-
-    def get_injuries(self):
-        """Returns the injuries sustained for preset status"""
-
-        injuries = []
-        # Trauma 
-        if self.trauma_head.status and self.trauma_head.value != TraumaSeverity.NORMAL:
-            injuries.append(str(self.trauma_head))
-            print(f"trauma_head detected for {self.casualty_id}")
-        if self.trauma_torso.status and self.trauma_torso.value != TraumaSeverity.NORMAL:
-            injuries.append(str(self.trauma_torso))
-            print(f"trauma_torso detected for {self.casualty_id}")
-        if self.trauma_lower_ext.status and self.trauma_lower_ext.value != TraumaSeverity.NORMAL:
-            injuries.append(str(self.trauma_lower_ext))
-            print(f"trauma_lower_ext detected for {self.casualty_id}")
-        if self.trauma_upper_ext.status and self.trauma_upper_ext.value != TraumaSeverity.NORMAL:
-            injuries.append(str(self.trauma_upper_ext))
-            print(f"trauma_upper_ext detected for {self.casualty_id}")
-        # Alertness 
-        if self.alertness_ocular.status and self.alertness_ocular.value != OcularAlertness.OPEN:
-            injuries.append(str(self.alertness_ocular))            
-            print(f"alertness_ocular detected for {self.casualty_id}")   
-        if self.alertness_verbal.status and self.alertness_verbal.value != AlertnessLevel.NORMAL:
-            injuries.append(str(self.alertness_verbal))            
-            print(f"alertness_verbal detected for {self.casualty_id}")   
-        if self.alertness_motor.status and self.alertness_motor.value != AlertnessLevel.NORMAL:
-            injuries.append(str(self.alertness_motor))            
-            print(f"alertness_motor detected for {self.casualty_id}")    
-        print(f"Current injuries : {injuries}")
-        return ", ".join(injuries)
-
-    def get_signs_symptoms(self):
-        """Returns the signs and symptoms for preset status"""
-
-        signs = []
-        if self.severe_hemorrhage.status and self.severe_hemorrhage.value == ConditionStatus.PRESENT:
-            sev_hem_string = str(self.severe_hemorrhage)
-            print(f"severe_hemorrhage: {self.severe_hemorrhage}")
-            signs.append(sev_hem_string)
-        if self.respiratory_distress.status and self.respiratory_distress.value == ConditionStatus.PRESENT:
-            signs.append(str(self.respiratory_distress))
-            print(f"Respiratory distress: {str(self.respiratory_distress)}")
-        if self.heart_rate.status:
-            signs.append(str(self.heart_rate))
-            print(f"heart rate: {str(self.heart_rate)}")
-        if self.respiratory_rate.status:
-            signs.append(str(self.respiratory_rate))
-            print(f"Respiratory rate: {str(self.respiratory_rate)}")
-        print(f"Current signs: {signs}")
-        return ", ".join(signs)
-
-    def get_treatments(self):
-        return "Unknown"
-
-    def update_casualty_metadata(self, msg: CasualtyMeta):
-        """Updates the casualty metadata with the message data."""
-
-        # Update GPS coordinates
-        if msg.valid_gps:
-            self.gps.set_gps(msg.gps.latitude, msg.gps.longitude, msg.gps.altitude)
-
-        # Update critical conditions
-        if msg.valid_severe_hemorrhage:
-            self.severe_hemorrhage.set_status(
-                system="Circulatory",
-                type_=ConditionType.SEVERE_HEMORRHAGE,
-                value=ConditionStatus(msg.severe_hemorrhage.value),
-                confidence=msg.severe_hemorrhage.confidence,
-            )
-        if msg.valid_respiratory_distress:
-            self.respiratory_distress.set_status(
-                system="Respiratory",
-                type_=ConditionType.RESPIRATORY_DISTRESS,
-                value=ConditionStatus(msg.respiratory_distress.value),
-                confidence=msg.respiratory_distress.confidence,
-            )
-
-        # Update vitals
-        if msg.valid_heart_rate:
-            self.heart_rate.set_vitals(
-                system="Cardiovascular",
-                type_=VitalType.HEART_RATE,
-                value=msg.heart_rate.value,
-                time_ago=msg.heart_rate.time_ago,
-                confidence=msg.heart_rate.confidence,
-            )
-        if msg.valid_respiratory_rate:
-            self.respiratory_rate.set_vitals(
-                system="Respiratory",
-                type_=VitalType.RESPIRATORY_RATE,
-                value=msg.respiratory_rate.value,
-                time_ago=msg.respiratory_rate.time_ago,
-                confidence=msg.respiratory_rate.confidence,
-            )
-
-        # Update injuries
-        if msg.valid_trauma_head:
-            self.trauma_head.set_status(
-                system="Head",
-                type_=TraumaType.TRAUMA_HEAD,
-                value=TraumaSeverity(msg.trauma_head.value),
-                confidence=msg.trauma_head.confidence,
-            )
-        if msg.valid_trauma_torso:
-            self.trauma_torso.set_status(
-                system="Torso",
-                type_=TraumaType.TRAUMA_TORSO,
-                value=TraumaSeverity(msg.trauma_torso.value),
-                confidence=msg.trauma_torso.confidence,
-            )
-        if msg.valid_trauma_lower_ext:
-            self.trauma_lower_ext.set_status(
-                system="Lower Extremity",
-                type_=TraumaType.TRAUMA_LOWER_EXT,
-                value=TraumaSeverity(msg.trauma_lower_ext.value),
-                confidence=msg.trauma_lower_ext.confidence,
-            )
-        if msg.valid_trauma_upper_ext:
-            self.trauma_upper_ext.set_status(
-                system="Upper Extremity",
-                type_=TraumaType.TRAUMA_UPPER_EXT,
-                value=TraumaSeverity(msg.trauma_upper_ext.value),
-                confidence=msg.trauma_upper_ext.confidence,
-            )
-
-        # Update alertness levels
-        if msg.valid_alertness_ocular:
-            self.alertness_ocular.set_status(
-                system="Neurological",
-                type_=TraumaType.ALERTNESS_OCULAR,
-                value=OcularAlertness(msg.alertness_ocular.value),
-                confidence=msg.alertness_ocular.confidence,
-            )
-        if msg.valid_alertness_verbal:
-            self.alertness_verbal.set_status(
-                system="Neurological",
-                type_=TraumaType.ALERTNESS_VERBAL,
-                value=AlertnessLevel(msg.alertness_verbal.value),
-                confidence=msg.alertness_verbal.confidence,
-            )
-        if msg.valid_alertness_motor:
-            self.alertness_motor.set_status(
-                system="Neurological",
-                type_=TraumaType.ALERTNESS_MOTOR,
-                value=AlertnessLevel(msg.alertness_motor.value),
-                confidence=msg.alertness_motor.confidence,
-            )
-
-    def generate_cot_event(self):
-        # Create root event element
-        event = Element(
-            "event",
-            {
-                "version": "2.0",
-                "uid": self.casualty_id,
-                "type": "b-r-f-h-c",
-                "how": "h-g-i-g-o",
-                "time": self.stamp,
-                "start": self.stamp,
-                "stale": pytak.cot_time(2400),
-            },
-        )
-
-        # Create point element
-        point = SubElement(
-            event,
-            "point",
-            {
-                "lat": f"{self.gps.latitude}",
-                "lon": f"{self.gps.longitude}",
-                "hae": "9999999.0",
-                "ce": "9999999.0",
-                "le": "9999999.0",
-            },
-        )
-
-        # Create detail element
-        detail = SubElement(event, "detail")
-
-        # Add contact element
-        contact = SubElement(detail, "contact", {"callsign": self.casualty_id})
-
-        # Add link element
-        link = SubElement(
-            detail,
-            "link",
-            {
-                "type": "a-f-G-U-C-I",
-                "uid": "S-1-5-21-942292099-3747883346-3641641706-1000",
-                "parent_callsign": self.casualty_id,
-                "relation": "p-p",
-                "production_time": self.stamp,
-            },
-        )
-
-        # Add archive and status elements
-        SubElement(detail, "archive")
-        SubElement(detail, "status", {"readiness": "false"})
-        SubElement(detail, "remarks")
-
-        # Create _medevac_ element with nested zMistsMap and zMist
-        medevac = SubElement(
-            detail,
-            "_medevac_",
-            {
-                "title": self.casualty_id.upper(),
-                "casevac": "false",
-                "freq": "0.0",
-                "equipment_none": "true",
-                "security": "0",
-                "hlz_marking": "3",
-                "terrain_none": "true",
-                "obstacles": "None",
-                "medline_remarks": "",
-                "zone_prot_selection": "0",
-            },
-        )
-
-        zMistsMap = SubElement(medevac, "zMistsMap")
-        zMist = SubElement(
-            zMistsMap,
-            "zMist",
-            {
-                "z": self.get_zap_num(),
-                "m": self.get_mechanism(),
-                "i": self.get_injuries(),
-                "s": self.get_signs_symptoms(),
-                "t": self.get_treatments(),
-                "title": "ZMIST1",
-            },
-        )
-
-        # Add _flow-tags_ element
-        flow_tags = SubElement(
-            detail,
-            "_flow-tags_",
-            {
-                "TAK-Server-f6edbf55ccfa4af1b4cfa2d7f177ea67": f"chiron-tak_subscriber: {datetime.utcnow().strftime('%Y-%m-%dT%H:%M:%SZ')}"
-            },
-        )
-
-        # Convert to a string
-        return tostring(event, encoding="utf-8").decode("utf-8")
+from tak_helper.logger import setup_logger
 
 
 def load_config(file_path):
@@ -582,47 +40,72 @@ def load_config(file_path):
 
 
 class ROS2COTPublisher(Node):
-    def __init__(self, config):
+    def __init__(self):
         super().__init__("ros2casevac_agent")
         self.subscribers = []
 
+        # Read config from config filename from the ros2 parameters
+        self.declare_parameter("config_file_path", "")
+        self.config_filepath = self.get_parameter("config_file_path").get_parameter_value().string_value
+
+        # Initialize a basic logger before loading the config
+        self.logger = logging.getLogger("ROS2CASEVAC")
+
+        self.get_logger().info(f"Loading configuration from {self.config_filepath}")
+
+        # Load the configuration
+        try:
+            config = load_config(self.config_filepath)
+
+            # Setup logger based on config
+            log_level = config.get('logging', {}).get('level', 'INFO')
+            self.logger = setup_logger(self, log_level)
+            self.logger.info(f"Logger configured with level: {log_level}")
+
+        except Exception as e:
+            self.get_logger().error(f"Failed to load configuration: {e}")
+            sys.exit(1)
+
+        # Read the credentials dir from the ros2 parameters
+        self.declare_parameter("creds_dir", "")
+        self.creds_dir = self.get_parameter("creds_dir").get_parameter_value().string_value
+        self.logger.info(f"Loading credentials from {self.creds_dir}")
+
         # Get host and port from the config
         self.host = config["services"]["host"]
         self.project_name = config["project"]["name"]
 
         # MQTT related configs
-        self.mqtt_broker = config["mqtt"]["host"]
-        self.mqtt_port = config["mqtt"]["port"]
-        self.mqtt_username = config["mqtt"]["username"]
-        self.mqtt_pwd = config["mqtt"]["password"]
-        self.mqtt_topicname = config["services"]["mediator"]["ros2casevac_agent"][
-            "topic_name"
-        ]
-
-        self.ros_casualty_meta_topic_name = config["services"]["mediator"][
-            "ros2casevac_agent"
-        ]["ros_casualty_meta_topic_name"]
-        self.ros_casualty_image_topic_name = config["services"]["mediator"][
-            "ros2casevac_agent"
-        ]["ros_casualty_image_topic_name"]
+        try:
+            self.mqtt_broker = config["mqtt"]["host"]
+            self.mqtt_port = config["mqtt"]["port"]
+            self.mqtt_username = config["mqtt"]["username"]
+            self.mqtt_pwd = config["mqtt"]["password"]
+            self.mqtt_topicname = config["services"]["mediator"]["ros2casevac_agent"]["topic_name"]
+
+            self.ros_casualty_meta_topic_name = config["services"]["mediator"]["ros2casevac_agent"]["ros_casualty_meta_topic_name"]
+            self.ros_casualty_image_topic_name = config["services"]["mediator"]["ros2casevac_agent"]["ros_casualty_image_topic_name"]
+
+            self.logger.info(
+                f"MQTT CONFIG: Broker={self.mqtt_broker}, Port={self.mqtt_port}, Topic={self.mqtt_topicname}")
+        except KeyError as e:
+            self.logger.error(f"Missing required configuration key: {e}")
+            sys.exit(1)
 
         # Setting MQTT
         self.mqtt_client = mqtt.Client()
         # Set the username and password
         self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd)
         try:
-            print(f"Trying to connect to {self.mqtt_broker}:{self.mqtt_port}")
+            self.logger.info(f"Attempting to connect to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
             self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port, keepalive=65535)
-            self.mqtt_client.loop_start()
-            self.get_logger().info(
-                f"Connected to MQTT ({self.mqtt_broker}:{self.mqtt_port})"
-            )
+            self.mqtt_client.loop_start()  # Start MQTT loop in background thread
+            self.logger.info(f"Connected to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
         except Exception as e:
-            print(f"Failed to connect or publish: {e}")
+            self.logger.error(f"Failed to connect to MQTT broker: {e}")
+            self.logger.error(f"Exception type: {type(e)}")
 
-        self.get_logger().info(
-            f"Starting ROS2CASEVAC_AGENT for project: {self.project_name}"
-        )
+        self.logger.info(f"Starting ROS2CASEVAC_AGENT for project: {self.project_name}")
 
         # Subscribe to the casualty meta data topic with msg type CasualtyMeta
         self.casualty_meta_subscriber = self.create_subscription(
@@ -631,15 +114,13 @@ def __init__(self, config):
             self.casualty_meta_callback,
             10,
         )
-        self.get_logger().info(
-            f"Subscribed to {self.ros_casualty_meta_topic_name} topic"
-        )
+        self.logger.info(f"Subscribed to {self.ros_casualty_meta_topic_name} topic")
 
     def casualty_meta_callback(self, msg):
         """Callback for the casualty meta data subscriber"""
         global CASUALTY_META_DATA
 
-        self.get_logger().info(f"Received CasualtyMeta message: {msg}")
+        self.logger.info(f"Received CasualtyMeta message: {msg}")
 
         # get the casualty id from the message
         casualty_id = create_casualty_id(msg.casualty_id)
@@ -647,61 +128,75 @@ def casualty_meta_callback(self, msg):
         if casualty_id not in CASUALTY_META_DATA:
             # create a new CasualtyCOT object
             CASUALTY_META_DATA[casualty_id] = CasualtyCOT(msg.casualty_id)
-            self.get_logger().info(
-                f"Created new CasualtyCOT object for casualty: {casualty_id}"
-            )
+            self.logger.info(f"Created new CasualtyCOT object for casualty: {casualty_id}")
 
         # update the CasualtyCOT object with the new data
         CASUALTY_META_DATA[casualty_id].update_casualty_metadata(msg)
-        self.get_logger().info(
-            f"Updated CasualtyCOT object for casualty: {casualty_id}"
-        )
+        self.logger.info(f"Updated CasualtyCOT object for casualty: {casualty_id}")
 
         # send the updated CoT event over MQTT if the GPS data is available
         if CASUALTY_META_DATA[casualty_id].gps.status:
             self.send_cot_event_over_mqtt(
-                CASUALTY_META_DATA[casualty_id].generate_cot_event()
+                CASUALTY_META_DATA[casualty_id].generate_cot_event(),
+                casualty_id
             )
-            self.get_logger().info(f"Sent CoT event for casualty: {casualty_id}")
+            self.logger.info(f"Sent CoT event for casualty: {casualty_id}")
 
-    def send_cot_event_over_mqtt(self, cot_event):
+    def send_cot_event_over_mqtt(self, cot_event, casualty_id=None):
         """Send CoT event over the MQTT network"""
+        log_prefix = f"ROS2CASEVAC.MQTT.{casualty_id}" if casualty_id else "ROS2CASEVAC.MQTT"
+        logger = logging.getLogger(log_prefix)
+
         try:
             self.mqtt_client.publish(self.mqtt_topicname, cot_event)
-            self.get_logger().info(
-                f"Message '{cot_event}' published to topic '{self.mqtt_topicname}'"
-            )
-        except:
-            self.get_logger().error(f"Failed to publish.")
+            logger.debug(f"CoT event published to topic '{self.mqtt_topicname}'")
+        except Exception as e:
+            logger.error(f"Failed to publish to MQTT: {e}")
+            logger.error(f"Exception type: {type(e)}")
 
 
 def main(args=None):
-    # Initialize ROS 2 Python client library
-    rclpy.init(args=args)
-
-    # Use argparse to handle command line arguments
-    parser = argparse.ArgumentParser(description="ROS to CoT Publisher Node")
-    parser.add_argument(
-        "--config", type=str, required=True, help="Path to the config YAML file."
+    # Initialize logger
+    logging.basicConfig(
+        level=logging.INFO,
+        format='%(asctime)s - %(levelname)s - %(name)s - %(message)s'
     )
+    logger = logging.getLogger("ROS2CASEVAC.main")
+    logger.info("Initializing ROS 2 Python client library")
 
-    # Parse the arguments
-    input_args = parser.parse_args()
-
-    # Load configuration
-    config = load_config(input_args.config)
-
-    # Create an instance of the ROS2COTPublisher node with the provided configuration
-    gps_cot_publisher = ROS2COTPublisher(config)
-
-    # Keep the node running to listen to incoming messages
-    rclpy.spin(gps_cot_publisher)
+    # Initialize ROS 2 Python client library
+    rclpy.init(args=args)
 
-    # Shutdown and cleanup
-    gps_cot_publisher.destroy_node()
-    rclpy.shutdown()
-    print("Node has shut down cleanly.")
+    try:
+        # Create an instance of the ROS2COTPublisher node
+        logger.info("Creating ROS2CASEVAC_AGENT node")
+        casevac_agent = ROS2COTPublisher()
+
+        # Keep the node running to listen to incoming messages
+        logger.info("Node initialized successfully, starting spin")
+        rclpy.spin(casevac_agent)
+    except Exception as e:
+        logger.critical(f"Fatal error occurred: {e}")
+    finally:
+        # Shutdown and cleanup
+        logger.info("Shutting down node")
+        if 'casevac_agent' in locals():
+            # Stop the MQTT client loop
+            if hasattr(casevac_agent, 'mqtt_client'):
+                casevac_agent.mqtt_client.loop_stop()
+                casevac_agent.mqtt_client.disconnect()
+            casevac_agent.destroy_node()
+        rclpy.shutdown()
+        logger.info("Node has shut down cleanly")
 
 
 if __name__ == "__main__":
-    main()
+    # Basic logger setup before config is loaded
+    logging.basicConfig(
+        level=logging.INFO,
+        format='%(asctime)s - %(levelname)s - %(name)s - %(message)s'
+    )
+    logger = logging.getLogger("ROS2CASEVAC.startup")
+    logger.info("Starting ROS 2 CASEVAC Agent")
+
+    main()
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py
index 591fafc5..6a46e93e 100755
--- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py
@@ -7,40 +7,42 @@
 
 This script acts as a ROS 2 node that subscribes to GPS data from multiple robots
 and converts that data into Cursor-On-Target (CoT) events. The CoT events are then sent 
-over a TCP socket to a designated server. The configuration for the host and port, as 
-well as the number of robots to subscribe to, is loaded from a YAML configuration file.
+over MQTT to a designated topic. The configuration for the MQTT connection,
+as well as the robot streaming configuration, is loaded from a YAML configuration file.
+
+The script now supports publishing GPS data at a specified frequency for each robot.
 
 Usage:
     1. Ensure you have Python 3.x installed with the necessary packages:
-       pip install rclpy sensor_msgs pytak pyyaml
+       pip install rclpy sensor_msgs paho-mqtt pytak pyyaml
 
     2. Create a YAML configuration file (e.g., config.yaml) with the following structure:
        project:
          name: test
-       robot:
-         count: 3
-         gps_topicname: '/iphone{n}/gps'  # Pattern for topic names
-       tak_server:
-         cot_url: {Enter the URL of the CoT server}
-         pytak_tls_client_cert: '/path/to/cert.pem'
-         pytak_tls_client_key: '/path/to/key.key'
+       logging:
+         level: 'INFO'  # Logging level for the services. Options: DEBUG, INFO, WARNING, ERROR, CRITICAL.
+       gps_streaming:
+         - name: 'drone1'
+           type: 'uav'
+           topicname: '/robot_1/interface/mavros/global_position/global'
+           frequency: 1  # Frequency in Hz
+         - name: 'drone2'
+           type: 'uav'
+           topicname: '/robot_2/interface/mavros/global_position/global'
+           frequency: 1  # Frequency in Hz
+       mqtt:
+         host: '127.0.0.1'
+         port: 1883
+         username: 'user'
+         password: 'pass'
        services:
          host: '127.0.0.1'
-         publisher:
-           tak_publisher:
-             port: 10000
          mediator:
            ros2cot_agent:
-             port: 10000
+             topic_name: 'ros2cot/events'
 
     3. Run the script with the following command, specifying the path to the config file:
-       python your_script.py --config config.yaml
-
-    4. The script will listen for incoming GPS messages and send CoT events to the 
-       configured server.
-
-Note:
-    Ensure the server receiving the CoT events is running and listening on the specified port.
+       ros2 run your_package your_script --ros-args -p config_file_path:=config.yaml -p creds_dir:=/path/to/creds
 """
 
 import rclpy
@@ -48,10 +50,15 @@
 from sensor_msgs.msg import NavSatFix
 import paho.mqtt.client as mqtt
 import pytak
-import xml.etree.ElementTree as ET
-import argparse
 import socket
 import yaml
+import logging
+import sys
+import time
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
+from threading import Lock
+from tak_helper.create_cot_msgs import create_gps_COT
+from tak_helper.logger import setup_logger
 
 
 def load_config(file_path):
@@ -60,154 +67,280 @@ def load_config(file_path):
         return yaml.safe_load(f)
 
 
-def create_COT_pos_event(uuid, latitude, longitude, altitude):
-    """Create a CoT event based on the GPS data."""
-    root = ET.Element("event")
-    root.set("version", "2.0")
-    root.set("type", "a-f-G")
-    root.set("uid", uuid)  # Use topic name as UID for identification
-    root.set("how", "m-g")
-    root.set("time", pytak.cot_time())
-    root.set("start", pytak.cot_time())
-    root.set("stale", pytak.cot_time(3600))
-
-    pt_attr = {
-        "lat": str(latitude),
-        "lon": str(longitude),
-        "hae": str(altitude),
-        "ce": "10",
-        "le": "10",
-    }
-
-    ET.SubElement(root, "point", attrib=pt_attr)
-
-    return ET.tostring(root, encoding="utf-8")
+class RobotGPSData:
+    """Class to store the latest GPS data for a robot."""
+
+    def __init__(self, robot_name, robot_type, frequency):
+        self.robot_name = robot_name
+        self.robot_type = robot_type
+        self.frequency = frequency
+        self.latitude = 0.0
+        self.longitude = 0.0
+        self.altitude = 0.0
+        self.last_update_time = 0.0
+        self.last_publish_time = 0.0
+        self.has_new_data = False
+        self.lock = Lock()  # For thread safety
+
+    def update_data(self, latitude, longitude, altitude):
+        """Update the GPS data for this robot."""
+        with self.lock:
+            self.latitude = latitude
+            self.longitude = longitude
+            self.altitude = altitude
+            self.last_update_time = time.time()
+            self.has_new_data = True
+
+    def should_publish(self):
+        """Check if it's time to publish data based on frequency."""
+        current_time = time.time()
+        # Only publish if:
+        # 1. We have new data since the last publish
+        # 2. The publish interval has elapsed (1/frequency seconds)
+        with self.lock:
+            if not self.has_new_data:
+                return False
+
+            time_since_last_publish = current_time - self.last_publish_time
+            should_publish = time_since_last_publish >= (1.0 / self.frequency)
+
+            if should_publish:
+                # Update the last publish time and mark data as published
+                self.last_publish_time = current_time
+                self.has_new_data = False
+
+            return should_publish
+
+    def get_data(self):
+        """Get the current GPS data."""
+        with self.lock:
+            return {
+                "latitude": self.latitude,
+                "longitude": self.longitude,
+                "altitude": self.altitude
+            }
 
 
 class ROS2COTPublisher(Node):
-    def __init__(self, config):
+    def __init__(self):
         super().__init__("ros2cot_publisher")
         self.subscribers = []
+        self.robot_data = {}  # Dictionary to store the latest data for each robot
+
+        # Read config from config filename from the ros2 parameters
+        self.declare_parameter("config_file_path", "")
+        self.config_filepath = self.get_parameter("config_file_path").get_parameter_value().string_value
+
+        # Initialize a basic logger before loading the config
+        self.logger = logging.getLogger("ROS2COT")
+
+        self.get_logger().info(f"Loading configuration from {self.config_filepath}")
+
+        # Load the configuration
+        try:
+            config = load_config(self.config_filepath)
+
+            # Setup logger based on config
+            log_level = config.get('logging', {}).get('level', 'INFO')
+            self.logger = setup_logger(self, log_level)
+            self.logger.info(f"Logger configured with level: {log_level}")
+
+        except Exception as e:
+            self.get_logger().error(f"Failed to load configuration: {e}")
+            sys.exit(1)
+
+        # Read the credentials dir from the ros2 parameters
+        self.declare_parameter("creds_dir", "")
+        self.creds_dir = self.get_parameter("creds_dir").get_parameter_value().string_value
+        self.logger.info(f"Loading credentials from {self.creds_dir}")
 
         # Get host and port from the config
         self.host = config["services"]["host"]
-        self.robots_count = config["robot"]["count"]
-        self.gps_topicname_pattern = config["robot"]["gps_topicname"]
         self.project_name = config["project"]["name"]
 
+        # Get GPS streaming configuration
+        self.gps_streaming = config.get("gps_streaming", [])
+
+        if not self.gps_streaming:
+            self.logger.warning("No GPS streaming configurations found in config file")
+
         # MQTT related configs
-        self.mqtt_broker = config["mqtt"]["host"]
-        self.mqtt_port = config["mqtt"]["port"]
-        self.mqtt_username = config["mqtt"]["username"]
-        self.mqtt_pwd = config["mqtt"]["password"]
-        self.mqtt_topicname = config["services"]["mediator"]["ros2cot_agent"][
-            "topic_name"
-        ]
+        try:
+            self.mqtt_broker = config["mqtt"]["host"]
+            self.mqtt_port = config["mqtt"]["port"]
+            self.mqtt_username = config["mqtt"]["username"]
+            self.mqtt_pwd = config["mqtt"]["password"]
+            self.mqtt_topicname = config["services"]["mediator"]["ros2cot_agent"]["topic_name"]
+
+            self.logger.info(
+                f"MQTT CONFIG: Broker={self.mqtt_broker}, Port={self.mqtt_port}, Topic={self.mqtt_topicname}")
+        except KeyError as e:
+            self.logger.error(f"Missing required MQTT configuration key: {e}")
+            sys.exit(1)
 
         # Setting MQTT
         self.mqtt_client = mqtt.Client()
         # Set the username and password
         self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd)
         try:
-            print(f"Trying to connect to {self.mqtt_broker}:{self.mqtt_port}")
+            self.logger.info(f"Attempting to connect to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
             self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port, keepalive=65535)
-            self.get_logger().info(
-                f"Connected to MQTT ({self.mqtt_broker}:{self.mqtt_port})"
-            )
+            self.mqtt_client.loop_start()  # Start MQTT loop in background thread
+            self.logger.info(f"Connected to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
         except Exception as e:
-            print(f"Failed to connect or publish: {e}")
+            self.logger.error(f"Failed to connect to MQTT broker: {e}")
+            self.logger.error(f"Exception type: {type(e)}")
 
-        self.get_logger().info(
-            f"Starting ROS2COTPublisher for project: {self.project_name}"
-        )
-        self.get_logger().info(
-            f"Subscribing to {self.robots_count} robot(s) on topics matching: {self.gps_topicname_pattern}"
+        self.logger.info(f"Starting ROS2COTPublisher for project: {self.project_name}")
+
+        # Create a QoS profile that matches the publisher
+        self.qos_profile = QoSProfile(
+            reliability=ReliabilityPolicy.BEST_EFFORT,  # Match the publisher's BEST_EFFORT
+            durability=DurabilityPolicy.VOLATILE,  # Match the publisher's VOLATILE
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
         )
 
-        # Subscribe to GPS topics based on the pattern
-        for i in range(1, self.robots_count + 1):
-            topic_name = self.gps_topicname_pattern.format(n=i)
+        # Subscribe to GPS topics based on the configuration
+        for robot_config in self.gps_streaming:
+            robot_name = robot_config.get("name")
+            robot_type = robot_config.get("type")
+            topic_name = robot_config.get("topicname")
+            frequency = robot_config.get("frequency", 1.0)  # Default to 1Hz if not specified
+
+            if not robot_name or not topic_name:
+                self.logger.warning(f"Skipping invalid robot config: {robot_config}")
+                continue
+
+            # Create a data structure to hold GPS data for this robot
+            self.robot_data[robot_name] = RobotGPSData(robot_name, robot_type, frequency)
+
             subscriber = self.create_subscription(
                 NavSatFix,
                 topic_name,
-                lambda msg, topic_name=topic_name: self.gps_callback(msg, f"/robot{i}"),
-                10,  # QoS depth
+                lambda msg, name=robot_name: self.gps_callback(msg, name),
+                self.qos_profile
             )
             self.subscribers.append(subscriber)
-            self.get_logger().info(f"Subscribed to GPS topic: {topic_name}")
+            self.logger.info(f"Subscribed to GPS topic for {robot_name}: {topic_name}, publishing at {frequency} Hz")
+
+        # Create a timer to check and publish data at regular intervals
+        # Use the shortest interval possible (0.01 seconds) to check all robots
+        self.publisher_timer = self.create_timer(0.01, self.publish_timer_callback)
 
-    def gps_callback(self, msg, topic_name):
+    def gps_callback(self, msg, robot_name):
         """Callback for processing GPS data."""
+        logger = logging.getLogger(f"ROS2COT.{robot_name}")
+
         latitude = msg.latitude
         longitude = msg.longitude
         altitude = msg.altitude
 
         # Log the received GPS data
-        self.get_logger().info(
-            f"Received GPS from {topic_name}: Lat {latitude}, Lon {longitude}, Alt {altitude}"
-        )
-
-        # Create a CoT event from the GPS data
-        cot_event = create_COT_pos_event(
-            f"{self.project_name}{topic_name}", latitude, longitude, altitude
+        logger.debug(
+            f"Received GPS data: Lat {latitude:.6f}, Lon {longitude:.6f}, Alt {altitude:.2f}"
         )
 
-        # Send the CoT event over MQTT
-        self.send_cot_event_over_mqtt(cot_event)
-
-    def send_cot_event_over_mqtt(self, cot_event):
+        # Update the stored data for this robot
+        if robot_name in self.robot_data:
+            self.robot_data[robot_name].update_data(latitude, longitude, altitude)
+        else:
+            logger.warning(f"Received data for unknown robot: {robot_name}")
+
+    def publish_timer_callback(self):
+        """Timer callback to check and publish data for all robots based on their frequency."""
+        for robot_name, robot_data in self.robot_data.items():
+            if robot_data.should_publish():
+                # Get the current data
+                data = robot_data.get_data()
+
+                # Create a CoT event
+                cot_event = create_gps_COT(
+                    f"{self.project_name}_{robot_name}",
+                    data["latitude"],
+                    data["longitude"],
+                    data["altitude"],
+                    "COT_Event",
+                    robot_data.robot_type
+                )
+
+                # Send the CoT event over MQTT
+                self.send_cot_event_over_mqtt(cot_event, robot_name)
+
+    def send_cot_event_over_mqtt(self, cot_event, robot_name=None):
         """Send CoT event over the MQTT network"""
+        log_prefix = f"ROS2COT.MQTT.{robot_name}" if robot_name else "ROS2COT.MQTT"
+        logger = logging.getLogger(log_prefix)
+
         try:
             self.mqtt_client.publish(self.mqtt_topicname, cot_event)
-            self.get_logger().info(
-                f"Message '{cot_event}' published to topic '{self.mqtt_topicname}'"
-            )
-        except:
-            self.get_logger().error(f"Failed to publish.")
+            logger.debug(f"CoT event published to topic '{self.mqtt_topicname}'")
+        except Exception as e:
+            logger.error(f"Failed to publish to MQTT: {e}")
+            logger.error(f"Exception type: {type(e)}")
 
-    def send_cot_event_over_network(self, cot_event):
+    def send_cot_event_over_network(self, cot_event, host=None, port=None):
         """Send CoT event over a TCP socket to the configured host."""
+        logger = logging.getLogger("ROS2COT.TCP")
+
+        # Use provided host/port or fall back to class attributes
+        host = host or self.host
+        port = port or getattr(self, 'port', None)
+
+        if not port:
+            logger.error("No port configured for TCP connection")
+            return
+
         try:
             with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
-                s.bind((self.host, self.port))
-                s.connect((self.host, self.port))
+                s.connect((host, port))
                 s.sendall(cot_event)
-                self.get_logger().info(f"Sent CoT event to {self.host}:{self.port}")
+                logger.info(f"Sent CoT event to {host}:{port}")
         except ConnectionRefusedError:
-            self.get_logger().error(
-                f"Connection to {self.host}:{self.port} refused. Ensure the server is running."
-            )
+            logger.error(f"Connection to {host}:{port} refused. Ensure the server is running.")
         except Exception as e:
-            self.get_logger().error(f"Failed to send CoT event: {e}")
+            logger.error(f"Failed to send CoT event via TCP: {e}")
+            logger.error(f"Exception type: {type(e)}")
 
 
 def main(args=None):
+    # Initialize logger
+    logger = logging.getLogger("ROS2COT.main")
+    logger.info("Initializing ROS 2 Python client library")
+
     # Initialize ROS 2 Python client library
     rclpy.init(args=args)
 
-    # Use argparse to handle command line arguments
-    parser = argparse.ArgumentParser(description="ROS to CoT Publisher Node")
-    parser.add_argument(
-        "--config", type=str, required=True, help="Path to the config YAML file."
-    )
-
-    # Parse the arguments
-    input_args = parser.parse_args()
-
-    # Load configuration
-    config = load_config(input_args.config)
-
-    # Create an instance of the ROS2COTPublisher node with the provided configuration
-    gps_cot_publisher = ROS2COTPublisher(config)
-
-    # Keep the node running to listen to incoming messages
-    rclpy.spin(gps_cot_publisher)
-
-    # Shutdown and cleanup
-    gps_cot_publisher.destroy_node()
-    rclpy.shutdown()
-    print("Node has shut down cleanly.")
+    try:
+        # Create an instance of the ROS2COTPublisher node
+        logger.info("Creating ROS2COTPublisher node")
+        gps_cot_publisher = ROS2COTPublisher()
+
+        # Keep the node running to listen to incoming messages
+        logger.info("Node initialized successfully, starting spin")
+        rclpy.spin(gps_cot_publisher)
+    except Exception as e:
+        logger.critical(f"Fatal error occurred: {e}")
+    finally:
+        # Shutdown and cleanup
+        logger.info("Shutting down node")
+        if 'gps_cot_publisher' in locals():
+            # Stop the MQTT client loop
+            if hasattr(gps_cot_publisher, 'mqtt_client'):
+                gps_cot_publisher.mqtt_client.loop_stop()
+                gps_cot_publisher.mqtt_client.disconnect()
+            gps_cot_publisher.destroy_node()
+        rclpy.shutdown()
+        logger.info("Node has shut down cleanly")
 
 
 if __name__ == "__main__":
-    main()
+    # Basic logger setup before config is loaded
+    logging.basicConfig(
+        level=logging.INFO,
+        format='%(asctime)s - %(levelname)s - %(name)s - %(message)s'
+    )
+    logger = logging.getLogger("ROS2COT.startup")
+    logger.info("Starting ROS 2 GPS to CoT Publisher")
+
+    main()
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py
index 8550d4d7..9463ecf9 100755
--- a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py
@@ -1,36 +1,68 @@
 #!/usr/bin/env python3
 
 """
-TAK Publisher Script
+TAK Publisher Script with Enhanced Logging
 
 Author: Aditya Rauniyar (rauniyar@cmu.edu)
-
-This script sends Cursor-On-Target (CoT) events to a TAK server by reading COT messages
-from a specified localhost TCP socket. The configuration for the TAK server and other
-parameters is loaded from a YAML file provided as a command-line argument.
-
-Usage:
-    1. Ensure you have Python 3.x installed with the necessary packages:
-       pip install asyncio pytak pyyaml
-
-    2. Create a YAML configuration file (e.g., config.yaml) with the necessary parameters.
-
-    3. Run the script with the following command:
-       python your_script.py --config path/to/config.yaml
 """
 
 import asyncio
-import xml.etree.ElementTree as ET
-import pytak
-import socket
+from pathlib import Path
+import sys
+import logging
+import logging.handlers
 import multiprocessing
 import argparse
 import yaml
 from configparser import ConfigParser
 import paho.mqtt.client as mqtt
+import pytak
+
+
+def setup_logger(log_level):
+    """
+    Set up the logger with appropriate log level and formatting.
+
+    Args:
+        log_level: The log level from config (DEBUG, INFO, WARNING, ERROR, CRITICAL)
+
+    Returns:
+        Configured logger object
+    """
+    # Convert string log level to logging constants
+    level_map = {
+        'DEBUG': logging.DEBUG,
+        'INFO': logging.INFO,
+        'WARNING': logging.WARNING,
+        'ERROR': logging.ERROR,
+        'CRITICAL': logging.CRITICAL
+    }
+
+    # Default to INFO if level not recognized
+    numeric_level = level_map.get(log_level, logging.INFO)
+
+    # Configure root logger
+    logger = logging.getLogger()
+    logger.setLevel(numeric_level)
+
+    # Console handler with improved formatting
+    console = logging.StreamHandler()
+    console.setLevel(numeric_level)
+
+    # Format: timestamp - level - component - message
+    formatter = logging.Formatter('%(asctime)s - %(levelname)s - %(name)s - %(message)s')
+    console.setFormatter(formatter)
+
+    # Add handler to logger
+    logger.addHandler(console)
+
+    return logger
+
 
 class MySender(pytak.QueueWorker):
     def __init__(self, tx_queue, config, event_loop):
+        self.logger = logging.getLogger("MySender")
+        self.logger.debug("Initializing MySender class")
         super().__init__(tx_queue, config["mycottool"])
 
         # MQTT parameters
@@ -39,87 +71,184 @@ def __init__(self, tx_queue, config, event_loop):
         self.mqtt_username = config['mqtt']['username']
         self.mqtt_pwd = config['mqtt']['password']
         self.mqtt_topicname = config['mqtt']['topicname']
-        
+
         # Capture the main event loop
         self.event_loop = event_loop
 
-        # Set up MQTT client
-        self.mqtt_client = mqtt.Client()
-        self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd)
-        self.mqtt_client.on_message = self._on_message_sync  # Use sync wrapper
+        # Log MQTT connection details
+        self.logger.info(
+            f"MQTT config - Broker: {self.mqtt_broker}, Port: {self.mqtt_port}, Topic: {self.mqtt_topicname}")
 
-        # Connect to MQTT broker and subscribe to topic
+        # Set up MQTT client with explicit callbacks
+        self.logger.debug("Creating MQTT client")
+        self.mqtt_client = mqtt.Client(client_id="tak_publisher_client", protocol=mqtt.MQTTv311)
+
+        # Set callbacks
+        self.logger.debug("Setting up MQTT callbacks")
+        self.mqtt_client.on_connect = self._on_connect
+        self.mqtt_client.on_disconnect = self._on_disconnect
+        self.mqtt_client.on_message = self._on_message_sync
+
+        # Set credentials if provided
+        if self.mqtt_username:
+            self.logger.debug(f"Setting up MQTT credentials for user: {self.mqtt_username}")
+            self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd)
+
+        # Connect to MQTT broker
+        self.logger.info(f"Attempting to connect to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
         try:
-            print(f"Connecting to {self.mqtt_broker}:{self.mqtt_port}")
-            self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port, keepalive=65535)
-            self.mqtt_client.subscribe(self.mqtt_topicname)
-            print(f"Connected and subscribed to MQTT topic '{self.mqtt_topicname}' on broker {self.mqtt_broker}:{self.mqtt_port}")
+            self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port, keepalive=60)
+            self.logger.debug("MQTT connect() method completed without exception")
         except Exception as e:
-            print(f"Failed to connect or subscribe to MQTT: {e}")
+            self.logger.error(f"MQTT Connection failed with exception: {e}")
+            self.logger.error(f"Exception type: {type(e)}")
+
+        self.logger.debug("MySender initialization completed")
+
+    def _on_connect(self, client, userdata, flags, rc):
+        """Callback for when the client connects to the broker"""
+        connection_responses = {
+            0: "Connection successful",
+            1: "Connection refused - incorrect protocol version",
+            2: "Connection refused - invalid client identifier",
+            3: "Connection refused - server unavailable",
+            4: "Connection refused - bad username or password",
+            5: "Connection refused - not authorized"
+        }
+
+        status = connection_responses.get(rc, f"Unknown error code: {rc}")
+        self.logger.info(f"MQTT CONNECTION STATUS: {status} (code={rc})")
+
+        if rc == 0:
+            self.logger.info(f"Connected to MQTT broker at {self.mqtt_broker}:{self.mqtt_port}")
+            # Subscribe to topic
+            result, mid = self.mqtt_client.subscribe(self.mqtt_topicname)
+            self.logger.debug(f"MQTT Subscribe result: {result}, Message ID: {mid}")
+            if result == 0:
+                self.logger.info(f"Subscribed to MQTT topic: {self.mqtt_topicname}")
+            else:
+                self.logger.error(f"Failed to subscribe to MQTT topic: {self.mqtt_topicname}")
+        else:
+            self.logger.error(f"MQTT connection failed: {status}")
+
+    def _on_disconnect(self, client, userdata, rc):
+        """Callback for when the client disconnects from the broker"""
+        if rc == 0:
+            self.logger.info("Disconnected from broker cleanly")
+        else:
+            self.logger.warning(f"Unexpected disconnect with code {rc}")
 
     def start_mqtt_loop(self):
         """Start MQTT loop in a separate thread."""
+        self.logger.debug("Starting MQTT client loop")
         self.mqtt_client.loop_start()
+        self.logger.debug("MQTT loop started")
 
     def _on_message_sync(self, client, userdata, message):
         """Synchronous wrapper for MQTT on_message to run handle_data in the main event loop."""
+        self.logger.debug(f"Message received on topic: {message.topic}")
         asyncio.run_coroutine_threadsafe(self.handle_data(client, userdata, message), self.event_loop)
 
     async def handle_data(self, client, userdata, message):
         """Handle incoming MQTT data and put it on the async queue."""
         event = message.payload
         await self.put_queue(event)
-        print(f"Received message on '{self.mqtt_topicname}'")
+        self.logger.debug(f"Processed message from topic '{message.topic}' and queued for transmission")
 
     async def run(self, number_of_iterations=-1):
+        self.logger.debug("MySender.run() method started")
         self.start_mqtt_loop()
+        self.logger.debug("MQTT loop started, now waiting in run loop")
         try:
             while True:
-                await asyncio.sleep(0.5)  # Keep the loop running
+                await asyncio.sleep(10)  # Keep the loop running, check every 10 seconds
+                self.logger.debug("Still running in the MySender.run() loop")
         except asyncio.CancelledError:
+            self.logger.debug("CancelledError caught, stopping MQTT loop")
             self.mqtt_client.loop_stop()
-            print("MQTT loop stopped.")
+            self.logger.debug("MQTT loop stopped")
+        except Exception as e:
+            self.logger.error(f"Unexpected exception in MySender.run(): {e}")
+            raise
+
 
 async def main(config):
+    logger = logging.getLogger("main")
+    logger.debug("main() function started")
     loop = asyncio.get_running_loop()  # Capture the main event loop
     clitool = pytak.CLITool(config["mycottool"])
     await clitool.setup()
+    logger.debug("Adding MySender task to CLITool")
     clitool.add_task(MySender(clitool.tx_queue, config, loop))  # Pass the loop
+    logger.debug("Running CLITool")
     await clitool.run()
 
+
 def run_main_in_process(config):
+    logger = logging.getLogger("process")
+    logger.debug("run_main_in_process() started")
     loop = asyncio.get_event_loop()
+    logger.debug("Event loop created, running main()")
     loop.run_until_complete(main(config))
 
+
 if __name__ == "__main__":
+    logger = logging.getLogger("startup")
+    logger.info("Script main block executing")
+
     parser = argparse.ArgumentParser(description="TAK Publisher Script")
-    parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.')
+    parser.add_argument('--config_file_path', type=str, required=True, help='Path to the config YAML file.')
+    parser.add_argument('--creds_path', type=str, required=True, help='Path to the creds directory.')
+
     args = parser.parse_args()
+    logger.info(f"Args parsed - config_file_path: {args.config_file_path}, creds_path: {args.creds_path}")
 
     # Load the YAML configuration
-    with open(args.config, 'r') as file:
-        print(f"Loading configuration from {args.config}")
-        config_data = yaml.safe_load(file)
+    try:
+        with open(args.config_file_path, 'r') as file:
+            logger.info(f"Loading configuration from {args.config_file_path}")
+            config_data = yaml.safe_load(file)
+            logger.info("Configuration loaded successfully")
+
+            # Setup logger based on config
+            log_level = config_data.get('logging', {}).get('level', 'INFO')
+            logger = setup_logger(log_level)
+            logger.info(f"Logger configured with level: {log_level}")
+
+    except Exception as e:
+        logger.error(f"Failed to load configuration: {e}")
+        sys.exit(1)
 
     # Extract necessary parameters
-    cot_url = config_data['tak_server']['cot_url']
-    pytak_tls_client_cert = config_data['tak_server']['pytak_tls_client_cert']
-    pytak_tls_client_key = config_data['tak_server']['pytak_tls_client_key']
-    host = config_data['services']['host']
-
-    # MQTT params
-    mqtt_broker = config_data['mqtt']['host']
-    mqtt_port = config_data['mqtt']['port']
-    mqtt_username = config_data['mqtt']['username']
-    mqtt_pwd = config_data['mqtt']['password']
-    mqtt_topicname = config_data['services']['publisher']['tak_publisher']['topic_name']
+    try:
+        cot_url = config_data['tak_server']['cot_url']
+        pytak_tls_client_cert = config_data['tak_server']['pytak_tls_client_cert']
+        # Add the creds_path to the pytak_tls_client_cert
+        pytak_tls_client_cert = Path(args.creds_path) / pytak_tls_client_cert
+        pytak_tls_client_key = config_data['tak_server']['pytak_tls_client_key']
+        # Add the creds_path to the pytak_tls_client_key
+        pytak_tls_client_key = Path(args.creds_path) / pytak_tls_client_key
+
+        host = config_data['services']['host']
+
+        # MQTT params
+        mqtt_broker = config_data['mqtt']['host']
+        mqtt_port = config_data['mqtt']['port']
+        mqtt_username = config_data['mqtt']['username']
+        mqtt_pwd = config_data['mqtt']['password']
+        mqtt_topicname = config_data['services']['publisher']['tak_publisher']['topic_name']
+
+        logger.info(f"MQTT CONFIG: Broker={mqtt_broker}, Port={mqtt_port}, Topic={mqtt_topicname}")
+    except KeyError as e:
+        logger.error(f"Missing required configuration key: {e}")
+        sys.exit(1)
 
     # Setup config for pytak
     config = ConfigParser()
     config["mycottool"] = {
         "COT_URL": cot_url,
-        "PYTAK_TLS_CLIENT_CERT": pytak_tls_client_cert,
-        "PYTAK_TLS_CLIENT_KEY": pytak_tls_client_key,
+        "PYTAK_TLS_CLIENT_CERT": str(pytak_tls_client_cert),
+        "PYTAK_TLS_CLIENT_KEY": str(pytak_tls_client_key),
         "PYTAK_TLS_CLIENT_PASSWORD": "atakatak",
         "PYTAK_TLS_DONT_VERIFY": "1"
     }
@@ -129,15 +258,17 @@ def run_main_in_process(config):
 
     config["mqtt"] = {
         "host": mqtt_broker,
-        "port": mqtt_port,
+        "port": str(mqtt_port),  # Convert to string
         "username": mqtt_username,
-        "password": mqtt_pwd,
+        "password": mqtt_pwd or "",  # Handle empty password
         "topicname": mqtt_topicname
     }
 
     # Start the asyncio event loop in a separate process
+    logger.info("Starting TAK publisher process")
     process = multiprocessing.Process(target=run_main_in_process, args=(config,))
     process.start()
 
-    print("Main() is now running in a separate process.")
-    process.join()
\ No newline at end of file
+    logger.info("Main() is now running in a separate process")
+    process.join()
+    logger.info("Process completed")
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py
index 6f398f49..10eb4dfd 100644
--- a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py
@@ -24,11 +24,12 @@
 import xml.etree.ElementTree as ET
 import pytak
 import argparse
-import socket
 import yaml
 from configparser import ConfigParser
 import logging
 import paho.mqtt.client as mqtt
+import sys
+from pathlib import Path
 
 # Log levels: DEBUG, INFO, WARNING, ERROR, CRITICAL
 LOG_LEVEL = "DEBUG"
@@ -144,35 +145,54 @@ async def run(self):  # pylint: disable=arguments-differ
 
 async def main():
     parser = argparse.ArgumentParser(description="TAK Subscriber Script")
-    parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.')
+    parser.add_argument('--config_file_path', type=str, required=True, help='Path to the config YAML file.')
+    parser.add_argument('--creds_path', type=str, required=True, help='Path to the creds directory.')
+
     args = parser.parse_args()
+    print(f"STARTUP: Args parsed - config_file_path: {args.config_file_path}, creds_path: {args.creds_path}",
+          flush=True)
 
     # Load the YAML configuration
-    with open(args.config, 'r') as file:
-        config_data = yaml.safe_load(file)
+    try:
+        with open(args.config_file_path, 'r') as file:
+            print(f"STARTUP: Loading configuration from {args.config_file_path}", flush=True)
+            config_data = yaml.safe_load(file)
+            print("STARTUP: Configuration loaded successfully", flush=True)
+    except Exception as e:
+        print(f"ERROR: Failed to load configuration: {e}", flush=True)
+        sys.exit(1)
 
     # Extract necessary parameters from the configuration
-    cot_url = config_data['tak_server']['cot_url']
-    pytak_tls_client_cert = config_data['tak_server']['pytak_tls_client_cert']
-    pytak_tls_client_key = config_data['tak_server']['pytak_tls_client_key']
-    host = config_data['services']['host']
-    filter_messages = config_data['services']['subscriber']['tak_subscriber']['filter_messages']
-    # Extract the filter name and corresponding mqtt topic_name
-    message_name2topic = [{"name": msg['name'], "mqtt_topic": msg["mqtt_topic_name"]} for msg in filter_messages]
-
-    # MQTT params
-    mqtt_broker = config_data['mqtt']['host']
-    # mqtt_broker = "localhost" # Uncomment if running from the host
-    mqtt_port = config_data['mqtt']['port']
-    mqtt_username = config_data['mqtt']['username']
-    mqtt_pwd = config_data['mqtt']['password']
+    try:
+        cot_url = config_data['tak_server']['cot_url']
+        pytak_tls_client_cert = config_data['tak_server']['pytak_tls_client_cert']
+        # Add the creds_path to the pytak_tls_client_cert
+        pytak_tls_client_cert = Path(args.creds_path) / pytak_tls_client_cert
+        pytak_tls_client_key = config_data['tak_server']['pytak_tls_client_key']
+        # Add the creds_path to the pytak_tls_client_key
+        pytak_tls_client_key = Path(args.creds_path) / pytak_tls_client_key
+
+        host = config_data['services']['host']
+        filter_messages = config_data['services']['subscriber']['tak_subscriber']['filter_messages']
+        # Extract the filter name and corresponding mqtt topic_name
+        message_name2topic = [{"name": msg['name'], "mqtt_topic": msg["mqtt_topic_name"]} for msg in filter_messages]
+
+        # MQTT params
+        mqtt_broker = config_data['mqtt']['host']
+        # mqtt_broker = "localhost" # Uncomment if running from the host
+        mqtt_port = config_data['mqtt']['port']
+        mqtt_username = config_data['mqtt']['username']
+        mqtt_pwd = config_data['mqtt']['password']
+    except KeyError as e:
+        print(f"ERROR: Missing parameter in configuration: {e}", flush=True)
+        sys.exit(1)
 
     # Setup config for pytak
     config = ConfigParser()
     config["mycottool"] = {
         "COT_URL": cot_url,
-        "PYTAK_TLS_CLIENT_CERT": pytak_tls_client_cert,
-        "PYTAK_TLS_CLIENT_KEY": pytak_tls_client_key,
+        "PYTAK_TLS_CLIENT_CERT": str(pytak_tls_client_cert),
+        "PYTAK_TLS_CLIENT_KEY": str(pytak_tls_client_key),
         "PYTAK_TLS_CLIENT_PASSWORD": "atakatak",
         "PYTAK_TLS_DONT_VERIFY": "1"
     }
@@ -183,9 +203,9 @@ async def main():
 
     config["mqtt"] = {
         "host": mqtt_broker,
-        "port": mqtt_port,
+        "port": str(mqtt_port),
         "username": mqtt_username,
-        "password": mqtt_pwd
+        "password": mqtt_pwd or ""
     }
 
     # Initialize worker queues and tasks.
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg b/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg
new file mode 100644
index 00000000..4b2e509b
--- /dev/null
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/ros2tak_tools
+[install]
+install-scripts=$base/lib/ros2tak_tools
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/setup.py b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py
index 7aeb110a..2d79406f 100644
--- a/ground_control_station/ros_ws/src/ros2tak_tools/setup.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py
@@ -1,22 +1,35 @@
 from setuptools import find_packages, setup
+import os
+from glob import glob
 
 package_name = 'ros2tak_tools'
 
 setup(
     name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version='1.0.0',
+    packages=find_packages(exclude=['test']) + ['tak_helper'],
     data_files=[
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        # Include all files in config directory
+        ('share/' + package_name + '/config', glob('config/*.*')),
+        # Include all files in scripts directory
+        ('share/' + package_name + '/scripts', glob('scripts/*.*')),
+        # Create lib directory for executables
+        ('lib/' + package_name, []),
+    ] + [
+        # This will recursively include all files in creds directory and its subdirectories
+        (os.path.join('share', package_name, os.path.dirname(p)), [p])
+        for p in glob('creds/**/*', recursive=True)
+        if os.path.isfile(p)
     ],
     install_requires=['setuptools'],
     zip_safe=True,
-    maintainer='mission-operator',
-    maintainer_email='mission-operator@todo.todo',
+    maintainer='Adi',
+    maintainer_email='rauniyar@cmu.edu',
     description='TODO: Package description',
-    license='TODO: License declaration',
+    license='BSD-3',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
@@ -27,4 +40,4 @@
             'chat2ros_agent = ros2tak_tools.chat2ros_agent:main',
         ],
     },
-)
+)
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Casualty.py b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/Casualty.py
old mode 100644
new mode 100755
similarity index 62%
rename from ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Casualty.py
rename to ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/Casualty.py
index 13fd51cc..a5514aa4
--- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Casualty.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/Casualty.py
@@ -41,6 +41,8 @@ class AlertnessLevel(Enum):
 class VitalType(Enum):
     HEART_RATE = Vitals.HEART_RATE
     RESPIRATORY_RATE = Vitals.RESPIRATORY_RATE
+    TEMPERATURE = Vitals.TEMPERATURE
+
 
 class ConditionType(Enum):
     SEVERE_HEMORRHAGE = Critical.SEVERE_HEMORRHAGE
@@ -245,6 +247,7 @@ def __init__(self, casualty_id:int):
 
         self.heart_rate = VitalsCOT("Heart Rate")
         self.respiratory_rate = VitalsCOT("Respiratory Rate")
+        self.temperature = VitalsCOT("Temperature")
 
         self.trauma_head = InjuryCOT("Trauma Head")
         self.trauma_torso = InjuryCOT("Trauma Torso")
@@ -270,30 +273,31 @@ def get_mechanism(self):
 
     def get_injuries(self):
         """Returns the injuries sustained for preset status"""
-
+        
         injuries = []
         if self.trauma_head.status:
-            injuries.append(self.trauma_head)
+            injuries.append(repr(self.trauma_head))
         if self.trauma_torso.status:
-            injuries.append(self.trauma_torso)
+            injuries.append(repr(self.trauma_torso))
         if self.trauma_lower_ext.status:
-            injuries.append(self.trauma_lower_ext)
+            injuries.append(repr(self.trauma_lower_ext))
         if self.trauma_upper_ext.status:
-            injuries.append(self.trauma_upper_ext)
+            injuries.append(repr(self.trauma_upper_ext))
         return ", ".join(injuries)
 
+
     def get_signs_symptoms(self):
         """Returns the signs and symptoms for preset status"""
 
         signs = []
         if self.severe_hemorrhage.status:
-            signs.append(self.severe_hemorrhage)
+            signs.append(repr(self.severe_hemorrhage))
         if self.respiratory_distress.status:
-            signs.append(self.respiratory_distress)
+            signs.append(repr(self.respiratory_distress))
         if self.heart_rate.status:
-            signs.append(self.heart_rate)
+            signs.append(repr(self.heart_rate))
         if self.respiratory_rate.status:
-            signs.append(self.respiratory_rate)
+            signs.append(repr(self.respiratory_rate))
         return ", ".join(signs)
 
     def get_treatments(self):
@@ -301,97 +305,165 @@ def get_treatments(self):
 
     def update_casualty_metadata(self, msg: CasualtyMeta):
         """Updates the casualty metadata with the message data."""
-
         # Update GPS coordinates
-        if msg.valid_gps:
-            self.gps.set_gps(msg.gps.latitude, msg.gps.longitude, msg.gps.altitude)
+        if msg.gps:  # Check if the array is not empty
+            try:
+                for gps_data in msg.gps:
+                    self.gps.set_gps(gps_data.latitude, gps_data.longitude, gps_data.altitude)
+            except Exception as e:
+                print(f"Error updating GPS data: {e}")
 
         # Update critical conditions
-        if msg.valid_severe_hemorrhage:
-            self.severe_hemorrhage.set_status(
-                system="Circulatory",
-                type_=ConditionType.SEVERE_HEMORRHAGE,
-                value=ConditionStatus(msg.severe_hemorrhage.value),
-                confidence=msg.severe_hemorrhage.confidence
-            )
-        if msg.valid_respiratory_distress:
-            self.respiratory_distress.set_status(
-                system="Respiratory",
-                type_=ConditionType.RESPIRATORY_DISTRESS,
-                value=ConditionStatus(msg.respiratory_distress.value),
-                confidence=msg.respiratory_distress.confidence
-            )
+        if msg.severe_hemorrhage:  # Check if the array is not empty
+            try:
+                for hemorrhage_data in msg.severe_hemorrhage:
+                    self.severe_hemorrhage.set_status(
+                        system="Circulatory",
+                        type_=ConditionType.SEVERE_HEMORRHAGE,
+                        value=ConditionStatus(hemorrhage_data.value),
+                        confidence=hemorrhage_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating severe hemorrhage data: {e}")
+
+        if msg.respiratory_distress:  # Check if the array is not empty
+            try:
+                for distress_data in msg.respiratory_distress:
+                    self.respiratory_distress.set_status(
+                        system="Respiratory",
+                        type_=ConditionType.RESPIRATORY_DISTRESS,
+                        value=ConditionStatus(distress_data.value),
+                        confidence=distress_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating respiratory distress data: {e}")
 
         # Update vitals
-        if msg.valid_heart_rate:
-            self.heart_rate.set_vitals(
-                system="Cardiovascular",
-                type_=VitalType.HEART_RATE,
-                value=msg.heart_rate.value,
-                time_ago=msg.heart_rate.time_ago,
-                confidence=msg.heart_rate.confidence
-            )
-        if msg.valid_respiratory_rate:
-            self.respiratory_rate.set_vitals(
-                system="Respiratory",
-                type_=VitalType.RESPIRATORY_RATE,
-                value=msg.respiratory_rate.value,
-                time_ago=msg.respiratory_rate.time_ago,
-                confidence=msg.respiratory_rate.confidence
-            )
+        if msg.heart_rate:  # Check if the array is not empty
+            try:
+                for heart_rate_data in msg.heart_rate:
+                    self.heart_rate.set_vitals(
+                        system="Cardiovascular",
+                        type_=VitalType.HEART_RATE,
+                        value=heart_rate_data.value,
+                        time_ago=heart_rate_data.time_ago,
+                        confidence=heart_rate_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating heart rate data: {e}")
+
+        if msg.respiratory_rate:  # Check if the array is not empty
+            try:
+                for resp_rate_data in msg.respiratory_rate:
+                    self.respiratory_rate.set_vitals(
+                        system="Respiratory",
+                        type_=VitalType.RESPIRATORY_RATE,
+                        value=resp_rate_data.value,
+                        time_ago=resp_rate_data.time_ago,
+                        confidence=resp_rate_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating respiratory rate data: {e}")
+
+        if msg.temperature:  # Check if the array is not empty
+            try:
+                for temp_data in msg.temperature:
+                    self.temperature.set_vitals(
+                        system="Body",
+                        type_=VitalType.TEMPERATURE,
+                        value=temp_data.value,
+                        time_ago=temp_data.time_ago,
+                        confidence=temp_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating temperature data: {e}")
 
         # Update injuries
-        if msg.valid_trauma_head:
-            self.trauma_head.set_status(
-                system="Head",
-                type_=TraumaType.TRAUMA_HEAD,
-                value=TraumaSeverity(msg.trauma_head.value),
-                confidence=msg.trauma_head.confidence
-            )
-        if msg.valid_trauma_torso:
-            self.trauma_torso.set_status(
-                system="Torso",
-                type_=TraumaType.TRAUMA_TORSO,
-                value=TraumaSeverity(msg.trauma_torso.value),
-                confidence=msg.trauma_torso.confidence
-            )
-        if msg.valid_trauma_lower_ext:
-            self.trauma_lower_ext.set_status(
-                system="Lower Extremity",
-                type_=TraumaType.TRAUMA_LOWER_EXT,
-                value=TraumaSeverity(msg.trauma_lower_ext.value),
-                confidence=msg.trauma_lower_ext.confidence
-            )
-        if msg.valid_trauma_upper_ext:
-            self.trauma_upper_ext.set_status(
-                system="Upper Extremity",
-                type_=TraumaType.TRAUMA_UPPER_EXT,
-                value=TraumaSeverity(msg.trauma_upper_ext.value),
-                confidence=msg.trauma_upper_ext.confidence
-            )
+        if msg.trauma_head:  # Check if the array is not empty
+            try:
+                for trauma_data in msg.trauma_head:
+                    self.trauma_head.set_status(
+                        system="Head",
+                        type_=TraumaType.TRAUMA_HEAD,
+                        value=TraumaSeverity(trauma_data.value),
+                        confidence=trauma_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating trauma head data: {e}")
+
+        if msg.trauma_torso:  # Check if the array is not empty
+            try:
+                for trauma_data in msg.trauma_torso:
+                    self.trauma_torso.set_status(
+                        system="Torso",
+                        type_=TraumaType.TRAUMA_TORSO,
+                        value=TraumaSeverity(trauma_data.value),
+                        confidence=trauma_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating trauma torso data: {e}")
+
+        if msg.trauma_lower_ext:  # Check if the array is not empty
+            try:
+                for trauma_data in msg.trauma_lower_ext:
+                    self.trauma_lower_ext.set_status(
+                        system="Lower Extremity",
+                        type_=TraumaType.TRAUMA_LOWER_EXT,
+                        value=TraumaSeverity(trauma_data.value),
+                        confidence=trauma_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating trauma lower extremity data: {e}")
+
+        if msg.trauma_upper_ext:  # Check if the array is not empty
+            try:
+                for trauma_data in msg.trauma_upper_ext:
+                    self.trauma_upper_ext.set_status(
+                        system="Upper Extremity",
+                        type_=TraumaType.TRAUMA_UPPER_EXT,
+                        value=TraumaSeverity(trauma_data.value),
+                        confidence=trauma_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating trauma upper extremity data: {e}")
 
         # Update alertness levels
-        if msg.valid_alertness_ocular:
-            self.alertness_ocular.set_status(
-                system="Neurological",
-                type_=TraumaType.ALERTNESS_OCULAR,
-                value=OcularAlertness(msg.alertness_ocular.value),
-                confidence=msg.alertness_ocular.confidence
-            )
-        if msg.valid_alertness_verbal:
-            self.alertness_verbal.set_status(
-                system="Neurological",
-                type_=TraumaType.ALERTNESS_VERBAL,
-                value=AlertnessLevel(msg.alertness_verbal.value),
-                confidence=msg.alertness_verbal.confidence
-            )
-        if msg.valid_alertness_motor:
-            self.alertness_motor.set_status(
-                system="Neurological",
-                type_=TraumaType.ALERTNESS_MOTOR,
-                value=AlertnessLevel(msg.alertness_motor.value),
-                confidence=msg.alertness_motor.confidence
-            )
+        if msg.alertness_ocular:  # Check if the array is not empty
+            try:
+                for alertness_data in msg.alertness_ocular:
+                    self.alertness_ocular.set_status(
+                        system="Neurological",
+                        type_=TraumaType.ALERTNESS_OCULAR,
+                        value=OcularAlertness(alertness_data.value),
+                        confidence=alertness_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating alertness ocular data: {e}")
+
+        if msg.alertness_verbal:  # Check if the array is not empty
+            try:
+                for alertness_data in msg.alertness_verbal:
+                    self.alertness_verbal.set_status(
+                        system="Neurological",
+                        type_=TraumaType.ALERTNESS_VERBAL,
+                        value=AlertnessLevel(alertness_data.value),
+                        confidence=alertness_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating alertness verbal data: {e}")
+
+        if msg.alertness_motor:  # Check if the array is not empty
+            try:
+                for alertness_data in msg.alertness_motor:
+                    self.alertness_motor.set_status(
+                        system="Neurological",
+                        type_=TraumaType.ALERTNESS_MOTOR,
+                        value=AlertnessLevel(alertness_data.value),
+                        confidence=alertness_data.confidence
+                    )
+            except Exception as e:
+                print(f"Error updating alertness motor data: {e}")
+
 
     def generate_cot_event(self):
         # Create root event element
diff --git a/docs/ground_control_station/multi_robot/multi_robot.md b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/__init__.py
similarity index 100%
rename from docs/ground_control_station/multi_robot/multi_robot.md
rename to ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/__init__.py
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/create_cot_msgs.py b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/create_cot_msgs.py
index b95f6b4c..a14367ec 100644
--- a/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/create_cot_msgs.py
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/create_cot_msgs.py
@@ -3,13 +3,26 @@
 from datetime import datetime
 import uuid
 from typing import List, Tuple
+import logging
 
-def create_gps_COT(uuid: str, cot_type : str, latitude : str, longitude: str, altitude : str):
+def create_gps_COT(uuid, latitude, longitude, altitude, logger_name, device_type=None):
     """Create a CoT event based on the GPS data."""
+    logger = logging.getLogger(logger_name)
+
+    # Define CoT type based on robot type
+    cot_type = "a-f-G"  # Default generic
+    if device_type:
+        if device_type.lower() == 'uav':
+            cot_type = "a-f-A"  # Aircraft
+        elif device_type.lower() in ['ugv', 'quadruped', 'offroad']:
+            cot_type = "a-f-G"  # Ground robot
+
+    logger.debug(f"Creating CoT event for {uuid} with type {cot_type}")
+
     root = ET.Element("event")
     root.set("version", "2.0")
     root.set("type", cot_type)
-    root.set("uid", uuid)  # Use topic name as UID for identification
+    root.set("uid", uuid)  # Use robot name as UID for identification
     root.set("how", "m-g")
     root.set("time", pytak.cot_time())
     root.set("start", pytak.cot_time())
@@ -25,6 +38,18 @@ def create_gps_COT(uuid: str, cot_type : str, latitude : str, longitude: str, al
 
     ET.SubElement(root, "point", attrib=pt_attr)
 
+    # Adding detail section
+    detail = ET.SubElement(root, "detail")
+
+    # Add robot type information if provided
+    if device_type:
+        contact = ET.SubElement(detail, "contact")
+        contact.set("callsign", uuid)
+
+        takv = ET.SubElement(detail, "takv")
+        takv.set("device", device_type)
+
+    logger.debug(f"CoT event created successfully for {uuid}")
     return ET.tostring(root, encoding="utf-8")
 
 def create_casevac_COT(uuid, casualty_id, gps, zap_num, mechanism, injury, signs_symptoms, treatments, physician_name):
diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/logger.py b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/logger.py
new file mode 100644
index 00000000..5da67aab
--- /dev/null
+++ b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/logger.py
@@ -0,0 +1,69 @@
+import logging
+
+
+def setup_logger(node, log_level):
+    """
+    Set up the logger with appropriate log level and formatting.
+
+    Args:
+        node: ROS2 node instance for logging through ROS system
+        log_level: The log level from config (DEBUG, INFO, WARNING, ERROR, CRITICAL)
+
+    Returns:
+        Configured logger object
+    """
+    # Convert string log level to logging constants
+    level_map = {
+        'DEBUG': logging.DEBUG,
+        'INFO': logging.INFO,
+        'WARNING': logging.WARNING,
+        'ERROR': logging.ERROR,
+        'CRITICAL': logging.CRITICAL
+    }
+
+    # Default to INFO if level not recognized
+    numeric_level = level_map.get(log_level, logging.INFO)
+
+    # Configure root logger
+    logger = logging.getLogger()
+    logger.setLevel(numeric_level)
+
+    # Remove any existing handlers to avoid duplicates
+    for handler in logger.handlers[:]:
+        logger.removeHandler(handler)
+
+    # Console handler with improved formatting
+    console = logging.StreamHandler()
+    console.setLevel(numeric_level)
+
+    # Format: timestamp - level - component - message
+    formatter = logging.Formatter('%(asctime)s - %(levelname)s - %(name)s - %(message)s')
+    console.setFormatter(formatter)
+
+    # Add handler to logger
+    logger.addHandler(console)
+
+    # Map Python logging levels to ROS2 logging levels
+    def log_bridge(record):
+        if record.levelno >= logging.CRITICAL:
+            node.get_logger().fatal(record.getMessage())
+        elif record.levelno >= logging.ERROR:
+            node.get_logger().error(record.getMessage())
+        elif record.levelno >= logging.WARNING:
+            node.get_logger().warn(record.getMessage())
+        elif record.levelno >= logging.INFO:
+            node.get_logger().info(record.getMessage())
+        elif record.levelno >= logging.DEBUG:
+            node.get_logger().debug(record.getMessage())
+
+    # Create a handler that bridges Python logging to ROS2 logging
+    class ROS2LogHandler(logging.Handler):
+        def emit(self, record):
+            log_bridge(record)
+
+    # Add ROS2 log handler
+    ros2_handler = ROS2LogHandler()
+    ros2_handler.setLevel(numeric_level)
+    logger.addHandler(ros2_handler)
+
+    return logger
\ No newline at end of file
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/CHANGELOG.rst b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/CHANGELOG.rst
new file mode 100644
index 00000000..0a789b78
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/CHANGELOG.rst
@@ -0,0 +1,149 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package rqt_py_console
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.2 (2021-08-31)
+------------------
+* Fix modern setuptools warning about dashes instead of underscores (`#11 <https://github.com/ros-visualization/rqt_py_console/issues/11>`_)
+* Contributors: Chris Lalancette
+
+1.0.1 (2021-04-27)
+------------------
+* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 <https://github.com/ros-visualization/rqt_py_console/issues/8>`_)
+* Contributors: Alejandro Hernández Cordero
+
+1.0.0 (2018-12-11)
+------------------
+* spyderlib -> spyder (`#5 <https://github.com/ros-visualization/rqt_py_console/issues/5>`_)
+* ros2 port (`#3 <https://github.com/ros-visualization/rqt_py_console/issues/3>`_)
+* autopep8 (`#2 <https://github.com/ros-visualization/rqt_py_console/issues/2>`_)
+* Contributors: Mike Lautman
+
+0.4.8 (2017-04-28)
+------------------
+
+0.4.7 (2017-03-02)
+------------------
+
+0.4.6 (2017-02-27)
+------------------
+
+0.4.5 (2017-02-03)
+------------------
+
+0.4.4 (2017-01-24)
+------------------
+* use Python 3 compatible syntax (`#421 <https://github.com/ros-visualization/rqt_common_plugins/pull/421>`_)
+
+0.4.3 (2016-11-02)
+------------------
+
+0.4.2 (2016-09-19)
+------------------
+
+0.4.1 (2016-05-16)
+------------------
+
+0.4.0 (2016-04-27)
+------------------
+* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 <https://github.com/ros-visualization/rqt_common_plugins/pull/359>`_)
+
+0.3.13 (2016-03-08)
+-------------------
+
+0.3.12 (2015-07-24)
+-------------------
+
+0.3.11 (2015-04-30)
+-------------------
+
+0.3.10 (2014-10-01)
+-------------------
+* update plugin scripts to use full name to avoid future naming collisions
+
+0.3.9 (2014-08-18)
+------------------
+
+0.3.8 (2014-07-15)
+------------------
+
+0.3.7 (2014-07-11)
+------------------
+* export architecture_independent flag in package.xml (`#254 <https://github.com/ros-visualization/rqt_common_plugins/issues/254>`_)
+
+0.3.6 (2014-06-02)
+------------------
+
+0.3.5 (2014-05-07)
+------------------
+
+0.3.4 (2014-01-28)
+------------------
+
+0.3.3 (2014-01-08)
+------------------
+* add groups for rqt plugins, renamed some plugins (`#167 <https://github.com/ros-visualization/rqt_common_plugins/issues/167>`_)
+
+0.3.2 (2013-10-14)
+------------------
+
+0.3.1 (2013-10-09)
+------------------
+
+0.3.0 (2013-08-28)
+------------------
+
+0.2.17 (2013-07-04)
+-------------------
+
+0.2.16 (2013-04-09 13:33)
+-------------------------
+
+0.2.15 (2013-04-09 00:02)
+-------------------------
+
+0.2.14 (2013-03-14)
+-------------------
+
+0.2.13 (2013-03-11 22:14)
+-------------------------
+
+0.2.12 (2013-03-11 13:56)
+-------------------------
+
+0.2.11 (2013-03-08)
+-------------------
+
+0.2.10 (2013-01-22)
+-------------------
+
+0.2.9 (2013-01-17)
+------------------
+
+0.2.8 (2013-01-11)
+------------------
+
+0.2.7 (2012-12-24)
+------------------
+
+0.2.6 (2012-12-23)
+------------------
+
+0.2.5 (2012-12-21 19:11)
+------------------------
+
+0.2.4 (2012-12-21 01:13)
+------------------------
+
+0.2.3 (2012-12-21 00:24)
+------------------------
+
+0.2.2 (2012-12-20 18:29)
+------------------------
+
+0.2.1 (2012-12-20 17:47)
+------------------------
+
+0.2.0 (2012-12-20 17:39)
+------------------------
+* first release of this package into groovy
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/README.md b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/README.md
new file mode 100644
index 00000000..e13fe32e
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/README.md
@@ -0,0 +1,19 @@
+# RQT Python AirstackControlPanel
+
+If you `colcon build` this package in a workspace and then run "rqt --force-discover" after sourcing the workspace, the plugin should show up as "Airstack Control Panel" in "Miscellaneous Tools" in the "Plugins" menu.
+
+You can use the `generate_rqt_py_package.sh` script to generate a new package by doing the following from the rqt_airstack_control_panel directory
+
+```
+./generate_rqt_py_package.sh [package name] [class name] [plugin title]
+```
+
+[package name] will be the name of the package and a directory with this name will be created above `rqt_airstack_control_panel/`. [class name] is the name of the class in `src/[package name]/template.py`. [plugin title] is what the plugin will be called in the "Miscellaneous Tools" menu.
+
+For example,
+
+```
+cd rqt_airstack_control_panel/
+./generate_rqt_py_package.sh new_rqt_package ClassName "Plugin Title"
+```
+
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/package.xml b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/package.xml
new file mode 100644
index 00000000..b85ad4a9
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/package.xml
@@ -0,0 +1,28 @@
+<package format="2">
+  <name>rqt_airstack_control_panel</name>
+  <version>1.0.2</version>
+  <description>rqt_airstack_control_panel is a Python GUI template.</description>
+  <maintainer email="jkeller2@andrew.cmu.edu">John Keller</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website"></url>
+  <url type="repository"></url>
+  <url type="bugtracker"></url>
+
+  <author>John Keller</author>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
+  <exec_depend>qt_gui</exec_depend>
+  <exec_depend>qt_gui_py_common</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_gui_py</exec_depend>
+
+  <export>
+    <architecture_independent/>
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/plugin.xml b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/plugin.xml
new file mode 100644
index 00000000..00e02b05
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/plugin.xml
@@ -0,0 +1,17 @@
+<library path="src">
+  <class name="AirstackControlPanel" type="rqt_airstack_control_panel.template.AirstackControlPanel" base_class_type="rqt_gui_py::Plugin">
+    <description>
+      A Python GUI plugin providing an interactive Python console.
+    </description>
+    <qtgui>
+      <group>
+        <label>Miscellaneous Tools</label>
+        <icon type="theme">folder</icon>
+        <statustip>Plugins related to miscellaneous tools.</statustip>
+      </group>
+      <label>Airstack Control Panel</label>
+      <icon type="theme">applications-python</icon>
+      <statustip>A Python RQT GUI template.</statustip>
+    </qtgui>
+  </class>
+</library>
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/resource/py_console_widget.ui b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/resource/py_console_widget.ui
new file mode 100644
index 00000000..12810f1c
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/resource/py_console_widget.ui
@@ -0,0 +1,53 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>PyConsole</class>
+ <widget class="QWidget" name="PyConsole">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>276</width>
+    <height>212</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>PyConsole</string>
+  </property>
+  <layout class="QVBoxLayout" name="_2">
+   <property name="spacing">
+    <number>0</number>
+   </property>
+   <property name="leftMargin">
+    <number>0</number>
+   </property>
+   <property name="topMargin">
+    <number>0</number>
+   </property>
+   <property name="rightMargin">
+    <number>3</number>
+   </property>
+   <property name="bottomMargin">
+    <number>0</number>
+   </property>
+   <item>
+    <layout class="QHBoxLayout" name="header_layout">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <item>
+      <widget class="PyConsoleTextEdit" name="py_console"/>
+     </item>
+    </layout>
+   </item>
+  </layout>
+ </widget>
+ <customwidgets>
+  <customwidget>
+   <class>PyConsoleTextEdit</class>
+   <extends>QTextEdit</extends>
+   <header>rqt_py_console.py_console_text_edit</header>
+  </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/resource/rqt_airstack_control_panel b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/resource/rqt_airstack_control_panel
new file mode 100644
index 00000000..e69de29b
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/setup.cfg b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/setup.cfg
new file mode 100644
index 00000000..7f42fcca
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/rqt_airstack_control_panel
+[install]
+install_scripts=$base/lib/rqt_airstack_control_panel
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/setup.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/setup.py
new file mode 100644
index 00000000..fe6b7c2f
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/setup.py
@@ -0,0 +1,39 @@
+from setuptools import setup
+
+package_name = 'rqt_airstack_control_panel'
+
+setup(
+    name=package_name,
+    version='1.0.2',
+    packages=[package_name],
+    package_dir={'': 'src'},
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name + '/resource',
+            ['resource/py_console_widget.ui']),
+        ('share/' + package_name, ['package.xml']),
+        ('share/' + package_name, ['plugin.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    author='',
+    maintainer='',
+    maintainer_email='',
+    keywords=['ROS'],
+    classifiers=[
+        '',
+        '',
+        '',
+        '',
+    ],
+    description=(
+        'rqt_airstack_control_panel'
+    ),
+    license='BSD',
+    entry_points={
+        'console_scripts': [
+            'rqt_airstack_control_panel = ' + package_name + '.main:main',
+        ],
+    },
+)
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/__init__.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/drag_and_drop.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/drag_and_drop.py
new file mode 100644
index 00000000..4e3a1e5f
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/drag_and_drop.py
@@ -0,0 +1,166 @@
+# adapted from https://www.pythonguis.com/faq/pyqt-drag-drop-widgets/
+from PyQt5 import QtCore
+from PyQt5.QtCore import QMimeData, Qt, pyqtSignal
+from PyQt5.QtGui import QDrag, QPixmap
+from PyQt5.QtWidgets import (
+    QApplication,
+    QHBoxLayout,
+    QLabel,
+    QMainWindow,
+    QVBoxLayout,
+    QGridLayout,
+    QWidget,
+    QComboBox,
+    QLineEdit
+)
+
+class DragTargetIndicator(QLabel):
+    def __init__(self, parent=None):
+        super().__init__(parent)
+        #self.setContentsMargins(25, 5, 25, 5)
+        #self.setStyleSheet("QLabel { background-color: #ccc; border: 1px solid black; }")
+
+    def set_size(self, size):
+        self.setFixedSize(size)
+
+
+class DragItem(QWidget):
+    def __init__(self, w):
+        super().__init__()
+        self.widget = w
+        self.widget.destroyed.connect(self.child_destroyed)
+        self.setObjectName('main')
+        self.layout = QVBoxLayout()
+        self.setLayout(self.layout)
+        self.setAttribute(QtCore.Qt.WA_StyledBackground, True)
+        #self.setStyleSheet('QWidget#main {background-color: lightcyan; border: 1px solid black;}')
+        
+        self.layout.addWidget(w)
+
+    def set_data(self, data):
+        self.data = data
+
+    def child_destroyed(self):
+        self.deleteLater()
+        
+    def mouseMoveEvent(self, e):
+        if e.buttons() == Qt.LeftButton:
+            drag = QDrag(self)
+            mime = QMimeData()
+            drag.setMimeData(mime)
+            
+            pixmap = QPixmap(self.size())
+            self.render(pixmap)
+            drag.setPixmap(pixmap)
+            
+            drag.exec_(Qt.MoveAction)
+            self.show() # Show this widget again, if it's dropped outside.
+
+            
+class DragWidget(QWidget):
+    """
+    Generic list sorting handler.
+    """
+
+    orderChanged = pyqtSignal(list)
+
+    def __init__(self, *args, orientation=Qt.Orientation.Horizontal, **kwargs):
+        super().__init__()
+        self.setAcceptDrops(True)
+
+        # Store the orientation for drag checks later.
+        self.orientation = orientation
+
+        if self.orientation == Qt.Orientation.Vertical:
+            self.blayout = QVBoxLayout()
+        else:
+            self.blayout = QHBoxLayout()
+
+        # Add the drag target indicator. This is invisible by default,
+        # we show it and move it around while the drag is active.
+        self._drag_target_indicator = DragTargetIndicator()
+        self.blayout.addWidget(self._drag_target_indicator)
+        self._drag_target_indicator.hide()
+
+        self.setLayout(self.blayout)
+
+    def dragEnterEvent(self, e):
+        e.accept()
+
+    def dragLeaveEvent(self, e):
+        self._drag_target_indicator.hide()
+        e.accept()
+
+    def getWidgets(self):
+        widgets = []
+        for i in range(self.blayout.count()):
+            try:
+                widgets.append(self.blayout.itemAt(i).widget().widget)
+            except:
+                pass
+        return widgets
+
+    def dragMoveEvent(self, e):
+        # Find the correct location of the drop target, so we can move it there.
+        index = self._find_drop_location(e)
+        if index is not None:
+            # Inserting moves the item if its alreaady in the layout.
+            self.blayout.insertWidget(index, self._drag_target_indicator)
+            # Hide the item being dragged.
+            e.source().hide()
+            # Show the target.
+            self._drag_target_indicator.set_size(e.source().size())
+            self._drag_target_indicator.show()
+        e.accept()
+
+    def dropEvent(self, e):
+        widget = e.source()
+        # Use drop target location for destination, then remove it.
+        self._drag_target_indicator.hide()
+        index = self.blayout.indexOf(self._drag_target_indicator)
+        if index is not None:
+            self.blayout.insertWidget(index, widget)
+            self.orderChanged.emit(self.get_item_data())
+            widget.show()
+            self.blayout.activate()
+        e.accept()
+
+    def _find_drop_location(self, e):
+        pos = e.pos()
+        spacing = self.blayout.spacing() / 2
+
+        for n in range(self.blayout.count()):
+            # Get the widget at each index in turn.
+            w = self.blayout.itemAt(n).widget()
+
+            if self.orientation == Qt.Orientation.Vertical:
+                # Drag drop vertically.
+                drop_here = (
+                    pos.y() >= w.y() - spacing
+                    and pos.y() <= w.y() + w.size().height() + spacing
+                )
+            else:
+                # Drag drop horizontally.
+                drop_here = (
+                    pos.x() >= w.x() - spacing
+                    and pos.x() <= w.x() + w.size().width() + spacing
+                )
+
+            if drop_here:
+                # Drop over this target.
+                break
+
+        return n
+
+    def add_item(self, item):
+        self.blayout.addWidget(item)
+
+    def get_item_data(self):
+        data = []
+        for n in range(self.blayout.count()):
+            # Get the widget at each index in turn.
+            w = self.blayout.itemAt(n).widget()
+            if hasattr(w, "data"):
+                # The target indicator has no data.
+                data.append(w.data)
+        return data
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/main.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/main.py
new file mode 100755
index 00000000..9a5c9376
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/main.py
@@ -0,0 +1,12 @@
+import sys
+
+from rqt_gui.main import Main
+
+
+def main():
+    main = Main()
+    sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole'))
+
+
+if __name__ == '__main__':
+    main()
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/py_console_text_edit.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/py_console_text_edit.py
new file mode 100644
index 00000000..dc9ce1a0
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/py_console_text_edit.py
@@ -0,0 +1,69 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Dorian Scholz
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import sys
+from code import InteractiveInterpreter
+
+from python_qt_binding import QT_BINDING, QT_BINDING_VERSION
+from python_qt_binding.QtCore import Qt, Signal
+
+from qt_gui_py_common.console_text_edit import ConsoleTextEdit
+
+
+class PyConsoleTextEdit(ConsoleTextEdit):
+    _color_stdin = Qt.darkGreen
+    _multi_line_char = ':'
+    _multi_line_indent = '    '
+    _prompt = ('>>> ', '... ')  # prompt for single and multi line
+    exit = Signal()
+
+    def __init__(self, parent=None):
+        super(PyConsoleTextEdit, self).__init__(parent)
+
+        self._interpreter_locals = {}
+        self._interpreter = InteractiveInterpreter(self._interpreter_locals)
+
+        self._comment_writer.write('Python %s on %s\n' %
+                                   (sys.version.replace('\n', ''), sys.platform))
+        self._comment_writer.write(
+            'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION))
+
+        self._add_prompt()
+
+    def update_interpreter_locals(self, newLocals):
+        self._interpreter_locals.update(newLocals)
+
+    def _exec_code(self, code):
+        try:
+            self._interpreter.runsource(code)
+        except SystemExit:  # catch sys.exit() calls, so they don't close the whole gui
+            self.exit.emit()
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/py_console_widget.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/py_console_widget.py
new file mode 100644
index 00000000..e69bde34
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/py_console_widget.py
@@ -0,0 +1,59 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Dorian Scholz
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import os
+from ament_index_python.resources import get_resource
+
+from python_qt_binding import loadUi
+from python_qt_binding.QtWidgets import QWidget
+from rqt_py_console.py_console_text_edit import PyConsoleTextEdit
+
+
+class PyConsoleWidget(QWidget):
+
+    def __init__(self, context=None):
+        super(PyConsoleWidget, self).__init__()
+
+        _, package_path = get_resource('packages', 'rqt_py_console')
+        ui_file = os.path.join(
+            package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui')
+
+        loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit})
+        self.setObjectName('PyConsoleWidget')
+
+        my_locals = {
+            'context': context
+        }
+        self.py_console.update_interpreter_locals(my_locals)
+        self.py_console.print_message(
+            'The variable "context" is set to the PluginContext of this plugin.')
+        self.py_console.exit.connect(context.close_plugin)
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/spyder_console_widget.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/spyder_console_widget.py
new file mode 100644
index 00000000..374ef7a5
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/spyder_console_widget.py
@@ -0,0 +1,60 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Dorian Scholz
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from python_qt_binding.QtGui import QFont
+
+from spyder.widgets.internalshell import InternalShell
+from spyder.utils.module_completion import moduleCompletion
+
+class SpyderConsoleWidget(InternalShell):
+
+    def __init__(self, context=None):
+        my_locals = {
+            'context': context
+        }
+        super(SpyderConsoleWidget, self).__init__(namespace=my_locals)
+        self.setObjectName('SpyderConsoleWidget')
+        self.set_pythonshell_font(QFont('Mono'))
+        self.interpreter.restore_stds()
+
+    def get_module_completion(self, objtxt):
+        """Return module completion list associated to object name"""
+        return moduleCompletion(objtxt)
+
+    def run_command(self, *args):
+        self.interpreter.redirect_stds()
+        super(SpyderConsoleWidget, self).run_command(*args)
+        self.flush()
+        self.interpreter.restore_stds()
+
+    def shutdown(self):
+        self.exit_interpreter()
diff --git a/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/template.py b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/template.py
new file mode 100644
index 00000000..7591064d
--- /dev/null
+++ b/ground_control_station/ros_ws/src/rqt_airstack_control_panel/src/rqt_airstack_control_panel/template.py
@@ -0,0 +1,513 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2012, Dorian Scholz
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Willow Garage, Inc. nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from python_qt_binding.QtWidgets import QVBoxLayout, QWidget
+from rqt_gui_py.plugin import Plugin
+from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog
+from rqt_py_console.py_console_widget import PyConsoleWidget
+
+import python_qt_binding.QtWidgets as qt
+import python_qt_binding.QtWidgets as QtWidgets
+import python_qt_binding.QtGui as QtGui
+import python_qt_binding.QtCore as QtCore
+
+import subprocess
+import threading
+import time
+import copy
+
+from std_msgs.msg import Bool
+
+from .drag_and_drop import DragWidget, DragItem
+
+try:
+    from rqt_py_console.spyder_console_widget import SpyderConsoleWidget
+    _has_spyderlib = True
+except ImportError:
+    _has_spyderlib = False
+
+
+logger = None
+
+def xor_encrypt_decrypt(data, key=983476):
+    return ''.join(chr(ord(c) ^ key) for c in data)
+    
+class InfoConfigDialog(qt.QDialog):
+    def __init__(self, settings):
+        super().__init__()
+        
+        self.setWindowTitle('Configuration')
+        layout = qt.QVBoxLayout()
+
+        self.name_label = qt.QLabel('Name:')
+        layout.addWidget(self.name_label)
+        self.name_entry = qt.QLineEdit()
+        self.name_entry.setText(settings['name'])
+        layout.addWidget(self.name_entry)
+        
+        self.username_label = qt.QLabel('Username:')
+        layout.addWidget(self.username_label)
+        self.username_entry = qt.QLineEdit()
+        self.username_entry.setText(settings['username'])
+        layout.addWidget(self.username_entry)
+
+        self.password_label = qt.QLabel('Password:')
+        layout.addWidget(self.password_label)
+        self.password_entry = qt.QLineEdit()
+        self.password_entry.setEchoMode(qt.QLineEdit.Password)
+        self.password_entry.setText(settings['password'])
+        layout.addWidget(self.password_entry)
+        
+        self.hostname_label = qt.QLabel('Hostname:')
+        layout.addWidget(self.hostname_label)
+        self.hostname_entry = qt.QLineEdit()
+        self.hostname_entry.setText(settings['hostname'])
+        layout.addWidget(self.hostname_entry)
+        
+        self.namespace_label = qt.QLabel('Namespace:')
+        layout.addWidget(self.namespace_label)
+        self.namespace_entry = qt.QLineEdit()
+        self.namespace_entry.setText(settings['namespace'])
+        layout.addWidget(self.namespace_entry)
+        
+        self.path_label = qt.QLabel('Docker Compose Path:')
+        layout.addWidget(self.path_label)
+        self.path_entry = qt.QLineEdit()
+        self.path_entry.setText(settings['path'])
+        layout.addWidget(self.path_entry)
+        
+        self.services_label = qt.QLabel('Services to Hide (comma separated):')
+        layout.addWidget(self.services_label)
+        self.services_entry = qt.QLineEdit()
+        self.services_entry.setText(', '.join(settings['excluded_services']))
+        layout.addWidget(self.services_entry)
+
+        self.enable_display_checkbox = qt.QCheckBox('Enable Display')
+        self.enable_display_checkbox.setChecked(settings['enable_display'])
+        layout.addWidget(self.enable_display_checkbox)
+        
+        self.submit_button = qt.QPushButton('Submit')
+        self.submit_button.clicked.connect(self.submit)
+        layout.addWidget(self.submit_button)
+        
+        self.setLayout(layout)
+        
+        self.result = None
+    
+    def submit(self):
+        name = self.name_entry.text()
+        hostname = self.hostname_entry.text()
+        username = self.username_entry.text()
+        password = self.password_entry.text()
+        namespace = self.namespace_entry.text()
+        path = self.path_entry.text()
+        services = list(map(lambda x:x.strip(), self.services_entry.text().split(',')))
+        try:
+            services.remove('')
+        except:
+            pass
+        enable_display = self.enable_display_checkbox.isChecked()
+        self.result = {'name': name, 'username': username, 'password': password, 'hostname': hostname, 'namespace': namespace, 'path': path, 'excluded_services': services, 'enable_display': enable_display}
+        self.accept()
+
+class CommandThread(QtCore.QThread):
+    output_signal = QtCore.pyqtSignal(str)
+    
+    def __init__(self, command, callback=None, wait_until_finished=False, timeout=None):
+        super().__init__()
+        self.command = command
+        if callback != None:
+            self.output_signal.connect(callback)
+        self.wait_until_finished = wait_until_finished
+        self.timeout = timeout
+        self.running = True
+        self.start()
+
+    def run(self):
+        process = subprocess.Popen(self.command, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True, shell=True)
+        
+        if not self.wait_until_finished:
+            while self.running:
+                line = process.stdout.readline()
+                if not line:
+                    break
+                self.output_signal.emit(line.strip())
+        if self.wait_until_finished:
+            try:
+                process.wait(timeout=self.timeout)
+                self.output_signal.emit(process.stdout.read())
+            except subprocess.TimeoutExpired:
+                process.kill()
+
+        process.terminate()
+
+    def stop(self):
+        self.running = False
+        self.terminate()
+
+class InfoWidget(qt.QWidget):
+    def __init__(self, node, settings={'name': 'Name', 'username': 'airlab', 'password': 'passme24', 'hostname': 'localhost', 'namespace': 'none', 'path': '~/airstack', 'excluded_services': [], 'enable_display': False}):
+        super(qt.QWidget, self).__init__()
+        self.layout = qt.QVBoxLayout(self)
+        #self.setObjectName('info_widget')
+        #self.setStyleSheet('#info_widget {border: 2px solid black;}')
+
+        self.node = node
+        self.recording_sub = None
+        self.recording_pub = None
+        
+        self.setAttribute(QtCore.Qt.WA_StyledBackground, True)
+        self.setObjectName('info_widget')
+        self.stylesheet = 'border: 2px solid lightgrey;  border-radius: 10px;'
+        self.setStyleSheet('QWidget#info_widget { ' + self.stylesheet + ' }')
+ 
+        self.settings = settings
+
+        self.ping_thread = None
+        self.connected = False
+        self.services = {}
+
+        # info panel
+        self.info_widget = qt.QWidget()
+        self.info_layout = qt.QHBoxLayout(self.info_widget)
+        self.layout.addWidget(self.info_widget)
+
+        self.name_label = qt.QLabel(self.settings['name'])
+        self.name_label.setStyleSheet('font-weight: bold;')
+        self.info_layout.addWidget(self.name_label)
+
+        self.hostname_label = qt.QLabel('Hostname: ' + self.settings['hostname'])
+        self.info_layout.addWidget(self.hostname_label)
+        
+        self.ping_label = qt.QLabel('Ping:')
+        self.info_layout.addWidget(self.ping_label)
+        
+        self.recording_label = qt.QLabel('Recording:')
+        self.info_layout.addWidget(self.recording_label)
+
+        line = qt.QFrame()
+        line.setFrameShape(qt.QFrame.HLine)
+        line.setFrameShadow(qt.QFrame.Plain)
+        line.setStyleSheet('QFrame {background-color: #cccccc; max-height: 1px; border: none; }')
+        self.layout.addWidget(line)
+
+        # command panel
+        self.command_widget = qt.QWidget()
+        self.command_layout = qt.QHBoxLayout(self.command_widget)
+        self.layout.addWidget(self.command_widget)
+
+        self.refresh_button = qt.QPushButton(text='Refresh')
+        self.refresh_button.clicked.connect(self.refresh_docker)
+        self.command_layout.addWidget(self.refresh_button)
+
+        self.restart_button = qt.QPushButton(text='Restart')
+        self.restart_button.clicked.connect(lambda :self.docker_command('restart'))
+        self.command_layout.addWidget(self.restart_button)
+
+        self.up_button = qt.QPushButton(text='Up')
+        self.up_button.clicked.connect(lambda :self.docker_command('up'))
+        self.command_layout.addWidget(self.up_button)
+
+        self.down_button = qt.QPushButton(text='Down')
+        self.down_button.clicked.connect(lambda :self.docker_command('down'))
+        self.command_layout.addWidget(self.down_button)
+        
+        self.record_button = qt.QPushButton(text='Record')
+        self.record_button.setCheckable(True)
+        self.record_button.clicked.connect(self.record)
+        self.command_layout.addWidget(self.record_button)
+        
+        self.ssh_button = qt.QPushButton(text='ssh')
+        self.ssh_button.clicked.connect(self.ssh)
+        self.command_layout.addWidget(self.ssh_button)
+
+        self.config_button = qt.QPushButton(text='Config')
+        self.config_button.clicked.connect(self.config_clicked)
+        self.command_layout.addStretch(1)
+        self.command_layout.addWidget(self.config_button)
+
+        self.delete_button = qt.QPushButton(text='X')
+        self.delete_button.clicked.connect(self.delete_clicked)
+        self.command_layout.addWidget(self.delete_button)
+        
+        line = qt.QFrame()
+        line.setFrameShape(qt.QFrame.HLine)
+        line.setFrameShadow(qt.QFrame.Plain)
+        line.setStyleSheet('QFrame {background-color: #cccccc; max-height: 1px; border: none; }')
+        self.layout.addWidget(line)
+
+        # docker panel
+        self.docker_widget = qt.QWidget()
+        self.docker_layout = qt.QHBoxLayout(self.docker_widget)
+        self.layout.addWidget(self.docker_widget)
+
+        self.update_info()
+        #self.refresh_docker()
+
+    def config_clicked(self):
+        dialog = InfoConfigDialog(self.settings)
+        if dialog.exec():
+            self.settings = dialog.result
+            self.update_info()
+
+    def delete_clicked(self):
+        self.delete_function()
+
+    def record(self):
+        if self.recording_pub != None:
+            msg = Bool()
+            msg.data = self.record_button.isChecked()
+            self.recording_pub.publish(msg)
+            
+
+    def update_info(self):
+        self.name_label.setText(self.settings['name'])
+        self.hostname_label.setText('Hostname: ' + self.settings['hostname'])
+
+        if self.ping_thread != None:
+            self.ping_thread.stop()
+            del self.ping_thread
+            self.ping_thread = None
+        #self.ping_thread = CommandThread('ping ' + self.settings['hostname'])
+        self.ping_thread = CommandThread('HOST="' + self.settings['hostname'] + '"; while true; do OUTPUT=$(ping -c 1 -w 3 $HOST 2>&1); if echo "$OUTPUT" | grep -q "time="; then PING_TIME=$(echo "$OUTPUT" | grep -oP "time=\K[\d.]+"); echo "$PING_TIME ms"; else echo "failed"; fi; sleep 1; done', self.handle_ping)
+
+        if self.recording_sub != None:
+            self.node.destroy_subscription(self.recording_sub)
+        self.recording_sub = self.node.create_subscription(Bool, self.settings['namespace'] + '/bag_record/bag_recording_status',
+                                                           self.recording_callback, 1)
+        self.recording_pub = self.node.create_publisher(Bool, self.settings['namespace'] + '/bag_record/set_recording_status', 1)
+
+    def recording_callback(self, msg):
+        if msg.data:
+            self.recording_label.setText('Recording: YES')
+        else:
+            self.recording_label.setText('Recording: NO')
+
+    def ssh_t(self, command):
+        ssh = 'ssh -t -o StrictHostKeyChecking=no'
+        if self.settings['enable_display']:
+            ssh += ' -X'
+
+        display = ''
+        if self.settings['enable_display']:
+            display = 'export DISPLAY=:1; '
+            
+        return 'sshpass -p "' + self.settings['password'] + '" ' + ssh + ' ' + \
+            self.settings['username'] + '@' + self.settings['hostname'] + ' "' + display + command + '"'
+
+    def refresh_docker(self):
+        command = self.ssh_t('cd ' + self.settings['path'] + ';  docker compose --profile \'*\' config --services && echo SPLIT && docker compose ps --format {{.Service}}')
+        
+        #result = subprocess.run(command, capture_output=True, text=True, check=True, shell=True)
+
+        self.refresh_thread = CommandThread(command, self.handle_refresh_docker, True, 5)
+
+    def handle_refresh_docker(self, stdout):
+        if 'SPLIT' not in stdout:
+            return
+        
+        services = sorted(stdout.split('SPLIT')[0].split('\n'))
+        running = stdout.split('SPLIT')[1].split('\n')
+        
+        for i in reversed(range(self.docker_layout.count())): 
+            self.docker_layout.itemAt(i).widget().setParent(None)
+        
+        for service in services:
+            if service != '' and service not in self.settings['excluded_services']:
+                button = qt.QPushButton(text=service)
+                if service in running:
+                    button.setStyleSheet('background-color: lightgreen;')
+                button.setCheckable(True)
+                if service in self.services:
+                    button.setChecked(self.services[service])
+                else:
+                    self.services[service] = False
+                def get_click_function(s, b):
+                    def click_function():
+                        self.services[s] = b.isChecked()
+                    return click_function
+                button.clicked.connect(get_click_function(service, button))
+
+                def get_open_menu_function(s, b):
+                    def menu_triggered(action):
+                        if action.text() == 'bash':
+                            self.docker_exec(s, 'bash')
+                        elif action.text() == 'tmux':
+                            self.docker_exec(s, 'tmux a')
+                    
+                    def open_menu_function():
+                        menu = qt.QMenu(self)
+                        action1 = menu.addAction('bash')
+                        action2 = menu.addAction('tmux')
+                        menu.triggered.connect(menu_triggered)
+                        menu.popup(b.mapToGlobal(b.rect().topLeft()))
+                    return open_menu_function
+                
+                button.setContextMenuPolicy(3)
+                button.customContextMenuRequested.connect(get_open_menu_function(service, button))
+                
+                self.docker_layout.addWidget(button)
+
+    def docker_command(self, command):
+        services_str = ' '.join(self.get_selected_services())
+
+        logger.info('docker command: ' + command + ' ' + services_str)
+                
+        if len(services_str) > 0:
+            command = 'dbus-launch gnome-terminal --wait -- bash -c \'' + \
+                self.ssh_t('cd ' + self.settings['path'] + ';  docker compose ' + command + ' ' + services_str + \
+                           (' -d' if command == 'up' else '')) + '\''
+            logger.info('docker_command: ' + command)
+            self.docker_command_thread = CommandThread(command, lambda :self.refresh_docker(), True)
+
+    def docker_exec(self, service, command):
+        proc = f'''
+        mapfile -t names <<< $(sshpass -p "{self.settings['password']}" \
+                               ssh -t -o StrictHostKeyChecking=no {self.settings['username']}@{self.settings['hostname']} \
+                                 "cd {self.settings['path']}; docker ps -f name={service} --format '{{{{.Names}}}}'");
+        for item in ${{names[@]}}; do
+            item=$(echo $item| tr -d '\\r')
+            command="dbus-launch gnome-terminal -- bash -c 'sshpass -p \\"{self.settings['password']}\\" \
+                                                ssh -t -o StrictHostKeyChecking=no \
+                                                  {self.settings['username']}@{self.settings['hostname']} \
+                                                    \\"cd {self.settings['path']};  docker exec -it $item {command};\\"';"
+            eval "$command"
+        done
+        '''
+        
+        logger.info(proc)
+        subprocess.Popen(proc, shell=True, executable='/usr/bin/bash')
+
+    def ssh(self):
+        proc = f'''dbus-launch gnome-terminal -- bash -c 'sshpass -p "{self.settings['password']}" \
+                                                          ssh -o StrictHostKeyChecking=no \
+                                                          {self.settings['username']}@{self.settings['hostname']}' '''
+        logger.info(proc)
+        subprocess.Popen(proc, shell=True, executable='/usr/bin/bash')
+
+    def handle_ping(self, text):
+        if text == 'failed':
+            self.setStyleSheet('QWidget#info_widget { ' + self.stylesheet + 'background-color: rgb(255, 144, 144) }')
+            self.connected = False
+        else:
+            self.setStyleSheet('QWidget#info_widget { ' + self.stylesheet + 'background-color: rgb(239, 239, 239) }')
+            if not self.connected:
+                self.refresh_docker()
+            self.connected = True
+            
+        self.ping_label.setText('Ping: ' + text)
+
+    def get_dct(self):
+        return self.settings
+    
+    def get_selected_services(self):
+        selected_services = []
+        for service, selected in self.services.items():
+            if selected:
+                selected_services.append(service)
+        return selected_services
+        
+    
+class AirstackControlPanel(Plugin):
+    def __init__(self, context):
+        super(AirstackControlPanel, self).__init__(context)
+        self.setObjectName('AirstackControlPanel')
+
+        self.context = context
+        # to access ros2 node use self.context.node
+
+        global logger
+        logger = self.context.node.get_logger()
+
+        self.widget = qt.QWidget()
+        self.layout = qt.QVBoxLayout(self.widget)
+
+        self.info_widget = qt.QWidget()
+        self.info_layout = qt.QVBoxLayout(self.info_widget)
+
+        self.drag_widget = DragWidget(orientation=QtCore.Qt.Orientation.Vertical)
+        self.info_layout.addWidget(self.drag_widget)
+        
+        self.info_scroll_area = qt.QScrollArea()
+        self.info_scroll_area.setWidget(self.info_widget)
+        self.info_scroll_area.setWidgetResizable(True)
+        self.info_scroll_area.setHorizontalScrollBarPolicy(QtCore.Qt.ScrollBarAlwaysOff)
+        self.info_scroll_area.setMinimumHeight(800)
+        self.layout.addWidget(self.info_scroll_area)
+
+        self.add_button = qt.QPushButton('+')
+        self.add_button.clicked.connect(lambda x:self.add_info_widget())
+        self.layout.addWidget(self.add_button)
+        self.layout.addStretch(1)
+        
+        self.context.add_widget(self.widget)
+
+    def add_info_widget(self, settings=None):
+        if settings == None:
+            info_widget = DragItem(InfoWidget(self.context.node))
+        else:
+            info_widget = DragItem(InfoWidget(self.context.node, settings))
+        info_widget.widget.delete_function = info_widget.deleteLater
+        self.drag_widget.add_item(info_widget)
+
+    def save_settings(self, plugin_settings, instance_settings):
+        info_dcts = [copy.deepcopy(w.get_dct()) for w in self.drag_widget.getWidgets()]
+        for settings in info_dcts:
+            settings['password'] = xor_encrypt_decrypt(settings['password'])
+        instance_settings.set_value('info_dcts', info_dcts)
+
+    def restore_settings(self, plugin_settings, instance_settings):
+        info_dcts = instance_settings.value('info_dcts', {})
+        for settings in info_dcts:
+            settings['password'] = xor_encrypt_decrypt(settings['password'])
+            self.add_info_widget(settings)
+
+    def trigger_configuration(self):
+        options = [
+            {'title': 'Option 1',
+             'description': 'Description of option 1.',
+             'enabled': True},
+            {'title': 'Option 2',
+             'description': 'Description of option 2.'},
+        ]
+        dialog = SimpleSettingsDialog(title='Options')
+        dialog.add_exclusive_option_group(title='List of options:', options=options, selected_index=0)
+        selected_index = dialog.get_settings()[0]
+        if selected_index != None:
+            selected_index = selected_index['selected_index']
+            print('selected_index ', selected_index)
+
+    def shutdown_console_widget(self):
+        pass
+
+    def shutdown_plugin(self):
+        self.shutdown_console_widget()
diff --git a/mkdocs.yml b/mkdocs.yml
index 8ba4fb5d..5a888add 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -43,13 +43,17 @@ markdown_extensions:
   - toc:
       permalink: true
 nav:
-  - Home: docs/README.md
+  - Home: docs/index.md
   - Getting Started: docs/getting_started.md
   - Development:
       - docs/development/index.md
       - docs/development/docker_usage.md
       - docs/development/vscode/index.md
       - docs/development/project_configuration.md
+      - AirStack CLI Tool:
+          - docs/development/airstack-cli/index.md
+          - docs/development/airstack-cli/extending.md
+          - docs/development/airstack-cli/architecture.md
       - Testing:
           - docs/development/testing/index.md
           - docs/development/testing/testing_frameworks.md
@@ -66,6 +70,7 @@ nav:
               - docs/robot/autonomy/0_interface/index.md
           - Sensors:
               - docs/robot/autonomy/1_sensors/index.md
+              - docs/robot/autonomy/1_sensors/gimbal.md              
           - Perception:
               - docs/robot/autonomy/2_perception/index.md
               - docs/robot/autonomy/2_perception/state_estimation.md
@@ -106,8 +111,10 @@ nav:
       - docs/ground_control_station/index.md
       - Usage:
           - docs/ground_control_station/usage/user_interface.md
-      - Multi-Robot:
-          - docs/ground_control_station/multi_robot/multi_robot.md
+      - Command Center:
+          - docs/ground_control_station/command_center/command_center.md
+      - Casualty Assessment:
+          - docs/ground_control_station/casualty_assessment/casualty_assessment.md
   - Simulation:
       - docs/simulation/index.md
       - Isaac Sim:
@@ -117,8 +124,11 @@ nav:
           - docs/simulation/isaac_sim/ascent_sitl_extension.md
   - Real World:
       - docs/real_world/index.md
+      - docs/real_world/data_offloading/index.md
       - Installation on Hardware:
           - docs/real_world/installation/index.md
+      - HITL Test:
+          - docs/real_world/HITL/index.md    
   - About: docs/about.md
 plugins:
   - search
diff --git a/plot/nx03_traj.png b/plot/nx03_traj.png
deleted file mode 100644
index d75137a2..00000000
Binary files a/plot/nx03_traj.png and /dev/null differ
diff --git a/plot/plot.py b/plot/plot.py
index 156b7cd7..4736400b 100755
--- a/plot/plot.py
+++ b/plot/plot.py
@@ -7,9 +7,12 @@
 import sys
 import math
 
+def get_time(stamp):
+    return stamp.sec + stamp.nanosec*1e-9
+
 def plot(bag_path, odom_topic_name, tracking_point_topic_name):
     reader = rosbag2_py.SequentialReader()
-    storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id='sqlite3')
+    storage_options = rosbag2_py.StorageOptions(uri=bag_path, storage_id='mcap')
     converter_options = rosbag2_py.ConverterOptions(input_serialization_format='', output_serialization_format='')
     reader.open(storage_options, converter_options)
 
@@ -17,45 +20,100 @@ def plot(bag_path, odom_topic_name, tracking_point_topic_name):
     type_map = {topic.name: topic.type for topic in topic_types}
     msg_type = get_message(type_map[odom_topic_name])
 
+    start_time = None
+    
     odoms = []
+    vels = []
+    odom_times = []
     tps = []
+    tp_times = []
     yaws = []
+
+    split_times = []
+    last_time = 0.
+    
     while reader.has_next():
-        topic, data, _ = reader.read_next()
+        topic, data, recorded_time = reader.read_next()
+        if start_time != None:
+            last_time = recorded_time/1e9 - start_time
         if topic == odom_topic_name:
             msg = deserialize_message(data, get_message(type_map[odom_topic_name]))
-            odoms.append([msg.pose.pose.position.x, msg.pose.pose.position.y])
+            odoms.append([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z])
+            vels.append([np.sqrt(msg.twist.twist.linear.x**2 + msg.twist.twist.linear.y**2 + msg.twist.twist.linear.z**2)])
+
+            t = get_time(msg.header.stamp)
+            if start_time == None:
+                start_time = t
+            odom_times.append(t - start_time)
             
             q = msg.pose.pose.orientation
             yaw = math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
             yaws.append(yaw)
         elif topic == tracking_point_topic_name:
             msg = deserialize_message(data, get_message(type_map[tracking_point_topic_name]))
-            tps.append([msg.pose.position.x, msg.pose.position.y])
+            tps.append([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
+
+            t = get_time(msg.header.stamp)
+            if start_time == None:
+                start_time = t
+            tp_times.append(t - start_time)
+        elif topic == fixed_trajectory_command_name:
+            split_times.append(last_time)
+    
+    if len(split_times) == 0:
+        split_times.append(last_time - 1.)
+    print(last_time)
             
 
     odoms = np.array(odoms)
     tps = np.array(tps)
-    
-    plt.figure()
-    plt.plot(odoms[:, 0], odoms[:, 1], marker='.', linestyle='-', label='odom')
-    plt.plot(tps[:, 0], tps[:, 1], marker='.', linestyle='-', label='tracking point')
-
-    for i, yaw in enumerate(yaws):
-        x = odoms[i, 0]
-        y = odoms[i, 1]
-        plt.arrow(x, y, 0.3 * math.cos(yaw), 0.3 * math.sin(yaw), head_width=0.05, head_length=0.05, fc='r', ec='r')
-    
-    plt.xlabel('X Position (m)')
-    plt.ylabel('Y Position (m)')
-    plt.axis('equal')
-    plt.legend()
-    plt.show()
+
+    prev_odom_split_index = 0
+    prev_tp_split_index = 0
+    fig_count = 0
+    for split_time in split_times:
+        fig = plt.figure()
+        ax1 = fig.add_subplot(111)
+        #ax2 = fig.add_subplot(212)
+
+        odom_start_index = prev_odom_split_index
+        odom_end_index = np.where(split_time < np.array(odom_times))[0][0]
+        tp_start_index = prev_tp_split_index
+        tp_end_index = np.where(split_time < np.array(tp_times))[0][0]
+        
+        fig.suptitle('Max Velocity: %0.02f m/s' % np.max(vels[odom_start_index:odom_end_index]))
+        
+        ax1.plot(odoms[odom_start_index:odom_end_index, 0], odoms[odom_start_index:odom_end_index, 1], marker='.', linestyle='-', label='odom')
+        ax1.plot(tps[tp_start_index:tp_end_index, 0], tps[tp_start_index:tp_end_index, 1], marker='.', linestyle='-', label='tracking point')
+
+        #ax2.plot(odom_times[odom_start_index:odom_end_index], odoms[odom_start_index:odom_end_index, 2], linestyle='-', label='odom')
+        #ax2.plot(tp_times[tp_start_index:tp_end_index], tps[tp_start_index:tp_end_index, 2], linestyle='-', label='tracking point')
+
+        for i, yaw in enumerate(yaws[odom_start_index:odom_end_index]):
+            x = odoms[i, 0]
+            y = odoms[i, 1]
+            #ax1.arrow(x, y, 0.3 * math.cos(yaw), 0.3 * math.sin(yaw), head_width=0.05, head_length=0.05, fc='r', ec='r')
+
+        ax1.set_xlabel('X Position (m)')
+        ax1.set_ylabel('Y Position (m)')
+        ax1.axis('equal')
+        ax1.legend()
+
+        #ax2.set_xlabel('time (seconds)')
+        #ax2.set_ylabel('Z Position (m)')
+        #ax2.legend()
+        fig.savefig(str(fig_count) + '.png')
+        fig_count += 1
+        plt.show()
+        
+        prev_odom_split_index = odom_end_index
+        prev_tp_split_index = tp_end_index
 
 if __name__ == "__main__":
     bag_path = sys.argv[1]
     namespace = '/' + sys.argv[2]
     odom_topic_name = namespace + '/odometry_conversion/odometry'
     tracking_point_topic_name = namespace + '/trajectory_controller/tracking_point'
+    fixed_trajectory_command_name = namespace + '/fixed_trajectory_generator/fixed_trajectory_command'
 
     plot(bag_path, odom_topic_name, tracking_point_topic_name)
diff --git a/robot/docker/Dockerfile.robot b/robot/docker/Dockerfile.robot
index 9c018655..22514ac5 100644
--- a/robot/docker/Dockerfile.robot
+++ b/robot/docker/Dockerfile.robot
@@ -80,6 +80,7 @@ RUN apt update -y && apt install -y \
     ros-humble-topic-tools \
     ros-humble-grid-map \
     ros-humble-domain-bridge \
+    ros-humble-rosbag2-storage-mcap \
     libcgal-dev \
     python3-colcon-common-extensions
 
@@ -115,7 +116,7 @@ RUN pip3 install \
     tabulate \
     einops \
     timm==0.9.12 \
-    rerun-sdk==0.17 \
+    rerun-sdk==0.22.0 \
     yacs \
     wandb
 
@@ -152,7 +153,9 @@ fi
 # Downloading model weights for MACVO
 WORKDIR /root/model_weights
 RUN wget -r "https://github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_FrontendCov.pth" && \ 
+    wget -r "https://github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_posenet.pkl" && \ 
     mv /root/model_weights/github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_FrontendCov.pth /root/model_weights/MACVO_FrontendCov.pth && \
+    mv /root/model_weights/github.com/MAC-VO/MAC-VO/releases/download/model/MACVO_posenet.pkl /root/model_weights/MACVO_posenet.pkl && \
     rm -rf /root/model_weights/github.com
 
 WORKDIR /root/ros_ws
diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml
index 635f100a..96703f05 100644
--- a/robot/docker/docker-compose.yaml
+++ b/robot/docker/docker-compose.yaml
@@ -10,7 +10,7 @@ services:
     extends:
       file: ./robot-base-docker-compose.yaml
       service: robot_base
-    image: &desktop_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_robot-x86-64
+    image: &desktop_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_robot-x86-64
     build:
       dockerfile: ./Dockerfile.robot
       args:
@@ -19,10 +19,10 @@ services:
         - *desktop_image
     # we use tmux send-keys so that the session stays alive
     command: >
-      bash -c "ssh service restart;
+      bash -c "service ssh restart;
       tmux new -d -s robot_bringup
       && tmux send-keys -t robot_bringup 
-      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
+      'bws && sws && 
       ros2 launch robot_bringup robot.launch.xml' ENTER
       && sleep infinity"
     # assumes you're connected to work internet, so creates a network to isolate from other developers on your work internet
@@ -33,16 +33,24 @@ services:
       # for ssh, starting from 2223-2243 on the host port all map to 22 in the container. Assumes no more than 21 robots
       - "2223-2243:22"
 
+  # -------------------------------------------------------------------------------------------------------------------
+  # dev container for developer debugging
+  robot-dev:
+    profiles: !override
+      - "dev"
+    extends: robot
+    command: sleep infinity
+
   # ===================================================================================================================
   # for running on an NVIIDA jetson (linux for tegra) device
-  robot_l4t:
+  robot-l4t:
     profiles:
       - hitl
       - deploy
     extends:
       file: ./robot-base-docker-compose.yaml
       service: robot_base
-    image: &l4t_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_robot-l4t
+    image: &l4t_image ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_robot-l4t
     build:
       dockerfile: ./Dockerfile.robot
       args:
@@ -51,15 +59,16 @@ services:
       tags:
         - *l4t_image
     # we use tmux send-keys so that the session stays alive
+    ipc: host
     command: >
       bash -c "ssh service restart;
       tmux new -d -s robot_bringup
       && tmux send-keys -t robot_bringup 
-      'if [ ! -f "/root/ros_ws/install/setup.bash" ]; then bws && sws; fi;
-      DATE=$(date | sed \"s/ /_/g\") ros2 launch robot_bringup robot.launch.xml sim:="false" ' ENTER
+      'bws && sws && 
+      DATE=$(date | sed \"s/ /_/g\" | sed \"s/:/_/g\") ros2 launch robot_bringup robot.launch.xml sim:="false" ' ENTER
       && sleep infinity"
     runtime: nvidia
     # assumes network isolation via a physical router, so uses network_mode=host
     network_mode: host
     volumes:
-      - $HOME/bags:/bags:rw
\ No newline at end of file
+      - /media/airlab/Storage/airstack_collection:/bags:rw
diff --git a/robot/docker/robot.env b/robot/docker/robot.env
index fe9021d3..807fc005 100644
--- a/robot/docker/robot.env
+++ b/robot/docker/robot.env
@@ -1,2 +1,2 @@
 # These become environment variables in the robot container
-USE_MACVO="false"
\ No newline at end of file
+USE_MACVO="true"
\ No newline at end of file
diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml
index 6301b0ea..1554948b 100644
--- a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml
+++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml
@@ -16,6 +16,7 @@
         <param name="interface" value="mavros_interface::MAVROSInterface" />
         <param name="is_ardupilot" value="true" />
         <param name="post_takeoff_command_delay_time" value="10." />
+	<param name="do_global_pose_command" value="false" />
 	
 	<remap from="ardupilot_takeoff" to="/$(env ROBOT_NAME)/takeoff_landing_planner/ardupilot_takeoff" />
     </node>
@@ -23,10 +24,11 @@
     <node pkg="mavros_interface" exec="position_setpoint_pub.py"
         namespace="interface"
         output="screen">
-
-      <param name="command_pose" value="False" />
-      <param name="max_velocity" value="1.0" />
+      
+      <param name="command_pose" value="false" />
+      <param name="max_velocity" value="3.0" />
       <param name="target_frame" value="base_link" />
+      <param name="publish_goal" value="false" />
       
     </node>
 
diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt
index 68c4b7fb..f1a33872 100644
--- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt
+++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt
@@ -10,16 +10,24 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)
 find_package(robot_interface REQUIRED)
 find_package(mavros_msgs REQUIRED)
+find_package(geographic_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(airstack_common REQUIRED)
 find_package(std_srvs REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib")
+find_package(GeographicLib REQUIRED)
 
 add_library(mavros_interface src/mavros_interface.cpp)
 target_compile_features(mavros_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
 target_include_directories(mavros_interface PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+  $<INSTALL_INTERFACE:include>
+  ${GeographicLib_INCLUDE_DIRS})
 
 ament_target_dependencies(
   mavros_interface
@@ -29,6 +37,7 @@ ament_target_dependencies(
   tf2
   airstack_common
   std_srvs
+  GeographicLib
 )
 
 pluginlib_export_plugin_description_file(robot_interface plugins.xml)
diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml
index bf85da15..c7459043 100644
--- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml
+++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml
@@ -12,12 +12,16 @@
   <depend>robot_interface</depend>
   <depend>mavros</depend>
   <depend>mavros_msgs</depend>
+  <depend>geographic_msgs</depend>
   <depend>vision_msgs</depend>
   <depend>ackerman_msgs</depend>
   <depend>pluginlib</depend>
   <depend>tf2</depend>
   <depend>airstack_common</depend>
   <depend>std_srvs</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py
index e82cfa3f..3217df53 100755
--- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py
+++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py
@@ -10,6 +10,7 @@
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
 from airstack_msgs.msg import Odometry
 from nav_msgs.msg import Odometry as NavOdometry
+from nav_msgs.msg import Path
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from scipy.spatial.transform import Rotation as R
 import numpy as np
@@ -36,19 +37,47 @@ def __init__(self):
 
         self.declare_parameter('target_frame', 'base_link')
         self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_value
+
+        self.declare_parameter('publish_goal', False)
+        self.publish_goal = self.get_parameter('publish_goal').get_parameter_value().bool_value
         
         self.tf_buffer = Buffer()
         self.tf_listener = TransformListener(self.tf_buffer, self)
         
         self.odom_subscriber = self.create_subscription(NavOdometry, "/" + os.getenv('ROBOT_NAME', "") + '/odometry_conversion/odometry', self.odom_callback, 1)
+        self.projected_pose_subsriber = self.create_subscription(PoseStamped, '/' + os.getenv('ROBOT_NAME', '') + '/trajectory_controller/projected_drone_pose', self.projected_pose_callback, 1)
 
         self.callback_group = MutuallyExclusiveCallbackGroup()
         self.tracking_point_subscriber = self.create_subscription(Odometry, "/" + os.getenv('ROBOT_NAME', "") + '/trajectory_controller/tracking_point', self.tracking_point_callback, 1, callback_group=self.callback_group)
         self.odom_publisher = self.create_publisher(PoseStamped, 'cmd_pose', 1)
         self.vel_publisher = self.create_publisher(TwistStamped, 'cmd_velocity', 1)
+        self.path_publisher = self.create_publisher(Path, "/" + os.getenv('ROBOT_NAME', "") + '/global_plan', 1)
+        self.cross_track_publisher = self.create_publisher(PoseStamped, "/" + os.getenv('ROBOT_NAME', "") + '/cross_track_error', 1)
 
         #self.odom_pos = [0., 0., 0.]
         self.odom = None
+        self.projected_pose = None
+
+        self.path = Path()
+        self.path.header.stamp = self.get_clock().now().to_msg()
+        self.path.header.frame_id = 'map'
+        pose1 = PoseStamped()
+        pose1.header = self.path.header
+        pose1.pose.position.x = 50.0
+        pose1.pose.position.y = 0.0
+        pose1.pose.position.z = 15.0
+        pose1.pose.orientation.w = 1.0
+        pose2 = PoseStamped()
+        pose2.header = self.path.header
+        pose2.pose.position.x = 60.0
+        pose2.pose.position.y = 0.0
+        pose2.pose.position.z = 15.0
+        pose2.pose.orientation.w = 1.0
+        self.path.poses.append(pose1)
+        self.path.poses.append(pose2)
+
+    def time_diff(self, t1, t2):
+        return (t1.sec + t1.nanosec*1e-9) - (t2.sec + t2.nanosec*1e-9)
 
     def get_yaw(self, q):
         yaw = R.from_quat([q.x, q.y, q.z, q.w])
@@ -56,10 +85,36 @@ def get_yaw(self, q):
         return yaw[2]
 
     def odom_callback(self, msg):
+        '''
+        if self.odom == None:
+            q = R.from_quat([msg.pose.pose.orientation.x,
+                             msg.pose.pose.orientation.y,
+                             msg.pose.pose.orientation.z,
+                             msg.pose.pose.orientation.w])
+            p0 = np.array([self.path.poses[0].pose.position.x,
+                           self.path.poses[0].pose.position.y,
+                           self.path.poses[0].pose.position.z])
+            p1 = np.array([self.path.poses[1].pose.position.x,
+                           self.path.poses[1].pose.position.y,
+                           self.path.poses[1].pose.position.z])
+            p0 = q.apply(p0)
+            p1 = q.apply(p1)
+
+            self.path.poses[0].pose.position.x = p0[0]
+            self.path.poses[0].pose.position.y = p0[1]
+            self.path.poses[0].pose.position.z = p0[2]
+            self.path.poses[1].pose.position.x = p1[0]
+            self.path.poses[1].pose.position.y = p1[1]
+            self.path.poses[1].pose.position.z = p1[2]
+        '''
         self.odom = msg
+        
         #self.odom_pos = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
         #self.odom_yaw = self.get_yaw(msg.pose.pose.orientation)
 
+    def projected_pose_callback(self, msg):
+        self.projected_pose = msg
+
     def get_tf(self, target_frame, source_frame, lookup_time, timeout=Duration(nanoseconds=0)):
         exception = False
         try:
@@ -85,6 +140,12 @@ def get_tf(self, target_frame, source_frame, lookup_time, timeout=Duration(nanos
     def tracking_point_callback(self, msg):
         if self.odom == None:
             return
+
+        if self.publish_goal:
+            self.path.header.stamp = self.get_clock().now().to_msg()
+            self.path.poses[0].header.stamp = self.path.header.stamp
+            self.path.poses[1].header.stamp = self.path.header.stamp
+            self.path_publisher.publish(self.path)
         
         if self.command_pose:
             out = PoseStamped()
@@ -118,6 +179,23 @@ def tracking_point_callback(self, msg):
             tracking_point_yaw = self.get_yaw(msg.pose.orientation)
             #self.get_logger().info('tp: ' + str(tracking_point_yaw) + ' ' + str(tracking_point_q.as_euler('xyz')[2]))
 
+            cross_track_x = 0.
+            cross_track_y = 0.
+            cross_track_z = 0.
+            if self.projected_pose != None and abs(self.time_diff(self.odom.header.stamp, self.projected_pose.header.stamp)) < 0.5:
+                #pose_t = self.get_tf(self.target_frame, self.projected_pose.header.frame_id, self.projected_pose.heaer.stamp)
+                cross_track_x = self.projected_pose.pose.position.x - self.odom.pose.pose.position.x
+                cross_track_y = self.projected_pose.pose.position.y - self.odom.pose.pose.position.y
+                cross_track_z = self.projected_pose.pose.position.z - self.odom.pose.pose.position.z
+            self.get_logger().info('cross track: ' + str(cross_track_x) + ' ' + str(cross_track_y) + ' ' + str(cross_track_z))
+            ct = PoseStamped()
+            ct.header = self.odom.header
+            ct.pose.position.x = cross_track_x
+            ct.pose.position.y = cross_track_y
+            ct.pose.position.z = cross_track_z
+            self.cross_track_publisher.publish(ct)
+                
+
             odom_yaw = self.get_yaw(self.odom.pose.pose.orientation)
             odom_t = self.get_tf(self.target_frame, self.odom.header.frame_id, self.odom.header.stamp)
             if odom_t == None:
@@ -139,9 +217,9 @@ def tracking_point_callback(self, msg):
             out = TwistStamped()
             out.header = msg.header
             out.header.frame_id = self.target_frame
-            out.twist.linear.x = vel[0]#msg.pose.position.x - self.odom_pos[0]
-            out.twist.linear.y = vel[1]#msg.pose.position.y - self.odom_pos[1]
-            out.twist.linear.z = vel[2]#msg.pose.position.z - self.odom_pos[2]
+            out.twist.linear.x = vel[0]# + cross_track_x#msg.pose.position.x - self.odom_pos[0]
+            out.twist.linear.y = vel[1]# + cross_track_y#msg.pose.position.y - self.odom_pos[1]
+            out.twist.linear.z = vel[2]*0.5# + cross_track_z#msg.pose.position.z - self.odom_pos[2]
             out.twist.angular.z = yawrate
             self.vel_publisher.publish(out)
 
diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp
index 73fd9203..ad2f2a89 100644
--- a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp
+++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp
@@ -21,19 +21,282 @@
 #include <mavros_msgs/msg/attitude_target.hpp>
 #include <mavros_msgs/msg/position_target.hpp>
 #include <mavros_msgs/msg/state.hpp>
+#include <mavros_msgs/msg/home_position.hpp>
+#include <geographic_msgs/msg/geo_pose_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
 #include <mavros_msgs/srv/command_bool.hpp>
 #include <mavros_msgs/srv/command_tol.hpp>
 #include <mavros_msgs/srv/set_mode.hpp>
 #include <std_srvs/srv/trigger.hpp>
 #include <robot_interface/robot_interface.hpp>
 
+#include "GeographicLib/Geoid.hpp"
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <message_filters/synchronizer.h>
+
 namespace mavros_interface {
 
+  void LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long,
+          double &UTMNorthing, double &UTMEasting, char* UTMZone);
+  void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
+          double& Lat, double& Long);
+  char UTMLetterDesignator(double Lat);
+  void LLtoSwissGrid(const double Lat, const double Long,
+          double &SwissNorthing, double &SwissEasting);
+  void SwissGridtoLL(const double SwissNorthing, const double SwissEasting,
+          double& Lat, double& Long);
+   
+  class Ellipsoid {
+  public:
+   
+      Ellipsoid() {
+      };
+   
+      Ellipsoid(int Id, const char* name, double radius, double ecc) {
+          id = Id;
+          ellipsoidName = name;
+          EquatorialRadius = radius;
+          eccentricitySquared = ecc;
+      }
+   
+      int id;
+      const char* ellipsoidName;
+      double EquatorialRadius;
+      double eccentricitySquared;
+   
+  };
+   
+ const double PI = 3.14159265;
+ const double FOURTHPI = PI / 4;
+ const double deg2rad = PI / 180;
+ const double rad2deg = 180.0 / PI;
+  
+ static Ellipsoid ellipsoid[] ={//  id, Ellipsoid name, Equatorial Radius, square of eccentricity    
+     Ellipsoid(-1, "Placeholder", 0, 0), //placeholder only, To allow array indices to match id numbers
+     Ellipsoid(1, "Airy", 6377563, 0.00667054),
+     Ellipsoid(2, "Australian National", 6378160, 0.006694542),
+     Ellipsoid(3, "Bessel 1841", 6377397, 0.006674372),
+     Ellipsoid(4, "Bessel 1841 (Nambia) ", 6377484, 0.006674372),
+     Ellipsoid(5, "Clarke 1866", 6378206, 0.006768658),
+     Ellipsoid(6, "Clarke 1880", 6378249, 0.006803511),
+     Ellipsoid(7, "Everest", 6377276, 0.006637847),
+     Ellipsoid(8, "Fischer 1960 (Mercury) ", 6378166, 0.006693422),
+     Ellipsoid(9, "Fischer 1968", 6378150, 0.006693422),
+     Ellipsoid(10, "GRS 1967", 6378160, 0.006694605),
+     Ellipsoid(11, "GRS 1980", 6378137, 0.00669438),
+     Ellipsoid(12, "Helmert 1906", 6378200, 0.006693422),
+     Ellipsoid(13, "Hough", 6378270, 0.00672267),
+     Ellipsoid(14, "International", 6378388, 0.00672267),
+     Ellipsoid(15, "Krassovsky", 6378245, 0.006693422),
+     Ellipsoid(16, "Modified Airy", 6377340, 0.00667054),
+     Ellipsoid(17, "Modified Everest", 6377304, 0.006637847),
+     Ellipsoid(18, "Modified Fischer 1960", 6378155, 0.006693422),
+     Ellipsoid(19, "South American 1969", 6378160, 0.006694542),
+     Ellipsoid(20, "WGS 60", 6378165, 0.006693422),
+     Ellipsoid(21, "WGS 66", 6378145, 0.006694542),
+     Ellipsoid(22, "WGS-72", 6378135, 0.006694318),
+     Ellipsoid(23, "WGS-84", 6378137, 0.00669438)
+ };
+
+ /*Reference ellipsoids derived from Peter H. Dana's website- 
+ http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
+ Department of Geography, University of Texas at Austin
+ Internet: pdana@mail.utexas.edu
+ 3/22/95
+  
+ Source
+ Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
+ 1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency
+  */
+  
+  
+  
+ void LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long,
+         double &UTMNorthing, double &UTMEasting, char* UTMZone) {
+     //converts lat/long to UTM coords.  Equations from USGS Bulletin 1532 
+     //East Longitudes are positive, West longitudes are negative. 
+     //North latitudes are positive, South latitudes are negative
+     //Lat and Long are in decimal degrees
+     //Written by Chuck Gantz- chuck.gantz@globalstar.com
+  
+     double a = ellipsoid[ReferenceEllipsoid].EquatorialRadius;
+     double eccSquared = ellipsoid[ReferenceEllipsoid].eccentricitySquared;
+     double k0 = 0.9996;
+  
+     double LongOrigin;
+     double eccPrimeSquared;
+     double N, T, C, A, M;
+  
+     //Make sure the longitude is between -180.00 .. 179.9
+     double LongTemp = (Long + 180) - int((Long + 180) / 360)*360 - 180; // -180.00 .. 179.9;
+  
+     double LatRad = Lat*deg2rad;
+     double LongRad = LongTemp*deg2rad;
+     double LongOriginRad;
+     int ZoneNumber;
+  
+     ZoneNumber = int((LongTemp + 180) / 6) + 1;
+  
+     if (Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0)
+         ZoneNumber = 32;
+  
+     // Special zones for Svalbard
+     if (Lat >= 72.0 && Lat < 84.0) {
+         if (LongTemp >= 0.0 && LongTemp < 9.0) ZoneNumber = 31;
+         else if (LongTemp >= 9.0 && LongTemp < 21.0) ZoneNumber = 33;
+         else if (LongTemp >= 21.0 && LongTemp < 33.0) ZoneNumber = 35;
+         else if (LongTemp >= 33.0 && LongTemp < 42.0) ZoneNumber = 37;
+     }
+     LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; //+3 puts origin in middle of zone
+     LongOriginRad = LongOrigin * deg2rad;
+  
+     //compute the UTM Zone from the latitude and longitude
+     sprintf(UTMZone, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
+  
+     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+  
+     N = a / sqrt(1 - eccSquared * sin(LatRad) * sin(LatRad));
+     T = tan(LatRad) * tan(LatRad);
+     C = eccPrimeSquared * cos(LatRad) * cos(LatRad);
+     A = cos(LatRad)*(LongRad - LongOriginRad);
+  
+     M = a * ((1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256) * LatRad
+             - (3 * eccSquared / 8 + 3 * eccSquared * eccSquared / 32 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(2 * LatRad)
+             + (15 * eccSquared * eccSquared / 256 + 45 * eccSquared * eccSquared * eccSquared / 1024) * sin(4 * LatRad)
+             - (35 * eccSquared * eccSquared * eccSquared / 3072) * sin(6 * LatRad));
+  
+     UTMEasting = (double) (k0 * N * (A + (1 - T + C) * A * A * A / 6
+             + (5 - 18 * T + T * T + 72 * C - 58 * eccPrimeSquared) * A * A * A * A * A / 120)
+             + 500000.0);
+  
+     UTMNorthing = (double) (k0 * (M + N * tan(LatRad)*(A * A / 2 + (5 - T + 9 * C + 4 * C * C) * A * A * A * A / 24
+             + (61 - 58 * T + T * T + 600 * C - 330 * eccPrimeSquared) * A * A * A * A * A * A / 720)));
+     if (Lat < 0)
+         UTMNorthing += 10000000.0; //10000000 meter offset for southern hemisphere
+ }
+  
+ char UTMLetterDesignator(double Lat) {
+     //This routine determines the correct UTM letter designator for the given latitude
+     //returns 'Z' if latitude is outside the UTM limits of 84N to 80S
+     //Written by Chuck Gantz- chuck.gantz@globalstar.com
+     char LetterDesignator;
+  
+     if ((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X';
+     else if ((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W';
+     else if ((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V';
+     else if ((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U';
+     else if ((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T';
+     else if ((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S';
+     else if ((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R';
+     else if ((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q';
+     else if ((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P';
+     else if ((8 > Lat) && (Lat >= 0)) LetterDesignator = 'N';
+     else if ((0 > Lat) && (Lat >= -8)) LetterDesignator = 'M';
+     else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
+     else if ((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
+     else if ((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
+     else if ((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
+     else if ((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
+     else if ((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
+     else if ((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
+     else if ((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
+     else if ((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
+     else LetterDesignator = 'Z'; //This is here as an error flag to show that the Latitude is outside the UTM limits
+  
+     return LetterDesignator;
+ }
+  
+ void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
+         double& Lat, double& Long) {
+     //converts UTM coords to lat/long.  Equations from USGS Bulletin 1532 
+     //East Longitudes are positive, West longitudes are negative. 
+     //North latitudes are positive, South latitudes are negative
+     //Lat and Long are in decimal degrees. 
+     //Written by Chuck Gantz- chuck.gantz@globalstar.com
+  
+     double k0 = 0.9996;
+     double a = ellipsoid[ReferenceEllipsoid].EquatorialRadius;
+     double eccSquared = ellipsoid[ReferenceEllipsoid].eccentricitySquared;
+     double eccPrimeSquared;
+     double e1 = (1 - sqrt(1 - eccSquared)) / (1 + sqrt(1 - eccSquared));
+     double N1, T1, C1, R1, D, M;
+     double LongOrigin;
+     double mu, phi1Rad;
+     //double phi1;
+     double x, y;
+     int ZoneNumber;
+     char* ZoneLetter;
+     //  int NorthernHemisphere; //1 for northern hemispher, 0 for southern
+  
+     x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
+     y = UTMNorthing;
+  
+     ZoneNumber = strtoul(UTMZone, &ZoneLetter, 10);
+     /*
+             if((*ZoneLetter - 'N') >= 0)
+                     NorthernHemisphere = 1;//point is in northern hemisphere
+             else
+             {
+                     NorthernHemisphere = 0;//point is in southern hemisphere
+                     y -= 10000000.0;//remove 10,000,000 meter offset used for southern hemisphere
+             }
+      */
+     if ((*ZoneLetter - 'N') < 0) {
+         y -= 10000000.0; //remove 10,000,000 meter offset used for southern hemisphere
+     }
+  
+     LongOrigin = (ZoneNumber - 1)*6 - 180 + 3; //+3 puts origin in middle of zone
+  
+     eccPrimeSquared = (eccSquared) / (1 - eccSquared);
+  
+     M = y / k0;
+     mu = M / (a * (1 - eccSquared / 4 - 3 * eccSquared * eccSquared / 64 - 5 * eccSquared * eccSquared * eccSquared / 256));
+  
+     phi1Rad = mu + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu)
+             + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu)
+             +(151 * e1 * e1 * e1 / 96) * sin(6 * mu);
+     // phi1 = phi1Rad*rad2deg;
+  
+     N1 = a / sqrt(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad));
+     T1 = tan(phi1Rad) * tan(phi1Rad);
+     C1 = eccPrimeSquared * cos(phi1Rad) * cos(phi1Rad);
+     R1 = a * (1 - eccSquared) / pow(1 - eccSquared * sin(phi1Rad) * sin(phi1Rad), 1.5);
+     D = x / (N1 * k0);
+  
+     Lat = phi1Rad - (N1 * tan(phi1Rad) / R1)*(D * D / 2 - (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * eccPrimeSquared) * D * D * D * D / 24
+             + (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * eccPrimeSquared - 3 * C1 * C1) * D * D * D * D * D * D / 720);
+     Lat = Lat * rad2deg;
+  
+     Long = (D - (1 + 2 * T1 + C1) * D * D * D / 6 + (5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * eccPrimeSquared + 24 * T1 * T1)
+             * D * D * D * D * D / 120) / cos(phi1Rad);
+     Long = LongOrigin + Long * rad2deg;
+  
+ }
+
+
+
+
+
+  
+
+
+
+
+
+
+
+
+  
+  
 class MAVROSInterface : public robot_interface::RobotInterface {
    private:
     // parameters
     bool is_ardupilot;
     float post_takeoff_command_delay_time;
+    bool do_global_pose_command;
 
     bool is_state_received_ = false;
     mavros_msgs::msg::State current_state_;
@@ -44,6 +307,10 @@ class MAVROSInterface : public robot_interface::RobotInterface {
     bool is_yaw_received_ = false;
     float yaw_ = 0.0;
 
+    bool is_home_received_ = false;
+    mavros_msgs::msg::HomePosition home_;
+    bool estimates_received_ = false;
+
     rclcpp::CallbackGroup::SharedPtr service_callback_group;
     rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;
     rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
@@ -51,16 +318,42 @@ class MAVROSInterface : public robot_interface::RobotInterface {
 
     rclcpp::Publisher<mavros_msgs::msg::AttitudeTarget>::SharedPtr attitude_target_pub_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr local_position_target_pub_;
-  rclcpp::Publisher<mavros_msgs::msg::PositionTarget>::SharedPtr velocity_target_pub_;
+    rclcpp::Publisher<geographic_msgs::msg::GeoPoseStamped>::SharedPtr global_position_target_pub_;
+    rclcpp::Publisher<mavros_msgs::msg::PositionTarget>::SharedPtr velocity_target_pub_;
+    rclcpp::Publisher<mavros_msgs::msg::HomePosition>::SharedPtr set_home_pub_;
 
     rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
+    rclcpp::Subscription<mavros_msgs::msg::HomePosition>::SharedPtr home_sub_;
     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr mavros_odometry_sub_;
+  
+    message_filters::Subscriber<nav_msgs::msg::Odometry> local_sub_;
+    message_filters::Subscriber<sensor_msgs::msg::NavSatFix> global_sub_;
+    std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ExactTime<
+						    nav_msgs::msg::Odometry, sensor_msgs::msg::NavSatFix>>> sync_;
+    nav_msgs::msg::Odometry local_estimate;
+    sensor_msgs::msg::NavSatFix global_estimate;
+  
+    std::shared_ptr<GeographicLib::Geoid> egm96_5;
 
    public:
     MAVROSInterface() : RobotInterface("mavros_interface") {
+        try {
+	  // Using smallest dataset with 5' grid,
+	  // From default location,
+	  // Use cubic interpolation, Thread safe
+	  egm96_5 = std::make_shared<GeographicLib::Geoid>("egm96-5", "", true, true);
+	} catch (const std::exception & e) {
+	  RCLCPP_ERROR_STREAM(this->get_logger(),
+			      "UAS: GeographicLib exception: %s " <<
+			      "| Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!" <<
+			      e.what());
+	  exit(1);
+	}
+      
         // params
         is_ardupilot = airstack::get_param(this, "is_ardupilot", false);
 	post_takeoff_command_delay_time = airstack::get_param(this, "post_takeoff_command_delay_time", 5.);
+	do_global_pose_command = airstack::get_param(this, "do_global_pose_command", false);
 
         // services
         service_callback_group =
@@ -77,17 +370,64 @@ class MAVROSInterface : public robot_interface::RobotInterface {
             "mavros/setpoint_raw/attitude", 1);
         local_position_target_pub_ = this->create_publisher<geometry_msgs::msg::PoseStamped>(
             "mavros/setpoint_position/local", 1);
+        global_position_target_pub_ = this->create_publisher<geographic_msgs::msg::GeoPoseStamped>(
+            "mavros/setpoint_position/global", 1);
 	velocity_target_pub_ = this->create_publisher<mavros_msgs::msg::PositionTarget>(
             "mavros/setpoint_raw/local", 1);
 
+	rclcpp::QoS qos_profile(1);
+	qos_profile.reliable(); 
+	set_home_pub_ = this->create_publisher<mavros_msgs::msg::HomePosition>(
+            "mavros/home_position/set", qos_profile);
+
         // subscribers
         state_sub_ = this->create_subscription<mavros_msgs::msg::State>(
             "mavros/state", 1,
             std::bind(&MAVROSInterface::state_callback, this, std::placeholders::_1));
+        home_sub_ = this->create_subscription<mavros_msgs::msg::HomePosition>(
+            "mavros/home_position/home", 1,
+            std::bind(&MAVROSInterface::home_callback, this, std::placeholders::_1));
+
+	rclcpp::QoS qos = rclcpp::SensorDataQoS();
+	local_sub_.subscribe(this, "mavros/global_position/local", qos.get_rmw_qos_profile());
+	global_sub_.subscribe(this, "mavros/global_position/global", qos.get_rmw_qos_profile());
+	/*
+	local_sub_.registerCallback([this](const nav_msgs::msg::Odometry::ConstSharedPtr &msg){
+				      RCLCPP_INFO(this->get_logger(), "local timestamp: %f",
+						  rclcpp::Time(msg->header.stamp).seconds());
+				    });
+	
+	global_sub_.registerCallback([this](const sensor_msgs::msg::NavSatFix::ConstSharedPtr &msg){
+				       RCLCPP_INFO(this->get_logger(), "global timestamp: %f",
+						   rclcpp::Time(msg->header.stamp).seconds());
+				     });
+	*/
+	using ApproxSyncPolicy = message_filters::sync_policies::ApproximateTime<
+	  nav_msgs::msg::Odometry,
+	  sensor_msgs::msg::NavSatFix
+	  >;
+
+	using ExactSyncPolicy = message_filters::sync_policies::ExactTime<
+	  nav_msgs::msg::Odometry,
+	  sensor_msgs::msg::NavSatFix
+	  >;
+
+	 sync_ = std::make_shared<message_filters::Synchronizer<ExactSyncPolicy>>(ExactSyncPolicy(100), local_sub_, global_sub_);
+	//sync_ = std::make_shared<message_filters::Synchronizer<ApproxSyncPolicy>>(ApproxSyncPolicy(10), local_sub_, global_sub_);
+
+	sync_->registerCallback(std::bind(&MAVROSInterface::local_global_callback, this, std::placeholders::_1, std::placeholders::_2));
+	
     }
 
     virtual ~MAVROSInterface() {}
 
+  void local_global_callback(const nav_msgs::msg::Odometry::ConstSharedPtr &local,
+			     const sensor_msgs::msg::NavSatFix::ConstSharedPtr &global){
+    estimates_received_ = true;
+    this->local_estimate = *local;
+    this->global_estimate = *global;
+  }
+
     // Control Callbacks. Translates commands to fit the MAVROS API.
     // The MAVROS API only has two types of control: Attitude Control and
     // Position Control.
@@ -154,10 +494,70 @@ class MAVROSInterface : public robot_interface::RobotInterface {
     }
 
     void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr cmd) override {
+      // TODO check if home_ has been set
         if (!is_ardupilot ||
             (in_air && ((this->get_clock()->now() - in_air_start_time).seconds() > post_takeoff_command_delay_time))) {
-            geometry_msgs::msg::PoseStamped cmd_copy = *cmd;
-            local_position_target_pub_->publish(cmd_copy);
+
+	  if(!do_global_pose_command){
+	    geometry_msgs::msg::PoseStamped cmd_copy = *cmd;
+	    local_position_target_pub_->publish(cmd_copy);
+	  }
+	  else{
+	    if(!estimates_received_)
+	      return;
+	    double utm_n, utm_e;
+	    char utm_z[8];
+	    LLtoUTM(23, global_estimate.latitude, global_estimate.longitude, utm_n, utm_e, utm_z);
+
+	    utm_e += cmd->pose.position.x - local_estimate.pose.pose.position.x;
+	    utm_n += cmd->pose.position.y - local_estimate.pose.pose.position.y;
+	  
+	    double lat, lon;
+	    UTMtoLL(23, utm_n, utm_e, utm_z, lat, lon);
+	  
+	    double alt = global_estimate.altitude + cmd->pose.position.z - local_estimate.pose.pose.position.z +
+	      egm96_5->ConvertHeight(lat, lon, 0.0, GeographicLib::Geoid::ELLIPSOIDTOGEOID);
+
+	    geographic_msgs::msg::GeoPoseStamped msg;
+	    msg.pose.position.latitude = lat;
+	    msg.pose.position.longitude = lon;
+	    msg.pose.position.altitude = alt;
+	    msg.pose.orientation = cmd->pose.orientation;
+
+	    global_position_target_pub_->publish(msg);
+	  
+	  
+	    /*
+	      double alt_offset = egm96_5->ConvertHeight(home_.geo.latitude, home_.geo.longitude, 0.0,
+	      GeographicLib::Geoid::ELLIPSOIDTOGEOID);
+
+	      double utm_n, utm_e;
+	      char utm_z[8];
+	      LLtoUTM(23, home_.geo.latitude, home_.geo.longitude, utm_n, utm_e, utm_z);
+
+	      tf2::Quaternion q(home_.orientation.x, home_.orientation.y, home_.orientation.z, home_.orientation.w);
+	      tf2::Vector3 v(cmd->pose.position.x, cmd->pose.position.y, 0.);
+	      v = tf2::quatRotate(q.inverse(), v);
+	    
+	      utm_n += v.x();//cmd->pose.position.y;
+	      utm_e += v.y();//cmd->pose.position.x;
+	      double alt = home_.geo.altitude + alt_offset + cmd->pose.position.z;
+
+	      double lat, lon;
+	      UTMtoLL(23, utm_n, utm_e, utm_z, lat, lon);
+
+	      RCLCPP_INFO_STREAM(this->get_logger(), "home: " << home_.geo.latitude << " " << home_.geo.longitude << " "
+	      << utm_n << " " << utm_e << " " << utm_z << " " << lat << " " << lon);
+
+	      geographic_msgs::msg::GeoPoseStamped msg;
+	      msg.pose.position.latitude = lat;
+	      msg.pose.position.longitude = lon;
+	      msg.pose.position.altitude = alt;
+	      msg.pose.orientation = home_.orientation;
+
+	      global_position_target_pub_->publish(msg);
+	    */
+	  }
         }
     }
 
@@ -235,6 +635,21 @@ class MAVROSInterface : public robot_interface::RobotInterface {
         is_state_received_ = true;
         current_state_ = *msg;
     }
+
+    void home_callback(const mavros_msgs::msg::HomePosition::SharedPtr msg) {
+        home_ = *msg;
+
+	if(!is_home_received_){
+	  home_.orientation.x = 0.;
+	  home_.orientation.y = 0.;
+	  home_.orientation.z = 0.;
+	  home_.orientation.w = 1.;
+	  
+	  set_home_pub_->publish(home_);
+	}
+	
+        is_home_received_ = true;
+    }
 };
 }  // namespace mavros_interface
 #include <pluginlib/class_list_macros.hpp>
diff --git a/robot/ros_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py b/robot/ros_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py
index 2bfe5e72..c923f44e 100644
--- a/robot/ros_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py
+++ b/robot/ros_ws/src/autonomy/1_sensors/gimbal_stabilizer/gimbal_stabilizer/gimbal_stabilizer_node.py
@@ -4,25 +4,38 @@
 from rclpy.node import Node
 from nav_msgs.msg import Odometry
 from sensor_msgs.msg import JointState
-from transforms3d.euler import quat2euler
+# from transforms3d.euler import quat2euler
+from std_msgs.msg import Float64  # Assuming the desired yaw is published as a Float64
 
 class GimbalStabilizerNode(Node):
     def __init__(self):
         super().__init__('gimbal_stabilizer')
         
         # Publisher to send joint commands
-        self.joint_pub = self.create_publisher(JointState, '/joint_command', 10)
+        self.joint_pub = self.create_publisher(JointState, 'gimbal/joint_command', 10)
         
         # Subscriber to receive drone odometry
-        self.create_subscription(Odometry, '/robot_1/odometry_conversion/odometry', self.odometry_callback, 10)
-        self.create_subscription(JointState, '/joint_states', self.joint_callback, 10)
+        self.create_subscription(Odometry, 'odometry_conversion/odometry', self.odometry_callback, 10)
+        self.create_subscription(JointState, 'gimbal/joint_states', self.joint_callback, 10)
+        self.create_subscription(Float64, 'gimbal/desired_gimbal_yaw', self.yaw_callback, 10)
+        self.create_subscription(Float64, 'gimbal/desired_gimbal_pitch', self.pitch_callback, 10)
 
         # Initialize joint state message
         self.joint_command = JointState()
         self.joint_command.name = ["yaw_joint","roll_joint", "pitch_joint"]
         self.joint_command.position = [0.0, 0.0, 0.0]
+        self.desired_yaw = 0.0
+        self.desired_pitch = 0.0
+
+    def yaw_callback(self, msg):
+        self.desired_yaw = msg.data
+        # self.get_logger().info(f"Received desired yaw angle: {self.desired_yaw}")
+
+    def pitch_callback(self, msg):
+        self.desired_pitch = msg.data
 
     def joint_callback(self, msg):
+        self.got_joint_states = True
         # Inverse the drone angles to stabilize the gimbal
         # self.joint_command.position[0] = -roll  # roll joint
         # self.joint_command.position[1] = -pitch  # pitch joint
@@ -33,7 +46,7 @@ def joint_callback(self, msg):
         # self.joint_command.position[0] = -20.0/180*3.14  # yaw joint
         # self.joint_command.position[1] = 10.0/180*3.14  # roll joint
         # self.joint_command.position[2] = 20.0/180*3.14  # pitch joint
-        self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
+        # self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
         # self.joint_command.velocity = [-1.0, -1.0, -1.0]
 
         # Publish the joint command
@@ -50,18 +63,18 @@ def odometry_callback(self, msg):
         ]
 
         # Convert quaternion to Euler angles (roll, pitch, yaw)
-        roll, pitch, yaw = quat2euler(quaternion, axes='sxyz')
+        # roll, pitch, yaw = quat2euler(quaternion, axes='sxyz')
         
         # Inverse the drone angles to stabilize the gimbal
         # self.joint_command.position[0] = -roll  # roll joint
         # self.joint_command.position[1] = -pitch  # pitch joint
         # self.joint_command.position[2] = -yaw  # yaw joint
 
-        self.joint_command.position[0] = -0.0/180*3.14  # yaw joint
-        self.joint_command.position[1] = -roll  # roll joint
-        self.joint_command.position[2] = 0.0/180*3.14  # pitch joint
+        self.joint_command.position[0] = -self.desired_yaw/180*3.14  # yaw joint
+        self.joint_command.position[1] = -0.0/180*3.14  # roll joint
+        self.joint_command.position[2] = self.desired_pitch/180*3.14  # pitch joint
         self.joint_command.velocity = [float('nan'), float('nan'), float('nan')]
-        self.joint_command.velocity = [-1.0, -1.0, -1.0]
+        # self.joint_command.velocity = [-1.0, -1.0, -1.0]
 
         # Publish the joint command
         self.joint_pub.publish(self.joint_command)
@@ -78,4 +91,4 @@ def main():
         rclpy.shutdown()
 
 if __name__ == '__main__':
-    main()
+    main()
\ No newline at end of file
diff --git a/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml
index d5976207..ff55ac15 100644
--- a/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml
+++ b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml
@@ -4,4 +4,7 @@
         <push-ros-namespace namespace="sensors"/>
         <include file="$(find-pkg-share camera_param_server)/launch/camera_param_server.launch.xml"/>
     </group>
+    <node
+        pkg="gimbal_stabilizer" exec="gimbal_stabilizer_node" name="gimbal_stabilizer" output="screen">
+    </node>    
 </launch>
\ No newline at end of file
diff --git a/robot/ros_ws/src/autonomy/2_perception/macvo/config/rviz_macvo.rviz b/robot/ros_ws/src/autonomy/2_perception/macvo/config/rviz_macvo.rviz
index 5c905f3d..9e873c28 100644
--- a/robot/ros_ws/src/autonomy/2_perception/macvo/config/rviz_macvo.rviz
+++ b/robot/ros_ws/src/autonomy/2_perception/macvo/config/rviz_macvo.rviz
@@ -59,7 +59,7 @@ Visualization Manager:
       Name: Pose
       Shaft Length: 1
       Shaft Radius: 0.05000000074505806
-      Shape: Arrow
+      Shape: Axes
       Topic:
         Depth: 5
         Durability Policy: Volatile
diff --git a/robot/ros_ws/src/autonomy/2_perception/macvo2 b/robot/ros_ws/src/autonomy/2_perception/macvo2
new file mode 160000
index 00000000..d25f1406
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/2_perception/macvo2
@@ -0,0 +1 @@
+Subproject commit d25f1406e092d66c5938788821706db27506c853
diff --git a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml
index e5399138..31883577 100644
--- a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml
+++ b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml
@@ -4,7 +4,7 @@
     <!-- State Estimation -->
     <!-- Visual Odometry -->
 
-    <node pkg="macvo" exec="macvo" if="$(env USE_MACVO)">
+    <!-- <node pkg="macvo" exec="macvo" if="$(env USE_MACVO)">
         <param name="camera_config" value="$(find-pkg-share macvo)/config/model_config.yaml" />
         <param from="$(find-pkg-share macvo)/config/interface_config.yaml" />
         <remap from="left/image_rect"
@@ -18,6 +18,22 @@
         <remap from="visual_odometry_points"
             to="/$(env ROBOT_NAME)/perception/visual_odometry_points" />
         <remap from="visual_odometry_img" to="/$(env ROBOT_NAME)/perception/visual_odometry_img" />
+    </node> -->
+    <node pkg="macvo2" exec="macvo2" if="$(env USE_MACVO)">
+        <param name="camera_config" value="$(find-pkg-share macvo2)/config/model_config.yaml" />
+        <param from="$(find-pkg-share macvo2)/config/interface_config.yaml" />
+        <remap from="left/image_rect"
+            to="/$(env ROBOT_NAME)/sensors/front_stereo/left/image_rect" />
+        <remap from="right/image_rect"
+            to="/$(env ROBOT_NAME)/sensors/front_stereo/right/image_rect" />
+        <remap from="/sensors/get_camera_params"
+            to="/$(env ROBOT_NAME)/sensors/get_camera_params" />
+        <remap from="visual_odometry_pose"
+            to="/$(env ROBOT_NAME)/perception/visual_odometry_pose" />
+        <remap from="visual_odometry_points"
+            to="/$(env ROBOT_NAME)/perception/visual_odometry_points" />
+        <remap from="visual_odometry_img"
+            to="/$(env ROBOT_NAME)/perception/visual_odometry_img" />
     </node>
 
     <!-- Depth Estimation -->
diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml
index e3a7881a..6443a3db 100644
--- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml
+++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml
@@ -1,14 +1,12 @@
 /**:
   ros__parameters:
     # parameters
-    scale: 1.0  # units size of a meter. depth value corresponds to this many meters
-    downsample_scale: 1.0  # the ratio to scale down the disparity image before processing
+    metric_depth_scale: 1.0  # units size of a meter. depth value corresponds to this many meters
+    downsample_scale: 0.5  # the ratio to scale down the disparity image before processing. DROAN expects 960x600. So if the incoming image is 480x300, set this to 0.5 so that it scales up
     baseline_fallback: 0.12 # if the baseline is 0 from the camera_info, use this value instead
     lut_max_disparity: 180  # maximum disparity value in the disparity image
     # expansion_radius: 0.325  # 0.325 is the spirit drone blade to blade
     expansion_radius: 0.1  # debug hack to make disparity expansion not crash
     bg_multiplier: 1.0
     # sensor_pixel_error: 0.5 # what is this? and why was it 0.5?
-    sensor_pixel_error: 0.0
-    
-  
\ No newline at end of file
+    sensor_pixel_error: 0.0
\ No newline at end of file
diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp
index 0c7ee03b..0403fa8a 100644
--- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp
+++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp
@@ -62,7 +62,7 @@ class DisparityExpansionNode : public rclcpp::Node {
     double padding;
     double bg_multiplier;
     double pixel_error;
-    double scale;
+    double metric_depth_scale;
     std::vector<std::vector<LUTCell>> table_u;
     std::vector<std::vector<LUTCell>> table_v;
     std::vector<double> table_d;
diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp
index 487ced12..410e7435 100755
--- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp
+++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp
@@ -10,8 +10,8 @@
 
 DisparityExpansionNode::DisparityExpansionNode(const rclcpp::NodeOptions& options)
     : Node("DisparityExpansionNode", options) {
-    this->declare_parameter("scale", 2.0);
-    this->get_parameter("scale", this->scale);
+    this->declare_parameter("metric_depth_scale", 2.0);
+    this->get_parameter("metric_depth_scale", this->metric_depth_scale);
     this->declare_parameter("expansion_radius", 2.0);
     this->get_parameter("expansion_radius", this->expansion_radius);
     this->declare_parameter("lut_max_disparity", 164);
@@ -73,7 +73,7 @@ void DisparityExpansionNode::set_cam_info(
             "Baseline from camera_info was 0, setting to this->baseline_fallback:"
                 << this->baseline);
     }
-    this->baseline *= this->downsample_scale;
+    // this->baseline *= this->downsample_scale;
     this->generate_expansion_lookup_table();
     this->got_cam_info = true;
     RCLCPP_INFO(this->get_logger(), "Baseline: %.4f meters", this->baseline);
@@ -98,8 +98,8 @@ void DisparityExpansionNode::generate_expansion_lookup_table() {
     double disparity;
 
     for (unsigned int disp_idx = 1; disp_idx < lut_max_disparity; disp_idx++) {
-        disparity = disp_idx / this->scale;  // 1 cell = 0.5m, z is in meters
-        r = this->expansion_radius;          // * exp(DEPTH_ERROR_COEFF*z);
+        disparity = disp_idx / this->metric_depth_scale;  // 1 cell = 0.5m, z is in meters
+        r = this->expansion_radius;                       // * exp(DEPTH_ERROR_COEFF*z);
         z = this->baseline * this->fx / disparity;
 
         double disp_new = this->baseline * this->fx / (z - this->expansion_radius) + 0.5;
@@ -117,7 +117,11 @@ void DisparityExpansionNode::generate_expansion_lookup_table() {
             v2 = this->fy * r2 / z + this->cy;
 
             if ((v2 - v1) < 0)
-                RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", v1, v2, disp_idx);
+                RCLCPP_ERROR(this->get_logger(),
+                             "Something MESSED disp_idx=%d v=%d cy=%f fy=%f r=%f x=%f y=%f z=%f "
+                             "beta=%f beta1=%f r1=%f r2=%f v1=%d v2=%d",
+                             disp_idx, v, this->cy, this->fy, r, x, y, z, beta, beta1, r1, r2, v1,
+                             v2);
 
             if (v1 < 0) v1 = 0;
             if (v1 > (height - 1)) v1 = height - 1;
@@ -141,7 +145,11 @@ void DisparityExpansionNode::generate_expansion_lookup_table() {
             u2 = this->fx * r2 / z + this->cx;
 
             if ((u2 - u1) < 0)
-                RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", u1, u2, disp_idx);
+                RCLCPP_ERROR(this->get_logger(),
+                             "Something MESSED disp_idx=%d u=%d cx=%f fx=%f r=%f x=%f y=%f z=%f "
+                             "alpha=%f alpha1=%f r1=%f r2=%f u1=%d u2=%d",
+                             disp_idx, u, this->cx, this->fx, r, x, y, z, alpha, alpha1, r1, r2, u1,
+                             u2);
 
             if (u1 < 0) u1 = 0;
             if (u1 > (this->width - 1)) u1 = this->width - 1;
@@ -281,19 +289,22 @@ void DisparityExpansionNode::process_disparity_image(
         for (int u = (int)this->width - 1; u >= 0; u -= 1) {
             float disparity_value = disparity32F.at<float>(v, u);
 
-            if (std::isnan(double(disparity_value * this->scale)) ||
-                ((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) ||
-                ((int(disparity_value * this->scale) + 1) <= 0)) {
+            if (std::isnan(double(disparity_value * this->metric_depth_scale)) ||
+                ((int(disparity_value * this->metric_depth_scale) + 1) >=
+                 this->lut_max_disparity) ||
+                ((int(disparity_value * this->metric_depth_scale) + 1) <= 0)) {
                 RCLCPP_INFO_STREAM_THROTTLE(
                     this->get_logger(), *this->get_clock(), 5000,
                     "Step 1 Expand u: Disparity out of range: "
                         << disparity_value << ", lut_max_disparity: " << this->lut_max_disparity
-                        << " Scale: " << this->scale);
+                        << " metric_depth_scale: " << this->metric_depth_scale);
                 continue;
             }
 
-            unsigned int u1 = this->table_u.at(int(disparity_value * this->scale) + 1).at(u).idx1;
-            unsigned int u2 = this->table_u.at(int(disparity_value * this->scale) + 1).at(u).idx2;
+            unsigned int u1 =
+                this->table_u.at(int(disparity_value * this->metric_depth_scale) + 1).at(u).idx1;
+            unsigned int u2 =
+                this->table_u.at(int(disparity_value * this->metric_depth_scale) + 1).at(u).idx2;
 
             if (disparity32F.empty()) {
                 RCLCPP_ERROR(this->get_logger(), "disparity32F matrix is empty.");
@@ -365,19 +376,22 @@ void DisparityExpansionNode::process_disparity_image(
             float disparity_value = disparity32F.at<float>(v, u) +
                                     this->pixel_error;  // disparity_row[v * row_step + u];// + 0.5;
 
-            if (std::isnan(double(disparity_value * this->scale)) ||
-                ((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) ||
-                ((int(disparity_value * this->scale) + 1) <= 0)) {
+            if (std::isnan(double(disparity_value * this->metric_depth_scale)) ||
+                ((int(disparity_value * this->metric_depth_scale) + 1) >=
+                 this->lut_max_disparity) ||
+                ((int(disparity_value * this->metric_depth_scale) + 1) <= 0)) {
                 RCLCPP_INFO_STREAM_THROTTLE(
                     this->get_logger(), *this->get_clock(), 5000,
                     "Step 2 Expand v: Disparity out of range: "
                         << disparity_value << ", lut_max_disparity: " << this->lut_max_disparity
-                        << " Scale: " << this->scale);
+                        << " metric_depth_scale: " << this->metric_depth_scale);
                 continue;
             }
 
-            unsigned int v1 = this->table_v.at(int(disparity_value * this->scale) + 1).at(v).idx1;
-            unsigned int v2 = this->table_v.at(int(disparity_value * this->scale) + 1).at(v).idx2;
+            unsigned int v1 =
+                this->table_v.at(int(disparity_value * this->metric_depth_scale) + 1).at(v).idx1;
+            unsigned int v2 =
+                this->table_v.at(int(disparity_value * this->metric_depth_scale) + 1).at(v).idx2;
 
             cv::Rect roi = cv::Rect(u, v1, 1, (v2 - v1));
 
diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp
index f97a6a0d..00f11548 100644
--- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp
+++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp
@@ -41,11 +41,16 @@ class DisparityGraph {
     std::string sensor_frame, fixed_frame, stabilized_frame;
     visualization_msgs::msg::Marker marker_;
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr disparity_graph_marker_pub_;
+    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr virtual_obstacles_marker_pub_;
     double angle_tol, displacement_tol, assume_seen_within_radius;
     bool first;
     bool got_cam_info;
     double fx_, fy_, cx_, cy_, baseline_, downsample_scale;
     unsigned int width_, height_;
+  
+    bool everything_seen_and_free;
+    bool use_virtual_cylinder;
+    double virtual_cylinder_x, virtual_cylinder_y, virtual_cylinder_radius;
 
     std::mutex io_mutex;
 
@@ -77,6 +82,8 @@ class DisparityGraph {
         this->tf_buffer_ptr = tf_buffer_ptr;
         disparity_graph_marker_pub_ =
             node_ptr->create_publisher<visualization_msgs::msg::MarkerArray>("disparity_graph", 10);
+        virtual_obstacles_marker_pub_ =
+	  node_ptr->create_publisher<visualization_msgs::msg::MarkerArray>("virtual_obstacles", 10);
 
         node_ptr->declare_parameter("baseline_fallback", 0.5);
 
@@ -97,6 +104,19 @@ class DisparityGraph {
         node_ptr->declare_parameter("angle_tolerance", 30.0);
         angle_tol = node_ptr->get_parameter("angle_tolerance").as_double() * M_PI / 180.0;
 
+	node_ptr->declare_parameter<bool>("everything_seen_and_free", false);
+	node_ptr->get_parameter("everything_seen_and_free", this->everything_seen_and_free);
+
+	// virtual obstacle parameters
+	node_ptr->declare_parameter<bool>("use_virtual_cylinder", false);
+	node_ptr->get_parameter("use_virtual_cylinder", this->use_virtual_cylinder);
+	node_ptr->declare_parameter<double>("virtual_cylinder_x", 0.0);
+	node_ptr->get_parameter("virtual_cylinder_x", this->virtual_cylinder_x);
+	node_ptr->declare_parameter<double>("virtual_cylinder_y", 0.0);
+	node_ptr->get_parameter("virtual_cylinder_y", this->virtual_cylinder_y);
+	node_ptr->declare_parameter<double>("virtual_cylinder_radius", 0.0);
+	node_ptr->get_parameter("virtual_cylinder_radius", this->virtual_cylinder_radius);
+
         node_ptr->declare_parameter("expanded_disparity_fg_topic", "expanded_disparity_fg");
         disp_fg_sub_.subscribe(node_ptr,
                                node_ptr->get_parameter("expanded_disparity_fg_topic").as_string());
@@ -353,6 +373,18 @@ class DisparityGraph {
         // RCLCPP_INFO_STREAM(node_ptr->get_logger(),"occupancy:" << occupancy << " is_free: " <<
         // is_free << " is_seen: " << is_seen);
 
+	if(everything_seen_and_free){
+	  is_seen = true;
+	  is_free = true;
+	}
+
+	if(use_virtual_cylinder){
+	  tf2::Vector3 p(world_point_stamped.point.x, world_point_stamped.point.y, 0.);
+	  tf2::Vector3 c(virtual_cylinder_x, virtual_cylinder_y, 0.);
+
+	  is_free = p.distance(c) >= virtual_cylinder_radius;
+	}
+
         return {is_seen, is_free, occupancy};
     }
 
@@ -408,6 +440,43 @@ class DisparityGraph {
         return true;
     }
 
+  void virtual_cylinder_viz(){
+    if(!use_virtual_cylinder)
+      return;
+    visualization_msgs::msg::MarkerArray marker_array;
+
+    visualization_msgs::msg::Marker cylinder;
+    cylinder.header.frame_id = this->fixed_frame;  // Change to your reference frame
+    cylinder.header.stamp = node_ptr->now();
+    cylinder.ns = "virtual_obstacle";
+    cylinder.id = 0;
+    cylinder.type = visualization_msgs::msg::Marker::CYLINDER;
+    cylinder.action = visualization_msgs::msg::Marker::ADD;
+
+    // Position
+    cylinder.pose.position.x = this->virtual_cylinder_x;
+    cylinder.pose.position.y = this->virtual_cylinder_y;
+    cylinder.pose.position.z = 0.0;  // Half height for proper placement on ground
+    cylinder.pose.orientation.w = 1.0;
+
+    // Size
+    cylinder.scale.x = this->virtual_cylinder_radius*2.;  // Diameter in x
+    cylinder.scale.y = this->virtual_cylinder_radius*2.;  // Diameter in y
+    cylinder.scale.z = 100.;  // Height
+
+    // Color
+    cylinder.color.r = 1.0f;
+    cylinder.color.g = 0.0f;
+    cylinder.color.b = 0.0f;
+    cylinder.color.a = 6.0f;
+
+    cylinder.lifetime = rclcpp::Duration::from_seconds(0);  // Forever
+
+    marker_array.markers.push_back(cylinder);
+
+    virtual_obstacles_marker_pub_->publish(marker_array);
+  }
+
     void clear_graph(void) {
         std::lock_guard<std::mutex> lock(io_mutex);
         disp_graph_.clear();
diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp
index cfd0d4c6..7d71c48a 100644
--- a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp
+++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/src/disparity_graph_cost_map.cpp
@@ -88,6 +88,8 @@ std::vector<std::vector<double> > DisparityGraphCostMap::get_trajectory_costs_pe
     marker_array.markers.clear();
     points_marker.points.clear();
     points_marker.colors.clear();
+    
+    disp_graph.virtual_cylinder_viz();
 
     // initialize cost_values to 0
     std::vector<std::vector<double> > cost_values(trajectories.size());
diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml
index a65daa22..a6f59821 100644
--- a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml
+++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml
@@ -27,6 +27,12 @@
     stabilized_frame: "base_link_stabilized"  # the frame of the stabilized robot (roll and pitch removed)
     baseline_fallback: 0.12  # if the baseline is 0 from the camera_info, use this value instead. in meters
     assume_seen_within_radius: 1.0  # anything within this radius, assume it is seen and don't check for obstacles
+    
+    everything_seen_and_free: false
+    use_virtual_cylinder: false
+    virtual_cylinder_x: 10.0
+    virtual_cylinder_y: 0.0
+    virtual_cylinder_radius: 4.0
 
     # GLOBAL_PLAN mode parameters
     # 0: use the yaw of the subscribed traj, 1: smoothly vary the yaw in the direction of the subscribed trajectory
diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py
index f724941d..da309067 100755
--- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py
+++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py
@@ -52,6 +52,49 @@ def get_velocities_dual(traj, velocity, max_acc):
 
     traj.waypoints[len(traj.waypoints) - 1].velocity = 0.0
 
+def get_velocities_adjust(traj, max_acc):
+    slow_down_adjust = []
+    speed_up_adjust = []
+    
+    for i in range(len(traj.waypoints)):
+        if i != len(traj.waypoints)-1:
+            if traj.waypoints[i].velocity > traj.waypoints[i+1].velocity:
+                v_start = traj.waypoints[i+1].velocity
+                v_target = traj.waypoints[i].velocity
+                slow_down_adjust.append((i+1, v_start, v_target))
+            elif traj.waypoints[i].velocity < traj.waypoints[i+1].velocity:
+                v_start = traj.waypoints[i].velocity
+                v_target = traj.waypoints[i+1].velocity
+                speed_up_adjust.append((i, v_start, v_target))
+
+    for start_index, v_start, v_target in slow_down_adjust:
+        prev_i = start_index
+        v_prev = v_start
+        for i in range(start_index-1, -1, -1):
+            dx = traj.waypoints[prev_i].position.x - traj.waypoints[i].position.x
+            dy = traj.waypoints[prev_i].position.y - traj.waypoints[i].position.y
+            dist = np.sqrt(dx**2 + dy**2)
+            v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
+            traj.waypoints[i].velocity = min(v_target, v_limit)
+            v_prev = traj.waypoints[i].velocity
+            if v_prev >= v_target:
+                break
+
+    for start_index, v_start, v_target in speed_up_adjust:
+        prev_i = start_index
+        v_prev = v_start
+        for i in range(start_index+1, len(traj.waypoints)):
+            dx = traj.waypoints[prev_i].position.x - traj.waypoints[i].position.x
+            dy = traj.waypoints[prev_i].position.y - traj.waypoints[i].position.y
+            dist = np.sqrt(dx**2 + dy**2)
+            v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist)
+            if v_limit > traj.waypoints[i].velocity:
+                break
+            traj.waypoints[i].velocity = min(v_target, v_limit)
+            v_prev = traj.waypoints[i].velocity
+            if v_prev >= v_target:
+                break
+
 
 def get_accelerations(traj):
     a_prev = 0.0
@@ -74,7 +117,7 @@ def get_accelerations(traj):
         v_nextx = traj.waypoints[j].velocity * (dx2) / dist2
         v_nexty = traj.waypoints[j].velocity * (dy2) / dist2
 
-        acc_limit = 50
+        acc_limit = 50.
         if abs(dx1 - 0) < 1e-6:
             traj.waypoints[i].acceleration.x = 0.0
         else:
@@ -105,6 +148,7 @@ def get_racetrack_waypoints(attributes):  # length, width, height):
     width = float(attributes["width"])
     height = float(attributes["height"])
     velocity = float(attributes["velocity"])
+    turn_velocity = float(attributes["turn_velocity"])
     max_acceleration = float(attributes["max_acceleration"])
 
     traj = TrajectoryXYZVYaw()
@@ -114,6 +158,7 @@ def get_racetrack_waypoints(attributes):  # length, width, height):
     # first straightaway
     xs1 = np.linspace(0, straightaway_length, 80)
     ys1 = np.zeros(xs1.shape)
+    vs1 = np.ones(xs1.shape)*velocity
     yaws1 = np.zeros(xs1.shape)
 
     # first turn
@@ -122,21 +167,27 @@ def get_racetrack_waypoints(attributes):  # length, width, height):
     ys2 = width / 2.0 * np.sin(t) + width / 2.0
     xs2d = -width / 2.0 * np.sin(t)  # derivative of xs
     ys2d = width / 2.0 * np.cos(t)  # derivative of ys
+    vs2 = np.ones(xs2.shape)*turn_velocity
     yaws2 = np.arctan2(ys2d, xs2d)
 
     # second straightaway
     xs3 = np.linspace(straightaway_length, 0, 80)
     ys3 = width * np.ones(xs3.shape)
+    vs3 = np.ones(xs3.shape)*velocity
     yaws3 = np.pi * np.ones(xs3.shape)
 
     # second turn
     t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1]
     xs4 = width / 2.0 * np.cos(t)
     ys4 = width / 2.0 * np.sin(t) + width / 2.0
+    vs4 = np.ones(xs4.shape)*turn_velocity
     yaws4 = yaws2 + np.pi
 
     xs = np.hstack((xs1, xs2, xs3, xs4))
     ys = np.hstack((ys1, ys2, ys3, ys4))
+    vs = np.hstack((vs1, vs2, vs3, vs4))
+    vs[0] = 0.
+    vs[-1] = 0.
     yaws = np.hstack((yaws1, yaws2, yaws3, yaws4))
 
     # now = rospy.Time.now()
@@ -145,11 +196,16 @@ def get_racetrack_waypoints(attributes):  # length, width, height):
         wp.position.x = xs[i]
         wp.position.y = ys[i]
         wp.position.z = height
+        wp.velocity = vs[i]
+
+        #logger.info(str(i) + ' ' + str(wp.velocity))
+        
         wp.yaw = yaws[i]
 
         traj.waypoints.append(wp)
 
-    get_velocities_dual(traj, velocity, max_acceleration)
+    #get_velocities_dual(traj, velocity, max_acceleration)
+    get_velocities_adjust(traj, max_acceleration)
     get_accelerations(traj)
 
     return traj
@@ -467,8 +523,24 @@ def fixed_trajectory_callback(self, msg):
         else:
             print("No trajectory sent.")
 
+#import matplotlib.pyplot as plt
 
 if __name__ == "__main__":
+    '''
+    dct = {'frame_id': 'base_link', 'length': 2, 'width': 1, 'height': 0, 'velocity': 2, 'turn_velocity': 0.5, 'max_acceleration': 0.1}
+    traj1 = get_racetrack_waypoints(dct)
+    dct['max_acceleration'] = 100000000.
+    traj2 = get_racetrack_waypoints(dct)
+
+    v1 = [wp.velocity for wp in traj1.waypoints]
+    v2 = [wp.velocity for wp in traj2.waypoints]
+
+    plt.plot(v1, '.-')
+    plt.plot(v2, '.-')
+    plt.show()
+    exit()
+
+    '''
     rclpy.init(args=None)
 
     node = FixedTrajectoryGenerator()
diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp
index 5183a45c..fcf2ef24 100644
--- a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp
+++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp
@@ -461,36 +461,24 @@ bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ah
                                                   double time_end, tf2::Vector3 sphere_center,
                                                   double sphere_radius, double min_velocity,
                                                   Waypoint* waypoint, Waypoint* end_waypoint) {
-    double current_path_distance = 0;
-    bool found = false;
-
+  int last_waypoint_index = -1;
     // ROS_INFO("\n\n");
     for (size_t i = 1; i < waypoints.size(); i++) {
         if (waypoints[i].get_time() < initial_time) continue;
 
         Waypoint wp_start = waypoints[i - 1];
         Waypoint wp_end = waypoints[i];
+	last_waypoint_index = i;
+
+	// if the very first waypoint we check isn't within the sphere, then return not found
+	if(i == 1 && wp_start.position().distance(sphere_center) > sphere_radius)
+	  return false;
 
         // handle the case that the initial_time is between waypoint i-1 and waypoint i
         if (wp_start.get_time() < initial_time)
             wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) /
                                                         (wp_end.get_time() - wp_start.get_time()));
 
-        double segment_distance = wp_start.position().distance(wp_end.position());
-        bool should_break = false;
-        if (current_path_distance + segment_distance >= ahead_distance) {
-            should_break = true;
-            wp_end = wp_start.interpolate(
-                wp_end, (ahead_distance - current_path_distance) / segment_distance);
-            if (end_waypoint != NULL) *end_waypoint = wp_end;
-        }/* else if (wp_end.get_time() > time_end) {
-            should_break = true;
-            wp_end = wp_start.interpolate(wp_end, (time_end - wp_start.get_time()) /
-                                                      (wp_end.get_time() - wp_start.get_time()));
-            if (end_waypoint != NULL) *end_waypoint = wp_end;
-        }*/ else
-            current_path_distance += segment_distance;
-
         // sphere line intersection equations:
         // http://www.ambrsoft.com/TrigoCalc/Sphere/SpherLineIntersection_.htm
         double x1 = wp_start.get_x();
@@ -534,17 +522,19 @@ bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ah
                 if (waypoint != NULL) {
                     // ROS_INFO_STREAM("t: " << t);
                     *waypoint = wp_start.interpolate(wp_end, t);
-                    found = true;
+		    return true;
                 }
             }
         }
-
-        if (should_break) break;
     }
-    // if(!found)
-    //   ROS_INFO("NOT FOUND!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
 
-    return found;
+    // if the last waypoint is within the sphere radius return it
+    if(last_waypoint_index != -1 && waypoints[last_waypoint_index].position().distance(sphere_center) <= sphere_radius){
+      *waypoint = waypoints[last_waypoint_index];
+      return true;
+    }
+    
+    return false;
 }
 
 bool Trajectory::get_waypoint(double time, Waypoint* waypoint) {
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/CMakeLists.txt
new file mode 100644
index 00000000..6fa04bb2
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/CMakeLists.txt
@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.8)
+project(attitude_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_executable(attitude_controller src/attitude_controller.cpp)
+target_include_directories(attitude_controller PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(attitude_controller PUBLIC c_std_99 cxx_std_17)
+install(TARGETS attitude_controller
+  DESTINATION lib/${PROJECT_NAME})
+
+add_executable(controller_ekf src/controller_ekf.cpp)
+target_include_directories(controller_ekf PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_compile_features(controller_ekf PUBLIC c_std_99 cxx_std_17)
+install(TARGETS controller_ekf
+  DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/COLCON_IGNORE b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/attitude_controller.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/attitude_controller.hpp
new file mode 100644
index 00000000..975411c0
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/attitude_controller.hpp
@@ -0,0 +1,122 @@
+#ifndef _DRONE_FLIGHT_CONTROL_H_
+#define _DRONE_FLIGHT_CONTROL_H_
+
+#include <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <mav_msgs/msg/roll_pitch_yawrate_thrust.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <sensor_msgs/msg/battery_state.hp>
+
+#include <attitude_controller_msgs/msg/kfState.h>
+#include <attitude_controller_msgs/msg/attitude_controller_debug.hpp>
+
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2/convert.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <ros/console.h>
+
+#include <string>
+
+#include <core_trajectory_library/trajectory_library.h>
+#include <tflib/tflib.h>
+#include <templib/templib.h>
+
+class DroneFlightControl : public BaseNode {
+private:
+
+  // params
+  std::string target_frame;
+  double p1, p2, p3, p1_volt, p2_volt;
+  double kp1, kp2, kp3, kp4, kd1, kd2, kd3, ki1_body, ki2_body, ki3_body, ki1_vel_body, ki2_vel_body,
+    ki1_ground, ki2_ground, ki3_ground, ki1_vel_ground, ki2_vel_ground, ki3_vel_ground, kf1, kf2, kf3;
+
+  double command_angle_limit;
+  double i_angle_limit;
+  double z_i_limit;
+  double g, m, hover_throttle, hover_throttle_param;
+  double min_dt;
+  double low_thrust_duration, ekf_delay;
+  bool use_ekf_state, use_ekf_dist, thrust_sim;
+  double yawrate_limit;
+  double upper_bbox;
+  bool compensate_control;
+
+  bool disturb_est_body_frame;
+  bool clip_disturbances, vel_only_stuck, zero_rp_stuck;
+  double attitude_stuck_limit, control_stuck_threshold, recover_threshold;
+  
+  // variables
+  bool got_odom, /*got_tracking_point,*/ got_closest_point, /*got_vtp,*/ got_accel, got_filtered_odom, got_filtered_state, 
+    got_in_air, got_run_local_planner, got_vtp_jerk, got_battery_volt;
+  nav_msgs::msg::Odometry odom, prev_odom, /*tracking_point,*/ closest_point, /*vtp, */filtered_odom, prev_filtered_odom, px4_odom;
+  subt_control_ekf::kfState filtered_state;
+  std_msgs::msg::Bool run_local_planner;
+  airstack_msgs::Odometry vtp_jerk;
+
+  double x_i_body, y_i_body, z_i_body, x_i_ground, y_i_ground, z_i_ground, vx_i_ground, vy_i_ground, vz_i_ground;
+  double vx_i_body, vy_i_body;
+  double x_e_prev, y_e_prev, z_e_prev;
+  ros::Time prev_time;
+  attitude_controller_msgs::AttitudeControllerDebug debug;
+  ros::Time low_thrust_start_time, start_time_ekf, lqr_start_time;
+  double boost_factor_angle_p;
+  std_msgs::Bool in_air;
+  double roll_px4_odom, pitch_px4_odom;
+  double battery_volt;
+
+  templib::TimeOutMonitor* control_stuck_monitor;
+  templib::TimeOutMonitor* recover_monitor;
+  
+  // accel variables
+  tf2::Vector3 setpoint_vel_target_frame_prev;
+  tf2::Vector3 actual_accel_target_frame, longterm_accel_filter, shortterm_accel_filter, command_filtered_accel;
+  double accelAlpha;
+  double accelLongTermAlpha;
+  double targetDT;
+  double angle_tilt;
+  tf2::Vector3 gainAccel,alphaCommandAccel,betaAccel,maxAccel;
+  
+  // publishers
+  ros::Publisher command_pub, debug_pub, control_stuck_pub;
+  
+  // subscribers
+  tf::TransformListener* listener;
+  ros::Subscriber odometry_sub, /*tracking_point_sub,*/ closest_point_sub, /*vtp_sub,*/ imu_sub, in_air_sub, filtered_odometry_sub;
+  ros::Subscriber filtered_state_sub, pixhawk_odom_sub, run_local_planner_sub, vtp_jerk_sub, battery_volt_sub;
+
+  // services
+  ros::ServiceServer reset_integrator_server;
+  
+  // callbacks
+  void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  //void tracking_point_callback(nav_msgs::Odometry msg);
+  void closest_point_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void vtp_jerk_callback(const airstack_msgs::msg::Odometry::SharedPtr msg);
+  //void vtp_callback(nav_msgs::Odometry msg);
+  bool reset_integrator_callback(std_srvs::srv::Empty::Request& request, std_srvs::srv::Empty::Response& response);
+  void reset_integrators();
+  void imu_callback(const sensor_msgs::msg::Imu::SharedPtr imu);
+  void in_air_callback(const std_msgs::msg::Bool::SharedPtr msg);
+  void filtered_odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void filtered_state_callback(subt_control_ekf::kfState msg);
+  void pixhawk_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+  void run_local_planner_callback(const std_msgs::msg::Bool::SharedPtr msg);
+  void battery_volt_callback(const sensor_msgs::msg::BatteryState::SharedPtr msg);
+  double thrustToThrottle(double thrust_normIn);
+  
+public:
+  DroneFlightControl(std::string node_name);
+  
+  virtual bool initialize();
+  virtual bool execute();
+  virtual ~DroneFlightControl();
+
+};
+
+
+#endif
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/control_ekf.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/control_ekf.hpp
new file mode 100644
index 00000000..f60f9bdb
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/control_ekf.hpp
@@ -0,0 +1,135 @@
+#ifndef _SUBT_CONTROL_EKF_
+#define _SUBT_CONTROL_EKF_
+
+#include <base/BaseNode.h>
+// #include <sensor_msgs/Imu.h>
+#include <string>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <mav_msgs/RollPitchYawrateThrust.h>
+#include <tflib/tflib.h>
+#include <subt_control_ekf/EKFControlDebug.h>
+#include <subt_control_ekf/kfState.h>
+#include <std_srvs/Empty.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <ros/console.h>
+#include "tf2/transform_datatypes.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "std_msgs/Bool.h"
+#include <sensor_msgs/Imu.h>
+#include <Eigen/Dense>
+#include <Eigen/Core>
+#include <tf/transform_broadcaster.h>
+#include <chrono>
+#include <sensor_msgs/BatteryState.h>
+#include <mavros_msgs/RCOut.h>
+
+using namespace Eigen;
+
+typedef Matrix<double, 9, 9> Matrix9d;
+typedef Matrix<double, 6, 6> Matrix6d;
+typedef Matrix<double, 9, 6> Matrix96d;
+typedef Matrix<double, 6, 9> Matrix69d;
+typedef Matrix<double, 9, 1> Vector9d;
+typedef Matrix<double, 6, 1> Vector6d;
+
+class EKFControl: public BaseNode{
+
+private:
+
+	//params
+	std::string target_frame;
+	double gravity, hover_throttle, hover_throttle_param;
+	double p1, p2, p3, p1_volt, p2_volt;
+	double init_cov_pos_xy, init_cov_pos_z, init_cov_vel_xy, init_cov_vel_z, init_cov_dis_xy, init_cov_dis_z;
+	double model_cov_pos_xy, model_cov_pos_z, model_cov_vel_xy, model_cov_vel_z, model_cov_dis_xy, model_cov_dis_z,
+		meas_cov_pos_xy, meas_cov_pos_z, meas_cov_vel_xy, meas_cov_vel_z; 
+	double alpha_motor;
+	double reject_threshold, attitude_comp_lim, g_lim;
+	int sat_rc_limit;
+
+	//variables
+	ros::Time prev_ekf_time, start_time_command;
+	double x_meas, y_meas, z_meas, yaw_meas, xdot_meas, ydot_meas, zdot_meas;
+    double roll_meas, pitch_meas;
+
+    double x_prev, y_prev, z_prev, yaw_prev, xdot_prev, ydot_prev, zdot_prev, roll_comp_prev, pitch_comp_prev, 
+    thrust_comp_prev, thrust_achieved_prev, roll_prev, pitch_prev, thrust_in_prev, thrust_command_prev;
+
+    std::string tf_prefix;
+
+    double x_ap, y_ap, z_ap, xdot_ap, ydot_ap, zdot_ap, roll_comp_ap, pitch_comp_ap, thrust_comp_ap;
+    subt_control_ekf::EKFControlDebug debug;    
+    bool got_odom, got_command, got_in_air, got_px4_odom, got_battery_volt, got_ekf_active, got_rc_out;
+    nav_msgs::Odometry odom, prev_odom, px4_odom;
+    mav_msgs::RollPitchYawrateThrust command, prev_command;
+    std_msgs::Bool in_air, ekf_active;
+    mavros_msgs::RCOut rc_out;
+
+    double targetDT, min_dt, min_command_dt;
+    double dt;
+    int odomCt, commandCt;
+    double roll_px4, pitch_px4, roll_px4_odom, pitch_px4_odom;
+    double battery_volt;
+
+    // double P_x[2][2], P_y[2][2], P_z[2][2];
+    // double Q_x[2][2], Q_y[2][2], Q_z[2][2];
+    Matrix3d rot_mat;
+    Matrix3d dis_pos;
+    Matrix3d dis_vel;
+    Matrix3d dis_acc;
+    Matrix9d P;
+    Matrix9d Q; 
+    Matrix6d R;
+
+    Matrix9d A_model; 
+    Matrix69d H;
+
+    Vector9d state_ap;
+    Vector6d state_meas;
+
+    bool disturb_est_on;
+    bool disturb_est_body_frame;
+    bool print_flag, thrust_sim;
+    int set_attitude;
+    tf::TransformBroadcaster* broadcaster;
+
+	//publishers
+	ros::Publisher stateOut_pub;
+	ros::Publisher debug_pub;
+	ros::Publisher  kfState_pub;
+
+	//subscribers
+	tf::TransformListener* listener;
+	ros::Subscriber odometry_sub, command_sub, in_air_sub, pixhawk_imu_sub, pixhawk_odom_sub;
+	ros::Subscriber battery_volt_sub, ekf_active_sub, rc_out_sub;
+
+	//services
+
+	//callbacks
+	void odometry_callback(nav_msgs::Odometry msg);
+	void command_callback(mav_msgs::RollPitchYawrateThrust msg);
+	void in_air_callback(std_msgs::Bool msg);
+	// void pixhawk_imu_callback(sensor_msgs::Imu msg);
+	void pixhawk_odom_callback(nav_msgs::Odometry msg);
+	void battery_volt_callback(sensor_msgs::BatteryState msg);
+	void ekf_active_callback(std_msgs::Bool msg);
+	void rc_out_callback(mavros_msgs::RCOut msg);
+
+	void calcAP( double dtIn, double thrust_In, double thrust_In_prev, double& x_ap_,
+	  double& y_ap_, double& z_ap_, double& xdot_ap_, double& ydot_ap_, double& zdot_ap_, double roll_comp_ap_, 
+	  double pitch_comp_ap_, double thrust_comp_ap_, double roll, double pitch, double yaw);
+
+	double throttleToThrust(double throttle);
+
+public:
+	EKFControl(std::string node_name);
+
+	virtual bool initialize();
+	virtual bool execute();
+	virtual ~EKFControl();
+};
+
+
+#endif
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/controller_ekf.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/controller_ekf.hpp
new file mode 100644
index 00000000..f60f9bdb
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/include/attitude_controller/controller_ekf.hpp
@@ -0,0 +1,135 @@
+#ifndef _SUBT_CONTROL_EKF_
+#define _SUBT_CONTROL_EKF_
+
+#include <base/BaseNode.h>
+// #include <sensor_msgs/Imu.h>
+#include <string>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <mav_msgs/RollPitchYawrateThrust.h>
+#include <tflib/tflib.h>
+#include <subt_control_ekf/EKFControlDebug.h>
+#include <subt_control_ekf/kfState.h>
+#include <std_srvs/Empty.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <ros/console.h>
+#include "tf2/transform_datatypes.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "std_msgs/Bool.h"
+#include <sensor_msgs/Imu.h>
+#include <Eigen/Dense>
+#include <Eigen/Core>
+#include <tf/transform_broadcaster.h>
+#include <chrono>
+#include <sensor_msgs/BatteryState.h>
+#include <mavros_msgs/RCOut.h>
+
+using namespace Eigen;
+
+typedef Matrix<double, 9, 9> Matrix9d;
+typedef Matrix<double, 6, 6> Matrix6d;
+typedef Matrix<double, 9, 6> Matrix96d;
+typedef Matrix<double, 6, 9> Matrix69d;
+typedef Matrix<double, 9, 1> Vector9d;
+typedef Matrix<double, 6, 1> Vector6d;
+
+class EKFControl: public BaseNode{
+
+private:
+
+	//params
+	std::string target_frame;
+	double gravity, hover_throttle, hover_throttle_param;
+	double p1, p2, p3, p1_volt, p2_volt;
+	double init_cov_pos_xy, init_cov_pos_z, init_cov_vel_xy, init_cov_vel_z, init_cov_dis_xy, init_cov_dis_z;
+	double model_cov_pos_xy, model_cov_pos_z, model_cov_vel_xy, model_cov_vel_z, model_cov_dis_xy, model_cov_dis_z,
+		meas_cov_pos_xy, meas_cov_pos_z, meas_cov_vel_xy, meas_cov_vel_z; 
+	double alpha_motor;
+	double reject_threshold, attitude_comp_lim, g_lim;
+	int sat_rc_limit;
+
+	//variables
+	ros::Time prev_ekf_time, start_time_command;
+	double x_meas, y_meas, z_meas, yaw_meas, xdot_meas, ydot_meas, zdot_meas;
+    double roll_meas, pitch_meas;
+
+    double x_prev, y_prev, z_prev, yaw_prev, xdot_prev, ydot_prev, zdot_prev, roll_comp_prev, pitch_comp_prev, 
+    thrust_comp_prev, thrust_achieved_prev, roll_prev, pitch_prev, thrust_in_prev, thrust_command_prev;
+
+    std::string tf_prefix;
+
+    double x_ap, y_ap, z_ap, xdot_ap, ydot_ap, zdot_ap, roll_comp_ap, pitch_comp_ap, thrust_comp_ap;
+    subt_control_ekf::EKFControlDebug debug;    
+    bool got_odom, got_command, got_in_air, got_px4_odom, got_battery_volt, got_ekf_active, got_rc_out;
+    nav_msgs::Odometry odom, prev_odom, px4_odom;
+    mav_msgs::RollPitchYawrateThrust command, prev_command;
+    std_msgs::Bool in_air, ekf_active;
+    mavros_msgs::RCOut rc_out;
+
+    double targetDT, min_dt, min_command_dt;
+    double dt;
+    int odomCt, commandCt;
+    double roll_px4, pitch_px4, roll_px4_odom, pitch_px4_odom;
+    double battery_volt;
+
+    // double P_x[2][2], P_y[2][2], P_z[2][2];
+    // double Q_x[2][2], Q_y[2][2], Q_z[2][2];
+    Matrix3d rot_mat;
+    Matrix3d dis_pos;
+    Matrix3d dis_vel;
+    Matrix3d dis_acc;
+    Matrix9d P;
+    Matrix9d Q; 
+    Matrix6d R;
+
+    Matrix9d A_model; 
+    Matrix69d H;
+
+    Vector9d state_ap;
+    Vector6d state_meas;
+
+    bool disturb_est_on;
+    bool disturb_est_body_frame;
+    bool print_flag, thrust_sim;
+    int set_attitude;
+    tf::TransformBroadcaster* broadcaster;
+
+	//publishers
+	ros::Publisher stateOut_pub;
+	ros::Publisher debug_pub;
+	ros::Publisher  kfState_pub;
+
+	//subscribers
+	tf::TransformListener* listener;
+	ros::Subscriber odometry_sub, command_sub, in_air_sub, pixhawk_imu_sub, pixhawk_odom_sub;
+	ros::Subscriber battery_volt_sub, ekf_active_sub, rc_out_sub;
+
+	//services
+
+	//callbacks
+	void odometry_callback(nav_msgs::Odometry msg);
+	void command_callback(mav_msgs::RollPitchYawrateThrust msg);
+	void in_air_callback(std_msgs::Bool msg);
+	// void pixhawk_imu_callback(sensor_msgs::Imu msg);
+	void pixhawk_odom_callback(nav_msgs::Odometry msg);
+	void battery_volt_callback(sensor_msgs::BatteryState msg);
+	void ekf_active_callback(std_msgs::Bool msg);
+	void rc_out_callback(mavros_msgs::RCOut msg);
+
+	void calcAP( double dtIn, double thrust_In, double thrust_In_prev, double& x_ap_,
+	  double& y_ap_, double& z_ap_, double& xdot_ap_, double& ydot_ap_, double& zdot_ap_, double roll_comp_ap_, 
+	  double pitch_comp_ap_, double thrust_comp_ap_, double roll, double pitch, double yaw);
+
+	double throttleToThrust(double throttle);
+
+public:
+	EKFControl(std::string node_name);
+
+	virtual bool initialize();
+	virtual bool execute();
+	virtual ~EKFControl();
+};
+
+
+#endif
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/package.xml
new file mode 100644
index 00000000..a6b0a244
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>attitude_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/src/attitude_controller.cpp b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/src/attitude_controller.cpp
new file mode 100644
index 00000000..e8c69f17
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/src/attitude_controller.cpp
@@ -0,0 +1,765 @@
+#include <attitude_controller/attitude_controller.hpp>
+
+DroneFlightControl::DroneFlightControl(std::string node_name)
+  : BaseNode(node_name){
+}
+
+bool DroneFlightControl::initialize(){
+  ros::NodeHandle* nh = get_node_handle();
+  ros::NodeHandle* pnh = get_private_node_handle();
+
+  // init params
+  target_frame = airstack::get_param(this, "target_frame", std::string("map"));
+  p1 = airstack::get_param(this, "p1", 24.28);
+  p2 = airstack::get_param(this, "p2", 6.287);
+  p3 = airstack::get_param(this, "p3", -1.844);
+  kp1 = airstack::get_param(this, "kp1", 1.0);
+  kp2 = airstack::get_param(this, "kp2", 1.0);
+  kp3 = airstack::get_param(this, "kp3", 1.0);
+  kp4 = airstack::get_param(this, "kp4", 1.0);
+  kd1 = airstack::get_param(this, "kd1", 1.0);
+  kd2 = airstack::get_param(this, "kd2", 1.0);
+  kd3 = airstack::get_param(this, "kd3", 1.0);
+  ki1_body = airstack::get_param(this, "ki1_body", 1.0);
+  ki2_body = airstack::get_param(this, "ki2_body", 1.0);
+  ki1_vel_body = airstack::get_param(this, "ki1_vel_body", 1.0);
+  ki2_vel_body = airstack::get_param(this, "ki2_vel_body", 1.0);
+  // ki3_body = airstack::get_param(this, "ki3_body", 1.0);
+  ki1_ground = airstack::get_param(this, "ki1_ground", 1.0);
+  ki2_ground = airstack::get_param(this, "ki2_ground", 1.0);
+  ki3_ground = airstack::get_param(this, "ki3_ground", 1.0);
+  ki1_vel_ground = airstack::get_param(this, "ki1_vel_ground", 0.0);
+  ki2_vel_ground = airstack::get_param(this, "ki2_vel_ground", 0.0);
+  ki3_vel_ground = airstack::get_param(this, "ki3_vel_ground", 0.0);
+  kf1 = airstack::get_param(this, "kf1", 1.0);
+  kf2 = airstack::get_param(this, "kf2", 1.0);
+  kf3 = airstack::get_param(this, "kf3", 0.0);
+  disturb_est_body_frame = airstack::get_param(this, "disturb_est_body_frame", false);
+  command_angle_limit = airstack::get_param(this, "command_angle_limit", 20.0)*M_PI/180.;
+  i_angle_limit = airstack::get_param(this, "i_angle_limit", 2.0)*M_PI/180.;
+  z_i_limit = airstack::get_param(this, "z_i_limit", 10000.);
+  g = airstack::get_param(this, "gravity", 9.81);
+  m = airstack::get_param(this, "mass", 5000.0);
+  hover_throttle_param = airstack::get_param(this, "hover_throttle", 0.4);
+  min_dt = airstack::get_param(this, "min_dt", 0.0001);
+  low_thrust_duration = airstack::get_param(this, "low_thrust_duration", 2.0);
+  ekf_delay = airstack::get_param(this, "ekf_delay", 2.0);
+  use_ekf_state = airstack::get_param(this, "use_ekf_state", true);
+  use_ekf_dist = airstack::get_param(this, "use_ekf_dist", true);
+  thrust_sim = airstack::get_param(this, "thrust_sim", true);
+  boost_factor_angle_p = airstack::get_param(this, "boost_factor_angle_p", 1.0);
+  yawrate_limit = fabs(airstack::get_param(this, "yawrate_limit", 10.0)*M_PI/180.);
+  upper_bbox = airstack::get_param(this, "upper_bbox", 2.0);
+  compensate_control = airstack::get_param(this, "compensate_control", false);
+  p1_volt = airstack::get_param(this, "p1_volt", 1.0);
+  p2_volt = airstack::get_param(this, "p2_volt", 1.0);
+
+  attitude_stuck_limit = airstack::get_param(this, "attitude_stuck_limit", 15.0)*M_PI/180.;
+  control_stuck_threshold = airstack::get_param(this, "control_stuck_threshold", 1.0);
+  recover_threshold = airstack::get_param(this, "recover_threshold", 1.0);
+  clip_disturbances = airstack::get_param(this, "clip_disturbances", true);
+  vel_only_stuck = airstack::get_param(this, "vel_only_stuck", true);
+  zero_rp_stuck = airstack::get_param(this, "zero_rp_stuck", true);
+  
+  // accel params
+  accelAlpha         = airstack::get_param(this, "accelAlpha", 0.05);
+  accelLongTermAlpha = airstack::get_param(this, "accelLongTermAlpha", 0.003);
+  gainAccel = tf2::Vector3(airstack::get_param(this, "gainAccelX", 0.015),
+			   airstack::get_param(this, "gainAccelY", 0.015),
+			   airstack::get_param(this, "gainAccelZ", 0.008));
+  betaAccel = tf2::Vector3(airstack::get_param(this, "betaAccelX", 1.0),
+			   airstack::get_param(this, "betaAccelY", 1.0),
+			   airstack::get_param(this, "betaAccelZ", 1.0));
+  maxAccel  = tf2::Vector3(airstack::get_param(this, "maxAccelX", 0.5),
+			   airstack::get_param(this, "maxAccelY", 0.5),
+			   airstack::get_param(this, "maxAccelZ", 0.5));
+  alphaCommandAccel = tf2::Vector3(airstack::get_param(this, "alphaCommandAccelX", 0.3),
+				   airstack::get_param(this, "alphaCommandAccelY", 0.3),
+				   airstack::get_param(this, "alphaCommandAccelZ", 0.3));
+  targetDT = 1.0/airstack::get_param(this, "execute_target", 50);
+  longterm_accel_filter = tf2::Vector3(0,0,9.81);
+  shortterm_accel_filter = tf2::Vector3(0,0,9.81);
+  command_filtered_accel = tf2::Vector3(0,0,0);
+  setpoint_vel_target_frame_prev = tf2::Vector3(0,0,0);
+  angle_tilt = 1.0;
+  low_thrust_start_time = ros::Time(0, 0); // make sure the time is in the past far enough so that it doesn't start out in low thrust mode
+  start_time_ekf = ros::Time(0, 0);
+
+  // init variables
+  got_odom = false; /*got_tracking_point = false;*/ got_closest_point = false; got_vtp_jerk = false;
+  /*got_vtp = false;*/  got_accel = false; got_filtered_odom = false; got_filtered_state = false;
+  in_air.data = false; got_in_air = false, got_battery_volt=false;
+  x_i_body = 0; y_i_body = 0; // z_i_body = 0; 
+  x_e_prev = 0; y_e_prev = 0; z_e_prev = 0; x_i_ground = 0; y_i_ground = 0; z_i_ground = 0;
+  vx_i_ground = 0; vy_i_ground = 0; vz_i_ground = 0;
+  hover_throttle = hover_throttle_param;
+
+  prev_time = ros::Time::now();
+  control_stuck_monitor = new templib::TimeOutMonitor(control_stuck_threshold);  
+  recover_monitor = new templib::TimeOutMonitor(recover_threshold);  
+  control_stuck_monitor->add_time();
+  recover_monitor->add_time();
+  
+  // init publishers
+  command_pub = nh->advertise<mav_msgs::RollPitchYawrateThrust>("roll_pitch_yawrate_thrust_command", 1);
+  debug_pub = nh->advertise<drone_flight_control::DroneFlightControlDebug>("drone_flight_control_debug", 1);
+  control_stuck_pub = nh->advertise<std_msgs::Bool>("control_stuck", 1);
+  
+  // init subscribers
+  listener = new tf::TransformListener();
+  //tracking_point_sub = nh->subscribe("tracking_point", 1, &DroneFlightControl::tracking_point_callback, this, ros::TransportHints().tcpNoDelay());
+  closest_point_sub = nh->subscribe("closest_point", 1, &DroneFlightControl::closest_point_callback, this, ros::TransportHints().tcpNoDelay());
+  //vtp_sub = nh->subscribe("virtual_target_point", 1, &DroneFlightControl::vtp_callback, this, ros::TransportHints().tcpNoDelay());
+  odometry_sub = nh->subscribe("odometry", 1, &DroneFlightControl::odometry_callback, this, ros::TransportHints().tcpNoDelay());
+  imu_sub = nh->subscribe("/imu/data", 2, &DroneFlightControl::imu_callback, this);
+  in_air_sub = nh->subscribe("in_air", 1, &DroneFlightControl::in_air_callback, this);
+  filtered_odometry_sub = nh->subscribe("filtered_odom_control", 1, &DroneFlightControl::filtered_odometry_callback, this, ros::TransportHints().tcpNoDelay());
+  filtered_state_sub = nh->subscribe("subt_control_ekf_state", 1, &DroneFlightControl::filtered_state_callback, this, ros::TransportHints().tcpNoDelay());
+  pixhawk_odom_sub = nh->subscribe("mavros/local_position/odom", 1, &DroneFlightControl::pixhawk_odom_callback, this, ros::TransportHints().tcpNoDelay());
+  run_local_planner_sub = nh->subscribe("run_local_planner", 1, &DroneFlightControl::run_local_planner_callback, this);
+  vtp_jerk_sub = nh->subscribe("tracking_point", 1, &DroneFlightControl::vtp_jerk_callback, this, ros::TransportHints().tcpNoDelay());
+  battery_volt_sub = nh->subscribe("mavros/battery", 1, &DroneFlightControl::battery_volt_callback, this);
+
+  // init services
+  reset_integrator_server = nh->advertiseService("reset_integrator", &DroneFlightControl::reset_integrator_callback, this);
+  
+  return true;
+}
+
+bool DroneFlightControl::execute(){
+
+  if(got_battery_volt && !thrust_sim)
+    hover_throttle = p1_volt * battery_volt + p2_volt;
+  else
+    hover_throttle = hover_throttle_param;
+
+  if(got_odom && got_vtp_jerk && odom.header.seq != prev_odom.header.seq){// && 
+    // filtered_odom.header.seq != prev_filtered_odom.header.seq){
+
+    ros::Time curr_time = odom.header.stamp;//ros::Time::now();
+    double dt = (curr_time - prev_time).toSec();
+    if(dt < min_dt)
+      return true;
+    prev_time = curr_time;
+    prev_odom = odom;
+
+    if((ros::Time::now() - low_thrust_start_time).toSec() < low_thrust_duration){
+      mav_msgs::RollPitchYawrateThrust command;
+      command.header.stamp = ros::Time::now();
+      command.roll = 0;
+      command.pitch = 0;
+      command.thrust.z = 0.15;
+      command.yaw_rate = 0;
+      command_pub.publish(command);
+      reset_integrators();
+
+      return true;
+    }    
+
+    // actual values
+    double x_a, y_a, z_a, vx_a, vy_a, vz_a;//, x_d, y_d, z_d, vx_d, vy_d, vz_d;
+    double roll_a, pitch_a, yaw_a, roll_c, pitch_c, yaw_c, roll_d, pitch_d, yaw_d, roll_v, pitch_v, yaw_v;
+    tf::Vector3 velocity_target_frame, position_target_frame;    
+    //tf::Vector3 tracking_position_target, tracking_velocity_target;
+    tf::Vector3 closest_position_target, closest_velocity_target;
+    tf::Vector3 vtp_position_target, vtp_velocity_target;
+    tf::StampedTransform velocity_transform_closest, velocity_transform_vtp;;
+    try{
+      
+      // transform for filtered odom
+      if(use_ekf_state && got_filtered_odom && start_time_ekf.toSec()>0.0 && (ros::Time::now() - start_time_ekf).toSec() > ekf_delay){
+      // && filtered_odom.header.seq != prev_filtered_odom.header.seq){
+
+        // tf::Vector3 filtered_velocity_target, filtered_position_target;
+        tf::StampedTransform filtered_transform;
+        listener->waitForTransform(target_frame, filtered_odom.header.frame_id, filtered_odom.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, filtered_odom.header.frame_id, filtered_odom.header.stamp, filtered_transform);
+        tf::StampedTransform filtered_velocity_transform;
+        listener->waitForTransform(target_frame, filtered_odom.child_frame_id, filtered_odom.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, filtered_odom.child_frame_id, filtered_odom.header.stamp, filtered_velocity_transform);
+        filtered_velocity_transform.setOrigin(tf::Vector3(0, 0, 0));
+
+        tf::Quaternion q_filtered_target = filtered_transform*tflib::to_tf(filtered_odom.pose.pose.orientation);
+        tf::Matrix3x3(q_filtered_target).getRPY(roll_a, pitch_a, yaw_a);
+
+        position_target_frame = filtered_transform*tflib::to_tf(filtered_odom.pose.pose.position);
+        x_a = position_target_frame.x();
+        y_a = position_target_frame.y();
+        z_a = position_target_frame.z();
+        
+        velocity_target_frame = filtered_velocity_transform*tflib::to_tf(filtered_odom.twist.twist.linear);
+        vx_a = velocity_target_frame.x();
+        vy_a = velocity_target_frame.y();
+        vz_a = velocity_target_frame.z();  
+      }
+      else{ // use absolute odom
+        
+        // transform for odom
+        tf::StampedTransform transform;
+        listener->waitForTransform(target_frame, odom.header.frame_id, odom.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, odom.header.frame_id, odom.header.stamp, transform);
+        tf::StampedTransform velocity_transform;
+        listener->waitForTransform(target_frame, odom.child_frame_id, odom.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, odom.child_frame_id, odom.header.stamp, velocity_transform);
+        velocity_transform.setOrigin(tf::Vector3(0, 0, 0));
+
+        tf::Quaternion q_target_frame = transform*tflib::to_tf(odom.pose.pose.orientation);
+        tf::Matrix3x3(q_target_frame).getRPY(roll_a, pitch_a, yaw_a);
+
+        position_target_frame = transform*tflib::to_tf(odom.pose.pose.position);      
+        x_a = position_target_frame.x();
+        y_a = position_target_frame.y();
+        z_a = position_target_frame.z();
+        
+        velocity_target_frame = velocity_transform*tflib::to_tf(odom.twist.twist.linear);
+        vx_a = velocity_target_frame.x();
+        vy_a = velocity_target_frame.y();
+        vz_a = velocity_target_frame.z();        
+      }
+      
+      // transform for tracking point
+      /*
+      tf::StampedTransform transform_tracking;
+      listener->waitForTransform(target_frame, tracking_point.header.frame_id, tracking_point.header.stamp, ros::Duration(0.1));
+      listener->lookupTransform(target_frame, tracking_point.header.frame_id, tracking_point.header.stamp, transform_tracking);
+      tf::StampedTransform velocity_transform_tracking;
+      listener->waitForTransform(target_frame, tracking_point.child_frame_id, tracking_point.header.stamp, ros::Duration(0.1));
+      listener->lookupTransform(target_frame, tracking_point.child_frame_id, tracking_point.header.stamp, velocity_transform_tracking);
+      velocity_transform_tracking.setOrigin(tf::Vector3(0, 0, 0));
+
+      tracking_position_target = transform_tracking*tflib::to_tf(tracking_point.pose.pose.position);
+      tf::Quaternion q_tracking_target = transform_tracking*tflib::to_tf(tracking_point.pose.pose.orientation);
+      x_d = tracking_position_target.x();
+      y_d = tracking_position_target.y();
+      z_d = tracking_position_target.z();
+      tf::Matrix3x3(q_tracking_target).getRPY(roll_d, pitch_d, yaw_d);
+      
+      tracking_velocity_target = velocity_transform_tracking*tflib::to_tf(tracking_point.twist.twist.linear);
+      vx_d = tracking_velocity_target.x();
+      vy_d = tracking_velocity_target.y();
+      vz_d = tracking_velocity_target.z();
+      */
+
+      // transform for closest point
+      if( got_closest_point ){
+
+        tf::StampedTransform transform_closest;
+        listener->waitForTransform(target_frame, closest_point.header.frame_id, closest_point.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, closest_point.header.frame_id, closest_point.header.stamp, transform_closest);
+        
+        listener->waitForTransform(target_frame, closest_point.child_frame_id, closest_point.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, closest_point.child_frame_id, closest_point.header.stamp, velocity_transform_closest);
+        velocity_transform_closest.setOrigin(tf::Vector3(0, 0, 0)); // TODO: what does this line do?
+
+        closest_position_target = transform_closest*tflib::to_tf(closest_point.pose.pose.position);
+        tf::Quaternion q_closest_target = transform_closest*tflib::to_tf(closest_point.pose.pose.orientation);
+        tf::Matrix3x3(q_closest_target).getRPY(roll_c, pitch_c, yaw_c);
+
+        closest_velocity_target = velocity_transform_closest*tflib::to_tf(closest_point.twist.twist.linear);
+      }
+
+      // transform for VTP
+      if( got_vtp_jerk ){
+
+        tf::StampedTransform transform_vtp;
+        listener->waitForTransform(target_frame, vtp_jerk.header.frame_id, vtp_jerk.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, vtp_jerk.header.frame_id, vtp_jerk.header.stamp, transform_vtp);
+        
+        listener->waitForTransform(target_frame, vtp_jerk.child_frame_id, vtp_jerk.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, vtp_jerk.child_frame_id, vtp_jerk.header.stamp, velocity_transform_vtp);
+        velocity_transform_vtp.setOrigin(tf::Vector3(0, 0, 0)); // TODO: what does this line do?
+
+        vtp_position_target = transform_vtp*tflib::to_tf(vtp_jerk.pose.position);
+        tf::Quaternion q_vtp_target = transform_vtp*tflib::to_tf(vtp_jerk.pose.orientation);
+        tf::Matrix3x3(q_vtp_target).getRPY(roll_v, pitch_v, yaw_v);
+
+        vtp_velocity_target = velocity_transform_vtp*tflib::to_tf(vtp_jerk.twist.linear);
+      }
+
+      // transform for px4
+      if(compensate_control){
+
+        double yaw_temp;
+        tf::StampedTransform transform_px4;
+        listener->waitForTransform(target_frame, px4_odom.header.frame_id, px4_odom.header.stamp, ros::Duration(0.1));
+        listener->lookupTransform(target_frame, px4_odom.header.frame_id, px4_odom.header.stamp, transform_px4);
+
+        tf::Quaternion px4_q_target_frame = transform_px4*tflib::to_tf(px4_odom.pose.pose.orientation); 
+        tf::Matrix3x3(px4_q_target_frame).getRPY(roll_px4_odom, pitch_px4_odom, yaw_temp);
+      }
+      
+    } 
+    catch(tf::TransformException& te){
+      ROS_ERROR_STREAM("TransformException while transform odometry: " << te.what());
+      return true;
+    }
+    
+    
+    double yaw_tf = yaw_a;
+
+    // declare variables
+    double x_e, y_e, z_e, yaw_e=0, vx_e, vy_e, vz_e, x_e_d, y_e_d, z_e_d, roll, pitch, yawrate, throttle, thrust_norm;
+    double x_e_body, y_e_body, vx_e_body, vy_e_body;
+    double x_ff=0, y_ff=0, z_ff=0;
+
+    ///// Control laws ///////// 
+
+    // check stuck condition      
+    bool velOnly=false, zero_rp=false;    
+    if( sqrt( roll_a*roll_a + pitch_a*pitch_a ) < attitude_stuck_limit &&
+      sqrt( roll_px4_odom*roll_px4_odom + pitch_px4_odom*pitch_px4_odom ) < attitude_stuck_limit ){
+      control_stuck_monitor->add_time();
+    }
+
+    std_msgs::Bool control_stuck_msg;      
+
+    debug.recover_mode.data = false; // default
+
+    if(control_stuck_monitor->is_timed_out()){
+      control_stuck_msg.data = true;
+
+      if(!recover_monitor->is_timed_out()){
+        // do recovery action
+        debug.recover_mode.data=true;    
+
+        // clip disturbances
+        if(clip_disturbances){
+          filtered_state.disOut.x=0.;
+          filtered_state.disOut.y=0.;
+        }
+
+        // switch to vel control
+        if(vel_only_stuck){
+          velOnly=true;
+        }
+
+        // switch to zero_rp
+        if(zero_rp_stuck){
+          zero_rp=true;
+        }
+      }
+    }
+    else{
+      control_stuck_msg.data = false;
+      recover_monitor->add_time();
+    }    
+    control_stuck_pub.publish(control_stuck_msg);
+
+    debug.stuck_mode.data = control_stuck_msg.data;
+
+    // When tracking VTP //
+    if(got_vtp_jerk){// && got_filtered_odom){// && filtered_odom.header.seq != prev_filtered_odom.header.seq){      
+
+      tf::Vector3 diffPos = vtp_position_target - position_target_frame;
+
+      // TODO: better way to get direction of motion
+      tf::Vector3 posErr;
+      if( fabs(vtp_velocity_target.length2()) < 1e-6 || diffPos.length() > upper_bbox){
+
+        posErr = diffPos;
+        // ROS_INFO_STREAM("Setpoint velocity went to zero");
+      }
+      else
+        posErr = diffPos - diffPos.dot(vtp_velocity_target)/( vtp_velocity_target.length2() )*vtp_velocity_target;
+
+      tf::Vector3 velErr = vtp_velocity_target - velocity_target_frame;
+
+      // errors
+      x_e = posErr.x();
+      y_e = posErr.y();
+      z_e = vtp_position_target.z() - z_a;
+
+      if(velOnly){
+        x_e=0.; y_e=0.;
+      }
+
+      yaw_e = atan2(sin(yaw_v - yaw_a), cos(yaw_v - yaw_a));
+      vx_e = velErr.x();
+      vy_e = velErr.y();
+      vz_e = velErr.z(); 
+                    
+      if(got_run_local_planner && run_local_planner.data && got_vtp_jerk){
+
+        // tf::Vector3 vtp_acceleration_target = velocity_transform_vtp*tflib::to_tf(vtp_jerk.acceleration);
+        // x_ff = vtp_acceleration_target.x(); y_ff = vtp_acceleration_target.y(); z_ff = vtp_acceleration_target.z();
+        x_ff = vtp_jerk.acceleration.x; y_ff = vtp_jerk.acceleration.y; z_ff = vtp_jerk.acceleration.z;
+      }      
+      else if(got_vtp_jerk){
+        x_ff = vtp_jerk.acceleration.x; y_ff = vtp_jerk.acceleration.y; z_ff = vtp_jerk.acceleration.z;
+      }
+    }  
+
+    // for integrators
+    x_e_body = cos(yaw_tf)*x_e + sin(yaw_tf)*y_e;
+    vx_e_body = cos(yaw_tf)*vx_e + sin(yaw_tf)*vy_e;
+    y_e_body =  -sin(yaw_tf)*x_e + cos(yaw_tf)*y_e;
+    vy_e_body =  -sin(yaw_tf)*vx_e + cos(yaw_tf)*vy_e;
+
+    double accel_xdes = kp1 * x_e + kd1 * vx_e  + ki1_ground * x_i_ground + ki1_vel_ground*vx_i_ground + kf1 * x_ff;
+    double accel_ydes = kp2 * y_e + kd2 * vy_e  + ki2_ground * y_i_ground + ki2_vel_ground*vy_i_ground + kf2 * y_ff;
+    double accel_zdes = kp3 * z_e + kd3 * vz_e  + ki3_ground * z_i_ground + ki3_vel_ground*vz_i_ground + kf3 * z_ff;
+
+    // add ground frame dist terms
+    if (use_ekf_dist && got_filtered_odom && !disturb_est_body_frame && start_time_ekf.toSec()>0.0 && 
+      (ros::Time::now() - start_time_ekf).toSec() > ekf_delay ){
+      accel_xdes += filtered_state.disOut.x;
+      accel_ydes += filtered_state.disOut.y;
+      accel_zdes += filtered_state.disOut.z * g;
+    }
+    
+    // generate control inputs
+    // thrust_norm = (tf::Vector3(accel_xdes, accel_ydes, accel_zdes) + tf::Vector3(0,0,g)).length();
+
+    roll = ( accel_xdes * sin(yaw_tf) - accel_ydes * cos(yaw_tf) )/g - ki2_body*y_i_body - ki2_vel_body*vy_i_body;
+    pitch = ( accel_xdes * cos(yaw_tf) + accel_ydes * sin(yaw_tf) )/g + ki1_body*x_i_body + ki1_vel_body*vx_i_body;
+    // roll = asin( std::min( 1.0, std::max( (accel_xdes*sin(yaw_tf) - accel_ydes*cos(yaw_tf) )/
+    //                     thrust_norm, -1.0 )) );
+    // pitch = acos( std::min(1.0, std::max(-1.0, (accel_zdes + g)/(thrust_norm*cos(roll_a)) )) );
+
+
+    if(compensate_control && in_air.data){
+      roll += roll_px4_odom - roll_a;
+      pitch += pitch_px4_odom - pitch_a;
+    }
+
+    // add body frame dist terms
+    if(use_ekf_dist && got_filtered_odom && disturb_est_body_frame && start_time_ekf.toSec()>0.0 && 
+      (ros::Time::now() - start_time_ekf).toSec() > ekf_delay ){
+      roll += filtered_state.disOut.x;
+      pitch += filtered_state.disOut.y;
+      accel_zdes += filtered_state.disOut.z * g; 
+    }
+
+    accel_zdes = std::max(accel_zdes, -g);    
+    thrust_norm = (accel_zdes+g);
+    // thrust_norm = (accel_zdes + g)/std::max(0.5, std::min(1.0,  cos(roll_a)*cos(pitch_a) ) );
+    yawrate = std::max(-yawrate_limit, std::min(yawrate_limit, kp4*yaw_e));
+
+    // get throttle from thrust
+    if(isnan(thrust_norm))
+      throttle = hover_throttle;
+    else
+      throttle = std::min( 1.0, thrustToThrottle(thrust_norm) ); 
+
+
+    // cap control angles
+    if(roll > command_angle_limit)
+      roll = command_angle_limit;
+    else if(roll < -command_angle_limit)
+      roll = -command_angle_limit;
+    else if(isnan(roll))
+      roll = 0;
+
+    if(pitch > command_angle_limit)
+      pitch = command_angle_limit;
+    else if(pitch < -command_angle_limit)
+      pitch = -command_angle_limit;
+    else if (isnan(pitch))
+      pitch=0;
+
+    if(isnan(yaw_e))
+      yawrate=0;     
+
+    // while taking off //
+    if(!in_air.data){
+
+      roll = 0; pitch=0; throttle = hover_throttle; yawrate=0;
+    }      
+
+    // recovery condition 
+    if(zero_rp){roll=0.; pitch=0.;}
+
+    // publish debug info
+    debug.header = odom.header;
+
+    if(got_closest_point){
+      
+      debug.position_closest.x = closest_position_target.x();
+      debug.position_closest.y = closest_position_target.y();
+      debug.position_closest.z = closest_position_target.z();
+      debug.yaw_closest = yaw_c;
+      debug.velocity_closest.x = closest_velocity_target.x();
+      debug.velocity_closest.y = closest_velocity_target.y();
+      debug.velocity_closest.z = closest_velocity_target.z();
+    }
+
+    if(got_vtp_jerk){
+
+      debug.position_vtp.x = vtp_position_target.x();
+      debug.position_vtp.y = vtp_position_target.y();
+      debug.position_vtp.z = vtp_position_target.z();
+      debug.yaw_vtp = yaw_c;
+      debug.velocity_vtp.x = vtp_velocity_target.x();
+      debug.velocity_vtp.y = vtp_velocity_target.y();
+      debug.velocity_vtp.z = vtp_velocity_target.z();
+      debug.acceleration_vtp.x = vtp_jerk.acceleration.x;
+      debug.acceleration_vtp.x = vtp_jerk.acceleration.y;
+      debug.acceleration_vtp.x = vtp_jerk.acceleration.z;
+      debug.accel_vtp_net = tf::Vector3(vtp_jerk.acceleration.x, vtp_jerk.acceleration.y, vtp_jerk.acceleration.z + g).length();
+    }
+    
+    debug.position_actual.x = x_a;
+    debug.position_actual.y = y_a;
+    debug.position_actual.z = z_a;
+    debug.yaw_actual = yaw_a;
+    debug.velocity_actual.x = vx_a;
+    debug.velocity_actual.y = vy_a;
+    debug.velocity_actual.z = vz_a;
+
+    debug.integrator_ground.x = x_i_ground;
+    debug.integrator_ground.y = y_i_ground;
+    debug.integrator_ground.z = z_i_ground;
+
+    debug.vel_integrator_ground.x = vx_i_ground;
+    debug.vel_integrator_ground.y = vy_i_ground;
+    debug.vel_integrator_ground.z = vz_i_ground;
+
+    debug.integrator_body.x = x_i_body;
+    debug.integrator_body.y = y_i_body;
+
+    debug.vel_integrator_body.x = vx_i_body;
+    debug.vel_integrator_body.y = vy_i_body;    
+
+    debug.position_error.x = x_e;
+    debug.position_error.y = y_e;
+    debug.position_error.z = z_e;
+    debug.yaw_error = yaw_e;
+    debug.velocity_error.x = vx_e;
+    debug.velocity_error.y = vy_e;
+    debug.velocity_error.z = vz_e;
+
+    debug.derivative.x = x_e_d;
+    debug.derivative.y = y_e_d;
+    debug.derivative.z = z_e_d;
+
+    debug.kp.x = kp1;
+    debug.kp.y = kp2;
+    debug.kp.z = kp3;
+    debug.ki_ground.x = ki1_ground;
+    debug.ki_ground.y = ki2_ground;
+    debug.ki_ground.z = ki3_ground;
+    debug.ki_vel_ground.x = ki1_vel_ground;
+    debug.ki_vel_ground.y = ki2_vel_ground;
+    debug.ki_vel_ground.z = ki3_vel_ground;
+    debug.kd.x = kd1;
+    debug.kd.y = kd2;
+    debug.kd.z = kd3;
+    
+    debug.dt = dt;
+    
+    debug.roll = roll * 180./M_PI;
+    debug.pitch = pitch * 180./M_PI;
+    debug.yawrate = yawrate;
+    debug.throttle = throttle;
+    debug.thrust_norm = thrust_norm;
+    
+    debug_pub.publish(debug);
+    
+    // accumulate integrators
+    // if(ki2_body != 0.)
+    //   x_i_body = std::max(-fabs(i_angle_limit*g/ki2_body), std::min(fabs(i_angle_limit*g/ki2_body), x_i_body + x_e_body));
+    // if(ki1_body != 0.)
+    //   y_i_body = std::max(-fabs(i_angle_limit*g/ki1_body), std::min(fabs(i_angle_limit*g/ki1_body), y_i_body + y_e_body));
+    // if(ki3_body != 0.)
+    //   z_i_body = std::max(-fabs(z_i_limit), std::min(fabs(z_i_limit), z_i_body + z_e));
+    double ki_ground = std::max(fabs(ki1_ground), fabs(ki2_ground));
+    if(ki_ground != 0.)
+      x_i_ground = std::max(-fabs(i_angle_limit*g/ki_ground), std::min(fabs(i_angle_limit*g/ki_ground), x_i_ground + x_e));
+    if(ki_ground != 0.)
+      y_i_ground = std::max(-fabs(i_angle_limit*g/ki_ground), std::min(fabs(i_angle_limit*g/ki_ground), y_i_ground + y_e));
+    z_i_ground += z_e;
+
+    double ki_vel_ground = std::max(fabs(ki1_vel_ground), fabs(ki2_vel_ground));
+    if(ki_vel_ground != 0.)
+      vx_i_ground = std::max(-fabs(i_angle_limit*g/ki_vel_ground), 
+          std::min(fabs(i_angle_limit*g/ki_vel_ground), vx_i_ground + vx_e));
+
+    if(ki_vel_ground != 0.)
+      vy_i_ground = std::max(-fabs(i_angle_limit*g/ki_vel_ground), 
+          std::min(fabs(i_angle_limit*g/ki_vel_ground), vy_i_ground + vy_e));
+
+    vz_i_ground += vz_e;
+
+    double ki_body = std::max(fabs(ki1_body), fabs(ki2_body));
+    if(ki_body != 0.)
+      x_i_body = std::max(-fabs(i_angle_limit*g/ki_body), 
+          std::min(fabs(i_angle_limit*g/ki_body), x_i_body + x_e_body));
+
+    if(ki_body != 0.)
+      y_i_body = std::max(-fabs(i_angle_limit*g/ki_body), 
+          std::min(fabs(i_angle_limit*g/ki_body), y_i_body + y_e_body));
+
+    double ki_vel_body = std::max(fabs(ki1_vel_body), fabs(ki2_vel_body));
+    if(ki_vel_body != 0.)
+      vx_i_body = std::max(-fabs(i_angle_limit*g/ki_vel_body), 
+          std::min(fabs(i_angle_limit*g/ki_vel_body), vx_i_body + vx_e_body));
+
+    if(ki_vel_body != 0.)
+      vy_i_body = std::max(-fabs(i_angle_limit*g/ki_vel_body), 
+          std::min(fabs(i_angle_limit*g/ki_vel_body), vy_i_body + vy_e_body));
+
+    // x_i_body += x_e_body;
+    // y_i_body += y_e_body;
+    // vx_i_body += vx_e_body;
+    // vy_i_body += vy_e_body;
+    
+    // set previous errors
+    x_e_prev = x_e;
+    y_e_prev = y_e;
+    z_e_prev = z_e; 
+
+    mav_msgs::RollPitchYawrateThrust command;
+    command.header.stamp = ros::Time::now();
+    command.roll = roll;
+    command.pitch = pitch;
+    command.thrust.z = throttle;
+    command.yaw_rate = yawrate;
+    command_pub.publish(command); 
+  }
+      
+  return true;
+}
+
+void DroneFlightControl::odometry_callback(nav_msgs::Odometry msg){
+  got_odom = true;
+  odom = msg;
+}
+/*
+void DroneFlightControl::tracking_point_callback(nav_msgs::Odometry msg){
+  got_tracking_point = true;
+  tracking_point = msg;
+}
+*/
+void DroneFlightControl::closest_point_callback(nav_msgs::Odometry msg){
+  got_closest_point = true;
+  closest_point = msg;
+}
+
+void DroneFlightControl::vtp_jerk_callback(core_trajectory_msgs::Odometry msg){
+  got_vtp_jerk = true;
+  vtp_jerk = msg;
+}
+/*
+void DroneFlightControl::vtp_callback(nav_msgs::Odometry msg){
+  got_vtp = true;
+  vtp = msg;
+}
+*/
+bool DroneFlightControl::reset_integrator_callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response){
+  reset_integrators();
+  low_thrust_start_time = ros::Time::now();
+  return true;
+}
+
+void DroneFlightControl::reset_integrators(){
+  x_i_body = 0;
+  y_i_body = 0;
+  vx_i_body=0; 
+  vy_i_body=0;
+  // z_i_body = 0;
+  x_i_ground = 0;
+  y_i_ground = 0;
+  z_i_ground = 0;
+  vx_i_ground = 0;
+  vy_i_ground = 0;
+  vz_i_ground = 0;
+}
+
+void DroneFlightControl::imu_callback(sensor_msgs::Imu imu){
+  //Assumes IMU is aligned with axis on vehicle and avoid transform
+  //Rotate to level
+  tf2::Quaternion quat_tf;
+  double roll,pitch,yaw;
+  tf2::convert(imu.orientation,quat_tf);
+  tf2::Matrix3x3(quat_tf).getRPY(roll,pitch,yaw);
+  tf2::Matrix3x3 levelMat;
+  levelMat.setRPY(roll,pitch,0.0);
+  tf2::Vector3 preRotateAccl,rotatedAccel;
+  tf2::convert(imu.linear_acceleration,preRotateAccl);
+  rotatedAccel = levelMat * preRotateAccl;//Undo the rotation by roll/pitch
+ 
+  angle_tilt = cos(pitch)*cos(roll);
+  
+  //Already filter and remove
+  //This is the long term world acceleration (such as gravity)
+  longterm_accel_filter = longterm_accel_filter + accelLongTermAlpha * (rotatedAccel - longterm_accel_filter); 
+
+  //This is the actual acceleration on the vehicle
+  shortterm_accel_filter = shortterm_accel_filter + accelAlpha *(rotatedAccel - shortterm_accel_filter); 
+  actual_accel_target_frame = -(shortterm_accel_filter - longterm_accel_filter);
+  //ROS_INFO_STREAM(roll<<","<<pitch<<","<<actual_accel_target_frame.z()<<","<<shortterm_accel_filter.z()<<","<<longterm_accel_filter.z());
+  //Abuse this message type for debugging:
+  imu.header.stamp = ros::Time::now();
+  tf2::convert(actual_accel_target_frame,imu.linear_acceleration);
+  /*
+  imu.angular_velocity.x = actual_vel_target_frame.x();
+  imu.angular_velocity.y = actual_vel_target_frame.y();
+  imu.angular_velocity.z = actual_vel_target_frame.z();
+  */
+  debug.rotated_accel.x = rotatedAccel.x();
+  debug.rotated_accel.y = rotatedAccel.y();
+  debug.rotated_accel.z = rotatedAccel.z();
+  //accel_pub.publish(imu);
+  
+  got_accel = true;
+
+}
+
+void DroneFlightControl::in_air_callback(std_msgs::Bool msg){
+  got_in_air = true;
+  in_air = msg;
+}
+
+void DroneFlightControl::filtered_odometry_callback(nav_msgs::Odometry msg){
+  if(!got_filtered_odom)
+    start_time_ekf = ros::Time::now();
+
+  got_filtered_odom = true;
+  filtered_odom = msg;
+}
+
+void DroneFlightControl::filtered_state_callback(subt_control_ekf::kfState msg){
+  got_filtered_state = true;
+  filtered_state = msg;
+}
+
+void DroneFlightControl::pixhawk_odom_callback(nav_msgs::Odometry msg){
+
+  got_odom = true;
+  px4_odom = msg;
+}
+
+void DroneFlightControl::run_local_planner_callback(std_msgs::Bool msg){
+
+  got_run_local_planner = true;
+  run_local_planner = msg;
+}
+
+void DroneFlightControl::battery_volt_callback(sensor_msgs::BatteryState msg){
+
+  got_battery_volt = true;
+  battery_volt = msg.voltage;
+}
+
+double DroneFlightControl::thrustToThrottle(double thrust_normIn){	
+
+  // this if for sim
+  if(thrust_sim)
+    return sqrt( thrust_normIn/g ) * hover_throttle;
+  
+  // this is for real drone
+  else{
+    double temp = p1*hover_throttle*hover_throttle + p2*hover_throttle + p3;
+    double quad_a = p1, quad_b = p2, quad_c = -temp*thrust_normIn/g + p3;
+    return (-quad_b + sqrt( quad_b*quad_b - 4*quad_a*quad_c ) )/(2*quad_a);
+  }
+  
+}
+
+DroneFlightControl::~DroneFlightControl(){
+}
+
+BaseNode* BaseNode::get(){
+  DroneFlightControl* drone_flight_control = new DroneFlightControl("DroneFlightControl");
+  return drone_flight_control;
+}
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/src/controller_ekf.cpp b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/src/controller_ekf.cpp
new file mode 100644
index 00000000..b3cc4e52
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller/src/controller_ekf.cpp
@@ -0,0 +1,605 @@
+#include "subt_control_ekf/subt_control_ekf.h"
+#include <base/BaseNode.h>
+#include <string>
+#include <vector>
+#include <math.h>
+
+// using namespace Eigen;
+
+// #define N_dof 6
+
+EKFControl::EKFControl(std::string node_name): BaseNode(node_name){}
+
+bool EKFControl::initialize(){
+
+	ros::NodeHandle* nh = get_node_handle();
+	ros::NodeHandle* pnh = get_private_node_handle();
+
+	// init params
+	tf_prefix = pnh->param("tf_prefix", std::string(""));
+  target_frame = pnh->param("target_frame", std::string("map"));
+  min_dt = pnh->param("min_dt", 0.01);
+  min_command_dt = pnh->param("min_command_dt", 0.01);
+	gravity =pnh->param("gravity", 9.81);
+  hover_throttle_param =pnh->param("hover_throttle", 0.66);
+  p1 = pnh->param("p1", 24.28); p2 = pnh->param("p2", 6.287); p3 = pnh->param("p3", -1.844);
+  targetDT = 1.0/pnh->param("execute_target", 50);
+  init_cov_pos_xy = pnh->param("init_cov_pos_xy", 1e-3);
+  init_cov_pos_z = pnh->param("init_cov_pos_z", 1e-3);
+  init_cov_vel_xy = pnh->param("init_cov_vel_xy", 1e-2);
+  init_cov_vel_z = pnh->param("init_cov_vel_z", 1e-2);
+  init_cov_dis_xy = pnh->param("init_cov_dis_xy", 1e-4); // 2
+  init_cov_dis_z = pnh->param("init_cov_dis_z", 1e-4);
+  model_cov_pos_xy = pnh->param("model_cov_pos_xy", 1e-6); //3
+  model_cov_pos_z = pnh->param("model_cov_pos_z", 1e-6);
+  model_cov_vel_xy = pnh->param("model_cov_vel_xy", 1e-6); // 2
+  model_cov_vel_z = pnh->param("model_cov_vel_z", 1e-6);
+  model_cov_dis_xy = pnh->param("model_cov_dis_xy", 1e-6); // 2
+  model_cov_dis_z = pnh->param("model_cov_dis_z", 1e-6);
+  meas_cov_pos_xy = pnh->param("meas_cov_pos_xy", 1e-3);
+  meas_cov_pos_z = pnh->param("meas_cov_pos_z", 1e-3);
+  meas_cov_vel_xy = pnh->param("meas_cov_vel_xy", 1e-2);
+  meas_cov_vel_z = pnh->param("meas_cov_vel_z", 1e-2);
+  disturb_est_on = pnh->param("disturb_est_on", true);
+  disturb_est_body_frame = pnh->param("disturb_est_body_frame", false);
+  print_flag = pnh->param("print_flag", false);
+  thrust_sim = pnh->param("thrust_sim", false);
+  set_attitude = pnh->param("set_attitude", 0);
+  alpha_motor = pnh->param("alpha_motor", 1.0);
+  p1_volt = pnh->param("p1_volt", 1.0);
+  p2_volt = pnh->param("p2_volt", 1.0);
+  reject_threshold = pnh->param("reject_threshold", 3.0);
+  attitude_comp_lim = pnh->param("attitude_comp_lim", 10.0);
+  g_lim = pnh->param("g_lim", 0.5);
+  sat_rc_limit = pnh->param("sat_rc_limit", 1900);
+
+  // init variables
+  odomCt=0; commandCt=0;
+  in_air.data = false;
+  ekf_active.data = false;
+
+  // roll_prev=0; pitch_prev=0; thrust_prev=0;
+  got_odom = false; got_command=false; got_in_air = false; got_battery_volt = false; got_ekf_active = false;
+  // prev_odom_time = odom.header.stamp;
+  prev_ekf_time = odom.header.stamp; // ros::Time::now();
+  // prev_odom = odom; prev_command = command;
+  prev_command.roll = 0.0; prev_command.pitch = 0.0; prev_command.thrust.z = 0.0;  
+
+  H <<1., 0., 0., 0., 0., 0., 0., 0., 0.,
+      0., 1., 0., 0., 0., 0., 0., 0., 0.,
+      0., 0., 1., 0., 0., 0., 0., 0., 0., 
+      0., 0., 0., 1., 0., 0., 0., 0., 0.,
+      0., 0., 0., 0., 1., 0., 0., 0., 0.,
+      0., 0., 0., 0., 0., 1., 0., 0., 0.; 
+
+  P << init_cov_pos_xy, 0., 0., 0., 0., 0., 0., 0., 0.,
+      0., init_cov_pos_xy, 0., 0., 0., 0., 0., 0., 0.,
+      0., 0., init_cov_pos_z, 0., 0., 0., 0., 0., 0.,
+      0., 0., 0., init_cov_vel_xy, 0., 0., 0., 0., 0.,
+      0., 0., 0., 0., init_cov_vel_xy, 0., 0., 0., 0.,
+      0., 0., 0., 0., 0., init_cov_vel_z, 0., 0., 0.,
+      0., 0., 0., 0., 0., 0., init_cov_dis_xy, 0., 0.,
+      0., 0., 0., 0., 0., 0., 0., init_cov_dis_xy, 0.,
+      0., 0., 0., 0., 0., 0., 0., 0., init_cov_dis_z;
+
+  Q << model_cov_pos_xy, 0., 0., 0., 0., 0., 0., 0., 0.,
+      0., model_cov_pos_xy, 0., 0., 0., 0., 0., 0., 0.,
+      0., 0., model_cov_pos_z, 0., 0., 0., 0., 0., 0.,
+      0., 0., 0., model_cov_vel_xy, 0., 0., 0., 0., 0.,
+      0., 0., 0., 0., model_cov_vel_xy, 0., 0., 0., 0.,
+      0., 0., 0., 0., 0., model_cov_vel_z, 0., 0., 0.,
+      0., 0., 0., 0., 0., 0., model_cov_dis_xy, 0., 0.,
+      0., 0., 0., 0., 0., 0., 0., model_cov_dis_xy, 0.,
+      0., 0., 0., 0., 0., 0., 0., 0., model_cov_dis_z;
+
+  R << meas_cov_pos_xy, 0., 0., 0., 0., 0.,
+      0., meas_cov_pos_xy, 0., 0., 0., 0., 
+      0., 0., meas_cov_pos_z, 0., 0., 0., 
+      0., 0., 0., meas_cov_vel_xy, 0., 0.,
+      0., 0., 0., 0., meas_cov_vel_xy, 0.,
+      0., 0., 0., 0., 0., meas_cov_vel_z;
+
+  // try{
+  //   tf::StampedTransform transform;
+  //   listener->waitForTransform(target_frame, odom.header.frame_id, odom.header.stamp, ros::Duration(0.1));
+  //   ROS_INFO_STREAM("wait for velo transform done\n");
+  //   listener->lookupTransform(target_frame, odom.header.frame_id, odom.header.stamp, transform);
+  //   ROS_INFO_STREAM("look up velo transform done\n");
+  //   tf::StampedTransform velocity_transform;
+  //   listener->waitForTransform(target_frame, odom.child_frame_id, odom.header.stamp, ros::Duration(0.1));
+  //   listener->lookupTransform(target_frame, odom.child_frame_id, odom.header.stamp, velocity_transform);
+  //   velocity_transform.setOrigin(tf::Vector3(0,
+  //   tf::Vector3 position_target_frame = transform*tflib::to_tf(odom.pose.pose.position);
+  //   tf::Quaternion q_target_frame = transform*tflib::to_tf(odom.pose.pose.orientation);
+  //   x_prev = position_target_frame.x();
+  //   y_prev = position_target_frame.y();
+  //   z_prev = position_target_frame.z();
+  //   tf::Matrix3x3(q_target_frame).getRPY(roll_prev, pitch_prev, yaw_prev);
+
+  //   tf::Vector3 velocity_target_frame = velocity_transform*tflib::to_tf(odom.twist.twist.linear);
+  //   xdot_prev = velocity_target_frame.x();
+  //   ydot_prev = velocity_target_frame.y();
+  //   zdot_prev = velocity_target_frame.z();
+  // }
+  // catch(tf::TransformException& te){
+  //   ROS_ERROR_STREAM("TransformException while transform odometry in initialize: " << te.what());
+  //   return true;
+  // }
+
+  // ^ TODO may not work in bagfile, can comment out and init prev to 0
+  // see below v
+
+  x_prev = 0.0; y_prev = 0.0; z_prev = 0.0;
+  xdot_prev = 0.0; ydot_prev = 0.0; zdot_prev = 0.0;
+  roll_prev=0.0; roll_comp_prev=0.0;
+  pitch_prev=0.0; pitch_comp_prev=0.0;
+  thrust_comp_prev = -1.0;
+  roll_px4_odom=0.0; pitch_px4_odom = 0.0;
+  hover_throttle = hover_throttle_param;
+	thrust_achieved_prev = 0.; thrust_command_prev = throttleToThrust(hover_throttle); thrust_in_prev = 0.; 
+	prev_ekf_time = ros::Time::now(); start_time_command = ros::Time::now();
+  battery_volt = 16.0;
+
+  // init publishers
+  stateOut_pub = nh->advertise<nav_msgs::Odometry>("filtered_odom_control", 1);
+  debug_pub = nh->advertise<subt_control_ekf::EKFControlDebug>("subt_control_ekf_debug", 1);
+  kfState_pub = nh->advertise<subt_control_ekf::kfState>("subt_control_ekf_state", 1);
+
+  // init subscribers
+  listener = new tf::TransformListener();
+  broadcaster = new tf::TransformBroadcaster();
+  odometry_sub = nh->subscribe("odometry", 1, &EKFControl::odometry_callback, this, ros::TransportHints().tcpNoDelay());
+  command_sub = nh->subscribe("roll_pitch_yawrate_thrust_command", 1, &EKFControl::command_callback, this);
+  in_air_sub = nh->subscribe("in_air", 1, &EKFControl::in_air_callback, this);
+  // pixhawk_imu_sub = nh->subscribe("mavros/imu/data", 1, &EKFControl::pixhawk_imu_callback, this);
+  pixhawk_odom_sub = nh->subscribe("mavros/local_position/odom", 1, &EKFControl::pixhawk_odom_callback, this, ros::TransportHints().tcpNoDelay());
+  battery_volt_sub = nh->subscribe("mavros/battery", 1, &EKFControl::battery_volt_callback, this);
+  ekf_active_sub = nh->subscribe("ekf_active", 1, &EKFControl::ekf_active_callback, this);
+  rc_out_sub = nh->subscribe("mavros/rc/out", 1, &EKFControl::rc_out_callback, this);
+
+  return true;
+}
+
+bool EKFControl::execute(){
+
+  ros::Time start_time = ros::Time::now();
+  double now = start_time.toSec();
+  double prev_time = start_time.toSec();
+
+  if(got_battery_volt && !thrust_sim)
+    hover_throttle = p1_volt * battery_volt + p2_volt;
+  else
+    hover_throttle = hover_throttle_param;
+  
+  if(got_odom && odom.header.seq != prev_odom.header.seq){
+
+    ros::Time curr_time = ros::Time::now();
+    // ROS_INFO_STREAM("Time according to odom is "<<odom.header.stamp.toSec()<<", Time according to ROS is "<<ros::Time::now().toSec());
+
+    // TODO what's going on here?
+
+  	// // double dt = (curr_time - prev_time).toSec();
+   //  // double command_dt = (command.header.stamp - prev_command_time).toSec();
+  	// prev_time = curr_time; 
+    // prev_command_time = command.header.stamp;
+
+    // if(dt < min_dt || command_dt < min_command_dt )
+    //   return true
+
+    double unused;
+    try{
+      
+      //monitor.tic("px4tf");
+      tf::StampedTransform transform_px4;
+      //listener->waitForTransform(target_frame, px4_odom.header.frame_id, px4_odom.header.stamp, ros::Duration(0.1));
+      //listener->lookupTransform(target_frame, px4_odom.header.frame_id, px4_odom.header.stamp, transform_px4);
+      listener->lookupTransform(target_frame, px4_odom.header.frame_id, ros::Time(0), transform_px4);
+      //ROS_INFO_STREAM("elapsed: " << monitor.toc("px4tf")/1000000.);
+      
+      tf::Quaternion px4_q_target_frame = transform_px4*tflib::to_tf(px4_odom.pose.pose.orientation); 
+      tf::Matrix3x3(px4_q_target_frame).getRPY(roll_px4_odom, pitch_px4_odom, unused);
+      
+      //monitor.tic("other4tf");
+      auto start1 = std::chrono::steady_clock::now();
+      tf::StampedTransform transform;
+      listener->waitForTransform(target_frame, odom.header.frame_id, odom.header.stamp, ros::Duration(0.1));
+      listener->lookupTransform(target_frame, odom.header.frame_id, odom.header.stamp, transform);
+      tf::StampedTransform velocity_transform;
+      listener->waitForTransform(target_frame, odom.child_frame_id, odom.header.stamp, ros::Duration(0.1));
+      listener->lookupTransform(target_frame, odom.child_frame_id, odom.header.stamp, velocity_transform);
+      velocity_transform.setOrigin(tf::Vector3(0, 0, 0));      
+      //ROS_INFO_STREAM("elapsed: " << monitor.toc("other4tf")/1000000.);
+      
+      tf::Vector3 position_target_frame = transform*tflib::to_tf(odom.pose.pose.position);
+      tf::Quaternion q_target_frame = transform*tflib::to_tf(odom.pose.pose.orientation);
+      x_meas = position_target_frame.x();
+      y_meas = position_target_frame.y();
+      z_meas = position_target_frame.z();
+      tf::Matrix3x3(q_target_frame).getRPY(roll_meas, pitch_meas, yaw_meas);
+      
+      tf::Vector3 velocity_target_frame = velocity_transform*tflib::to_tf(odom.twist.twist.linear);
+      xdot_meas = velocity_target_frame.x();
+      ydot_meas = velocity_target_frame.y();
+      zdot_meas = velocity_target_frame.z();
+      // if (zdot_meas > 6.0 ) {
+      //   zdot_meas = 6.0;
+      // }
+      // else if (zdot_meas < -30.0) {
+      //   zdot_meas = -30.0;
+      // }      
+      // ROS_INFO_STREAM("transform time: " <<elapsed_seconds1.count());
+    }
+    catch(tf::TransformException& te){
+      ROS_ERROR_STREAM("TransformException while transform odometry in execute: " << te.what());
+      return true;
+    }
+
+    // ** find a-priori estimate ** //
+    x_ap = x_prev; y_ap = y_prev; z_ap = z_prev;
+    xdot_ap = xdot_prev; ydot_ap = ydot_prev; zdot_ap = zdot_prev;
+    roll_comp_ap = roll_comp_prev; pitch_comp_ap = pitch_comp_prev; thrust_comp_ap = thrust_comp_prev;   
+
+    if(in_air.data){
+      // auto start2 = std::chrono::steady_clock::now();
+
+      double dt_ekf = (ros::Time::now() - prev_ekf_time).toSec();
+      
+      double pitch_in;
+      double roll_in; 
+      double yaw_in = yaw_meas; 
+
+      // if(set_attitude==0){
+      //   pitch_in = (pitch_prev + prev_command.pitch)/2.0;
+      //   roll_in = (roll_prev + prev_command.roll)/2.0;
+      // }
+      if(set_attitude==1){
+        pitch_in = (pitch_prev + pitch_meas)/2.0;
+        roll_in = (roll_prev + roll_meas)/2.0;
+      }
+      else if(set_attitude==2){
+        pitch_in = (pitch_prev + pitch_px4_odom)/2.0;
+        roll_in = (roll_prev + roll_px4_odom)/2.0;
+      }
+
+      double dt_command = (ros::Time::now() - start_time_command).toSec();
+      double thrust_in = thrust_achieved_prev + (thrust_command_prev - thrust_achieved_prev)*(1 - exp(-alpha_motor*dt_command));
+
+      calcAP( dt_ekf, thrust_in, thrust_in_prev, x_ap, y_ap, z_ap, xdot_ap, ydot_ap, zdot_ap, roll_comp_ap, pitch_comp_ap, 
+      	thrust_comp_ap, roll_in, pitch_in, yaw_in );      
+      
+      /// ***Kalman Filter*** ///
+      if (prev_ekf_time.toSec() == 0.0) {
+        prev_ekf_time = curr_time;
+      }
+
+
+      if (disturb_est_on && disturb_est_body_frame){
+
+        A_model << 1., 0., 0., dt_ekf, 0., 0., -gravity*pow(dt_ekf, 2)/2.0*sin(yaw_in), -gravity*pow(dt_ekf, 2)/2.0*cos(yaw_in), 0.,
+                  0., 1., 0., 0., dt_ekf, 0., gravity*pow(dt_ekf, 2)/2.0*cos(yaw_in), -gravity*pow(dt_ekf, 2)/2.0*sin(yaw_in), 0.,
+                  0., 0., 1., 0., 0., dt_ekf, 0., 0., -gravity*pow(dt_ekf, 2)/2.0,
+                  0., 0., 0., 1., 0., 0., -gravity*dt_ekf*sin(yaw_in), -gravity*dt_ekf*cos(yaw_in), 0.,
+                  0., 0., 0., 0., 1., 0., gravity*dt_ekf*cos(yaw_in), -gravity*dt_ekf*sin(yaw_in), 0.,
+                  0., 0., 0., 0., 0., 1., 0., 0., -gravity*dt_ekf,
+                  0., 0., 0., 0., 0., 0., 1., 0., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 1., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 0., 1.;
+      }
+
+      else if (disturb_est_on && not disturb_est_body_frame) {
+
+        A_model << 1., 0., 0., dt_ekf, 0., 0., -pow(dt_ekf, 2)/2., 0., 0.,
+                  0., 1., 0., 0., dt_ekf, 0., 0., -pow(dt_ekf, 2)/2., 0.,
+                  0., 0., 1., 0., 0., dt_ekf, 0., 0., -gravity*pow(dt_ekf, 2)/2.,
+                  0., 0., 0., 1., 0., 0., -dt_ekf, 0., 0.,
+                  0., 0., 0., 0., 1., 0., 0., -dt_ekf, 0.,
+                  0., 0., 0., 0., 0., 1., 0., 0., -gravity*dt_ekf,
+                  0., 0., 0., 0., 0., 0., 1., 0., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 1., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 0., 1.;
+      }
+
+      else{
+        A_model << 1., 0., 0., dt_ekf, 0., 0., 0., 0., 0.,
+                  0., 1., 0., 0., dt_ekf, 0., 0., 0., 0.,
+                  0., 0., 1., 0., 0., dt_ekf, 0., 0., 0.,
+                  0., 0., 0., 1., 0., 0., 0., 0., 0.,
+                  0., 0., 0., 0., 1., 0., 0., 0., 0.,
+                  0., 0., 0., 0., 0., 1., 0., 0., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 0., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 0., 0.,
+                  0., 0., 0., 0., 0., 0., 0., 0., 0.;
+      }
+
+      P = A_model * P * A_model.transpose() + Q;
+
+      Matrix6d S = H * P * H.transpose() + R;
+      Matrix96d K = P * H.transpose() * S.inverse();
+
+      state_ap(0) = x_ap; state_ap(1) = y_ap; state_ap(2) = z_ap;
+      state_ap(3) = xdot_ap; state_ap(4) = ydot_ap; state_ap(5) = zdot_ap;
+      state_ap(6) = roll_comp_ap; state_ap(7) = pitch_comp_ap; state_ap(8) = thrust_comp_ap;
+      state_meas(0) = x_meas; state_meas(1) = y_meas; state_meas(2) = z_meas;
+      state_meas(3) = xdot_meas; state_meas(4) = ydot_meas; state_meas(5) = zdot_meas;
+      Vector6d innovation = state_meas - H * state_ap;
+      Vector9d state_out;
+
+      double sqMahanolobis = innovation.dot(S.inverse() * innovation);
+      // ROS_INFO_STREAM(" S inverse is "<<S.inverse()<< ", innovation is "<<innovation);
+      // ROS_INFO_STREAM("Sq Mahanolobis dist is "<<sqMahanolobis);
+
+      bool saturated=false;
+      if( rc_out.channels[0] >= sat_rc_limit || rc_out.channels[1] >= sat_rc_limit  ||
+        rc_out.channels[2] >= sat_rc_limit || rc_out.channels[3] >= sat_rc_limit )        
+        saturated = true;
+
+      if( !saturated && ekf_active.data && (sqMahanolobis >= reject_threshold*reject_threshold) ){
+
+      	state_out = state_ap;
+      	ROS_INFO_STREAM("Outlier measurement rejected. Sq Mahanolobis is "<< sqMahanolobis);
+      }
+      else{
+      	state_out = state_ap + K * innovation;
+      	P = P - K * H * P;
+      }
+
+      double radian_lim  = attitude_comp_lim*3.1415/180.0;
+      state_out(6) = std::min(std::max( state_out(6), -radian_lim ), radian_lim);
+      state_out(7) = std::min(std::max( state_out(7), -radian_lim ), radian_lim);
+      state_out(8) = std::min(std::max( state_out(8), -g_lim ), g_lim);
+
+      double xOut = state_out(0), xdotOut = state_out(3);
+      double yOut = state_out(1), ydotOut = state_out(4);
+      double zOut = state_out(2), zdotOut = state_out(5);
+      double roll_compOut = state_out(6);
+      double pitch_compOut = state_out(7);
+      double thrust_compOut = state_out(8);
+
+      // set previous //
+
+      // if(set_attitude==0){
+      //   pitch_prev = postCommandBuffer.back().pitch;
+      //   roll_prev = postCommandBuffer.back().roll;
+      // }
+      if(set_attitude==1){
+        pitch_prev = pitch_meas;
+        roll_prev = roll_meas;
+      }
+      else if(set_attitude==2){
+        pitch_prev = pitch_px4_odom;
+        roll_prev = roll_px4_odom;
+      }
+
+      x_prev = xOut; y_prev = yOut; z_prev = zOut;
+      xdot_prev = xdotOut; ydot_prev = ydotOut; zdot_prev = zdotOut;
+      roll_comp_prev=roll_compOut; pitch_comp_prev=pitch_compOut; thrust_comp_prev=thrust_compOut;
+
+      prev_ekf_time = ros::Time::now();
+      thrust_in_prev = thrust_in;
+      prev_odom = odom;
+
+      //publish debug info
+      debug.header = odom.header;
+      debug.positionOut.x = xOut;
+      debug.positionOut.y = yOut;
+      debug.positionOut.z = zOut;
+      debug.velOut.x = xdotOut;
+      debug.velOut.y = ydotOut;
+      debug.velOut.z = zdotOut;
+      debug.disOut.x = roll_compOut;
+      debug.disOut.y = pitch_compOut;
+      debug.disOut.z = thrust_compOut;
+
+      debug.position_ap.x = x_ap;
+      debug.position_ap.y = y_ap;
+      debug.position_ap.z = z_ap;
+      debug.vel_ap.x = xdot_ap;
+      debug.vel_ap.y = ydot_ap;
+      debug.vel_ap.z = zdot_ap;
+      debug.dis_ap.x = roll_comp_ap;
+      debug.dis_ap.y = pitch_comp_ap;
+      debug.dis_ap.z = thrust_comp_ap;
+
+      debug.position_meas.x = x_meas;
+      debug.position_meas.y = y_meas;
+      debug.position_meas.z = z_meas;
+      debug.vel_meas.x = xdot_meas;
+      debug.vel_meas.y = ydot_meas;
+      debug.vel_meas.z = zdot_meas;  
+
+      debug.roll_meas = roll_meas;
+      debug.pitch_meas = pitch_meas;
+      debug.yaw_meas = yaw_meas;
+      // debug.roll_px4 = roll_px4;
+      // debug.pitch_px4 = pitch_px4;
+      debug.roll_px4_odom = roll_px4_odom;
+      debug.pitch_px4_odom = pitch_px4_odom;
+
+      debug.model_cov_pos_xy = model_cov_pos_xy;
+      debug.model_cov_pos_z = model_cov_pos_z;
+      debug.model_cov_vel_xy = model_cov_vel_xy;
+      debug.model_cov_vel_z = model_cov_vel_z;
+
+      debug.model_cov_dis_xy= model_cov_dis_xy;
+      debug.model_cov_dis_z= model_cov_dis_z;
+
+      debug.meas_cov_pos_xy = meas_cov_pos_xy;
+      debug.meas_cov_pos_z = meas_cov_pos_z;
+      debug.meas_cov_vel_xy = meas_cov_vel_xy;
+      debug.meas_cov_vel_z = meas_cov_vel_z;
+      
+      debug.thrust_in = thrust_in;
+
+      debug_pub.publish(debug);
+      // ROS_INFO_STREAM("Published Debug");
+
+      nav_msgs::Odometry stateOut;
+      stateOut.header.stamp = odom.header.stamp; //  ros::Time::now() in actual system;
+      stateOut.header.frame_id = odom.header.frame_id;
+      stateOut.pose.pose.position.x = xOut;
+      stateOut.pose.pose.position.y = yOut;
+      stateOut.pose.pose.position.z = zOut;
+
+      // stateOut.child_frame_id = odom.child_frame_id;
+      stateOut.child_frame_id = odom.header.frame_id;      
+      stateOut.twist.twist.linear.x = xdotOut;
+      stateOut.twist.twist.linear.y = ydotOut;
+      stateOut.twist.twist.linear.z = zdotOut;
+
+      stateOut.twist.twist.angular = odom.twist.twist.angular;
+      stateOut.pose.pose.orientation = odom.pose.pose.orientation;
+
+      stateOut_pub.publish(stateOut);
+
+      // publish Kalman Filter state
+      subt_control_ekf::kfState augmentedState;
+      augmentedState.header = odom.header;
+
+      augmentedState.disOut.x = roll_compOut;
+      augmentedState.disOut.y = pitch_compOut;
+      augmentedState.disOut.z = thrust_compOut;
+
+      kfState_pub.publish(augmentedState);
+
+      // create tf for filtered_state
+      tf::StampedTransform transform = tflib::to_tf(stateOut, tf_prefix + "/filtered_odom");
+      tf::StampedTransform transform_stabilized = tflib::get_stabilized(transform);
+      transform_stabilized.child_frame_id_ = tf_prefix + "/filtered_odom_stabilized";
+      broadcaster->sendTransform(transform);
+      broadcaster->sendTransform(transform_stabilized);
+
+    }
+  }
+
+  //monitor.print_time_statistics();
+  
+  return true;
+}
+
+
+void EKFControl::odometry_callback(nav_msgs::Odometry msg){
+  got_odom = true;
+  odom = msg;
+}
+
+void EKFControl::command_callback(mav_msgs::RollPitchYawrateThrust msg){
+  got_command = true;
+  // command = msg;
+  ros::Time time_curr = ros::Time::now();
+  double dt_temp_command = (time_curr - start_time_command).toSec();
+  thrust_achieved_prev += (thrust_command_prev - thrust_achieved_prev)*(1 - exp(-alpha_motor*dt_temp_command));
+  thrust_command_prev = throttleToThrust(msg.thrust.z);
+  start_time_command = time_curr;
+}
+
+void EKFControl::in_air_callback(std_msgs::Bool msg){
+  got_in_air = true;
+  in_air = msg;
+}
+
+// void EKFControl::pixhawk_imu_callback(sensor_msgs::Imu msg){
+
+//   tf::Matrix3x3 m(tf::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w));
+//   double yaw;
+//   m.getRPY(roll_px4, pitch_px4, yaw);
+// }
+
+void EKFControl::pixhawk_odom_callback(nav_msgs::Odometry msg){
+
+  got_odom = true;
+  px4_odom = msg;
+}
+
+void EKFControl::battery_volt_callback(sensor_msgs::BatteryState msg){
+
+  got_battery_volt = true;
+  battery_volt = msg.voltage;
+}
+
+void EKFControl::ekf_active_callback(std_msgs::Bool msg){
+  got_ekf_active = true;
+  ekf_active = msg;
+}
+
+void EKFControl::rc_out_callback(mavros_msgs::RCOut msg){
+  got_rc_out = true;
+  rc_out = msg;
+}
+
+void EKFControl::calcAP( double dtIn, double thrust_In, double thrust_In_prev, double& x_ap_,
+  double& y_ap_, double& z_ap_, double& xdot_ap_, double& ydot_ap_, double& zdot_ap_, double roll_comp_ap_, 
+  double pitch_comp_ap_, double thrust_comp_ap_, double roll, double pitch, double yaw){
+
+  // incorporate thrust delay
+	// double thrust_eff_av = thrust_eff + (thrust_prev_running - thrust_eff) * (1 - pow(e, (-alpha_motor * dtIn)) )/alpha_motor;
+	// thrust_prev_running += (thrust_eff - thrust_prev_running) * (1 - exp(-alpha_motor*dtIn));
+
+	double thrust_eff_av = (thrust_In + thrust_In_prev)/2.0;// - thrust_comp_ap_*gravity;
+	double roll_eff = roll; double pitch_eff = pitch;
+	if(disturb_est_on && disturb_est_body_frame){
+		roll_eff += -roll_comp_ap_;
+		pitch_eff += -pitch_comp_ap_;
+	} 
+
+  double a_x_comm, a_y_comm, a_z_comm;
+  a_x_comm = thrust_eff_av*(cos(yaw)*sin(pitch_eff)*cos(roll_eff) + sin(yaw)*sin(roll_eff));
+  a_y_comm = thrust_eff_av*(sin(yaw)*sin(pitch_eff)*cos(roll_eff) - cos(yaw)*sin(roll_eff));
+  a_z_comm = - gravity + thrust_eff_av*(cos(pitch_eff)*cos(roll_eff));    
+
+  if(disturb_est_on && !disturb_est_body_frame){
+  	a_x_comm += -roll_comp_ap_;
+  	a_y_comm += -pitch_comp_ap_;
+  }
+  if(disturb_est_on) 
+  	a_z_comm += -thrust_comp_ap_ * gravity;
+
+  // this is Ax+Bu (pretty sure need to use yaw_meas)
+  x_ap_ += xdot_ap_*dtIn + 0.5*a_x_comm*dtIn*dtIn;
+  y_ap_ += ydot_ap_*dtIn + 0.5*a_y_comm*dtIn*dtIn;
+  z_ap_ += zdot_ap_*dtIn + 0.5*a_z_comm*dtIn*dtIn;// - 0.5*gravity*dtIn*dtIn*thrust_comp_ap_;
+
+  xdot_ap_ += dtIn*( a_x_comm ); 
+  ydot_ap_ += dtIn*( a_y_comm ); 
+  zdot_ap_ += dtIn*( a_z_comm );// - gravity*dtIn*thrust_comp_ap_;
+
+  // if (disturb_est_body_frame) {
+  //   x_ap_ += -0.5*gravity*dtIn*dtIn*sin(yaw_meas)*roll_comp_ap_ - 0.5*gravity*dtIn*dtIn*cos(yaw_meas)*pitch_comp_ap_;
+  //   y_ap_ += 0.5*gravity*dtIn*dtIn*cos(yaw_meas)*roll_comp_ap_ - 0.5*gravity*dtIn*dtIn*sin(yaw_meas)*pitch_comp_ap_;
+  // }
+  // else {
+  //   x_ap_ -= 0.5*dtIn*dtIn*roll_comp_ap_;
+  //   y_ap_ -= 0.5*dtIn*dtIn*pitch_comp_ap_;
+  // }  
+  // if (disturb_est_body_frame) {
+  //   xdot_ap_ += -gravity*dtIn*sin(yaw_meas)*roll_comp_ap_ - gravity*dtIn*cos(yaw_meas)*pitch_comp_ap_;
+  //   ydot_ap_ += gravity*dtIn*cos(yaw_meas)*roll_comp_ap_ - gravity*dtIn*sin(yaw_meas)*pitch_comp_ap_;
+  // }
+  // else {
+  //   xdot_ap_ -= dtIn*roll_comp_ap_;
+  //   ydot_ap_ -= dtIn*pitch_comp_ap_;
+  // }  
+
+  // TODO may not need to multiply by gravity above since you multiply by gravity in A but idk, it may matter for the controller
+
+  // roll_comp_ap, pitch_comp_ap, and thrust_comp_ap are just their prev values
+
+  // ROS_INFO_STREAM("exiting calcAP");
+}
+
+double EKFControl::throttleToThrust(double throttle){
+
+  if(thrust_sim)
+    return gravity* (throttle / hover_throttle)*(throttle / hover_throttle);
+  else
+    return gravity * ( p1*throttle*throttle + p2*throttle + p3 ) 
+                / ( p1*(hover_throttle)*(hover_throttle) + p2*(hover_throttle) + p3 ) ;
+}
+
+
+EKFControl::~EKFControl(){}
+
+
+BaseNode* BaseNode::get(){
+  EKFControl* subt_control_ekf = new EKFControl("EKFControl");
+  return subt_control_ekf;
+}
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/CMakeLists.txt
new file mode 100644
index 00000000..19d6dac0
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(attitude_controller_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/COLCON_IGNORE b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/COLCON_IGNORE
new file mode 100644
index 00000000..e69de29b
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/package.xml
new file mode 100644
index 00000000..9e0253a6
--- /dev/null
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/attitude_controller_msgs/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>attitude_controller_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp
index 5f951556..4b4ff0da 100644
--- a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp
+++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp
@@ -27,6 +27,7 @@ class TrajectoryControlNode : public rclcpp::Node {
     rclcpp::Publisher<airstack_msgs::msg::Odometry>::SharedPtr tracking_point_pub;
     rclcpp::Publisher<airstack_msgs::msg::Odometry>::SharedPtr look_ahead_pub;
     rclcpp::Publisher<airstack_msgs::msg::Odometry>::SharedPtr drone_point_pub;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr drone_pose_pub;
     rclcpp::Publisher<airstack_msgs::msg::Odometry>::SharedPtr virtual_tracking_point_pub;
     rclcpp::Publisher<airstack_msgs::msg::Odometry>::SharedPtr closest_point_pub;
     rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr trajectory_completion_percentage_pub;
@@ -74,9 +75,12 @@ class TrajectoryControlNode : public rclcpp::Node {
     bool new_rewind;
     float rewind_skip_max_velocity;
     float rewind_skip_max_distance;
+    float velocity_sphere_radius_multiplier;
 
     vis::MarkerArray markers;
 
+    float get_sphere_radius(float vel);
+  
    public:
     TrajectoryControlNode();
     void timer_callback();
@@ -109,6 +113,8 @@ TrajectoryControlNode::TrajectoryControlNode() : rclcpp::Node("trajectory_contro
     traj_vis_thickness = airstack::get_param(this, "traj_vis_thickness", 0.03);
     rewind_skip_max_velocity = airstack::get_param(this, "rewind_skip_max_velocity", 0.1);
     rewind_skip_max_distance = airstack::get_param(this, "rewind_skip_max_distance", 0.1);
+    velocity_sphere_radius_multiplier = airstack::get_param(this, "velocity_sphere_radius_multiplier", -1.0);
+    
     got_odom = false;
 
     trajectory_mode = airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE;
@@ -135,6 +141,7 @@ TrajectoryControlNode::TrajectoryControlNode() : rclcpp::Node("trajectory_contro
     tracking_point_pub = this->create_publisher<airstack_msgs::msg::Odometry>("tracking_point", 1);
     look_ahead_pub = this->create_publisher<airstack_msgs::msg::Odometry>("look_ahead", 1);
     drone_point_pub = this->create_publisher<airstack_msgs::msg::Odometry>("traj_drone_point", 1);
+    drone_pose_pub = this->create_publisher<geometry_msgs::msg::PoseStamped>("projected_drone_pose", 1);
     virtual_tracking_point_pub =
         this->create_publisher<airstack_msgs::msg::Odometry>("virtual_tracking_point", 1);
     closest_point_pub = this->create_publisher<airstack_msgs::msg::Odometry>("closest_point", 1);
@@ -176,6 +183,13 @@ TrajectoryControlNode::TrajectoryControlNode() : rclcpp::Node("trajectory_contro
     prev_vtp_time = 0;
 }
 
+float TrajectoryControlNode::get_sphere_radius(float vel){
+  if(velocity_sphere_radius_multiplier > 0)
+    return vel*velocity_sphere_radius_multiplier;
+  else
+    return sphere_radius;
+}
+
 void TrajectoryControlNode::timer_callback() {
     static rclcpp::Time prev_execute_time = this->get_clock()->now();
     static rclcpp::Time curr_execute_time = this->get_clock()->now();
@@ -206,11 +220,7 @@ void TrajectoryControlNode::timer_callback() {
 
     // visualization
     markers.overwrite();
-    markers
-        .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), robot_point.x(),
-                    robot_point.y(), robot_point.z(), sphere_radius)
-        .set_color(0.f, 0.f, 1.f, 0.7f);
-    trajectory_vis_pub->publish(trajectory->get_markers(now - rclcpp::Duration::from_seconds(0.3),
+    trajectory_vis_pub->publish(trajectory->get_markers(now,
                                                         "traj_controller", 1, 1, 0, 1, false, false,
                                                         traj_vis_thickness));
 
@@ -237,11 +247,24 @@ void TrajectoryControlNode::timer_callback() {
             bool closest_ahead_valid = trajectory->get_closest_waypoint(
                 robot_point, virtual_time, prev_vtp_time + look_ahead_time, &closest_ahead_wp);
             if (closest_ahead_valid) {
-                virtual_time = closest_ahead_wp.get_time();
 
+ 	        geometry_msgs::msg::PoseStamped drone_pose;
+		drone_pose.header = drone_point.header;
+		drone_pose.pose.position.x = closest_ahead_wp.position().x();
+		drone_pose.pose.position.y = closest_ahead_wp.position().y();
+		drone_pose.pose.position.z = closest_ahead_wp.position().z();
+		drone_pose.pose.orientation.w = 1.0;
+		drone_pose_pub->publish(drone_pose);
+		
                 // visualization
+	        markers
+  		    .add_sphere(target_frame, now, robot_point.x(),
+		  	        robot_point.y(), robot_point.z(), get_sphere_radius(closest_ahead_wp.velocity().length()))
+		    .set_color(0.f, 0.f, 1.f, 0.7f);
+                virtual_time = closest_ahead_wp.get_time();
+		
                 markers
-                    .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3),
+                    .add_sphere(target_frame, now,
                                 closest_ahead_wp.get_x(), closest_ahead_wp.get_y(),
                                 closest_ahead_wp.get_z(), 0.025f)
                     .set_color(0.f, 1.f, 0.f);
@@ -249,29 +272,35 @@ void TrajectoryControlNode::timer_callback() {
                 Waypoint vtp_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
                 Waypoint end_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
                 bool vtp_valid = trajectory->get_waypoint_sphere_intersection(
-                    virtual_time, search_ahead_factor * sphere_radius,
-                    prev_vtp_time + look_ahead_time, robot_point, sphere_radius,
+                    virtual_time, search_ahead_factor * get_sphere_radius(closest_ahead_wp.velocity().length()),
+                    prev_vtp_time + look_ahead_time, robot_point, get_sphere_radius(closest_ahead_wp.velocity().length()),
                     min_virtual_tracking_velocity, &vtp_wp, &end_wp);
                 if (vtp_valid) current_virtual_ahead_time = vtp_wp.get_time() - virtual_time;
 
                 // visualization
                 if (vtp_valid)
                     markers
-                        .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3),
+                        .add_sphere(target_frame, now,
                                     vtp_wp.get_x(), vtp_wp.get_y(), vtp_wp.get_z(), 0.025f)
                         .set_color(0.f, 1.f, 0.f);
                 else
                     markers
-                        .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3),
+                        .add_sphere(target_frame, now,
                                     closest_ahead_wp.get_x(), closest_ahead_wp.get_y(),
-                                    closest_ahead_wp.get_z() + sphere_radius, 0.025f)
+                                    closest_ahead_wp.get_z() + get_sphere_radius(closest_ahead_wp.velocity().length()), 0.025f)
                         .set_color(1.f, 0.f, 0.f);
                 markers
-                    .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3),
+                    .add_sphere(target_frame, now,
                                 end_wp.get_x(), end_wp.get_y(), end_wp.get_z(), 0.025f)
                     .set_color(0.f, 0.f, 1.f);
-            } else
+            } else{
                 RCLCPP_INFO(this->get_logger(), "AHEAD NOT VALID");
+		
+	        markers
+  		    .add_sphere(target_frame, now, robot_point.x(),
+		  	        robot_point.y(), robot_point.z(), 1.)
+		    .set_color(1.f, 0.f, 0.f, 0.7f);
+	    }
         } else {
             if (new_rewind) {
                 float before = virtual_time;
diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml
index bbdc7dbc..2bf16731 100644
--- a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml
+++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml
@@ -36,7 +36,7 @@
             <remap from="look_ahead" to="/$(env ROBOT_NAME)/trajectory_controller/look_ahead" />
             <remap from="tracking_point"
                 to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
-            <remap from="camera_info" to="/$(env ROBOT_NAME)/sensors/front_stereo/left/camera_info" />
+            <remap from="camera_info" to="/$(env ROBOT_NAME)/sensors/front_stereo/right/camera_info" />
             <remap from="trajectory_segment_to_add" to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_segment_to_add" />
         </node>
         <!-- DROAN local model -->
@@ -46,8 +46,12 @@
             <!-- use estimated disparity -->
             <!-- <remap from="disparity" to="/$(env ROBOT_NAME)/stereo_image_proc/disparity" /> -->
             <!-- create disparity from ground truth depth instead of using estimated disparity -->
-            <remap from="depth" to="/$(env ROBOT_NAME)/sensors/front_stereo/left/depth" />
+	    <remap from="depth" to="/$(env ROBOT_NAME)/sensors/front_stereo/left/depth" />
             <remap from="camera_info" to="/$(env ROBOT_NAME)/sensors/front_stereo/left/camera_info" />
+	    <!--
+            <remap from="depth" to="/$(env ROBOT_NAME)/sensors/front_stereo/depth/depth_registered" />
+            <remap from="camera_info" to="/$(env ROBOT_NAME)/sensors/front_stereo/right/camera_info" />
+	    -->
         </node>
     </group>
 
@@ -69,9 +73,9 @@
         <param name="search_ahead_factor" value="1.5" />
         <param name="transition_velocity_scale" value="1.0" />
         <param name="traj_vis_thickness" value="0.03" />
-
         <param name="rewind_skip_max_velocity" value="0.1" />
         <param name="rewind_skip_max_distance" value="0.1" />
+	<param name="velocity_sphere_radius_multiplier" value="1.0" />
 
         <!-- remap glue to AirStack -->
         <remap from="odometry" to="/$(env ROBOT_NAME)/odometry_conversion/odometry" />
diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml
index 69544b86..3cbe5a98 100644
--- a/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml
+++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml
@@ -9,10 +9,7 @@
     <node pkg="random_walk_planner" exec="random_walk_planner" name="random_walk_planner"
         output="screen">
         <param from="$(find-pkg-share random_walk_planner)/config/random_walk_config.yaml" />
-        <!-- <remap from="/tf" to="/$(env ROBOT_NAME)/tf" /> -->
         <remap from="vdb_map_visualization" to="/$(env ROBOT_NAME)/vdb_mapping/vdb_map_visualization" />
         <remap from="~/global_plan" to="/$(env ROBOT_NAME)/global_plan" />
     </node>
-
-    
-</launch>
\ No newline at end of file
+</launch>
diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml
index 2085f01c..549a166c 100644
--- a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml
+++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml
@@ -24,6 +24,7 @@
             <remap from="takeoff_state" to="/$(env ROBOT_NAME)/takeoff_landing_planner/takeoff_state" />
             <remap from="landing_state" to="/$(env ROBOT_NAME)/takeoff_landing_planner/landing_state" />
             <remap from="state_estimate_timed_out" to="/$(env ROBOT_NAME)/drone_safety_monitor/state_estimate_timed_out" />
+            <remap from="set_recording_status" to="/$(env ROBOT_NAME)/bag_record/set_recording_status" />
 
         </node>
 
diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp
index 3d67045d..e5be9b09 100644
--- a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp
+++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp
@@ -60,6 +60,7 @@ class BehaviorExecutive : public rclcpp::Node {
     rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr state_estimate_timed_out_sub;
 
     // publishers
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr recording_pub;
 
     // services
     rclcpp::CallbackGroup::SharedPtr service_callback_group;
diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp
index 68beaa03..e90f4db6 100644
--- a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp
+++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp
@@ -81,6 +81,7 @@ BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") {
 									 
 
     // publishers
+    recording_pub = this->create_publisher<std_msgs::msg::Bool>("set_recording_status", 1);
 
     // services
     service_callback_group =
@@ -119,6 +120,10 @@ void BehaviorExecutive::timer_callback() {
 
     if (arm_action->is_active()) {
         if (arm_action->active_has_changed()) {
+	    std_msgs::msg::Bool start_msg;
+	    start_msg.data = true;
+	    recording_pub->publish(start_msg);
+	
             airstack_msgs::srv::RobotCommand::Request::SharedPtr request =
                 std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
             request->command = airstack_msgs::srv::RobotCommand::Request::ARM;
@@ -136,6 +141,10 @@ void BehaviorExecutive::timer_callback() {
 
     if (disarm_action->is_active()) {
         if (disarm_action->active_has_changed()) {
+	    std_msgs::msg::Bool stop_msg;
+	    stop_msg.data = false;
+	    recording_pub->publish(stop_msg);
+	
 	    in_air_condition->set(false);
             airstack_msgs::srv::RobotCommand::Request::SharedPtr request =
                 std::make_shared<airstack_msgs::srv::RobotCommand::Request>();
diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml
index 08f03032..0fdeb0af 100644
--- a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml
+++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml
@@ -11,6 +11,7 @@ trajectories:
       attributes:
         - frame_id
         - velocity
+        - turn_velocity
         - max_acceleration
         - length
         - width
diff --git a/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt b/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt
index 2312ed6e..d583951c 100644
--- a/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt
+++ b/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt
@@ -26,7 +26,7 @@ endif()
 # Install files.
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 # install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
-# install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
 # install(DIRECTORY params DESTINATION share/${PROJECT_NAME})
 
 ament_package()
diff --git a/robot/ros_ws/src/logging/logging_bringup/config/log.yaml b/robot/ros_ws/src/logging/logging_bringup/config/log.yaml
new file mode 100644
index 00000000..eeb15643
--- /dev/null
+++ b/robot/ros_ws/src/logging/logging_bringup/config/log.yaml
@@ -0,0 +1,31 @@
+# Any line that does not start with a / will be automatically prefixed using the 
+# namespace of the bag_record_pid node.
+# mcap_qos is the filename of the MCAP QoS profile. The actual directory will be prefixed by the
+# bag_record_pid node via a user specified argument. All MCAP QoS files must be in the same directory.
+# The -o or --output argument should not be specified here.
+# The -s mcap option will be added automatically.
+sections:
+  airstack:
+    mcap_qos: mcap_qos.yaml
+    args: 
+      - -b
+      - 4000000000 # ~4GB
+      - --max-cache-size 
+      - 1073741824 # 1GB
+    exclude:
+      - sensors/*
+  zed:
+    mcap_qos: mcap_qos.yaml
+    args: 
+      - -b
+      - 4000000000 # ~4GB
+      - --max-cache-size 
+      - 1073741824 # 1GB
+    topics:
+      - sensors/front_stereo/left/image_rect_color
+      - sensors/front_stereo/right/image_rect_color
+      - sensors/front_stereo/left/camera_info
+      - sensors/front_stereo/right/camera_info
+      - sensors/front_stereo/pose
+      - sensors/front_stereo/disparity/disparity_image
+      - sensors/front_stereo/confidence/confidence_map
\ No newline at end of file
diff --git a/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml b/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml
index a62ec22a..95f8d27c 100644
--- a/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml
+++ b/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml
@@ -1,4 +1,10 @@
 <!-- LOGGING -->
 <launch>
-
-</launch>
\ No newline at end of file
+  
+  <node name="bag_record" pkg="bag_record_pid" exec="bag_record_node">
+    <param name="cfg_path" value="$(find-pkg-share logging_bringup)/config/log.yaml" />
+    <param name="output_dir" value="/bags" />
+    <param name="mcap_qos_dir" value="$(find-pkg-share bag_record_pid)/config" />
+  </node>
+  
+</launch>
diff --git a/robot/ros_ws/src/robot_bringup/config/core.perspective b/robot/ros_ws/src/robot_bringup/config/core.perspective
index 309cd73c..c2209174 100644
--- a/robot/ros_ws/src/robot_bringup/config/core.perspective
+++ b/robot/ros_ws/src/robot_bringup/config/core.perspective
@@ -4,12 +4,12 @@
     "mainwindow": {
       "keys": {
         "geometry": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000002800000015c000009ff000004920000028000000181000009ff0000049200000000000000000a000000028000000181000009ff00000492')",
+          "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000022d00000225000009ac0000055b0000022d0000024a000009ac0000055b00000000000000000a000000022d0000024a000009ac0000055b')",
           "type": "repr(QByteArray.hex)",
-          "pretty-print": "       \\                         "
+          "pretty-print": "     - %   [ - J   [      - J   ["
         },
         "state": {
-          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000002e8fc0100000002fc00000000000007800000044200fffffffc0200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000014000000fe0000004a00fffffffc00000118000001e4000001e400fffffffc0100000002fb00000072007200710074005f006200650068006100760069006f0072005f0074007200650065005f0063006f006d006d0061006e0064005f005f004200650068006100760069006f007200540072006500650043006f006d006d0061006e00640050006c007500670069006e005f005f0031005f005f0100000000000005ba000002cd00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000005c0000001c00000016f00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000004b20000007e0000000000000000000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+          "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000002e8fc0100000002fc00000000000007800000044200fffffffc0200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000014000000b10000004a00fffffffc000000cb000002310000021300fffffffc0100000002fb00000072007200710074005f006200650068006100760069006f0072005f0074007200650065005f0063006f006d006d0061006e0064005f005f004200650068006100760069006f007200540072006500650043006f006d006d0061006e00640050006c007500670069006e005f005f0031005f005f0100000000000005ba000002cd00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000005c0000001c00000016f00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000004b20000007e0000000000000000000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
           "type": "repr(QByteArray.hex)",
           "pretty-print": "                     B                                              J               rrqt_behavior_tree_command__BehaviorTreeCommandPlugin__1__          zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__          zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__                            6MinimizedDockWidgetsToolbar        "
         }
@@ -110,7 +110,7 @@
             "plugin": {
               "keys": {
                 "attribute_settings": {
-                  "repr": "b'\\x80\\x04\\x95\\x97\\x02\\x00\\x00\\x00\\x00\\x00\\x00\\x8c\\x0bcollections\\x94\\x8c\\x0bOrderedDict\\x94\\x93\\x94)R\\x94(\\x8c\\x07Figure8\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x012\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x012\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tRacetrack\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x06Circle\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x06radius\\x94\\x8c\\x012\\x94u\\x8c\\x04Line\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x012\\x94\\x8c\\x05width\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x05Point\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x01x\\x94\\x8c\\x010\\x94\\x8c\\x01y\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tLawnmower\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x06length\\x94\\x8c\\x0210\\x94\\x8c\\x05width\\x94\\x8c\\x011\\x94\\x8c\\x06height\\x94\\x8c\\x0210\\x94\\x8c\\x08velocity\\x94\\x8c\\x011\\x94\\x8c\\x08vertical\\x94\\x8c\\x010\\x94uu.'",
+                  "repr": "b'\\x80\\x04\\x95\\xa9\\x02\\x00\\x00\\x00\\x00\\x00\\x00\\x8c\\x0bcollections\\x94\\x8c\\x0bOrderedDict\\x94\\x93\\x94)R\\x94(\\x8c\\x07Figure8\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x012\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x012\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tRacetrack\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x015\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x011\\x94\\x8c\\x06length\\x94\\x8c\\x0245\\x94\\x8c\\x05width\\x94\\x8c\\x0210\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94\\x8c\\rturn_velocity\\x94\\x8c\\x012\\x94u\\x8c\\x06Circle\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x06radius\\x94\\x8c\\x012\\x94u\\x8c\\x04Line\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x012\\x94\\x8c\\x05width\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x05Point\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x01x\\x94\\x8c\\x010\\x94\\x8c\\x01y\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tLawnmower\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x06length\\x94\\x8c\\x0210\\x94\\x8c\\x05width\\x94\\x8c\\x011\\x94\\x8c\\x06height\\x94\\x8c\\x0210\\x94\\x8c\\x08velocity\\x94\\x8c\\x011\\x94\\x8c\\x08vertical\\x94\\x8c\\x010\\x94uu.'",
                   "type": "repr"
                 },
                 "config_filename": {
diff --git a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml
index 8e16a065..6c93953a 100644
--- a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml
+++ b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml
@@ -25,10 +25,10 @@
   <group unless="$(var sim)">
     <push-ros-namespace namespace="interface" />
     <include file="$(find-pkg-share mavros)/launch/apm.launch">
-      <arg name="fcu_url" value="/dev/ttyTHS1:115200" />
+      <arg name="fcu_url" value="/dev/ttyTHS4:115200" />
     </include>
 
-    <executable cmd="ros2 bag record -a -o /bags/autonomy_$(env DATE)" />
+    <!-- <executable cmd="ros2 bag record -a -o /bags/autonomy_$(env DATE) -x '/robot_2/sensors/*'" /> -->
   </group>
 
   
diff --git a/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml b/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml
index 0ebb2bfb..e5fa843b 100644
--- a/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml
+++ b/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml
@@ -6,6 +6,14 @@
         name="static_transform_publisher"
         args="0 0 0 -1.57079632679 0 0 map_FLU map" />
 
+    <!--
+    <node
+	pkg="tf2_ros"
+	exec="static_transform_publisher"
+	name="zed_link_static_transform_publisher"
+	args="0 0 0 0 0.261799 0 base_link front_stereo_camera_link" />
+    -->
+    
     <!-- Robot TFs -->
     <!-- currently none -->
-</launch>
\ No newline at end of file
+</launch>
diff --git a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml
index ef426a07..a46f0b56 100644
--- a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml
+++ b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml
@@ -52,11 +52,13 @@ topics:
     type: std_msgs/msg/String
     from_domain: 2
     to_domain: 0
+
   /robot_3/behavior/behavior_tree_graphviz:
     type: std_msgs/msg/String
     from_domain: 3
     to_domain: 0
 
+#  GPS related topics -----------
   /robot_1/interface/mavros/global_position/raw/fix:
     type: sensor_msgs/msg/NavSatFix
     from_domain: 1
@@ -69,7 +71,19 @@ topics:
     type: sensor_msgs/msg/NavSatFix
     from_domain: 3
     to_domain: 0
-    
+  /robot_1/interface/mavros/global_position/global:
+    type: sensor_msgs/msg/NavSatFix
+    from_domain: 1
+    to_domain: 0
+  /robot_2/interface/mavros/global_position/global:
+    type: sensor_msgs/msg/NavSatFix
+    from_domain: 2
+    to_domain: 0
+  /robot_3/interface/mavros/global_position/global:
+    type: sensor_msgs/msg/NavSatFix
+    from_domain: 3
+    to_domain: 0
+
 
   # Bridge "/clock" topic from doman ID 2 to domain ID 3,
   # Override durability to be 'volatile' and override depth to be 1
diff --git a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz
index 65d21489..6800a4d1 100644
--- a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz
+++ b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz
@@ -12,7 +12,7 @@ Panels:
         - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1
         - /Global1
       Splitter Ratio: 0.590062141418457
-    Tree Height: 570
+    Tree Height: 704
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -79,6 +79,8 @@ Visualization Manager:
           Value: false
         map_ned:
           Value: false
+        mavros_enu:
+          Value: true
         odom:
           Value: false
         odom_ned:
@@ -99,32 +101,30 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        world:
-          map_FLU:
-            base_link_ground_truth:
-              base_link:
-                base_link_frd:
+        map_FLU:
+          map:
+            base_link:
+              base_link_frd:
+                {}
+              front_stereo:
+                left_camera:
                   {}
-                front_stereo:
-                  left_camera:
-                    {}
-                  right_camera:
-                    {}
-                ouster:
+                right_camera:
                   {}
-            map:
-              base_link_stabilized:
-                {}
-              look_ahead_point:
-                {}
-              look_ahead_point_stabilized:
-                {}
-              map_ned:
-                {}
-              tracking_point:
-                {}
-              tracking_point_stabilized:
+              ouster:
                 {}
+            base_link_stabilized:
+              {}
+            look_ahead_point:
+              {}
+            look_ahead_point_stabilized:
+              {}
+            map_ned:
+              {}
+            tracking_point:
+              {}
+            tracking_point_stabilized:
+              {}
       Update Interval: 0
       Value: true
     - Class: rviz_common/Group
@@ -235,11 +235,11 @@ Visualization Manager:
     - Class: rviz_common/Group
       Displays:
         - Class: rviz_default_plugins/Image
-          Enabled: true
+          Enabled: false
           Max Value: 1
           Median window: 5
           Min Value: 0
-          Name: Visual Odometry Features
+          Name: MACVO Image Features
           Normalize Range: true
           Topic:
             Depth: 5
@@ -247,7 +247,7 @@ Visualization Manager:
             History Policy: Keep Last
             Reliability Policy: Reliable
             Value: /robot_1/perception/visual_odometry_img
-          Value: true
+          Value: false
         - Alpha: 1
           Axes Length: 1
           Axes Radius: 0.10000000149011612
@@ -256,10 +256,10 @@ Visualization Manager:
           Enabled: true
           Head Length: 0.30000001192092896
           Head Radius: 0.10000000149011612
-          Name: Visual Odometry
+          Name: MACVO State Estimation
           Shaft Length: 1
           Shaft Radius: 0.05000000074505806
-          Shape: Arrow
+          Shape: Axes
           Topic:
             Depth: 5
             Durability Policy: Volatile
@@ -268,6 +268,40 @@ Visualization Manager:
             Reliability Policy: Reliable
             Value: /robot_1/perception/visual_odometry_pose
           Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud
+          Color: 255; 255; 255
+          Color Transformer: RGBF32
+          Decay Time: 1000
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: MACVO PointCloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Spheres
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /robot_1/perception/visual_odometry_points
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
       Enabled: true
       Name: Perception
     - Class: rviz_common/Group
@@ -315,7 +349,7 @@ Visualization Manager:
               Enabled: true
               Name: Trimmed Global Plan for DROAN
               Namespaces:
-                {}
+                global_plan: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -373,7 +407,168 @@ Visualization Manager:
               Enabled: true
               Name: Traj Library
               Namespaces:
-                {}
+                trajectory_0: true
+                trajectory_1: true
+                trajectory_10: true
+                trajectory_100: true
+                trajectory_101: true
+                trajectory_102: true
+                trajectory_103: true
+                trajectory_104: true
+                trajectory_105: true
+                trajectory_106: true
+                trajectory_107: true
+                trajectory_108: true
+                trajectory_109: true
+                trajectory_11: true
+                trajectory_110: true
+                trajectory_111: true
+                trajectory_112: true
+                trajectory_113: true
+                trajectory_114: true
+                trajectory_115: true
+                trajectory_116: true
+                trajectory_117: true
+                trajectory_118: true
+                trajectory_119: true
+                trajectory_12: true
+                trajectory_120: true
+                trajectory_121: true
+                trajectory_122: true
+                trajectory_123: true
+                trajectory_124: true
+                trajectory_125: true
+                trajectory_126: true
+                trajectory_127: true
+                trajectory_128: true
+                trajectory_129: true
+                trajectory_13: true
+                trajectory_130: true
+                trajectory_131: true
+                trajectory_132: true
+                trajectory_133: true
+                trajectory_134: true
+                trajectory_135: true
+                trajectory_136: true
+                trajectory_137: true
+                trajectory_138: true
+                trajectory_139: true
+                trajectory_14: true
+                trajectory_140: true
+                trajectory_141: true
+                trajectory_142: true
+                trajectory_143: true
+                trajectory_144: true
+                trajectory_145: true
+                trajectory_146: true
+                trajectory_147: true
+                trajectory_148: true
+                trajectory_149: true
+                trajectory_15: true
+                trajectory_150: true
+                trajectory_151: true
+                trajectory_152: true
+                trajectory_153: true
+                trajectory_154: true
+                trajectory_155: true
+                trajectory_156: true
+                trajectory_157: true
+                trajectory_158: true
+                trajectory_159: true
+                trajectory_16: true
+                trajectory_160: true
+                trajectory_161: true
+                trajectory_17: true
+                trajectory_18: true
+                trajectory_19: true
+                trajectory_2: true
+                trajectory_20: true
+                trajectory_21: true
+                trajectory_22: true
+                trajectory_23: true
+                trajectory_24: true
+                trajectory_25: true
+                trajectory_26: true
+                trajectory_27: true
+                trajectory_28: true
+                trajectory_29: true
+                trajectory_3: true
+                trajectory_30: true
+                trajectory_31: true
+                trajectory_32: true
+                trajectory_33: true
+                trajectory_34: true
+                trajectory_35: true
+                trajectory_36: true
+                trajectory_37: true
+                trajectory_38: true
+                trajectory_39: true
+                trajectory_4: true
+                trajectory_40: true
+                trajectory_41: true
+                trajectory_42: true
+                trajectory_43: true
+                trajectory_44: true
+                trajectory_45: true
+                trajectory_46: true
+                trajectory_47: true
+                trajectory_48: true
+                trajectory_49: true
+                trajectory_5: true
+                trajectory_50: true
+                trajectory_51: true
+                trajectory_52: true
+                trajectory_53: true
+                trajectory_54: true
+                trajectory_55: true
+                trajectory_56: true
+                trajectory_57: true
+                trajectory_58: true
+                trajectory_59: true
+                trajectory_6: true
+                trajectory_60: true
+                trajectory_61: true
+                trajectory_62: true
+                trajectory_63: true
+                trajectory_64: true
+                trajectory_65: true
+                trajectory_66: true
+                trajectory_67: true
+                trajectory_68: true
+                trajectory_69: true
+                trajectory_7: true
+                trajectory_70: true
+                trajectory_71: true
+                trajectory_72: true
+                trajectory_73: true
+                trajectory_74: true
+                trajectory_75: true
+                trajectory_76: true
+                trajectory_77: true
+                trajectory_78: true
+                trajectory_79: true
+                trajectory_8: true
+                trajectory_80: true
+                trajectory_81: true
+                trajectory_82: true
+                trajectory_83: true
+                trajectory_84: true
+                trajectory_85: true
+                trajectory_86: true
+                trajectory_87: true
+                trajectory_88: true
+                trajectory_89: true
+                trajectory_9: true
+                trajectory_90: true
+                trajectory_91: true
+                trajectory_92: true
+                trajectory_93: true
+                trajectory_94: true
+                trajectory_95: true
+                trajectory_96: true
+                trajectory_97: true
+                trajectory_98: true
+                trajectory_99: true
               Topic:
                 Depth: 5
                 Durability Policy: Volatile
@@ -504,7 +699,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 14.042609214782715
+      Distance: 26.4907283782959
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -519,10 +714,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5953989028930664
+      Pitch: 0.8853982090950012
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 1.1703985929489136
+      Yaw: 3.735398530960083
     Saved: ~
 Window Geometry:
   Displays:
@@ -531,10 +726,12 @@ Window Geometry:
     collapsed: false
   Front Left RGB:
     collapsed: false
-  Height: 1140
+  Height: 1043
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 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
+  MACVO Image Features:
+    collapsed: false
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -543,8 +740,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Visual Odometry Features:
-    collapsed: false
-  Width: 2190
-  X: 245
-  Y: 72
+  Width: 1920
+  X: 72
+  Y: 155
diff --git a/shutdown.sh b/shutdown.sh
new file mode 100755
index 00000000..98abb278
--- /dev/null
+++ b/shutdown.sh
@@ -0,0 +1,54 @@
+#!/bin/bash
+
+echo "-- Starting AirStack shutdown procedure --"
+echo ""
+
+# Ask about WinTAK shutdown
+read -p "Would you like to shut down WinTAK? (y/n): " shutdown_wintak
+
+if [[ $shutdown_wintak =~ ^[Yy]$ ]]; then
+    echo "Attempting to stop WinTAK virtual machine..."
+
+    # Check if WinTAK is running
+    vm_info=$(VBoxManage showvminfo "WinTAK" 2>/dev/null | grep "State:" || echo "")
+
+    if [[ $vm_info == *"running"* ]]; then
+        echo "Shutting down WinTAK..."
+        # Attempt graceful shutdown first
+        if ! VBoxManage controlvm "WinTAK" acpipowerbutton 2>/dev/null; then
+            echo "Graceful shutdown failed. Forcing power off..."
+            VBoxManage controlvm "WinTAK" poweroff 2>/dev/null
+        else
+            echo "WinTAK is shutting down gracefully."
+            # Wait a bit for the VM to shut down
+            echo "Waiting for WinTAK to finish shutting down..."
+            sleep 5
+        fi
+    else
+        echo "WinTAK is not currently running."
+    fi
+else
+    echo "WinTAK shutdown skipped."
+fi
+
+echo ""
+
+# Check if we're in the AirStack directory
+if [ ! -f "docker-compose.yaml" ]; then
+    echo "Error: docker-compose.yml not found."
+    echo "Please make sure you are in the AirStack directory."
+    exit 1
+fi
+
+# Stop docker compose services
+echo "Stopping Docker Compose services (Isaac Sim and robots)..."
+docker compose down
+
+# Revoke docker access to X-Server
+echo "Revoking Docker access to X-Server..."
+xhost -
+
+echo ""
+echo "Docker services have been stopped."
+echo ""
+echo "-- AirStack shutdown completed successfully --"
\ No newline at end of file
diff --git a/simulation/isaac-sim/docker/.gitignore b/simulation/isaac-sim/docker/.gitignore
index 94d7315c..c83ddadf 100644
--- a/simulation/isaac-sim/docker/.gitignore
+++ b/simulation/isaac-sim/docker/.gitignore
@@ -1,3 +1,4 @@
+isaac_sim_data/
 omni_pass.env
 .bash_history
 user.config.json
diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml
index bd4c7b9e..1d2b6469 100644
--- a/simulation/isaac-sim/docker/docker-compose.yaml
+++ b/simulation/isaac-sim/docker/docker-compose.yaml
@@ -1,6 +1,9 @@
 services:
   isaac-sim:
-    image: &image_tag ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${PROJECT_VERSION}_isaac-sim
+    profiles:
+      - ""
+      - sitl
+    image: &image_tag ${PROJECT_DOCKER_REGISTRY}/${PROJECT_NAME}:v${DOCKER_IMAGE_TAG}_isaac-sim
     build:
       context: ../
       dockerfile: docker/Dockerfile.isaac-ros
@@ -10,7 +13,7 @@ services:
     entrypoint: "" # override the default entrypoint with nothing
     # for some reason the tmux session manager only works when isaac is running in tmux
     command: >
-      bash -c " tmux new -d -s isaac 
+      bash -c "tmux new -d -s isaac 
       && tmux send-keys -t isaac \"ros2 launch isaacsim run_isaacsim.launch.py gui:=${DEFAULT_ISAAC_SCENE} play_sim_on_start:=${PLAY_SIM_ON_START}\" ENTER 
       && sleep infinity"
     # Interactive shell
@@ -44,23 +47,46 @@ services:
       - $HOME/.Xauthority:/root/.Xauthority
       - /tmp/.X11-unix:/tmp/.X11-unix
       # isaac sim stuff
-      - ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw
-      - ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw
-      - ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw
-      - ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw
-      - ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw
-      - ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw
-      - ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw
-      - ~/docker/isaac-sim/documents:/root/Documents:rw
+      - ./isaac_sim_data/cache/kit:/isaac-sim/kit/cache:rw
+      - ./isaac_sim_data/cache/ov:/root/.cache/ov:rw
+      - ./isaac_sim_data/cache/pip:/root/.cache/pip:rw
+      - ./isaac_sim_data/cache/glcache:/root/.cache/nvidia/GLCache:rw
+      - ./isaac_sim_data/cache/computecache:/root/.nv/ComputeCache:rw
+      - ./isaac_sim_data/logs:/root/.nvidia-omniverse/logs:rw
+      - ./isaac_sim_data/data:/root/.local/share/ov/data:rw
+      - ./isaac_sim_data/documents:/root/Documents:rw
       # IMPORTANT: set the version number without the trailing .0
       - ./user.config.json:/root/.local/share/ov/data/Kit/Isaac-Sim/4.2/user.config.json:rw
       - ./ui.py:/isaac-sim/kit/exts/omni.kit.widget.nucleus_connector/omni/kit/widget/nucleus_connector/ui.py:rw
       # developer stuff
       - .dev:/root/.dev:rw # developer config
       - .bashrc:/root/.bashrc:rw # bash config
-      - ../../common/inputrc:/etc/inputrc:rw # for using page up/down to search through command history	
+      - ../../common/inputrc:/etc/inputrc:rw # for using page up/down to search through command history
       # code
       - ../sitl_integration/kit-app-template/source/extensions/:/root/Documents/Kit/shared/exts/
       - ../sitl_integration:/sitl_integration:rw
       - ../sitl_integration/inputrc:/etc/inputrc
       - ../sitl_integration/config:/root/.nvidia-omniverse/config:rw
+
+  # -------------------------------------------------------------------------------------------------------------------
+  # dev container for developer debugging
+  isaac-sim-dev:
+    profiles: !override
+      - dev
+    extends: isaac-sim
+    container_name: isaac-sim-dev
+    command: >
+      bash -c "tmux new -d -s isaac 
+      && tmux send-keys -t isaac \"ros2 launch isaacsim run_isaacsim.launch.py\" ENTER 
+      && sleep infinity"
+
+  # ===================================================================================================================
+  # for running over ethernet for hitl to stream to the NVIDIA jetson device
+  isaac-sim-hitl:
+    extends:
+      service: isaac-sim
+    profiles: !override
+      - hitl
+    container_name: isaac-sim
+    network_mode: host
+    networks: !reset null
diff --git a/simulation/isaac-sim/sitl_integration/control_test.py b/simulation/isaac-sim/sitl_integration/control_test.py
index f62f4f38..b28a8875 100755
--- a/simulation/isaac-sim/sitl_integration/control_test.py
+++ b/simulation/isaac-sim/sitl_integration/control_test.py
@@ -3,12 +3,13 @@
 from rclpy.node import Node
 from geometry_msgs.msg import PoseStamped
 from mavros_msgs.srv import CommandBool, CommandTOL, SetMode
-from mavros_msgs.msg import PositionTarget
+from mavros_msgs.msg import PositionTarget, AttitudeTarget
 import time
 
 POS = 1
 POS_RAW = 2
 POS_VEL_RAW = 3
+ATT = 4
 
 class TimeValueSeries:
     def __init__(self):
@@ -40,7 +41,7 @@ def __init__(self):
         # Get parameters
         self.takeoff_wait_time = 10.
         self.takeoff_height = 5.
-        self.setpoint_mode = POS_VEL_RAW
+        self.setpoint_mode = ATT#POS_VEL_RAW
         self.control_rate = 3.
         self.do_plan = False
 
@@ -63,15 +64,17 @@ def __init__(self):
         self.y_plan.add(40., 0.)
 
         # MAVROS service clients
-        self.set_mode_client = self.create_client(SetMode, 'mavros/set_mode')
-        self.arming_client = self.create_client(CommandBool, 'mavros/cmd/arming')
-        self.takeoff_client = self.create_client(CommandTOL, 'mavros/cmd/takeoff')
+        self.set_mode_client = self.create_client(SetMode, 'interface/mavros/set_mode')
+        self.arming_client = self.create_client(CommandBool, 'interface/mavros/cmd/arming')
+        self.takeoff_client = self.create_client(CommandTOL, 'interface/mavros/cmd/takeoff')
 
         # Publishers
         if self.setpoint_mode == POS:
-            self.control_publisher = self.create_publisher(PoseStamped, 'mavros/setpoint_position/local', 10)
+            self.control_publisher = self.create_publisher(PoseStamped, 'interface/mavros/setpoint_position/local', 10)
+        elif self.setpoint_mode == ATT:
+            self.control_publisher = self.create_publisher(AttitudeTarget, 'interface/mavros/setpoint_raw/attitude', 10)
         else:
-            self.control_publisher = self.create_publisher(PositionTarget, 'mavros/setpoint_raw/local', 10)
+            self.control_publisher = self.create_publisher(PositionTarget, 'interface/mavros/setpoint_raw/local', 10)
 
         # Start mission
         self.set_guided_mode_and_takeoff()
@@ -162,6 +165,17 @@ def start_publishing_control(self):
                 msg.velocity.y = 0.
                 msg.velocity.z = 0.
                 msg.yaw = 0.0
+            elif self.setpoint_mode == ATT:
+                msg = AttitudeTarget()
+                msg.type_mask = AttitudeTarget.IGNORE_ROLL_RATE | AttitudeTarget.IGNORE_PITCH_RATE
+                msg.orientation.x = 0.0871557
+                msg.orientation.y = 0.
+                msg.orientation.z = 0.
+                msg.orientation.w = 0.9961947
+                msg.body_rate.x = 2.
+                msg.body_rate.y = 2.
+                msg.body_rate.z = 2.
+                msg.thrust = .65
 
             self.control_publisher.publish(msg)
             #rate.sleep()
diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml
index 185a394c..ed8e093f 100644
--- a/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml
+++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/standalone/standalone.yaml
@@ -12,10 +12,10 @@ windows:
         - echo $AUTONOMY_STACK_PORT
         - mavproxy.py --streamrate=100 --master tcp:127.0.0.1:$BASE_PORT --out udp:127.0.0.1:$ASCENT_SITL_PORT --out udp:127.0.0.1:$ISAAC_SIM_PORT --out udp:127.0.0.1:$AUTONOMY_STACK_PORT
       - shell_command:
-        - sleep 10
         - export ROS_DOMAIN_ID=$DOMAIN_ID
         # - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" namespace:=$NAMESPACE/interface/mavros --ros-args -p use_sim_time:=true
         - >
+          while true; do
           ros2 run mavros mavros_node 
           --ros-args 
           -r __ns:=/$NAMESPACE/interface/mavros 
@@ -24,4 +24,5 @@ windows:
           -p tgt_component:=1 
           -p fcu_protocol:=v2.0
           --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_pluginlists.yaml
-          --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_config.yaml
\ No newline at end of file
+          --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_config.yaml 2>&1 | tee >(grep -m 1 "channel closed" && kill $$);
+          done
\ No newline at end of file
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.airstack/airlab/airstack/ogn/OgnAscentNodeDatabase.py b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.airstack/airlab/airstack/ogn/OgnAscentNodeDatabase.py
index 959d07db..a34ff8a0 100644
--- a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.airstack/airlab/airstack/ogn/OgnAscentNodeDatabase.py
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.airstack/airlab/airstack/ogn/OgnAscentNodeDatabase.py
@@ -88,7 +88,7 @@ def incremental_rotate(prim):
     else:
         o = prim.GetAttribute("xformOp:rotateXYZ")
         if o.Get() != None:
-            o.Set(o.Get() + Gf.Vec3d(0.0, 27, 0.0))
+            o.Set(o.Get() + Gf.Vec3d(0.0, 50, 0.0))
 
 
 class OgnAscentNodeDatabase(og.Database):
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/__init__.py b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/__init__.py
new file mode 100644
index 00000000..a54368b2
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/__init__.py
@@ -0,0 +1,11 @@
+# SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: LicenseRef-NvidiaProprietary
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+
+from .extension import *
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/extension.py b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/extension.py
new file mode 100644
index 00000000..b58d5e9d
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/extension.py
@@ -0,0 +1,219 @@
+import omni.ext
+import subprocess
+
+import omni.ui as ui
+import omni.kit.window.filepicker as filepicker
+from omni.isaac.core import World
+from omni.isaac.core.utils.stage import add_reference_to_stage
+from pxr import Usd
+from omni.isaac.core import World
+# from omni.ext import get_extension_path
+import omni.usd
+from pxr import UsdGeom, Gf, UsdPhysics
+import omni.graph.core as og
+import time
+
+class MyExtension(omni.ext.IExt):
+    def on_startup(self):
+        """Called when the extension is loaded."""
+        print("[GimbalExtension] Startup called")  # Debug log
+
+        # self.world = World.get_instance()
+        self.ui = GimbalUI()
+        print("[GimbalExtension] UI initialized")  # Debug log
+
+    def on_shutdown(self):
+        """Called when the extension is unloaded."""
+        # self.world.cleanup()
+        self.ui.window.destroy()
+        
+class GimbalUI:
+    def __init__(self):
+        self.window = ui.Window("Gimbal Extension UI", width=400, height=300)
+        self.usd_file_path = ""
+        self.robot_prim_path = ""
+        self.robot_name = 1
+        self.default_usd_path = "omniverse://airlab-storage.andrew.cmu.edu:8443/Library/Assets/Gimbal/gimbal_test.usd"
+        self.default_robot_path = "/World/TEMPLATE_spirit_uav_robot/map_FLU/spirit_uav"
+
+        self.build_ui()
+
+    def build_ui(self):
+        with self.window.frame:
+            with ui.VStack(spacing=10):
+                # USD File Input
+                ui.Label("USD File Path", height=20)
+                with ui.HStack(height=30):
+                    self.usd_input_field = ui.StringField(
+                        model=ui.SimpleStringModel(self.default_usd_path), height=20
+                    )
+                    ui.Button("Browse", clicked_fn=self.open_file_picker)
+    
+                # Robot Prim Path Input
+                ui.Label("Robot Prim Path (copy from Stage)", height=20)
+                self.robot_prim_input_field = ui.StringField(
+                    model=ui.SimpleStringModel(self.default_robot_path), height=20
+                )
+
+                # Robot Name Input
+                ui.Label("Robot Index", height=20)
+                self.robot_name_input_field = ui.StringField(
+                    model=ui.SimpleStringModel("2"), height=20
+                )
+                # Apply Button
+                ui.Button("Apply and Add Gimbal", height=40, clicked_fn=self.on_apply)
+
+    def open_file_picker(self):
+        """Opens a file picker to select the USD file."""
+        file_picker = filepicker.FilePickerDialog(
+            title="Select Gimbal USD File",
+            apply_button_label="Select",
+            file_extension_filter="*.usd",
+            apply_clicked_fn=self.on_file_selected
+        )
+        file_picker.show()
+
+    def on_file_selected(self, file_path):
+        """Callback for file selection."""
+        self.usd_input_field.model.set_value(file_path)
+
+    def on_apply(self):
+        """Apply button callback to add the gimbal and configure the ActionGraph."""
+        self.usd_file_path = self.usd_input_field.model.get_value_as_string()
+        self.robot_prim_path = self.robot_prim_input_field.model.get_value_as_string()
+        self.robot_name = self.robot_name_input_field.model.get_value_as_string()
+
+        # Validate input
+        if not self.usd_file_path or not self.robot_prim_path or not self.robot_name:
+            print("Please fill in all fields.")
+            return
+
+        # Add the gimbal to the stage
+        self.add_gimbal_to_stage()
+
+    def find_existing_op(self, xform_ops, op_type):
+        for op in xform_ops:
+            if op.GetOpName() == op_type:
+                return op
+        return None
+
+    def add_gimbal_to_stage(self):
+        """Adds the gimbal to the robot and configures the OmniGraph."""
+        stage = omni.usd.get_context().get_stage()
+
+        # Add the USD file reference
+        gimbal_prim_path = f"{self.robot_prim_path}/gimbal"
+        add_reference_to_stage(self.usd_file_path, gimbal_prim_path)
+
+
+        # Apply transformations (scale and translation)
+        gimbal_prim = stage.GetPrimAtPath(gimbal_prim_path)
+        if gimbal_prim.IsValid():
+            gimbal_xform = UsdGeom.Xformable(gimbal_prim)
+            xform_ops = gimbal_xform.GetOrderedXformOps()
+
+            # Check if a scale operation already exists
+            scale_op = self.find_existing_op(xform_ops, "xformOp:scale")
+            if not scale_op:
+                scale_op = gimbal_xform.AddScaleOp()
+            scale_value = Gf.Vec3d(0.01, 0.01, 0.01)
+            scale_op.Set(scale_value)
+
+            # Check if a translate operation already exists
+            translate_op = self.find_existing_op(xform_ops, "xformOp:translate")
+            if not translate_op:
+                translate_op = gimbal_xform.AddTranslateOp()
+            translation_value = Gf.Vec3d(0.02, 0.015, 0.1)
+            translate_op.Set(translation_value)
+
+            print(f"Gimbal added at {gimbal_prim_path} with scale {scale_value} and translation {translation_value}.")
+
+        # Add a fixed joint between the gimbal and the robot
+        self.add_fixed_joint(stage, self.robot_prim_path, gimbal_prim_path)
+
+        # """Enables the ActionGraph within the gimbal and sets inputs."""
+        action_graph_path = f"{gimbal_prim_path}/ActionGraph"
+        action_graph_prim = stage.GetPrimAtPath(action_graph_path)
+
+        if action_graph_prim.IsValid():
+            # Access the graph
+            graph = og.Controller.graph(action_graph_path)
+            graph_handle = og.get_graph_by_path(action_graph_path)
+
+            # Set the input value
+            node_path = "/World/ActionGraph/MyNode"  # Replace with the path to your node
+            input_name = "myInput"  # Replace with the name of the input
+            value = 10  # Replace with the value you want to set
+
+            # Define the path to ros2_context node
+            ros2_context_path = action_graph_path+"/ros2_context"
+            self.list_node_attributes(ros2_context_path)
+
+            self.set_or_create_node_attribute(ros2_context_path, "inputs:domain_id", int(self.robot_name))     
+            self.set_or_create_node_attribute(action_graph_path+"/ros2_subscriber", "inputs:topicName", "robot_"+self.robot_name+"/gimbal/not_used")
+            self.set_or_create_node_attribute(action_graph_path+"/ros2_subscribe_joint_state", "inputs:topicName", "robot_"+self.robot_name+"/gimbal/joint_command")
+            self.set_or_create_node_attribute(action_graph_path+"/ros2_publish_joint_state", "inputs:topicName", "robot_"+self.robot_name+"/gimbal/joint_states")
+            self.set_or_create_node_attribute(action_graph_path+"/ros2_camera_helper", "inputs:topicName", "robot_"+self.robot_name+"/gimbal/rgb")
+
+            # og.Controller.attribute(action_graph_path+"/ros2_context.inputs:domain_id").set(int(self.robot_name))
+            # og.Controller.attribute(action_graph_path+"/ros2_subscriber.inputs:topicName").set("robot_"+self.robot_name+"/gimbal_yaw_control")
+            # og.Controller.attribute(action_graph_path+"/ros2_subscribe_joint_state.inputs:topicName").set("robot_"+self.robot_name+"/joint_command")
+            # og.Controller.attribute(action_graph_path+"/ros2_publish_joint_state.inputs:topicName").set("robot_"+self.robot_name+"/joint_states")
+
+
+    def add_fixed_joint(self, stage, robot_prim_path, gimbal_prim_path):
+        """Adds a fixed joint between the robot and the gimbal."""
+        joint_path = f"{gimbal_prim_path}/FixedJoint"
+        joint_prim = stage.DefinePrim(joint_path, "PhysicsFixedJoint")
+
+        # Define the fixed joint's relationship to the robot and gimbal components
+        joint = UsdPhysics.FixedJoint(joint_prim)
+        if not joint:
+            print(f"Failed to create fixed joint at {joint_path}")
+            return
+
+        # Set joint relationships
+        joint.CreateBody0Rel().SetTargets([f"{robot_prim_path}/base_link/meshes/Cone"])
+        joint.CreateBody1Rel().SetTargets([f"{gimbal_prim_path}/yaw"])
+
+        print(f"Fixed joint created between {robot_prim_path} and {gimbal_prim_path}.")
+
+    def list_node_attributes(self, node_path):
+        """Lists all attributes of a given node in OmniGraph."""
+        stage = omni.usd.get_context().get_stage()
+        
+        if not stage:
+            print("Error: USD stage not found.")
+            return
+        
+        node_prim = stage.GetPrimAtPath(node_path)
+
+        if not node_prim.IsValid():
+            print(f"Error: Node not found at {node_path}")
+            return
+
+        print(f"Attributes in node '{node_path}':")
+
+        for attr in node_prim.GetAttributes():
+            print(f"- {attr.GetName()}")       
+
+    def set_or_create_node_attribute(self, node_path, attribute_name, value):
+        """Sets an attribute value for a given node in OmniGraph, creating it if necessary."""
+        stage = omni.usd.get_context().get_stage()
+        node_prim = stage.GetPrimAtPath(node_path)
+
+        if not node_prim.IsValid():
+            print(f"Error: Node not found at {node_path}")
+            return
+
+        attr = node_prim.GetAttribute(attribute_name)
+
+        if not attr:
+            print(f"Attribute {attribute_name} not found, creating it...")
+            attr = node_prim.CreateAttribute(attribute_name, Sdf.ValueTypeNames.Int)
+        
+        if attr:
+            attr.Set(value)
+            print(f"Set {attribute_name} to {value} on node {node_path}.")
+        else:
+            print(f"Failed to create or set attribute {attribute_name}.")
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/tests/__init__.py b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/tests/__init__.py
new file mode 100644
index 00000000..880c75da
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/tests/__init__.py
@@ -0,0 +1,11 @@
+# SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: LicenseRef-NvidiaProprietary
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+
+from .test_hello_world import *
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/tests/test_hello_world.py b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/tests/test_hello_world.py
new file mode 100644
index 00000000..77e854a4
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/airlab/gimbal/tests/test_hello_world.py
@@ -0,0 +1,55 @@
+# SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: LicenseRef-NvidiaProprietary
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+
+# NOTE:
+#   omni.kit.test - std python's unittest module with additional wrapping to add suport for async/await tests
+#   For most things refer to unittest docs: https://docs.python.org/3/library/unittest.html
+import omni.kit.test
+
+# Extension for writing UI tests (to simulate UI interaction)
+import omni.kit.ui_test as ui_test
+
+# Import extension python module we are testing with absolute import path, as if we are external user (other extension)
+import airlab.tmux_manager
+
+
+# Having a test class dervived from omni.kit.test.AsyncTestCase declared on the root of module will make it auto-discoverable by omni.kit.test
+class Test(omni.kit.test.AsyncTestCase):
+    # Before running each test
+    async def setUp(self):
+        pass
+
+    # After running each test
+    async def tearDown(self):
+        pass
+
+    # Actual test, notice it is an "async" function, so "await" can be used if needed
+    async def test_hello_public_function(self):
+        result = airlab.tmux_manager.some_public_function(4)
+        self.assertEqual(result, 256)
+
+    async def test_window_button(self):
+
+        # Find a label in our window
+        label = ui_test.find("TMUX Manager//Frame/**/Label[*]")
+
+        # Find buttons in our window
+        add_button = ui_test.find("TMUX Manager//Frame/**/Button[*].text=='Add'")
+        reset_button = ui_test.find("TMUX Manager//Frame/**/Button[*].text=='Reset'")
+
+        # Click reset button
+        await reset_button.click()
+        self.assertEqual(label.widget.text, "empty")
+
+        await add_button.click()
+        self.assertEqual(label.widget.text, "count: 1")
+
+        await add_button.click()
+        self.assertEqual(label.widget.text, "count: 2")
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/config/extension.toml b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/config/extension.toml
new file mode 100644
index 00000000..6f891211
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/config/extension.toml
@@ -0,0 +1,73 @@
+# SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: LicenseRef-NvidiaProprietary
+#
+# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
+# property and proprietary rights in and to this material, related
+# documentation and any modifications thereto. Any use, reproduction,
+# disclosure or distribution of this material and related documentation
+# without an express license agreement from NVIDIA CORPORATION or
+# its affiliates is strictly prohibited.
+
+[package]
+# Semantic Versionning is used: https://semver.org/
+version = "0.1.0"
+
+# Lists people or organizations that are considered the "authors" of the package.
+authors = [
+    "Author Name <author_name@example.com>",
+]
+
+# The title and description fields are primarily for displaying extension info in the UI
+title = 'Gimbal extension'
+description = "The simplest python UI extension example. Use it as a starting point for your extensions."
+
+# Path (relative to the root) or content of readme markdown file for UI.
+readme = "docs/README.md"
+
+# Path (relative to the root) of changelog
+# More info on writing changelog: https://keepachangelog.com/en/1.0.0/
+changelog = "docs/CHANGELOG.md"
+
+# URL of the extension source repository.
+# repository = "https://github.com/example/repository_name"
+
+# One of categories for the UI.
+category = "Example"
+
+# Keywords for the extension
+keywords = ["kit", "example"]
+
+# Watch the .ogn files for hot reloading (only works for Python files)
+[fswatcher.patterns]
+include = ["*.ogn", "*.py"]
+exclude = ["Ogn*Database.py"]
+
+# Preview image and icon. Folder named "data" automatically goes in git lfs (see .gitattributes file).
+# Preview image is shown in "Overview" of Extensions window. Screenshot of an extension might be a good preview image.
+preview_image = "data/preview.png"
+
+# Icon is shown in Extension manager. It is recommended to be square, of size 256x256.
+icon = "data/icon.png"
+
+# Use omni.ui to build simple UI
+[dependencies]
+"omni.kit.uiapp" = {}
+"omni.kit.test" = {}
+"omni.graph" = {}
+
+# Main python module this extension provides, it will be publicly available as "import airlab.gimbal".
+[[python.module]]
+name = "airlab.gimbal"
+
+[[test]]
+# Extra dependencies only to be used during test run
+dependencies = [
+    "omni.kit.test",
+    "omni.kit.ui_test"  # UI testing extension
+]
+
+[documentation]
+pages = [
+    "docs/Overview.md",
+    "docs/CHANGELOG.md",
+]
\ No newline at end of file
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/data/icon.png b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/data/icon.png
new file mode 100644
index 00000000..70e17a5c
Binary files /dev/null and b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/data/icon.png differ
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/data/preview.png b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/data/preview.png
new file mode 100644
index 00000000..5c3fc196
Binary files /dev/null and b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/data/preview.png differ
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/CHANGELOG.md b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/CHANGELOG.md
new file mode 100644
index 00000000..3c031de5
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/CHANGELOG.md
@@ -0,0 +1,7 @@
+# Changelog
+
+The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
+
+
+## [0.1.0] - 2024-07-17
+- Initial version of extension UI template with a window
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/Overview.md b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/Overview.md
new file mode 100644
index 00000000..4bba659e
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/Overview.md
@@ -0,0 +1 @@
+# Overview
\ No newline at end of file
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/README.md b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/README.md
new file mode 100644
index 00000000..1c0c5033
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/docs/README.md
@@ -0,0 +1,3 @@
+# TMUX Manager [airlab.tmux_manager]
+
+A simple python UI extension example. Use it as a starting point for your extensions.
diff --git a/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/premake5.lua b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/premake5.lua
new file mode 100644
index 00000000..e2926d0e
--- /dev/null
+++ b/simulation/isaac-sim/sitl_integration/kit-app-template/source/extensions/airlab.gimbal/premake5.lua
@@ -0,0 +1,11 @@
+-- Use folder name to build extension name and tag. Version is specified explicitly.
+local ext = get_current_extension_info()
+
+project_ext (ext)
+
+-- Link only those files and folders into the extension target directory
+repo_build.prebuild_link {
+    { "data", ext.target_dir.."/data" },
+    { "docs", ext.target_dir.."/docs" },
+    { "airlab", ext.target_dir.."/airlab" },
+}