Launch gzserver and the bridge as composable nodes

ros_gz_bridge/CMakeLists.txt

@@ -133,6 +133,11 @@ install(
   DESTINATION include/${PROJECT_NAME}
 )
 
+install(
+  DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
 set(bridge_executables
   parameter_bridge
   static_bridge

ros_gz_bridge/launch/ros_gz_bridge.launch.py

@@ -0,0 +1,109 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch ros_gz bridge in a component container."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+
+    config_file = LaunchConfiguration('config_file')
+    container_name = LaunchConfiguration('container_name')
+    namespace = LaunchConfiguration('namespace')
+    use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    declare_config_file_cmd = DeclareLaunchArgument(
+        'config_file', default_value='', description='YAML config file'
+    )
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name',
+        default_value='ros_gz_container',
+        description='Name of container that nodes will load in if use composition',
+    )
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace', default_value='', description='Top-level namespace'
+    )
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='False', description='Use composed bringup if True'
+    )
+
+    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='ros_gz_bridge',
+                executable='bridge_node',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[{'config_file': config_file}],
+                arguments=['--ros-args', '--log-level', log_level],
+            ),
+        ],
+    )
+
+    load_composable_nodes = ComposableNodeContainer(
+        condition=IfCondition(use_composition),
+        name=container_name,
+        namespace=namespace,
+        package='rclcpp_components',
+        executable='component_container',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='ros_gz_bridge',
+                plugin='ros_gz_bridge::RosGzBridge',
+                name='ros_gz_bridge',
+                parameters=[{'config_file': config_file}],
+                extra_arguments=[{'use_intra_process_comms': True}],
+            ),
+        ],
+        output='screen',
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_config_file_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    # Add the actions to launch all of the bridge nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

ros_gz_sim/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 
 find_package(gz_math_vendor REQUIRED)
@@ -33,6 +34,9 @@ gz_find_package(gflags
     PKGCONFIG gflags)
 find_package(std_msgs REQUIRED)
 
