selectively copy models and search for dependencies

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

@@ -1,29 +1,36 @@
 #!/usr/bin/env python3
 
+import importlib
+import json
+import logging
 import os
+import shutil
+from pathlib import Path
+
 import launch
 from launch import LaunchDescription
 from launch.actions import (
+    DeclareLaunchArgument,
     ExecuteProcess,
-    LogInfo,
-    RegisterEventHandler,
     OpaqueFunction,
-    DeclareLaunchArgument,
 )
-import logging
 from launch.substitutions import LaunchConfiguration
-from launch.event_handlers import OnProcessStart, OnProcessExit, OnProcessIO
 from launch_ros.actions import Node
-from sim.utils import find_package_resource, load_yaml_to_dict, build_node_arguments, camel_to_snake
-import importlib
-from pathlib import Path
+
 from sim.constants import (
-    Competition, 
-    COMPETITION_NAMES, 
+    Competition,
+    COMPETITION_NAMES,
     DEFAULT_COMPETITION,
-    DEFAULT_USE_SCORING
+    DEFAULT_USE_SCORING,
+)
+from sim.utils import (
+    build_node_arguments,
+    copy_models_to_gazebo,
+    copy_px4_models,
+    extract_models_from_sdf,
+    find_package_resource,
+    load_yaml_to_dict,
 )
-import json
 
 def load_sim_launch_parameters():
     """Load simulation launch parameters from YAML."""
@@ -37,15 +44,11 @@ def load_sim_launch_parameters():
         if path.exists():
             return load_yaml_to_dict(path)
 
-    # Fallback to installed location (for production/deployed packages)
-    try:
-        from ament_index_python.packages import get_package_share_directory
-        package_share = Path(get_package_share_directory('sim'))
-        installed_params = package_share / 'launch' / 'launch_params.yaml'
-        if installed_params.exists():
-            return load_yaml_to_dict(installed_params)
-    except Exception:
-        pass
+    from ament_index_python.packages import get_package_share_directory
+    package_share = Path(get_package_share_directory('sim'))
+    installed_params = package_share / 'launch' / 'launch_params.yaml'
+    if installed_params.exists():
+        return load_yaml_to_dict(installed_params)
 
     # If not found, raise error
     raise FileNotFoundError(
@@ -60,12 +63,10 @@ def load_sim_parameters(competition: str, logger: logging.Logger) -> str:
 
     Args:
         competition: Competition name (e.g., 'in_house')
+        logger: Logger object
 
     Returns:
         Path to the simulation config file (as string)
-
-    Raises:
-        FileNotFoundError: If config file cannot be found
     """
     config_filename = f"{competition}.yaml"
     config_path = find_package_resource(
@@ -87,11 +88,6 @@ def initialize_mode(logger: logging.Logger, node_path: str, params: dict) -> Nod
 
     Returns:
         Initialized node instance
-
-    Raises:
-        ValueError: If node_path is invalid or parameters are missing/invalid
-        ImportError: If the module cannot be imported
-        AttributeError: If the class is not found in the module
     """
     logger.debug(f"Initializing mode: {node_path} with params: {params}")
 
@@ -149,7 +145,7 @@ def launch_setup(context, *args, **kwargs):
             f"Invalid competition: {competition_num}. Must be one of {valid_values}"
         )
 
-    scoring_param = params.get("scoring", DEFAULT_USE_SCORING)
+    scoring_param = params.get("use_scoring", DEFAULT_USE_SCORING)
 
     if isinstance(scoring_param, str):
         use_scoring = scoring_param.lower() == 'true'
@@ -158,53 +154,120 @@ def launch_setup(context, *args, **kwargs):
 
 
     px4_path_raw = LaunchConfiguration("px4_path").perform(context)
+    vehicle_model = LaunchConfiguration("vehicle_model").perform(context)
 
     if px4_path_raw is None:
         raise RuntimeError("PX4 path is required")
-
-    px4_path = os.path.expanduser(px4_path_raw)
 
