Skip to content
Snippets Groups Projects
Commit 064eaf26 authored by Steven Macenski's avatar Steven Macenski
Browse files

Merge branch 'simplify-bringup' of github.com:stevemacenski/navigation2 into simplify-bringup

parents 4ab19beb 2346ad74
No related branches found
No related tags found
No related merge requests found
......@@ -1079,7 +1079,6 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
#if NEW_UNIFORM_SAMPLING
createFreeSpaceVector();
#endif
}
void
......
......@@ -24,8 +24,6 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import PushRosNamespace
from nav2_common.launch import Node
def generate_launch_description():
# Get the launch directory
......@@ -35,6 +33,12 @@ def generate_launch_description():
# Create the launch configuration variables
namespace = LaunchConfiguration('namespace')
use_namespace = LaunchConfiguration('use_namespace')
map_yaml_file = LaunchConfiguration('map')
use_sim_time = LaunchConfiguration('use_sim_time')
params_file = LaunchConfiguration('params_file')
bt_xml_file = LaunchConfiguration('bt_xml_file')
autostart = LaunchConfiguration('autostart')
use_remappings = LaunchConfiguration('use_remappings')
stdout_linebuf_envvar = SetEnvironmentVariable(
'RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1')
......@@ -49,6 +53,35 @@ def generate_launch_description():
default_value='false',
description='Whether to apply a namespace to the navigation stack')
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_bt_xml_cmd = DeclareLaunchArgument(
'bt_xml_file',
default_value=os.path.join(
get_package_share_directory('nav2_bt_navigator'),
'behavior_trees', 'navigate_w_replanning_and_recovery.xml'),
description='Full path to the behavior tree xml file to use')
declare_autostart_cmd = DeclareLaunchArgument(
'autostart', default_value='true',
description='Automatically startup the nav2 stack')
declare_use_remappings_cmd = DeclareLaunchArgument(
'use_remappings', default_value='false',
description='Arguments to pass to all nodes launched by the file')
# Specify the actions
bringup_cmd_group = GroupAction([
PushRosNamespace(
......@@ -56,10 +89,26 @@ def generate_launch_description():
namespace=namespace),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_localization_launch.py'))),
PythonLaunchDescriptionSource(os.path.join(launch_dir,
'nav2_localization_launch.py')),
launch_arguments={'namespace': namespace,
'map': map_yaml_file,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'use_lifecycle_mgr': 'false',
'use_remappings': use_remappings}.items()),
IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_navigation_launch.py'))),
PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_navigation_launch.py')),
launch_arguments={'namespace': namespace,
'use_sim_time': use_sim_time,
'autostart': autostart,
'params_file': params_file,
'bt_xml_file': bt_xml_file,
'use_lifecycle_mgr': 'false',
'use_remappings': use_remappings,
'map_subscribe_transient_local': 'true'}.items()),
])
# Create the launch description and populate
......@@ -71,6 +120,12 @@ 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_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_bt_xml_cmd)
ld.add_action(declare_use_remappings_cmd)
# Add the actions to launch all of the navigation nodes
ld.add_action(bringup_cmd_group)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment