From 9014a4481ac3fa2dfe93afb706b5ed96de249b9f Mon Sep 17 00:00:00 2001 From: Kevin Song Date: Fri, 9 Jan 2026 01:51:53 +0000 Subject: [PATCH 1/5] selectively copy models and search for dependencies --- controls/sae_2025_ws/src/sim/sim/utils.py | 43 +++++++--- .../src/sim/sim/world_gen/HoopCourse.py | 11 ++- .../src/sim/sim/world_gen/WorldNode.py | 24 +++++- .../sae_2025_ws/src/uav/launch/main.launch.py | 5 +- controls/sae_2025_ws/src/uav/uav/utils.py | 80 ++++++++++++++++++- 5 files changed, 144 insertions(+), 19 deletions(-) diff --git a/controls/sae_2025_ws/src/sim/sim/utils.py b/controls/sae_2025_ws/src/sim/sim/utils.py index 1cbd4ad26..c768a8231 100644 --- a/controls/sae_2025_ws/src/sim/sim/utils.py +++ b/controls/sae_2025_ws/src/sim/sim/utils.py @@ -197,35 +197,52 @@ def find_package_resource( ) -def copy_models_to_gazebo(src_models_dir: Path, dst_models_dir: Path) -> None: +def copy_models_to_gazebo(src_models_dir: Path, dst_models_dir: Path, models: Optional[list[str]] = None) -> None: """ Copy model files from source to Gazebo models directory. - + + Note: PX4 vehicle models are copied separately by the UAV launch system, + which clears the directory first. + Args: - src_models_dir: Source models directory + src_models_dir: Source models directory (sim package models) dst_models_dir: Destination models directory (Gazebo models path) - + 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 + + # Determine which models to copy + if models is None: + # Copy all models + items_to_copy = list(src_models_dir.iterdir()) + else: + # Copy only specified models + items_to_copy = [] + for model_name in models: + model_path = src_models_dir / model_name + if not model_path.exists(): + raise FileNotFoundError(f"Required model '{model_name}' not found in {src_models_dir}") + items_to_copy.append(model_path) + + # Copy the selected sim models try: - for item in src_models_dir.iterdir(): - src_item = src_models_dir / item.name - dst_item = dst_models_dir / item.name - + for src_item in items_to_copy: + dst_item = dst_models_dir / src_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) 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 index 9a625cb5f..0ceef1fc7 100644 --- a/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py +++ b/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py @@ -554,10 +554,19 @@ def strip_whitespace(elem): except Exception as e: raise RuntimeError(f"Failed to write output world file {out_path}: {e}") + def get_required_models(self) -> list[str]: + """ + Return the list of models required for the hoop course competition. + + Returns: + List of model names: hoop, dlz (base), dlz_red, dlz_green, dlz_blue, payload + """ + return ['hoop', 'dlz', 'dlz_red', 'dlz_green', 'dlz_blue', 'payload'] + def generate_world(self) -> None: """ Generate the world file with hoops and DLZs. - + Implements the abstract method from WorldNode. """ courses = ['ascent', 'descent', 'slalom', 'bezier'] 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..d9a6fe7ff 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 @@ -52,6 +52,7 @@ 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. + Only copies models required for this competition. """ try: src_models_dir = find_package_resource( @@ -61,13 +62,17 @@ def setup_gazebo_models(self, logger: logging.Logger) -> None: 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' - copy_models_to_gazebo(src_models_dir, dst_models_dir) + # Get list of required models for this competition + required_models = self.get_required_models() + logger.info(f"Copying {len(required_models)} required models: {required_models}") + + copy_models_to_gazebo(src_models_dir, dst_models_dir, models=required_models) except Exception as e: logger.warning(f"Model setup failed (continuing anyway): {e}") @@ -75,7 +80,7 @@ def setup_gazebo_models(self, logger: logging.Logger) -> None: 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 @@ -83,6 +88,19 @@ def generate_world(self) -> None: """ pass + @abstractmethod + def get_required_models(self) -> list[str]: + """ + Return a list of model names required for this competition. + + This method should return the names of all models that need to be + copied to the Gazebo models directory for this world to function. + + Returns: + List of model directory names (e.g., ['hoop', 'dlz_red', 'payload']) + """ + pass + def get_world_path(self) -> Path: """ Get the path to the generated world 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 1c9dfe86e..3235de173 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -8,7 +8,7 @@ from launch.events.process import ProcessIO 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 +from uav.utils import vehicle_map, find_folder_with_heuristic, load_launch_parameters, extract_vision_nodes, copy_px4_models from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from ament_index_python.packages import get_package_share_directory @@ -167,6 +167,9 @@ def handler(event: ProcessIO): # Find required paths. px4_path = find_folder_with_heuristic('PX4-Autopilot', os.path.expanduser(LaunchConfiguration('px4_path').perform(context))) + # Copy required PX4 model for the vehicle (with dependencies) + copy_px4_models(px4_path, [topic_model_name]) + # Prepare sim launch arguments with all simulation parameters sim_launch_args = { 'px4_path': px4_path, diff --git a/controls/sae_2025_ws/src/uav/uav/utils.py b/controls/sae_2025_ws/src/uav/uav/utils.py index b5788cda1..8596d7357 100644 --- a/controls/sae_2025_ws/src/uav/uav/utils.py +++ b/controls/sae_2025_ws/src/uav/uav/utils.py @@ -1,6 +1,7 @@ import os import re import yaml +import shutil from pathlib import Path R_earth = 6378137.0 # Earth's radius in meters (WGS84) @@ -93,4 +94,81 @@ 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 + """ + import re + + 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_name and model://model_name patterns + uri_patterns = re.findall(r'(?:model://)?([^]+)', content) + dependencies.extend(uri_patterns) + 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 models from PX4-Autopilot to Gazebo models directory. + Clears the destination directory first and recursively copies model dependencies. + + 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 From 3bffd8ea0c02e35b9fbbb7cb3ae02848a18d5d1e Mon Sep 17 00:00:00 2001 From: Kevin Song Date: Mon, 12 Jan 2026 01:06:08 +0000 Subject: [PATCH 2/5] recursively search/copy the sim models --- .../sae_2025_ws/src/sim/launch/sim.launch.py | 32 ++-- .../src/sim/sim/simulations/in_house.yaml | 5 +- controls/sae_2025_ws/src/sim/sim/utils.py | 145 +++++++++++++--- .../src/sim/sim/world_gen/HoopCourse.py | 158 +++++++++++++----- .../src/sim/sim/world_gen/WorldNode.py | 88 +++++----- .../src/uav/launch/launch_params.yaml | 2 +- .../src/uav/uav/missions/basic_waypoints.yaml | 2 +- controls/sae_2025_ws/src/uav/uav/utils.py | 17 +- 8 files changed, 300 insertions(+), 149 deletions(-) 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..b655b3304 100644 --- a/controls/sae_2025_ws/src/sim/launch/sim.launch.py +++ b/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 ) ) ) 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..5ad9765b3 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 @@ -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 diff --git a/controls/sae_2025_ws/src/sim/sim/utils.py b/controls/sae_2025_ws/src/sim/sim/utils.py index c768a8231..b3e932f8d 100644 --- a/controls/sae_2025_ws/src/sim/sim/utils.py +++ b/controls/sae_2025_ws/src/sim/sim/utils.py @@ -197,16 +197,84 @@ def find_package_resource( ) +def _get_model_dependencies(model_path: Path) -> list[str]: + """ + 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 + 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 model files from source to Gazebo models directory. + 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. - Note: PX4 vehicle models are copied separately by the UAV launch system, - which clears the directory first. + 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 (Gazebo models path) + 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: @@ -225,32 +293,63 @@ def copy_models_to_gazebo(src_models_dir: Path, dst_models_dir: Path, models: Op except OSError as e: raise OSError(f"Failed to create destination directory {dst_models_dir}: {e}") + # Track which models have been copied to avoid duplicates + copied_models = set() + # Determine which models to copy if models is None: - # Copy all models - items_to_copy = list(src_models_dir.iterdir()) + # Copy all models in the directory + models_to_copy = [item.name for item in src_models_dir.iterdir() if item.is_dir()] else: - # Copy only specified models - items_to_copy = [] - for model_name in models: - model_path = src_models_dir / model_name - if not model_path.exists(): - raise FileNotFoundError(f"Required model '{model_name}' not found in {src_models_dir}") - items_to_copy.append(model_path) - - # Copy the selected sim models + models_to_copy = models + + # Copy each model with dependencies try: - for src_item in items_to_copy: - dst_item = dst_models_dir / src_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'] + """ + 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 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() \ 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 index 0ceef1fc7..76e5fa2a1 100644 --- a/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py +++ b/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py @@ -367,27 +367,28 @@ class HoopCourseNode(WorldNode): """ def __init__(self, + competition_name: str, 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): + height: int): """ Initialize the hoop course world generator. - + Args: + competition_name: Competition name (e.g., 'in_house') 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) + height: Height parameter for course generation """ - super().__init__(competition_name="in_house", output_filename=output_filename) + # Store parameters self.course = course self.dlz = dlz self.uav = uav @@ -396,8 +397,13 @@ def __init__(self, self.world_name = world_name self.height = height self.hoop_positions: List[Pose] = [] - self.generate_world() - + + # Validate parameters before proceeding + self._validate_parameters() + + # Parent calls generate_world() and setup_gazebo_models() + super().__init__(competition_name=competition_name) + # Create service for providing hoop positions self.srv = self.create_service(HoopList, "list_hoops", self.hoop_list_req) @@ -414,14 +420,86 @@ def hoop_list_req(self, request, response): response.hoop_positions.append(hp) return response + def _validate_parameters(self): + """Validate constructor parameters.""" + valid_courses = ['ascent', 'descent', 'slalom', 'bezier', 'straight', 'random', 'previous'] + if self.course.lower() not in valid_courses: + raise ValueError( + f"Invalid course type: '{self.course}'. " + f"Must be one of {valid_courses}" + ) + + # Validate numeric parameters + if self.num_hoops < 1: + raise ValueError(f"num_hoops must be >= 1, got {self.num_hoops}") + + if self.num_hoops > 100: + self.get_logger().warning(f"num_hoops={self.num_hoops} is very large, may cause performance issues") + + 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}") + + # Validate coordinates + 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}") + + # Check if UAV and DLZ are the same (for courses that need them different) + if self.course.lower() 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 _extract_hoops_from_world(self, world_path: Path) -> List[Pose]: + """Extract hoop positions from existing world SDF file.""" + tree = ET.parse(str(world_path)) + root = tree.getroot() + + hoops = [] + # Handle namespace if present + ns = "" + if root.tag.startswith("{"): + ns = root.tag.split("}")[0] + "}" + + # Find world element + world = root.find(f"{ns}world") or root.find("world") or root.find(".//world") + if world is None: + return hoops + + # Extract all hoop includes + for include in world.iter('include'): + uri_elem = include.find('uri') + if uri_elem is not None and uri_elem.text == 'model://hoop': + pose_elem = include.find('pose') + if pose_elem is not None: + # Parse "x y z roll pitch yaw" format + values = [float(v) for v in pose_elem.text.split()] + if len(values) == 6: + hoops.append(tuple(values)) + + return hoops def add_hoops(self, input_file, output_file): - # Expand ~, make absolute, and validate paths - in_path = Path(input_file).expanduser().resolve() + # Resolve template path using find_package_resource + if not Path(input_file).is_absolute(): + # Template path is relative to package root, resolve it + from sim.utils import find_package_resource + in_path = find_package_resource( + relative_path=input_file, + package_name='sim', + resource_type='file', + logger=self.get_logger(), + base_file=Path(__file__) + ) + else: + # Already absolute (backward compatibility) + 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()}" @@ -554,21 +632,8 @@ def strip_whitespace(elem): except Exception as e: raise RuntimeError(f"Failed to write output world file {out_path}: {e}") - def get_required_models(self) -> list[str]: - """ - Return the list of models required for the hoop course competition. - - Returns: - List of model names: hoop, dlz (base), dlz_red, dlz_green, dlz_blue, payload - """ - return ['hoop', 'dlz', 'dlz_red', 'dlz_green', 'dlz_blue', 'payload'] - def generate_world(self) -> None: - """ - Generate the world file with hoops and DLZs. - - Implements the abstract method from WorldNode. - """ + """Generate the world file with hoops and DLZs.""" courses = ['ascent', 'descent', 'slalom', 'bezier'] course = None @@ -576,34 +641,39 @@ def generate_world(self) -> None: 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 + if self.output_path.exists(): + self.hoop_positions = self._extract_hoops_from_world(self.output_path) + self.get_logger().info(f"Reusing previous world with {len(self.hoop_positions)} hoops") + return + else: + self.get_logger().warning("Previous world not found, using straight course") + self.course = "straight" elif self.course.lower() == "ascent": - course = AscentCourse(dlz=self.dlz, - uav=self.uav, - num_hoops=self.num_hoops, - max_dist=self.max_dist, + 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, + 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, + 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, + course = StraightCourse(dlz=self.dlz, + uav=self.uav, + num_hoops=self.num_hoops, max_dist=self.max_dist, height=self.height, spacing=2) @@ -616,7 +686,7 @@ def generate_world(self) -> None: 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)) 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 d9a6fe7ff..e95a6dc29 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 @@ -9,7 +9,6 @@ """ from pathlib import Path -from typing import Optional from abc import ABC, abstractmethod from rclpy.node import Node from sim.utils import camel_to_snake, find_package_resource, copy_models_to_gazebo @@ -18,41 +17,45 @@ 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 + + Subclasses implement generate_world() to create their world SDF file. + Parent handles file generation, model detection, and model copying automatically. """ - def __init__(self, competition_name: str, output_filename: Optional[str] = None): + def __init__(self, competition_name: str): """ 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" - + + # Output filename is always {competition_name}.sdf + 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}") - + + # Generate the world file (subclass implements this) + self.generate_world() + + # Auto-detect and copy models from the generated world file self.setup_gazebo_models(self.get_logger()) - self.get_logger().info(f"Wrote world file to: {self.output_path}") + self.get_logger().info(f"World file ready at: {self.output_path}") + + @abstractmethod + def generate_world(self) -> None: + """Generate the world SDF file and write it to self.output_path.""" + pass 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. - Only copies models required for this competition. + Setup Gazebo sim models by automatically detecting and copying them. """ try: src_models_dir = find_package_resource( @@ -68,38 +71,23 @@ def setup_gazebo_models(self, logger: logging.Logger) -> None: output_world_dir.mkdir(parents=True, exist_ok=True) dst_models_dir = Path.home() / '.simulation-gazebo' / 'models' - # Get list of required models for this competition - required_models = self.get_required_models() - logger.info(f"Copying {len(required_models)} required models: {required_models}") - - copy_models_to_gazebo(src_models_dir, dst_models_dir, models=required_models) + # Automatically extract models from the generated world file + if self.output_path.exists(): + from sim.utils import extract_models_from_sdf + required_models = extract_models_from_sdf(self.output_path) + logger.info(f"Auto-detected {len(required_models)} models from world file: {required_models}") + + if required_models: + copy_models_to_gazebo(src_models_dir, dst_models_dir, models=required_models) + else: + logger.warning(f"No models detected in {self.output_path}") + else: + logger.warning(f"World file not found at {self.output_path}, skipping model setup") + except FileNotFoundError as e: + logger.warning(f"World file not found, skipping model setup: {e}") except Exception as e: - logger.warning(f"Model setup failed (continuing anyway): {e}") - - @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 - - @abstractmethod - def get_required_models(self) -> list[str]: - """ - Return a list of model names required for this competition. - - This method should return the names of all models that need to be - copied to the Gazebo models directory for this world to function. - - Returns: - List of model directory names (e.g., ['hoop', 'dlz_red', 'payload']) - """ - pass + logger.error(f"Model setup failed: {e}") + raise RuntimeError(f"Failed to setup Gazebo models: {e}") from e def get_world_path(self) -> Path: """ 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..14c9379df 100644 --- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml +++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml @@ -1,5 +1,5 @@ # launch_params.yaml -mission_name: test_straight_course +mission_name: basic_waypoints uav_debug: false vision_debug: false run_mission: true 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 8596d7357..23249b034 100644 --- a/controls/sae_2025_ws/src/uav/uav/utils.py +++ b/controls/sae_2025_ws/src/uav/uav/utils.py @@ -117,9 +117,11 @@ def _get_model_dependencies(model_path: Path) -> list[str]: try: with open(model_sdf, 'r') as f: content = f.read() - # Find all model_name and model://model_name patterns - uri_patterns = re.findall(r'(?:model://)?([^]+)', content) - dependencies.extend(uri_patterns) + # 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}") @@ -127,8 +129,13 @@ def _get_model_dependencies(model_path: Path) -> list[str]: def copy_px4_models(px4_path: str, models: list[str]) -> None: """ - Copy required PX4 models from PX4-Autopilot to Gazebo models directory. - Clears the destination directory first and recursively copies model dependencies. + 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 From b8dba166858e68b86d4feb9935874bf6d3d3ce54 Mon Sep 17 00:00:00 2001 From: Kevin Song Date: Fri, 16 Jan 2026 20:02:14 +0000 Subject: [PATCH 3/5] move px4 and gazebo model copying to the sim launch file --- .../sae_2025_ws/src/sim/launch/sim.launch.py | 198 +++-- controls/sae_2025_ws/src/sim/setup.py | 4 +- .../{HoopScore.py => HoopScoringNode.py} | 6 +- .../src/sim/sim/simulations/in_house.yaml | 26 +- controls/sae_2025_ws/src/sim/sim/utils.py | 73 +- .../src/sim/sim/world_gen/HoopCourse.py | 705 ------------------ .../src/sim/sim/world_gen/InHouse.py | 517 +++++++++++++ .../src/sim/sim/world_gen/WorldNode.py | 83 +-- .../worlds/{template.sdf => in_house.sdf} | 0 .../src/uav/launch/launch_params.yaml | 4 +- .../sae_2025_ws/src/uav/launch/main.launch.py | 32 +- controls/sae_2025_ws/src/uav/uav/utils.py | 5 +- 12 files changed, 755 insertions(+), 898 deletions(-) rename controls/sae_2025_ws/src/sim/sim/scoring/{HoopScore.py => HoopScoringNode.py} (99%) delete mode 100644 controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py create mode 100644 controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py rename controls/sae_2025_ws/src/sim/sim/world_gen/worlds/{template.sdf => in_house.sdf} (100%) 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 b655b3304..7eaab9cee 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,43 +154,123 @@ 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()) + # 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) + world_generator_class = getattr(world_module, world_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__) + ) + + # Find all .sdf world files + world_files = list(worlds_dir.glob('*.sdf')) + if not world_files: + logger.warning(f"No world files found in {worlds_dir}") + else: + # Extract models from all world files + all_required_models = set() + for world_file in world_files: + models = extract_models_from_sdf(world_file) + all_required_models.update(models) + logger.info(f"Found {len(models)} sim models in {world_file.name}: {models}") + + if all_required_models: + logger.info(f"Copying {len(all_required_models)} sim models: {sorted(all_required_models)}") + dst_models_dir = Path.home() / '.simulation-gazebo' / 'models' + copy_models_to_gazebo(src_models_dir, dst_models_dir, models=list(all_required_models)) + else: + logger.warning("No sim models detected in world files") + 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() - 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"]), - 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: @@ -203,39 +279,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 - # 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 = [ - world, - RegisterEventHandler( - OnProcessIO( - target_action=world, - on_stderr=lambda event: ( - [spawn_world, LogInfo(msg="World generated, starting Gazebo."), scoring] if b"World file ready at:" 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 @@ -244,6 +295,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 5ad9765b3..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,21 +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: "world_gen/worlds/template.sdf" # Gazebo world template (relative to package root) + 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 b3e932f8d..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.""" @@ -219,6 +221,9 @@ def _get_model_dependencies(model_path: Path) -> list[str]: # 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 @@ -265,21 +270,10 @@ def copy_models_to_gazebo(src_models_dir: Path, dst_models_dir: Path, models: Op """ 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}") @@ -329,8 +323,6 @@ def extract_models_from_sdf(sdf_path: Path) -> list[str]: For model://hoop, returns ['hoop'] For model://dlz_red, returns ['dlz_red'] """ - import xml.etree.ElementTree as ET - if not sdf_path.exists(): return [] @@ -350,6 +342,59 @@ def extract_models_from_sdf(sdf_path: Path) -> list[str]: # 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 76e5fa2a1..000000000 --- a/controls/sae_2025_ws/src/sim/sim/world_gen/HoopCourse.py +++ /dev/null @@ -1,705 +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, - competition_name: str, - course: str, - dlz: Tuple[float, float, float], - uav: Tuple[float, float, float], - num_hoops: int, - max_dist: int, - world_name: str, - height: int): - """ - Initialize the hoop course world generator. - - Args: - competition_name: Competition name (e.g., 'in_house') - 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 - height: Height parameter for course generation - """ - # Store parameters - 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] = [] - - # Validate parameters before proceeding - self._validate_parameters() - - # Parent calls generate_world() and setup_gazebo_models() - super().__init__(competition_name=competition_name) - - # 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 _validate_parameters(self): - """Validate constructor parameters.""" - valid_courses = ['ascent', 'descent', 'slalom', 'bezier', 'straight', 'random', 'previous'] - if self.course.lower() not in valid_courses: - raise ValueError( - f"Invalid course type: '{self.course}'. " - f"Must be one of {valid_courses}" - ) - - # Validate numeric parameters - if self.num_hoops < 1: - raise ValueError(f"num_hoops must be >= 1, got {self.num_hoops}") - - if self.num_hoops > 100: - self.get_logger().warning(f"num_hoops={self.num_hoops} is very large, may cause performance issues") - - 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}") - - # Validate coordinates - 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}") - - # Check if UAV and DLZ are the same (for courses that need them different) - if self.course.lower() 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 _extract_hoops_from_world(self, world_path: Path) -> List[Pose]: - """Extract hoop positions from existing world SDF file.""" - tree = ET.parse(str(world_path)) - root = tree.getroot() - - hoops = [] - # Handle namespace if present - ns = "" - if root.tag.startswith("{"): - ns = root.tag.split("}")[0] + "}" - - # Find world element - world = root.find(f"{ns}world") or root.find("world") or root.find(".//world") - if world is None: - return hoops - - # Extract all hoop includes - for include in world.iter('include'): - uri_elem = include.find('uri') - if uri_elem is not None and uri_elem.text == 'model://hoop': - pose_elem = include.find('pose') - if pose_elem is not None: - # Parse "x y z roll pitch yaw" format - values = [float(v) for v in pose_elem.text.split()] - if len(values) == 6: - hoops.append(tuple(values)) - - return hoops - - def add_hoops(self, input_file, output_file): - # Resolve template path using find_package_resource - if not Path(input_file).is_absolute(): - # Template path is relative to package root, resolve it - from sim.utils import find_package_resource - in_path = find_package_resource( - relative_path=input_file, - package_name='sim', - resource_type='file', - logger=self.get_logger(), - base_file=Path(__file__) - ) - else: - # Already absolute (backward compatibility) - in_path = Path(input_file).expanduser().resolve() - - out_path = Path(output_file).expanduser().resolve() - 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.""" - 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": - if self.output_path.exists(): - self.hoop_positions = self._extract_hoops_from_world(self.output_path) - self.get_logger().info(f"Reusing previous world with {len(self.hoop_positions)} hoops") - return - else: - self.get_logger().warning("Previous world not found, using straight course") - self.course = "straight" - - 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..3acd9896c --- /dev/null +++ b/controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py @@ -0,0 +1,517 @@ +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 rclpy.node import Node +from sim_interfaces.msg import HoopPose +from sim_interfaces.srv import HoopList +from sim.utils import find_package_resource + +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 InHouse: + """ + 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. + """ + + 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 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(Node): + """ + ROS node for in_house competition. + + Receives hoop positions from InHouse (via launch file) and provides + the list_hoops service for querying hoop positions. + """ + + def __init__(self, hoop_positions: List[Pose]): + super().__init__("InHouseNode") + + 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/WorldNode.py b/controls/sae_2025_ws/src/sim/sim/world_gen/WorldNode.py index e95a6dc29..e978857f7 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 @@ -8,95 +8,38 @@ 3. Provides services/topics for world information (e.g., obstacle positions) """ -from pathlib import Path from abc import ABC, abstractmethod +from pathlib import Path + from rclpy.node import Node -from sim.utils import camel_to_snake, find_package_resource, copy_models_to_gazebo -import logging + +from sim.utils import camel_to_snake class WorldNode(Node, ABC): """ Abstract base class for world generation nodes. Subclasses implement generate_world() to create their world SDF file. - Parent handles file generation, model detection, and model copying automatically. + Parent only handles path setup and calls generate_world(). + Directory creation and all model copying is handled by sim.launch.py. """ - def __init__(self, competition_name: str): + def __init__(self): """ Initialize the WorldNode. - - Args: - competition_name: Name of the competition (e.g., 'in_house') + Subclass must call generate_world(output_dir) after initialization. """ super().__init__(self.__class__.__name__) - self.competition_name = competition_name - - # Output filename is always {competition_name}.sdf - 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}") - - # Generate the world file (subclass implements this) - self.generate_world() - - # Auto-detect and copy models from the generated world file - self.setup_gazebo_models(self.get_logger()) - self.get_logger().info(f"World file ready at: {self.output_path}") @abstractmethod - def generate_world(self) -> None: - """Generate the world SDF file and write it to self.output_path.""" - pass - - def setup_gazebo_models(self, logger: logging.Logger) -> None: + def generate_world(self, output_dir: Path) -> None: """ - Setup Gazebo sim models by automatically detecting and copying them. - """ - 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' + Generate the world SDF file and write it to output_dir. - # Automatically extract models from the generated world file - if self.output_path.exists(): - from sim.utils import extract_models_from_sdf - required_models = extract_models_from_sdf(self.output_path) - logger.info(f"Auto-detected {len(required_models)} models from world file: {required_models}") - - if required_models: - copy_models_to_gazebo(src_models_dir, dst_models_dir, models=required_models) - else: - logger.warning(f"No models detected in {self.output_path}") - else: - logger.warning(f"World file not found at {self.output_path}, skipping model setup") - except FileNotFoundError as e: - logger.warning(f"World file not found, skipping model setup: {e}") - except Exception as e: - logger.error(f"Model setup failed: {e}") - raise RuntimeError(f"Failed to setup Gazebo models: {e}") from e - - def get_world_path(self) -> Path: - """ - Get the path to the generated world file. - - Returns: - Path to the world SDF file + Args: + output_dir: Directory where world file will be written (created by sim.launch.py) """ - return self.output_path + pass @classmethod def node_name(cls) -> str: 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 14c9379df..4f283d19d 100644 --- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml +++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml @@ -1,5 +1,5 @@ # launch_params.yaml -mission_name: basic_waypoints +mission_name: test_straight_course uav_debug: false vision_debug: false run_mission: true @@ -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 3235de173..f855d1db3 100644 --- a/controls/sae_2025_ws/src/uav/launch/main.launch.py +++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py @@ -1,17 +1,30 @@ #!/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, copy_px4_models -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory + +from uav.utils import ( + extract_vision_nodes, + find_folder_with_heuristic, + load_launch_parameters, + vehicle_map, +) def launch_setup(context, *args, **kwargs): # Load launch parameters from the YAML file. @@ -167,12 +180,11 @@ def handler(event: ProcessIO): # Find required paths. px4_path = find_folder_with_heuristic('PX4-Autopilot', os.path.expanduser(LaunchConfiguration('px4_path').perform(context))) - # Copy required PX4 model for the vehicle (with dependencies) - copy_px4_models(px4_path, [topic_model_name]) - # 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( @@ -196,7 +208,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/utils.py b/controls/sae_2025_ws/src/uav/uav/utils.py index 23249b034..4a7cbc495 100644 --- a/controls/sae_2025_ws/src/uav/uav/utils.py +++ b/controls/sae_2025_ws/src/uav/uav/utils.py @@ -1,9 +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)) @@ -106,8 +107,6 @@ def _get_model_dependencies(model_path: Path) -> list[str]: Returns: List of model names that this model depends on """ - import re - dependencies = [] model_sdf = model_path / 'model.sdf' From 8385ee9b8397732217274994cc963e2bc295f5db Mon Sep 17 00:00:00 2001 From: Kevin Song Date: Fri, 16 Jan 2026 20:14:39 +0000 Subject: [PATCH 4/5] only copy from the created world file --- .../sae_2025_ws/src/sim/launch/sim.launch.py | 26 ++++++++----------- .../src/sim/sim/world_gen/WorldNode.py | 1 - 2 files changed, 11 insertions(+), 16 deletions(-) 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 7eaab9cee..8f16abe9c 100644 --- a/controls/sae_2025_ws/src/sim/launch/sim.launch.py +++ b/controls/sae_2025_ws/src/sim/launch/sim.launch.py @@ -220,24 +220,20 @@ def launch_setup(context, *args, **kwargs): base_file=Path(__file__) ) - # Find all .sdf world files - world_files = list(worlds_dir.glob('*.sdf')) - if not world_files: - logger.warning(f"No world files found in {worlds_dir}") + # 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: - # Extract models from all world files - all_required_models = set() - for world_file in world_files: - models = extract_models_from_sdf(world_file) - all_required_models.update(models) - logger.info(f"Found {len(models)} sim models in {world_file.name}: {models}") - - if all_required_models: - logger.info(f"Copying {len(all_required_models)} sim models: {sorted(all_required_models)}") + 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(all_required_models)) + copy_models_to_gazebo(src_models_dir, dst_models_dir, models=list(models)) else: - logger.warning("No sim models detected in world files") + logger.warning("No sim models detected in world file") except Exception as e: logger.error(f"Failed to copy sim models: {e}") raise 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 e978857f7..247f24632 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 @@ -27,7 +27,6 @@ class WorldNode(Node, ABC): def __init__(self): """ Initialize the WorldNode. - Subclass must call generate_world(output_dir) after initialization. """ super().__init__(self.__class__.__name__) From 40b839f70b2e0484c53cf68604b1f2d5c6fc9950 Mon Sep 17 00:00:00 2001 From: Kevin Song Date: Thu, 22 Jan 2026 00:44:17 +0000 Subject: [PATCH 5/5] abstract classes for generator and node --- .../sae_2025_ws/src/sim/launch/sim.launch.py | 3 +- .../src/sim/sim/world_gen/InHouse.py | 19 ++++--- .../src/sim/sim/world_gen/WorldGenerator.py | 51 +++++++++++++++++++ .../src/sim/sim/world_gen/WorldNode.py | 36 +++++-------- .../src/sim/sim/world_gen/__init__.py | 4 +- 5 files changed, 80 insertions(+), 33 deletions(-) create mode 100644 controls/sae_2025_ws/src/sim/sim/world_gen/WorldGenerator.py 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 8f16abe9c..79f33cc88 100644 --- a/controls/sae_2025_ws/src/sim/launch/sim.launch.py +++ b/controls/sae_2025_ws/src/sim/launch/sim.launch.py @@ -194,7 +194,8 @@ def launch_setup(context, *args, **kwargs): try: module_name = f"sim.world_gen.{world_class_name}" world_module = importlib.import_module(module_name) - world_generator_class = getattr(world_module, world_class_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) 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 index 3acd9896c..db650f6b6 100644 --- a/controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py +++ b/controls/sae_2025_ws/src/sim/sim/world_gen/InHouse.py @@ -10,10 +10,11 @@ from xml.dom import minidom import rclpy -from rclpy.node import Node 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] @@ -288,13 +289,13 @@ def bezier_xy_deriv(t: float) -> Tuple[float, float]: return hoops -class InHouse: +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. + be passed to InHouseNode via create_node(). """ COURSES = ['ascent', 'descent', 'slalom', 'bezier', 'straight'] @@ -318,6 +319,10 @@ def __init__(self, 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 @@ -466,16 +471,16 @@ def strip_whitespace(elem): self._logger.info(f"Generated world file: {output_path}") -class InHouseNode(Node): +class InHouseNode(WorldNode): """ ROS node for in_house competition. - Receives hoop positions from InHouse (via launch file) and provides - the list_hoops service for querying hoop positions. + 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__("InHouseNode") + super().__init__() self.hoop_positions = hoop_positions 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 247f24632..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,45 +1,33 @@ #!/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 abc import ABC, abstractmethod -from pathlib import Path +from abc import ABC from rclpy.node import Node from sim.utils import camel_to_snake + class WorldNode(Node, ABC): """ - Abstract base class for world generation nodes. + Abstract base class for world nodes. + + 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. - Subclasses implement generate_world() to create their world SDF file. - Parent only handles path setup and calls generate_world(). - Directory creation and all model copying is handled by sim.launch.py. + Directory creation and model copying is handled by sim.launch.py. """ def __init__(self): - """ - Initialize the WorldNode. - """ + """Initialize the WorldNode.""" super().__init__(self.__class__.__name__) - @abstractmethod - def generate_world(self, output_dir: Path) -> None: - """ - Generate the world SDF file and write it to output_dir. - - Args: - output_dir: Directory where world file will be written (created by sim.launch.py) - """ - pass - @classmethod def node_name(cls) -> str: """Get the node name in snake_case.""" 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