+    px4_path = os.path.expanduser(px4_path_raw)
     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",
-    )
+    # Clear both models and worlds directories for clean state
+    models_dir = Path.home() / '.simulation-gazebo' / 'models'
+    worlds_dir = Path.home() / '.simulation-gazebo' / 'worlds'
+
+    if models_dir.exists():
+        shutil.rmtree(models_dir)
+        logger.info(f"Cleared models directory: {models_dir}")
+
+    if worlds_dir.exists():
+        shutil.rmtree(worlds_dir)
+        logger.info(f"Cleared worlds directory: {worlds_dir}")
+
+    # Copy PX4 models
+    logger.info(f"Copying PX4 vehicle model: {vehicle_model}")
+    copy_px4_models(px4_path, [vehicle_model])
+
+    sim_params, sim_config_path = load_sim_parameters(competition, logger)
+
+    if "world" not in sim_params:
+        raise ValueError(f"Missing 'world' section in simulation config: {sim_config_path}")
+
+    world_params = sim_params["world"]
+
+    # Infer world class name from competition name (convert snake_case to PascalCase)
+    world_class_name = ''.join(word.capitalize() for word in competition.split('_'))
+    world_file_name = competition
+
+    # Generate world file
+    logger.info(f"Generating world file for competition: {competition}")
+    try:
+        module_name = f"sim.world_gen.{world_class_name}"
+        world_module = importlib.import_module(module_name)
+        generator_class_name = f"{world_class_name}Generator"
+        world_generator_class = getattr(world_module, generator_class_name)
+
+        # Create generator instance
+        world_generator = world_generator_class(**world_params)
+
+        # Call generate_world with output directory
+        world_generator.generate_world(worlds_dir)
+
+        # Store hoop positions for passing to node
+        hoop_positions = world_generator.hoop_positions
+        logger.info(f"World file generated successfully with {len(hoop_positions)} hoops")
+    except Exception as e:
+        logger.error(f"Failed to generate world file: {e}")
+        raise
+
+    # Copy sim models after world generation
+    logger.info("Copying sim models...")
+    try:
+        src_models_dir = find_package_resource(
+            relative_path='world_gen/models',
+            package_name='sim',
+            resource_type='directory',
+            logger=logger,
+            base_file=Path(__file__)
+        )
+
+        # Extract models from the generated world file
+        world_file = worlds_dir / f"{world_file_name}.sdf"
+        if not world_file.exists():
+            logger.warning(f"World file not found: {world_file}")
+        else:
+            models = extract_models_from_sdf(world_file)
+            logger.info(f"Found {len(models)} sim models in {world_file.name}: {models}")
+
+            if models:
+                logger.info(f"Copying {len(models)} sim models: {sorted(models)}")
+                dst_models_dir = Path.home() / '.simulation-gazebo' / 'models'
+                copy_models_to_gazebo(src_models_dir, dst_models_dir, models=list(models))
+            else:
+                logger.warning("No sim models detected in world file")
+    except Exception as e:
+        logger.error(f"Failed to copy sim models: {e}")
+        raise
 
     spawn_world = ExecuteProcess(
         cmd=[
             "python3",
             "Tools/simulation/gz/simulation-gazebo",
-            f"--world={competition}",
+            f"--world={world_file_name}",
         ],
         cwd=px4_path,
         output="screen",
         name="spawn_world",
     )
 
-    sim_params, sim_config_path = load_sim_parameters(competition, logger)
+    # Start world node for services (like hoop list)
+    # Pass hoop_positions directly to avoid regenerating
+    world_node_params = {'hoop_positions': hoop_positions}
 
-    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 = Node(
         package="sim",
-        executable=camel_to_snake(world_params["name"]),
-        arguments=[json.dumps(world_params["params"])],
+        executable=competition,
+        arguments=[json.dumps(world_node_params)],
         output="screen",
-        name=world_params["name"],
+        name=world_class_name,
         cwd=sae_ws_path,
     )
 
