diff --git a/launch/robot_navigation.launch.py b/launch/robot_navigation.launch.py index a5fb2a3..6e64d10 100644 --- a/launch/robot_navigation.launch.py +++ b/launch/robot_navigation.launch.py @@ -4,7 +4,7 @@ from launch_ros.actions import Node from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, ExecuteProcess, TimerAction +from launch.actions import IncludeLaunchDescription, TimerAction from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare @@ -13,6 +13,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction from launch.conditions import LaunchConfigurationEquals +from nav2_common.launch import ReplaceString + def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') @@ -28,8 +30,19 @@ def generate_launch_description(): default_value="True", description="Run using a premade map, no SLAM (True/False)", ) - - + + nav_to_pose_tree_arg = DeclareLaunchArgument( + "nav_to_pose_tree", + default_value="navigate_to_pose_w_replanning_and_recovery.xml", + description="Which Nav2 navigate_to_pose BT to use", + ) + + nav_through_poses_tree_arg = DeclareLaunchArgument( + "nav_through_poses_tree", + default_value="navigate_through_poses_w_replanning_and_recovery.xml", + description="Which Nav2 navigate_through_poses BT to use", + ) + # Get the package share directory pkg_mirte_navigation = get_package_share_directory('mirte_navigation') @@ -38,12 +51,33 @@ def generate_launch_description(): pkg_mirte_navigation, 'maps', 'robocup_map.yaml') - - params_file = os.path.join( + + nav2_params_file = os.path.join( pkg_mirte_navigation, 'params', 'mirte_nav2_params.yaml') + trees_path = os.path.join( + get_package_share_directory("mirte_navigation"), + "trees" + ) + + rewritten_nav2_params_file = ReplaceString( + source_file=nav2_params_file, + replacements={ + "": ( + trees_path, + os.path.sep, + LaunchConfiguration("nav_to_pose_tree"), + ), + "": ( + trees_path, + os.path.sep, + LaunchConfiguration("nav_through_poses_tree"), + ), + }, + ) + slam_params_file = os.path.join( pkg_mirte_navigation, 'params', @@ -64,7 +98,7 @@ def generate_launch_description(): ])), launch_arguments={ "map": map_file, - "params_file": params_file, + "params_file": rewritten_nav2_params_file, 'use_sim_time': use_sim_time, }.items() ) @@ -77,7 +111,7 @@ def generate_launch_description(): navigation_launch, IncludeLaunchDescription( AnyLaunchDescriptionSource(slam_tb_launch_path), - launch_arguments = { + launch_arguments={ 'slam_params_file': slam_params_file, 'use_sim_time': use_sim_time, }.items() @@ -86,7 +120,6 @@ def generate_launch_description(): condition=LaunchConfigurationEquals("use_map", "False"), ) - nav_and_localization = GroupAction( actions=[ localization_launch, @@ -98,6 +131,7 @@ def generate_launch_description(): rviz_file = PathJoinSubstitution([ FindPackageShare("nav2_bringup"), "rviz", "nav2_default_view.rviz" ]) + start_rviz_cmd = Node( package="rviz2", executable="rviz2", @@ -120,6 +154,8 @@ def generate_launch_description(): return LaunchDescription([ use_map_arg, use_sim_time_arg, + nav_to_pose_tree_arg, + nav_through_poses_tree_arg, initial_pose_node, nav_and_localization, nav2_with_slam, diff --git a/params/mirte_nav2_params.yaml b/params/mirte_nav2_params.yaml index bee4144..0827f0e 100644 --- a/params/mirte_nav2_params.yaml +++ b/params/mirte_nav2_params.yaml @@ -52,10 +52,8 @@ bt_navigator: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" navigate_through_poses: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" - # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml - # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml - # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + default_nav_to_pose_bt_xml: "" + default_nav_through_poses_bt_xml: "" # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings). # Built-in plugins are added automatically diff --git a/setup.py b/setup.py index dbbaa3e..29a213b 100644 --- a/setup.py +++ b/setup.py @@ -16,6 +16,8 @@ (os.path.join('share', package_name, 'launch'), glob('launch/*launch.py')), (os.path.join('share', package_name, 'params'), glob('params/*')), (os.path.join('share', package_name, 'maps'), glob('maps/*')), + (os.path.join('share', package_name, 'trees'), glob('trees/*')), + ], install_requires=['setuptools'], zip_safe=True, @@ -28,10 +30,5 @@ 'console_scripts': [ 'set_initial_pose = mirte_navigation.set_initial_pose: main', ], - }, ) - - - - diff --git a/trees/navigate_through_poses_w_replanning_and_recovery.xml b/trees/navigate_through_poses_w_replanning_and_recovery.xml new file mode 100644 index 0000000..1378863 --- /dev/null +++ b/trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/trees/navigate_to_pose_w_replanning_and_recovery.xml b/trees/navigate_to_pose_w_replanning_and_recovery.xml new file mode 100644 index 0000000..2396b84 --- /dev/null +++ b/trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +