diff --git a/controls/sae_2025_ws/src/sim/launch/sim.launch.py b/controls/sae_2025_ws/src/sim/launch/sim.launch.py
index 516dc66df..79f33cc88 100644
--- a/controls/sae_2025_ws/src/sim/launch/sim.launch.py
+++ b/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),
]
)
diff --git a/controls/sae_2025_ws/src/sim/setup.py b/controls/sae_2025_ws/src/sim/setup.py
index 10c96db9d..e2acc2fd7 100644
--- a/controls/sae_2025_ws/src/sim/setup.py
+++ b/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',
],
},
)
diff --git a/controls/sae_2025_ws/src/sim/sim/scoring/HoopScore.py b/controls/sae_2025_ws/src/sim/sim/scoring/HoopScoringNode.py
similarity index 99%
rename from controls/sae_2025_ws/src/sim/sim/scoring/HoopScore.py
rename to controls/sae_2025_ws/src/sim/sim/scoring/HoopScoringNode.py
index 521cf0e93..a11532661 100644
--- a/controls/sae_2025_ws/src/sim/sim/scoring/HoopScore.py
+++ b/controls/sae_2025_ws/src/sim/sim/scoring/HoopScoringNode.py
@@ -1,7 +1,7 @@
#!/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
@@ -9,6 +9,7 @@
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)
diff --git a/controls/sae_2025_ws/src/sim/sim/simulations/in_house.yaml b/controls/sae_2025_ws/src/sim/sim/simulations/in_house.yaml
index 65550bdbd..90789800f 100644
--- a/controls/sae_2025_ws/src/sim/sim/simulations/in_house.yaml
+++ b/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"
\ No newline at end of file
+ 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"
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/sim/sim/utils.py b/controls/sae_2025_ws/src/sim/sim/utils.py
index 1cbd4ad26..0fc85a50e 100644
--- a/controls/sae_2025_ws/src/sim/sim/utils.py
+++ b/controls/sae_2025_ws/src/sim/sim/utils.py
@@ -6,11 +6,13 @@
import ast
import inspect
import os
+import re
import shutil
+import xml.etree.ElementTree as ET
from pathlib import Path
from typing import Any, Dict, Optional, Union
+
import yaml
-import re
def load_yaml_to_dict(params_file: Path) -> dict:
"""Load and validate YAML file."""
@@ -197,43 +199,202 @@ 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)
+ # Also find plain model_name without model:// prefix (used by PX4 models)
+ plain_uri_patterns = re.findall(r'([a-zA-Z0-9_-]+)', content)
+ dependencies.extend(plain_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)
-
- Raises:
- OSError: If copy operations fail
+ 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.
+
+ 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.
"""
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 model://... 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 model://hoop, returns ['hoop']
+ For model://dlz_red, returns ['dlz_red']
+ """
+ if not sdf_path.exists():
+ return []
+
+ try:
+ tree = ET.parse(str(sdf_path))
+ root = tree.getroot()
+
+ models = set()
+ # Find all 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 copy_px4_models(px4_path: str, models: list[str]) -> None:
+ """
+ Copy required PX4 vehicle models from PX4-Autopilot to Gazebo models directory.
+
+ Clears ~/.simulation-gazebo/models/ first to ensure a clean state,
+ then recursively copies the specified models and all their dependencies.
+
+ This is called before sim.launch starts, setting up the base PX4 models that
+ will later be merged with sim models (hoop, dlz, etc.) by WorldNode.
+
+ Args:
+ px4_path: Path to PX4-Autopilot directory
+ models: List of model names to copy (e.g., ['x500_mono_cam'])
+ """
+ px4_models_dir = Path(px4_path) / 'Tools' / 'simulation' / 'gz' / 'models'
+ dst_models_dir = Path.home() / '.simulation-gazebo' / 'models'
+
+ # Clear destination directory first to ensure clean state
+ if dst_models_dir.exists():
+ shutil.rmtree(dst_models_dir)
+
+ # Create destination directory
+ dst_models_dir.mkdir(parents=True, exist_ok=True)
+
+ # Track models to copy and already copied to avoid duplicates
+ models_to_copy = set(models)
+ copied_models = set()
+
+ while models_to_copy:
+ model_name = models_to_copy.pop()
+
+ if model_name in copied_models:
+ continue
+
+ src_model = px4_models_dir / model_name
+ dst_model = dst_models_dir / model_name
+
+ if not src_model.exists():
+ print(f"Warning: PX4 model '{model_name}' not found in {px4_models_dir}")
+ continue
+
+ # Copy the model
+ if src_model.is_dir():
+ shutil.copytree(src_model, dst_model)
+ copied_models.add(model_name)
+ print(f"Copied PX4 model: {model_name}")
+
+ # Find and queue dependencies
+ dependencies = _get_model_dependencies(src_model)
+ for dep in dependencies:
+ if dep not in copied_models:
+ models_to_copy.add(dep)
+
def camel_to_snake(name: str) -> str:
"""Convert CamelCase to snake_case."""
return re.sub('(.)([A-Z][a-z]+)', r'\1_\2', name).lower()
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py b/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py
deleted file mode 100644
index 9a625cb5f..000000000
--- a/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py
+++ /dev/null
@@ -1,626 +0,0 @@
-import random as r
-import xml.etree.ElementTree as ET
-from sim.world_gen import WorldNode
-from xml.dom import minidom
-from pathlib import Path
-from abc import ABC, abstractmethod
-from typing import List, Tuple, Optional
-from sim_interfaces.srv import HoopList
-from sim.world_gen import WorldNode
-import math
-import rclpy
-import sys
-from sim_interfaces.msg import HoopPose
-import json
-
-Pose = Tuple[float, float, float, float, float, float]
-
-class CourseStyle(ABC):
- """Abstract base class for all course styles."""
-
- def __init__(self,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- height: int
- ):
- self.dlz = dlz
- self.uav = uav
- self.num_hoops = num_hoops
- self.height = height
- # max_dist represents max distance we want next step for hoop in our course
- self.max_dist = max_dist
-
- @abstractmethod
- def generate_course(self) -> List[Pose]:
- """Generate a list of (x, y, z, roll, pitch, yaw) for hoop placement."""
- pass
-
-class AscentCourse(CourseStyle):
- def __init__(self,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- start_height: int
- ):
- super().__init__(dlz, uav, num_hoops, max_dist, start_height)
- self.start_height = start_height
-
- def generate_course(self) -> List[Pose]:
- hoops: List[Pose] = []
- x_dlz, y_dlz, z_dlz = self.dlz
- x_uav, y_uav, z_uav = self.uav
-
- # Direction vector UAV → DLZ
- dx = x_dlz - x_uav
- dy = y_dlz - y_uav
- dz = z_dlz - z_uav
-
- # Total distance
- dist = math.sqrt(dx**2 + dy**2 + dz**2)
- if dist == 0:
- raise ValueError("DLZ and UAV cannot be the same point.")
-
- # Normalize to get unit vector
- dir_x = dx / dist
- dir_y = dy / dist
- dir_z = dz / dist
-
- # Distance between hoops
- segment_len = dist / self.num_hoops
-
- for i in range(self.num_hoops):
- # Progress along line (slightly randomized)
- along = segment_len * (i + r.uniform(0.2, 0.8))
-
- # Height transition from UAV’s start_height to DLZ altitude
- frac = (i + 1) / self.num_hoops
- z_interp = self.start_height + frac * (z_dlz - self.start_height)
- z_noise = r.uniform(-0.3, 0.3) * (z_dlz - self.start_height) / self.num_hoops
-
- new_x = x_uav + dir_x * along
- new_y = y_uav + dir_y * along
- new_z = z_interp + z_noise
-
- # Each hoop faces along the course direction
- yaw_deg = math.degrees(math.atan2(dy, dx))
- hoops.append((new_x, new_y, new_z, 0, yaw_deg, 0))
-
- return hoops
-
-class DescentCourse(CourseStyle):
- def __init__(self,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- start_height: int
- ):
- super().__init__(dlz, uav, num_hoops, max_dist, start_height)
- self.start_height = start_height
-
- def generate_course(self) -> List[Pose]:
- hoops: List[Pose] = []
- x_dlz, y_dlz, z_dlz = self.dlz
- x_uav, y_uav, z_uav = self.uav
-
- # Direction vector UAV → DLZ
- dx = x_dlz - x_uav
- dy = y_dlz - y_uav
- dz = z_dlz - z_uav
-
- # Total distance
- dist = math.sqrt(dx**2 + dy**2 + dz**2)
- if dist == 0:
- raise ValueError("DLZ and UAV cannot be the same point.")
-
- # Normalize to get unit vector
- dir_x = dx / dist
- dir_y = dy / dist
- dir_z = dz / dist
-
- # Distance between hoops
- segment_len = dist / self.num_hoops
-
- for i in range(self.num_hoops):
- # Progress along line (slightly randomized)
- along = segment_len * (i + r.uniform(0.2, 0.8))
-
- # Height transition from start_height down to DLZ
- frac = (i + 1) / self.num_hoops
- z_interp = self.start_height + frac * (z_dlz - self.start_height) # dz negative if descending
- z_noise = r.uniform(-0.3, 0.3) * abs(z_dlz - self.start_height) / self.num_hoops
-
- new_x = x_uav + dir_x * along
- new_y = y_uav + dir_y * along
- new_z = z_interp + z_noise
-
- # Orientation along course
- yaw_deg = math.degrees(math.atan2(dy, dx))
- hoops.append((new_x, new_y, new_z, 0, yaw_deg, 0))
-
- return hoops
-
-class SlalomCourse(CourseStyle):
- def __init__(self,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- width: int,
- height: int
- ):
- super().__init__(dlz, uav, num_hoops, max_dist, height)
- self.width = width
- self.height = height
-
- def generate_course(self) -> List[Pose]:
- hoops: List[Pose] = []
- x_dlz, y_dlz, z_dlz = self.dlz
- x_uav, y_uav, z_uav = self.uav
-
- # Direction vector from UAV → DLZ
- dx = x_dlz - x_uav
- dy = y_dlz - y_uav
- dz = z_dlz - z_uav
-
- # Distance between UAV and DLZ
- dist = math.sqrt(dx**2 + dy**2 + dz**2)
- if dist == 0:
- raise ValueError("DLZ and UAV cannot be at the same position.")
-
- # Normalize to get unit direction vector
- dir_x = dx / dist
- dir_y = dy / dist
- dir_z = dz / dist
-
- # Get perpendicular vector in XY plane for slalom offset
- # (-dy, dx) is perpendicular to (dx, dy)
- perp_x = -dir_y
- perp_y = dir_x
-
- zone_len = dist / self.num_hoops
-
- for i in range(self.num_hoops):
- alt = 1 if i % 2 == 0 else -1
-
- # Move forward along direction vector
- along_dist = zone_len * (i + r.uniform(0.2, 0.8)) # slight randomness
- offset = alt * self.width * 0.5 * r.uniform(0.5, 1.0)
-
- # Compute new hoop position
- new_x = x_uav + dir_x * along_dist + perp_x * offset
- new_y = y_uav + dir_y * along_dist + perp_y * offset
- new_z = r.uniform(self.height * 0.3, self.height * 0.7)
-
- hoops.append((new_x, new_y, new_z, 0, 90, 0))
-
- return hoops
-
-class StraightCourse(CourseStyle):
- """
- Simple straight course with hoops in a straight line.
- Perfect for testing basic navigation and scoring.
- """
-
- def __init__(self,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- height: float = 2.0,
- spacing: float = 2.0
- ):
- # Call parent initializer
- super().__init__(dlz, uav, num_hoops, max_dist, height)
-
- # Height of all hoops (same for straight course)
- self.height = height
-
- # Distance between hoops
- self.spacing = spacing
-
- def generate_course(self) -> List[Pose]:
- """
- Generate a straight line of hoops.
-
- Returns:
- List of (x, y, z, roll, pitch, yaw) positions for each hoop
- """
- hoops = []
- x_dlz, y_dlz, z_dlz = self.dlz
- x_uav, y_uav, z_uav = self.uav
-
- # Calculate total course length
- total_length = (self.num_hoops - 1) * self.spacing
-
- # Start position (slightly offset from UAV start)
- start_x = x_uav + 1.0 # 1 meter forward from UAV
- start_y = y_uav
-
- # Generate hoops in a straight line
- for i in range(self.num_hoops):
- # Calculate position
- hoop_x = start_x + (i * self.spacing)
- hoop_y = start_y
- hoop_z = self.height
-
- # Add small random variation (±0.2m) for realism
- variation_x = 0
- variation_y = 0
- variation_z = 0
-
- # Final position with variation
- final_x = hoop_x + variation_x
- final_y = hoop_y + variation_y
- final_z = hoop_z + variation_z
-
- # All hoops face forward (yaw = 0)
- hoop_pose = (final_x, final_y, final_z, 0, 90, 0)
- hoops.append(hoop_pose)
-
- return hoops
-
-class BezierCourse(CourseStyle):
- """
- Smooth cubic Bezier curve between UAV start and DLZ.
- P0 = UAV, P3 = DLZ
- P1/P2 are along the line with lateral offsets to create an arc.
- """
-
- def __init__(self,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- height: float = 2.0,
- lateral_offset: float = 4.0):
- super().__init__(dlz, uav, num_hoops, max_dist, height)
- self.height = height
- self.lateral_offset = lateral_offset
-
- def generate_course(self) -> List[Pose]:
- hoops: List[Pose] = []
- x_dlz, y_dlz, z_dlz = self.dlz
- x_uav, y_uav, z_uav = self.uav
-
- # Direction UAV -> DLZ in XY
- dx = x_dlz - x_uav
- dy = y_dlz - y_uav
- dz = z_dlz - z_uav
-
- dist_xy = math.hypot(dx, dy)
- dist_3d = math.sqrt(dx**2 + dy**2 + dz**2)
- if dist_xy == 0 or dist_3d == 0:
- raise ValueError("DLZ and UAV cannot be the same point.")
-
- dir_x = dx / dist_xy
- dir_y = dy / dist_xy
-
- # Perpendicular vector in XY plane
- perp_x = -dir_y
- perp_y = dir_x
-
- # Bezier control points in XY
- # P0 and P3 are UAV and DLZ
- P0x, P0y = x_uav, y_uav
- P3x, P3y = x_dlz, y_dlz
-
- # Place P1 and P2 along the line with opposite lateral offsets
- one_third = dist_xy / 3.0
- two_third = 2.0 * dist_xy / 3.0
-
- P1x = P0x + dir_x * one_third + perp_x * self.lateral_offset
- P1y = P0y + dir_y * one_third + perp_y * self.lateral_offset
-
- P2x = P0x + dir_x * two_third - perp_x * self.lateral_offset
- P2y = P0y + dir_y * two_third - perp_y * self.lateral_offset
-
- # Helper: cubic Bezier in 2D
- def bezier_xy(t: float) -> Tuple[float, float]:
- mt = 1.0 - t
- mt2 = mt * mt
- t2 = t * t
- x = (mt2 * mt) * P0x + 3 * (mt2) * t * P1x + 3 * mt * t2 * P2x + (t2 * t) * P3x
- y = (mt2 * mt) * P0y + 3 * (mt2) * t * P1y + 3 * mt * t2 * P2y + (t2 * t) * P3y
- return x, y
-
- # Helper: derivative for tangent direction
- def bezier_xy_deriv(t: float) -> Tuple[float, float]:
- mt = 1.0 - t
- x = 3 * mt * mt * (P1x - P0x) + 6 * mt * t * (P2x - P1x) + 3 * t * t * (P3x - P2x)
- y = 3 * mt * mt * (P1y - P0y) + 6 * mt * t * (P2y - P1y) + 3 * t * t * (P3y - P2y)
- return x, y
-
- # Place hoops along t in (0, 1)
- for i in range(self.num_hoops):
- # keep hoops off the exact endpoints
- t = (i + 1) / (self.num_hoops + 1)
-
- x, y = bezier_xy(t)
-
- # Smooth altitude: interpolate between UAV z and DLZ z
- z_linear = z_uav + t * (z_dlz - z_uav)
- # small noise
- z_noise = r.uniform(-0.2, 0.2)
- z = max(0.5, z_linear + z_noise) # keep above ground a bit
-
- # Orientation from tangent of curve
- dx_dt, dy_dt = bezier_xy_deriv(t)
- if dx_dt == 0 and dy_dt == 0:
- yaw_deg = 0.0
- else:
- yaw = math.atan2(dy_dt, dx_dt)
- yaw_deg = math.degrees(yaw)
-
- hoops.append((x, y, z, 0.0, yaw_deg, 0.0))
-
- return hoops
-
-
-class HoopCourseNode(WorldNode):
- """
- World generation node for in_house competition.
- Generates a hoop course with various course styles (ascent, descent, slalom, etc.).
- """
-
- def __init__(self,
- course: str,
- dlz: Tuple[float, float, float],
- uav: Tuple[float, float, float],
- num_hoops: int,
- max_dist: int,
- world_name: str,
- height: int,
- output_filename: Optional[str] = None):
- """
- Initialize the hoop course world generator.
-
- Args:
- course: Course style ("ascent", "descent", "slalom", "bezier", "straight", "random", "previous")
- dlz: Drop zone coordinates [x, y, z]
- uav: UAV starting position [x, y, z]
- num_hoops: Number of hoops to generate
- max_dist: Maximum distance parameter for course generation
- world_name: Path to template world SDF file
- output_filename: Optional output filename (defaults to competition name)
- """
- super().__init__(competition_name="in_house", output_filename=output_filename)
- self.course = course
- self.dlz = dlz
- self.uav = uav
- self.num_hoops = num_hoops
- self.max_dist = max_dist
- self.world_name = world_name
- self.height = height
- self.hoop_positions: List[Pose] = []
- self.generate_world()
-
- # Create service for providing hoop positions
- self.srv = self.create_service(HoopList, "list_hoops", self.hoop_list_req)
-
- def hoop_list_req(self, request, response):
- response.hoop_positions = []
- for (x, y, z, roll, pitch, yaw) in self.hoop_positions:
- hp = HoopPose()
- hp.x = float(x)
- hp.y = float(y)
- hp.z = float(z)
- hp.roll = float(roll)
- hp.pitch = float(pitch)
- hp.yaw = float(yaw)
- response.hoop_positions.append(hp)
- return response
-
-
- def add_hoops(self, input_file, output_file):
- # Expand ~, make absolute, and validate paths
- in_path = Path(input_file).expanduser().resolve()
- out_path = Path(output_file).expanduser().resolve()
-
- self.get_logger().debug(f"Input world file: {in_path}")
- self.get_logger().debug(f"Output world file: {out_path}")
- if not in_path.exists():
- raise FileNotFoundError(
- f"Input SDF not found: {in_path}\nCWD: {Path.cwd().resolve()}"
- )
- try:
- out_path.parent.mkdir(parents=True, exist_ok=True)
- except OSError as e:
- raise OSError(f"Failed to create output directory {out_path.parent}: {e}")
-
- # Parse
- try:
- tree = ET.parse(str(in_path)) # str() for safety on older libs
- root = tree.getroot()
- except ET.ParseError as e:
- raise ValueError(f"Failed to parse XML file {in_path}: {e}")
- except Exception as e:
- raise RuntimeError(f"Error reading SDF file {in_path}: {e}")
-
- # handle namespace if present (root.tag may be like "{...}sdf")
- ns = ""
- if root.tag.startswith("{"):
- ns = root.tag.split("}")[0] + "}"
-
- # try to find with/without namespace
- world = root.find(f"{ns}world") or root.find("world") or root.find(".//world")
- if world is None:
- raise RuntimeError("No tag found in the SDF!")
-
- # Add new hoops with given positions
- for i, pos in enumerate(self.hoop_positions, start=1):
- inc = ET.Element("include")
- x, y, z, roll, pitch, yaw = pos
- z = 1 if z < 1 else z
- ET.SubElement(inc, "uri").text = "model://hoop"
- ET.SubElement(inc, "pose").text = f"{x} {y} {z} {roll} {pitch} {yaw}"
- ET.SubElement(inc, "name").text = f"hoop_{i}"
- world.append(inc)
-
-
- # Use last two hoops to define the approach direction
- if len(self.hoop_positions) >= 2:
- (prev_x, prev_y, _prev_z, *_), (end_x, end_y, end_z, *_) = self.hoop_positions[-2], self.hoop_positions[-1]
- else:
- # Fallback: only one hoop; use UAV->hoop direction
- start_x, start_y, _ = self.uav
- end_x, end_y, end_z, *_, = self.hoop_positions[-1]
- prev_x, prev_y = start_x, start_y
-
- # Direction vector in XY from previous hoop to final hoop
- dir_x = end_x - prev_x
- dir_y = end_y - prev_y
- length = math.hypot(dir_x, dir_y)
- if length == 0:
- # Degenerate case: fall back to +X
- dir_x, dir_y = 1.0, 0.0
- else:
- dir_x /= length
- dir_y /= length
-
- # Perpendicular vector (rotate by +90 degrees)
- perp_x = -dir_y
- perp_y = dir_x
-
- # How far apart to space the DLZs (meters)
- spacing = 3.0
- num_dlz = 3
-
- # Center DLZs around the final hoop's XY position
- center_x = end_x
- center_y = end_y
-
- # DLZs on the ground
- dlz_z = 0.0
-
- # Yaw aligned with the approach direction
- yaw = math.atan2(dir_y, dir_x)
- roll = 0.0
- pitch = 0.0
-# Colors for 3 DLZs
- colors = [
- ("red", "1 0 0 1"),
- ("green", "0 1 0 1"),
- ("blue", "0 0 1 1"),
- ]
-
- for i in range(num_dlz):
- offset = (i - (num_dlz - 1) / 2.0) * spacing # -s, 0, +s
- dlz_x = center_x + offset * perp_x
- dlz_y = center_y + offset * perp_y
-
- color_name, rgba = colors[i]
-
- dlz_inc = ET.Element("include")
- ET.SubElement(dlz_inc, "uri").text = "model://dlz_" + color_name
- ET.SubElement(dlz_inc, "name").text = f"dlz_{i+1}"
- ET.SubElement(dlz_inc, "pose").text = f"{dlz_x} {dlz_y} {dlz_z} {roll} {pitch} {yaw}"
-
- # --- Material override ---
- # This forces all visuals of the included model to use the chosen color
- model_override = ET.SubElement(dlz_inc, "model")
- link_override = ET.SubElement(model_override, "link", {"name": "link"}) # assumes main link is "link"
- visual_override = ET.SubElement(link_override, "visual", {"name": "visual"})
- material = ET.SubElement(visual_override, "material")
- # ambient = ET.SubElement(material, "ambient")
- diffuse = ET.SubElement(material, "diffuse")
-
- # ambient.text = rgba
- diffuse.text = rgba
-
- world.append(dlz_inc)
-
-
- # --- remove whitespace-only text/tail nodes to avoid minidom producing extra blank lines ---
- def strip_whitespace(elem):
- if elem.text is not None and elem.text.strip() == "":
- elem.text = None
- for child in list(elem):
- strip_whitespace(child)
- if child.tail is not None and child.tail.strip() == "":
- child.tail = None
- strip_whitespace(root)
-
- # Pretty print and write
- try:
- rough_bytes = ET.tostring(root, encoding="utf-8")
- pretty = minidom.parseString(rough_bytes.decode("utf-8")).toprettyxml(indent=" ")
- lines = [ln for ln in pretty.splitlines() if ln.strip() != ""]
- out_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
- self.get_logger().info(f"Successfully generated world file: {out_path}")
- except Exception as e:
- raise RuntimeError(f"Failed to write output world file {out_path}: {e}")
-
- def generate_world(self) -> None:
- """
- Generate the world file with hoops and DLZs.
-
- Implements the abstract method from WorldNode.
- """
- courses = ['ascent', 'descent', 'slalom', 'bezier']
- course = None
-
- try:
- if self.course.lower() == 'random':
- course_id = int(r.uniform(0, len(courses)))
- course = courses[course_id]
-
- if self.course.lower() == "previous":
- self.get_logger().info("Using previous world configuration")
- return
-
- elif self.course.lower() == "ascent":
- course = AscentCourse(dlz=self.dlz,
- uav=self.uav,
- num_hoops=self.num_hoops,
- max_dist=self.max_dist,
- start_height=self.height)
- elif self.course.lower() == "descent":
- course = DescentCourse(dlz=self.dlz,
- uav=self.uav,
- num_hoops=self.num_hoops,
- max_dist=self.max_dist,
- start_height=self.height)
- elif self.course.lower() == "slalom":
- course = SlalomCourse(dlz=self.dlz,
- uav=self.uav,
- num_hoops=self.num_hoops,
- max_dist=self.max_dist,
- width=4,
- height=self.height)
- elif self.course.lower() == "straight":
- course = StraightCourse(dlz=self.dlz,
- uav=self.uav,
- num_hoops=self.num_hoops,
- max_dist=self.max_dist,
- height=self.height,
- spacing=2)
- elif self.course.lower() == "bezier":
- course = BezierCourse(dlz=self.dlz,
- uav=self.uav,
- num_hoops=self.num_hoops,
- max_dist=self.max_dist,
- height=self.height,
- lateral_offset=4.0)
- else:
- raise ValueError(f"Invalid course: {self.course}")
-
- self.hoop_positions = course.generate_course()
- self.get_logger().info(f"Generated {len(self.hoop_positions)} hoops for {self.course} course")
- self.add_hoops(input_file=self.world_name, output_file=str(self.output_path))
- except Exception as e:
- self.get_logger().error(f"Failed to generate world: {e}")
- raise
-
-def main(args=None):
- rclpy.init(args=args)
- node = HoopCourseNode(**json.loads(sys.argv[1]))
- try:
- rclpy.spin(node)
- except Exception as e:
- print(e)
- rclpy.shutdown()
-
diff --git a/controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py b/controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py
new file mode 100644
index 000000000..db650f6b6
--- /dev/null
+++ b/controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py
@@ -0,0 +1,522 @@
+import json
+import logging
+import math
+import random as r
+import sys
+import xml.etree.ElementTree as ET
+from abc import ABC, abstractmethod
+from pathlib import Path
+from typing import List, Tuple
+from xml.dom import minidom
+
+import rclpy
+from sim_interfaces.msg import HoopPose
+from sim_interfaces.srv import HoopList
+from sim.utils import find_package_resource
+from sim.world_gen.WorldGenerator import WorldGenerator
+from sim.world_gen.WorldNode import WorldNode
+
+Pose = Tuple[float, float, float, float, float, float]
+
+
+class CourseStyle(ABC):
+ """Abstract base class for all course styles."""
+
+ def __init__(self,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ height: int
+ ):
+ self.dlz = dlz
+ self.uav = uav
+ self.num_hoops = num_hoops
+ self.height = height
+ self.max_dist = max_dist
+
+ @abstractmethod
+ def generate_course(self) -> List[Pose]:
+ """Generate a list of (x, y, z, roll, pitch, yaw) for hoop placement."""
+ pass
+
+
+class AscentCourse(CourseStyle):
+ def __init__(self,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ start_height: int
+ ):
+ super().__init__(dlz, uav, num_hoops, max_dist, start_height)
+ self.start_height = start_height
+
+ def generate_course(self) -> List[Pose]:
+ hoops: List[Pose] = []
+ x_dlz, y_dlz, z_dlz = self.dlz
+ x_uav, y_uav, z_uav = self.uav
+
+ dx = x_dlz - x_uav
+ dy = y_dlz - y_uav
+ dz = z_dlz - z_uav
+
+ dist = math.sqrt(dx**2 + dy**2 + dz**2)
+ if dist == 0:
+ raise ValueError("DLZ and UAV cannot be the same point.")
+
+ dir_x = dx / dist
+ dir_y = dy / dist
+
+ segment_len = dist / self.num_hoops
+
+ for i in range(self.num_hoops):
+ along = segment_len * (i + r.uniform(0.2, 0.8))
+ frac = (i + 1) / self.num_hoops
+ z_interp = self.start_height + frac * (z_dlz - self.start_height)
+ z_noise = r.uniform(-0.3, 0.3) * (z_dlz - self.start_height) / self.num_hoops
+
+ new_x = x_uav + dir_x * along
+ new_y = y_uav + dir_y * along
+ new_z = z_interp + z_noise
+
+ yaw_deg = math.degrees(math.atan2(dy, dx))
+ hoops.append((new_x, new_y, new_z, 0, yaw_deg, 0))
+
+ return hoops
+
+
+class DescentCourse(CourseStyle):
+ def __init__(self,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ start_height: int
+ ):
+ super().__init__(dlz, uav, num_hoops, max_dist, start_height)
+ self.start_height = start_height
+
+ def generate_course(self) -> List[Pose]:
+ hoops: List[Pose] = []
+ x_dlz, y_dlz, z_dlz = self.dlz
+ x_uav, y_uav, z_uav = self.uav
+
+ dx = x_dlz - x_uav
+ dy = y_dlz - y_uav
+ dz = z_dlz - z_uav
+
+ dist = math.sqrt(dx**2 + dy**2 + dz**2)
+ if dist == 0:
+ raise ValueError("DLZ and UAV cannot be the same point.")
+
+ dir_x = dx / dist
+ dir_y = dy / dist
+
+ segment_len = dist / self.num_hoops
+
+ for i in range(self.num_hoops):
+ along = segment_len * (i + r.uniform(0.2, 0.8))
+ frac = (i + 1) / self.num_hoops
+ z_interp = self.start_height + frac * (z_dlz - self.start_height)
+ z_noise = r.uniform(-0.3, 0.3) * abs(z_dlz - self.start_height) / self.num_hoops
+
+ new_x = x_uav + dir_x * along
+ new_y = y_uav + dir_y * along
+ new_z = z_interp + z_noise
+
+ yaw_deg = math.degrees(math.atan2(dy, dx))
+ hoops.append((new_x, new_y, new_z, 0, yaw_deg, 0))
+
+ return hoops
+
+
+class SlalomCourse(CourseStyle):
+ def __init__(self,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ width: int,
+ height: int
+ ):
+ super().__init__(dlz, uav, num_hoops, max_dist, height)
+ self.width = width
+ self.height = height
+
+ def generate_course(self) -> List[Pose]:
+ hoops: List[Pose] = []
+ x_dlz, y_dlz, z_dlz = self.dlz
+ x_uav, y_uav, z_uav = self.uav
+
+ dx = x_dlz - x_uav
+ dy = y_dlz - y_uav
+
+ dist = math.sqrt(dx**2 + dy**2)
+ if dist == 0:
+ raise ValueError("DLZ and UAV cannot be at the same position.")
+
+ dir_x = dx / dist
+ dir_y = dy / dist
+
+ perp_x = -dir_y
+ perp_y = dir_x
+
+ zone_len = dist / self.num_hoops
+
+ for i in range(self.num_hoops):
+ alt = 1 if i % 2 == 0 else -1
+ along_dist = zone_len * (i + r.uniform(0.2, 0.8))
+ offset = alt * self.width * 0.5 * r.uniform(0.5, 1.0)
+
+ new_x = x_uav + dir_x * along_dist + perp_x * offset
+ new_y = y_uav + dir_y * along_dist + perp_y * offset
+ new_z = r.uniform(self.height * 0.3, self.height * 0.7)
+
+ hoops.append((new_x, new_y, new_z, 0, 90, 0))
+
+ return hoops
+
+
+class StraightCourse(CourseStyle):
+ """Simple straight course with hoops in a straight line."""
+
+ def __init__(self,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ height: float = 2.0,
+ spacing: float = 2.0
+ ):
+ super().__init__(dlz, uav, num_hoops, max_dist, height)
+ self.height = height
+ self.spacing = spacing
+
+ def generate_course(self) -> List[Pose]:
+ hoops = []
+ x_uav, y_uav, z_uav = self.uav
+
+ start_x = x_uav + 1.0
+ start_y = y_uav
+
+ for i in range(self.num_hoops):
+ hoop_x = start_x + (i * self.spacing)
+ hoop_y = start_y
+ hoop_z = self.height
+
+ hoop_pose = (hoop_x, hoop_y, hoop_z, 0, 90, 0)
+ hoops.append(hoop_pose)
+
+ return hoops
+
+
+class BezierCourse(CourseStyle):
+ """Smooth cubic Bezier curve between UAV start and DLZ."""
+
+ def __init__(self,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ height: float = 2.0,
+ lateral_offset: float = 4.0):
+ super().__init__(dlz, uav, num_hoops, max_dist, height)
+ self.height = height
+ self.lateral_offset = lateral_offset
+
+ def generate_course(self) -> List[Pose]:
+ hoops: List[Pose] = []
+ x_dlz, y_dlz, z_dlz = self.dlz
+ x_uav, y_uav, z_uav = self.uav
+
+ dx = x_dlz - x_uav
+ dy = y_dlz - y_uav
+
+ dist_xy = math.hypot(dx, dy)
+ dist_3d = math.sqrt(dx**2 + dy**2 + (z_dlz - z_uav)**2)
+ if dist_xy == 0 or dist_3d == 0:
+ raise ValueError("DLZ and UAV cannot be the same point.")
+
+ dir_x = dx / dist_xy
+ dir_y = dy / dist_xy
+
+ perp_x = -dir_y
+ perp_y = dir_x
+
+ P0x, P0y = x_uav, y_uav
+ P3x, P3y = x_dlz, y_dlz
+
+ one_third = dist_xy / 3.0
+ two_third = 2.0 * dist_xy / 3.0
+
+ P1x = P0x + dir_x * one_third + perp_x * self.lateral_offset
+ P1y = P0y + dir_y * one_third + perp_y * self.lateral_offset
+
+ P2x = P0x + dir_x * two_third - perp_x * self.lateral_offset
+ P2y = P0y + dir_y * two_third - perp_y * self.lateral_offset
+
+ def bezier_xy(t: float) -> Tuple[float, float]:
+ mt = 1.0 - t
+ mt2 = mt * mt
+ t2 = t * t
+ x = (mt2 * mt) * P0x + 3 * (mt2) * t * P1x + 3 * mt * t2 * P2x + (t2 * t) * P3x
+ y = (mt2 * mt) * P0y + 3 * (mt2) * t * P1y + 3 * mt * t2 * P2y + (t2 * t) * P3y
+ return x, y
+
+ def bezier_xy_deriv(t: float) -> Tuple[float, float]:
+ mt = 1.0 - t
+ x = 3 * mt * mt * (P1x - P0x) + 6 * mt * t * (P2x - P1x) + 3 * t * t * (P3x - P2x)
+ y = 3 * mt * mt * (P1y - P0y) + 6 * mt * t * (P2y - P1y) + 3 * t * t * (P3y - P2y)
+ return x, y
+
+ for i in range(self.num_hoops):
+ t = (i + 1) / (self.num_hoops + 1)
+ x, y = bezier_xy(t)
+
+ z_linear = z_uav + t * (z_dlz - z_uav)
+ z_noise = r.uniform(-0.2, 0.2)
+ z = max(0.5, z_linear + z_noise)
+
+ dx_dt, dy_dt = bezier_xy_deriv(t)
+ if dx_dt == 0 and dy_dt == 0:
+ yaw_deg = 0.0
+ else:
+ yaw_deg = math.degrees(math.atan2(dy_dt, dx_dt))
+
+ hoops.append((x, y, z, 0.0, yaw_deg, 0.0))
+
+ return hoops
+
+
+class InHouseGenerator(WorldGenerator):
+ """
+ World generator for in_house competition.
+
+ Generates a hoop course with various course styles and writes the world file.
+ After generation, hoop_positions contains the generated positions which can
+ be passed to InHouseNode via create_node().
+ """
+
+ COURSES = ['ascent', 'descent', 'slalom', 'bezier', 'straight']
+
+ def __init__(self,
+ course: str,
+ dlz: Tuple[float, float, float],
+ uav: Tuple[float, float, float],
+ num_hoops: int,
+ max_dist: int,
+ height: int):
+ self.course = course
+ self.dlz = tuple(dlz) if isinstance(dlz, list) else dlz
+ self.uav = tuple(uav) if isinstance(uav, list) else uav
+ self.num_hoops = num_hoops
+ self.max_dist = max_dist
+ self.height = height
+ self.hoop_positions: List[Pose] = []
+ self._logger = logging.getLogger(self.__class__.__name__)
+
+ self.world_name = "world_gen/worlds/in_house.sdf"
+ self._validate_parameters()
+
+ def create_node(self) -> 'InHouseNode':
+ """Create and return the InHouseNode with hoop positions from generation."""
+ return InHouseNode(hoop_positions=self.hoop_positions)
+
+ def get_logger(self):
+ return self._logger
+
+ def _validate_parameters(self):
+ if self.course not in self.COURSES and self.course != 'random':
+ raise ValueError(f"Invalid course type: '{self.course}'. Must be one of {self.COURSES}")
+ if self.num_hoops < 1:
+ raise ValueError(f"num_hoops must be >= 1, got {self.num_hoops}")
+ if self.height < 0:
+ raise ValueError(f"height must be >= 0, got {self.height}")
+ if self.max_dist < 0:
+ raise ValueError(f"max_dist must be >= 0, got {self.max_dist}")
+ if not (isinstance(self.dlz, (list, tuple)) and len(self.dlz) == 3):
+ raise ValueError(f"dlz must be a list/tuple of 3 numbers, got {self.dlz}")
+ if not (isinstance(self.uav, (list, tuple)) and len(self.uav) == 3):
+ raise ValueError(f"uav must be a list/tuple of 3 numbers, got {self.uav}")
+ if self.course not in ['previous', 'straight']:
+ if tuple(self.uav) == tuple(self.dlz):
+ raise ValueError("UAV and DLZ coordinates cannot be identical for this course type")
+
+ def generate_world(self, output_dir: Path) -> None:
+ """Generate the world file with hoops and DLZs."""
+ output_path = output_dir / "in_house.sdf"
+
+ course = self.course
+ if course == 'random':
+ course = r.choice(self.COURSES)
+ self.course = course
+
+ base_params = {
+ "dlz": self.dlz,
+ "uav": self.uav,
+ "num_hoops": self.num_hoops,
+ "max_dist": self.max_dist
+ }
+
+ if course == "ascent":
+ generator = AscentCourse(**{**base_params, "start_height": self.height})
+ elif course == "descent":
+ generator = DescentCourse(**{**base_params, "start_height": self.height})
+ elif course == "slalom":
+ generator = SlalomCourse(**{**base_params, "width": 4, "height": self.height})
+ elif course == "straight":
+ generator = StraightCourse(**{**base_params, "height": self.height, "spacing": 2})
+ elif course == "bezier":
+ generator = BezierCourse(**{**base_params, "height": self.height, "lateral_offset": 4.0})
+ else:
+ raise ValueError(f"Invalid course: {course}")
+
+ self.hoop_positions = generator.generate_course()
+ self._logger.info(f"Generated {len(self.hoop_positions)} hoops for {course} course")
+
+ self._write_world_file(output_path)
+
+ def _write_world_file(self, output_path: Path) -> None:
+ """Write the world SDF file with hoops and DLZs."""
+ in_path = find_package_resource(
+ relative_path=self.world_name,
+ package_name='sim',
+ resource_type='file',
+ logger=self._logger,
+ base_file=Path(__file__)
+ )
+
+ output_path.parent.mkdir(parents=True, exist_ok=True)
+
+ tree = ET.parse(str(in_path))
+ root = tree.getroot()
+
+ ns = ""
+ if root.tag.startswith("{"):
+ ns = root.tag.split("}")[0] + "}"
+
+ world = root.find(f"{ns}world") or root.find("world") or root.find(".//world")
+ if world is None:
+ raise RuntimeError("No tag found in the SDF!")
+
+ # Add hoops
+ for i, pos in enumerate(self.hoop_positions, start=1):
+ inc = ET.Element("include")
+ x, y, z, roll, pitch, yaw = pos
+ z = 1 if z < 1 else z
+ ET.SubElement(inc, "uri").text = "model://hoop"
+ ET.SubElement(inc, "pose").text = f"{x} {y} {z} {roll} {pitch} {yaw}"
+ ET.SubElement(inc, "name").text = f"hoop_{i}"
+ world.append(inc)
+
+ # Add DLZs
+ if len(self.hoop_positions) >= 2:
+ (prev_x, prev_y, *_), (end_x, end_y, *_) = self.hoop_positions[-2], self.hoop_positions[-1]
+ else:
+ start_x, start_y, _ = self.uav
+ end_x, end_y, *_ = self.hoop_positions[-1]
+ prev_x, prev_y = start_x, start_y
+
+ dir_x = end_x - prev_x
+ dir_y = end_y - prev_y
+ length = math.hypot(dir_x, dir_y)
+ if length == 0:
+ dir_x, dir_y = 1.0, 0.0
+ else:
+ dir_x /= length
+ dir_y /= length
+
+ perp_x = -dir_y
+ perp_y = dir_x
+ spacing = 3.0
+ yaw = math.atan2(dir_y, dir_x)
+
+ colors = [("red", "1 0 0 1"), ("green", "0 1 0 1"), ("blue", "0 0 1 1")]
+
+ for i in range(3):
+ offset = (i - 1) * spacing
+ dlz_x = end_x + offset * perp_x
+ dlz_y = end_y + offset * perp_y
+
+ color_name, rgba = colors[i]
+
+ dlz_inc = ET.Element("include")
+ ET.SubElement(dlz_inc, "uri").text = "model://dlz_" + color_name
+ ET.SubElement(dlz_inc, "name").text = f"dlz_{i+1}"
+ ET.SubElement(dlz_inc, "pose").text = f"{dlz_x} {dlz_y} 0 0 0 {yaw}"
+
+ model_override = ET.SubElement(dlz_inc, "model")
+ link_override = ET.SubElement(model_override, "link", {"name": "link"})
+ visual_override = ET.SubElement(link_override, "visual", {"name": "visual"})
+ material = ET.SubElement(visual_override, "material")
+ diffuse = ET.SubElement(material, "diffuse")
+ diffuse.text = rgba
+
+ world.append(dlz_inc)
+
+ def strip_whitespace(elem):
+ if elem.text is not None and elem.text.strip() == "":
+ elem.text = None
+ for child in list(elem):
+ strip_whitespace(child)
+ if child.tail is not None and child.tail.strip() == "":
+ child.tail = None
+ strip_whitespace(root)
+
+ rough_bytes = ET.tostring(root, encoding="utf-8")
+ pretty = minidom.parseString(rough_bytes.decode("utf-8")).toprettyxml(indent=" ")
+ lines = [ln for ln in pretty.splitlines() if ln.strip() != ""]
+ output_path.write_text("\n".join(lines) + "\n", encoding="utf-8")
+ self._logger.info(f"Generated world file: {output_path}")
+
+
+class InHouseNode(WorldNode):
+ """
+ ROS node for in_house competition.
+
+ Created by InHouseGenerator.create_node() with hoop positions from generation.
+ Provides the list_hoops service for querying hoop positions.
+ """
+
+ def __init__(self, hoop_positions: List[Pose]):
+ super().__init__()
+
+ self.hoop_positions = hoop_positions
+
+ # Set up ROS services
+ self.srv = self.create_service(HoopList, "list_hoops", self._hoop_list_callback)
+ self.get_logger().info("World node ready") # Standard marker for launch detection
+ self.get_logger().info(f"InHouseNode ready with {len(self.hoop_positions)} hoops")
+
+ def _hoop_list_callback(self, request, response):
+ """Handle list_hoops service request."""
+ response.hoop_positions = []
+ for (x, y, z, roll, pitch, yaw) in self.hoop_positions:
+ hp = HoopPose()
+ hp.x = float(x)
+ hp.y = float(y)
+ hp.z = float(z)
+ hp.roll = float(roll)
+ hp.pitch = float(pitch)
+ hp.yaw = float(yaw)
+ response.hoop_positions.append(hp)
+ return response
+
+
+def main(args=None):
+ """Entry point for running InHouseNode."""
+ rclpy.init(args=args)
+
+ params = json.loads(sys.argv[1])
+ # hoop_positions is passed as a list of lists from JSON
+ hoop_positions = [tuple(p) for p in params['hoop_positions']]
+
+ node = InHouseNode(hoop_positions=hoop_positions)
+
+ try:
+ rclpy.spin(node)
+ except Exception as e:
+ node.get_logger().error(f"Error: {e}")
+ finally:
+ rclpy.shutdown()
diff --git a/controls/sae_2025_ws/src/sim/sim/world_gen/WorldGenerator.py b/controls/sae_2025_ws/src/sim/sim/world_gen/WorldGenerator.py
new file mode 100644
index 000000000..b3f544e25
--- /dev/null
+++ b/controls/sae_2025_ws/src/sim/sim/world_gen/WorldGenerator.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+"""
+Abstract base class for world generators.
+
+WorldGenerator is the primary abstraction for world generation. Each generator:
+1. Handles full world generation (SDF file creation)
+2. Creates and returns the associated ROS node with generation data
+"""
+
+from abc import ABC, abstractmethod
+from pathlib import Path
+
+from sim.world_gen.WorldNode import WorldNode
+
+
+class WorldGenerator(ABC):
+ """
+ Abstract base class for world generators.
+
+ Subclasses implement world generation and node creation.
+ """
+
+ @abstractmethod
+ def generate_world(self, output_dir: Path) -> None:
+ """
+ Generate the world SDF file.
+
+ Args:
+ output_dir: Directory where world file will be written
+ """
+ pass
+
+ @abstractmethod
+ def create_node(self) -> WorldNode:
+ """
+ Create and return the ROS node.
+
+ Returns:
+ Configured WorldNode instance with data from generation
+ """
+ pass
+
+ @abstractmethod
+ def _validate_parameters(self) -> None:
+ """
+ Validate constructor parameters.
+
+ Called during __init__ to ensure parameters are valid before generation.
+ Raises ValueError if parameters are invalid.
+ """
+ pass
diff --git a/controls/sae_2025_ws/src/sim/sim/world_gen/WorldNode.py b/controls/sae_2025_ws/src/sim/sim/world_gen/WorldNode.py
index 53ca8a3e1..b5c95b592 100644
--- a/controls/sae_2025_ws/src/sim/sim/world_gen/WorldNode.py
+++ b/controls/sae_2025_ws/src/sim/sim/world_gen/WorldNode.py
@@ -1,96 +1,32 @@
#!/usr/bin/env python3
"""
-Abstract base class for world generation nodes.
+Abstract base class for world nodes.
-Each competition should have a corresponding WorldNode subclass that:
-1. Generates the world file based on competition-specific parameters
-2. Writes the world file to ~/.simulation-gazebo/worlds/
-3. Provides services/topics for world information (e.g., obstacle positions)
+WorldNode is the ROS node abstraction for world-related services.
+Nodes are created BY WorldGenerator subclasses via create_node().
"""
-from pathlib import Path
-from typing import Optional
-from abc import ABC, abstractmethod
+from abc import ABC
+
from rclpy.node import Node
-from sim.utils import camel_to_snake, find_package_resource, copy_models_to_gazebo
-import logging
-class WorldNode(Node, ABC):
- """
- Abstract base class for world generation nodes.
-
- Subclasses must implement:
- - generate_world(): Generate and write the world file
- - get_world_path(): Return the path to the generated world file
- """
+from sim.utils import camel_to_snake
- def __init__(self, competition_name: str, output_filename: Optional[str] = None):
- """
- Initialize the WorldNode.
-
- Args:
- competition_name: Name of the competition (e.g., 'in_house')
- output_filename: Optional custom output filename. If None, uses {competition_name}.sdf
- """
- super().__init__(self.__class__.__name__)
- self.competition_name = competition_name
-
- # Determine output path
- if output_filename is None:
- output_filename = f"{competition_name}.sdf"
-
- self.output_dir = Path.home() / '.simulation-gazebo' / 'worlds'
- self.output_dir.mkdir(parents=True, exist_ok=True)
- self.output_path = self.output_dir / output_filename
-
- self.get_logger().info(f"Initializing world node for competition: {competition_name}")
-
- self.setup_gazebo_models(self.get_logger())
- self.get_logger().info(f"Wrote world file to: {self.output_path}")
- def setup_gazebo_models(self, logger: logging.Logger) -> None:
- """
- Setup Gazebo models by copying from package to user's Gazebo models directory.
- This ensures models are available to Gazebo at runtime.
- """
- try:
- src_models_dir = find_package_resource(
- relative_path='world_gen/models',
- package_name='sim',
- resource_type='directory',
- logger=logger,
- base_file=Path(__file__)
- )
-
- # Ensure output directory exists
- output_world_dir = Path.home() / '.simulation-gazebo' / 'worlds'
- output_world_dir.mkdir(parents=True, exist_ok=True)
- dst_models_dir = Path.home() / '.simulation-gazebo' / 'models'
+class WorldNode(Node, ABC):
+ """
+ Abstract base class for world nodes.
- copy_models_to_gazebo(src_models_dir, dst_models_dir)
- except Exception as e:
- logger.warning(f"Model setup failed (continuing anyway): {e}")
+ WorldNode subclasses provide ROS services/topics for world information
+ (e.g., obstacle positions, hoop lists). They are created BY WorldGenerator
+ subclasses via create_node(), which passes generation data to the node.
- @abstractmethod
- def generate_world(self) -> None:
- """
- Generate the world file and write it to self.output_path.
-
- This method should:
- 1. Generate world geometry/obstacles based on competition parameters
- 2. Write the SDF world file to self.output_path
- 3. Store any world metadata needed for services/topics
- """
- pass
+ Directory creation and model copying is handled by sim.launch.py.
+ """
- def get_world_path(self) -> Path:
- """
- Get the path to the generated world file.
-
- Returns:
- Path to the world SDF file
- """
- return self.output_path
+ def __init__(self):
+ """Initialize the WorldNode."""
+ super().__init__(self.__class__.__name__)
@classmethod
def node_name(cls) -> str:
diff --git a/controls/sae_2025_ws/src/sim/sim/world_gen/__init__.py b/controls/sae_2025_ws/src/sim/sim/world_gen/__init__.py
index 4cbbd0e00..936a5611e 100644
--- a/controls/sae_2025_ws/src/sim/sim/world_gen/__init__.py
+++ b/controls/sae_2025_ws/src/sim/sim/world_gen/__init__.py
@@ -1,2 +1,4 @@
-from .WorldNode import WorldNode # make sure to import the parent class FIRST (to avoid circular imports)
+from .WorldNode import WorldNode # Import parent classes FIRST (to avoid circular imports)
+from .WorldGenerator import WorldGenerator
+from .InHouse import InHouseGenerator, InHouseNode
from .HoopCourse import HoopCourseNode
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/sim/sim/world_gen/worlds/template.sdf b/controls/sae_2025_ws/src/sim/sim/world_gen/worlds/in_house.sdf
similarity index 100%
rename from controls/sae_2025_ws/src/sim/sim/world_gen/worlds/template.sdf
rename to controls/sae_2025_ws/src/sim/sim/world_gen/worlds/in_house.sdf
diff --git a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
index 230e846d8..4f283d19d 100644
--- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
+++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
@@ -9,4 +9,4 @@ servo_only: false
camera_offsets: [0, 0, 0] #Should denote the position of the camera relative to the payload mechanism, in meters
# In NED frame: x is forward, y is right, and z is down.
-sim: true
+sim: true
\ No newline at end of file
diff --git a/controls/sae_2025_ws/src/uav/launch/main.launch.py b/controls/sae_2025_ws/src/uav/launch/main.launch.py
index be53462e8..83a08da64 100644
--- a/controls/sae_2025_ws/src/uav/launch/main.launch.py
+++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py
@@ -1,11 +1,21 @@
#!/usr/bin/env python3
import os
-import re
import platform
+import re
+
+from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
-from launch.actions import ExecuteProcess, LogInfo, OpaqueFunction, RegisterEventHandler, DeclareLaunchArgument
+from launch.actions import (
+ DeclareLaunchArgument,
+ ExecuteProcess,
+ IncludeLaunchDescription,
+ LogInfo,
+ OpaqueFunction,
+ RegisterEventHandler,
+)
from launch.event_handlers import OnProcessIO, OnProcessStart
from launch.events.process import ProcessIO
+from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from uav.utils import vehicle_map, find_folder_with_heuristic, load_launch_parameters, extract_vision_nodes
@@ -170,8 +180,10 @@ def handler(event: ProcessIO):
px4_path = find_folder_with_heuristic('PX4-Autopilot', os.path.expanduser(LaunchConfiguration('px4_path').perform(context)))
# Prepare sim launch arguments with all simulation parameters
+ # Model copying is now handled by sim.launch.py
sim_launch_args = {
'px4_path': px4_path,
+ 'vehicle_model': topic_model_name,
}
sim = IncludeLaunchDescription(
@@ -195,7 +207,7 @@ def handler(event: ProcessIO):
sim,
RegisterEventHandler(
OnProcessIO(on_stderr=lambda event: (
- [LogInfo(msg="Gazebo process started."), px4_sitl, *vision_node_actions, middleware] if b"Successfully generated world file:" in event.text else None
+ [LogInfo(msg="World node ready, starting PX4 SITL."), px4_sitl, *vision_node_actions, middleware] if b"World node ready" in event.text else None
)
)
),
diff --git a/controls/sae_2025_ws/src/uav/uav/missions/basic_waypoints.yaml b/controls/sae_2025_ws/src/uav/uav/missions/basic_waypoints.yaml
index 5e2945ebc..1a8f40a4a 100644
--- a/controls/sae_2025_ws/src/uav/uav/missions/basic_waypoints.yaml
+++ b/controls/sae_2025_ws/src/uav/uav/missions/basic_waypoints.yaml
@@ -1,7 +1,7 @@
start:
class: uav.autonomous_modes.NavGPSMode
params:
- coordinates: '[((5, 3, -10), 5, "LOCAL"), ((10, 1, -1), 3, "LOCAL")]'
+ coordinates: '[((50, 30, -20), 5, "LOCAL"), ((10, 1, -10), 3, "LOCAL")]'
transitions:
complete: land
diff --git a/controls/sae_2025_ws/src/uav/uav/utils.py b/controls/sae_2025_ws/src/uav/uav/utils.py
index b5788cda1..4a7cbc495 100644
--- a/controls/sae_2025_ws/src/uav/uav/utils.py
+++ b/controls/sae_2025_ws/src/uav/uav/utils.py
@@ -1,8 +1,10 @@
import os
import re
-import yaml
+import shutil
from pathlib import Path
+import yaml
+
R_earth = 6378137.0 # Earth's radius in meters (WGS84)
pink = ((140, 120, 120), (175, 255, 255))
@@ -93,4 +95,86 @@ def extract_vision_nodes(yaml_path):
def load_launch_parameters():
params_file = os.path.join(os.getcwd(), 'src', 'uav', 'launch', 'launch_params.yaml')
with open(params_file, 'r', encoding='utf-8') as f:
- return yaml.safe_load(f)
\ No newline at end of file
+ return yaml.safe_load(f)
+
+def _get_model_dependencies(model_path: Path) -> list[str]:
+ """
+ Parse model.sdf to find dependencies (included models).
+
+ 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)
+ # Also find plain model references like model_name
+ model_uris = re.findall(r'model://([^/<>]+)', content)
+ plain_uris = re.findall(r'(?!model://)([^]+)', content)
+ dependencies.extend(model_uris + plain_uris)
+ except Exception as e:
+ print(f"Warning: Could not parse {model_sdf}: {e}")
+
+ return dependencies
+
+def copy_px4_models(px4_path: str, models: list[str]) -> None:
+ """
+ Copy required PX4 vehicle models from PX4-Autopilot to Gazebo models directory.
+
+ Clears ~/.simulation-gazebo/models/ first to ensure a clean state,
+ then recursively copies the specified models and all their dependencies.
+
+ This is called before sim.launch starts, setting up the base PX4 models that
+ will later be merged with sim models (hoop, dlz, etc.) by WorldNode.
+
+ Args:
+ px4_path: Path to PX4-Autopilot directory
+ models: List of model names to copy (e.g., ['x500_mono_cam'])
+ """
+ px4_models_dir = Path(px4_path) / 'Tools' / 'simulation' / 'gz' / 'models'
+ dst_models_dir = Path.home() / '.simulation-gazebo' / 'models'
+
+ # Clear destination directory first to ensure clean state
+ if dst_models_dir.exists():
+ shutil.rmtree(dst_models_dir)
+
+ # Create destination directory
+ dst_models_dir.mkdir(parents=True, exist_ok=True)
+
+ # Track models to copy and already copied to avoid duplicates
+ models_to_copy = set(models)
+ copied_models = set()
+
+ while models_to_copy:
+ model_name = models_to_copy.pop()
+
+ if model_name in copied_models:
+ continue
+
+ src_model = px4_models_dir / model_name
+ dst_model = dst_models_dir / model_name
+
+ if not src_model.exists():
+ print(f"Warning: PX4 model '{model_name}' not found in {px4_models_dir}")
+ continue
+
+ # Copy the model
+ if src_model.is_dir():
+ shutil.copytree(src_model, dst_model)
+ copied_models.add(model_name)
+ print(f"Copied PX4 model: {model_name}")
+
+ # Find and queue dependencies
+ dependencies = _get_model_dependencies(src_model)
+ for dep in dependencies:
+ if dep not in copied_models:
+ models_to_copy.add(dep)
\ No newline at end of file