+    actions = [
+        spawn_world,
+        world,
+    ]
+
     # Initialize scoring node if requested
     scoring = None
     if use_scoring:
@@ -213,41 +276,14 @@ def launch_setup(context, *args, **kwargs):
         else:
             scoring = Node(
                 package="sim",
-                executable=camel_to_snake(sim_params["scoring"]["name"]),
-                arguments=[sim_params["scoring"]["params"]],
+                executable="hoop_scoring_node",
+                arguments=[json.dumps(sim_params["scoring"])],
                 output="screen",
-                name=sim_params["scoring"]["name"],
+                name="HoopScoringNode",
                 cwd=sae_ws_path,
             )
 
-    # Build and return the complete list of actions
-    actions = [
-        download_gz_models,
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=download_gz_models,
-                on_exit=[LogInfo(msg="Gazebo models downloaded."), 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
-                )
-            )
-        )
-    ]
-
-    if scoring:
-        actions.append(
-            RegisterEventHandler(
-                OnProcessStart(
-                    target_action=scoring,
-                    on_start=[LogInfo(msg="Scoring node started.")],
-                )
-            )
-        )
+            actions.append(scoring)
 
     return actions
 
@@ -256,6 +292,7 @@ def generate_launch_description():
     return LaunchDescription(
         [
             DeclareLaunchArgument("px4_path", default_value="~/PX4-Autopilot"),
+            DeclareLaunchArgument("vehicle_model", default_value="x500_mono_cam"),
             OpaqueFunction(function=launch_setup),
         ]
     )

controls/sae_2025_ws/src/sim/setup.py

@@ -48,8 +48,8 @@
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'hoop_course = sim.world_gen.HoopCourse:main',
-            'hoop_score = sim.scoring.HoopScore:main',
+            'in_house = sim.world_gen.InHouse:main',
+            'hoop_scoring_node = sim.scoring.HoopScoringNode:main',
         ],
     },
 )

controls/sae_2025_ws/src/sim/sim/scoring/HoopScore.py → controls/sae_2025_ws/src/sim/sim/scoring/HoopScoringNode.py

@@ -1,14 +1,15 @@
 #!/usr/bin/env python3
 
+import json
 import rclpy
-from rclpy.node import Node
 from px4_msgs.msg import VehicleLocalPosition
 from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy
 from std_msgs.msg import Float32, String, Float32MultiArray
 from geometry_msgs.msg import Point
 from typing import List, Tuple, Optional, Dict, Any
 import math
 import time
+import sys
 from sim_interfaces.srv import HoopList
 from sim.scoring import ScoringNode
 
@@ -316,10 +317,9 @@ def position_callback(self, msg: VehicleLocalPosition):
         self.drone_publisher.publish(drone_position)
 
 def main(args=None):
-    """Main function for scoring node. (SAME AS BEFORE)"""
     rclpy.init(args=args)
 
-    scoring_node = ScoringNode()
+    scoring_node = HoopScoringNode(**json.loads(sys.argv[1]))
 
     try:
         rclpy.spin(scoring_node)

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

@@ -1,22 +1,15 @@
 # Simulation Configuration
 world:
-  name: HoopCourse
-  params:
-    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
+  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
 
-
 # Scoring parameters
 scoring:
-  name: HoopScore
-  params:
-    hoop_tolerance: 1.5       # meters - Distance tolerance for hoop passage
-    landing_bonus: 5.0        # Points awarded for landing bonus
-    landing_tolerance: 2.0    # meters - Distance tolerance for landing
-    landing_altitude_max: 0.4 # meters - Maximum altitude to be considered "landed"
+  hoop_tolerance: 1.5       # meters - Distance tolerance for hoop passage
+  landing_bonus: 5.0        # Points awarded for landing bonus
+  landing_tolerance: 2.0    # meters - Distance tolerance for landing
+  landing_altitude_max: 0.4 # meters - Maximum altitude to be considered "landed"