From 7067c89b4a99eeee27e20e10a8ee3261c98812fc Mon Sep 17 00:00:00 2001 From: Adi Date: Sun, 3 Nov 2024 22:11:04 -0500 Subject: [PATCH 1/8] refactor: refactoring Dockerfiles from ros2tak_tools to ground_control-station/docker/ --- .../Dockerfile.ros2cot_agent | 9 +- .../Dockerfile.tak_publisher | 6 +- .../Dockerfile.tak_subscriber | 6 +- .../docker/docker-compose.yaml | 105 ++++++++++++++++++ ground_control_station/ros_ws/.gitignore | 1 + .../src/ros2tak_tools/docker-compose.yaml | 98 ---------------- 6 files changed, 116 insertions(+), 109 deletions(-) rename ground_control_station/{ros_ws/src/ros2tak_tools/ros2tak_tools => docker}/Dockerfile.ros2cot_agent (69%) rename ground_control_station/{ros_ws/src/ros2tak_tools/scripts => docker}/Dockerfile.tak_publisher (55%) rename ground_control_station/{ros_ws/src/ros2tak_tools/scripts => docker}/Dockerfile.tak_subscriber (55%) delete mode 100644 ground_control_station/ros_ws/src/ros2tak_tools/docker-compose.yaml diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Dockerfile.ros2cot_agent b/ground_control_station/docker/Dockerfile.ros2cot_agent similarity index 69% rename from ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Dockerfile.ros2cot_agent rename to ground_control_station/docker/Dockerfile.ros2cot_agent index e5d82fbb0..bfd906920 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/Dockerfile.ros2cot_agent +++ b/ground_control_station/docker/Dockerfile.ros2cot_agent @@ -2,8 +2,10 @@ # Base image with ROS2 Humble FROM ros:humble +ARG ROS_WS_DIR + # Set working directory -WORKDIR /home/mission-operator/ros2_ws +WORKDIR ${ROS_WS_DIR} # Install necessary dependencies RUN apt-get update && apt-get install -y \ @@ -19,13 +21,10 @@ RUN pip3 install --upgrade pip \ && pip3 install paho-mqtt # Copy the local 'ros2tak_tools' directory into the container -COPY ../../ros2tak_tools /home/mission-operator/ros2_ws/src/ros2tak_tools +COPY /ros_ws/src/ros2tak_tools ${ROS_WS_DIR}/src/ros2tak_tools # Build the ROS2 workspace RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ colcon build --symlink-install" -# Source the ROS 2 environment in .bashrc -RUN echo 'source /opt/ros/humble/setup.bash' >> ~/.bashrc && \ - echo 'source /home/mission-operator/ros2_ws/install/setup.bash' >> ~/.bashrc diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/Dockerfile.tak_publisher b/ground_control_station/docker/Dockerfile.tak_publisher similarity index 55% rename from ground_control_station/ros_ws/src/ros2tak_tools/scripts/Dockerfile.tak_publisher rename to ground_control_station/docker/Dockerfile.tak_publisher index ee1a576db..999469f53 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/Dockerfile.tak_publisher +++ b/ground_control_station/docker/Dockerfile.tak_publisher @@ -1,12 +1,12 @@ # Base image with Python 3 FROM python:3.10-slim +ARG ROS_WS_DIR + # Set working directory -WORKDIR /home/mission-operator/ros2_ws/ +WORKDIR ${ROS_WS_DIR} # Install necessary dependencies # Install necessary dependencies RUN pip install --no-cache-dir asyncio pytak pyyaml cryptography paho-mqtt -# Copy the local 'ros2tak_tools' directory into the container -COPY ../ros2tak_tools /home/mission-operator/ros2_ws/src/ros2tak_tools/ diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/Dockerfile.tak_subscriber b/ground_control_station/docker/Dockerfile.tak_subscriber similarity index 55% rename from ground_control_station/ros_ws/src/ros2tak_tools/scripts/Dockerfile.tak_subscriber rename to ground_control_station/docker/Dockerfile.tak_subscriber index ee1a576db..999469f53 100644 --- a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/Dockerfile.tak_subscriber +++ b/ground_control_station/docker/Dockerfile.tak_subscriber @@ -1,12 +1,12 @@ # Base image with Python 3 FROM python:3.10-slim +ARG ROS_WS_DIR + # Set working directory -WORKDIR /home/mission-operator/ros2_ws/ +WORKDIR ${ROS_WS_DIR} # Install necessary dependencies # Install necessary dependencies RUN pip install --no-cache-dir asyncio pytak pyyaml cryptography paho-mqtt -# Copy the local 'ros2tak_tools' directory into the container -COPY ../ros2tak_tools /home/mission-operator/ros2_ws/src/ros2tak_tools/ diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 7f560c44c..9eb0a1400 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -41,3 +41,108 @@ services: # autonomy stack stuff - ../../common/ros_ws:/root/common/ros_ws:rw # common ROS packages - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages + + +####################### 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" + - "9001:9001" + 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 + networks: + - airstack_network + + ################## ROS2COT_AGENT + ros2cot_agent: + build: + context: ../ + dockerfile: docker/Dockerfile.ros2cot_agent + args: + - ROS_WS_DIR=${ROS_WS_DIR} + container_name: "${PROJECT_NAME}_ros2cot_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/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} + container_name: "${PROJECT_NAME}_tak_publisher" + 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_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} + 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" + ] + + +########### NETWORKS ########### +networks: + airstack_network: + driver: bridge diff --git a/ground_control_station/ros_ws/.gitignore b/ground_control_station/ros_ws/.gitignore index 134565718..a7bf8a2ef 100644 --- a/ground_control_station/ros_ws/.gitignore +++ b/ground_control_station/ros_ws/.gitignore @@ -1,3 +1,4 @@ build/ install/ log/ +/src/ros2tak_tools/creds/ diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/docker-compose.yaml b/ground_control_station/ros_ws/src/ros2tak_tools/docker-compose.yaml deleted file mode 100644 index 64858070f..000000000 --- a/ground_control_station/ros_ws/src/ros2tak_tools/docker-compose.yaml +++ /dev/null @@ -1,98 +0,0 @@ -# Instructions: - -# Build: docker-compose build --no-cache -# Run as interactive mode: docker run -it --rm tak_tools_ros2cot_agent /bin/bash - -version: '3.8' - - -services: -################### MQTT - mqtt: - container_name: "mqtt" - image: eclipse-mosquitto:2.0.20 - restart: always - volumes: - - ./mosquitto/config:/mosquitto/config - - ./mosquitto/data:/mosquitto/data - - ./mosquitto/log:/mosquitto/log - ports: - - "1883:1883" - - "9001:9001" - 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 - networks: - - basestation-network - -################## ROS2COT_AGENT - ros2cot_agent: - build: - context: ../ - dockerfile: ros2tak_tools/ros2tak_tools/Dockerfile.ros2cot_agent - container_name: "${PROJECT_NAME}_ros2cot_agent" - volumes: - - ./config:/home/mission-operator/ros2_ws/src/ros2tak_tools/config - stdin_open: true - tty: true - restart: unless-stopped - env_file: - - .env - depends_on: - mqtt: - condition: service_healthy - networks: - - basestation-network - command: ["/bin/bash", "-c", "source /opt/ros/humble/setup.bash && source /home/mission-operator/ros2_ws/install/setup.bash && ros2 run ros2tak_tools ros2cot_agent --config $CONFIG_PATH"] - -# ################### TAK_PUBLISHER - tak_publisher: - build: - context: ../ - dockerfile: ros2tak_tools/scripts/Dockerfile.tak_publisher - container_name: "${PROJECT_NAME}_tak_publisher" - volumes: - - ./config:/home/mission-operator/ros2_ws/src/ros2tak_tools/config - - ./scripts:/home/mission-operator/ros2_ws/src/ros2tak_tools/scripts - stdin_open: true - tty: true - restart: unless-stopped - env_file: - - .env - depends_on: - mqtt: - condition: service_healthy - networks: - - basestation-network - command: ["python3", "$TAK_PUBLISHER_FILE_PATH", "--config", "$CONFIG_PATH"] - -################### TAK_SUBSCRIBER - tak_subscriber: - build: - context: ../ - dockerfile: ros2tak_tools/scripts/Dockerfile.tak_subscriber - container_name: "${PROJECT_NAME}_tak_subscriber" - volumes: - - ./config:/home/mission-operator/ros2_ws/src/ros2tak_tools/config - - ./scripts:/home/mission-operator/ros2_ws/src/ros2tak_tools/scripts - stdin_open: true - tty: true - restart: unless-stopped - env_file: - - .env - depends_on: - mqtt: - condition: service_healthy - networks: - - basestation-network - command: ["python3", "$TAK_SUBSCRIBER_FILE_PATH", "--config", "$CONFIG_PATH"] - - -########### NETWORKS ########### -networks: - basestation-network: - driver: bridge - From af06e1b6cfc0664e1c46007f70a5bd4494ac72b7 Mon Sep 17 00:00:00 2001 From: Adi Date: Sun, 3 Nov 2024 22:27:06 -0500 Subject: [PATCH 2/8] refactor: remove ros2tak_tools .env file --- .../ros_ws/src/ros2tak_tools/.env | 13 ------------- 1 file changed, 13 deletions(-) delete mode 100644 ground_control_station/ros_ws/src/ros2tak_tools/.env diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/.env b/ground_control_station/ros_ws/src/ros2tak_tools/.env deleted file mode 100644 index c0badce20..000000000 --- a/ground_control_station/ros_ws/src/ros2tak_tools/.env +++ /dev/null @@ -1,13 +0,0 @@ -PROJECT_NAME=PROJECT_NAME -ROS2_WS_PATH=~/ground_control_station/ros_ws - -CONFIG_PATH=src/ros2tak_tools/config/config.yaml - -TAK_PUBLISHER_FILE_PATH=src/ros2tak_tools/scripts/tak_publisher.py -TAK_SUBSCRIBER_FILE_PATH=src/ros2tak_tools/scripts/tak_subscriber.py - -ROS2_AGENT_PORT=1883 -TAK_PUBLISHER_PORT=1883 - -MQTT_USERNAME = airlab -MQTT_PASSWORD = # Enter your password here From 90f72201ee04ab8283b45f55c3949692c71acd18 Mon Sep 17 00:00:00 2001 From: Adi Date: Tue, 5 Nov 2024 14:55:41 -0500 Subject: [PATCH 3/8] refactor: adding .env file for the docker-compose for GCS --- ground_control_station/docker/.env | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 ground_control_station/docker/.env diff --git a/ground_control_station/docker/.env b/ground_control_station/docker/.env new file mode 100644 index 000000000..ce071aa08 --- /dev/null +++ b/ground_control_station/docker/.env @@ -0,0 +1,21 @@ +PROJECT_NAME= GCS # Enter the project name + + +# ROS -------------------------------------------------------- +ROS_WS_DIR=/root/ros_ws + + + + + +# ROS2TAK_TOOLS -------------------------------------------------------- + +# NOTE: Update the config file +ROS2TAK_TOOLS_CONFIG_DIR=src/ros2tak_tools/config +ROS2TAK_TOOLS_CONFIG_FILENAME=config.yaml + +TAK_PUBLISHER_FILEPATH=src/ros2tak_tools/scripts/tak_publisher.py +TAK_SUBSCRIBER_FILEPATH=src/ros2tak_tools/scripts/tak_subscriber.py + +MQTT_USERNAME= # Enter the MQTT username +MQTT_PASSWORD= # Enter the MQTT password From c1bc12f507697eaeb3be7a35dc23e6a95042286b Mon Sep 17 00:00:00 2001 From: Adi Date: Wed, 6 Nov 2024 16:41:38 -0500 Subject: [PATCH 4/8] feat: adding cot msgs from TAK interface to ros2 topic for global planner --- .../docker/Dockerfile.cot2planner_agent | 34 +++ .../docker/docker-compose.yaml | 24 ++ .../ros2tak_tools/cot2planner_agent.py | 222 +++++++++++++++--- 3 files changed, 251 insertions(+), 29 deletions(-) create mode 100644 ground_control_station/docker/Dockerfile.cot2planner_agent diff --git a/ground_control_station/docker/Dockerfile.cot2planner_agent b/ground_control_station/docker/Dockerfile.cot2planner_agent new file mode 100644 index 000000000..01b8af476 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.cot2planner_agent @@ -0,0 +1,34 @@ +# Base image with ROS2 Humble +# Base image with ROS2 Humble +FROM ros:humble + +ARG ROS_WS_DIR + +# Set working directory +WORKDIR ${ROS_WS_DIR} + +# Install necessary dependencies +RUN apt-get update && apt-get install -y \ + python3-colcon-common-extensions \ + ros-humble-ros2cli \ + python3-pip \ + libglib2.0-dev && \ + 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 + +# Copy the local 'ros2tak_tools' directory into the container +COPY ground_control_station/ros_ws/src/ros2tak_tools ${ROS_WS_DIR}/src/ros2tak_tools + +# Copy the common airstack ros_package into the container +COPY common/ros_packages/airstack_msgs ${ROS_WS_DIR}/src/airstack_msgs + +# Build the ROS2 workspace +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + colcon build --symlink-install --packages-select airstack_msgs && \ + colcon build --symlink-install" + + diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 9eb0a1400..92b71f6eb 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -142,6 +142,30 @@ services: ] + ################## ROS2COT_AGENT + cot2planner_agent: + build: + context: ../../ + dockerfile: ground_control_station/docker/Dockerfile.cot2planner_agent + args: + - ROS_WS_DIR=${ROS_WS_DIR} + 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" + ] + ########### NETWORKS ########### networks: airstack_network: 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 e00ede8c4..43adc9ab1 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 @@ -1,13 +1,85 @@ #!/usr/bin/env python3 +""" +Description: +This script listens to the MQTT messages from the TAK server and converts them to ROS 2 messages. +Mainly used for Global Planner for getting Search Bounds, Search Priors, and Keep Out Zones. + +Usage: +ros2 run ros2tak_tools cot2planner_agent --config + +Author: +Aditya Rauniyar (2024) + +""" + import rclpy from rclpy.node import Node -import yaml +import xml.etree.ElementTree as ET import paho.mqtt.client as mqtt -from airstack_msgs.msg import SearchMissionRequest # Import the custom message +from airstack_msgs.msg import SearchMissionRequest, SearchPrior, KeepOutZone # Import the custom message from std_msgs.msg import Header -from geometry_msgs.msg import Polygon +from geometry_msgs.msg import Polygon, Point32 from rclpy.qos import QoSProfile +import yaml +from enum import Enum +import re + +# Constants and Enums +DEFAULT_FRAME_ID = 'map' + +# Define event types using Enum for better type safety +class EventType(Enum): + POLYGON = "u-d-f" + ROUTE = "b-m-r" + POINT = "u-d-c-p" + CIRCLE = "u-d-c-c" + +def extract_polygon_points(root): + """Extract points for the polygon based on link coordinates.""" + points32 = [] + + # Find all elements and extract their 'point' attribute + for link in root.findall('.//link'): + point_str = link.attrib.get('point') + # Check if point attribute exists. + # NOTE: WinTAK generated blank points for some reason. ATAK is fine. + if point_str: + # Split the single point into latitude, longitude, and altitude + lat, lon, _ = map(float, point_str.split(',')) + + # Create a Point32 object with lat and lon, set z=0.0 (altitude is not used here) + point = Point32() + point.x = lat + point.y = lon + point.z = 0.0 # Default z value, can be adjusted if needed + + # Add the point to the list + points32.append(point) + + return points32 + +def extract_value_priority(callsign): + """Extract value and priority from a callsign like 'planner_sp_v0.2_p3.4'.""" + match = re.search(r'v([0-9.]+)_p([0-9.]+)', callsign) + if match: + value = float(match.group(1)) + priority = float(match.group(2)) + return value, priority + return 0.0, 1.0 # Default values if no match + + +def _process_polygons(root): + """Process XML for 'u-d-f' type and convert to SearchBound and SearchPrior.""" + points32 = extract_polygon_points(root) + # Create ROS message + polygon = Polygon() + + # Set the points for the polygon + polygon.points = points32 + + return polygon + class Cot2Planner(Node): def __init__(self, config_file): @@ -21,12 +93,17 @@ def __init__(self, config_file): mqtt_config = config['mqtt'] self.mqtt_broker = mqtt_config['host'] self.mqtt_port = int(mqtt_config['port']) - self.mqtt_topicname = mqtt_config['topicname'] self.mqtt_username = mqtt_config['username'] self.mqtt_password = mqtt_config['password'] # ROS Configurations - self.ros_topic = config['ros_topic'] # Topic to publish to + # Reading the ros_topic_name for the planner under filter_messages -> planner + planner_config = next( + item for item in config['services']['subscriber']['tak_subscriber']['filter_messages'] + if item['name'] == 'planner' + ) + self.ros_topic = planner_config['ros_topic_name'] # Read the ros_topic_name dynamically + self.mqtt_topicname = planner_config['mqtt_topic_name'] # Read the MQTT topic name for planner events # ROS 2 Publisher qos_profile = QoSProfile(depth=10) @@ -39,46 +116,133 @@ def __init__(self, config_file): # Connect to MQTT broker and start loop self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port) - self.mqtt_client.subscribe(self.mqtt_topicname) + self.mqtt_client.subscribe(self.mqtt_topicname) # Subscribe to the dynamic planner topic self.mqtt_client.loop_start() + # Planner message request to be sent + self.plan_msg_request = self._initialize_ros_message_header(SearchMissionRequest(), frame_id=DEFAULT_FRAME_ID) + self.get_logger().info(f"Subscribed to MQTT topic '{self.mqtt_topicname}' on broker '{self.mqtt_broker}:{self.mqtt_port}'") + def _initialize_ros_message_header(self, message, frame_id=DEFAULT_FRAME_ID): + """Initialize the ROS message with header info.""" + message.header = Header() + message.header.stamp = self.get_clock().now().to_msg() + message.header.frame_id = frame_id + return message + def _on_mqtt_message(self, client, userdata, msg): """Callback for incoming MQTT messages.""" try: - # Assuming message payload is in JSON format and matches SearchMissionRequest structure - data = yaml.safe_load(msg.payload) - message = self._create_ros_message(data) - self.publisher.publish(message) - self.get_logger().info(f"Published SearchMissionRequest to '{self.ros_topic}'") - except Exception as e: - self.get_logger().error(f"Error processing message: {e}") - - def _create_ros_message(self, data): - """Create and populate the SearchMissionRequest message from MQTT data.""" - message = SearchMissionRequest() - - # Set fields based on incoming data - message.header = Header() - message.header.stamp = self.get_clock().now().to_msg() - message.header.frame_id = 'map' + # Parse the XML message + root = ET.fromstring(msg.payload.decode('utf-8')) - # Populate search_bounds, search_priors, keep_out_zones here based on data format - # Example (ensure your incoming data structure matches this): - message.search_bounds = Polygon() # Populate with points if available - # Example for search_priors and keep_out_zones: - # message.search_priors = [...] # Fill in details from data - # message.keep_out_zones = [...] # Fill in details from data + # Extract event type from the XML + event_type = root.attrib.get('type') - return message + # Extract the callsign from the XML + callsign = root.find('.//contact').attrib.get('callsign', '') + + # Handle event types using Enum values + if event_type == EventType.POLYGON.value or event_type == EventType.ROUTE.value or event_type == EventType.POINT.value: + polygon = _process_polygons(root) + + # Check if the callsign contains 'sb' for search bounds + if 'sb' in callsign.lower(): + self.plan_msg_request.search_bounds = polygon + self.get_logger().info(f"Added SearchBounds with callsign '{callsign}' to the SearchMissionRequest") + + event_shape_type = "polygon" if event_type == EventType.POLYGON.value else \ + "route" if event_type == EventType.ROUTE.value else "point" + + # Process as search prior + self._process_search_priors(polygon, call_sign=callsign, shape_type=event_shape_type) + + elif event_type == EventType.CIRCLE.value: + self._process_keep_out_zones(root) + self.get_logger().info(f"Added KeepOutZones with callsign '{callsign}' to the SearchMissionRequest") + else: + self.get_logger().info(f"Event type '{event_type}' not supported") + + # Print the current self.plan_msg_request + # self.get_logger().info(f"Current SearchMissionRequest: {self.plan_msg_request}") + + # Check if the xml contains word "end" in the remarks field mostly the remarks field is kept empty + remarks = root.find('.//remarks').text + if remarks and 'end' in remarks.lower(): + # Publish the SearchMissionRequest message + self.publisher.publish(self.plan_msg_request) + self.get_logger().info(f"Published SearchMissionRequest to '{self.ros_topic}'") + # Reset the plan_msg_request for next mission + self.plan_msg_request = self._initialize_ros_message_header(SearchMissionRequest(), frame_id=DEFAULT_FRAME_ID) + + except (ET.ParseError, mqtt.MQTTException) as e: + self.get_logger().error(f"Error processing MQTT message: {e}") + + def _process_search_priors(self, polygon, call_sign="", shape_type="polygon"): + """Process XML for 'b-m-r' type and convert to SearchBound and SearchPrior.""" + + # Extract the value and priority from the callsign + value, priority = extract_value_priority(call_sign) + + # Create a SearchPrior message + search_prior = self._initialize_ros_message_header(SearchPrior(), frame_id=DEFAULT_FRAME_ID) + + # Set the prior type as polygon + if shape_type == "polygon": + search_prior.grid_prior_type = SearchPrior.POLYGON_PRIOR + elif shape_type == "route": + search_prior.grid_prior_type = SearchPrior.LINE_SEG_PRIOR + elif shape_type == "point": + search_prior.grid_prior_type = SearchPrior.POINT_PRIOR + + # set polygon, value, and priority + search_prior.points_list = polygon + search_prior.value = [value] + search_prior.priority = [priority] + + # Update the plan_msg_request with the search_prior + self.plan_msg_request.search_priors.append(search_prior) + self.get_logger().info(f"Added SearchPriors to the SearchMissionRequest with callSign={call_sign}, " + f"type={shape_type}, value={value}, priority={priority}") + + + def _process_keep_out_zones(self, root): + """Process XML for 'u-d-c-c' type and convert to KeepOutZone.""" + # Extract data from XML + point = root.find('.//point') + if point is not None: + lat = float(point.attrib.get('lat', 0.0)) + lon = float(point.attrib.get('lon', 0.0)) + else: + lat = lon = 0.0 + + ellipse = root.find('.//ellipse') + major_ellipse = float(ellipse.attrib.get('major', 0.0)) if ellipse is not None else 0.0 + + # Create ROS message + message = self._initialize_ros_message_header(KeepOutZone(), frame_id=DEFAULT_FRAME_ID) + + # Set the x, y, z_min, z_max, and radius fields + message.x = lat + message.y = lon + message.z_min = 0.0 + message.z_max = 0.0 + message.radius = major_ellipse # Radius is the major ellipse axis in meters + + # Append the KeepOutZone message to the plan_msg_request + self.plan_msg_request.keep_out_zones.append(message) def destroy_node(self): + """Stop MQTT loop and destroy the node.""" self.mqtt_client.loop_stop() + self.mqtt_client.disconnect() # Explicit disconnect super().destroy_node() def main(args=None): rclpy.init(args=args) + + 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 = parser.parse_args() From d2674a76f035163c51e95f345bd6fb1b486045a5 Mon Sep 17 00:00:00 2001 From: Adi Date: Thu, 7 Nov 2024 13:44:08 -0500 Subject: [PATCH 5/8] fix: avoiding link tag in xml when the point arg is not present - usually the case in WinTAK. --- .../ros2tak_tools/cot2planner_agent.py | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) 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 43adc9ab1..a90b50ccc 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 @@ -42,20 +42,23 @@ def extract_polygon_points(root): # Find all elements and extract their 'point' attribute for link in root.findall('.//link'): point_str = link.attrib.get('point') - # Check if point attribute exists. - # NOTE: WinTAK generated blank points for some reason. ATAK is fine. + # Check if point attribute exists and is non-empty. + # WinTAK sometimes generates blank points, ATAK is fine. if point_str: - # Split the single point into latitude, longitude, and altitude - lat, lon, _ = map(float, point_str.split(',')) - - # Create a Point32 object with lat and lon, set z=0.0 (altitude is not used here) - point = Point32() - point.x = lat - point.y = lon - point.z = 0.0 # Default z value, can be adjusted if needed - - # Add the point to the list - points32.append(point) + try: + # Split the single point into latitude and longitude + lat, lon = map(float, point_str.split(',')) + + # Create a Point32 object with lat and lon, set z=0.0 (altitude is not used here) + point = Point32() + point.x = lat + point.y = lon + point.z = 0.0 # Default z value (altitude not available) + + # Add the point to the list + points32.append(point) + except ValueError as e: + print(f"Warning: Skipping invalid point: {point_str} - Error: {e}") return points32 From 00ec1d11febecab72534658dc09ec0e348c82c59 Mon Sep 17 00:00:00 2001 From: Adi Date: Mon, 11 Nov 2024 12:13:36 -0500 Subject: [PATCH 6/8] feat: adding Gstreamer to ROS2 topic support --- ground_control_station/docker/.env | 10 +++ .../docker/Dockerfile.gst-ros-bridge | 72 +++++++++++++++++++ .../docker/docker-compose.yaml | 33 +++++++-- .../docker/resources/fastrtps-profile.xml | 19 +++++ 4 files changed, 130 insertions(+), 4 deletions(-) create mode 100644 ground_control_station/docker/Dockerfile.gst-ros-bridge create mode 100644 ground_control_station/docker/resources/fastrtps-profile.xml diff --git a/ground_control_station/docker/.env b/ground_control_station/docker/.env index ce071aa08..85b358b1d 100644 --- a/ground_control_station/docker/.env +++ b/ground_control_station/docker/.env @@ -19,3 +19,13 @@ TAK_SUBSCRIBER_FILEPATH=src/ros2tak_tools/scripts/tak_subscriber.py MQTT_USERNAME= # Enter the MQTT username MQTT_PASSWORD= # Enter the MQTT password + +# GSTREAMER TO ROS NODES ----------------------------------------------- + +# CAMERA 1 +CAMERA1_STREAM_IP=rtsp://10.4.1.33:554/vio +CAMERA1_ROS_TOPIC=/view1/image_raw + +# CAMERA 2 +CAMERA2_STREAM_IP=rtsp:// +CAMERA2_ROS_TOPIC=/view2/image_raw \ No newline at end of file diff --git a/ground_control_station/docker/Dockerfile.gst-ros-bridge b/ground_control_station/docker/Dockerfile.gst-ros-bridge new file mode 100644 index 000000000..ddc6e2ad2 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.gst-ros-bridge @@ -0,0 +1,72 @@ +FROM nvidia/cuda:12.4.1-devel-ubuntu22.04 + +# Install humble +RUN apt-get update && apt-get install -y locales && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +ENV LANG=en_US.UTF-8 +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update && \ + apt-get install -y software-properties-common && \ + apt-add-repository universe + +RUN apt-get update && \ + apt-get install -y curl + +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + +RUN apt-get update && apt-get install -y \ + ros-dev-tools \ + ros-humble-ros-base \ + ros-humble-image-view + +# Utilities +RUN apt-get update && apt-get install -y \ + tmux \ + vim \ + wget + +# Install DIVOTrack dependencies +RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-venv + +# Install ros-gst-bridge +RUN apt-get update && apt-get install -y \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + 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 +RUN mkdir -p /ros_ws/src +WORKDIR /ros_ws +RUN git clone https://github.com/BrettRD/ros-gst-bridge.git src/ros-gst-bridge +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + rosdep init && \ + rosdep update && \ + rosdep install --from-paths /ros_ws/src/ --ignore-src -r -y && \ + colcon build" + +RUN apt-get update && apt-get install -y \ + libnvidia-decode-525-server \ + libnvidia-encode-525-server + +# Disable shared memory for ROS +COPY resources/fastrtps-profile.xml /fastrtps-profile.xml +ENV FASTRTPS_DEFAULT_PROFILES_FILE=/fastrtps-profile.xml + +RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc +RUN echo "source /ros_ws/install/setup.bash" >> ~/.bashrc + +# Entrypoint +CMD ["/bin/bash"] + diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 92b71f6eb..1909f9d3a 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -42,6 +42,27 @@ services: - ../../common/ros_ws:/root/common/ros_ws:rw # common ROS packages - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages +####################### GSTREAMER TO ROS TOPICS ###################### + gst-ros-bridge-topic1: + container_name: "${PROJECT_NAME}/gst-ros-bridge1" + image: "${PROJECT_NAME}/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 @@ -74,7 +95,8 @@ services: dockerfile: docker/Dockerfile.ros2cot_agent args: - ROS_WS_DIR=${ROS_WS_DIR} - container_name: "${PROJECT_NAME}_ros2cot_agent" + image: "${PROJECT_NAME}/ros2cot_agent" + container_name: "${PROJECT_NAME}/ros2cot_agent" stdin_open: true tty: true restart: unless-stopped @@ -98,7 +120,8 @@ services: dockerfile: docker/Dockerfile.tak_publisher args: - ROS_WS_DIR=${ROS_WS_DIR} - container_name: "${PROJECT_NAME}_tak_publisher" + image: "${PROJECT_NAME}/tak_publisher" + container_name: "${PROJECT_NAME}/tak_publisher" stdin_open: true tty: true restart: unless-stopped @@ -123,7 +146,8 @@ services: dockerfile: docker/Dockerfile.tak_subscriber args: - ROS_WS_DIR=${ROS_WS_DIR} - container_name: "${PROJECT_NAME}_tak_subscriber" + image: "${PROJECT_NAME}/tak_subscriber" + container_name: "${PROJECT_NAME}/tak_subscriber" stdin_open: true tty: true restart: unless-stopped @@ -149,7 +173,8 @@ services: dockerfile: ground_control_station/docker/Dockerfile.cot2planner_agent args: - ROS_WS_DIR=${ROS_WS_DIR} - container_name: "${PROJECT_NAME}_cot2planner_agent" + image: "${PROJECT_NAME}/cot2planner_agent" + container_name: "${PROJECT_NAME}/cot2planner_agent" stdin_open: true tty: true restart: unless-stopped diff --git a/ground_control_station/docker/resources/fastrtps-profile.xml b/ground_control_station/docker/resources/fastrtps-profile.xml new file mode 100644 index 000000000..edacf5020 --- /dev/null +++ b/ground_control_station/docker/resources/fastrtps-profile.xml @@ -0,0 +1,19 @@ + + + + + CustomUdpTransport + UDPv4 + + + + + + + CustomUdpTransport + + + false + + + From f0d6135b9b3057fdac55b48d5c90a639d0f4ad45 Mon Sep 17 00:00:00 2001 From: Adi Date: Mon, 11 Nov 2024 12:22:12 -0500 Subject: [PATCH 7/8] fix: fixing typos at docker-compose.yaml --- ground_control_station/docker/docker-compose.yaml | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 74f16b345..4b1070a9f 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -98,7 +98,6 @@ services: - ROS_WS_DIR=${ROS_WS_DIR} image: "${PROJECT_NAME}/ros2cot_agent" container_name: "${PROJECT_NAME}/ros2cot_agent" - container_name: "${PROJECT_NAME}_ros2cot_agent" stdin_open: true tty: true restart: unless-stopped @@ -124,7 +123,6 @@ services: - ROS_WS_DIR=${ROS_WS_DIR} image: "${PROJECT_NAME}/tak_publisher" container_name: "${PROJECT_NAME}/tak_publisher" - container_name: "${PROJECT_NAME}_tak_publisher" stdin_open: true tty: true restart: unless-stopped @@ -151,7 +149,6 @@ services: - ROS_WS_DIR=${ROS_WS_DIR} image: "${PROJECT_NAME}/tak_subscriber" container_name: "${PROJECT_NAME}/tak_subscriber" - container_name: "${PROJECT_NAME}_tak_subscriber" stdin_open: true tty: true restart: unless-stopped @@ -179,7 +176,6 @@ services: - ROS_WS_DIR=${ROS_WS_DIR} image: "${PROJECT_NAME}/cot2planner_agent" container_name: "${PROJECT_NAME}/cot2planner_agent" - container_name: "${PROJECT_NAME}_cot2planner_agent" stdin_open: true tty: true restart: unless-stopped From 2739d576fc7fb426231567f7826ae7b1dca85fbf Mon Sep 17 00:00:00 2001 From: Adi Date: Mon, 11 Nov 2024 13:02:27 -0500 Subject: [PATCH 8/8] fix: adding hierarchy in docker images under docker-compose.yaml --- .../docker/docker-compose.yaml | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml index 4b1070a9f..1416b82cd 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -45,8 +45,8 @@ services: ####################### GSTREAMER TO ROS TOPICS ###################### gst-ros-bridge-topic1: - container_name: "${PROJECT_NAME}/gst-ros-bridge1" - image: "${PROJECT_NAME}/gst-ros-bridge" + container_name: "${PROJECT_NAME}-gst_ros_bridge1" + image: "${PROJECT_NAME}/gcs/gst-ros-bridge" build: context: . dockerfile: Dockerfile.gst-ros-bridge @@ -96,8 +96,8 @@ services: dockerfile: docker/Dockerfile.ros2cot_agent args: - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/ros2cot_agent" - container_name: "${PROJECT_NAME}/ros2cot_agent" + image: "${PROJECT_NAME}/gcs/ros2cot_agent" + container_name: "${PROJECT_NAME}-ros2cot_agent" stdin_open: true tty: true restart: unless-stopped @@ -121,8 +121,8 @@ services: dockerfile: docker/Dockerfile.tak_publisher args: - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/tak_publisher" - container_name: "${PROJECT_NAME}/tak_publisher" + image: "${PROJECT_NAME}/gcs/tak_publisher" + container_name: "${PROJECT_NAME}-tak_publisher" stdin_open: true tty: true restart: unless-stopped @@ -147,8 +147,8 @@ services: dockerfile: docker/Dockerfile.tak_subscriber args: - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/tak_subscriber" - container_name: "${PROJECT_NAME}/tak_subscriber" + image: "${PROJECT_NAME}/gcs/tak_subscriber" + container_name: "${PROJECT_NAME}-tak_subscriber" stdin_open: true tty: true restart: unless-stopped @@ -174,8 +174,8 @@ services: dockerfile: ground_control_station/docker/Dockerfile.cot2planner_agent args: - ROS_WS_DIR=${ROS_WS_DIR} - image: "${PROJECT_NAME}/cot2planner_agent" - container_name: "${PROJECT_NAME}/cot2planner_agent" + image: "${PROJECT_NAME}/gcs/cot2planner_agent" + container_name: "${PROJECT_NAME}-cot2planner_agent" stdin_open: true tty: true restart: unless-stopped