diff --git a/ianvs/app/transform_file_broadcaster.cpp b/ianvs/app/transform_file_broadcaster.cpp index c3b31a8..4a470e9 100644 --- a/ianvs/app/transform_file_broadcaster.cpp +++ b/ianvs/app/transform_file_broadcaster.cpp @@ -28,6 +28,7 @@ class TransformFileBroadcaster : public rclcpp::Node { const auto stamp = now(); std::vector transforms; + RCLCPP_INFO_STREAM(get_logger(), "Loading transforms from " << transform_file); try { const auto node = YAML::LoadFile(transform_file); for (const auto& tf : node["frames"]) { diff --git a/ianvs/ianvs/launch.py b/ianvs/ianvs/launch.py index dd7394d..ccd3766 100644 --- a/ianvs/ianvs/launch.py +++ b/ianvs/ianvs/launch.py @@ -1,10 +1,14 @@ import pathlib +import tempfile + from typing import Optional, Sequence, Text from launch.launch_context import LaunchContext -from launch.frontend import expose_action, Entity, Parser +from launch.frontend import expose_action, expose_substitution, Entity, Parser +from launch.frontend.parse_substitution import parse_substitution from launch_ros.actions import Node from launch.some_substitutions_type import SomeSubstitutionsType from launch.substitution import Substitution +from launch.substitutions import SubstitutionFailure from launch.utilities import perform_substitutions, normalize_to_list_of_substitutions @@ -40,7 +44,7 @@ def perform(self, context: LaunchContext) -> Text: if pyenv: pyinterp = pathlib.Path(pyenv) / "bin" / "python" if not pyinterp.exists(): - raise ValueError(f"Interperter '{pyinterp}' not found for venv '{pyenv}'") + raise ValueError(f"Interperter '{pyinterp}' not found for '{pyenv}'") else: pyinterp = "" @@ -65,3 +69,45 @@ def parse(cls, entity: Entity, parser: Parser): env = parser.parse_substitution(entity.get_attr("pyenv", optional=False)) kwargs["pyenv"] = env return cls, kwargs + + +@expose_substitution("subs-file") +class RenderFileSubstitution(Substitution): + """Custom substitution for performing substitutions on file contents.""" + + def __init__(self, path: SomeSubstitutionsType): + super().__init__() + self.path = normalize_to_list_of_substitutions(path) + self._new_file = None + + @classmethod + def parse(cls, data: Sequence[SomeSubstitutionsType]): + if not data or len(data) != 1: + raise AttributeError("Path required for file rendering substitution") + + return cls, {"path": data[0]} + + def describe(self) -> Text: + path_repr = " + ".join([s.describe() for s in self.path]) + return f"RenderFile(path={path_repr})" + + def perform(self, context: LaunchContext) -> Text: + path = perform_substitutions(context, self.path) + path = pathlib.Path(path).expanduser().absolute() + if not path.exists(): + raise SubstitutionFailure(f"File '{path}' does not exist!") + + with path.open("r") as fin: + contents = fin.read() + + contents = parse_substitution(contents) + contents = perform_substitutions(context, contents) + with tempfile.NamedTemporaryFile(mode="w", delete=False) as fout: + self._new_file = pathlib.Path(fout.name) + fout.write(contents) + + return str(self._new_file) + + def __del__(self): + if self._new_file: + self._new_file.unlink()