XML world parser

2026-02-21 17:18:30 -05:00
parent 5a60d22458
commit 8ec4bc7846
5 changed files with 387 additions and 74 deletions
--- a/config/search.yaml
+++ b/config/search.yaml
@@ -10,10 +10,11 @@ marker:
  size: 0.5                # Physical marker size in meters
  landing_ids: [0]         # Marker IDs that trigger landing (on UGV)
  target_ids: [1]          # Marker IDs to find and report to UGV
+  target_position: [-5.0, -5.0] # Initial X, Y location of the target Aruco map in the map

 # ── Search Patterns ──────────────────────────────────────────
 spiral:
-  max_legs: 60
+  max_legs: 35

 lawnmower:
  width: 30.0
--- a/scripts/generate_world.py
+++ b/scripts/generate_world.py
@@ -0,0 +1,121 @@
+#!/usr/bin/env python3
+import os
+import yaml
+import sys
+import xml.etree.ElementTree as ET
+
+SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
+PROJECT_DIR = os.path.dirname(SCRIPT_DIR)
+CONFIG_DIR = os.path.join(PROJECT_DIR, "config")
+WORLD_DIR = os.path.join(PROJECT_DIR, "worlds")
+
+def load_config(name):
+    path = os.path.join(CONFIG_DIR, name)
+    if not os.path.exists(path):
+        return {}
+    with open(path, 'r') as f:
+        return yaml.safe_load(f) or {}
+
+def generate_world(base_filename="uav_ugv_search_base.sdf", output_filename="uav_ugv_search.sdf"):
+    ugv_cfg = load_config("ugv.yaml")
+    search_cfg = load_config("search.yaml")
+
+    ugv_x = ugv_cfg.get("position", {}).get("x", 0.0)
+    ugv_y = ugv_cfg.get("position", {}).get("y", 0.0)
+    
+    uav_x = ugv_x
+    uav_y = ugv_y
+    uav_z = 0.40  # Start on top of UGV (0.18 top + 0.195 legs)
+
+    marker = search_cfg.get("marker", {})
+    target_pos = marker.get("target_position", [8.0, -6.0])
+    target_x = target_pos[0]
+    target_y = target_pos[1]
+
+    base_path = os.path.join(WORLD_DIR, base_filename)
+    output_path = os.path.join(WORLD_DIR, output_filename)
+
+    if not os.path.exists(base_path):
+        print(f"[ERROR] Base world file not found: {base_path}")
+        sys.exit(1)
+
+    try:
+        tree = ET.parse(base_path)
+        root = tree.getroot()
+        
+        world = root.find('world')
+        if world is None:
+            print("[ERROR] No <world> tag found in SDF.")
+            sys.exit(1)
+            
+        for include in world.findall('include'):
+            uri = include.find('uri')
+            if uri is not None and uri.text == 'model://iris_with_gimbal':
+                pose = include.find('pose')
+                if pose is not None:
+                    pose.text = f"{uav_x} {uav_y} {uav_z} 0 0 90"
+                    
+            name = include.find('name')
+            if name is not None and name.text == 'ugv':
+                pose = include.find('pose')
+                if pose is not None:
+                    pose.text = f"{ugv_x} {ugv_y} 0 0 0 0"
+
+        for model in world.findall('model'):
+            name_attr = model.get('name')
+            if name_attr == 'target_tag_1':
+                pose = model.find('pose')
+                if pose is not None:
+                    pose.text = f"{target_x} {target_y} 0.005 0 0 0"
+
+        geofence_cfg = search_cfg.get("geofence", {})
+        if geofence_cfg.get("enabled", False):
+            points = geofence_cfg.get("points", [])
+            if len(points) >= 3:
+                # Remove old geofence visual if it exists
+                for old_model in world.findall('model'):
+                    if old_model.get('name') == 'geofence_visual':
+                        world.remove(old_model)
+
+                gf_model = ET.SubElement(world, "model", name="geofence_visual")
+                ET.SubElement(gf_model, "static").text = "true"
+                link = ET.SubElement(gf_model, "link", name="link")
+                
+                import math
+                for i in range(len(points)):
+                    p1 = points[i]
+                    p2 = points[(i + 1) % len(points)]
+                    x1, y1 = float(p1[0]), float(p1[1])
+                    x2, y2 = float(p2[0]), float(p2[1])
+                    
+                    dx = x2 - x1
+                    dy = y2 - y1
+                    length = math.sqrt(dx*dx + dy*dy)
+                    cx = x1 + dx / 2.0
+                    cy = y1 + dy / 2.0
+                    yaw = math.atan2(dy, dx)
+                    
+                    visual = ET.SubElement(link, "visual", name=f"edge_{i}")
+                    ET.SubElement(visual, "pose").text = f"{cx} {cy} 0.01 0 0 {yaw}"
+                    
+                    geometry = ET.SubElement(visual, "geometry")
+                    box = ET.SubElement(geometry, "box")
+                    # size is Length(X), Width(Y), Thickness(Z)
+                    ET.SubElement(box, "size").text = f"{length} 0.2 0.02"
+                    
+                    material = ET.SubElement(visual, "material")
+                    ET.SubElement(material, "ambient").text = "1 0 0 1"
+                    ET.SubElement(material, "diffuse").text = "1 0 0 1"
+                    ET.SubElement(material, "emissive").text = "0.8 0 0 0.5"
+
+        tree.write(output_path, encoding="utf-8", xml_declaration=True)
+        print(f"[INFO] Generated world file: {output_path}")
+        print(f"[INFO] UGV set to ({ugv_x}, {ugv_y})")
+        print(f"[INFO] Target Marker set to ({target_x}, {target_y})")
+        
+    except Exception as e:
+        print(f"[ERROR] Failed to parse or write XML: {e}")
+        sys.exit(1)
+
+if __name__ == "__main__":
+    generate_world()
--- a/scripts/run_autonomous.sh
+++ b/scripts/run_autonomous.sh
@@ -107,30 +107,8 @@ else
    exit 1
 fi