+# Install the python module for this package
+ament_python_install_package(${PROJECT_NAME})
+
 add_executable(create src/create.cpp)
 ament_target_dependencies(create
   rclcpp
@@ -60,15 +64,22 @@ install(DIRECTORY include/${PROJECT_NAME}/
   DESTINATION include/${PROJECT_NAME}
 )
 
-add_executable(gzserver src/gzserver.cpp)
-ament_target_dependencies(gzserver
+add_library(gzserver_component SHARED src/gzserver.cpp)
+rclcpp_components_register_nodes(gzserver_component "ros_gz_sim::GzServer")
+ament_target_dependencies(gzserver_component
   rclcpp
+  rclcpp_components
   std_msgs
 )
-target_link_libraries(gzserver
+target_link_libraries(gzserver_component
   gz-sim::core
 )
-ament_target_dependencies(gzserver std_msgs)
+ament_target_dependencies(gzserver_component std_msgs)
+rclcpp_components_register_node(
+  gzserver_component
+  PLUGIN "ros_gz_sim::GzServer"
+  EXECUTABLE gzserver
+)
 
 configure_file(
   launch/gz_sim.launch.py.in
@@ -85,6 +96,15 @@ install(FILES
   DESTINATION share/${PROJECT_NAME}/launch
 )
 
+install(FILES
+  "launch/gz_server.launch"
+  "launch/gz_server.launch.py"
+  "launch/gz_spawn_model.launch.py"
+  "launch/ros_gz_sim.launch.py"
+  "launch/ros_gz_spawn_model.launch.py"
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
 install(TARGETS
   create
   DESTINATION lib/${PROJECT_NAME}
@@ -94,6 +114,14 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+ament_export_targets(export_gzserver_component)
+install(TARGETS gzserver_component
+        EXPORT export_gzserver_component
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION bin
+)
+
 install(
   TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}
   ARCHIVE DESTINATION lib

ros_gz_sim/launch/gz_server.launch

@@ -0,0 +1,12 @@
+<launch>
+  <arg name="world_sdf_file" default="empty.sdf" />
+  <arg name="world_sdf_string" default="" />
+  <arg name="container_name" default="ros_gz_container" />
+  <arg name="use_composition" default="False" />
+  <gz_server 
+    world_sdf_file="$(var world_sdf_file)"
+    world_sdf_string="$(var world_sdf_string)"
+    container_name="$(var container_name)"
+    use_composition="$(var use_composition)">
+  </gz_server>
+</launch>

ros_gz_sim/launch/gz_server.launch.py

@@ -0,0 +1,80 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch gz_server in a component container."""
+
+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, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+
+    declare_world_sdf_file_cmd = DeclareLaunchArgument(
+        'world_sdf_file', default_value=TextSubstitution(text=''),
+        description='Path to the SDF world file')
+    declare_world_sdf_string_cmd = DeclareLaunchArgument(
+        'world_sdf_string', default_value=TextSubstitution(text=''),
+        description='SDF world string')
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='ros_gz_container',
+        description='Name of container that nodes will load in if use composition',)
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        '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 = ComposableNodeContainer(
+        condition=IfCondition(LaunchConfiguration('use_composition')),
+        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',
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_world_sdf_file_cmd)
+    ld.add_action(declare_world_sdf_string_cmd)
+    ld.add_action(declare_container_name_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)
+
+    return ld

ros_gz_sim/launch/gz_spawn_model.launch.py

@@ -0,0 +1,94 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Launch create to spawn models in gz sim."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    world = LaunchConfiguration('world')
+    file = LaunchConfiguration('file')
+    xml_string = LaunchConfiguration('string')
+    topic = LaunchConfiguration('topic')
+    name = LaunchConfiguration('name')
+    allow_renaming = LaunchConfiguration('allow_renaming')
+    x = LaunchConfiguration('x', default='0.0')
+    y = LaunchConfiguration('y', default='0.0')
+    z = LaunchConfiguration('z', default='0.0')
+    roll = LaunchConfiguration('R', default='0.0')
+    pitch = LaunchConfiguration('P', default='0.0')
+    yaw = LaunchConfiguration('Y', default='0.0')
+
+    declare_world_cmd = DeclareLaunchArgument(
+        'world', default_value=TextSubstitution(text=''),
+        description='World name')
+    declare_file_cmd = DeclareLaunchArgument(
+        'file', default_value=TextSubstitution(text=''),
+        description='SDF filename')
+    declare_xml_string_cmd = DeclareLaunchArgument(
+        'string',
+        default_value='',
+        description='XML string',
+    )
+    declare_topic_cmd = DeclareLaunchArgument(
+        'topic', default_value=TextSubstitution(text=''),
+        description='Get XML from this topic'
+    )
+    declare_name_cmd = DeclareLaunchArgument(
+        'name', default_value=TextSubstitution(text=''),
+        description='Name of the entity'
+    )
+    declare_allow_renaming_cmd = DeclareLaunchArgument(
+        'allow_renaming', default_value='False',
+        description='Whether the entity allows renaming or not'
+    )
+
+    load_nodes = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        parameters=[{'world': world,
+                     'file': file,
+                     'string': xml_string,
+                     'topic': topic,
+                     'name': name,
+                     'allow_renaming': allow_renaming,
+                     'x': x,
+                     'y': y,
+                     'z': z,
+                     'R': roll,
+                     'P': pitch,
+                     'Y': yaw,
+                     }],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_world_cmd)
+    ld.add_action(declare_file_cmd)
+    ld.add_action(declare_xml_string_cmd)
+    ld.add_action(declare_topic_cmd)
+    ld.add_action(declare_name_cmd)
+    ld.add_action(declare_allow_renaming_cmd)
+    # Add the actions to launch all of the create nodes
+    ld.add_action(load_nodes)
+
+    return ld