From c1edb912917b4bf7eccdedf5448f9b8ce078f420 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 22 Dec 2023 21:12:52 +0100 Subject: [PATCH] Spawn robot and world separately (#594) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Spawn robot separately from the world. Signed-off-by: Carlos Agüero Co-authored-by: Jessica <61019526+j-herman@users.noreply.github.com> --- vrx_gz/launch/spawn.launch.py | 9 +++ vrx_gz/launch/vrx_environment.launch.py | 101 ++++++++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 vrx_gz/launch/vrx_environment.launch.py diff --git a/vrx_gz/launch/spawn.launch.py b/vrx_gz/launch/spawn.launch.py index 903b4ebe9..0646c8c35 100644 --- a/vrx_gz/launch/spawn.launch.py +++ b/vrx_gz/launch/spawn.launch.py @@ -35,8 +35,13 @@ def parse_from_cli(context, world_name): y_rot = LaunchConfiguration('Y').perform(context) position = [x_pos, y_pos, z_pos, r_rot, p_rot, y_rot] + robot_urdf = LaunchConfiguration('urdf').perform(context) + model = Model(robot_name, model_type, position) + if robot_urdf and robot_urdf != '': + model.set_urdf(robot_urdf) + slot0_payload = LaunchConfiguration('slot0').perform(context) slot1_payload = LaunchConfiguration('slot1').perform(context) slot2_payload = LaunchConfiguration('slot2').perform(context) @@ -215,6 +220,10 @@ def generate_launch_description(): 'slot7_rpy', default_value='0 0 0', description='Roll, Pitch, Yaw in degrees of payload mount'), + DeclareLaunchArgument( + 'urdf', + default_value='', + description='URDF file of the wam-v model. '), # launch setup OpaqueFunction(function=launch) ]) diff --git a/vrx_gz/launch/vrx_environment.launch.py b/vrx_gz/launch/vrx_environment.launch.py new file mode 100644 index 000000000..4d68324e0 --- /dev/null +++ b/vrx_gz/launch/vrx_environment.launch.py @@ -0,0 +1,101 @@ +# Copyright 2021 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. + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +import os + +import vrx_gz.launch +from vrx_gz.model import Model + + +def launch(context, *args, **kwargs): + config_file = LaunchConfiguration('config_file').perform(context) + world_name = LaunchConfiguration('world').perform(context) + sim_mode = LaunchConfiguration('sim_mode').perform(context) + bridge_competition_topics = LaunchConfiguration( + 'bridge_competition_topics').perform(context).lower() == 'true' + robot = LaunchConfiguration('robot').perform(context) + headless = LaunchConfiguration('headless').perform(context).lower() == 'true' + gz_paused = LaunchConfiguration('paused').perform(context).lower() == 'true' + competition_mode = LaunchConfiguration('competition_mode').perform(context).lower() == 'true' + extra_gz_args = LaunchConfiguration('extra_gz_args').perform(context) + + launch_processes = [] + + models = [] + if config_file and config_file != '': + with open(config_file, 'r') as stream: + models = Model.FromConfig(stream) + + world_name, ext = os.path.splitext(world_name) + launch_processes.extend(vrx_gz.launch.simulation(world_name, headless, + gz_paused, extra_gz_args)) + world_name_base = os.path.basename(world_name) + launch_processes.extend(vrx_gz.launch.spawn(sim_mode, world_name_base, models, robot)) + + if (sim_mode == 'bridge' or sim_mode == 'full') and bridge_competition_topics: + launch_processes.extend(vrx_gz.launch.competition_bridges(world_name_base, competition_mode)) + + return launch_processes + + +def generate_launch_description(): + return LaunchDescription([ + # Launch Arguments + DeclareLaunchArgument( + 'world', + default_value='sydney_regatta', + description='Name of world'), + DeclareLaunchArgument( + 'sim_mode', + default_value='full', + description='Simulation mode: "full", "sim", "bridge".' + 'full: spawns robot and launch ros_gz bridges, ' + 'sim: spawns robot only, ' + 'bridge: launch ros_gz bridges only.'), + DeclareLaunchArgument( + 'bridge_competition_topics', + default_value='True', + description='True to bridge competition topics, False to disable bridge.'), + DeclareLaunchArgument( + 'config_file', + default_value='', + description='YAML configuration file to spawn'), + DeclareLaunchArgument( + 'robot', + default_value='', + description='Name of robot to spawn if specified. ' + 'This must match one of the robots in the config_file'), + DeclareLaunchArgument( + 'headless', + default_value='False', + description='True to run simulation headless (no GUI). '), + DeclareLaunchArgument( + 'paused', + default_value='False', + description='True to start the simulation paused. '), + DeclareLaunchArgument( + 'competition_mode', + default_value='False', + description='True to disable debug topics. '), + DeclareLaunchArgument( + 'extra_gz_args', + default_value='', + description='Additional arguments to be passed to gz sim. '), + OpaqueFunction(function=launch), + ])