-UGV_CONFIG="$PROJECT_DIR/config/ugv.yaml"
-if [ -f "$UGV_CONFIG" ] && [ -f "$WORLD_FILE" ]; then
-    python3 -c "
-import yaml, re
-cfg = yaml.safe_load(open('$UGV_CONFIG'))
-pos = cfg.get('position', {})
-x, y = pos.get('x', 0.0), pos.get('y', 0.0)
-with open('$WORLD_FILE', 'r') as f:
-    sdf = f.read()
-# Sync UGV include position
-sdf = re.sub(
-    r'(<name>ugv</name>\s*)<pose>[^<]*</pose>',
-    rf'\1<pose>{x} {y} 0 0 0 0</pose>',
-    sdf, count=1)
-# Sync UAV on top of UGV (body top=0.18 + iris legs=0.195 ≈ 0.40)
-uav_z = 0.40
-sdf = re.sub(
-    r'(<uri>model://iris_with_gimbal</uri>\s*)<pose[^>]*>[^<]*</pose>',
-    rf'\1<pose degrees=\"true\">{x} {y} {uav_z} 0 0 90</pose>',
-    sdf, count=1)
-with open('$WORLD_FILE', 'w') as f:
-    f.write(sdf)
-print(f'[INFO] UGV at ({x}, {y}) — UAV on top at z={uav_z}')
-" 2>/dev/null || print_info "Position sync skipped"
+if [ -f "$PROJECT_DIR/scripts/generate_world.py" ]; then
+    python3 "$PROJECT_DIR/scripts/generate_world.py"
 fi

 print_info "==================================="
--- a/worlds/uav_ugv_search.sdf
+++ b/worlds/uav_ugv_search.sdf
@@ -1,52 +1,38 @@
-<?xml version="1.0" ?>
-<!--
-  Custom UAV-UGV Search & Land World
-  ===================================
-  - ArduPilot iris quadcopter (with SITL plugin + gimbal camera)
-  - UGV target vehicle at (10, 5) for the drone to search for and land on
-  - Visual navigation markers (colored squares on the ground)
-  - Optimized for WSL: lower physics rate, no shadows
-->
+<?xml version='1.0' encoding='utf-8'?>
 <sdf version="1.9">
  <world name="uav_ugv_search">

-    <!-- Physics: 4ms step (250 Hz) for better WSL performance -->
+    
    <physics name="2ms" type="ignore">
      <max_step_size>0.002</max_step_size>
      <real_time_factor>1.0</real_time_factor>
    </physics>

-    <!-- Required Gazebo Harmonic system plugins -->
-    <plugin filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
+    
+    <plugin filename="gz-sim-physics-system" name="gz::sim::systems::Physics">
    </plugin>
-    <plugin filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
+    <plugin filename="gz-sim-sensors-system" name="gz::sim::systems::Sensors">
      <render_engine>ogre2</render_engine>
    </plugin>
-    <plugin filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
+    <plugin filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands">
    </plugin>
