import os
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.conditions import IfCondition
from nav2_common.launch import RewrittenYaml
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode, ParameterFile
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
# Get the launch directory
bringup_dir = get_package_share_directory('nav_hub')
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 = ['map_server',
'amcl',
'controller_server',
'smoother_server',
'planner_server',
'behavior_server',
'bt_navigator',
'waypoint_follower'
#'velocity_smoother'
]
remappings = []
# 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, 'config', 'nav_dinamico.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_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',
parameters=[configured_params],
),
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],
),
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],
# prefix=['xterm -e gdb -ex run --args'],
# arguments=['--ros-args', '--log-level', 'debug']
),
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],
),
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),
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}]),
]
)
# 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_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)
return ld
Steps to reproduce issue
I cannot reliably reproduce the crash; it happens intermittently during startup of the navigation stack with AMCL.
Typical steps that sometimes trigger it:
Expected behavior
AMCL should start and configure correctly
Actual behavior
Occasionally, AMCL crashes during startup with a stack smashing detected error at
initTransforms. The crash happens intermittently and is not reliably reproducible.Error on backtrace:
Reproduction instructions
Cannot reliably reproduce. Typical usage that sometimes triggers the crash:
Wait for AMCL to configure and initialize. The crash may appear in the console and GDB backtrace shows the failure in
eigen_decomposition()insidelibpf_lib.so.Additional information
AMCL parameters
Entire nav params, if relevant:
Nav params
Launch being used, if relevant:
Navigation Launch