diff --git a/.env b/.env index 345803b0d..c633cafdd 100644 --- a/.env +++ b/.env @@ -1,10 +1,10 @@ # This top-level .env file under AirStack/ defines variables that are propagated through docker-compose.yaml PROJECT_NAME="airstack" # auto-generated from git commit hash -DOCKER_IMAGE_TAG="3f238b1" +DOCKER_IMAGE_TAG="008839f" # 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/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/common/ros_packages/straps_msgs b/common/ros_packages/straps_msgs new file mode 160000 index 000000000..290987632 --- /dev/null +++ b/common/ros_packages/straps_msgs @@ -0,0 +1 @@ +Subproject commit 290987632f7486bca099f5552f49f4f9dd100112 diff --git a/configure.sh b/configure.sh index a07af52c9..997a53b9b 100755 --- a/configure.sh +++ b/configure.sh @@ -56,3 +56,10 @@ 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 + +# Git Hooks +echo -e "${BOLDCYAN}3. Setting up Git Hooks${ENDCOLOR}" +cp ${SCRIPT_DIR}/git-hooks/docker-versioning/update-docker-image-tag.pre-commit ${SCRIPT_DIR}/.git/hooks/pre-commit + + +echo -e "${BOLDCYAN}Setup Complete${ENDCOLOR}" \ No newline at end of file diff --git a/docs/getting_started.md b/docs/getting_started.md index 251f457ea..14ea994e9 100644 --- a/docs/getting_started.md +++ b/docs/getting_started.md @@ -75,13 +75,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 +./launch.sh # This will launch the docker containers, isaac sim, and WinTAK ``` This will automatically launch and play the Isaac scene specified under `AirStack/.env` (default is the Fire Academy). @@ -99,5 +93,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 +./shutdown.sh # This will stop and remove the docker containers, isaac sim, and WinTAK ``` 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 000000000..b4e7b0506 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 000000000..ed19d334c 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 000000000..73d215166 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 000000000..a7f058e67 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 000000000..e0835d9b5 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 000000000..6affaecad --- /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 000000000..cf118439a --- /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: -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` (`-operator`). +3. The extracted query is published to `/query/text`. +4. The response to the query is expected on `/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 find ` - 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 eb9848635..0c94e6266 100644 --- a/docs/ground_control_station/index.md +++ b/docs/ground_control_station/index.md @@ -13,27 +13,17 @@ WinTAK is setup as auto start on boot and connects to Airlabs TAK-Server. Its ru ![Setup](asset/setup.png) -1. Set your `ANDREWID` as an environment variable. +Run the following command from the setup folder ```bash -export ANDREWID= -``` -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" - -``` -3. Run WinTAK using the following commands. -```bash -VBoxManage startvm "WinTAK" +# Move to the directory: +cd ground_control_station/setup +# Execute the script +./setup_ground_control_station.sh ``` NOTE: If it asks to reset the password, please reset to current password. -![WinTAK](asset/WinTAK_on_windows_virtualbox_vm.png) \ No newline at end of file +![WinTAK](asset/WinTAK_on_windows_virtualbox_vm.png) + +## 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 e69de29bb..ad99141af 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/ground_control_station/docker/.bash_history b/ground_control_station/docker/.bash_history index 929c68700..c656cdd8b 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 e15dfc4cf..a12c0fc45 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 8b57b92e3..7fbfa2ff8 100644 --- a/ground_control_station/docker/Dockerfile.gcs +++ b/ground_control_station/docker/Dockerfile.gcs @@ -16,26 +16,20 @@ 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 \ + && 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 88b3f5ee8..0899ee41b 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -6,7 +6,6 @@ services: extends: file: ./ground-control-station-base-docker-compose.yaml service: ground-control-station-base - container_name: ground-control-station networks: - airstack_network ports: @@ -25,6 +24,3 @@ services: - $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 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 af108a917..000000000 --- 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 000000000..51d4bede8 --- /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/launch/gcs.launch.xml b/ground_control_station/ros_ws/src/gcs_bringup/launch/gcs.launch.xml index a10e3ce84..1814019d0 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 @@ --> + + + + 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 139fff7db..8e5f95deb 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 @@ - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + 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 000000000..5b6b0720c --- /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 f44a4e10d..2ec3da59e 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 c4903234e..000000000 --- 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 000000000..95fd25357 --- /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 2a58cc563..b9eb356f9 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 591fafc52..6a46e93e2 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 8550d4d7e..9463ecf92 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 6f398f491..10eb4dfd0 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 000000000..4b2e509be --- /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 7aeb110ae..2d79406f6 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 13fd51cc5..a5514aa42 --- 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 b95f6b4c3..a14367ec6 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 000000000..5da67aabc --- /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/launch.sh b/launch.sh new file mode 100755 index 000000000..9e025f598 --- /dev/null +++ b/launch.sh @@ -0,0 +1,67 @@ +#!/bin/bash + +# Allow docker access to X-Server +echo "Allowing Docker access to X-Server..." +xhost + + +# 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 + +# Start docker compose services +echo "Starting Docker Compose services (Isaac Sim and robots)..." +docker compose up -d + +echo "" +echo "Docker services are now running." +echo "" + +# Display information about WinTAK +echo "To learn more about WinTAK, visit: https://www.youtube.com/watch?v=fiBt0wEiKh8" +echo "AirStack uses WinTAK for human-robot team awareness and collaboration in mission environments." + +# Ask about WinTAK +read -p "Would you like to run WinTAK? (y/n): " run_wintak + +if [[ $run_wintak =~ ^[Yy]$ ]]; then + echo "Attempting to start WinTAK virtual machine..." + + # Check if WinTAK is already running + vm_info=$(VBoxManage showvminfo "WinTAK" 2>/dev/null | grep "State:" || echo "") + + if [[ $vm_info == *"running"* ]]; then + echo "WinTAK is already running. Please check your windows." + # If not running, try to start the VM + elif ! VBoxManage startvm "WinTAK" 2>/dev/null; then + echo "Failed to start WinTAK. It may not be set up properly." + read -p "Would you like to set up WinTAK now? (y/n): " setup_wintak + + if [[ $setup_wintak =~ ^[Yy]$ ]]; then + # Check if setup script exists + if [ -f "ground_control_station/installation/setup_ground_control_station.sh" ]; then + echo "Running WinTAK setup script..." + bash ground_control_station/installation/setup_ground_control_station.sh + else + echo "Error: Setup script not found at ground_control_station/installation/setup_ground_control_station.sh" + exit 1 + fi + else + echo "WinTAK setup skipped." + fi + else + echo "WinTAK virtual machine started successfully." + fi +else + echo "WinTAK startup skipped." +fi + +echo "-------------------------------------------------------------" +echo -e "\e[38;5;51m █████ \e[38;5;39m██ \e[38;5;27m██████ \e[38;5;63m███████ \e[38;5;99m████████ \e[38;5;135m█████ \e[38;5;171m██████ \e[38;5;207m██ ██ \e[0m" +echo -e "\e[38;5;51m ██ ██ \e[38;5;39m██ \e[38;5;27m██ ██ \e[38;5;63m██ \e[38;5;99m██ \e[38;5;135m██ ██ \e[38;5;171m██ \e[38;5;207m██ ██ \e[0m" +echo -e "\e[38;5;51m ███████ \e[38;5;39m██ \e[38;5;27m██████ \e[38;5;63m███████ \e[38;5;99m██ \e[38;5;135m███████ \e[38;5;171m██ \e[38;5;207m█████ \e[0m" +echo -e "\e[38;5;51m ██ ██ \e[38;5;39m██ \e[38;5;27m██ ██ \e[38;5;63m ██ \e[38;5;99m██ \e[38;5;135m██ ██ \e[38;5;171m██ \e[38;5;207m██ ██ \e[0m" +echo -e "\e[38;5;51m ██ ██ \e[38;5;39m██ \e[38;5;27m██ ██ \e[38;5;63m███████ \e[38;5;99m██ \e[38;5;135m██ ██ \e[38;5;171m ██████ \e[38;5;207m██ ██ \e[0m" +echo " ------------- Launched AirStack Successfully ---------------" \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index 7d6dddf3d..c742b6bb0 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -107,8 +107,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: 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 f89913896..a46f0b56b 100644 --- a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml +++ b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml @@ -19,11 +19,6 @@ topics: to_domain: 0 type: nav_msgs/msg/Odometry - /robot_1/sensors/front_stereo/left/image_rect: - from_domain: 1 - to_domain: 0 - type: sensor_msgs/msg/Image - /robot_2/vdb_mapping/vdb_map_visualization: from_domain: 2 to_domain: 0 @@ -57,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 @@ -74,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/shutdown.sh b/shutdown.sh new file mode 100755 index 000000000..98abb278e --- /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