-    <plugin filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
+    <plugin filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster">
      <state_hertz>25</state_hertz>
    </plugin>
-    <plugin filename="gz-sim-imu-system"
-      name="gz::sim::systems::Imu">
+    <plugin filename="gz-sim-imu-system" name="gz::sim::systems::Imu">
    </plugin>
-    <plugin filename="gz-sim-navsat-system"
-      name="gz::sim::systems::NavSat">
+    <plugin filename="gz-sim-navsat-system" name="gz::sim::systems::NavSat">
    </plugin>

-    <!-- Scene: no shadows for WSL performance -->
+    
    <scene>
      <ambient>1.0 1.0 1.0</ambient>
      <background>0.6 0.75 0.9</background>
-      <sky></sky>
+      <sky />
      <shadows>false</shadows>
    </scene>

-    <!-- Spherical coordinates (matches ArduPilot SITL default location) -->
+    
    <spherical_coordinates>
      <latitude_deg>-35.363262</latitude_deg>
      <longitude_deg>149.165237</longitude_deg>
@@ -55,7 +41,7 @@
      <surface_model>EARTH_WGS84</surface_model>
    </spherical_coordinates>

-    <!-- Sun (no shadows for performance) -->
+    
    <light type="directional" name="sun">
      <cast_shadows>false</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
@@ -70,7 +56,7 @@
      <direction>-0.5 0.1 -0.9</direction>
    </light>

-    <!-- ===================== GROUND PLANE ===================== -->
+    
    <model name="ground_plane">
      <static>true</static>
      <link name="link">
@@ -83,6 +69,7 @@
          </geometry>
        </collision>
        <visual name="visual">
+          <pose>0 0 -0.02 0 0 0</pose>
          <geometry>
            <plane>
              <normal>0 0 1</normal>
@@ -97,11 +84,9 @@
      </link>
    </model>

-    <!-- ===================== COORDINATE AXES ===================== -->
    <model name="axes">
      <static>1</static>
      <link name="link">
-        <!-- X axis (red) = North -->
        <visual name="r">
          <cast_shadows>0</cast_shadows>
          <pose>5 0 0.05 0 0 0</pose>
@@ -112,7 +97,6 @@
            <emissive>1 0 0 0.5</emissive>
          </material>
        </visual>
-        <!-- Y axis (green) = East -->
        <visual name="g">
          <cast_shadows>0</cast_shadows>
          <pose>0 5 0.05 0 0 0</pose>
@@ -123,7 +107,6 @@
            <emissive>0 1 0 0.5</emissive>
          </material>
        </visual>
-        <!-- Z axis (blue) = Up -->
        <visual name="b">
          <cast_shadows>0</cast_shadows>
          <pose>0 0 5.05 0 0 0</pose>
@@ -134,7 +117,6 @@
            <emissive>0 0 1 0.5</emissive>
          </material>
        </visual>
-        <!-- NavSat sensor (needed for GPS/geofence) -->
        <sensor name="navsat_sensor" type="navsat">
          <always_on>1</always_on>
          <update_rate>1</update_rate>
@@ -142,26 +124,23 @@
      </link>
    </model>

-    <!-- ===================== UAV: ArduPilot Iris ===================== -->
-    <!-- Starts on top of the UGV. Body top z=0.18 + iris legs 0.195 = 0.375 -->
+    
    <include>
      <uri>model://iris_with_gimbal</uri>
      <pose degrees="true">0.0 0.0 0.4 0 0 90</pose>
    </include>

-    <!-- ===================== UGV: Driveable Rover ===================== -->
-    <!-- Dynamic 4-wheel skid-steer, drone takes off from it, then it drives -->
+    
    <include>
      <uri>model://custom_ugv</uri>
      <name>ugv</name>
      <pose>0.0 0.0 0 0 0 0</pose>
    </include>

-    <!-- ===================== TARGET ArUco Tag (ID 1) ===================== -->
-    <!-- On the ground — the UAV searches for this, then UGV drives here -->
+    
    <model name="target_tag_1">
      <static>true</static>
-      <pose>8 -6 0.005 0 0 0</pose>
+      <pose>-5.0 -5.0 0.005 0 0 0</pose>
      <link name="link">
        <visual name="v">
          <geometry><box><size>0.5 0.5 0.01</size></box></geometry>
@@ -174,10 +153,8 @@
      </link>
    </model>

-    <!-- ===================== VISUAL MARKERS ===================== -->
-    <!-- Ground markers for visual navigation reference -->
-
-    <!-- Origin marker (yellow circle) -->
+    
+    
    <model name="origin_marker">
      <static>true</static>
      <pose>0 0 0.005 0 0 0</pose>
