diff --git a/nav2_bringup/bringup/launch/nav2_localization_launch.py b/nav2_bringup/bringup/launch/nav2_localization_launch.py index 14855b69670..fc432d821ff 100644 --- a/nav2_bringup/bringup/launch/nav2_localization_launch.py +++ b/nav2_bringup/bringup/launch/nav2_localization_launch.py @@ -53,12 +53,10 @@ def generate_launch_description(): 'use_sim_time': use_sim_time, 'yaml_filename': map_yaml_file} - namespace_substitutions = {'navigation_namespace': namespace} - configured_params = RewrittenYaml( source_file=params_file, + root_key=namespace, param_rewrites=param_substitutions, - key_rewrites=namespace_substitutions, convert_types=True) return LaunchDescription([ diff --git a/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py new file mode 100644 index 00000000000..99ccb3caa46 --- /dev/null +++ b/nav2_bringup/bringup/launch/nav2_multi_tb3_simulation_launch.py @@ -0,0 +1,171 @@ +# Copyright (c) 2018 Intel Corporation +# +# 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. + +''' This is an example on how to create a launch file for spawning multiple robots into Gazebo + and launch multiple instances of the navigation stack, each controlling one robot. + The robots co-exist on a shared environment and are controlled by independent nav stacks ''' + +import os + +from ament_index_python.packages import get_package_prefix +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, + LogInfo, GroupAction, ExecuteProcess) +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, TextSubstitution +from launch_ros.actions import PushRosNamespace + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + # Names and poses of the robots + robots = [ + {'name': 'robot1', 'x_pose': 0.0, 'y_pose': 0.5, 'z_pose': 0.01}, + {'name': 'robot2', 'x_pose': 0.0, 'y_pose': -0.5, 'z_pose': 0.01}] + + # Simulation settings + world = LaunchConfiguration('world') + simulator = LaunchConfiguration('simulator') + + # On this example all robots are launched with the same settings + map_yaml_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + bt_xml_file = LaunchConfiguration('bt_xml_file') + log_settings = LaunchConfiguration('log_settings', default='true') + + # Declare the launch arguments + declare_world_cmd = DeclareLaunchArgument( + 'world', + default_value=os.path.join(bringup_dir, 'worlds', 'world_only.model'), + description='Full path to world file to load') + + declare_simulator_cmd = DeclareLaunchArgument( + 'simulator', + default_value='gazebo', + description='The simulator to use (gazebo or gzserver)') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), + description='Full path to map file to load') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_bt_xml_cmd = DeclareLaunchArgument( + 'bt_xml_file', + default_value=os.path.join( + get_package_prefix('nav2_bt_navigator'), + 'behavior_trees', 'navigate_w_replanning_and_recovery.xml'), + description='Full path to the behavior tree xml file to use') + + # Start Gazebo with plugin providing the robot spawing service + start_gazebo_cmd = ExecuteProcess( + cmd=[simulator, '--verbose', '-s', 'libgazebo_ros_factory.so', world], + output='screen') + + # Define commands for spawing the robots into Gazebo + spawn_robots_cmds = [] + for robot in robots: + spawn_robots_cmds.append( + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'spawn_robot_launch.py')), + launch_arguments={ + 'x_pose': TextSubstitution(text=str(robot['x_pose'])), + 'y_pose': TextSubstitution(text=str(robot['y_pose'])), + 'z_pose': TextSubstitution(text=str(robot['z_pose'])), + 'robot_name': robot['name'], + 'turtlebot_type': TextSubstitution(text='waffle') + }.items())) + + # Define commands for launching the navigation instances + nav_instances_cmds = [] + for robot in robots: + group = GroupAction([ + # TODO(orduno) Each `action.Node` within the `localization` and `navigation` launch + # files has two versions, one with the required remaps and another without. + # The `use_remappings` flag specifies which runs. + # A better mechanism would be to have a PushNodeRemapping() action: + # https://github.com/ros2/launch_ros/issues/56 + # For more on why we're remapping topics, see the note below + + # PushNodeRemapping(remappings) + + # Instances use the robot's name for namespace + PushRosNamespace(robot['name']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup_dir, 'launch', 'nav2_tb3_simulation_launch.py')), + launch_arguments={ + #TODO(orduno) might not be necessary to pass the robot name + 'namespace': robot['name'], + 'map_yaml_file': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': params_file, + 'bt_xml_file': bt_xml_file, + 'autostart': 'False', + 'use_remappings': 'True', + 'use_simulator': 'False'}.items()), + + LogInfo( + condition=IfCondition(log_settings), + msg=['Launching ', robot['name']]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' map yaml: ', map_yaml_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' params yaml: ', params_file]), + LogInfo( + condition=IfCondition(log_settings), + msg=[robot['name'], ' behavior tree xml: ', bt_xml_file]) + ]) + + nav_instances_cmds.append(group) + + # A note on the `remappings` variable defined above and the fact it's passed as a node arg. + # A few topics have fully qualified names (have a leading '/'), these need to be remapped + # to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # for multi-robot transforms: + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + + # Create the launch description and populate + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_simulator_cmd) + ld.add_action(declare_world_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_bt_xml_cmd) + + # Add the actions to start gazebo, robots and simulations + ld.add_action(start_gazebo_cmd) + + for spawn_robot_cmd in spawn_robots_cmds: + ld.add_action(spawn_robot_cmd) + + for simulation_instance_cmd in nav_instances_cmds: + ld.add_action(simulation_instance_cmd) + + return ld diff --git a/nav2_bringup/bringup/launch/nav2_navigation_launch.py b/nav2_bringup/bringup/launch/nav2_navigation_launch.py index be9210ccd2a..53c01f2ca97 100644 --- a/nav2_bringup/bringup/launch/nav2_navigation_launch.py +++ b/nav2_bringup/bringup/launch/nav2_navigation_launch.py @@ -56,12 +56,10 @@ def generate_launch_description(): 'autostart': autostart, 'map_subscribe_transient_local': map_subscribe_transient_local} - namespace_substitutions = {'navigation_namespace': namespace} - configured_params = RewrittenYaml( source_file=params_file, + root_key=namespace, param_rewrites=param_substitutions, - key_rewrites=namespace_substitutions, convert_types=True) return LaunchDescription([ diff --git a/nav2_bringup/bringup/launch/spawn_robot_launch.py b/nav2_bringup/bringup/launch/spawn_robot_launch.py index ee2c57f0576..2014d71e848 100644 --- a/nav2_bringup/bringup/launch/spawn_robot_launch.py +++ b/nav2_bringup/bringup/launch/spawn_robot_launch.py @@ -28,9 +28,10 @@ def generate_launch_description(): node_executable='spawn_turtlebot', output='screen', arguments=[ - launch.substitutions.LaunchConfiguration('robot_name'), - launch.substitutions.LaunchConfiguration('robot_name'), - launch.substitutions.LaunchConfiguration('x_pose'), - launch.substitutions.LaunchConfiguration('y_pose'), - launch.substitutions.LaunchConfiguration('z_pose')]), + '--robot_name', launch.substitutions.LaunchConfiguration('robot_name'), + '--robot_namespace', launch.substitutions.LaunchConfiguration('robot_name'), + '--turtlebot_type', launch.substitutions.LaunchConfiguration('turtlebot_type'), + '-x', launch.substitutions.LaunchConfiguration('x_pose'), + '-y', launch.substitutions.LaunchConfiguration('y_pose'), + '-z', launch.substitutions.LaunchConfiguration('z_pose')]), ]) diff --git a/nav2_bringup/bringup/worlds/world_only.model b/nav2_bringup/bringup/worlds/world_only.model new file mode 100644 index 00000000000..4c45d4e2f94 --- /dev/null +++ b/nav2_bringup/bringup/worlds/world_only.model @@ -0,0 +1,54 @@ + + + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + 1 + + model://turtlebot3_world + + + + + diff --git a/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py b/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py index d31ac849ba1..ef101f12c6e 100644 --- a/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py +++ b/nav2_bringup/spawn_turtlebot3/src/spawn_turtlebot3/spawn_turtlebot.py @@ -28,7 +28,8 @@ def main(): help='the y component of the initial position [meters]') parser.add_argument('-z', type=float, default=0, help='the z component of the initial position [meters]') - args = parser.parse_args() + + args, unknown = parser.parse_known_args() # Start node rclpy.init() diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py index 2e521b4ca77..0c1cfb51bb4 100644 --- a/nav2_common/nav2_common/launch/rewritten_yaml.py +++ b/nav2_common/nav2_common/launch/rewritten_yaml.py @@ -16,6 +16,7 @@ from typing import List from typing import Text from typing import Optional + import yaml import tempfile import launch @@ -33,15 +34,25 @@ def setValue(self, value): class RewrittenYaml(launch.Substitution): """ - Substitution that modifies the given Yaml file. + Substitution that modifies the given YAML file. """ def __init__(self, source_file: launch.SomeSubstitutionsType, param_rewrites: Dict, + root_key: Optional[launch.SomeSubstitutionsType] = None, key_rewrites: Optional[Dict] = None, convert_types = False) -> None: super().__init__() + """ + Construct the substitution + + :param: source_file the original YAML file to modify + :param: param_rewrites mappings to replace + :param: root_key if provided, the contents are placed under this key + :param: key_rewrites keys of mappings to replace + :param: convert_types whether to attempt converting the string to a number or boolean + """ from launch.utilities import normalize_to_list_of_substitutions # import here to avoid loop self.__source_file = normalize_to_list_of_substitutions(source_file) @@ -53,6 +64,8 @@ def __init__(self, if key_rewrites is not None: for key in key_rewrites: self.__key_rewrites[key] = normalize_to_list_of_substitutions(key_rewrites[key]) + if root_key is not None: + self.__root_key = normalize_to_list_of_substitutions(root_key) @property def name(self) -> List[launch.Substitution]: @@ -70,6 +83,10 @@ def perform(self, context: launch.LaunchContext) -> Text: data = yaml.safe_load(open(yaml_filename, 'r')) self.substitute_params(data, param_rewrites) self.substitute_keys(data, keys_rewrites) + if self.__root_key is not None: + root_key = launch.utilities.perform_substitutions(context, self.__root_key) + if root_key: + data = {root_key: data} yaml.dump(data, rewritten_yaml) rewritten_yaml.close() return rewritten_yaml.name