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 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/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..92b71f6eb 100644 --- a/ground_control_station/docker/docker-compose.yaml +++ b/ground_control_station/docker/docker-compose.yaml @@ -41,3 +41,132 @@ 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" + ] + + + ################## 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: + 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/.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 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 - 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..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 @@ -1,13 +1,88 @@ #!/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 and is non-empty. + # WinTAK sometimes generates blank points, ATAK is fine. + if point_str: + 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 + +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 +96,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 +119,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()