@@ -193,7 +170,7 @@
      </link>
    </model>

-    <!-- Waypoint marker 1 (red) at 5,0 -->
+    
    <model name="marker_red">
      <static>true</static>
      <pose>5 0 0.005 0 0 0</pose>
@@ -209,7 +186,7 @@
      </link>
    </model>

-    <!-- Waypoint marker 2 (green) at 10,0 -->
+    
    <model name="marker_green">
      <static>true</static>
      <pose>10 0 0.005 0 0 0</pose>
@@ -225,7 +202,7 @@
      </link>
    </model>

-    <!-- Waypoint marker 3 (blue) at 10,10 -->
+    
    <model name="marker_blue">
      <static>true</static>
      <pose>10 10 0.005 0 0 0</pose>
@@ -241,5 +218,5 @@
      </link>
    </model>

-  </world>
-</sdf>
+  <model name="geofence_visual"><static>true</static><link name="link"><visual name="edge_0"><pose>-15.0 0.0 0.01 0 0 1.5707963267948966</pose><geometry><box><size>30.0 0.2 0.02</size></box></geometry><material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse><emissive>0.8 0 0 0.5</emissive></material></visual><visual name="edge_1"><pose>0.0 15.0 0.01 0 0 0.0</pose><geometry><box><size>30.0 0.2 0.02</size></box></geometry><material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse><emissive>0.8 0 0 0.5</emissive></material></visual><visual name="edge_2"><pose>15.0 0.0 0.01 0 0 -1.5707963267948966</pose><geometry><box><size>30.0 0.2 0.02</size></box></geometry><material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse><emissive>0.8 0 0 0.5</emissive></material></visual><visual name="edge_3"><pose>0.0 -15.0 0.01 0 0 3.141592653589793</pose><geometry><box><size>30.0 0.2 0.02</size></box></geometry><material><ambient>1 0 0 1</ambient><diffuse>1 0 0 1</diffuse><emissive>0.8 0 0 0.5</emissive></material></visual></link></model></world>
+</sdf>
--- a/worlds/uav_ugv_search_base.sdf
+++ b/worlds/uav_ugv_search_base.sdf
@@ -0,0 +1,236 @@
+<?xml version="1.0" ?>
+<!--
+  Custom UAV-UGV Search & Land World
+  ===================================
+  - ArduPilot iris quadcopter (with SITL plugin + gimbal camera)
+  - UGV target vehicle at (10, 5) for the drone to search for and land on
+  - Visual navigation markers (colored squares on the ground)
+  - Optimized for WSL: lower physics rate, no shadows
+-->
+<sdf version="1.9">
+  <world name="uav_ugv_search">
+
+    <!-- Physics: 4ms step (250 Hz) for better WSL performance -->
+    <physics name="2ms" type="ignore">
+      <max_step_size>0.002</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+
+    <!-- Required Gazebo Harmonic system plugins -->
+    <plugin filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+      <state_hertz>25</state_hertz>
+    </plugin>
+    <plugin filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin>
+    <plugin filename="gz-sim-navsat-system"
+      name="gz::sim::systems::NavSat">
+    </plugin>
+
+    <!-- Scene: no shadows for WSL performance -->
+    <scene>
+      <ambient>1.0 1.0 1.0</ambient>
+      <background>0.6 0.75 0.9</background>
+      <sky></sky>
+      <shadows>false</shadows>
+    </scene>
+
+    <!-- Spherical coordinates (matches ArduPilot SITL default location) -->
+    <spherical_coordinates>
+      <latitude_deg>-35.363262</latitude_deg>
+      <longitude_deg>149.165237</longitude_deg>
+      <elevation>584</elevation>
+      <heading_deg>0</heading_deg>
+      <surface_model>EARTH_WGS84</surface_model>
+    </spherical_coordinates>
+
+    <!-- Sun (no shadows for performance) -->
+    <light type="directional" name="sun">
+      <cast_shadows>false</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.9 0.9 0.9 1</diffuse>
+      <specular>0.5 0.5 0.5 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <!-- ===================== GROUND PLANE ===================== -->
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <pose>0 0 -0.02 0 0 0</pose>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.5 0.55 0.45 1</ambient>
+            <diffuse>0.5 0.55 0.45 1</diffuse>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <model name="axes">
+      <static>1</static>
+      <link name="link">
+        <visual name="r">
+          <cast_shadows>0</cast_shadows>
+          <pose>5 0 0.05 0 0 0</pose>
+          <geometry><box><size>10 0.02 0.02</size></box></geometry>
+          <material>
+            <ambient>1 0 0 0.8</ambient>
+            <diffuse>1 0 0 0.8</diffuse>
+            <emissive>1 0 0 0.5</emissive>
+          </material>
+        </visual>
+        <visual name="g">
+          <cast_shadows>0</cast_shadows>
+          <pose>0 5 0.05 0 0 0</pose>
+          <geometry><box><size>0.02 10 0.02</size></box></geometry>
+          <material>
+            <ambient>0 1 0 0.8</ambient>
+            <diffuse>0 1 0 0.8</diffuse>
+            <emissive>0 1 0 0.5</emissive>
+          </material>
+        </visual>
+        <visual name="b">
+          <cast_shadows>0</cast_shadows>
+          <pose>0 0 5.05 0 0 0</pose>
+          <geometry><box><size>0.02 0.02 10</size></box></geometry>
+          <material>
+            <ambient>0 0 1 0.8</ambient>
+            <diffuse>0 0 1 0.8</diffuse>
+            <emissive>0 0 1 0.5</emissive>
+          </material>
+        </visual>
+        <sensor name="navsat_sensor" type="navsat">
+          <always_on>1</always_on>
+          <update_rate>1</update_rate>
+        </sensor>
+      </link>
+    </model>
+
+    <!-- ===================== UAV: ArduPilot Iris ===================== -->
+    <include>
+      <uri>model://iris_with_gimbal</uri>
+      <pose degrees="true">0.0 0.0 0.4 0 0 90</pose>
+    </include>
+
+    <!-- ===================== UGV: Driveable Rover ===================== -->
+    <include>
+      <uri>model://custom_ugv</uri>
+      <name>ugv</name>
+      <pose>0.0 0.0 0 0 0 0</pose>
+    </include>
+
+    <!-- ===================== TARGET ArUco Tag (ID 1) ===================== -->
+    <model name="target_tag_1">
+      <static>true</static>
+      <pose>8 -6 0.005 0 0 0</pose>
+      <link name="link">
+        <visual name="v">
+          <geometry><box><size>0.5 0.5 0.01</size></box></geometry>
+          <material>
+            <ambient>1 1 1 1</ambient>
+            <diffuse>1 1 1 1</diffuse>
+            <pbr><metal><albedo_map>tags/aruco_DICT_4X4_50_1.png</albedo_map></metal></pbr>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- ===================== VISUAL MARKERS ===================== -->
+    <!-- Origin marker (yellow circle) -->
+    <model name="origin_marker">
+      <static>true</static>
+      <pose>0 0 0.005 0 0 0</pose>
+      <link name="link">
+        <visual name="v">
+          <geometry><cylinder><radius>0.4</radius><length>0.01</length></cylinder></geometry>
+          <material>
+            <ambient>1 1 0 1</ambient>
+            <diffuse>1 1 0 1</diffuse>
+            <emissive>0.8 0.8 0 0.6</emissive>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Waypoint marker 1 (red) at 5,0 -->
+    <model name="marker_red">
+      <static>true</static>
+      <pose>5 0 0.005 0 0 0</pose>
+      <link name="link">
+        <visual name="v">
+          <geometry><box><size>0.5 0.5 0.01</size></box></geometry>
+          <material>
+            <ambient>1 0.1 0.1 1</ambient>
+            <diffuse>1 0.1 0.1 1</diffuse>
+            <emissive>0.8 0 0 0.5</emissive>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Waypoint marker 2 (green) at 10,0 -->
+    <model name="marker_green">
+      <static>true</static>
+      <pose>10 0 0.005 0 0 0</pose>
+      <link name="link">
+        <visual name="v">
+          <geometry><box><size>0.5 0.5 0.01</size></box></geometry>
+          <material>
+            <ambient>0.1 1 0.1 1</ambient>
+            <diffuse>0.1 1 0.1 1</diffuse>
+            <emissive>0 0.8 0 0.5</emissive>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <!-- Waypoint marker 3 (blue) at 10,10 -->
+    <model name="marker_blue">
+      <static>true</static>
+      <pose>10 10 0.005 0 0 0</pose>
+      <link name="link">
+        <visual name="v">
+          <geometry><box><size>0.5 0.5 0.01</size></box></geometry>
+          <material>
+            <ambient>0.1 0.1 1 1</ambient>
+            <diffuse>0.1 0.1 1 1</diffuse>
+            <emissive>0 0 0.8 0.5</emissive>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+  </world>
+</sdf>