selectively copy models and search for dependencies

controls/sae_2025_ws/src/sim/launch/sim.launch.py

@@ -166,17 +166,6 @@ def launch_setup(context, *args, **kwargs):
 
     sae_ws_path = os.path.expanduser(os.getcwd())
 
-    download_gz_models = ExecuteProcess(
-        cmd=[
-            "python3",
-            "Tools/simulation/gz/simulation-gazebo",
-            "--dryrun",
-        ],
-        cwd=px4_path,
-        output="screen",
-        name="download_gz_models",
-    )
-
     spawn_world = ExecuteProcess(
         cmd=[
             "python3",
@@ -192,10 +181,11 @@ def launch_setup(context, *args, **kwargs):
 
     if "world" not in sim_params:
         raise ValueError(f"Missing 'world' section in simulation config: {sim_config_path}")
-    
+
     world_params = sim_params["world"].copy()
-
-    # Initialize world node - it will generate the world file automatically
+    world_params["params"]["competition_name"] = competition
+
+    # Initialize world node - it will generate the world file and copy sim models
     world = Node(
         package="sim",
         executable=camel_to_snake(world_params["name"]),
@@ -221,19 +211,17 @@ def launch_setup(context, *args, **kwargs):
             )
 
     # Build and return the complete list of actions
+    # Model copying flow:
+    # 1. UAV launch clears models dir and copies PX4 vehicle models + dependencies
+    # 2. WorldNode generates world file and auto-detects/copies sim models (merges with PX4 models)
+    # 3. spawn_world starts Gazebo with the generated world and all models
     actions = [
-        download_gz_models,
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=download_gz_models,
-                on_exit=[LogInfo(msg="Gazebo models downloaded."), world],
-            )
-        ),
+        world,
         RegisterEventHandler(
             OnProcessIO(
                 target_action=world,
                 on_stderr=lambda event: (
-                    [spawn_world, LogInfo(msg="Simulation world node started."), scoring] if b"Successfully generated world file:" in event.text else None
+                    [spawn_world, LogInfo(msg="World generated, starting Gazebo."), scoring] if b"World file ready at:" in event.text else None
                 )
             )
         )

controls/sae_2025_ws/src/sim/sim/simulations/in_house.yaml

@@ -2,14 +2,13 @@
 world:
   name: HoopCourse
   params:
-    course: "ascent" 
+    course: "ascent"
     dlz: [10, 5, 0]     # Drop zone coordinates [x, y, z]
     uav: [0, 0, 0]       # UAV starting position [x, y, z]
     num_hoops: 2          # Number of hoops to generate
     height: 5
     max_dist: 1
-    world_name: "src/sim/sim/world_gen/worlds/template.sdf"   # Gazebo world template
-    # output_filename is optional - defaults to competition name
+    world_name: "world_gen/worlds/template.sdf"   # Gazebo world template (relative to package root)
 
 
 # Scoring parameters

controls/sae_2025_ws/src/sim/sim/utils.py

@@ -197,43 +197,159 @@ def find_package_resource(
     )
 
 
-def copy_models_to_gazebo(src_models_dir: Path, dst_models_dir: Path) -> None:
+def _get_model_dependencies(model_path: Path) -> list[str]:
     """
-    Copy model files from source to Gazebo models directory.
-
+    Parse model.sdf to find model:// dependencies.
+
+    Args:
+        model_path: Path to the model directory
+
+    Returns:
+        List of model names that this model depends on
+    """
+    dependencies = []
+    model_sdf = model_path / 'model.sdf'
+
+    if not model_sdf.exists():
+        return dependencies
+
+    try:
+        with open(model_sdf, 'r') as f:
+            content = f.read()
+            # Find all model:// references (handles both model://name and model://name/path/to/file)
+            uri_patterns = re.findall(r'model://([^/<>]+)', content)
+            dependencies.extend(uri_patterns)
+    except Exception:
+        # Silently ignore parse errors for dependency detection
+        pass
+
+    return dependencies
+
+def _copy_model_with_dependencies(
+    src_models_dir: Path,
+    dst_models_dir: Path,
+    model_name: str,
+    copied_models: set[str]
+) -> None:
+    """
+    Recursively copy a model and its dependencies.
+
     Args:
         src_models_dir: Source models directory
-        dst_models_dir: Destination models directory (Gazebo models path)
-
+        dst_models_dir: Destination models directory
+        model_name: Name of model to copy
+        copied_models: Set of already-copied model names (modified in-place)
+    """
+    # Skip if already copied
+    if model_name in copied_models:
+        return
+
+    src_model = src_models_dir / model_name
+    if not src_model.exists():
+        # Model might be a built-in Gazebo model or PX4 model, skip
+        return
+
+    # Copy the model
+    dst_model = dst_models_dir / model_name
+    if src_model.is_dir():
+        shutil.copytree(src_model, dst_model, dirs_exist_ok=True)
+
+    copied_models.add(model_name)
+
+    # Recursively copy dependencies
+    dependencies = _get_model_dependencies(src_model)
+    for dep in dependencies:
+        _copy_model_with_dependencies(src_models_dir, dst_models_dir, dep, copied_models)
+
+def copy_models_to_gazebo(src_models_dir: Path, dst_models_dir: Path, models: Optional[list[str]] = None) -> None:
+    """
+    Copy sim model files to Gazebo models directory.
+
+    This function merges sim models (hoop, dlz, payload, etc.) with existing PX4 models
+    using dirs_exist_ok=True. It should be called *after* PX4 vehicle models are copied.
+
+    Model copying flow:
+    1. UAV launch: copy_px4_models() clears ~/.simulation-gazebo/models/ and copies PX4 vehicle models
+    2. WorldNode: This function merges sim models with existing PX4 models (dirs_exist_ok=True)
+
+    Args:
+        src_models_dir: Source models directory (sim package models)
+        dst_models_dir: Destination models directory (~/.simulation-gazebo/models)
+        models: Optional list of specific sim model names to copy. If None, copies all models.
+
     Raises:
         OSError: If copy operations fail
+        FileNotFoundError: If a specified model doesn't exist
     """
     if not src_models_dir.exists():
         raise FileNotFoundError(f"Source models directory does not exist: {src_models_dir}")
-    
+
     if not src_models_dir.is_dir():
         raise ValueError(f"Source path is not a directory: {src_models_dir}")
-    
-    # Create destination directory if it doesn't exist
+
+    # Create destination directory if needed
     try:
         dst_models_dir.mkdir(parents=True, exist_ok=True)
     except OSError as e:
         raise OSError(f"Failed to create destination directory {dst_models_dir}: {e}")
-
-    # Copy all content from src_models_dir to dst_models_dir, merging contents
+
+    # Track which models have been copied to avoid duplicates
+    copied_models = set()
+
+    # Determine which models to copy
+    if models is None:
+        # Copy all models in the directory
+        models_to_copy = [item.name for item in src_models_dir.iterdir() if item.is_dir()]
+    else:
+        models_to_copy = models
+
+    # Copy each model with dependencies
     try:
-        for item in src_models_dir.iterdir():
-            src_item = src_models_dir / item.name
-            dst_item = dst_models_dir / item.name
-
-            if src_item.is_dir():
-                # Copytree with dirs_exist_ok=True to allow merging/updating
-                shutil.copytree(src_item, dst_item, dirs_exist_ok=True)
-            elif src_item.is_file():
-                shutil.copy2(src_item, dst_item)
+        for model_name in models_to_copy:
+            _copy_model_with_dependencies(src_models_dir, dst_models_dir, model_name, copied_models)
     except (OSError, shutil.Error) as e:
         raise OSError(f"Failed to copy models from {src_models_dir} to {dst_models_dir}: {e}")
 
+def extract_models_from_sdf(sdf_path: Path) -> list[str]:
+    """
+    Parse an SDF file and extract all model:// URI references.
+
+    This function scans an SDF world file for <uri>model://...</uri> tags
+    and returns a list of unique model names that need to be copied to
+    the Gazebo models directory.
+
+    Args:
+        sdf_path: Path to the SDF world file
+
+    Returns:
+        List of unique model names referenced in the world file.
+        Returns empty list if file doesn't exist or parsing fails.
+
+    Example:
+        For <uri>model://hoop</uri>, returns ['hoop']
+        For <uri>model://dlz_red</uri>, returns ['dlz_red']
+    """
+    import xml.etree.ElementTree as ET
+
+    if not sdf_path.exists():
+        return []
+
+    try:
+        tree = ET.parse(str(sdf_path))
+        root = tree.getroot()
+
+        models = set()
+        # Find all <uri> elements containing model:// references
+        for uri_elem in root.iter('uri'):
+            if uri_elem.text and uri_elem.text.startswith('model://'):
+                model_name = uri_elem.text.replace('model://', '').strip()
+                models.add(model_name)
+
+        return sorted(list(models))
+    except ET.ParseError as e:
+        # Log parse error but don't crash - just return empty list
+        return []
+
 def camel_to_snake(name: str) -> str:
     """Convert CamelCase to snake_case."""
     return re.sub('(.)([A-Z][a-z]+)', r'\1_\2', name).lower()