Move gzserver logic to its action (#646) (#649)

Signed-off-by: Carlos Agüero <[email protected]> (cherry picked from commit 6bbb9e4) Co-authored-by: Carlos Agüero <[email protected]>
gazebosim · Nov 22, 2024 · a7355c9 · a7355c9
1 parent 22ba635
commit a7355c9
Show file tree

Hide file tree

Showing 3 changed files with 101 additions and 105 deletions.
diff --git a/ros_gz_sim/launch/gz_server.launch.py b/ros_gz_sim/launch/gz_server.launch.py
@@ -16,10 +16,8 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression, TextSubstitution
-from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node
-from launch_ros.descriptions import ComposableNode
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from ros_gz_sim.actions import GzServer
 
 
 def generate_launch_description():
@@ -40,51 +38,12 @@ def generate_launch_description():
         'use_composition', default_value='False',
         description='Use composed bringup if True')
 
-    load_nodes = Node(
-        condition=IfCondition(PythonExpression(['not ', LaunchConfiguration('use_composition')])),
-        package='ros_gz_sim',
-        executable='gzserver',
-        output='screen',
-        parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'),
-                     'world_sdf_string': LaunchConfiguration('world_sdf_string')}],
-    )
-
-    load_composable_nodes_with_container = ComposableNodeContainer(
-        condition=IfCondition(
-            PythonExpression([LaunchConfiguration('use_composition'), ' and ',
-                              LaunchConfiguration('create_own_container')])),
-        name=LaunchConfiguration('container_name'),
-        namespace='',
-        package='rclcpp_components',
-        executable='component_container',
-        composable_node_descriptions=[
-            ComposableNode(
-                package='ros_gz_sim',
-                plugin='ros_gz_sim::GzServer',
-                name='gz_server',
-                parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'),
-                             'world_sdf_string': LaunchConfiguration('world_sdf_string')}],
-                extra_arguments=[{'use_intra_process_comms': True}],
-            ),
-        ],
-        output='screen',
-    )
-
-    load_composable_nodes_without_container = LoadComposableNodes(
-        condition=IfCondition(
-            PythonExpression([LaunchConfiguration('use_composition'), ' and not ',
-                              LaunchConfiguration('create_own_container')])),
-        target_container=LaunchConfiguration('container_name'),
-        composable_node_descriptions=[
-            ComposableNode(
-                package='ros_gz_sim',
-                plugin='ros_gz_sim::GzServer',
-                name='gz_server',
-                parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'),
-                             'world_sdf_string': LaunchConfiguration('world_sdf_string')}],
-                extra_arguments=[{'use_intra_process_comms': True}],
-            ),
-        ],
+    gz_server_action = GzServer(
+        world_sdf_file=LaunchConfiguration('world_sdf_file'),
+        world_sdf_string=LaunchConfiguration('world_sdf_string'),
+        container_name=LaunchConfiguration('container_name'),
+        create_own_container=LaunchConfiguration('create_own_container'),
+        use_composition=LaunchConfiguration('use_composition'),
     )
 
     # Create the launch description and populate
@@ -96,9 +55,7 @@ def generate_launch_description():
     ld.add_action(declare_container_name_cmd)
     ld.add_action(declare_create_own_container_cmd)
     ld.add_action(declare_use_composition_cmd)
-    # Add the actions to launch all of the gz_server nodes
-    ld.add_action(load_nodes)
-    ld.add_action(load_composable_nodes_with_container)
-    ld.add_action(load_composable_nodes_without_container)
+    # Add the gz_server action
+    ld.add_action(gz_server_action)
 
     return ld
diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py
@@ -15,28 +15,14 @@
 """Launch gzsim + ros_gz_bridge in a component container."""
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
-from launch_ros.substitutions import FindPackageShare
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, TextSubstitution
 from ros_gz_bridge.actions import RosGzBridge
+from ros_gz_sim.actions import GzServer
 
 
 def generate_launch_description():
 
-    bridge_name = LaunchConfiguration('bridge_name')
-    config_file = LaunchConfiguration('config_file')
-    container_name = LaunchConfiguration('container_name')
-    create_own_container = LaunchConfiguration('create_own_container')
-    namespace = LaunchConfiguration('namespace')
-    use_composition = LaunchConfiguration('use_composition')
-    use_respawn = LaunchConfiguration('use_respawn')
-    bridge_log_level = LaunchConfiguration('bridge_log_level')
-    bridge_params = LaunchConfiguration('bridge_params')
-
-    world_sdf_file = LaunchConfiguration('world_sdf_file')
-    world_sdf_string = LaunchConfiguration('world_sdf_string')
-
     declare_bridge_name_cmd = DeclareLaunchArgument(
         'bridge_name', description='Name of the bridge'
     )
@@ -89,27 +75,24 @@ def generate_launch_description():
         description='SDF world string'
     )
 
