[UPDATE] converter to ROS2 launch description

Author: janstrohbeck

.gitignore

@@ -1,3 +1,4 @@
+/run_hooks
 docs/
 src/*
 cfg/cpp/

library/aduulm_launch_lib_py/launch_config.py

@@ -36,28 +36,33 @@ def group(self, group_name: str):
         if group_name in data.modules:
             group = data.modules[group_name]
             if not isinstance(group, LaunchGroup):
-                raise Exception(f'trying to enter group {group_name}, but it already exists and is not a group! Type: {type(group)}')
+                raise Exception(
+                    f'trying to enter group {group_name}, but it already exists and is not a group! Type: {type(group)}')
         else:
             group = LaunchGroup()
             data.modules.add(group_name, group)
         return LaunchConfig(self.config, self.path + [group_name], group)
 
     def add(self, name: str, child: LeafLaunch):
         if name in self.data.modules:
-            raise Exception(f'trying to add {name} to current group, but something already exists there! {self.data.modules[name]}')
+            raise Exception(
+                f'trying to add {name} to current group, but something already exists there! {self.data.modules[name]}')
         self.data.modules.add(name, child)
 
-    def add_sublaunch_ros(self, name: str, package_name: str, launch_filename: str, **kwargs: Any):
-        self.add(name, SubLaunchROS_(package_name, launch_filename, args=SaferDict(**kwargs)))
+    def add_sublaunch_ros(self, name: str, package_name: str, launch_filename: str, args: Dict[str, Any]):
+        self.add(name, SubLaunchROS_(package_name,
+                 launch_filename, args=SaferDict(**args)))
 
     def exec_sublaunch(self, func: ConfigGeneratorFunc, **args: Any):
         func(self, **args)
 
     def exec_sublaunch_lazy(self, name: str, func: ConfigGeneratorFunc, **args: Any):
         self.add(name, SubLaunchExecLazy_(func, args=SaferDict(**args)))
 
-    def add_node(self, name: str, package_name: str, executable_name: str, remappings: Dict[str, Topic] = {}, **parameters: Any):
-        self.add(name, RunNode_(package_name, executable_name, remappings=SaferDict(**remappings), parameters=SaferDict(**parameters)))
+    def add_node(self, name: str, package_name: str, executable_name: str, remappings: Dict[str, Topic] = {},
+                 parameters: Dict[str, Any] = {}, output: str = 'screen', emulate_tty: bool = True):
+        self.add(name, RunNode_(package_name, executable_name, remappings=SaferDict(**remappings),
+                                parameters=SaferDict(**parameters), output=output, emulate_tty=emulate_tty))
 
     def evaluate(self):
         def do_evaluate(config: LaunchConfig, mod: LeafLaunch, name: str, parent: LaunchGroup):
@@ -66,8 +71,9 @@ def do_evaluate(config: LaunchConfig, mod: LeafLaunch, name: str, parent: Launch
                 mod.func(config, **mod.args)
         self.recurse(do_evaluate)
 
-    def add_executable(self, name: str, executable_name: str, **args: Any):
-        self.add(name, Executable_(executable_name, args=SaferDict(**args)))
+    def add_executable(self, name: str, executable_name: str, args: List[str] = [], output: str = 'screen', emulate_tty: bool = True):
+        self.add(name, Executable_(executable_name, args=args,
+                 output=output, emulate_tty=emulate_tty))
 
     def enable_all(self):
         self.set_enabled(True)
@@ -77,7 +83,8 @@ def disable_all(self):
 
     def recurse(self, cb_leaf: RecurseLeafFunc, cb_enter: Optional[RecurseEnterFunc] = None, cb_exit:
                 Optional[RecurseExitFunc] = None):
-        self._do_recurse(self, self.data, None, None, cb_leaf, cb_enter, cb_exit)
+        self._do_recurse(self, self.data, None, None,
+                         cb_leaf, cb_enter, cb_exit)
 
     def _do_recurse(self, config: LaunchConfig, mod: AnyLaunch, name: Optional[str], parent: Optional[LaunchGroup],
                     cb_leaf: RecurseLeafFunc, cb_enter: Optional[RecurseEnterFunc] = None, cb_exit: Optional[RecurseExitFunc] = None):
@@ -89,11 +96,13 @@ def _do_recurse(self, config: LaunchConfig, mod: AnyLaunch, name: Optional[str],
                     cb_enter(config, mod, name, parent)
             old_keys = set(mod.modules.keys())
             for key in old_keys:
-                self._do_recurse(config, mod.modules[key], key, mod, cb_leaf, cb_enter)
+                self._do_recurse(
+                    config, mod.modules[key], key, mod, cb_leaf, cb_enter)
             # in case new modules were added
             new_keys = set(mod.modules.keys())
             for key in new_keys-old_keys:
-                self._do_recurse(config, mod.modules[key], key, mod, cb_leaf, cb_enter)
+                self._do_recurse(
+                    config, mod.modules[key], key, mod, cb_leaf, cb_enter)
             if name is not None and cb_exit is not None:
                 cb_exit(old_config, mod, name, parent)
         else:
@@ -128,7 +137,9 @@ def __str__(self):
 
 
 RecurseLeafFunc = Callable[[LaunchConfig, LeafLaunch, str, LaunchGroup], None]
-RecurseEnterFunc = Callable[[LaunchConfig, LaunchGroup, str, Optional[LaunchGroup]], None]
-RecurseExitFunc = Callable[[LaunchConfig, LaunchGroup, str, Optional[LaunchGroup]], None]
+RecurseEnterFunc = Callable[[LaunchConfig,
+                             LaunchGroup, str, Optional[LaunchGroup]], None]
+RecurseExitFunc = Callable[[LaunchConfig,
+                            LaunchGroup, str, Optional[LaunchGroup]], None]
 P = ParamSpec('P')
 ConfigGeneratorFunc = Callable[Concatenate[LaunchConfig, P], None]

library/aduulm_launch_lib_py/types.py

@@ -5,6 +5,7 @@
 
 Topic = str
 
+
 @dataclass(kw_only=True, slots=True)
 class Enableable:
     enabled: bool = False
@@ -15,89 +16,123 @@ def enable(self):
     def disable(self):
         self.enabled = False
 
+
 K = TypeVar('K')
 V = TypeVar('V')
+
+
 class SaferDict(Generic[K, V]):
     def __init__(self, **kwargs: V):
         self._data: Dict[K, V] = dict(**kwargs)
 
     # only allow __setitem__ if key already exists
     def __setitem__(self, key: K, value: V):
         if key not in self._data:
-            raise Exception(f"Could not replace argument {key} because it does not exist!")
+            raise Exception(
+                f"Could not replace argument {key} because it does not exist!")
         self._data[key] = value
 
     # only allow add if key does not already exist
     def add(self, key: K, value: V):
         if key in self._data:
-            raise Exception(f"Could not add argument {key} because it already exists!")
+            raise Exception(
+                f"Could not add argument {key} because it already exists!")
         self._data[key] = value
         return value
 
     # The following methods are proxies for dict methods.
     # Maybe these could be replaced by inheriting from dict, but the typing seems to require Python >= 3.12
+    def todict(self):
+        return self._data
+
     def __getitem__(self, key: K) -> V:
         return self._data[key]
+
     def __iter__(self):
         return self._data.__iter__()
+
     def keys(self):
         return self._data.keys()
+
     def values(self):
         return self._data.values()
+
     def items(self):
         return self._data.items()
+
     def __repr__(self):
         return repr(self._data)
+
     def __str__(self):
         return str(self._data)
+
     def __delitem__(self, key: K):
         del self._data[key]
+
     def __eq__(self, other: Any):
         if isinstance(other, SaferDict):
             return self._data == other._data
         return self._data == other
 
+
 @dataclass(slots=True)
 class Executable_(Enableable):
     executable_name: str
-    args: SaferDict[str, Any] = field(default_factory=SaferDict)
+    args: List[str] = field(default_factory=list)
+    output: str = 'screen'
+    emulate_tty: bool = True
+
 
 @dataclass(slots=True)
 class RunNode_(Enableable):
     package_name: str
     executable_name: str
     parameters: SaferDict[str, Any] = field(default_factory=SaferDict)
     remappings: SaferDict[str, Topic] = field(default_factory=SaferDict)
+    output: str = 'screen'
+    emulate_tty: bool = True
+
 
 @dataclass(slots=True)
 class SubLaunchROS_(Enableable):
     package_name: str
     launch_filename: str
     args: SaferDict[str, Any] = field(default_factory=SaferDict)
 
+
 ConfigGeneratorFuncAny = Callable[..., None]
+
+
 @dataclass(slots=True)
 class SubLaunchExecLazy_(Enableable):
     func: ConfigGeneratorFuncAny
     args: SaferDict[str, Any] = field(default_factory=SaferDict)
 
+
 SubLaunch_ = SubLaunchROS_ | SubLaunchExecLazy_
 
+
 @dataclass
 class LaunchGroup:
-    modules: SaferDict[str, 'Executable_ | RunNode_ | SubLaunch_ | LaunchGroup'] = field(default_factory=SaferDict)
+    modules: SaferDict[str, 'Executable_ | RunNode_ | SubLaunch_ | LaunchGroup'] = field(
+        default_factory=SaferDict)
+
+
+def Executable(executable_name: str, args: List[Any] = [], output: str = 'screen', emulate_tty: bool = True):
+    return Executable_(executable_name, args=args, output=output, emulate_tty=emulate_tty)
 
-def Executable(executable_name: str, **kwargs):
-    return Executable_(executable_name, args=SaferDict(**kwargs))
 
-def RunNode(package_name: str, executable_name: str, remappings: Dict[str, Topic]  ={}, **parameters: Any):
-    return RunNode_(package_name, executable_name, remappings=SaferDict(**remappings), parameters=SaferDict(**parameters))
+def RunNode(package_name: str, executable_name: str, remappings: Dict[str, Topic] = {}, parameters: Dict[str, Any] = {}, output: str = 'screen', emulate_tty: bool = True):
+    return RunNode_(package_name, executable_name, remappings=SaferDict(**remappings), parameters=SaferDict(**parameters), output=output, emulate_tty=emulate_tty)
+
 
 def SubLaunchROS(package_name: str, launch_filename: str, **args: Any):
     return SubLaunchROS_(package_name, launch_filename, args=SaferDict(**args))
 
+
 def SubLaunchExecLazy(func: ConfigGeneratorFuncAny, **args: Any):
     return SubLaunchExecLazy_(func, args=SaferDict(**args))
 
+
 LeafLaunch = Executable_ | RunNode_ | SubLaunch_
 AnyLaunch = LeafLaunch | LaunchGroup

library/test/python/test_launch_config.py

@@ -7,13 +7,15 @@
 class LaunchConfigTest(unittest.TestCase):
     def test_simple(self):
         config = LaunchConfig()
-        node_args: dict[str, Any] = dict(package_name='test_package', executable_name='test_executable')
+        node_args: dict[str, Any] = dict(
+            package_name='test_package', executable_name='test_executable')
         params: dict[str, Any] = dict(arg1='value1')
         with config.group('test'):
-            config.add_node(name='test_node', **node_args, **params)
+            config.add_node(name='test_node', **node_args, parameters=params)
         ref = LaunchGroup(modules=SaferDict(
             test=LaunchGroup(modules=SaferDict(
-                test_node=RunNode_(**node_args, parameters=SaferDict(**params), remappings=SaferDict())
+                test_node=RunNode_(
+                    **node_args, parameters=SaferDict(**params), remappings=SaferDict())
             ))
         ))
         self.assertEqual(config.data, ref)

ros2/aduulm_launch_py/converter_ros2.py

@@ -1,43 +1,83 @@
-from aduulm_launch_lib_py import LaunchConfig, LaunchGroup, AnyLaunch, SubLaunchROS_, Executable_, RunNode_, SubLaunchExecLazy_
+from aduulm_launch_lib_py import LaunchConfig, LaunchGroup, AnyLaunch, SubLaunchROS_, Executable_, RunNode_
 from typing import Any, List, Optional
 
 from launch import LaunchDescription
 from launch.actions import IncludeLaunchDescription, GroupAction, DeclareLaunchArgument, ExecuteProcess
-from launch_ros.actions import PushRosNamespace
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import PushRosNamespace, Node
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from aduulm_tools_python.launch_utils import get_package_share_directory
+import os
+
 
 def convert_config_to_ros2_launch(config: LaunchConfig):
     config.evaluate()
-
     modules = []
 
     def recurse(config: LaunchConfig, mod: AnyLaunch, name: Optional[str], modules: List[Any]):
-        print(mod, name, modules)
         if isinstance(mod, LaunchGroup):
             if name is not None:
                 config = config.group(name)
             group_modules = []
-            for key, mod in mod.modules.items():
-                recurse(config, mod, key, group_modules)
+            for key, child in mod.modules.items():
+                recurse(config, child, key, group_modules)
             if name is not None:
+                arg_name = 'launch_' + '__'.join(config.path)
                 modules.extend([
-                    GroupAction(actions=[
-                        PushRosNamespace(name),
-                        *modules
-                    ])
+                    DeclareLaunchArgument(arg_name, default_value='true'),
+                    GroupAction(
+                        actions=[
+                            PushRosNamespace(name),
+                            *group_modules
+                        ],
+                        # scoped=False,
+                        # forwarding=False,
+                        condition=IfCondition(LaunchConfiguration(arg_name))
+                    )
                 ])
             else:
                 modules.extend(group_modules)
             return
         assert isinstance(name, str)
+        arg_name = 'launch_' + '__'.join(config.path + [name])
+        arg = DeclareLaunchArgument(arg_name, default_value=str(mod.enabled)),
+        modules.append(arg)
         if isinstance(mod, SubLaunchROS_):
-            pass
+            desc = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(
+                        get_package_share_directory(mod.package_name),
+                        'launch',
+                        mod.launch_filename
+                    )
+                ),
+                launch_arguments=mod.args.items(),
+                condition=IfCondition(LaunchConfiguration(arg_name))
+            )
+            modules.append(desc)
         elif isinstance(mod, Executable_):
-            pass
+            desc = ExecuteProcess(
+                name=name,
+                cmd=[mod.executable_name, *mod.args],
+                output='screen',
+                emulate_tty=True,
+                condition=IfCondition(LaunchConfiguration(arg_name))
+            )
+            modules.append(desc)
         elif isinstance(mod, RunNode_):
-            pass
-        elif isinstance(mod, SubLaunchExecLazy_):
-            pass
+            desc = Node(
+                name=name,
+                package=mod.package_name,
+                executable=mod.executable_name,
+                output='screen',
+                emulate_tty=True,
+                parameters=[mod.parameters.todict()],
+                condition=IfCondition(LaunchConfiguration(arg_name))
+            )
+            modules.append(desc)
         else:
+            # SubLaunchExecLazy_ must not occur here, because we called evaluate() beforehand!
             assert False
 
     modules = []