diff --git a/ros_gz_bridge/CMakeLists.txt b/ros_gz_bridge/CMakeLists.txt index 125acb5e..f46f50cc 100644 --- a/ros_gz_bridge/CMakeLists.txt +++ b/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 diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch.py b/ros_gz_bridge/launch/ros_gz_bridge.launch.py new file mode 100644 index 00000000..7809299c --- /dev/null +++ b/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 diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index 125b28dd..a9b3a1cc 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/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 diff --git a/ros_gz_sim/launch/gz_server.launch b/ros_gz_sim/launch/gz_server.launch new file mode 100644 index 00000000..9a617649 --- /dev/null +++ b/ros_gz_sim/launch/gz_server.launch @@ -0,0 +1,12 @@ + + + + + + + + diff --git a/ros_gz_sim/launch/gz_server.launch.py b/ros_gz_sim/launch/gz_server.launch.py new file mode 100644 index 00000000..c73be0fe --- /dev/null +++ b/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 diff --git a/ros_gz_sim/launch/gz_spawn_model.launch.py b/ros_gz_sim/launch/gz_spawn_model.launch.py new file mode 100644 index 00000000..59cb4361 --- /dev/null +++ b/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 diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py new file mode 100644 index 00000000..cad862ea --- /dev/null +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -0,0 +1,111 @@ +# 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 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 + + +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') + bridge_log_level = LaunchConfiguration('bridge_log_level') + + world_sdf_file = LaunchConfiguration('world_sdf_file') + world_sdf_string = LaunchConfiguration('world_sdf_string') + + 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_bridge_log_level_cmd = DeclareLaunchArgument( + 'bridge_log_level', default_value='info', description='Bridge log level' + ) + + 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' + ) + + bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('config_file', config_file), + ('container_name', container_name), + ('namespace', namespace), + ('use_composition', use_composition), + ('use_respawn', use_respawn), + ('bridge_log_level', bridge_log_level)]) + + 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), + ('use_composition', use_composition), ]) + + # 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_bridge_log_level_cmd) + 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(bridge_description) + ld.add_action(gz_server_description) + + return ld diff --git a/ros_gz_sim/launch/ros_gz_spawn_model.launch.py b/ros_gz_sim/launch/ros_gz_spawn_model.launch.py new file mode 100644 index 00000000..e5de33aa --- /dev/null +++ b/ros_gz_sim/launch/ros_gz_spawn_model.launch.py @@ -0,0 +1,154 @@ +# 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 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 + + +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') + + 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_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' + ) + + 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' + ) + + bridge_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), + 'launch', + 'ros_gz_bridge.launch.py'])]), + launch_arguments=[('config_file', config_file), + ('container_name', container_name), + ('namespace', namespace), + ('use_composition', use_composition), + ('use_respawn', use_respawn), + ('log_level', log_level), ]) + + spawn_model_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare('ros_gz_sim'), + 'launch', + 'gz_spawn_model.launch.py'])]), + launch_arguments=[('world', world), + ('file', file), + ('xml_string', xml_string), + ('topic', topic), + ('name', name), + ('allow_renaming', allow_renaming), + ('x', x), + ('y', y), + ('z', z), + ('R', roll), + ('P', pitch), + ('Y', yaw), + ('use_composition', use_composition), ]) + + # 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) + 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 bridge + spawn_model nodes + ld.add_action(bridge_description) + ld.add_action(spawn_model_description) + + return ld diff --git a/ros_gz_sim/package.xml b/ros_gz_sim/package.xml index 1047196d..246a2fff 100644 --- a/ros_gz_sim/package.xml +++ b/ros_gz_sim/package.xml @@ -10,13 +10,19 @@ Apache 2.0 Alejandro Hernandez + Addisu Taddese + Carlos Agüero ament_cmake + ament_cmake_python pkg-config ament_index_python + launch + launch_ros libgflags-dev rclcpp + rclcpp_components std_msgs gz_math_vendor diff --git a/ros_gz_sim/resource/ros_gz_sim b/ros_gz_sim/resource/ros_gz_sim new file mode 100644 index 00000000..e69de29b diff --git a/ros_gz_sim/ros_gz_sim/__init__.py b/ros_gz_sim/ros_gz_sim/__init__.py new file mode 100644 index 00000000..17b9696c --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/__init__.py @@ -0,0 +1,22 @@ +# 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. + +"""Main entry point for the `ros_gz_sim` package.""" + +from . import actions + + +__all__ = [ + 'actions', +] diff --git a/ros_gz_sim/ros_gz_sim/actions/__init__.py b/ros_gz_sim/ros_gz_sim/actions/__init__.py new file mode 100644 index 00000000..03fe5afc --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/actions/__init__.py @@ -0,0 +1,22 @@ +# 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. + +"""Actions module.""" + +from .gzserver import GzServer + + +__all__ = [ + 'GzServer', +] diff --git a/ros_gz_sim/ros_gz_sim/actions/gzserver.py b/ros_gz_sim/ros_gz_sim/actions/gzserver.py new file mode 100644 index 00000000..14df9aab --- /dev/null +++ b/ros_gz_sim/ros_gz_sim/actions/gzserver.py @@ -0,0 +1,111 @@ +# 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. + +"""Module for the GzServer action.""" + +from typing import List +from typing import Optional + +from launch.action import Action +from launch.actions import IncludeLaunchDescription +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 + + +@expose_action('gz_server') +class GzServer(Action): + """Action that executes a gz_server ROS [composable] node.""" + + def __init__( + self, + *, + world_sdf_file: Optional[SomeSubstitutionsType] = None, + world_sdf_string: Optional[SomeSubstitutionsType] = None, + container_name: Optional[SomeSubstitutionsType] = None, + use_composition: Optional[SomeSubstitutionsType] = None, + **kwargs + ) -> None: + """ + Construct a gz_server action. + + All arguments are forwarded to `ros_gz_sim.launch.gz_server.launch.py`, + so see the documentation of that class for further details. + + :param: world_sdf_file Path to the SDF world file. + :param: world_sdf_string SDF world string. + :param: container_name Name of container that nodes will load in if use composition. + :param: use_composition Use composed bringup if True. + """ + super().__init__(**kwargs) + self.__world_sdf_file = world_sdf_file + self.__world_sdf_string = world_sdf_string + self.__container_name = container_name + self.__use_composition = use_composition + + @classmethod + def parse(cls, entity: Entity, parser: Parser): + """Parse gz_server.""" + _, kwargs = super().parse(entity, parser) + + world_sdf_file = entity.get_attr( + 'world_sdf_file', data_type=str, + optional=True) + + world_sdf_string = entity.get_attr( + 'world_sdf_string', data_type=str, + optional=True) + + container_name = entity.get_attr( + 'container_name', data_type=str, + optional=True) + + use_composition = entity.get_attr( + 'use_composition', data_type=str, + optional=True) + + if isinstance(world_sdf_file, str): + world_sdf_file = parser.parse_substitution(world_sdf_file) + kwargs['world_sdf_file'] = world_sdf_file + + if isinstance(world_sdf_string, str): + world_sdf_string = parser.parse_substitution(world_sdf_string) + kwargs['world_sdf_string'] = world_sdf_string + + if isinstance(container_name, str): + container_name = parser.parse_substitution(container_name) + kwargs['container_name'] = container_name + + if isinstance(use_composition, str): + use_composition = parser.parse_substitution(use_composition) + kwargs['use_composition'] = use_composition + + return cls, kwargs + + 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), + ('use_composition', self.__use_composition), ]) + + return [gz_server_description] diff --git a/ros_gz_sim/setup.cfg b/ros_gz_sim/setup.cfg new file mode 100644 index 00000000..c044b41d --- /dev/null +++ b/ros_gz_sim/setup.cfg @@ -0,0 +1,3 @@ +[options.entry_points] +launch.frontend.launch_extension = + ros_gz_sim = ros_gz_sim diff --git a/ros_gz_sim/src/gzserver.cpp b/ros_gz_sim/src/gzserver.cpp index 530f23c7..aa4361f5 100644 --- a/ros_gz_sim/src/gzserver.cpp +++ b/ros_gz_sim/src/gzserver.cpp @@ -12,33 +12,69 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include #include #include +#include // ROS node that executes a gz-sim Server given a world SDF file or string. -int main(int _argc, char ** _argv) +namespace ros_gz_sim { - auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv); - auto node = rclcpp::Node::make_shared("gzserver"); - auto world_sdf_file = node->declare_parameter("world_sdf_file", ""); - auto world_sdf_string = node->declare_parameter("world_sdf_string", ""); - - gz::common::Console::SetVerbosity(4); - gz::sim::ServerConfig server_config; - - if (!world_sdf_file.empty()) { - server_config.SetSdfFile(world_sdf_file); - } else if (!world_sdf_string.empty()) { - server_config.SetSdfString(world_sdf_string); - } else { - RCLCPP_ERROR( - node->get_logger(), "Must specify either 'world_sdf_file' or 'world_sdf_string'"); - return -1; +class GzServer : public rclcpp::Node +{ +public: + // Class constructor. + explicit GzServer(const rclcpp::NodeOptions & options) + : Node("gzserver", options) + { + thread_ = std::thread(std::bind(&GzServer::OnStart, this)); + } + +public: + // Class destructor. + ~GzServer() + { + // Make sure to join the thread on shutdown. + if (thread_.joinable()) { + thread_.join(); + } } - gz::sim::Server server(server_config); - server.Run(true /*blocking*/, 0, false /*paused*/); -} +public: + /// \brief Run the gz sim server. + void OnStart() + { + auto world_sdf_file = this->declare_parameter("world_sdf_file", ""); + auto world_sdf_string = this->declare_parameter("world_sdf_string", ""); + + gz::common::Console::SetVerbosity(4); + gz::sim::ServerConfig server_config; + + if (!world_sdf_file.empty()) { + server_config.SetSdfFile(world_sdf_file); + } else if (!world_sdf_string.empty()) { + server_config.SetSdfString(world_sdf_string); + } else { + RCLCPP_ERROR( + this->get_logger(), + "Must specify either 'world_sdf_file' or 'world_sdf_string'"); + rclcpp::shutdown(); + return; + } + + gz::sim::Server server(server_config); + server.Run(true /*blocking*/, 0, false /*paused*/); + rclcpp::shutdown(); + } + +private: + /// \brief We don't want to block the ROS thread. + std::thread thread_; +}; +} // namespace ros_gz_sim + +RCLCPP_COMPONENTS_REGISTER_NODE(ros_gz_sim::GzServer)