diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index bd471472276..4377f38e20c 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -42,6 +42,7 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') # 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 @@ -107,6 +108,10 @@ def generate_launch_description(): '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') + # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace( @@ -119,6 +124,7 @@ def generate_launch_description(): package='rclcpp_components', executable='component_container_isolated', parameters=[configured_params, {'autostart': autostart}], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings, output='screen'), @@ -171,6 +177,7 @@ def generate_launch_description(): ld.add_action(declare_autostart_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 navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 218debd9185..9eee3f06a9d 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -38,6 +38,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') lifecycle_nodes = ['map_server', 'amcl'] @@ -99,6 +100,10 @@ def generate_launch_description(): '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=[ @@ -110,6 +115,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_amcl', @@ -119,12 +125,14 @@ def generate_launch_description(): 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}]) @@ -172,6 +180,7 @@ def generate_launch_description(): 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) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index fa1be218f28..bcc16a67190 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -37,6 +37,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') container_name = LaunchConfiguration('container_name') use_respawn = LaunchConfiguration('use_respawn') + log_level = LaunchConfiguration('log_level') lifecycle_nodes = ['controller_server', 'smoother_server', @@ -100,6 +101,10 @@ def generate_launch_description(): '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=[ @@ -110,6 +115,7 @@ def generate_launch_description(): 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', @@ -119,6 +125,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_planner', @@ -128,6 +135,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_behaviors', @@ -137,6 +145,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_bt_navigator', @@ -146,6 +155,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_waypoint_follower', @@ -155,6 +165,7 @@ def generate_launch_description(): respawn=use_respawn, respawn_delay=2.0, parameters=[configured_params], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings), Node( package='nav2_velocity_smoother', @@ -164,6 +175,7 @@ def generate_launch_description(): 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( @@ -171,6 +183,7 @@ def generate_launch_description(): 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}]), @@ -248,7 +261,7 @@ def generate_launch_description(): 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) diff --git a/nav2_bringup/launch/slam_launch.py b/nav2_bringup/launch/slam_launch.py index 6c3b1c1bd31..944b6d1fe1d 100644 --- a/nav2_bringup/launch/slam_launch.py +++ b/nav2_bringup/launch/slam_launch.py @@ -32,6 +32,7 @@ def generate_launch_description(): 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'] @@ -75,6 +76,10 @@ def generate_launch_description(): '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( @@ -83,6 +88,7 @@ def generate_launch_description(): output='screen', respawn=use_respawn, respawn_delay=2.0, + arguments=['--ros-args', '--log-level', log_level], parameters=[configured_params]) start_lifecycle_manager_cmd = Node( @@ -90,6 +96,7 @@ def generate_launch_description(): 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}]) @@ -119,6 +126,7 @@ def generate_launch_description(): 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)