diff --git a/ground_control_station/docker/.env b/ground_control_station/docker/.env index 97dc4f7b..e15dfc4c 100644 --- a/ground_control_station/docker/.env +++ b/ground_control_station/docker/.env @@ -20,6 +20,8 @@ TAK_SUBSCRIBER_FILEPATH=src/ros2tak_tools/scripts/tak_subscriber.py MQTT_USERNAME= # Enter the MQTT username MQTT_PASSWORD= # Enter the MQTT password +# Chat interface configuration ----------------------------------------- +AI_AGENT_NAME=aerolens.ai (BOT) # GSTREAMER TO ROS NODES ----------------------------------------------- @@ -30,3 +32,7 @@ CAMERA1_ROS_TOPIC=/view1/image_raw # CAMERA 2 CAMERA2_STREAM_IP=rtsp:// CAMERA2_ROS_TOPIC=/view2/image_raw + + +# ROS VARIABLES ------------------------------------------------------- +ROS_DOMAIN_ID=200 \ No newline at end of file diff --git a/ground_control_station/docker/Dockerfile.cot2planner_agent b/ground_control_station/docker/Dockerfile.TAK similarity index 100% rename from ground_control_station/docker/Dockerfile.cot2planner_agent rename to ground_control_station/docker/Dockerfile.TAK diff --git a/ground_control_station/docker/Dockerfile.ros2casevac_agent b/ground_control_station/docker/Dockerfile.ros2casevac_agent new file mode 100644 index 00000000..18301286 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.ros2casevac_agent @@ -0,0 +1,37 @@ +# Use the official ROS 2 Humble base image +FROM ros:humble + +ARG ROS_WS_DIR + +# Set working directory +WORKDIR ${ROS_WS_DIR} + +# Update and install necessary dependencies +RUN apt-get update && apt-get install -y \ + python3-pip \ + ros-humble-rosbag2 \ + ros-humble-rosbag2-storage-mcap \ + python3-colcon-common-extensions \ + && rm -rf /var/lib/apt/lists/* + +# Upgrade pip and install Python dependencies +RUN pip3 install --upgrade pip \ + && pip3 install setuptools==57.5.0 pytak pyyaml \ + && pip3 install paho-mqtt + +# Source ROS 2 setup file to ensure environment variables are set +# You may want to add this to ENTRYPOINT or CMD if you're working interactively +RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc + +# COPY the agent code into the container +COPY ground_control_station/ros_ws/src/ros2tak_tools ${ROS_WS_DIR}/src/ros2tak_tools +COPY common/ros_packages/straps_msgs ${ROS_WS_DIR}/src/straps_msgs + +# Build the ROS 2 workspace +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + colcon build --symlink-install --packages-select straps_msgs && \ + colcon build --symlink-install" + +# Source the workspace setup file to ensure environment variables are set +RUN echo "source ${ROS_WS_DIR}/install/setup.bash" >> ~/.bashrc + diff --git a/ground_control_station/docker/Dockerfile.ros2cot_agent b/ground_control_station/docker/Dockerfile.ros2tak_tools similarity index 100% rename from ground_control_station/docker/Dockerfile.ros2cot_agent rename to ground_control_station/docker/Dockerfile.ros2tak_tools diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index c6c0d133..bfd385d1 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -47,3 +47,185 @@ services: - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml + + + ####################### GSTREAMER TO ROS TOPICS ###################### + # gst-ros-bridge-topic1: + # container_name: "${PROJECT_NAME}-gst_ros_bridge1" + # image: "${PROJECT_NAME}/gcs/gst-ros-bridge" + # build: + # context: . + # dockerfile: Dockerfile.gst-ros-bridge + # command: > + # /bin/bash -c 'source /ros_ws/install/setup.bash && gst-launch-1.0 --gst-plugin-path=/ros_ws/install/gst_bridge/lib/gst_bridge/ + # rtspsrc location="${CAMERA1_STREAM_IP}" latency=0 ! + # rtph265depay ! h265parse ! avdec_h265 ! videoconvert ! + # rosimagesink ros-topic="${CAMERA1_ROS_TOPIC}"' + # environment: + # - DISPLAY=${DISPLAY} + # - DOCKER_BUILDKIT=0 + # - CAMERA1_STREAM_IP=${CAMERA1_STREAM_IP} + # - CAMERA1_ROS_TOPIC=${CAMERA1_ROS_TOPIC} + # volumes: + # - /tmp/.X11-unix:/tmp/.X11-unix + # network_mode: host + + ####################### 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: + build: + context: ../ + dockerfile: docker/Dockerfile.ros2tak_tools + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/ros2cot_agent" + 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: + build: + context: ../ + dockerfile: docker/Dockerfile.tak_publisher + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/tak_publisher" + 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: + build: + context: ../ + dockerfile: docker/Dockerfile.tak_subscriber + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/tak_subscriber" + 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: + build: + context: ../../ + dockerfile: ground_control_station/docker/Dockerfile.TAK + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/cot2planner_agent" + 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" ] + + # ################## ROS2COT_AGENT + ros2casevac_agent: + build: + context: ../../ + dockerfile: ground_control_station/docker/Dockerfile.ros2casevac_agent + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/ros2casevac_agent" + container_name: "${PROJECT_NAME}-ros2casevac_agent" + stdin_open: true + tty: true + restart: unless-stopped + depends_on: + mqtt: + condition: service_healthy + environment: + - ROS_WS_DIR=${ROS_WS_DIR} + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} + working_dir: $ROS_WS_DIR + # 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/ros2casevac_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" ] + + # ################## GCS_AI_AGENT + gcs_ai_agent: + build: + context: ../../ + dockerfile: ground_control_station/docker/Dockerfile.TAK + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/gcs_ai_agent" + 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" ] + + + ########### NETWORKS ########### +networks: + airstack_network: + driver: bridge diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml b/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml index c347cba6..f44a4e10 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml +++ b/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml @@ -72,29 +72,46 @@ services: # TAK_Subscriber (below) service has more information on the ROS topics. topic_name: planner_events # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + chat2ros_agent: + mqtt_subcribe_topic: dsta-operator # 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: dsta-operator + + ros2casevac_agent: + # this service is used to generate ROS messages from HOSTIP:PORT to ROS topics. + # TAK_Subscriber (below) service has more information on the ROS topics. + topic_name: to_tak # MQTT topic name to send the COT messages to. + ros_casualty_meta_topic_name: '/casualty/meta' # ROS Topic name to publish the casevac messages. + ros_casualty_image_topic_name: '/casualty/image' # ROS message type for the casevac messages. + subscriber: tak_subscriber: # this service is used to subscribe to CoT messages from TAK server and send them to HOSTIP:PORT. filter_messages: # Type of messages to subscribe to. Options: - - name: 'target' - # ROS Topic name to publish the target messages. Use {n} as a placeholder for the robot number. - # If provided, the topic name will be formatted with the robot number extracted from the message name. - ros_topic_name: '/target{n}/gps/gt' - ros_msg_type: NavSatFix # ROS message type for the target messages. - mqtt_topic_name: target_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. - - name: 'iphone' - # ROS Topic name to publish the target messages. Use {n} as a placeholder for the robot number. - # If provided, the topic name will be formatted with the robot number extracted from the message name. - ros_topic_name: '/iphone{n}/gps/gt' - ros_msg_type: NavSatFix # ROS message type for the target messages. - mqtt_topic_name: iphone_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. - - name: 'base' - ros_topic_name: '/basestation/gps' - ros_msg_type: NavSatFix # ROS message type for the target messages. - mqtt_topic_name: base_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. - - name: 'planner' - ros_topic_name: '/planner/planconfig' # ROS Topic name to publish the shapes messages. - ros_msg_type: MarkerArray # ROS message type for the shapes messages. - mqtt_topic_name: planner_events # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'target' + # ROS Topic name to publish the target messages. Use {n} as a placeholder for the robot number. + # If provided, the topic name will be formatted with the robot number extracted from the message name. + ros_topic_name: '/target{n}/gps/gt' + ros_msg_type: NavSatFix # ROS message type for the target messages. + mqtt_topic_name: target_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'iphone' + # ROS Topic name to publish the target messages. Use {n} as a placeholder for the robot number. + # If provided, the topic name will be formatted with the robot number extracted from the message name. + ros_topic_name: '/iphone{n}/gps/gt' + ros_msg_type: NavSatFix # ROS message type for the target messages. + mqtt_topic_name: iphone_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'base' + ros_topic_name: '/basestation/gps' + ros_msg_type: NavSatFix # ROS message type for the target messages. + mqtt_topic_name: base_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'planner' + ros_topic_name: '/planner/planconfig' # ROS Topic name to publish the shapes messages. + ros_msg_type: MarkerArray # ROS message type for the shapes messages. + mqtt_topic_name: dsta-operator # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'dsta-operator' + ros_topic_name: 'NA' # ROS Topic name to publish the shapes messages. + ros_msg_type: NA # ROS message type for the shapes messages. + mqtt_topic_name: dsta-operator # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. # target: Target messages # planner: Planner messages \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/package.xml b/ground_control_station/ros_ws/src/ros2tak_tools/package.xml deleted file mode 100644 index c45ad27f..00000000 --- a/ground_control_station/ros_ws/src/ros2tak_tools/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - ros2tak_tools - 0.0.0 - TODO: Package description - mission-operator - TODO: License declaration - - rclpy - std_msgs - geometry_msgs - paho-mqtt - pytak - pyyaml - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/PKG-INFO b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/PKG-INFO new file mode 100644 index 00000000..f767375f --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/PKG-INFO @@ -0,0 +1,8 @@ +Metadata-Version: 2.1 +Name: ros2tak_tools +Version: 0.0.0 +Summary: TODO: Package description +Maintainer: mission-operator +Maintainer-email: mission-operator@todo.todo +License: TODO: License declaration +Requires-Dist: setuptools diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/SOURCES.txt b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/SOURCES.txt new file mode 100644 index 00000000..ed73670f --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/SOURCES.txt @@ -0,0 +1,24 @@ +README.md +package.xml +setup.py +resource/ros2tak_tools +ros2tak_tools/Casualty.py +ros2tak_tools/__init__.py +ros2tak_tools/chat2ros_agent.py +ros2tak_tools/cot2planner_agent.py +ros2tak_tools/ros2casevac_agent.py +ros2tak_tools/ros2cot_agent.py +ros2tak_tools.egg-info/PKG-INFO +ros2tak_tools.egg-info/SOURCES.txt +ros2tak_tools.egg-info/dependency_links.txt +ros2tak_tools.egg-info/entry_points.txt +ros2tak_tools.egg-info/requires.txt +ros2tak_tools.egg-info/top_level.txt +ros2tak_tools.egg-info/zip-safe +tak_helper/__init__.py +tak_helper/create_cot_msgs.py +tak_helper/tak_publisher.py +tak_helper/tak_subscriber.py +test/test_copyright.py +test/test_flake8.py +test/test_pep257.py \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/dependency_links.txt b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/dependency_links.txt new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/dependency_links.txt @@ -0,0 +1 @@ + diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/entry_points.txt b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/entry_points.txt new file mode 100644 index 00000000..6e8e8e88 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/entry_points.txt @@ -0,0 +1,6 @@ +[console_scripts] +chat2ros_agent = ros2tak_tools.chat2ros_agent:main +cot2planner_agent = ros2tak_tools.cot2planner_agent:main +cot2ros_agent = ros2tak_tools.cot2ros_agent:main +ros2casevac_agent = ros2tak_tools.ros2casevac_agent:main +ros2cot_agent = ros2tak_tools.ros2cot_agent:main diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/requires.txt b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/requires.txt new file mode 100644 index 00000000..49fe098d --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/requires.txt @@ -0,0 +1 @@ +setuptools diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/top_level.txt b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/top_level.txt new file mode 100644 index 00000000..c6489a38 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/top_level.txt @@ -0,0 +1,2 @@ +ros2tak_tools +tak_helper diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/zip-safe b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/zip-safe new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools.egg-info/zip-safe @@ -0,0 +1 @@ + diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Casualty.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Casualty.py new file mode 100644 index 00000000..13fd51cc --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Casualty.py @@ -0,0 +1,473 @@ +from distutils.core import setup + +from sphinx.addnodes import index +from straps_msgs.msg import CasualtyMeta, Injury, Critical, Vitals +from xml.etree.ElementTree import Element, SubElement, tostring +import uuid +from datetime import datetime, timedelta +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): + TRAUMA_NORMAL = Injury.TRAUMA_NORMAL + TRAUMA_WOUND = Injury.TRAUMA_WOUND + TRAUMA_AMPUTATION = Injury.TRAUMA_AMPUTATION + +class OcularAlertness(Enum): + OCULAR_OPEN = Injury.OCULAR_OPEN + OCULAR_CLOSED = Injury.OCULAR_CLOSED + OCULAR_NOT_TESTABLE = Injury.OCULAR_NOT_TESTABLE + +class AlertnessLevel(Enum): + ALERTNESS_NORMAL = Injury.ALERTNESS_NORMAL + ALERTNESS_ABNORMAL = Injury.ALERTNESS_ABNORMAL + ALERTNESS_ABSENT = Injury.ALERTNESS_ABSENT + ALERTNESS_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 TRAUMA_NORMAL or TRAUMA_WOUND + return value in [TraumaSeverity.TRAUMA_NORMAL, TraumaSeverity.TRAUMA_WOUND] + + elif trauma_type in [TraumaType.TRAUMA_LOWER_EXT, TraumaType.TRAUMA_UPPER_EXT]: + # TRAUMA_LOWER_EXT and TRAUMA_UPPER_EXT should have values TRAUMA_NORMAL, TRAUMA_WOUND, or TRAUMA_AMPUTATION + return value in [TraumaSeverity.TRAUMA_NORMAL, TraumaSeverity.TRAUMA_WOUND, TraumaSeverity.TRAUMA_AMPUTATION] + + elif trauma_type == TraumaType.ALERTNESS_OCULAR: + # ALERTNESS_OCULAR should have values OCULAR_OPEN, OCULAR_CLOSED, or OCULAR_NOT_TESTABLE + return value in [OcularAlertness.OCULAR_OPEN, OcularAlertness.OCULAR_CLOSED, OcularAlertness.OCULAR_NOT_TESTABLE] + + elif trauma_type in [TraumaType.ALERTNESS_VERBAL, TraumaType.ALERTNESS_MOTOR]: + # ALERTNESS_VERBAL and ALERTNESS_MOTOR should have values ALERTNESS_NORMAL, ALERTNESS_ABNORMAL, ALERTNESS_ABSENT, or ALERTNESS_NOT_TESTABLE + return value in [AlertnessLevel.ALERTNESS_NORMAL, AlertnessLevel.ALERTNESS_ABNORMAL, AlertnessLevel.ALERTNESS_ABSENT, AlertnessLevel.ALERTNESS_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 VitalsCOT: + """ + Vitals class to store the vitals + """ + # Define the class types + vitals_name: str + status: bool # True if diagnosis has been made, False otherwise + system: str + type: VitalType + value: float + time_ago: float + confidence: float + + # initialize the class with default values + def __init__(self, vitals_name="", system='', type_=None, value=None, time_ago=None, confidence=0.0): + self.vitals_name = vitals_name + self.status = False + self.system = system + self.type = type_ + self.value = value + self.time_ago = time_ago + self.confidence = confidence + + def set_vitals(self, system, type_, value, time_ago, confidence): + # Ensuring that type is either HEART_RATE or RESPIRATORY_RATE + if type_ not in [VitalType.HEART_RATE, VitalType.RESPIRATORY_RATE]: + raise ValueError("Type must be either HEART_RATE (2) or RESPIRATORY_RATE (3)") + + self.system = system + self.type = type_ + self.value = value + self.time_ago = time_ago + self.confidence = confidence + self.status = True + + def __repr__(self): + return f"{self.vitals_name}(Confidence={self.confidence*100:0.2f}%)" + +class InjuryCOT: + """ + Injury class to store the injury status + """ + # Define the class types + injury_name: str + status: bool # True if diagnosis has been made, False otherwise + system: str + type: TraumaType + value: TraumaSeverity + confidence: float + + def __init__(self, injury_name="", system='', type_=None, value=None, confidence=0.0): + self.injury_name = injury_name + self.status = False + self.system = system + self.type_ = type_ + self.value = value + self.confidence = confidence + + def set_status(self, system, type_, value, confidence): + """Sets the status of the injury, with validation for type and value.""" + if not is_valid_type_injury_value(type_, value): + raise ValueError(f"Invalid value for type {type_}: {value}") + + self.system = system + self.type_ = type_ + self.value = value + self.confidence = confidence + self.status = True + + def __repr__(self): + return f"{self.injury_name}(Confidence={self.confidence*100:0.2f}%)" + + +class CriticalCOT: + """ + Critical class to store the critical condition status + """ + # Define the class types + critical_name: str + status: bool # True if diagnosis has been made, False otherwise + system: str + type: ConditionType + value: ConditionStatus + confidence: float + + def __init__(self, critical_name="", system='', type_=None, value=None, confidence=0.0): + self.critical_name = critical_name + self.status = False + self.system = system + self.type = type_ + self.value = value + self.confidence = confidence + + def set_status(self, system: str, type_: ConditionType, value: ConditionStatus, confidence: float): + """Sets the status of the injury, with validation for type and value.""" + self.system = system + self.type = type_ + self.value = value + self.confidence = confidence + self.status = True + + def __repr__(self): + return f"{self.critical_name}(Confidence={self.confidence*100:0.2f}%)" + + +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 = [] + if self.trauma_head.status: + injuries.append(self.trauma_head) + if self.trauma_torso.status: + injuries.append(self.trauma_torso) + if self.trauma_lower_ext.status: + injuries.append(self.trauma_lower_ext) + if self.trauma_upper_ext.status: + injuries.append(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) + if self.respiratory_distress.status: + signs.append(self.respiratory_distress) + if self.heart_rate.status: + signs.append(self.heart_rate) + if self.respiratory_rate.status: + signs.append(self.respiratory_rate) + 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': str(uuid.uuid4()), # Generate a unique UID + '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': "MED.12.201008", + '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') + + + + 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 index 7a199acb..dda4a5e9 100644 --- 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 @@ -118,7 +118,7 @@ def __init__(self, config_file): self.mqtt_client.on_message = self._on_mqtt_message # Connect to MQTT broker and start loop - self.mqtt_client.connect(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) # Subscribe to the dynamic planner topic self.mqtt_client.loop_start() @@ -248,7 +248,7 @@ def main(args=None): import argparse parser = argparse.ArgumentParser(description="COT to Planner") parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') - args, unknown = parser.parse_known_args() + args = parser.parse_args() cot2planner = Cot2Planner(args.config) rclpy.spin(cot2planner) 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 new file mode 100755 index 00000000..2a58cc56 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2casevac_agent.py @@ -0,0 +1,707 @@ +#!/usr/bin/env python3 + +""" +ROS 2 CASEVAC Agent + +Subscribes to 2 topics that has casualty meta data and image data for the casualty. + +Author: Aditya Rauniyar (rauniyar@cmu.edu) + +""" + +import rclpy +from rclpy.node import Node +import paho.mqtt.client as mqtt +import argparse +import yaml +from straps_msgs.msg import CasualtyMeta, Injury, Critical, Vitals +from xml.etree.ElementTree import Element, SubElement, tostring +from datetime import datetime +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") + + +def load_config(file_path): + """Load configuration from a YAML file.""" + with open(file_path, "r") as f: + return yaml.safe_load(f) + + +############################################################################################################ +""" +Global dictionary to store the casualty meta data. +""" +CASUALTY_META_DATA = {} +############################################################################################################ + + +class ROS2COTPublisher(Node): + def __init__(self, config): + super().__init__("ros2casevac_agent") + self.subscribers = [] + + # 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"] + + # 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.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})" + ) + except Exception as e: + print(f"Failed to connect or publish: {e}") + + self.get_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( + CasualtyMeta, + self.ros_casualty_meta_topic_name, + self.casualty_meta_callback, + 10, + ) + self.get_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}") + + # get the casualty id from the message + casualty_id = create_casualty_id(msg.casualty_id) + + 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}" + ) + + # 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}" + ) + + # 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() + ) + self.get_logger().info(f"Sent CoT event for casualty: {casualty_id}") + + def send_cot_event_over_mqtt(self, cot_event): + """Send CoT event over the MQTT network""" + 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.") + + +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." + ) + + # 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.") + + +if __name__ == "__main__": + main() 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 15883207..591fafc5 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 @@ -64,12 +64,12 @@ 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-h-A-M-A") + 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(60)) + root.set("stale", pytak.cot_time(3600)) pt_attr = { "lat": str(latitude), @@ -110,7 +110,7 @@ def __init__(self, config): 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.mqtt_client.connect(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})" ) @@ -130,7 +130,7 @@ def __init__(self, config): subscriber = self.create_subscription( NavSatFix, topic_name, - lambda msg, topic_name=topic_name: self.gps_callback(msg, topic_name), + lambda msg, topic_name=topic_name: self.gps_callback(msg, f"/robot{i}"), 10, # QoS depth ) self.subscribers.append(subscriber) @@ -192,8 +192,8 @@ def main(args=None): ) # Parse the arguments - input_args, unknown = parser.parse_known_args() - + input_args = parser.parse_args() + # Load configuration config = load_config(input_args.config) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_publisher.py b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py similarity index 97% rename from ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_publisher.py rename to ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py index 734405a1..8550d4d7 100755 --- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_publisher.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py @@ -51,7 +51,7 @@ def __init__(self, tx_queue, config, event_loop): # Connect to MQTT broker and subscribe to topic try: print(f"Connecting to {self.mqtt_broker}:{self.mqtt_port}") - self.mqtt_client.connect(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}") except Exception as e: @@ -80,7 +80,7 @@ async def run(self, number_of_iterations=-1): self.mqtt_client.loop_stop() print("MQTT loop stopped.") -async def async_main(config): +async def main(config): loop = asyncio.get_running_loop() # Capture the main event loop clitool = pytak.CLITool(config["mycottool"]) await clitool.setup() @@ -92,12 +92,9 @@ def run_main_in_process(config): loop.run_until_complete(main(config)) if __name__ == "__main__": - main() - -def main(): parser = argparse.ArgumentParser(description="TAK Publisher Script") parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') - args, unknown = parser.parse_known_args() + args = parser.parse_args() # Load the YAML configuration with open(args.config, 'r') as file: @@ -143,4 +140,4 @@ def main(): process.start() print("Main() is now running in a separate process.") - process.join() + process.join() \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_subscriber.py b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py similarity index 98% rename from ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_subscriber.py rename to ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py index 7ccab2ef..6f398f49 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/tak_subscriber.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py @@ -69,7 +69,7 @@ def __init__(self, rx_queue, config, filter_names2topic): # Connect to MQTT broker and subscribe to topic try: self._logger.info(f"Connecting to {self.mqtt_broker}:{self.mqtt_port}") - self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port) + self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port, keepalive=65535) self._logger.info(f"Connected and subscribed to MQTT on broker {self.mqtt_broker}:{self.mqtt_port}") except Exception as e: self._logger.error(f"Failed to connect or subscribe to MQTT: {e}") @@ -135,7 +135,6 @@ async def send_to_mqtt(self, data, mqtt_topic): except: self._logger.info(f"Failed to publish.") - async def run(self): # pylint: disable=arguments-differ """Read from the receive queue, put data onto handler.""" while True: @@ -143,10 +142,10 @@ async def run(self): # pylint: disable=arguments-differ await self.handle_data(data) -async def async_main(): +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.') - args, unknown = parser.parse_known_args() + args = parser.parse_args() # Load the YAML configuration with open(args.config, 'r') as file: @@ -197,8 +196,6 @@ async def async_main(): # Start all tasks. await clitool.run() -def main(): - asyncio.run(async_main()) if __name__ == "__main__": - main() + asyncio.run(main()) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg b/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg deleted file mode 100644 index 23d12fb8..00000000 --- a/ground_control_station/ros_ws/src/ros2tak_tools/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[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 c675e697..7aeb110a 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/setup.py +++ b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py @@ -1,5 +1,4 @@ from setuptools import find_packages, setup -import glob package_name = 'ros2tak_tools' @@ -11,7 +10,6 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/config/', glob.glob('config/*')), ], install_requires=['setuptools'], zip_safe=True, @@ -24,9 +22,9 @@ 'console_scripts': [ 'ros2cot_agent = ros2tak_tools.ros2cot_agent:main', 'cot2ros_agent = ros2tak_tools.cot2ros_agent:main', - 'cot2planner_agent = ros2tak_tools.cot2planner_agent:main', - 'tak_subscriber = ros2tak_tools.tak_subscriber:main', - 'tak_publisher = ros2tak_tools.tak_publisher:main', + 'cot2planner_agent = ros2tak_tools.cot2planner_agent:main', + 'ros2casevac_agent = ros2tak_tools.ros2casevac_agent:main', + 'chat2ros_agent = ros2tak_tools.chat2ros_agent:main', ], }, ) 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 new file mode 100644 index 00000000..b95f6b4c --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/tak_helper/create_cot_msgs.py @@ -0,0 +1,284 @@ +import xml.etree.ElementTree as ET +import pytak +from datetime import datetime +import uuid +from typing import List, Tuple + +def create_gps_COT(uuid: str, cot_type : str, latitude : str, longitude: str, altitude : str): + """Create a CoT event based on the GPS data.""" + 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("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") + +def create_casevac_COT(uuid, casualty_id, gps, zap_num, mechanism, injury, signs_symptoms, treatments, physician_name): + # Create root event element + event = ET.Element( + "event", + { + "version": "2.0", + "uid": casualty_id, + "type": "b-r-f-h-c", + "how": "h-g-i-g-o", + "time": pytak.cot_time(), + "start": pytak.cot_time(), + "stale": pytak.cot_time(3600), + }, + ) + + # Create point element + point = ET.SubElement( + event, + "point", + { + "lat": f"{gps.latitude}", + "lon": f"{gps.longitude}", + "hae": "9999999.0", + "ce": "9999999.0", + "le": "9999999.0", + }, + ) + + # Create detail element + detail = ET.SubElement(event, "detail") + + # Add contact element + contact = ET.SubElement(detail, "contact", {"callsign": casualty_id}) + + # Add link element + link = ET.SubElement( + detail, + "link", + { + "type": "a-f-G-U-C-I", + "uid": "S-1-5-21-942292099-3747883346-3641641706-1000", + "parent_callsign": casualty_id, + "relation": "p-p", + "production_time": pytak.cot_time(), + }, + ) + + # Add archive and status elements + ET.SubElement(detail, "archive") + ET.SubElement(detail, "status", {"readiness": "false"}) + ET.SubElement(detail, "remarks") + + # Create _medevac_ element with nested zMistsMap and zMist + medevac = ET.SubElement( + detail, + "_medevac_", + { + "title": 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 = ET.SubElement(medevac, "zMistsMap") + zMist = ET.SubElement( + zMistsMap, + "zMist", + { + "z": zap_num, + "m": mechanism, + "i": injury, + "s": signs_symptoms, + "t": treatments, + "title": physician_name, + }, + ) + + # Add _flow-tags_ element + flow_tags = ET.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 ET.tostring(event, encoding="utf-8").decode("utf-8") + +def create_chat_COT(uuid, callsign: str, message: str) -> str: + + # Create root event element + event = ET.Element( + "event", + { + "version": "2.0", + "uid": f"GeoChat.{uuid}", + "type": "b-t-f", + "how": "h-g-i-g-o", + "time": pytak.cot_time(), + "start": pytak.cot_time(), + "stale": pytak.cot_time(3600), + } + ) + + # Create point element + point = ET.SubElement( + event, + "point", + { + "lat": "0.0", + "lon": "0.0", + "hae": "9999999.0", + "ce": "9999999.0", + "le": "9999999.0", + }, + ) + + # Create detail element + detail = ET.SubElement(event, "detail") + + # Create __chat element + chat = ET.SubElement( + detail, + "__chat", + { + "id": "All Chat Rooms", + "chatroom": "All Chat Rooms", + "senderCallsign": callsign, + "groupOwner": "false", + } + ) + + # Add chatgrp element + chatgrp = ET.SubElement( + chat, + "chatgrp", + { + "id": "All Chat Rooms", + "uid0": uuid, + "uid1": "All Chat Rooms", + }, + ) + + # Add link element + link = ET.SubElement( + detail, + "link", + { + "uid": uuid, + "type": "a-f-G-U-C-I", + "relation": "p-p", + }, + ) + + # Add remarks element + remarks = ET.SubElement( + detail, + "remarks", + { + "source": f"BAO.F.AIRLAB_CLI_MANAGER_{uuid}", + "sourceID": uuid, + "to": "All Chat Rooms", + "time": datetime.utcnow().strftime("%Y-%m-%dT%H:%M:%S.%fZ"), + } + ) + remarks.text = message + + # Add _flow-tags_ element + flow_tags = ET.SubElement( + detail, + "_flow-tags_", + { + "TAK-Server-f6edbf55ccfa4af1b4cfa2d7f177ea67": datetime.utcnow().strftime("%Y-%m-%dT%H:%M:%SZ"), + }, + ) + + # Convert to a string + return ET.tostring(event, encoding="utf-8").decode("utf-8") + +from datetime import datetime, timedelta + +def create_polygon_COT( + uuid: str, + callsign: str, + gps_coordinates: List[Tuple[float, float]], + fill_color: int = 16777215, +) -> bytes: + """ + Creates a CoT message for a polygon using GPS coordinates. + + Args: + gps_coordinates (list of tuples): List of (latitude, longitude) coordinates. + uuid (str): Unique identifier for the CoT event. + callsign (str): Callsign for the contact. + fill_color (int, optional): Fill color value. Default is 16777215. + fill_color = (Red (0-255) << 16) + (Green (0-255) << 8) + Blue (0-255) + + + Returns: + bytes: CoT message as a UTF-8 encoded XML byte string. + """ + if not gps_coordinates: + raise ValueError("GPS coordinates list cannot be empty.") + + # Ensure the polygon is closed + if gps_coordinates[0] != gps_coordinates[-1]: + gps_coordinates.append(gps_coordinates[0]) + + # Create the root element + root = ET.Element("event", { + "version": "2.0", + "uid": uuid, + "type": "u-d-f", + "how": "h-e", + "time": pytak.cot_time(), + "start": pytak.cot_time(), + "stale": pytak.cot_time(3600), + }) + + # Add the point element + first_point = gps_coordinates[0] + ET.SubElement(root, "point", { + "lat": str(first_point[0]), + "lon": str(first_point[1]), + "hae": "9999999.0", + "ce": "9999999.0", + "le": "9999999.0", + }) + + # Add the detail element + detail = ET.SubElement(root, "detail") + ET.SubElement(detail, "contact", {"callsign": callsign}) + ET.SubElement(detail, "strokeColor", {"value": "-1"}) + ET.SubElement(detail, "fillColor", {"value": str(fill_color)}) + ET.SubElement(detail, "remarks") + ET.SubElement(detail, "height", {"value": "0.00"}) + ET.SubElement(detail, "height_unit", {"value": "4"}) + ET.SubElement(detail, "archive") + + # Add link elements for each GPS point + for lat, lon in gps_coordinates: + ET.SubElement(detail, "link", {"point": f"{lat},{lon}"}) + + ET.SubElement(detail, "archive") + + # Generate the XML string + return ET.tostring(root, encoding="utf-8", xml_declaration=True) \ No newline at end of file