diff --git a/launch/localization.launch.py b/launch/localization.launch.py new file mode 100644 index 0000000..9f8dc39 --- /dev/null +++ b/launch/localization.launch.py @@ -0,0 +1,192 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['map_server', 'amcl'] + + # Map fully qualified names 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 + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + 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_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + 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='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_amcl', + executable='amcl', + name='amcl', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_localization', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_amcl', + plugin='nav2_amcl::AmclNode', + name='amcl', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_localization', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld \ No newline at end of file diff --git a/launch/navigation.launch.py b/launch/navigation.launch.py new file mode 100644 index 0000000..94f73b1 --- /dev/null +++ b/launch/navigation.launch.py @@ -0,0 +1,272 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + use_composition = LaunchConfiguration('use_composition') + container_name = LaunchConfiguration('container_name') + container_name_full = (namespace, '/', container_name) + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + lifecycle_nodes = ['controller_server', + 'smoother_server', + 'planner_server', + 'behavior_server', + 'bt_navigator', + 'waypoint_follower', + 'velocity_smoother'] + + # Map fully qualified names 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 + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + 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_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Use composed bringup if True') + + declare_container_name_cmd = DeclareLaunchArgument( + 'container_name', default_value='nav2_container', + description='the name of conatiner that nodes will load in if use composition') + + 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='nav2_controller', + executable='controller_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), + Node( + package='nav2_velocity_smoother', + executable='velocity_smoother', + name='velocity_smoother', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]), + ] + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package='nav2_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_velocity_smoother', + plugin='nav2_velocity_smoother::VelocitySmoother', + name='velocity_smoother', + parameters=[configured_params], + remappings=remappings + + [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld \ No newline at end of file diff --git a/launch/robot_navigation.launch.py b/launch/robot_navigation.launch.py index 1339e48..a3ccde3 100644 --- a/launch/robot_navigation.launch.py +++ b/launch/robot_navigation.launch.py @@ -17,37 +17,34 @@ import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.actions import (DeclareLaunchArgument, GroupAction, + IncludeLaunchDescription, SetEnvironmentVariable) from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import LoadComposableNodes from launch_ros.actions import Node -from launch_ros.descriptions import ComposableNode -from nav2_common.launch import RewrittenYaml +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml, ReplaceString def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('cps_rmp220_support') + launch_dir = os.path.join(bringup_dir, 'launch') + # Create the launch configuration variables namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') + slam = LaunchConfiguration('slam') + map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') - container_name = LaunchConfiguration('container_name') - container_name_full = (namespace, '/', container_name) use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') - lifecycle_nodes = ['controller_server', - 'smoother_server', - 'planner_server', - 'behavior_server', - 'bt_navigator', - 'waypoint_follower', - 'velocity_smoother'] - # Map fully qualified names 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 # https://github.com/ros/geometry2/issues/32 @@ -60,13 +57,24 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { 'use_sim_time': use_sim_time, - 'autostart': autostart} + 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( + # Only it applys when `use_namespace` is True. + # '' keyword shall be replaced by 'namespace' launch argument + # in config file 'nav2_multirobot_params.yaml' as a default & example. + # User defined config file should contain '' keyword for the replacements. + params_file = ReplaceString( + source_file=params_file, + replacements={'': ('/', namespace)}, + condition=IfCondition(use_namespace)) + + configured_params = ParameterFile( + RewrittenYaml( source_file=params_file, root_key=namespace, param_rewrites=param_substitutions, - convert_types=True) + convert_types=True), + allow_substs=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -76,6 +84,20 @@ def generate_launch_description(): default_value='', description='Top-level namespace') + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack') + + declare_slam_cmd = DeclareLaunchArgument( + 'slam', + default_value='False', + description='Whether run a SLAM') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -91,12 +113,8 @@ def generate_launch_description(): description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True') - - declare_container_name_cmd = DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition') + 'use_composition', default_value='True', + description='Whether to use composed bringup') declare_use_respawn_cmd = DeclareLaunchArgument( 'use_respawn', default_value='False', @@ -106,147 +124,54 @@ def generate_launch_description(): 'log_level', default_value='info', description='log level') - load_nodes = GroupAction( - condition=IfCondition(PythonExpression(['not ', use_composition])), - actions=[ - Node( - package='nav2_controller', - executable='controller_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), - Node( - package='nav2_smoother', - executable='smoother_server', - name='smoother_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_behaviors', - executable='behavior_server', - name='behavior_server', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings), - Node( - package='nav2_velocity_smoother', - executable='velocity_smoother', - name='velocity_smoother', - output='screen', - respawn=use_respawn, - respawn_delay=2.0, - parameters=[configured_params], - arguments=['--ros-args', '--log-level', log_level], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - arguments=['--ros-args', '--log-level', log_level], - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - ] - ) + # Specify the actions + bringup_cmd_group = GroupAction([ + PushRosNamespace( + condition=IfCondition(use_namespace), + namespace=namespace), - load_composable_nodes = LoadComposableNodes( - condition=IfCondition(use_composition), - target_container=container_name_full, - composable_node_descriptions=[ - ComposableNode( - package='nav2_controller', - plugin='nav2_controller::ControllerServer', - name='controller_server', - parameters=[configured_params], - remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), - ComposableNode( - package='nav2_smoother', - plugin='nav2_smoother::SmootherServer', - name='smoother_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_planner', - plugin='nav2_planner::PlannerServer', - name='planner_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_behaviors', - plugin='behavior_server::BehaviorServer', - name='behavior_server', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_bt_navigator', - plugin='nav2_bt_navigator::BtNavigator', - name='bt_navigator', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_waypoint_follower', - plugin='nav2_waypoint_follower::WaypointFollower', - name='waypoint_follower', - parameters=[configured_params], - remappings=remappings), - ComposableNode( - package='nav2_velocity_smoother', - plugin='nav2_velocity_smoother::VelocitySmoother', - name='velocity_smoother', - parameters=[configured_params], - remappings=remappings + - [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]), - ComposableNode( - package='nav2_lifecycle_manager', - plugin='nav2_lifecycle_manager::LifecycleManager', - name='lifecycle_manager_navigation', - parameters=[{'use_sim_time': use_sim_time, - 'autostart': autostart, - 'node_names': lifecycle_nodes}]), - ], - ) + Node( + condition=IfCondition(use_composition), + name='nav2_container', + package='rclcpp_components', + executable='component_container_isolated', + parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings, + output='screen'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')), + condition=IfCondition(slam), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'use_respawn': use_respawn, + 'params_file': params_file}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, + 'localization_launch.py')), + condition=IfCondition(PythonExpression(['not ', slam])), + launch_arguments={'namespace': namespace, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'autostart': autostart, + 'params_file': params_file, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'container_name': 'nav2_container'}.items()), + ]) # Create the launch description and populate ld = LaunchDescription() @@ -256,15 +181,17 @@ def generate_launch_description(): # Declare the launch options ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_namespace_cmd) + ld.add_action(declare_slam_cmd) + ld.add_action(declare_map_yaml_cmd) ld.add_action(declare_use_sim_time_cmd) ld.add_action(declare_params_file_cmd) ld.add_action(declare_autostart_cmd) ld.add_action(declare_use_composition_cmd) - ld.add_action(declare_container_name_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes - ld.add_action(load_nodes) - ld.add_action(load_composable_nodes) + ld.add_action(bringup_cmd_group) return ld \ No newline at end of file diff --git a/launch/slam.launch.py b/launch/slam.launch.py new file mode 100644 index 0000000..29eb2cd --- /dev/null +++ b/launch/slam.launch.py @@ -0,0 +1,142 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# 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 os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import HasNodeParams, RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + namespace = LaunchConfiguration('namespace') + params_file = LaunchConfiguration('params_file') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') + + # Variables + lifecycle_nodes = ['map_saver'] + + # Getting directories and launch-files + bringup_dir = get_package_share_directory('nav2_bringup') + slam_toolbox_dir = get_package_share_directory('slam_toolbox') + slam_launch_file = os.path.join(slam_toolbox_dir, 'launch', 'online_sync_launch.py') + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True), + allow_substs=True) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + 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_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='True', + description='Use simulation (Gazebo) clock if true') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='True', + description='Automatically startup the nav2 stack') + + 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') + + # Nodes launching commands + + start_map_saver_server_cmd = Node( + package='nav2_map_server', + executable='map_saver_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level], + parameters=[configured_params]) + + start_lifecycle_manager_cmd = Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_slam', + output='screen', + arguments=['--ros-args', '--log-level', log_level], + parameters=[{'use_sim_time': use_sim_time}, + {'autostart': autostart}, + {'node_names': lifecycle_nodes}]) + + # If the provided param file doesn't have slam_toolbox params, we must remove the 'params_file' + # LaunchConfiguration, or it will be passed automatically to slam_toolbox and will not load + # the default file + has_slam_toolbox_params = HasNodeParams(source_file=params_file, + node_name='slam_toolbox') + + start_slam_toolbox_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time}.items(), + condition=UnlessCondition(has_slam_toolbox_params)) + + start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={'use_sim_time': use_sim_time, + 'slam_params_file': params_file}.items(), + condition=IfCondition(has_slam_toolbox_params)) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Running Map Saver Server + ld.add_action(start_map_saver_server_cmd) + ld.add_action(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox (Only one of them will be run) + ld.add_action(start_slam_toolbox_cmd) + ld.add_action(start_slam_toolbox_cmd_with_params) + + return ld \ No newline at end of file