From 04446e08e5a1153043e074528a7a3f2ec55449ab Mon Sep 17 00:00:00 2001 From: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> Date: Thu, 14 Nov 2024 10:20:27 +0100 Subject: [PATCH 1/5] Add remove entity node (#629) Signed-off-by: Wiktor Bajor --- ros_gz_sim/CMakeLists.txt | 26 ++++- ros_gz_sim/launch/gz_remove_model.launch.py | 50 +++++++++ ros_gz_sim/src/remove.cpp | 110 ++++++++++++++++++++ ros_gz_sim/test/test_remove.cpp | 56 ++++++++++ ros_gz_sim/test/test_remove_node.launch.py | 49 +++++++++ 5 files changed, 289 insertions(+), 2 deletions(-) create mode 100644 ros_gz_sim/launch/gz_remove_model.launch.py create mode 100644 ros_gz_sim/src/remove.cpp create mode 100644 ros_gz_sim/test/test_remove.cpp create mode 100644 ros_gz_sim/test/test_remove_node.launch.py diff --git a/ros_gz_sim/CMakeLists.txt b/ros_gz_sim/CMakeLists.txt index dc21f8a9..e61622f9 100644 --- a/ros_gz_sim/CMakeLists.txt +++ b/ros_gz_sim/CMakeLists.txt @@ -48,7 +48,17 @@ target_link_libraries(create gz-msgs::core gz-transport::core ) -ament_target_dependencies(create std_msgs) + +add_executable(remove src/remove.cpp) +ament_target_dependencies(remove + rclcpp + std_msgs +) + +target_link_libraries(remove + gz-msgs::core + gz-transport::core +) add_library(${PROJECT_NAME} SHARED src/Stopwatch.cpp) ament_target_dependencies(${PROJECT_NAME} @@ -104,13 +114,16 @@ install(FILES "launch/ros_gz_sim.launch" "launch/ros_gz_sim.launch.py" "launch/ros_gz_spawn_model.launch.py" + "launch/gz_remove_model.launch.py" DESTINATION share/${PROJECT_NAME}/launch ) install(TARGETS create + remove DESTINATION lib/${PROJECT_NAME} ) + install(TARGETS gzserver DESTINATION lib/${PROJECT_NAME} @@ -151,6 +164,10 @@ if(BUILD_TESTING) test/test_create.cpp ) + ament_add_gtest_executable(test_remove + test/test_remove.cpp + ) + ament_target_dependencies(test_stopwatch rclcpp) target_include_directories(test_stopwatch PUBLIC @@ -165,12 +182,17 @@ if(BUILD_TESTING) gz-transport::core ) + target_link_libraries(test_remove + gz-transport::core + ) + install( - TARGETS test_stopwatch test_create + TARGETS test_stopwatch test_create test_remove DESTINATION lib/${PROJECT_NAME} ) ament_add_gtest_test(test_stopwatch) add_launch_test(test/test_create_node.launch.py TIMEOUT 200) + add_launch_test(test/test_remove_node.launch.py TIMEOUT 200) endif() ament_package() diff --git a/ros_gz_sim/launch/gz_remove_model.launch.py b/ros_gz_sim/launch/gz_remove_model.launch.py new file mode 100644 index 00000000..14dd643a --- /dev/null +++ b/ros_gz_sim/launch/gz_remove_model.launch.py @@ -0,0 +1,50 @@ +# 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 remove 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') + entity_name = LaunchConfiguration('entity_name') + + declare_world_cmd = DeclareLaunchArgument( + 'world', default_value=TextSubstitution(text=''), + description='World name') + declare_entity_name_cmd = DeclareLaunchArgument( + 'entity_name', default_value=TextSubstitution(text=''), + description='SDF filename') + + remove = Node( + package='ros_gz_sim', + executable='remove', + output='screen', + parameters=[{'world': world, 'entity_name': entity_name}], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_world_cmd) + ld.add_action(declare_entity_name_cmd) + ld.add_action(remove) + + return ld diff --git a/ros_gz_sim/src/remove.cpp b/ros_gz_sim/src/remove.cpp new file mode 100644 index 00000000..43df1758 --- /dev/null +++ b/ros_gz_sim/src/remove.cpp @@ -0,0 +1,110 @@ +// 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. + +#include +#include +#include + +#include +#include +#include + +#include +#include + + +constexpr unsigned int timeout{5000}; +std::optional retrieve_world_name(const rclcpp::Node::SharedPtr & ros2_node) +{ + gz::transport::Node node; + bool executed{}; + bool result{}; + const std::string service{"/gazebo/worlds"}; + gz::msgs::StringMsg_V worlds_msg; + + // This loop is here to allow the server time to download resources. + while (rclcpp::ok() && !executed) { + RCLCPP_INFO(ros2_node->get_logger(), "Requesting list of world names."); + executed = node.Request(service, timeout, worlds_msg, result); + } + + if (!executed) { + RCLCPP_INFO(ros2_node->get_logger(), "Timed out when getting world names."); + return std::nullopt; + } + + if (!result || worlds_msg.data().empty()) { + RCLCPP_INFO(ros2_node->get_logger(), "Failed to get world names."); + return std::nullopt; + } + + return worlds_msg.data(0); +} + +int remove_entity( + const std::string & entity_name, + const std::string & world_name, + const rclcpp::Node::SharedPtr & ros2_node) +{ + const std::string service{"/world/" + world_name + "/remove"}; + gz::msgs::Entity entity_remove_request; + entity_remove_request.set_name(entity_name); + entity_remove_request.set_type(gz::msgs::Entity_Type_MODEL); + gz::transport::Node node; + gz::msgs::Boolean response; + bool result; + + while(rclcpp::ok() && !node.Request(service, entity_remove_request, timeout, response, result)) { + RCLCPP_WARN( + ros2_node->get_logger(), "Waiting for service [%s] to become available ...", + service.c_str()); + } + if (result && response.data()) { + RCLCPP_INFO(ros2_node->get_logger(), "Entity removal successful."); + return 0; + } else { + RCLCPP_ERROR( + ros2_node->get_logger(), "Entity removal failed.\n %s", + entity_remove_request.DebugString().c_str()); + return 1; + } + RCLCPP_INFO(ros2_node->get_logger(), "Entity removal was interrupted."); + return 1; +} + +int main(int _argc, char ** _argv) +{ + rclcpp::init(_argc, _argv); + auto ros2_node = rclcpp::Node::make_shared("ros_gz_sim_remove_entity"); + ros2_node->declare_parameter("world", ""); + ros2_node->declare_parameter("entity_name", ""); + + std::string world_name = ros2_node->get_parameter("world").as_string(); + if (world_name.empty()) { + world_name = retrieve_world_name(ros2_node).value_or(""); + if (world_name.empty()) { + return -1; + } + } + + const std::string entity_name = + ros2_node->get_parameter("entity_name").as_string(); + if (entity_name.empty()) { + RCLCPP_INFO(ros2_node->get_logger(), + "Entity to remove name is not provided, entity will not be removed."); + return -1; + } + + return remove_entity(entity_name, world_name, ros2_node); +} diff --git a/ros_gz_sim/test/test_remove.cpp b/ros_gz_sim/test/test_remove.cpp new file mode 100644 index 00000000..74a8f2ce --- /dev/null +++ b/ros_gz_sim/test/test_remove.cpp @@ -0,0 +1,56 @@ +// 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. + +#include +#include + +#include +#include +#include +#include + +#include + +// Simple application that provides a `/remove` service and prints out the +// request's entity name. This works in conjuction with +// test_remove_node.launch.py +int main() +{ + std::mutex m; + std::condition_variable cv; + bool test_complete = false; + + gz::transport::Node node; + auto cb = std::function( + [&]( + const gz::msgs::Entity & _req, + gz::msgs::Boolean & _res) -> bool { + std::cout << _req.name() << std::endl; + _res.set_data(true); + + { + std::lock_guard lk(m); + test_complete = true; + } + cv.notify_one(); + return true; + }); + + node.Advertise("/world/default/remove", cb); + // wait until we receive a message. + std::unique_lock lk(m); + cv.wait(lk, [&] {return test_complete;}); + // Sleep so that the service response can be sent before exiting. + std::this_thread::sleep_for(std::chrono::seconds(1)); +} diff --git a/ros_gz_sim/test/test_remove_node.launch.py b/ros_gz_sim/test/test_remove_node.launch.py new file mode 100644 index 00000000..ebca2429 --- /dev/null +++ b/ros_gz_sim/test/test_remove_node.launch.py @@ -0,0 +1,49 @@ +# 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. + +import unittest + +from launch import LaunchDescription + +from launch_ros.actions import Node + +import launch_testing + + +def generate_test_description(): + entity_name = 'my_robot' + remove = Node(package='ros_gz_sim', + executable='remove', + parameters=[{'world': 'default', 'entity_name': entity_name}], + output='screen') + test_remove = Node(package='ros_gz_sim', executable='test_remove', output='screen') + return LaunchDescription([ + remove, + test_remove, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest(), + ]), locals() + + +class WaitForTests(unittest.TestCase): + + def test_termination(self, test_remove, proc_info): + proc_info.assertWaitForShutdown(process=test_remove, timeout=200) + + +@launch_testing.post_shutdown_test() +class RemoveTest(unittest.TestCase): + + def test_output(self, entity_name, test_remove, proc_output): + launch_testing.asserts.assertInStdout(proc_output, entity_name, test_remove) From 558a1cfd55f9921e78a87c563d8ed847e9eae6bd Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Thu, 21 Nov 2024 00:37:37 +0530 Subject: [PATCH 2/5] Add a way to pass extra parameters to ros_gz_bridge (#628) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add bridge_params argument to ros_gz_bridge Signed-off-by: Aarav Gupta Signed-off-by: Alejandro Hernández Cordero Signed-off-by: Wiktor Bajor Co-authored-by: Alejandro Hernández Cordero Co-authored-by: Wiktor Bajor <69388767+Wiktor-99@users.noreply.github.com> --- ros_gz_bridge/launch/ros_gz_bridge.launch | 4 +- ros_gz_bridge/launch/ros_gz_bridge.launch.py | 86 +++--------- .../ros_gz_bridge/actions/ros_gz_bridge.py | 126 +++++++++++++++--- ros_gz_sim/launch/ros_gz_sim.launch | 4 +- ros_gz_sim/launch/ros_gz_sim.launch.py | 33 +++-- ros_gz_sim/launch/ros_gz_spawn_model.launch | 4 +- .../launch/ros_gz_spawn_model.launch.py | 33 +++-- 7 files changed, 169 insertions(+), 121 deletions(-) diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch b/ros_gz_bridge/launch/ros_gz_bridge.launch index fa76221e..96757e4e 100644 --- a/ros_gz_bridge/launch/ros_gz_bridge.launch +++ b/ros_gz_bridge/launch/ros_gz_bridge.launch @@ -7,6 +7,7 @@ + + log_level="$(var log_level)" + bridge_params="$(var bridge_params)"> diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch.py b/ros_gz_bridge/launch/ros_gz_bridge.launch.py index 3e52dee6..e1690a84 100644 --- a/ros_gz_bridge/launch/ros_gz_bridge.launch.py +++ b/ros_gz_bridge/launch/ros_gz_bridge.launch.py @@ -15,24 +15,13 @@ """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, LoadComposableNodes, Node -from launch_ros.descriptions import ComposableNode +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ros_gz_bridge.actions import RosGzBridge 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') - log_level = LaunchConfiguration('log_level') - declare_bridge_name_cmd = DeclareLaunchArgument( 'bridge_name', description='Name of ros_gz_bridge node' ) @@ -74,57 +63,20 @@ def generate_launch_description(): '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', - name=bridge_name, - namespace=namespace, - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[{'config_file': config_file}], - arguments=['--ros-args', '--log-level', log_level], - ), - ], - ) - - load_composable_nodes_with_container = ComposableNodeContainer( - condition=IfCondition( - PythonExpression([use_composition, ' and ', create_own_container])), - name=LaunchConfiguration('container_name'), - namespace='', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='ros_gz_bridge', - plugin='ros_gz_bridge::RosGzBridge', - name=bridge_name, - namespace=namespace, - parameters=[{'config_file': config_file}], - extra_arguments=[{'use_intra_process_comms': True}], - ), - ], - output='screen', + declare_bridge_params_cmd = DeclareLaunchArgument( + 'bridge_params', default_value='', description='Extra parameters to pass to the bridge.' ) - load_composable_nodes_without_container = LoadComposableNodes( - condition=IfCondition( - PythonExpression([use_composition, ' and not ', create_own_container])), - target_container=container_name, - composable_node_descriptions=[ - ComposableNode( - package='ros_gz_bridge', - plugin='ros_gz_bridge::RosGzBridge', - name=bridge_name, - namespace=namespace, - parameters=[{'config_file': config_file}], - extra_arguments=[{'use_intra_process_comms': True}], - ), - ], + ros_gz_bridge_action = RosGzBridge( + 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'), + log_level=LaunchConfiguration('log_level'), + bridge_params=LaunchConfiguration('bridge_params') ) # Create the launch description and populate @@ -139,9 +91,7 @@ def generate_launch_description(): 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_with_container) - ld.add_action(load_composable_nodes_without_container) - + ld.add_action(declare_bridge_params_cmd) + # Add the ros_gz_bridge action + ld.add_action(ros_gz_bridge_action) return ld diff --git a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py index 8459de4f..4cbee322 100644 --- a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py +++ b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.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 PythonExpression +from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode @expose_action('ros_gz_bridge') @@ -42,14 +43,12 @@ def __init__( use_composition: Optional[SomeSubstitutionsType] = 'False', use_respawn: Optional[SomeSubstitutionsType] = 'False', log_level: Optional[SomeSubstitutionsType] = 'info', + bridge_params: Optional[SomeSubstitutionsType] = '', **kwargs ) -> None: """ Construct a ros_gz bridge action. - All arguments are forwarded to `ros_gz_bridge.launch.ros_gz_bridge.launch.py`, - so see the documentation of that class for further details. - :param: bridge_name Name of ros_gz_bridge node :param: config_file YAML config file. :param: container_name Name of container that nodes will load in if use composition. @@ -58,6 +57,7 @@ def __init__( :param: use_composition Use composed bringup if True. :param: use_respawn Whether to respawn if a node crashes (when composition is disabled). :param: log_level Log level. + :param: bridge_params Extra parameters to pass to the bridge. """ super().__init__(**kwargs) self.__bridge_name = bridge_name @@ -68,6 +68,7 @@ def __init__( self.__use_composition = use_composition self.__use_respawn = use_respawn self.__log_level = log_level + self.__bridge_params = bridge_params @classmethod def parse(cls, entity: Entity, parser: Parser): @@ -106,6 +107,10 @@ def parse(cls, entity: Entity, parser: Parser): 'log_level', data_type=str, optional=True) + bridge_params = entity.get_attr( + 'bridge_params', data_type=str, + optional=True) + if isinstance(bridge_name, str): bridge_name = parser.parse_substitution(bridge_name) kwargs['bridge_name'] = bridge_name @@ -139,22 +144,99 @@ def parse(cls, entity: Entity, parser: Parser): log_level = parser.parse_substitution(log_level) kwargs['log_level'] = log_level + if isinstance(bridge_params, str): + bridge_params = parser.parse_substitution(bridge_params) + kwargs['bridge_params'] = bridge_params + return cls, kwargs def execute(self, context: LaunchContext) -> Optional[List[Action]]: """Execute the action.""" - ros_gz_bridge_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), - 'launch', - 'ros_gz_bridge.launch.py'])]), - launch_arguments=[('bridge_name', self.__bridge_name), - ('config_file', self.__config_file), - ('container_name', self.__container_name), - ('create_own_container', self.__create_own_container), - ('namespace', self.__namespace), - ('use_composition', self.__use_composition), - ('use_respawn', self.__use_respawn), - ('log_level', self.__log_level), ]) - - return [ros_gz_bridge_description] + if hasattr(self.__bridge_params, 'perform'): + string_bridge_params = self.__bridge_params.perform(context) + elif isinstance(self.__bridge_params, list): + if hasattr(self.__bridge_params[0], 'perform'): + string_bridge_params = self.__bridge_params[0].perform(context) + else: + string_bridge_params = str(self.__bridge_params) + # Remove unnecessary symbols + simplified_bridge_params = string_bridge_params.translate( + {ord(i): None for i in '{} "\''} + ) + # Parse to dictionary + parsed_bridge_params = {} + if simplified_bridge_params: + bridge_params_pairs = simplified_bridge_params.split(',') + parsed_bridge_params = dict(pair.split(':') for pair in bridge_params_pairs) + + 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_bridge', + executable='bridge_node', + name=self.__bridge_name, + namespace=self.__namespace, + output='screen', + respawn=self.__use_respawn, + respawn_delay=2.0, + parameters=[{'config_file': self.__config_file, **parsed_bridge_params}], + arguments=['--ros-args', '--log-level', self.__log_level], + ), + ], + ) + + # 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_bridge', + plugin='ros_gz_bridge::RosGzBridge', + name=self.__bridge_name, + namespace=self.__namespace, + parameters=[{'config_file': self.__config_file, **parsed_bridge_params}], + 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_bridge', + plugin='ros_gz_bridge::RosGzBridge', + name=self.__bridge_name, + namespace=self.__namespace, + parameters=[{'config_file': self.__config_file, **parsed_bridge_params}], + extra_arguments=[{'use_intra_process_comms': True}], + ), + ], + ) + + return [ + load_nodes, + load_composable_nodes_with_container, + load_composable_nodes_without_container + ] diff --git a/ros_gz_sim/launch/ros_gz_sim.launch b/ros_gz_sim/launch/ros_gz_sim.launch index 6130ce0d..e39f10b6 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch +++ b/ros_gz_sim/launch/ros_gz_sim.launch @@ -7,6 +7,7 @@ + + log_level="$(var log_level)" + bridge_params="$(var bridge_params)"> diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py index aba047ee..6b3650cf 100644 --- a/ros_gz_sim/launch/ros_gz_sim.launch.py +++ b/ros_gz_sim/launch/ros_gz_sim.launch.py @@ -19,6 +19,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution from launch_ros.substitutions import FindPackageShare +from ros_gz_bridge.actions import RosGzBridge def generate_launch_description(): @@ -31,6 +32,7 @@ def generate_launch_description(): 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') @@ -73,6 +75,10 @@ def generate_launch_description(): 'bridge_log_level', default_value='info', description='Bridge log level' ) + declare_bridge_params_cmd = DeclareLaunchArgument( + 'bridge_params', default_value='', description='Extra parameters to pass to the bridge.' + ) + declare_world_sdf_file_cmd = DeclareLaunchArgument( 'world_sdf_file', default_value=TextSubstitution(text=''), description='Path to the SDF world file' @@ -94,19 +100,17 @@ def generate_launch_description(): ('create_own_container', create_own_container), ('use_composition', use_composition), ]) - bridge_description = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'), - 'launch', - 'ros_gz_bridge.launch.py'])]), - launch_arguments=[('bridge_name', bridge_name), - ('config_file', config_file), - ('container_name', container_name), - ('namespace', namespace), - ('create_own_container', str(False)), - ('use_composition', use_composition), - ('use_respawn', use_respawn), - ('bridge_log_level', bridge_log_level), ]) + ros_gz_bridge_action = RosGzBridge( + bridge_name=bridge_name, + config_file=config_file, + container_name=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, + ) # Create the launch description and populate ld = LaunchDescription() @@ -120,10 +124,11 @@ def generate_launch_description(): 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_bridge_params_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(gz_server_description) - ld.add_action(bridge_description) + ld.add_action(ros_gz_bridge_action) return ld diff --git a/ros_gz_sim/launch/ros_gz_spawn_model.launch b/ros_gz_sim/launch/ros_gz_spawn_model.launch index b1ff49de..e465e7cc 100644 --- a/ros_gz_sim/launch/ros_gz_spawn_model.launch +++ b/ros_gz_sim/launch/ros_gz_spawn_model.launch @@ -7,6 +7,7 @@ + @@ -28,7 +29,8 @@ namespace="$(var namespace)" use_composition="$(var use_composition)" use_respawn="$(var use_respawn)" - log_level="$(var log_level)"> + log_level="$(var log_level)" + bridge_params="$(var bridge_params)"> Date: Thu, 21 Nov 2024 12:30:42 +0100 Subject: [PATCH 3/5] Move gzserver logic to its action (#646) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/launch/gz_server.launch.py | 63 +++------------- ros_gz_sim/launch/ros_gz_sim.launch.py | 55 +++++--------- ros_gz_sim/ros_gz_sim/actions/gzserver.py | 88 ++++++++++++++++++----- 3 files changed, 101 insertions(+), 105 deletions(-) diff --git a/ros_gz_sim/launch/gz_server.launch.py b/ros_gz_sim/launch/gz_server.launch.py index 70de5d24..3c231a1d 100644 --- 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 index 6b3650cf..1465c44e 100644 --- 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 index 64461baf..12d288f4 100644 --- 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 + ] From cfd0f8c74ded9efdcb35410135d0a1da1727dcff Mon Sep 17 00:00:00 2001 From: Aarav Gupta Date: Fri, 22 Nov 2024 01:38:30 +0530 Subject: [PATCH 4/5] Fix use_respawn argument causing errors (#651) Signed-off-by: Aarav Gupta --- ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py index 4cbee322..bcebee0c 100644 --- a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py +++ b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py @@ -175,6 +175,9 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]: if isinstance(self.__create_own_container, list): self.__create_own_container = self.__create_own_container[0] + if isinstance(self.__use_respawn, list): + self.__use_respawn = self.__use_respawn[0] + # Standard node configuration load_nodes = GroupAction( condition=IfCondition(PythonExpression(['not ', self.__use_composition])), @@ -185,7 +188,7 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]: name=self.__bridge_name, namespace=self.__namespace, output='screen', - respawn=self.__use_respawn, + respawn=bool(self.__use_respawn), respawn_delay=2.0, parameters=[{'config_file': self.__config_file, **parsed_bridge_params}], arguments=['--ros-args', '--log-level', self.__log_level], From 5c1251a658ad8dd276664c2c88300d27572e25da Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 22 Nov 2024 09:51:28 +0100 Subject: [PATCH 5/5] Use member variables instead. (#653) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim/ros_gz_sim/actions/gzserver.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ros_gz_sim/ros_gz_sim/actions/gzserver.py b/ros_gz_sim/ros_gz_sim/actions/gzserver.py index 12d288f4..2665ea4d 100644 --- a/ros_gz_sim/ros_gz_sim/actions/gzserver.py +++ b/ros_gz_sim/ros_gz_sim/actions/gzserver.py @@ -23,7 +23,7 @@ from launch.frontend import Entity, expose_action, Parser from launch.launch_context import LaunchContext from launch.some_substitutions_type import SomeSubstitutionsType -from launch.substitutions import LaunchConfiguration, PythonExpression +from launch.substitutions import PythonExpression from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node from launch_ros.descriptions import ComposableNode @@ -125,8 +125,8 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]: package='ros_gz_sim', executable='gzserver', output='screen', - parameters=[{'world_sdf_file': LaunchConfiguration('world_sdf_file'), - 'world_sdf_string': LaunchConfiguration('world_sdf_string')}], + parameters=[{'world_sdf_file': self.__world_sdf_file, + 'world_sdf_string': self.__world_sdf_string}], ), ], ) @@ -145,8 +145,8 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]: 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')}], + parameters=[{'world_sdf_file': self.__world_sdf_file, + 'world_sdf_string': self.__world_sdf_string}], extra_arguments=[{'use_intra_process_comms': True}], ), ], @@ -166,8 +166,8 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]: 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')}], + parameters=[{'world_sdf_file': self.__world_sdf_file, + 'world_sdf_string': self.__world_sdf_string}], extra_arguments=[{'use_intra_process_comms': True}], ), ],