Added composable nodes in launch file

Signed-off-by: Alberto Tudela <[email protected]>

scitos2_mira/CHANGELOG.rst

@@ -15,6 +15,7 @@ Changelog for package scitos2_mira
 * Rename default.launch.py to mira.launch.py and added lifecycle manager.
 * Added drive as a default module.
 * Added unit testing.
+* Added composable nodes in launch file.
 
 1.1.3 (10-10-2023)
 ------------------

scitos2_mira/launch/mira.launch.py

@@ -19,11 +19,11 @@
 
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
 from nav2_common.launch import RewrittenYaml
 
 
@@ -37,58 +37,150 @@ def generate_launch_description():
     default_scitos_config_file = os.path.join('/opt/SCITOS/', 'SCITOSDriver.xml')
 
     # Create the launch configuration variables.
-    params_file = LaunchConfiguration('params_file', default=default_params_file)
-
-    scitos_config_file = LaunchConfiguration(
-        'scitos_config_file', default=default_scitos_config_file)
+    namespace = LaunchConfiguration('namespace')
+    autostart = LaunchConfiguration('autostart')
+    params_file = LaunchConfiguration('params_file')
+    scitos_config_file = LaunchConfiguration('scitos_config_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    lifecycle_nodes = ['mira']
 
     # Create our own temporary YAML files that include substitutions
     param_substitutions = {
+        'autostart': autostart,
         'scitos_config': scitos_config_file
     }
 
     configured_params = RewrittenYaml(
         source_file=params_file,
-        root_key='',
+        root_key=namespace,
         param_rewrites=param_substitutions,
         convert_types=True
     )
 
+    stdout_linebuf_envvar = SetEnvironmentVariable(
+        'RCUTILS_LOGGING_BUFFERED_STREAM', '1'
+    )
+
     # Map these variables to arguments: can be set from the command line or a default will be used
-    params_file_launch_arg = DeclareLaunchArgument(
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace',
+        default_value='',
+        description='Top-level namespace'
+    )
+
+    declare_params_file_cmd = DeclareLaunchArgument(
         'params_file',
         default_value=default_params_file,
         description='Full path to the Mira parameter file to use'
     )
 
-    scitos_config_file_launch_arg = DeclareLaunchArgument(
+    declare_scitos_config_file_cmd = DeclareLaunchArgument(
         'scitos_config_file',
         default_value=default_scitos_config_file,
         description='Full path to the Scitos parameter file to use'
     )
 
-    # Prepare the Scitos mira
-    scitos_mira = Node(
-        package='scitos2_mira',
-        namespace='',
-        executable='mira_framework',
-        name='mira',
-        output='screen',
-        parameters=[configured_params],
-        emulate_tty=True
+    declare_autostart_cmd = DeclareLaunchArgument(
+        'autostart',
+        default_value='true',
+        description='Automatically startup the nav2 stack',
+    )
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition',
+        default_value='False',
+        description='Use composed bringup if True',
+    )
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name',
+        default_value='nav2_container',
+        description='the name of conatiner that nodes will load in if use composition',
+    )
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn',
+        default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.',
+    )
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level',
+        default_value='info',
+        description='log level'
+    )
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='scitos2_mira',
+                executable='mira_framework',
+                name='mira',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[configured_params],
+                arguments=['--ros-args', '--log-level', log_level]
+            ),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_scitos',
+                output='screen',
+                arguments=['--ros-args', '--log-level', log_level],
+                parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+            ),
+        ],
     )
 
-    lifecycle_manager = Node(
-        package='nav2_lifecycle_manager',
-        executable='lifecycle_manager',
-        name='lifecycle_manager_scitos',
-        output='screen',
-        parameters=[{'autostart': True}, {'node_names': ['mira']}],
+    load_composable_nodes = GroupAction(
+        condition=IfCondition(use_composition),
+        actions=[
+            LoadComposableNodes(
+                target_container=container_name_full,
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='scitos2_mira',
+                        plugin='scitos2_mira::MiraFramework',
+                        name='mira',
+                        parameters=[configured_params],
+                    ),
+                    ComposableNode(
+                        package='nav2_lifecycle_manager',
+                        plugin='nav2_lifecycle_manager::LifecycleManager',
+                        name='lifecycle_manager_scitos',
+                        parameters=[
+                            {'autostart': autostart, 'node_names': lifecycle_nodes}
+                        ],
+                    ),
+                ],
+            ),
+        ],
     )
 
-    return LaunchDescription([
-        params_file_launch_arg,
-        scitos_config_file_launch_arg,
-        scitos_mira,
-        lifecycle_manager
-    ])
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Set environment variables
+    ld.add_action(stdout_linebuf_envvar)
+
+    # Declare the launch options
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_scitos_config_file_cmd)
+    ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    # Add the actions to launch all of the navigation nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld