diff --git a/src/rod/sdf/model.py b/src/rod/sdf/model.py index efc8d53..3312f28 100644 --- a/src/rod/sdf/model.py +++ b/src/rod/sdf/model.py @@ -120,6 +120,16 @@ def joints(self) -> List[Joint]: assert isinstance(self.joint, list), type(self.joint) return self.joint + def resolve_uris(self) -> None: + from rod.utils import resolve_uris + + for link in self.links(): + for visual in link.visuals(): + resolve_uris.resolve_geometry_uris(geometry=visual.geometry) + + for collision in link.collisions(): + resolve_uris.resolve_geometry_uris(geometry=collision.geometry) + def resolve_frames( self, is_top_level: bool = True, explicit_frames: bool = True ) -> None: diff --git a/src/rod/utils/resolve_uris.py b/src/rod/utils/resolve_uris.py new file mode 100644 index 0000000..c0f4566 --- /dev/null +++ b/src/rod/utils/resolve_uris.py @@ -0,0 +1,48 @@ +import os +import pathlib +from typing import List + +from rod import Geometry, logging + + +def resolve_local_uri(uri: str) -> pathlib.Path: + # Remove the prefix of the URI + uri_no_prefix = uri.split(sep="//")[-1] + + paths = [] + paths += paths_from_environment_variable("GZ_SIM_RESOURCE_PATH") + paths += paths_from_environment_variable("IGN_GAZEBO_RESOURCE_PATH") + paths += paths_from_environment_variable("GAZEBO_MODEL_PATH") + + # Remove possible duplicates + paths = list(set(paths)) + + for path in paths: + tentative = pathlib.Path(path) / uri_no_prefix + + if tentative.is_file(): + logging.debug(f"Resolved URI: '{tentative}'") + return tentative + + raise RuntimeError(f"Failed to resolve URI: {uri}") + + +def resolve_geometry_uris(geometry: Geometry) -> None: + # Resolve only mesh geometries + if geometry.mesh is None: + return + + geometry.mesh.uri = str(resolve_local_uri(uri=geometry.mesh.uri)) + + +def paths_from_environment_variable(variable_name: str) -> List[str]: + if variable_name not in os.environ: + return [] + + # Collect all paths removing the empty ones (if '::' is part of the variable) + paths = [p for p in os.environ[variable_name].split(":") if p != ""] + + # Remove duplicates that might occur + paths = list(set(paths)) + + return paths