-    gz_server_description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                   'launch',
-                                   'gz_server.launch.py'])]),
-        launch_arguments=[('world_sdf_file', world_sdf_file),
-                          ('world_sdf_string', world_sdf_string),
-                          ('container_name', container_name),
-                          ('create_own_container', create_own_container),
-                          ('use_composition', use_composition), ])
+    gz_server_action = GzServer(
+        world_sdf_file=LaunchConfiguration('world_sdf_file'),
+        world_sdf_string=LaunchConfiguration('world_sdf_string'),
+        container_name=LaunchConfiguration('container_name'),
+        create_own_container=LaunchConfiguration('create_own_container'),
+        use_composition=LaunchConfiguration('use_composition'),
+    )
 
     ros_gz_bridge_action = RosGzBridge(
-        bridge_name=bridge_name,
-        config_file=config_file,
-        container_name=container_name,
+        bridge_name=LaunchConfiguration('bridge_name'),
+        config_file=LaunchConfiguration('config_file'),
+        container_name=LaunchConfiguration('container_name'),
         create_own_container=str(False),
-        namespace=namespace,
-        use_composition=use_composition,
-        use_respawn=use_respawn,
-        log_level=bridge_log_level,
-        bridge_params=bridge_params,
+        namespace=LaunchConfiguration('namespace'),
+        use_composition=LaunchConfiguration('use_composition'),
+        use_respawn=LaunchConfiguration('use_respawn'),
+        log_level=LaunchConfiguration('bridge_log_level'),
+        bridge_params=LaunchConfiguration('bridge_params'),
     )
 
     # Create the launch description and populate
@@ -128,7 +111,7 @@ def generate_launch_description():
     ld.add_action(declare_world_sdf_file_cmd)
     ld.add_action(declare_world_sdf_string_cmd)
     # Add the actions to launch all of the bridge + gz_server nodes
-    ld.add_action(gz_server_description)
+    ld.add_action(gz_server_action)
     ld.add_action(ros_gz_bridge_action)
 
     return ld
diff --git a/ros_gz_sim/ros_gz_sim/actions/gzserver.py b/ros_gz_sim/ros_gz_sim/actions/gzserver.py
@@ -18,13 +18,14 @@
 from typing import Optional
 
 from launch.action import Action
-from launch.actions import IncludeLaunchDescription
+from launch.actions import GroupAction
+from launch.conditions import IfCondition
 from launch.frontend import Entity, expose_action, Parser
 from launch.launch_context import LaunchContext
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.some_substitutions_type import SomeSubstitutionsType
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
 
 
 @expose_action('gz_server')
@@ -110,15 +111,70 @@ def parse(cls, entity: Entity, parser: Parser):
 
     def execute(self, context: LaunchContext) -> Optional[List[Action]]:
         """Execute the action."""
-        gz_server_description = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_server.launch.py'])]),
-            launch_arguments=[('world_sdf_file', self.__world_sdf_file),
-                              ('world_sdf_string', self.__world_sdf_string),
-                              ('container_name',   self.__container_name),
-                              ('create_own_container', self.__create_own_container),
-                              ('use_composition',  self.__use_composition), ])
-
-        return [gz_server_description]
+        if isinstance(self.__use_composition, list):
+            self.__use_composition = self.__use_composition[0]
+
+        if isinstance(self.__create_own_container, list):
+            self.__create_own_container = self.__create_own_container[0]
+
+        # Standard node configuration
+        load_nodes = GroupAction(
+            condition=IfCondition(PythonExpression(['not ', self.__use_composition])),
+            actions=[
+                Node(
+                    package='ros_gz_sim',
+                    executable='gzserver',
+                    output='screen',
+                    parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'),
+                                 'world_sdf_string': LaunchConfiguration('world_sdf_string')}],
+                ),
+            ],
+        )
+
+        # Composable node with container configuration
+        load_composable_nodes_with_container = ComposableNodeContainer(
+            condition=IfCondition(
+                PythonExpression([self.__use_composition, ' and ', self.__create_own_container])
+            ),
+            name=self.__container_name,
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='ros_gz_sim',
+                    plugin='ros_gz_sim::GzServer',
+                    name='gz_server',
+                    parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'),
+                                 'world_sdf_string': LaunchConfiguration('world_sdf_string')}],
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+            output='screen',
+        )
+
+        # Composable node without container configuration
+        load_composable_nodes_without_container = LoadComposableNodes(
+            condition=IfCondition(
+                PythonExpression(
+                    [self.__use_composition, ' and not ', self.__create_own_container]
+                )
+            ),
+            target_container=self.__container_name,
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='ros_gz_sim',
+                    plugin='ros_gz_sim::GzServer',
+                    name='gz_server',
+                    parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'),
+                                 'world_sdf_string': LaunchConfiguration('world_sdf_string')}],
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+        )
+
+        return [
+            load_nodes,
+            load_composable_nodes_with_container,
+            load_composable_nodes_without_container
+        ]