From 60d2e612b3968234d6b80435b48a1c28b42c2ea9 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Tue, 11 Jun 2024 19:01:56 -0700
Subject: [PATCH 01/18] remove multirobot (#4421)

---
 .../cloned_multi_tb3_simulation_launch.py     | 233 -----------
 .../unique_multi_tb3_simulation_launch.py     | 256 ------------
 .../params/nav2_multirobot_params_1.yaml      | 269 -------------
 .../params/nav2_multirobot_params_2.yaml      | 268 -------------
 .../params/nav2_multirobot_params_all.yaml    | 330 ---------------
 nav2_bringup/rviz/nav2_namespaced_view.rviz   | 378 ------------------
 6 files changed, 1734 deletions(-)
 delete mode 100644 nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
 delete mode 100644 nav2_bringup/launch/unique_multi_tb3_simulation_launch.py
 delete mode 100644 nav2_bringup/params/nav2_multirobot_params_1.yaml
 delete mode 100644 nav2_bringup/params/nav2_multirobot_params_2.yaml
 delete mode 100644 nav2_bringup/params/nav2_multirobot_params_all.yaml
 delete mode 100644 nav2_bringup/rviz/nav2_namespaced_view.rviz

diff --git a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
deleted file mode 100644
index 54f9d60f80..0000000000
--- a/nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py
+++ /dev/null
@@ -1,233 +0,0 @@
-# Copyright (c) 2023 LG Electronics.
-# Copyright (c) 2024 Open Navigation LLC
-#
-# 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 pathlib import Path
-import tempfile
-
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    GroupAction,
-    IncludeLaunchDescription,
-    LogInfo,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnShutdown
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, TextSubstitution
-from nav2_common.launch import ParseMultiRobotPose
-
-
-def generate_launch_description():
-    """
-    Bring up the multi-robots with given launch arguments.
-
-    Launch arguments consist of robot name(which is namespace) and pose for initialization.
-    Keep general yaml format for pose information.
-    ex) robots:='robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}'
-    ex) robots:='robot3={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707};
-                 robot4={x: 1.0, y: 1.0, z: 1.0, roll: 0.0, pitch: 1.5707, yaw: 1.5707}'
-    """
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('nav2_bringup')
-    launch_dir = os.path.join(bringup_dir, 'launch')
-    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
-
-    # Simulation settings
-    world = LaunchConfiguration('world')
-
-    # On this example all robots are launched with the same settings
-    map_yaml_file = LaunchConfiguration('map')
-    params_file = LaunchConfiguration('params_file')
-    autostart = LaunchConfiguration('autostart')
-    rviz_config_file = LaunchConfiguration('rviz_config')
-    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
-    use_rviz = LaunchConfiguration('use_rviz')
-    log_settings = LaunchConfiguration('log_settings', default='true')
-
-    # Declare the launch arguments
-    declare_world_cmd = DeclareLaunchArgument(
-        'world',
-        default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'),
-        description='Full path to world file to load',
-    )
-
-    declare_map_yaml_cmd = DeclareLaunchArgument(
-        'map',
-        default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
-        description='Full path to map file to load',
-    )
-
-    declare_params_file_cmd = DeclareLaunchArgument(
-        'params_file',
-        default_value=os.path.join(
-            bringup_dir, 'params', 'nav2_multirobot_params_all.yaml'
-        ),
-        description='Full path to the ROS2 parameters file to use for all launched nodes',
-    )
-
-    declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart',
-        default_value='false',
-        description='Automatically startup the stacks',
-    )
-
-    declare_rviz_config_file_cmd = DeclareLaunchArgument(
-        'rviz_config',
-        default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'),
-        description='Full path to the RVIZ config file to use.',
-    )
-
-    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-        'use_robot_state_pub',
-        default_value='True',
-        description='Whether to start the robot state publisher',
-    )
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        'use_rviz', default_value='True', description='Whether to start RVIZ'
-    )
-
-    # Start Gazebo with plugin providing the robot spawning service
-    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
-    world_sdf_xacro = ExecuteProcess(
-        cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world])
-    start_gazebo_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
-                         'gz_sim.launch.py')),
-        launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
-
-    remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
-        on_shutdown=[
-            OpaqueFunction(function=lambda _: os.remove(world_sdf))
-        ]))
-
-    robots_list = ParseMultiRobotPose('robots').value()
-
-    # Define commands for launching the navigation instances
-    bringup_cmd_group = []
-    for robot_name in robots_list:
-        init_pose = robots_list[robot_name]
-        group = GroupAction(
-            [
-                LogInfo(
-                    msg=[
-                        'Launching namespace=',
-                        robot_name,
-                        ' init_pose=',
-                        str(init_pose),
-                    ]
-                ),
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        os.path.join(launch_dir, 'rviz_launch.py')
-                    ),
-                    condition=IfCondition(use_rviz),
-                    launch_arguments={
-                        'namespace': TextSubstitution(text=robot_name),
-                        'use_namespace': 'True',
-                        'rviz_config': rviz_config_file,
-                    }.items(),
-                ),
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
-                    ),
-                    launch_arguments={
-                        'namespace': robot_name,
-                        'use_namespace': 'True',
-                        'map': map_yaml_file,
-                        'use_sim_time': 'True',
-                        'params_file': params_file,
-                        'autostart': autostart,
-                        'use_rviz': 'False',
-                        'use_simulator': 'False',
-                        'headless': 'False',
-                        'use_robot_state_pub': use_robot_state_pub,
-                        'x_pose': TextSubstitution(text=str(init_pose['x'])),
-                        'y_pose': TextSubstitution(text=str(init_pose['y'])),
-                        'z_pose': TextSubstitution(text=str(init_pose['z'])),
-                        'roll': TextSubstitution(text=str(init_pose['roll'])),
-                        'pitch': TextSubstitution(text=str(init_pose['pitch'])),
-                        'yaw': TextSubstitution(text=str(init_pose['yaw'])),
-                        'robot_name': TextSubstitution(text=robot_name),
-                    }.items(),
-                ),
-            ]
-        )
-
-        bringup_cmd_group.append(group)
-
-    set_env_vars_resources = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models'))
-    set_env_vars_resources2 = AppendEnvironmentVariable(
-            'GZ_SIM_RESOURCE_PATH',
-            str(Path(os.path.join(sim_dir)).parent.resolve()))
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-    ld.add_action(set_env_vars_resources)
-    ld.add_action(set_env_vars_resources2)
-
-    # Declare the launch options
-    ld.add_action(declare_world_cmd)
-    ld.add_action(declare_map_yaml_cmd)
-    ld.add_action(declare_params_file_cmd)
-    ld.add_action(declare_use_rviz_cmd)
-    ld.add_action(declare_autostart_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
-    ld.add_action(declare_use_robot_state_pub_cmd)
-
-    # Add the actions to start gazebo, robots and simulations
-    ld.add_action(world_sdf_xacro)
-    ld.add_action(start_gazebo_cmd)
-    ld.add_action(remove_temp_sdf_file)
-
-    ld.add_action(LogInfo(msg=['number_of_robots=', str(len(robots_list))]))
-
-    ld.add_action(
-        LogInfo(condition=IfCondition(log_settings), msg=['map yaml: ', map_yaml_file])
-    )
-    ld.add_action(
-        LogInfo(condition=IfCondition(log_settings), msg=['params yaml: ', params_file])
-    )
-    ld.add_action(
-        LogInfo(
-            condition=IfCondition(log_settings),
-            msg=['rviz config file: ', rviz_config_file],
-        )
-    )
-    ld.add_action(
-        LogInfo(
-            condition=IfCondition(log_settings),
-            msg=['using robot state pub: ', use_robot_state_pub],
-        )
-    )
-    ld.add_action(
-        LogInfo(condition=IfCondition(log_settings), msg=['autostart: ', autostart])
-    )
-
-    for cmd in bringup_cmd_group:
-        ld.add_action(cmd)
-
-    return ld
diff --git a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py b/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py
deleted file mode 100644
index 7eaaf1a97d..0000000000
--- a/nav2_bringup/launch/unique_multi_tb3_simulation_launch.py
+++ /dev/null
@@ -1,256 +0,0 @@
-# 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.
-
-"""
-Example for spawning multiple robots in Gazebo.
-
-This is an example on how to create a launch file for spawning multiple robots into Gazebo
-and launch multiple instances of the navigation stack, each controlling one robot.
-The robots co-exist on a shared environment and are controlled by independent nav stacks.
-"""
-
-import os
-from pathlib import Path
-import tempfile
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    ExecuteProcess,
-    GroupAction,
-    IncludeLaunchDescription,
-    LogInfo,
-    OpaqueFunction,
-    RegisterEventHandler,
-)
-from launch.conditions import IfCondition
-from launch.event_handlers import OnShutdown
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, TextSubstitution
-
-
-def generate_launch_description():
-    # Get the launch directory
-    bringup_dir = get_package_share_directory('nav2_bringup')
-    launch_dir = os.path.join(bringup_dir, 'launch')
-    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
-
-    # Names and poses of the robots
-    robots = [
-        {
-            'name': 'robot1',
-            'x_pose': 0.0,
-            'y_pose': 0.5,
-            'z_pose': 0.01,
-            'roll': 0.0,
-            'pitch': 0.0,
-            'yaw': 0.0,
-        },
-        {
-            'name': 'robot2',
-            'x_pose': 0.0,
-            'y_pose': -0.5,
-            'z_pose': 0.01,
-            'roll': 0.0,
-            'pitch': 0.0,
-            'yaw': 0.0,
-        },
-    ]
-
-    # Simulation settings
-    world = LaunchConfiguration('world')
-
-    # On this example all robots are launched with the same settings
-    map_yaml_file = LaunchConfiguration('map')
-
-    autostart = LaunchConfiguration('autostart')
-    rviz_config_file = LaunchConfiguration('rviz_config')
-    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
-    use_rviz = LaunchConfiguration('use_rviz')
-    log_settings = LaunchConfiguration('log_settings', default='true')
-
-    # Declare the launch arguments
-    declare_world_cmd = DeclareLaunchArgument(
-        'world',
-        default_value=os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro'),
-        description='Full path to world file to load',
-    )
-
-    declare_map_yaml_cmd = DeclareLaunchArgument(
-        'map',
-        default_value=os.path.join(bringup_dir, 'maps', 'tb3_sandbox.yaml'),
-        description='Full path to map file to load',
-    )
-
-    declare_robot1_params_file_cmd = DeclareLaunchArgument(
-        'robot1_params_file',
-        default_value=os.path.join(
-            bringup_dir, 'params', 'nav2_multirobot_params_1.yaml'
-        ),
-        description='Full path to the ROS2 parameters file to use for robot1 launched nodes',
-    )
-
-    declare_robot2_params_file_cmd = DeclareLaunchArgument(
-        'robot2_params_file',
-        default_value=os.path.join(
-            bringup_dir, 'params', 'nav2_multirobot_params_2.yaml'
-        ),
-        description='Full path to the ROS2 parameters file to use for robot2 launched nodes',
-    )
-
-    declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart',
-        default_value='false',
-        description='Automatically startup the stacks',
-    )
-
-    declare_rviz_config_file_cmd = DeclareLaunchArgument(
-        'rviz_config',
-        default_value=os.path.join(bringup_dir, 'rviz', 'nav2_namespaced_view.rviz'),
-        description='Full path to the RVIZ config file to use.',
-    )
-
-    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-        'use_robot_state_pub',
-        default_value='True',
-        description='Whether to start the robot state publisher',
-    )
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        'use_rviz', default_value='True', description='Whether to start RVIZ'
-    )
-
-    # Start Gazebo with plugin providing the robot spawning service
-    world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf')
-    world_sdf_xacro = ExecuteProcess(
-        cmd=['xacro', '-o', world_sdf, ['headless:=', 'False'], world])
-    start_gazebo_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch',
-                         'gz_sim.launch.py')),
-        launch_arguments={'gz_args': ['-r -s ', world_sdf]}.items())
-
-    remove_temp_sdf_file = RegisterEventHandler(event_handler=OnShutdown(
-        on_shutdown=[
-            OpaqueFunction(function=lambda _: os.remove(world_sdf))
-        ]))
-
-    # Define commands for launching the navigation instances
-    nav_instances_cmds = []
-    for robot in robots:
-        params_file = LaunchConfiguration(f"{robot['name']}_params_file")
-
-        group = GroupAction(
-            [
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        os.path.join(launch_dir, 'rviz_launch.py')
-                    ),
-                    condition=IfCondition(use_rviz),
-                    launch_arguments={
-                        'namespace': TextSubstitution(text=robot['name']),
-                        'use_namespace': 'True',
-                        'rviz_config': rviz_config_file,
-                    }.items(),
-                ),
-                IncludeLaunchDescription(
-                    PythonLaunchDescriptionSource(
-                        os.path.join(bringup_dir, 'launch', 'tb3_simulation_launch.py')
-                    ),
-                    launch_arguments={
-                        'namespace': robot['name'],
-                        'use_namespace': 'True',
-                        'map': map_yaml_file,
-                        'use_sim_time': 'True',
-                        'params_file': params_file,
-                        'autostart': autostart,
-                        'use_rviz': 'False',
-                        'use_simulator': 'False',
-                        'headless': 'False',
-                        'use_robot_state_pub': use_robot_state_pub,
-                        'x_pose': TextSubstitution(text=str(robot['x_pose'])),
-                        'y_pose': TextSubstitution(text=str(robot['y_pose'])),
-                        'z_pose': TextSubstitution(text=str(robot['z_pose'])),
-                        'roll': TextSubstitution(text=str(robot['roll'])),
-                        'pitch': TextSubstitution(text=str(robot['pitch'])),
-                        'yaw': TextSubstitution(text=str(robot['yaw'])),
-                        'robot_name': TextSubstitution(text=robot['name']),
-                    }.items(),
-                ),
-                LogInfo(
-                    condition=IfCondition(log_settings),
-                    msg=['Launching ', robot['name']],
-                ),
-                LogInfo(
-                    condition=IfCondition(log_settings),
-                    msg=[robot['name'], ' map yaml: ', map_yaml_file],
-                ),
-                LogInfo(
-                    condition=IfCondition(log_settings),
-                    msg=[robot['name'], ' params yaml: ', params_file],
-                ),
-                LogInfo(
-                    condition=IfCondition(log_settings),
-                    msg=[robot['name'], ' rviz config file: ', rviz_config_file],
-                ),
-                LogInfo(
-                    condition=IfCondition(log_settings),
-                    msg=[
-                        robot['name'],
-                        ' using robot state pub: ',
-                        use_robot_state_pub,
-                    ],
-                ),
-                LogInfo(
-                    condition=IfCondition(log_settings),
-                    msg=[robot['name'], ' autostart: ', autostart],
-                ),
-            ]
-        )
-
-        nav_instances_cmds.append(group)
-
-    set_env_vars_resources = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models'))
-    set_env_vars_resources2 = AppendEnvironmentVariable(
-            'GZ_SIM_RESOURCE_PATH',
-            str(Path(os.path.join(sim_dir)).parent.resolve()))
-
-    # Create the launch description and populate
-    ld = LaunchDescription()
-    ld.add_action(set_env_vars_resources)
-    ld.add_action(set_env_vars_resources2)
-
-    # Declare the launch options
-    ld.add_action(declare_world_cmd)
-    ld.add_action(declare_map_yaml_cmd)
-    ld.add_action(declare_robot1_params_file_cmd)
-    ld.add_action(declare_robot2_params_file_cmd)
-    ld.add_action(declare_use_rviz_cmd)
-    ld.add_action(declare_autostart_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
-    ld.add_action(declare_use_robot_state_pub_cmd)
-
-    # Add the actions to start gazebo, robots and simulations
-    ld.add_action(world_sdf_xacro)
-    ld.add_action(start_gazebo_cmd)
-    ld.add_action(remove_temp_sdf_file)
-
-    for simulation_instance_cmd in nav_instances_cmds:
-        ld.add_action(simulation_instance_cmd)
-
-    return ld
diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
deleted file mode 100644
index a62d1221c2..0000000000
--- a/nav2_bringup/params/nav2_multirobot_params_1.yaml
+++ /dev/null
@@ -1,269 +0,0 @@
-amcl:
-  ros__parameters:
-    alpha1: 0.2
-    alpha2: 0.2
-    alpha3: 0.2
-    alpha4: 0.2
-    alpha5: 0.2
-    base_frame_id: "base_footprint"
-    beam_skip_distance: 0.5
-    beam_skip_error_threshold: 0.9
-    beam_skip_threshold: 0.3
-    do_beamskip: false
-    global_frame_id: "map"
-    lambda_short: 0.1
-    laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
-    laser_model_type: "likelihood_field"
-    max_beams: 60
-    max_particles: 2000
-    min_particles: 500
-    odom_frame_id: "odom"
-    pf_err: 0.05
-    pf_z: 0.99
-    recovery_alpha_fast: 0.0
-    recovery_alpha_slow: 0.0
-    resample_interval: 1
-    robot_model_type: "nav2_amcl::DifferentialMotionModel"
-    save_pose_rate: 0.5
-    sigma_hit: 0.2
-    tf_broadcast: true
-    transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
-    z_max: 0.05
-    z_rand: 0.5
-    z_short: 0.05
-    scan_topic: scan
-
-bt_navigator:
-  ros__parameters:
-    global_frame: map
-    robot_base_frame: base_link
-    odom_topic: /odom
-    bt_loop_duration: 10
-    default_server_timeout: 20
-    wait_for_service_timeout: 1000
-    action_server_result_timeout: 900.0
-    navigators: ["navigate_to_pose", "navigate_through_poses"]
-    navigate_to_pose:
-      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.
-    
-    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
-    # Built-in plugins are added automatically
-    # plugin_lib_names: []
-
-    error_code_names:
-      - compute_path_error_code
-      - follow_path_error_code
-
-controller_server:
-  ros__parameters:
-    controller_frequency: 20.0
-    min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
-    min_theta_velocity_threshold: 0.001
-    progress_checker_plugins: ["progress_checker"]
-    goal_checker_plugins: ["goal_checker"]
-    controller_plugins: ["FollowPath"]
-
-    # Progress checker parameters
-    progress_checker:
-      plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
-      movement_time_allowance: 10.0
-    # Goal checker parameters
-    goal_checker:
-      plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
-      stateful: True
-    # DWB parameters
-    FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 0.0
-      GoalAlign.scale: 0.0
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
-
-local_costmap:
-  local_costmap:
-    ros__parameters:
-      global_frame: odom
-      rolling_window: true
-      width: 3
-      height: 3
-      resolution: 0.05
-      robot_radius: 0.22
-      plugins: ["voxel_layer", "inflation_layer"]
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: False
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan
-        scan:
-          topic: /robot1/scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
-
-global_costmap:
-  global_costmap:
-    ros__parameters:
-      robot_radius: 0.22
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
-      obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
-        enabled: True
-        observation_sources: scan
-        scan:
-          topic: /robot1/scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55
-      always_send_full_costmap: True
-
-map_server:
-  ros__parameters:
-    yaml_filename: "tb3_sandbox.yaml"
-    save_map_timeout: 5.0
-
-planner_server:
-  ros__parameters:
-    planner_plugins: ["GridBased"]
-    GridBased:
-      plugin: "nav2_navfn_planner::NavfnPlanner"
-      tolerance: 0.5
-      use_astar: false
-      allow_unknown: true
-
-behavior_server:
-  ros__parameters:
-    costmap_topic: local_costmap/costmap_raw
-    footprint_topic: local_costmap/published_footprint
-    cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
-    spin:
-      plugin: "nav2_behaviors::Spin"
-    backup:
-      plugin: "nav2_behaviors::BackUp"
-    drive_on_heading:
-      plugin: "nav2_behaviors::DriveOnHeading"
-    wait:
-      plugin: "nav2_behaviors::Wait"
-    global_frame: odom
-    robot_base_frame: base_link
-    transform_tolerance: 0.1
-    simulate_ahead_time: 2.0
-    max_rotational_vel: 1.0
-    min_rotational_vel: 0.4
-    rotational_acc_lim: 3.2
-
-waypoint_follower:
-  ros__parameters:
-    loop_rate: 20
-    stop_on_failure: false
-    action_server_result_timeout: 900.0
-    waypoint_task_executor_plugin: "wait_at_waypoint"
-    wait_at_waypoint:
-      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
-      enabled: True
-      waypoint_pause_duration: 200
-
-collision_monitor:
-  ros__parameters:
-    base_frame_id: "base_footprint"
-    odom_frame_id: "odom"
-    cmd_vel_in_topic: "cmd_vel_smoothed"
-    cmd_vel_out_topic: "cmd_vel"
-    state_topic: "collision_monitor_state"
-    transform_tolerance: 0.2
-    source_timeout: 1.0
-    base_shift_correction: True
-    stop_pub_timeout: 2.0
-    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
-    # and robot footprint for "approach" action type.
-    polygons: ["FootprintApproach"]
-    FootprintApproach:
-      type: "polygon"
-      action_type: "approach"
-      footprint_topic: "local_costmap/published_footprint"
-      time_before_collision: 2.0
-      simulation_time_step: 0.1
-      min_points: 6
-      visualize: False
-      enabled: True
-    observation_sources: ["scan"]
-    scan:
-      type: "scan"
-      topic: "/robot1/scan"
-      min_height: 0.15
-      max_height: 2.0
-      enabled: True
diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
deleted file mode 100644
index d2befc70f5..0000000000
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ /dev/null
@@ -1,268 +0,0 @@
-amcl:
-  ros__parameters:
-    alpha1: 0.2
-    alpha2: 0.2
-    alpha3: 0.2
-    alpha4: 0.2
-    alpha5: 0.2
-    base_frame_id: "base_footprint"
-    beam_skip_distance: 0.5
-    beam_skip_error_threshold: 0.9
-    beam_skip_threshold: 0.3
-    do_beamskip: false
-    global_frame_id: "map"
-    lambda_short: 0.1
-    laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
-    laser_model_type: "likelihood_field"
-    max_beams: 60
-    max_particles: 2000
-    min_particles: 500
-    odom_frame_id: "odom"
-    pf_err: 0.05
-    pf_z: 0.99
-    recovery_alpha_fast: 0.0
-    recovery_alpha_slow: 0.0
-    resample_interval: 1
-    robot_model_type: "nav2_amcl::DifferentialMotionModel"
-    save_pose_rate: 0.5
-    sigma_hit: 0.2
-    tf_broadcast: true
-    transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
-    z_max: 0.05
-    z_rand: 0.5
-    z_short: 0.05
-    scan_topic: scan
-
-bt_navigator:
-  ros__parameters:
-    global_frame: map
-    robot_base_frame: base_link
-    odom_topic: /odom
-    bt_loop_duration: 10
-    default_server_timeout: 20
-    wait_for_service_timeout: 1000
-    action_server_result_timeout: 900.0
-    navigators: ["navigate_to_pose", "navigate_through_poses"]
-    navigate_to_pose:
-      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.
-
-    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
-    # Built-in plugins are added automatically
-    # plugin_lib_names: []
-
-    error_code_names:
-      - compute_path_error_code
-      - follow_path_error_code
-
-controller_server:
-  ros__parameters:
-    controller_frequency: 20.0
-    min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
-    min_theta_velocity_threshold: 0.001
-    progress_checker_plugins: ["progress_checker"]
-    goal_checker_plugins: ["goal_checker"]
-    controller_plugins: ["FollowPath"]
-
-    # Progress checker parameters
-    progress_checker:
-      plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
-      movement_time_allowance: 10.0
-    # Goal checker parameters
-    goal_checker:
-      plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
-      stateful: True
-    # DWB parameters
-    FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 0.0
-      GoalAlign.scale: 0.0
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
-
-local_costmap:
-  local_costmap:
-    ros__parameters:
-      global_frame: odom
-      rolling_window: true
-      width: 3
-      height: 3
-      resolution: 0.05
-      robot_radius: 0.22
-      plugins: ["voxel_layer", "inflation_layer"]
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: False
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan
-        scan:
-          topic: /robot2/scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
-
-global_costmap:
-  global_costmap:
-    ros__parameters:
-      robot_radius: 0.22
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
-      obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
-        enabled: True
-        observation_sources: scan
-        scan:
-          topic: /robot2/scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55
-      always_send_full_costmap: True
-
-map_server:
-  ros__parameters:
-    yaml_filename: "tb3_sandbox.yaml"
-    save_map_timeout: 5.0
-
-planner_server:
-  ros__parameters:
-    planner_plugins: ["GridBased"]
-    GridBased:
-      plugin: "nav2_navfn_planner::NavfnPlanner"
-      tolerance: 0.5
-      use_astar: false
-      allow_unknown: true
-
-behavior_server:
-  ros__parameters:
-    costmap_topic: local_costmap/costmap_raw
-    footprint_topic: local_costmap/published_footprint
-    cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"]
-    spin:
-      plugin: "nav2_behaviors::Spin"
-    backup:
-      plugin: "nav2_behaviors::BackUp"
-    drive_on_heading:
-      plugin: "nav2_behaviors::DriveOnHeading"
-    wait:
-      plugin: "nav2_behaviors::Wait"
-    global_frame: odom
-    robot_base_frame: base_link
-    transform_tolerance: 0.1
-    simulate_ahead_time: 2.0
-    max_rotational_vel: 1.0
-    min_rotational_vel: 0.4
-    rotational_acc_lim: 3.2
-
-waypoint_follower:
-  ros__parameters:
-    loop_rate: 20
-    stop_on_failure: false
-    action_server_result_timeout: 900.0
-    waypoint_task_executor_plugin: "wait_at_waypoint"
-    wait_at_waypoint:
-      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
-      enabled: True
-      waypoint_pause_duration: 200
-
-collision_monitor:
-  ros__parameters:
-    base_frame_id: "base_footprint"
-    odom_frame_id: "odom"
-    cmd_vel_in_topic: "cmd_vel_smoothed"
-    cmd_vel_out_topic: "cmd_vel"
-    state_topic: "collision_monitor_state"
-    transform_tolerance: 0.2
-    source_timeout: 1.0
-    base_shift_correction: True
-    stop_pub_timeout: 2.0
-    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
-    # and robot footprint for "approach" action type.
-    polygons: ["FootprintApproach"]
-    FootprintApproach:
-      type: "polygon"
-      action_type: "approach"
-      footprint_topic: "local_costmap/published_footprint"
-      time_before_collision: 2.0
-      simulation_time_step: 0.1
-      min_points: 6
-      visualize: False
-      enabled: True
-    observation_sources: ["scan"]
-    scan:
-      type: "scan"
-      topic: "/robot2/scan"
-      min_height: 0.15
-      max_height: 2.0
-      enabled: True
diff --git a/nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_bringup/params/nav2_multirobot_params_all.yaml
deleted file mode 100644
index b5960cbbd4..0000000000
--- a/nav2_bringup/params/nav2_multirobot_params_all.yaml
+++ /dev/null
@@ -1,330 +0,0 @@
-amcl:
-  ros__parameters:
-    alpha1: 0.2
-    alpha2: 0.2
-    alpha3: 0.2
-    alpha4: 0.2
-    alpha5: 0.2
-    base_frame_id: "base_footprint"
-    beam_skip_distance: 0.5
-    beam_skip_error_threshold: 0.9
-    beam_skip_threshold: 0.3
-    do_beamskip: false
-    global_frame_id: "map"
-    lambda_short: 0.1
-    laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
-    laser_min_range: -1.0
-    laser_model_type: "likelihood_field"
-    max_beams: 60
-    max_particles: 2000
-    min_particles: 500
-    odom_frame_id: "odom"
-    pf_err: 0.05
-    pf_z: 0.99
-    recovery_alpha_fast: 0.0
-    recovery_alpha_slow: 0.0
-    resample_interval: 1
-    robot_model_type: "nav2_amcl::DifferentialMotionModel"
-    save_pose_rate: 0.5
-    sigma_hit: 0.2
-    tf_broadcast: true
-    transform_tolerance: 1.0
-    update_min_a: 0.2
-    update_min_d: 0.25
-    z_hit: 0.5
-    z_max: 0.05
-    z_rand: 0.5
-    z_short: 0.05
-    scan_topic: scan
-
-bt_navigator:
-  ros__parameters:
-    global_frame: map
-    robot_base_frame: base_link
-    odom_topic: /odom
-    bt_loop_duration: 10
-    default_server_timeout: 20
-    wait_for_service_timeout: 1000
-    action_server_result_timeout: 900.0
-    navigators: ["navigate_to_pose", "navigate_through_poses"]
-    navigate_to_pose:
-      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.
-    
-    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
-    # Built-in plugins are added automatically
-    # plugin_lib_names: []
-
-    error_code_names:
-      - compute_path_error_code
-      - follow_path_error_code
-
-controller_server:
-  ros__parameters:
-    controller_frequency: 20.0
-    min_x_velocity_threshold: 0.001
-    min_y_velocity_threshold: 0.5
-    min_theta_velocity_threshold: 0.001
-    failure_tolerance: 0.3
-    progress_checker_plugins: ["progress_checker"]
-    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
-    controller_plugins: ["FollowPath"]
-
-    # Progress checker parameters
-    progress_checker:
-      plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
-      movement_time_allowance: 10.0
-    # Goal checker parameters
-    #precise_goal_checker:
-    #  plugin: "nav2_controller::SimpleGoalChecker"
-    #  xy_goal_tolerance: 0.25
-    #  yaw_goal_tolerance: 0.25
-    #  stateful: True
-    general_goal_checker:
-      stateful: True
-      plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.25
-      yaw_goal_tolerance: 0.25
-    # DWB parameters
-    FollowPath:
-      plugin: "dwb_core::DWBLocalPlanner"
-      debug_trajectory_details: True
-      min_vel_x: 0.0
-      min_vel_y: 0.0
-      max_vel_x: 0.26
-      max_vel_y: 0.0
-      max_vel_theta: 1.0
-      min_speed_xy: 0.0
-      max_speed_xy: 0.26
-      min_speed_theta: 0.0
-      acc_lim_x: 2.5
-      acc_lim_y: 0.0
-      acc_lim_theta: 3.2
-      decel_lim_x: -2.5
-      decel_lim_y: 0.0
-      decel_lim_theta: -3.2
-      vx_samples: 20
-      vy_samples: 5
-      vtheta_samples: 20
-      sim_time: 1.7
-      linear_granularity: 0.05
-      angular_granularity: 0.025
-      transform_tolerance: 0.2
-      xy_goal_tolerance: 0.25
-      trans_stopped_velocity: 0.25
-      short_circuit_trajectory_evaluation: True
-      stateful: True
-      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
-      BaseObstacle.scale: 0.02
-      PathAlign.scale: 32.0
-      PathAlign.forward_point_distance: 0.1
-      GoalAlign.scale: 24.0
-      GoalAlign.forward_point_distance: 0.1
-      PathDist.scale: 32.0
-      GoalDist.scale: 24.0
-      RotateToGoal.scale: 32.0
-      RotateToGoal.slowing_factor: 5.0
-      RotateToGoal.lookahead_time: -1.0
-
-local_costmap:
-  local_costmap:
-    ros__parameters:
-      update_frequency: 5.0
-      publish_frequency: 2.0
-      global_frame: odom
-      robot_base_frame: base_link
-      rolling_window: true
-      width: 3
-      height: 3
-      resolution: 0.05
-      robot_radius: 0.22
-      plugins: ["voxel_layer", "inflation_layer"]
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55
-      voxel_layer:
-        plugin: "nav2_costmap_2d::VoxelLayer"
-        enabled: True
-        publish_voxel_map: True
-        origin_z: 0.0
-        z_resolution: 0.05
-        z_voxels: 16
-        max_obstacle_height: 2.0
-        mark_threshold: 0
-        observation_sources: scan
-        scan:
-          # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
-          # It doesn't need to start with '/'
-          topic: <robot_namespace>/scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
-      always_send_full_costmap: True
-
-global_costmap:
-  global_costmap:
-    ros__parameters:
-      update_frequency: 1.0
-      publish_frequency: 1.0
-      global_frame: map
-      robot_base_frame: base_link
-      robot_radius: 0.22
-      resolution: 0.05
-      track_unknown_space: true
-      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
-      obstacle_layer:
-        plugin: "nav2_costmap_2d::ObstacleLayer"
-        enabled: True
-        observation_sources: scan
-        scan:
-          # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
-          # It doesn't need to start with '/'
-          topic: <robot_namespace>/scan
-          max_obstacle_height: 2.0
-          clearing: True
-          marking: True
-          data_type: "LaserScan"
-          raytrace_max_range: 3.0
-          raytrace_min_range: 0.0
-          obstacle_max_range: 2.5
-          obstacle_min_range: 0.0
-      static_layer:
-        plugin: "nav2_costmap_2d::StaticLayer"
-        map_subscribe_transient_local: True
-      inflation_layer:
-        plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.55
-      always_send_full_costmap: True
-
-# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
-# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
-# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
-# map_server:
-#   ros__parameters:
-#     yaml_filename: ""
-
-map_saver:
-  ros__parameters:
-    save_map_timeout: 5.0
-    free_thresh_default: 0.25
-    occupied_thresh_default: 0.65
-    map_subscribe_transient_local: True
-
-planner_server:
-  ros__parameters:
-    expected_planner_frequency: 20.0
-    planner_plugins: ["GridBased"]
-    GridBased:
-      plugin: "nav2_navfn_planner::NavfnPlanner"
-      tolerance: 0.5
-      use_astar: false
-      allow_unknown: true
-
-smoother_server:
-  ros__parameters:
-    smoother_plugins: ["simple_smoother"]
-    simple_smoother:
-      plugin: "nav2_smoother::SimpleSmoother"
-      tolerance: 1.0e-10
-      max_its: 1000
-      do_refinement: True
-
-behavior_server:
-  ros__parameters:
-    local_costmap_topic: local_costmap/costmap_raw
-    global_costmap_topic: global_costmap/costmap_raw
-    local_footprint_topic: local_costmap/published_footprint
-    global_footprint_topic: global_costmap/published_footprint
-    cycle_frequency: 10.0
-    behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
-    spin:
-      plugin: "nav2_behaviors::Spin"
-    backup:
-      plugin: "nav2_behaviors::BackUp"
-    drive_on_heading:
-      plugin: "nav2_behaviors::DriveOnHeading"
-    wait:
-      plugin: "nav2_behaviors::Wait"
-    assisted_teleop:
-      plugin: "nav2_behaviors::AssistedTeleop"
-    local_frame: odom
-    global_frame: map
-    robot_base_frame: base_link
-    transform_tolerance: 0.1
-    simulate_ahead_time: 2.0
-    max_rotational_vel: 1.0
-    min_rotational_vel: 0.4
-    rotational_acc_lim: 3.2
-
-waypoint_follower:
-  ros__parameters:
-    loop_rate: 20
-    stop_on_failure: false
-    action_server_result_timeout: 900.0
-    waypoint_task_executor_plugin: "wait_at_waypoint"
-    wait_at_waypoint:
-      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
-      enabled: True
-      waypoint_pause_duration: 200
-
-velocity_smoother:
-  ros__parameters:
-    smoothing_frequency: 20.0
-    scale_velocities: False
-    feedback: "OPEN_LOOP"
-    max_velocity: [0.26, 0.0, 1.0]
-    min_velocity: [-0.26, 0.0, -1.0]
-    max_accel: [2.5, 0.0, 3.2]
-    max_decel: [-2.5, 0.0, -3.2]
-    odom_topic: "odom"
-    odom_duration: 0.1
-    deadband_velocity: [0.0, 0.0, 0.0]
-    velocity_timeout: 1.0
-
-collision_monitor:
-  ros__parameters:
-    base_frame_id: "base_footprint"
-    odom_frame_id: "odom"
-    cmd_vel_in_topic: "cmd_vel_smoothed"
-    cmd_vel_out_topic: "cmd_vel"
-    state_topic: "collision_monitor_state"
-    transform_tolerance: 0.2
-    source_timeout: 1.0
-    base_shift_correction: True
-    stop_pub_timeout: 2.0
-    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
-    # and robot footprint for "approach" action type.
-    polygons: ["FootprintApproach"]
-    FootprintApproach:
-      type: "polygon"
-      action_type: "approach"
-      footprint_topic: "/local_costmap/published_footprint"
-      time_before_collision: 2.0
-      simulation_time_step: 0.1
-      min_points: 6
-      visualize: False
-      enabled: True
-    observation_sources: ["scan"]
-    scan:
-      type: "scan"
-      topic: "<robot_namespace>/scan"
-      min_height: 0.15
-      max_height: 2.0
-      enabled: True
diff --git a/nav2_bringup/rviz/nav2_namespaced_view.rviz b/nav2_bringup/rviz/nav2_namespaced_view.rviz
deleted file mode 100644
index 9f70030775..0000000000
--- a/nav2_bringup/rviz/nav2_namespaced_view.rviz
+++ /dev/null
@@ -1,378 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 195
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /RobotModel1/Status1
-        - /TF1/Frames1
-        - /TF1/Tree1
-        - /Global Planner1/Global Costmap1
-      Splitter Ratio: 0.5833333134651184
-    Tree Height: 464
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-  - Class: nav2_rviz_plugins/Navigation 2
-    Name: Navigation 2
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: <robot_namespace>/robot_description
-      Enabled: false
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: false
-      Tree:
-        {}
-      Update Interval: 0
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/LaserScan
-      Color: 255; 255; 255
-      Color Transformer: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 0
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: LaserScan
-      Position Transformer: XYZ
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Best Effort
-        Value: <robot_namespace>/scan
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz_default_plugins/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: ""
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: Bumper Hit
-      Position Transformer: ""
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.07999999821186066
-      Style: Spheres
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Best Effort
-        Value: <robot_namespace>/mobile_base/sensors/bumper_pointcloud
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/Map
-      Color Scheme: map
-      Draw Behind: true
-      Enabled: true
-      Name: Map
-      Topic:
-        Depth: 1
-        Durability Policy: Transient Local
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: <robot_namespace>/map
-      Use Timestamp: false
-      Value: true
-    - Alpha: 1
-      Class: nav2_rviz_plugins/ParticleCloud
-      Color: 0; 180; 0
-      Enabled: true
-      Max Arrow Length: 0.3
-      Min Arrow Length: 0.02
-      Name: Amcl Particle Swarm
-      Shape: Arrow (Flat)
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Best Effort
-        Value: <robot_namespace>/particle_cloud
-      Value: true
-    - Class: rviz_common/Group
-      Displays:
-        - Alpha: 0.30000001192092896
-          Class: rviz_default_plugins/Map
-          Color Scheme: costmap
-          Draw Behind: false
-          Enabled: true
-          Name: Global Costmap
-          Topic:
-            Depth: 1
-            Durability Policy: Transient Local
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/global_costmap/costmap
-          Use Timestamp: false
-          Value: true
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz_default_plugins/Path
-          Color: 255; 0; 0
-          Enabled: true
-          Head Diameter: 0.019999999552965164
-          Head Length: 0.019999999552965164
-          Length: 0.30000001192092896
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: Path
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: Arrows
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.004999999888241291
-          Shaft Length: 0.019999999552965164
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/plan
-          Value: true
-        - Alpha: 1
-          Class: rviz_default_plugins/Polygon
-          Color: 25; 255; 0
-          Enabled: false
-          Name: Polygon
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/global_costmap/published_footprint
-          Value: false
-      Enabled: true
-      Name: Global Planner
-    - Class: rviz_common/Group
-      Displays:
-        - Alpha: 0.699999988079071
-          Class: rviz_default_plugins/Map
-          Color Scheme: costmap
-          Draw Behind: false
-          Enabled: true
-          Name: Local Costmap
-          Topic:
-            Depth: 1
-            Durability Policy: Transient Local
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/local_costmap/costmap
-          Use Timestamp: false
-          Value: true
-        - Alpha: 1
-          Buffer Length: 1
-          Class: rviz_default_plugins/Path
-          Color: 0; 12; 255
-          Enabled: true
-          Head Diameter: 0.30000001192092896
-          Head Length: 0.20000000298023224
-          Length: 0.30000001192092896
-          Line Style: Lines
-          Line Width: 0.029999999329447746
-          Name: Local Plan
-          Offset:
-            X: 0
-            Y: 0
-            Z: 0
-          Pose Color: 255; 85; 255
-          Pose Style: None
-          Radius: 0.029999999329447746
-          Shaft Diameter: 0.10000000149011612
-          Shaft Length: 0.10000000149011612
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/local_plan
-          Value: true
-        - Class: rviz_default_plugins/MarkerArray
-          Enabled: false
-          Name: Trajectories
-          Namespaces:
-            {}
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/marker
-          Value: false
-        - Alpha: 1
-          Class: rviz_default_plugins/Polygon
-          Color: 25; 255; 0
-          Enabled: true
-          Name: Polygon
-          Topic:
-            Depth: 5
-            Durability Policy: Volatile
-            History Policy: Keep Last
-            Reliability Policy: Reliable
-            Value: <robot_namespace>/local_costmap/published_footprint
-          Value: true
-      Enabled: true
-      Name: Controller
-    - Class: rviz_default_plugins/MarkerArray
-      Enabled: true
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: <robot_namespace>/waypoints
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: map
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: <robot_namespace>/initialpose
-    - Class: nav2_rviz_plugins/GoalTool
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Angle: -0.0008007669821381569
-      Class: rviz_default_plugins/TopDownOrtho
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Scale: 54
-      Target Frame: <Fixed Frame>
-      Value: TopDownOrtho (rviz_default_plugins)
-      X: -5.41
-      Y: 0
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 914
-  Hide Left Dock: false
-  Hide Right Dock: true
-  Navigation 2:
-    collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000338fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000003070000006e0000006200ffffff000000010000010f00000338fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000338000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004990000033800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: true
-  Width: 1545
-  X: 186
-  Y: 117

From eb560c4769d746707e916490655eb9f24cb846e3 Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Wed, 12 Jun 2024 19:43:17 +0200
Subject: [PATCH 02/18] [nav2_costmap_2s]: free correctly the memory in the
 InflationLayer (#4424)

* [nav2_costmap_2s]: free correctly the memory in the InflationLayer

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* Remove 4 redundant bytes from CellData in Inflationlayer

Note: performance was measured and this is slightly faster, even if we
need to recompute the index. Probably a benefit at the level of the CPU
cache

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* formatting

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

---------

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
---
 .../nav2_costmap_2d/inflation_layer.hpp       |  6 ++--
 nav2_costmap_2d/plugins/inflation_layer.cpp   | 33 +++++++++----------
 .../test/integration/inflation_tests.cpp      | 19 +++++------
 3 files changed, 25 insertions(+), 33 deletions(-)

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
index b83010c62d..0ae010e94b 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
@@ -60,18 +60,16 @@ class CellData
 public:
   /**
    * @brief  Constructor for a CellData objects
-   * @param  i The index of the cell in the cost map
    * @param  x The x coordinate of the cell in the cost map
    * @param  y The y coordinate of the cell in the cost map
    * @param  sx The x coordinate of the closest obstacle cell in the costmap
    * @param  sy The y coordinate of the closest obstacle cell in the costmap
    * @return
    */
-  CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
-  : index_(static_cast<unsigned int>(i)), x_(x), y_(y), src_x_(sx), src_y_(sy)
+  CellData(unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
+  : x_(x), y_(y), src_x_(sx), src_y_(sy)
   {
   }
-  unsigned int index_;
   unsigned int x_, y_;
   unsigned int src_x_, src_y_;
 };
diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index 067877e548..e2542aa9cf 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -230,12 +230,13 @@ InflationLayer::updateCosts(
 
   // Start with lethal obstacles: by definition distance is 0.0
   auto & obs_bin = inflation_cells_[0];
+  obs_bin.reserve(200);
   for (int j = min_j; j < max_j; j++) {
     for (int i = min_i; i < max_i; i++) {
       int index = static_cast<int>(master_grid.getIndex(i, j));
       unsigned char cost = master_array[index];
       if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION)) {
-        obs_bin.emplace_back(index, i, j, i, j);
+        obs_bin.emplace_back(i, j, i, j);
       }
     }
   }
@@ -243,12 +244,18 @@ InflationLayer::updateCosts(
   // Process cells by increasing distance; new cells are appended to the
   // corresponding distance bin, so they
   // can overtake previously inserted but farther away cells
-  for (const auto & dist_bin : inflation_cells_) {
+  for (auto & dist_bin : inflation_cells_) {
+    dist_bin.reserve(200);
     for (std::size_t i = 0; i < dist_bin.size(); ++i) {
       // Do not use iterator or for-range based loops to
       // iterate though dist_bin, since it's size might
       // change when a new cell is enqueued, invalidating all iterators
-      unsigned int index = dist_bin[i].index_;
+      const CellData & cell = dist_bin[i];
+      unsigned int mx = cell.x_;
+      unsigned int my = cell.y_;
+      unsigned int sx = cell.src_x_;
+      unsigned int sy = cell.src_y_;
+      unsigned int index = master_grid.getIndex(mx, my);
 
       // ignore if already visited
       if (seen_[index]) {
@@ -257,11 +264,6 @@ InflationLayer::updateCosts(
 
       seen_[index] = true;
 
-      unsigned int mx = dist_bin[i].x_;
-      unsigned int my = dist_bin[i].y_;
-      unsigned int sx = dist_bin[i].src_x_;
-      unsigned int sy = dist_bin[i].src_y_;
-
       // assign the cost associated with the distance from an obstacle to the cell
       unsigned char cost = costLookup(mx, my, sx, sy);
       unsigned char old_cost = master_array[index];
@@ -296,11 +298,9 @@ InflationLayer::updateCosts(
         enqueue(index + size_x, mx, my + 1, sx, sy);
       }
     }
-  }
-
-  for (auto & dist : inflation_cells_) {
-    dist.clear();
-    dist.reserve(200);
+    // This level of inflation_cells_ is not needed anymore. We can free the memory
+    // Note that dist_bin.clear() is not enough, because it won't free the memory
+    dist_bin = std::vector<CellData>();
   }
 
   current_ = true;
@@ -334,8 +334,8 @@ InflationLayer::enqueue(
     const unsigned int r = cell_inflation_radius_ + 2;
 
     // push the cell data onto the inflation list and mark
-    inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(
-      index, mx, my, src_x, src_y);
+    const auto dist = distance_matrix_[mx - src_x + r][my - src_y + r];
+    inflation_cells_[dist].emplace_back(mx, my, src_x, src_y);
   }
 }
 
@@ -372,9 +372,6 @@ InflationLayer::computeCaches()
   int max_dist = generateIntegerDistances();
   inflation_cells_.clear();
   inflation_cells_.resize(max_dist + 1);
-  for (auto & dist : inflation_cells_) {
-    dist.reserve(200);
-  }
 }
 
 int
diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp
index ad867128b1..b88c373688 100644
--- a/nav2_costmap_2d/test/integration/inflation_tests.cpp
+++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp
@@ -125,15 +125,16 @@ void TestNode::validatePointInflation(
   bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
   memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
   std::map<double, std::vector<CellData>> m;
-  CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
+  CellData initial(mx, my, mx, my);
   m[0].push_back(initial);
   for (std::map<double, std::vector<CellData>>::iterator bin = m.begin();
     bin != m.end(); ++bin)
   {
     for (unsigned int i = 0; i < bin->second.size(); ++i) {
       const CellData cell = bin->second[i];
-      if (!seen[cell.index_]) {
-        seen[cell.index_] = true;
+      const auto index = costmap->getIndex(cell.x_, cell.y_);
+      if (!seen[index]) {
+        seen[index] = true;
         unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
         unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
         double dist = std::hypot(dx, dy);
@@ -152,23 +153,19 @@ void TestNode::validatePointInflation(
         }
 
         if (cell.x_ > 0) {
-          CellData data(costmap->getIndex(cell.x_ - 1, cell.y_),
-            cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
+          CellData data(cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
         if (cell.y_ > 0) {
-          CellData data(costmap->getIndex(cell.x_, cell.y_ - 1),
-            cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
+          CellData data(cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
         if (cell.x_ < costmap->getSizeInCellsX() - 1) {
-          CellData data(costmap->getIndex(cell.x_ + 1, cell.y_),
-            cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
+          CellData data(cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
         if (cell.y_ < costmap->getSizeInCellsY() - 1) {
-          CellData data(costmap->getIndex(cell.x_, cell.y_ + 1),
-            cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
+          CellData data(cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
       }

From 23ddd865a558aeeee9388d1bdab15dc2e29cea73 Mon Sep 17 00:00:00 2001
From: Davide Faconti <davide.faconti@gmail.com>
Date: Wed, 12 Jun 2024 22:10:35 +0200
Subject: [PATCH 03/18] [AMCL] reduce the amount of memory used (#4426)

* iprove memory used by AMCL and add parameter "freespace_downsampling"

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* fix message and linting

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

* add class member

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>

---------

Signed-off-by: Davide Faconti <davide.faconti@gmail.com>
---
 nav2_amcl/include/nav2_amcl/amcl_node.hpp |  4 +++-
 nav2_amcl/include/nav2_amcl/map/map.hpp   |  9 +++++----
 nav2_amcl/src/amcl_node.cpp               | 22 +++++++++++++++-------
 3 files changed, 23 insertions(+), 12 deletions(-)

diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
index 030a85f38c..514bba4c55 100644
--- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp
+++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp
@@ -152,7 +152,8 @@ class AmclNode : public nav2_util::LifecycleNode
   std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
-  static std::vector<std::pair<int, int>> free_space_indices;
+  struct Point2D { int32_t x; int32_t y; };
+  static std::vector<Point2D> free_space_indices;
 #endif
 
   // Transforms
@@ -391,6 +392,7 @@ class AmclNode : public nav2_util::LifecycleNode
   double z_rand_;
   std::string scan_topic_{"scan"};
   std::string map_topic_{"map"};
+  bool freespace_downsampling_ = false;
 };
 
 }  // namespace nav2_amcl
diff --git a/nav2_amcl/include/nav2_amcl/map/map.hpp b/nav2_amcl/include/nav2_amcl/map/map.hpp
index c9557efa0b..3cf6f0d597 100644
--- a/nav2_amcl/include/nav2_amcl/map/map.hpp
+++ b/nav2_amcl/include/nav2_amcl/map/map.hpp
@@ -41,20 +41,21 @@ struct _rtk_fig_t;
 // Limits
 #define MAP_WIFI_MAX_LEVELS 8
 
-
+// make sure that the sizeof(map_cell_t) == 5
+#pragma pack(push, 1)
 // Description for a single map cell.
 typedef struct
 {
   // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
-  int occ_state;
+  int8_t occ_state;
 
   // Distance to the nearest occupied cell
-  double occ_dist;
+  float occ_dist;
 
   // Wifi levels
   // int wifi_levels[MAP_WIFI_MAX_LEVELS];
 } map_cell_t;
-
+#pragma pack(pop)
 
 // Description for a map
 typedef struct
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index cdd4c5adbd..1a10c7aa41 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -227,6 +227,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
   add_parameter(
     "first_map_only", rclcpp::ParameterValue(false),
     "Set this to true, when you want to load a new map published from the map_server");
+
+  add_parameter(
+    "freespace_downsampling", rclcpp::ParameterValue(
+      false),
+    "Downsample the free space used by the Pose Generator. Use it with large maps to save memory");
 }
 
 AmclNode::~AmclNode()
@@ -404,7 +409,7 @@ AmclNode::checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time las
 }
 
 #if NEW_UNIFORM_SAMPLING
-std::vector<std::pair<int, int>> AmclNode::free_space_indices;
+std::vector<AmclNode::Point2D> AmclNode::free_space_indices;
 #endif
 
 bool
@@ -446,10 +451,10 @@ AmclNode::uniformPoseGenerator(void * arg)
 
 #if NEW_UNIFORM_SAMPLING
   unsigned int rand_index = drand48() * free_space_indices.size();
-  std::pair<int, int> free_point = free_space_indices[rand_index];
+  AmclNode::Point2D free_point = free_space_indices[rand_index];
   pf_vector_t p;
-  p.v[0] = MAP_WXGX(map, free_point.first);
-  p.v[1] = MAP_WYGY(map, free_point.second);
+  p.v[0] = MAP_WXGX(map, free_point.x);
+  p.v[1] = MAP_WYGY(map, free_point.y);
   p.v[2] = drand48() * 2 * M_PI - M_PI;
 #else
   double min_x, max_x, min_y, max_y;
@@ -1106,6 +1111,7 @@ AmclNode::initParameters()
   get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
   get_parameter("scan_topic", scan_topic_);
   get_parameter("map_topic", map_topic_);
+  get_parameter("freespace_downsampling", freespace_downsampling_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
   transform_tolerance_ = tf2::durationFromSec(tmp_tol);
@@ -1435,12 +1441,14 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
 void
 AmclNode::createFreeSpaceVector()
 {
+  int delta = freespace_downsampling_ ? 2 : 1;
   // Index of free space
   free_space_indices.resize(0);
-  for (int i = 0; i < map_->size_x; i++) {
-    for (int j = 0; j < map_->size_y; j++) {
+  for (int i = 0; i < map_->size_x; i += delta) {
+    for (int j = 0; j < map_->size_y; j += delta) {
       if (map_->cells[MAP_INDEX(map_, i, j)].occ_state == -1) {
-        free_space_indices.push_back(std::make_pair(i, j));
+        AmclNode::Point2D point = {i, j};
+        free_space_indices.push_back(point);
       }
     }
   }

From 29ba4b1520d58c47daca5032d400177fff85aa17 Mon Sep 17 00:00:00 2001
From: Alberto Tudela <ajtudela@gmail.com>
Date: Fri, 14 Jun 2024 03:13:00 +0200
Subject: [PATCH 04/18] Added dynamic reconfigure and export controller in
 docking (#4429)

* Added dynamic reconfigure and export controller in docking

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Fix vector

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update controller.cpp

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
---
 nav2_docking/opennav_docking/CMakeLists.txt   | 17 +++--
 .../include/opennav_docking/controller.hpp    | 15 +++++
 .../opennav_docking/src/controller.cpp        | 66 +++++++++++++++----
 .../opennav_docking/test/test_controller.cpp  | 34 ++++++++++
 4 files changed, 117 insertions(+), 15 deletions(-)

diff --git a/nav2_docking/opennav_docking/CMakeLists.txt b/nav2_docking/opennav_docking/CMakeLists.txt
index 38e9b79516..e50590d3e4 100644
--- a/nav2_docking/opennav_docking/CMakeLists.txt
+++ b/nav2_docking/opennav_docking/CMakeLists.txt
@@ -56,8 +56,15 @@ set(dependencies
   opennav_docking_core
 )
 
-add_library(${library_name} SHARED
+add_library(controller SHARED
   src/controller.cpp
+)
+
+ament_target_dependencies(controller
+  ${dependencies}
+)
+
+add_library(${library_name} SHARED
   src/docking_server.cpp
   src/dock_database.cpp
   src/navigator.cpp
@@ -68,7 +75,9 @@ ament_target_dependencies(${library_name}
 )
 
 target_link_libraries(${library_name}
-  yaml-cpp::yaml-cpp)
+  yaml-cpp::yaml-cpp
+  controller
+)
 
 add_library(pose_filter SHARED
   src/pose_filter.cpp
@@ -100,7 +109,7 @@ target_link_libraries(simple_charging_dock pose_filter)
 
 pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml)
 
-install(TARGETS ${library_name} pose_filter simple_charging_dock
+install(TARGETS ${library_name} controller pose_filter simple_charging_dock
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -127,6 +136,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name} pose_filter)
+ament_export_libraries(${library_name} controller pose_filter)
 ament_export_dependencies(${dependencies} yaml-cpp)
 ament_package()
diff --git a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
index 54472e720f..afae5965d2 100644
--- a/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
+++ b/nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
@@ -16,6 +16,7 @@
 #define OPENNAV_DOCKING__CONTROLLER_HPP_
 
 #include <memory>
+#include <vector>
 
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/twist.hpp"
@@ -47,8 +48,22 @@ class Controller
     const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd,
     bool backward = false);
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::mutex dynamic_params_lock_;
+
 protected:
   std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
+
+  double k_phi_, k_delta_, beta_, lambda_;
+  double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
 };
 
 }  // namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/src/controller.cpp b/nav2_docking/opennav_docking/src/controller.cpp
index 3060aaf8c8..8613f2d27f 100644
--- a/nav2_docking/opennav_docking/src/controller.cpp
+++ b/nav2_docking/opennav_docking/src/controller.cpp
@@ -40,25 +40,69 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
   nav2_util::declare_parameter_if_not_declared(
     node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
 
-  double k_phi, k_delta, beta, lambda;
-  double slowdown_radius, v_linear_min, v_linear_max, v_angular_max;
-  node->get_parameter("controller.k_phi", k_phi);
-  node->get_parameter("controller.k_delta", k_delta);
-  node->get_parameter("controller.beta", beta);
-  node->get_parameter("controller.lambda", lambda);
-  node->get_parameter("controller.v_linear_min", v_linear_min);
-  node->get_parameter("controller.v_linear_max", v_linear_max);
-  node->get_parameter("controller.v_angular_max", v_angular_max);
-  node->get_parameter("controller.slowdown_radius", slowdown_radius);
+  node->get_parameter("controller.k_phi", k_phi_);
+  node->get_parameter("controller.k_delta", k_delta_);
+  node->get_parameter("controller.beta", beta_);
+  node->get_parameter("controller.lambda", lambda_);
+  node->get_parameter("controller.v_linear_min", v_linear_min_);
+  node->get_parameter("controller.v_linear_max", v_linear_max_);
+  node->get_parameter("controller.v_angular_max", v_angular_max_);
+  node->get_parameter("controller.slowdown_radius", slowdown_radius_);
   control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
-    k_phi, k_delta, beta, lambda, slowdown_radius, v_linear_min, v_linear_max, v_angular_max);
+    k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
+    v_angular_max_);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node->add_on_set_parameters_callback(
+    std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
 }
 
 bool Controller::computeVelocityCommand(
   const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward)
 {
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
   cmd = control_law_->calculateRegularVelocity(pose, backward);
   return true;
 }
 
+rcl_interfaces::msg::SetParametersResult
+Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::mutex> lock(dynamic_params_lock_);
+
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+
+    if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
+      if (name == "controller.k_phi") {
+        k_phi_ = parameter.as_double();
+      } else if (name == "controller.k_delta") {
+        k_delta_ = parameter.as_double();
+      } else if (name == "controller.beta") {
+        beta_ = parameter.as_double();
+      } else if (name == "controller.lambda") {
+        lambda_ = parameter.as_double();
+      } else if (name == "controller.v_linear_min") {
+        v_linear_min_ = parameter.as_double();
+      } else if (name == "controller.v_linear_max") {
+        v_linear_max_ = parameter.as_double();
+      } else if (name == "controller.v_angular_max") {
+        v_angular_max_ = parameter.as_double();
+      } else if (name == "controller.slowdown_radius") {
+        slowdown_radius_ = parameter.as_double();
+      }
+
+      // Update the smooth control law with the new params
+      control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
+      control_law_->setSlowdownRadius(slowdown_radius_);
+      control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
 }  // namespace opennav_docking
diff --git a/nav2_docking/opennav_docking/test/test_controller.cpp b/nav2_docking/opennav_docking/test/test_controller.cpp
index da8885d89d..c2f1e25f1c 100644
--- a/nav2_docking/opennav_docking/test/test_controller.cpp
+++ b/nav2_docking/opennav_docking/test/test_controller.cpp
@@ -45,4 +45,38 @@ TEST(ControllerTests, ObjectLifecycle)
   controller.reset();
 }
 
+TEST(ControllerTests, DynamicParameters) {
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
+  auto controller = std::make_shared<opennav_docking::Controller>(node);
+
+  auto params = std::make_shared<rclcpp::AsyncParametersClient>(
+    node->get_node_base_interface(), node->get_node_topics_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_services_interface());
+
+  // Set parameters
+  auto results = params->set_parameters_atomically(
+    {rclcpp::Parameter("controller.k_phi", 1.0),
+      rclcpp::Parameter("controller.k_delta", 2.0),
+      rclcpp::Parameter("controller.beta", 3.0),
+      rclcpp::Parameter("controller.lambda", 4.0),
+      rclcpp::Parameter("controller.v_linear_min", 5.0),
+      rclcpp::Parameter("controller.v_linear_max", 6.0),
+      rclcpp::Parameter("controller.v_angular_max", 7.0),
+      rclcpp::Parameter("controller.slowdown_radius", 8.0)});
+
+  // Spin
+  rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
+
+  // Check parameters
+  EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0);
+  EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0);
+  EXPECT_EQ(node->get_parameter("controller.beta").as_double(), 3.0);
+  EXPECT_EQ(node->get_parameter("controller.lambda").as_double(), 4.0);
+  EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0);
+  EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0);
+  EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0);
+  EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0);
+}
+
 }  // namespace opennav_docking

From 635880d3d6ad1f23eb75d793409189c653797f58 Mon Sep 17 00:00:00 2001
From: Guillaume Doisy <doisyg@users.noreply.github.com>
Date: Fri, 14 Jun 2024 03:15:39 +0200
Subject: [PATCH 05/18] Throttle and switch to DEBUG bt loop rate warning
 (#4430)

Signed-off-by: Guillaume Doisy <guillaume@dexory.com>
Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
---
 .../include/nav2_behavior_tree/behavior_tree_engine.hpp   | 6 +++++-
 .../include/nav2_behavior_tree/bt_action_server_impl.hpp  | 2 +-
 nav2_behavior_tree/src/behavior_tree_engine.cpp           | 8 ++++++--
 3 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
index 9ee903fd78..a0d86649c0 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/behavior_tree_engine.hpp
@@ -47,7 +47,8 @@ class BehaviorTreeEngine
    * @param plugin_libraries vector of BT plugin library names to load
    */
   explicit BehaviorTreeEngine(
-    const std::vector<std::string> & plugin_libraries);
+    const std::vector<std::string> & plugin_libraries,
+    rclcpp::Node::SharedPtr node);
   virtual ~BehaviorTreeEngine() {}
 
   /**
@@ -93,6 +94,9 @@ class BehaviorTreeEngine
 protected:
   // The factory that will be used to dynamically construct the behavior tree
   BT::BehaviorTreeFactory factory_;
+
+  // Clock
+  rclcpp::Clock::SharedPtr clock_;
 };
 
 }  // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
index 75dfff5597..d2b120f44c 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp
@@ -173,7 +173,7 @@ bool BtActionServer<ActionT>::on_configure()
   error_code_names_ = node->get_parameter("error_code_names").as_string_array();
 
   // Create the class that registers our custom nodes and executes the BT
-  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);
+  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_, client_node_);
 
   // Create the blackboard that will be shared by all of the nodes in the tree
   blackboard_ = BT::Blackboard::create();
diff --git a/nav2_behavior_tree/src/behavior_tree_engine.cpp b/nav2_behavior_tree/src/behavior_tree_engine.cpp
index 8ed1cd4e71..70a192d719 100644
--- a/nav2_behavior_tree/src/behavior_tree_engine.cpp
+++ b/nav2_behavior_tree/src/behavior_tree_engine.cpp
@@ -26,13 +26,16 @@ namespace nav2_behavior_tree
 {
 
 BehaviorTreeEngine::BehaviorTreeEngine(
-  const std::vector<std::string> & plugin_libraries)
+  const std::vector<std::string> & plugin_libraries, rclcpp::Node::SharedPtr node)
 {
   BT::SharedLibrary loader;
   for (const auto & p : plugin_libraries) {
     factory_.registerFromPlugin(loader.getOSName(p));
   }
 
+  // clock for throttled debug log
+  clock_ = node->get_clock();
+
   // FIXME: the next two line are needed for back-compatibility with BT.CPP 3.8.x
   // Note that the can be removed, once we migrate from BT.CPP 4.5.x to 4.6+
   BT::ReactiveSequence::EnableException(false);
@@ -62,8 +65,9 @@ BehaviorTreeEngine::run(
       onLoop();
 
       if (!loopRate.sleep()) {
-        RCLCPP_WARN(
+        RCLCPP_DEBUG_THROTTLE(
           rclcpp::get_logger("BehaviorTreeEngine"),
+          *clock_, 1000,
           "Behavior Tree tick rate %0.2f was exceeded!",
           1.0 / (loopRate.period().count() * 1.0e-9));
       }

From d5128204d98a735b3edba81cfcb48d764c7a1774 Mon Sep 17 00:00:00 2001
From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com>
Date: Tue, 18 Jun 2024 08:47:36 -0700
Subject: [PATCH 06/18] =?UTF-8?q?=F0=9F=9B=A0=EF=B8=8F=20Bump=20docker/bui?=
 =?UTF-8?q?ld-push-action=20from=205=20to=206=20(#4444)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Bumps [docker/build-push-action](https://github.com/docker/build-push-action) from 5 to 6.
- [Release notes](https://github.com/docker/build-push-action/releases)
- [Commits](https://github.com/docker/build-push-action/compare/v5...v6)

---
updated-dependencies:
- dependency-name: docker/build-push-action
  dependency-type: direct:production
  update-type: version-update:semver-major
...

Signed-off-by: dependabot[bot] <support@github.com>
Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com>
---
 .github/workflows/update_ci_image.yaml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/.github/workflows/update_ci_image.yaml b/.github/workflows/update_ci_image.yaml
index ce987eb118..16d172aff8 100644
--- a/.github/workflows/update_ci_image.yaml
+++ b/.github/workflows/update_ci_image.yaml
@@ -99,7 +99,7 @@ jobs:
       - name: Build and push ${{ github.ref_name }}
         if: steps.config.outputs.trigger == 'true'
         id: docker_build
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           pull: true

From 91b4f8ee3e892469992695eef61f6086128598db Mon Sep 17 00:00:00 2001
From: Tony Najjar <tony.najjar.1997@gmail.com>
Date: Tue, 18 Jun 2024 17:48:24 +0200
Subject: [PATCH 07/18] Warn if inflation_radius_ < inscribed_radius_ (#4423)

* Warn if inflation_radius_ < inscribed_radius_

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* convert to error

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>
---
 nav2_costmap_2d/plugins/inflation_layer.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
index e2542aa9cf..01b788a7cc 100644
--- a/nav2_costmap_2d/plugins/inflation_layer.cpp
+++ b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -170,6 +170,16 @@ InflationLayer::onFootprintChanged()
   computeCaches();
   need_reinflation_ = true;
 
+  if (inflation_radius_ < inscribed_radius_) {
+    RCLCPP_ERROR(
+      logger_,
+      "The configured inflation radius (%.3f) is smaller than "
+      "the computed inscribed radius (%.3f) of your footprint, "
+      "it is highly recommended to set inflation radius to be at "
+      "least as big as the inscribed radius to avoid collisions",
+      inflation_radius_, inscribed_radius_);
+  }
+
   RCLCPP_DEBUG(
     logger_, "InflationLayer::onFootprintChanged(): num footprint points: %zu,"
     " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",

From ef40391157874eb3586e4512663d74dd6de5bdc6 Mon Sep 17 00:00:00 2001
From: Rein Appeldoorn <reinzor@gmail.com>
Date: Tue, 18 Jun 2024 17:48:48 +0200
Subject: [PATCH 08/18] chore: cleanup ros1 leftovers (#4446)

Signed-off-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
---
 nav2_costmap_2d/cfg/Costmap2D.cfg             | 23 ----------
 nav2_costmap_2d/cfg/GenericPlugin.cfg         |  7 ---
 nav2_costmap_2d/cfg/InflationPlugin.cfg       | 12 ------
 nav2_costmap_2d/cfg/ObstaclePlugin.cfg        | 22 ----------
 nav2_costmap_2d/cfg/VoxelPlugin.cfg           | 22 ----------
 nav2_costmap_2d/launch/example.launch         | 21 ---------
 nav2_costmap_2d/launch/example_params.yaml    | 43 -------------------
 .../dwb_plugins/cfg/KinematicParams.cfg       | 33 --------------
 8 files changed, 183 deletions(-)
 delete mode 100755 nav2_costmap_2d/cfg/Costmap2D.cfg
 delete mode 100755 nav2_costmap_2d/cfg/GenericPlugin.cfg
 delete mode 100755 nav2_costmap_2d/cfg/InflationPlugin.cfg
 delete mode 100755 nav2_costmap_2d/cfg/ObstaclePlugin.cfg
 delete mode 100755 nav2_costmap_2d/cfg/VoxelPlugin.cfg
 delete mode 100644 nav2_costmap_2d/launch/example.launch
 delete mode 100644 nav2_costmap_2d/launch/example_params.yaml
 delete mode 100644 nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg

diff --git a/nav2_costmap_2d/cfg/Costmap2D.cfg b/nav2_costmap_2d/cfg/Costmap2D.cfg
deleted file mode 100755
index 07c4a1628b..0000000000
--- a/nav2_costmap_2d/cfg/Costmap2D.cfg
+++ /dev/null
@@ -1,23 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, str_t
-
-gen = ParameterGenerator()
-
-gen.add("transform_tolerance", double_t, 0, "Specifies the delay in transform (tf) data that is tolerable in seconds.", 0.3, 0, 10)
-gen.add("update_frequency", double_t, 0, "The frequency in Hz for the map to be updated.", 5, 0, 100)
-gen.add("publish_frequency", double_t, 0, "The frequency in Hz for the map to be publish display information.", 0, 0, 100)
-
-#map params
-gen.add("width", int_t, 0, "The width of the map in meters.", 10, 0)
-gen.add("height", int_t, 0, "The height of the map in meters.", 10, 0)
-gen.add("resolution", double_t, 0, "The resolution of the map in meters/cell.", 0.05, 0, 50)
-gen.add("origin_x", double_t, 0, "The x origin of the map in the global frame in meters.", 0)
-gen.add("origin_y", double_t, 0, "The y origin of the map in the global frame in meters.", 0)
-
-# robot footprint shape
-gen.add("footprint", str_t, 0, "The footprint of the robot specified in the robot_base_frame coordinate frame as a list in the format: [ [x1, y1], [x2, y2], ...., [xn, yn] ].", "[]")
-gen.add("robot_radius", double_t, 0, 'The radius of the robot in meters, this parameter should only be set for circular robots, all others should use the footprint parameter described above.', 0.46, 0, 10)
-gen.add("footprint_padding", double_t, 0, "How much to pad (increase the size of) the footprint, in meters.", 0.01)
-
-exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "Costmap2D"))
diff --git a/nav2_costmap_2d/cfg/GenericPlugin.cfg b/nav2_costmap_2d/cfg/GenericPlugin.cfg
deleted file mode 100755
index 555e2b5415..0000000000
--- a/nav2_costmap_2d/cfg/GenericPlugin.cfg
+++ /dev/null
@@ -1,7 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t
-
-gen = ParameterGenerator()
-gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True)
-exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "GenericPlugin"))
diff --git a/nav2_costmap_2d/cfg/InflationPlugin.cfg b/nav2_costmap_2d/cfg/InflationPlugin.cfg
deleted file mode 100755
index 5c11eaf791..0000000000
--- a/nav2_costmap_2d/cfg/InflationPlugin.cfg
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t
-
-gen = ParameterGenerator()
-
-gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True)
-gen.add("cost_scaling_factor", double_t, 0, "A scaling factor to apply to cost values during inflation.", 10, 0, 100)
-gen.add("inflation_radius", double_t, 0, "The radius in meters to which the map inflates obstacle cost values.", 0.55, 0, 50)
-gen.add("inflate_unknown", bool_t, 0, "Whether to inflate unknown cells.", False)
-
-exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "InflationPlugin"))
diff --git a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg b/nav2_costmap_2d/cfg/ObstaclePlugin.cfg
deleted file mode 100755
index a94e98fbbe..0000000000
--- a/nav2_costmap_2d/cfg/ObstaclePlugin.cfg
+++ /dev/null
@@ -1,22 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t
-
-gen = ParameterGenerator()
-
-gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True)
-gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True)
-gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50)
-
-combo_enum = gen.enum([gen.const("Overwrite", int_t,  0, "Overwrite values"),
-                       gen.const("Maximum",   int_t,  1, "Take the maximum of the values"),
-                       gen.const("Nothing",   int_t, 99, "Do nothing")],
-                       "Method for combining layers enum")
-gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, edit_method=combo_enum)
-
-
-# gen.add("obstacle_max_range", double_t, 0, "The default maximum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 2.5, 0, 50)
-# gen.add("obstacle_min_range", double_t, 0, "The default minimum distance from the robot at which an obstacle will be inserted into the cost map in meters.", 0.0, 0, 50)
-# gen.add("raytrace_max_range", double_t, 0, "The default maximum range in meters at which to raytrace out obstacles from the map using sensor data.", 3, 0, 50)
-# gen.add("raytrace_min_range", double_t, 0, "The default minimum range in meters from which to raytrace out obstacles from the map using sensor data.", 0.0, 0, 50)
-exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "ObstaclePlugin"))
diff --git a/nav2_costmap_2d/cfg/VoxelPlugin.cfg b/nav2_costmap_2d/cfg/VoxelPlugin.cfg
deleted file mode 100755
index 977fcb99d4..0000000000
--- a/nav2_costmap_2d/cfg/VoxelPlugin.cfg
+++ /dev/null
@@ -1,22 +0,0 @@
-#!/usr/bin/env python
-
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, bool_t, double_t, int_t
-
-gen = ParameterGenerator()
-
-gen.add("enabled", bool_t, 0, "Whether to use this plugin or not", True)
-gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True)
-gen.add("max_obstacle_height", double_t, 0, "Max Obstacle Height", 2.0, 0, 50)
-gen.add("origin_z", double_t, 0, "The z origin of the map in meters.", 0, 0)
-gen.add("z_resolution", double_t, 0, "The z resolution of the map in meters/cell.", 0.2, 0, 50)
-gen.add("z_voxels", int_t, 0, "The number of voxels to in each vertical column.", 10, 0, 16)
-gen.add("unknown_threshold", int_t, 0, 'The number of unknown cells allowed in a column considered to be known', 15, 0, 16)
-gen.add("mark_threshold", int_t, 0, 'The maximum number of marked cells allowed in a column considered to be free', 0, 0, 16)
-
-combo_enum = gen.enum([gen.const("Overwrite", int_t,  0, "Overwrite values"),
-                       gen.const("Maximum",   int_t,  1, "Take the maximum of the values"),
-                       gen.const("Nothing",   int_t, 99, "Do nothing")],
-                       "Method for combining layers enum")
-gen.add("combination_method", int_t, 0, "Method for combining two layers", 1, 0, 2, edit_method=combo_enum)
-
-exit(gen.generate("nav2_costmap_2d", "nav2_costmap_2d", "VoxelPlugin"))
diff --git a/nav2_costmap_2d/launch/example.launch b/nav2_costmap_2d/launch/example.launch
deleted file mode 100644
index ac089abfba..0000000000
--- a/nav2_costmap_2d/launch/example.launch
+++ /dev/null
@@ -1,21 +0,0 @@
-<launch>
-  <!-- 
-  NOTE: You'll need to bring up something that publishes sensor data (see
-  rosstage), something that publishes a map (see map_server), and something to
-  visualize a costmap (see nav_view), to see things work.
-
-  Also, on a real robot, you'd want to set the "use_sim_time" parameter to false, or just not set it.
-  -->
-  <param name="/use_sim_time" value="true"/>
-
-  <!-- Publishes the voxel grid to rviz for display -->
-  <node pkg="nav2_costmap_2d" type="nav2_costmap_2d_markers" name="voxel_visualizer">
-    <remap from="voxel_grid" to="costmap/voxel_grid"/>
-  </node>
-
-  <!-- Run the costmap node -->
-  <node name="costmap_node" pkg="nav2_costmap_2d" type="nav2_costmap_2d_node" >
-    <rosparam file="$(find nav2_costmap_2d)/launch/example_params.yaml" command="load" ns="costmap" />
-  </node>
-
-</launch>
diff --git a/nav2_costmap_2d/launch/example_params.yaml b/nav2_costmap_2d/launch/example_params.yaml
deleted file mode 100644
index 72bf695fad..0000000000
--- a/nav2_costmap_2d/launch/example_params.yaml
+++ /dev/null
@@ -1,43 +0,0 @@
-global_frame: map
-robot_base_frame: base_link
-update_frequency: 5.0
-publish_frequency: 1.0
-
-#set if you want the voxel map published
-publish_voxel_map: true
-
-#set to true if you want to initialize the costmap from a static map
-static_map: false
-
-#begin - COMMENT these lines if you set static_map to true
-rolling_window: true
-width: 6.0
-height: 6.0
-resolution: 0.025
-#end - COMMENT these lines if you set static_map to true
-
-#START VOXEL STUFF
-map_type: voxel
-origin_z: 0.0
-z_resolution: 0.2
-z_voxels: 10
-unknown_threshold: 10
-mark_threshold: 0
-#END VOXEL STUFF
-
-transform_tolerance: 0.3
-obstacle_max_range: 2.5
-obstacle_min_range: 0.0
-max_obstacle_height: 2.0
-raytrace_max_range: 3.0
-raytrace_min_range: 0.0
-
-footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
-#robot_radius: 0.46
-footprint_padding: 0.01
-inflation_radius: 0.55
-cost_scaling_factor: 10.0
-lethal_cost_threshold: 100
-observation_sources: base_scan
-base_scan: {data_type: LaserScan, expected_update_rate: 0.4,
-  observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}
diff --git a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg b/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg
deleted file mode 100644
index 51dab28aa4..0000000000
--- a/nav2_dwb_controller/dwb_plugins/cfg/KinematicParams.cfg
+++ /dev/null
@@ -1,33 +0,0 @@
-#!/usr/bin/env python
-from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t
-
-gen = ParameterGenerator()
-
-# velocities
-gen.add('min_vel_x', double_t, 0, 'The minimum x velocity for the robot in m/s', 0.0)
-gen.add('max_vel_x', double_t, 0, 'The maximum x velocity for the robot in m/s', 0.55)
-gen.add('min_vel_y', double_t, 0, 'The minimum y velocity for the robot in m/s', -0.1)
-gen.add('max_vel_y', double_t, 0, 'The maximum y velocity for the robot in m/s', 0.1)
-gen.add('max_vel_theta', double_t, 0, 'The absolute value of the maximum rotational velocity for the robot in rad/s. '
-                                      'The minimum rotational velocity is assumed to be -max_vel_theta', 1.0)
-
-# acceleration
-gen.add('acc_lim_x', double_t, 0, 'The acceleration limit of the robot in the x direction in m/s^2', 2.5)
-gen.add('acc_lim_y', double_t, 0, 'The acceleration limit of the robot in the y direction in m/s^2', 2.5)
-gen.add('acc_lim_theta', double_t, 0, 'The acceleration limit of the robot in the theta direction in rad/s^2', 3.2)
-
-gen.add('decel_lim_x', double_t, 0, 'The deceleration limit of the robot in the x direction in m/s^2', -2.5)
-gen.add('decel_lim_y', double_t, 0, 'The deceleration limit of the robot in the y direction in m/s^2', -2.5)
-gen.add('decel_lim_theta', double_t, 0, 'The deceleration limit of the robot in the theta direction in rad/s^2', -3.2)
-
-gen.add('min_speed_xy', double_t, 0, 'The absolute value of the minimum translational/xy velocity in m/s. '
-                                     'If the value is negative, then the min speed will be arbitrarily close to 0.0. '
-                                     'Previously called min_trans_vel', 0.1)
-gen.add('max_speed_xy', double_t, 0, 'The absolute value of the maximum translational/xy velocity in m/s. '
-                                     'If the value is negative, then the max speed is hypot(max_vel_x, max_vel_y). '
-                                     'Previously called max_trans_vel', 0.55)
-gen.add('min_speed_theta', double_t, 0, 'The absolute value of the minimum rotational velocity in rad/s. '
-                                        'If the value is negative, then the min speed will be arbitrarily close to 0.0.'
-                                        ' Previously called min_rot_vel', 0.4)
-
-exit(gen.generate('dwb_plugins', 'dwb_plugins', 'KinematicParams'))

From 6a52f463f54bc289df663aba208e2b363f0a755c Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor
 <61468301+stevedanomodolor@users.noreply.github.com>
Date: Tue, 18 Jun 2024 19:55:58 +0200
Subject: [PATCH 09/18] Port Localization nav2_system_tests to new gazebo
 (#4431)

* include localization

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* uncomment error code in cmakelist

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* formmatting

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* formmatting

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* formmatting python

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* formmatting python

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* removed unused path

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

---------

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
---
 nav2_system_tests/CMakeLists.txt              |  2 +-
 .../src/localization/CMakeLists.txt           |  3 --
 .../localization/test_localization_launch.py  | 51 +++++++++++++++----
 3 files changed, 43 insertions(+), 13 deletions(-)

diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index 45a8097b35..d94298da77 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -114,7 +114,7 @@ if(BUILD_TESTING)
   add_subdirectory(src/behavior_tree)
   add_subdirectory(src/planning)
   # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
-  # add_subdirectory(src/localization)
+  add_subdirectory(src/localization)
   # add_subdirectory(src/system)
   # add_subdirectory(src/system_failure)
   # add_subdirectory(src/updown)
diff --git a/nav2_system_tests/src/localization/CMakeLists.txt b/nav2_system_tests/src/localization/CMakeLists.txt
index 740d38f53e..7fa35172c3 100644
--- a/nav2_system_tests/src/localization/CMakeLists.txt
+++ b/nav2_system_tests/src/localization/CMakeLists.txt
@@ -11,8 +11,5 @@ ament_add_test(test_localization
   COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_localization_launch.py"
   TIMEOUT 180
   ENV
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
     TEST_EXECUTABLE=$<TARGET_FILE:test_localization_node>
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
 )
diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py
index d638e02c4e..aedca4fcf7 100755
--- a/nav2_system_tests/src/localization/test_localization_launch.py
+++ b/nav2_system_tests/src/localization/test_localization_launch.py
@@ -17,23 +17,54 @@
 import os
 import sys
 
+from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch import LaunchService
-import launch.actions
-from launch.actions import ExecuteProcess
+from launch.actions import AppendEnvironmentVariable, ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
 
 def main(argv=sys.argv[1:]):
-    mapFile = os.getenv('TEST_MAP')
     testExecutable = os.getenv('TEST_EXECUTABLE')
-    world = os.getenv('TEST_WORLD')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
 
-    launch_gazebo = launch.actions.ExecuteProcess(
-        cmd=['gzserver', '-s', 'libgazebo_ros_init.so', '--minimal_comms', world],
-        output='screen',
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
+
+    set_env_vars_resources = AppendEnvironmentVariable(
+        'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
+    )
+
+    start_gazebo_server = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+        ),
+        launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+    )
+
+    spawn_robot = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+        ),
+        launch_arguments={
+            'use_sim_time': 'True',
+            'robot_sdf': robot_sdf,
+            'x_pose': '-2.0',
+            'y_pose': '-0.5',
+            'z_pose': '0.01',
+            'roll': '0.0',
+            'pitch': '0.0',
+            'yaw': '0.0',
+        }.items(),
     )
+
     link_footprint = launch_ros.actions.Node(
         package='tf2_ros',
         executable='static_transform_publisher',
@@ -51,7 +82,7 @@ def main(argv=sys.argv[1:]):
         executable='map_server',
         name='map_server',
         output='screen',
-        parameters=[{'yaml_filename': mapFile}],
+        parameters=[{'yaml_filename': map_yaml_file}],
     )
     run_amcl = launch_ros.actions.Node(
         package='nav2_amcl', executable='amcl', output='screen'
@@ -65,7 +96,9 @@ def main(argv=sys.argv[1:]):
     )
     ld = LaunchDescription(
         [
-            launch_gazebo,
+            set_env_vars_resources,
+            start_gazebo_server,
+            spawn_robot,
             link_footprint,
             footprint_scan,
             run_map_server,

From 8f795a2bc5802b71da982d9ab48910c7d3dfc6da Mon Sep 17 00:00:00 2001
From: Jatin Patil <89979346+JatinPatil2003@users.noreply.github.com>
Date: Tue, 18 Jun 2024 23:56:32 +0530
Subject: [PATCH 10/18] Added RPP use_cancel_deceleration parameter (#4441)

* Added RPP use_cancel_deceleration parameter

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

* Fixed Linting code style

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>

---------

Signed-off-by: Jatin Patil <jatinpatil2003@gmail.com>
---
 .../parameter_handler.hpp                                    | 1 +
 .../src/parameter_handler.cpp                                | 5 +++++
 .../src/regulated_pure_pursuit_controller.cpp                | 4 ++++
 3 files changed, 10 insertions(+)

diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
index d2258ceb80..b033289622 100644
--- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
+++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp
@@ -53,6 +53,7 @@ struct Parameters
   double curvature_lookahead_dist;
   bool use_rotate_to_heading;
   double max_angular_accel;
+  bool use_cancel_deceleration;
   double cancel_deceleration;
   double rotate_to_heading_min_angle;
   bool allow_reversing;
diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
index a47af13ef0..42876c248e 100644
--- a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp
@@ -85,6 +85,8 @@ ParameterHandler::ParameterHandler(
     node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
+  declare_parameter_if_not_declared(
+    node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false));
   declare_parameter_if_not_declared(
     node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2));
   declare_parameter_if_not_declared(
@@ -153,6 +155,7 @@ ParameterHandler::ParameterHandler(
   node->get_parameter(
     plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
   node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
+  node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration);
   node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration);
   node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing);
   node->get_parameter(
@@ -258,6 +261,8 @@ ParameterHandler::dynamicParametersCallback(
         params_.use_collision_detection = parameter.as_bool();
       } else if (name == plugin_name_ + ".use_rotate_to_heading") {
         params_.use_rotate_to_heading = parameter.as_bool();
+      } else if (name == plugin_name_ + ".use_cancel_deceleration") {
+        params_.use_cancel_deceleration = parameter.as_bool();
       } else if (name == plugin_name_ + ".allow_reversing") {
         if (params_.use_rotate_to_heading && parameter.as_bool()) {
           RCLCPP_WARN(
diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
index 423d0d1a04..edcd6db558 100644
--- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
+++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
@@ -290,6 +290,10 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
 
 bool RegulatedPurePursuitController::cancel()
 {
+  // if false then publish zero velocity
+  if (!params_->use_cancel_deceleration) {
+    return true;
+  }
   cancelling_ = true;
   return finished_cancelling_;
 }

From 23a48154bef1be3b063c7566698a2fdf4f497f66 Mon Sep 17 00:00:00 2001
From: Alberto Tudela <ajtudela@gmail.com>
Date: Tue, 18 Jun 2024 20:48:30 +0200
Subject: [PATCH 11/18] Fix unexpected behavior of docking when already in
 charger (#4448)

* Check isDocked before dock/undock

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Linting

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Add testing

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
---
 nav2_docking/opennav_docking/src/docking_server.cpp  | 12 ++++++++++++
 .../test/test_docking_server_unit.cpp                |  8 +++++---
 nav2_docking/opennav_docking/test/testing_dock.cpp   |  4 +++-
 3 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/nav2_docking/opennav_docking/src/docking_server.cpp b/nav2_docking/opennav_docking/src/docking_server.cpp
index c70e4b29aa..2d533c0871 100644
--- a/nav2_docking/opennav_docking/src/docking_server.cpp
+++ b/nav2_docking/opennav_docking/src/docking_server.cpp
@@ -235,6 +235,12 @@ void DockingServer::dockRobot()
       dock = generateGoalDock(goal);
     }
 
+    // Check if the robot is docked or charging before proceeding
+    if (dock->plugin->isDocked() || dock->plugin->isCharging()) {
+      RCLCPP_INFO(get_logger(), "Robot is already charging, no need to dock");
+      return;
+    }
+
     // Send robot to its staging pose
     publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE);
     const auto initial_staging_pose = dock->getStagingPose();
@@ -569,6 +575,12 @@ void DockingServer::undockRobot()
       get_logger(),
       "Attempting to undock robot from charger of type %s.", dock->getName().c_str());
 
+    // Check if the robot is docked before proceeding
+    if (!dock->isDocked()) {
+      RCLCPP_INFO(get_logger(), "Robot is not in the charger, no need to undock");
+      return;
+    }
+
     // Get "dock pose" by finding the robot pose
     geometry_msgs::msg::PoseStamped dock_pose = getRobotPoseInFrame(fixed_frame_);
 
diff --git a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
index 74da62205f..144a612f3b 100644
--- a/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
+++ b/nav2_docking/opennav_docking/test/test_docking_server_unit.cpp
@@ -86,9 +86,8 @@ TEST(DockingServerTests, testErrorExceptions)
   node->on_configure(rclcpp_lifecycle::State());
   node->on_activate(rclcpp_lifecycle::State());
 
-  node->declare_parameter(
-    "exception_to_throw",
-    rclcpp::ParameterValue(""));
+  node->declare_parameter("exception_to_throw", rclcpp::ParameterValue(""));
+  node->declare_parameter("dock_action_called", rclcpp::ParameterValue(false));
 
   // Error codes docking
   std::vector<std::string> error_ids{
@@ -142,6 +141,9 @@ TEST(DockingServerTests, testErrorExceptions)
     "DockingException", "exception"};
   std::vector<int> error_codes_undocking{999, 902, 905, 999, 999};
 
+  // Set dock_action_called to true to simulate robot being docked in dock plugin
+  node->set_parameter(rclcpp::Parameter("dock_action_called", true));
+
   // Call action, check error code
   for (unsigned int i = 0; i != error_ids_undocking.size(); i++) {
     node->set_parameter(
diff --git a/nav2_docking/opennav_docking/test/testing_dock.cpp b/nav2_docking/opennav_docking/test/testing_dock.cpp
index a51924533e..4c17eb000c 100644
--- a/nav2_docking/opennav_docking/test/testing_dock.cpp
+++ b/nav2_docking/opennav_docking/test/testing_dock.cpp
@@ -80,7 +80,9 @@ class TestFailureDock : public opennav_docking_core::ChargingDock
 
   virtual bool isDocked()
   {
-    return false;
+    bool dock_action_called;
+    node_->get_parameter("dock_action_called", dock_action_called);
+    return dock_action_called;
   }
 
   virtual bool isCharging()

From 6026754da6d1fb2986b4c6be260ea8f500768a0e Mon Sep 17 00:00:00 2001
From: Chris Lalancette <clalancette@gmail.com>
Date: Wed, 19 Jun 2024 11:28:16 -0400
Subject: [PATCH 12/18] Revamp nav2_util CMakeLists.txt to use modern idioms.
 (#4393)

This commit does a number of things:

1.  Switches to using target_link_libraries everywhere.
This gives us finer-grained control over what dependencies
are exported to downstream as public, or private.  In the
particular case of nav2_util, this actually doesn't matter
*too* much, but it will help for other packages.
2.  Moves the include directory down one level to
include/${PROJECT_NAME}, which is best practice in ROS 2
since Humble.
3.  Makes sure to export nav2_util as a CMake target, so
downstream users of it can use that target.
4.  Moves the base_footprint_publisher.hpp header file into
the src directory, as it isn't functionality that an external
project could use.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
---
 nav2_util/CMakeLists.txt                      | 59 +++++++++----------
 .../include/nav2_util/lifecycle_node.hpp      |  1 +
 nav2_util/package.xml                         | 30 ++++------
 nav2_util/src/CMakeLists.txt                  | 39 +++++++-----
 nav2_util/src/base_footprint_publisher.cpp    |  2 +-
 .../base_footprint_publisher.hpp              |  6 +-
 nav2_util/src/lifecycle_utils.cpp             | 19 +++---
 nav2_util/src/string_utils.cpp                | 10 ++--
 nav2_util/test/CMakeLists.txt                 | 39 +++++-------
 .../test/test_base_footprint_publisher.cpp    |  2 +-
 10 files changed, 96 insertions(+), 111 deletions(-)
 rename nav2_util/{include/nav2_util => src}/base_footprint_publisher.hpp (96%)

diff --git a/nav2_util/CMakeLists.txt b/nav2_util/CMakeLists.txt
index 1c8b9b1d64..6a82f01511 100644
--- a/nav2_util/CMakeLists.txt
+++ b/nav2_util/CMakeLists.txt
@@ -2,54 +2,36 @@ cmake_minimum_required(VERSION 3.5)
 project(nav2_util)
 
 find_package(ament_cmake REQUIRED)
+find_package(bond REQUIRED)
+find_package(bondcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(nav2_msgs REQUIRED)
-find_package(tf2_ros REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_geometry_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(lifecycle_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-find_package(bondcpp REQUIRED)
-find_package(bond REQUIRED)
-find_package(action_msgs REQUIRED)
-
-set(dependencies
-    nav2_msgs
-    tf2_ros
-    tf2
-    tf2_geometry_msgs
-    geometry_msgs
-    nav_msgs
-    rclcpp
-    lifecycle_msgs
-    rclcpp_action
-    rclcpp_lifecycle
-    bondcpp
-    bond
-    action_msgs
-    rcl_interfaces
-)
+find_package(tf2_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
 
 nav2_package()
 
-include_directories(include)
-
 set(library_name ${PROJECT_NAME}_core)
+
 add_subdirectory(src)
 
 install(DIRECTORY include/
-  DESTINATION include/
+  DESTINATION include/${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_cmake_pytest REQUIRED)
-  find_package(launch_testing_ament_cmake REQUIRED)
   # skip copyright linting
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
@@ -58,8 +40,23 @@ if(BUILD_TESTING)
   add_subdirectory(test)
 endif()
 
-ament_export_include_directories(include)
+ament_export_include_directories(include/${PROJECT_NAME})
 ament_export_libraries(${library_name})
-ament_export_dependencies(${dependencies})
+ament_export_dependencies(
+  bondcpp
+  geometry_msgs
+  lifecycle_msgs
+  nav2_msgs
+  nav_msgs
+  rcl_interfaces
+  rclcpp
+  rclcpp_action
+  rclcpp_lifecycle
+  std_msgs
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+)
+ament_export_targets(${library_name})
 
 ament_package()
diff --git a/nav2_util/include/nav2_util/lifecycle_node.hpp b/nav2_util/include/nav2_util/lifecycle_node.hpp
index 7f1ef07157..d652982aef 100644
--- a/nav2_util/include/nav2_util/lifecycle_node.hpp
+++ b/nav2_util/include/nav2_util/lifecycle_node.hpp
@@ -19,6 +19,7 @@
 #include <string>
 #include <thread>
 
+#include "rcl_interfaces/msg/parameter_descriptor.hpp"
 #include "nav2_util/node_thread.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 #include "rclcpp/rclcpp.hpp"
diff --git a/nav2_util/package.xml b/nav2_util/package.xml
index 9de53f5fd5..26274e10d6 100644
--- a/nav2_util/package.xml
+++ b/nav2_util/package.xml
@@ -10,37 +10,31 @@
   <license>BSD-3-Clause</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>libboost-program-options-dev</build_depend>
+
   <depend>nav2_common</depend>
 
+  <depend>bond</depend>
+  <depend>bondcpp</depend>
+  <depend>builtin_interfaces</depend>
   <depend>geometry_msgs</depend>
-  <depend>rclcpp</depend>
+  <depend>lifecycle_msgs</depend>
   <depend>nav2_msgs</depend>
   <depend>nav_msgs</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>lifecycle_msgs</depend>
-  <depend>bondcpp</depend>
-  <depend>bond</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
   <depend>rclcpp_lifecycle</depend>
-  <depend>launch</depend>
-  <depend>launch_testing_ament_cmake</depend>
-  <depend>action_msgs</depend>
-  <depend>rcl_interfaces</depend>
-
-  <exec_depend>libboost-program-options</exec_depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
-  <test_depend>launch</test_depend>
-  <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>std_srvs</test_depend>
   <test_depend>test_msgs</test_depend>
-  <test_depend>action_msgs</test_depend>
-  <test_depend>launch_testing_ros</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
 
   <export>
diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt
index 6ddcdc6d32..96f69b5458 100644
--- a/nav2_util/src/CMakeLists.txt
+++ b/nav2_util/src/CMakeLists.txt
@@ -10,34 +10,41 @@ add_library(${library_name} SHARED
   odometry_utils.cpp
   array_parser.cpp
 )
-
-ament_target_dependencies(${library_name}
-  rclcpp
-  nav2_msgs
-  tf2
-  tf2_ros
-  nav_msgs
-  geometry_msgs
-  lifecycle_msgs
-  rclcpp_lifecycle
-  tf2_geometry_msgs
-  bondcpp
+target_include_directories(${library_name}
+  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+target_link_libraries(${library_name} PUBLIC
+  bondcpp::bondcpp
+  ${geometry_msgs_TARGETS}
+  ${lifecycle_msgs_TARGETS}
+  ${nav2_msgs_TARGETS}
+  ${nav_msgs_TARGETS}
+  ${rcl_interfaces_TARGETS}
+  rclcpp::rclcpp
+  rclcpp_action::rclcpp_action
+  rclcpp_lifecycle::rclcpp_lifecycle
+  tf2_ros::tf2_ros
+  tf2::tf2
+  ${tf2_geometry_msgs_TARGETS}
+)
+target_link_libraries(${library_name} PRIVATE
+  ${bond_TARGETS}
 )
 
 add_executable(lifecycle_bringup
   lifecycle_bringup_commandline.cpp
 )
-target_link_libraries(lifecycle_bringup ${library_name})
+target_link_libraries(lifecycle_bringup PRIVATE ${library_name} rclcpp::rclcpp)
 
 add_executable(base_footprint_publisher
   base_footprint_publisher.cpp
 )
-target_link_libraries(base_footprint_publisher ${library_name})
-
-find_package(Boost REQUIRED COMPONENTS program_options)
+target_link_libraries(base_footprint_publisher PRIVATE ${library_name} rclcpp::rclcpp ${tf2_msgs_TARGETS})
 
 install(TARGETS
   ${library_name}
+  EXPORT ${library_name}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp
index f3b6791db4..144ba14ab6 100644
--- a/nav2_util/src/base_footprint_publisher.cpp
+++ b/nav2_util/src/base_footprint_publisher.cpp
@@ -14,7 +14,7 @@
 
 #include <memory>
 
-#include "nav2_util/base_footprint_publisher.hpp"
+#include "base_footprint_publisher.hpp"
 
 int main(int argc, char ** argv)
 {
diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/src/base_footprint_publisher.hpp
similarity index 96%
rename from nav2_util/include/nav2_util/base_footprint_publisher.hpp
rename to nav2_util/src/base_footprint_publisher.hpp
index 52bcdb53eb..25d58d504b 100644
--- a/nav2_util/include/nav2_util/base_footprint_publisher.hpp
+++ b/nav2_util/src/base_footprint_publisher.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_
-#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_
+#ifndef BASE_FOOTPRINT_PUBLISHER_HPP_
+#define BASE_FOOTPRINT_PUBLISHER_HPP_
 
 #include <string>
 #include <memory>
@@ -126,4 +126,4 @@ class BaseFootprintPublisher : public rclcpp::Node
 
 }  // end namespace nav2_util
 
-#endif  // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_
+#endif  // BASE_FOOTPRINT_PUBLISHER_HPP_
diff --git a/nav2_util/src/lifecycle_utils.cpp b/nav2_util/src/lifecycle_utils.cpp
index c778e2abdf..e84bbbc55b 100644
--- a/nav2_util/src/lifecycle_utils.cpp
+++ b/nav2_util/src/lifecycle_utils.cpp
@@ -13,16 +13,13 @@
 // limitations under the License.
 
 #include <chrono>
+#include <stdexcept>
 #include <string>
-#include <thread>
 #include <vector>
 
-#include "lifecycle_msgs/srv/change_state.hpp"
-#include "lifecycle_msgs/srv/get_state.hpp"
-#include "nav2_util/lifecycle_service_client.hpp"
+#include "lifecycle_msgs/msg/transition.hpp"
 
-using std::string;
-using lifecycle_msgs::msg::Transition;
+#include "nav2_util/lifecycle_service_client.hpp"
 
 namespace nav2_util
 {
@@ -34,7 +31,7 @@ namespace nav2_util
       try { \
         fn; \
         break; \
-      } catch (std::runtime_error & e) { \
+      } catch (const std::runtime_error & e) { \
         ++count; \
         if (count > (retries)) { \
           throw e;} \
@@ -53,10 +50,10 @@ static void startupLifecycleNode(
   // service calls still frequently hang. To get reliable startup it's necessary
   // to timeout the service call and retry it when that happens.
   RETRY(
-    sc.change_state(Transition::TRANSITION_CONFIGURE, service_call_timeout),
+    sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, service_call_timeout),
     retries);
   RETRY(
-    sc.change_state(Transition::TRANSITION_ACTIVATE, service_call_timeout),
+    sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE, service_call_timeout),
     retries);
 }
 
@@ -81,10 +78,10 @@ static void resetLifecycleNode(
   // service calls still frequently hang. To get reliable reset it's necessary
   // to timeout the service call and retry it when that happens.
   RETRY(
-    sc.change_state(Transition::TRANSITION_DEACTIVATE, service_call_timeout),
+    sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, service_call_timeout),
     retries);
   RETRY(
-    sc.change_state(Transition::TRANSITION_CLEANUP, service_call_timeout),
+    sc.change_state(lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP, service_call_timeout),
     retries);
 }
 
diff --git a/nav2_util/src/string_utils.cpp b/nav2_util/src/string_utils.cpp
index 47d86aaf57..df447b5492 100644
--- a/nav2_util/src/string_utils.cpp
+++ b/nav2_util/src/string_utils.cpp
@@ -15,14 +15,12 @@
 #include "nav2_util/string_utils.hpp"
 #include <string>
 
-using std::string;
-
 namespace nav2_util
 {
 
-std::string strip_leading_slash(const string & in)
+std::string strip_leading_slash(const std::string & in)
 {
-  string out = in;
+  std::string out = in;
 
   if ((!in.empty()) && (in[0] == '/')) {
     out.erase(0, 1);
@@ -31,13 +29,13 @@ std::string strip_leading_slash(const string & in)
   return out;
 }
 
-Tokens split(const string & tokenstring, char delimiter)
+Tokens split(const std::string & tokenstring, char delimiter)
 {
   Tokens tokens;
 
   size_t current_pos = 0;
   size_t pos = 0;
-  while ((pos = tokenstring.find(delimiter, current_pos)) != string::npos) {
+  while ((pos = tokenstring.find(delimiter, current_pos)) != std::string::npos) {
     tokens.push_back(tokenstring.substr(current_pos, pos - current_pos));
     current_pos = pos + 1;
   }
diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt
index 4c0f274721..14d774c298 100644
--- a/nav2_util/test/CMakeLists.txt
+++ b/nav2_util/test/CMakeLists.txt
@@ -1,4 +1,5 @@
 ament_add_gtest(test_execution_timer test_execution_timer.cpp)
+target_link_libraries(test_execution_timer ${library_name})
 
 ament_add_gtest(test_node_utils test_node_utils.cpp)
 target_link_libraries(test_node_utils ${library_name})
@@ -7,56 +8,46 @@ find_package(std_srvs REQUIRED)
 find_package(test_msgs REQUIRED)
 
 ament_add_gtest(test_service_client test_service_client.cpp)
-ament_target_dependencies(test_service_client std_srvs)
-target_link_libraries(test_service_client ${library_name})
+target_link_libraries(test_service_client ${library_name} ${std_srvs_TARGETS})
 
 ament_add_gtest(test_string_utils test_string_utils.cpp)
 target_link_libraries(test_string_utils ${library_name})
 
 find_package(rclcpp_lifecycle REQUIRED)
 ament_add_gtest(test_lifecycle_utils test_lifecycle_utils.cpp)
-ament_target_dependencies(test_lifecycle_utils rclcpp_lifecycle)
-target_link_libraries(test_lifecycle_utils ${library_name})
+target_link_libraries(test_lifecycle_utils ${library_name} rclcpp_lifecycle::rclcpp_lifecycle)
 
 ament_add_gtest(test_actions test_actions.cpp)
-ament_target_dependencies(test_actions rclcpp_action test_msgs)
-target_link_libraries(test_actions ${library_name})
+target_link_libraries(test_actions ${library_name} rclcpp_action::rclcpp_action ${test_msgs_TARGETS})
 
 ament_add_gtest(test_lifecycle_node test_lifecycle_node.cpp)
-ament_target_dependencies(test_lifecycle_node rclcpp_lifecycle)
-target_link_libraries(test_lifecycle_node ${library_name})
+target_link_libraries(test_lifecycle_node ${library_name} rclcpp_lifecycle::rclcpp_lifecycle)
 
 ament_add_gtest(test_lifecycle_cli_node test_lifecycle_cli_node.cpp)
-ament_target_dependencies(test_lifecycle_cli_node rclcpp_lifecycle)
-target_link_libraries(test_lifecycle_cli_node ${library_name})
+target_link_libraries(test_lifecycle_cli_node ${library_name} rclcpp_lifecycle::rclcpp_lifecycle)
 
 ament_add_gtest(test_geometry_utils test_geometry_utils.cpp)
-ament_target_dependencies(test_geometry_utils geometry_msgs)
-target_link_libraries(test_geometry_utils ${library_name})
+target_link_libraries(test_geometry_utils ${library_name} ${geometry_msgs_TARGETS})
 
 ament_add_gtest(test_odometry_utils test_odometry_utils.cpp)
-ament_target_dependencies(test_odometry_utils nav_msgs geometry_msgs)
-target_link_libraries(test_odometry_utils ${library_name})
+target_link_libraries(test_odometry_utils ${library_name} ${nav_msgs_TARGETS} ${geometry_msgs_TARGETS})
 
 ament_add_gtest(test_robot_utils test_robot_utils.cpp)
-ament_target_dependencies(test_robot_utils geometry_msgs)
-target_link_libraries(test_robot_utils ${library_name})
+target_link_libraries(test_robot_utils ${library_name} ${geometry_msgs_TARGETS})
 
 ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp)
-ament_target_dependencies(test_base_footprint_publisher geometry_msgs)
-target_link_libraries(test_base_footprint_publisher ${library_name})
+target_include_directories(test_base_footprint_publisher PRIVATE "$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/src>")
+
+target_link_libraries(test_base_footprint_publisher ${library_name} tf2_ros::tf2_ros rclcpp::rclcpp ${geometry_msgs_TARGETS})
 
 ament_add_gtest(test_array_parser test_array_parser.cpp)
 target_link_libraries(test_array_parser ${library_name})
 
 ament_add_gtest(test_twist_publisher test_twist_publisher.cpp)
-ament_target_dependencies(test_twist_publisher rclcpp_lifecycle)
-target_link_libraries(test_twist_publisher ${library_name})
+target_link_libraries(test_twist_publisher ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS})
 
 ament_add_gtest(test_twist_subscriber test_twist_subscriber.cpp)
-ament_target_dependencies(test_twist_subscriber rclcpp_lifecycle)
-target_link_libraries(test_twist_subscriber ${library_name})
+target_link_libraries(test_twist_subscriber ${library_name} rclcpp::rclcpp ${geometry_msgs_TARGETS})
 
 ament_add_gtest(test_validation_messages test_validation_messages.cpp)
-ament_target_dependencies(test_validation_messages rclcpp_lifecycle)
-target_link_libraries(test_validation_messages ${library_name})
\ No newline at end of file
+target_link_libraries(test_validation_messages ${library_name} ${builtin_interfaces_TARGETS} ${std_msgs_TARGETS} ${geometry_msgs_TARGETS})
diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp
index 47dc83c7f3..cd78883132 100644
--- a/nav2_util/test/test_base_footprint_publisher.cpp
+++ b/nav2_util/test/test_base_footprint_publisher.cpp
@@ -15,7 +15,7 @@
 #include <string>
 #include <memory>
 
-#include "nav2_util/base_footprint_publisher.hpp"
+#include "base_footprint_publisher.hpp"
 #include "gtest/gtest.h"
 #include "tf2/exceptions.h"
 

From 1788615b9b45ceec344c09fe882542e96de5e7b9 Mon Sep 17 00:00:00 2001
From: Vincent <46542431+VincidaB@users.noreply.github.com>
Date: Wed, 19 Jun 2024 10:37:41 -0500
Subject: [PATCH 13/18] precomputeDistanceHeuristic is now computed once
 (#4451)

Signed-off-by: Vincent Belpois <vincent.belpois@gmail.com>
Co-authored-by: SiddharthaUpase <s1dupase34@gmail.com>
---
 nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 1 +
 nav2_smac_planner/src/a_star.cpp                       | 6 +++++-
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
index 64c905d01c..c95d8c82cf 100644
--- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
+++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp
@@ -251,6 +251,7 @@ class AStarAlgorithm
     const NodePtr & node, std::vector<std::tuple<float, float, float>> * expansions_log);
 
   bool _traverse_unknown;
+  bool _is_initialized;
   int _max_iterations;
   int _max_on_approach_iterations;
   int _terminal_checking_interval;
diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp
index a3cfbb4b4f..3c22ce450f 100644
--- a/nav2_smac_planner/src/a_star.cpp
+++ b/nav2_smac_planner/src/a_star.cpp
@@ -36,6 +36,7 @@ AStarAlgorithm<NodeT>::AStarAlgorithm(
   const MotionModel & motion_model,
   const SearchInfo & search_info)
 : _traverse_unknown(true),
+  _is_initialized(false),
   _max_iterations(0),
   _terminal_checking_interval(5000),
   _max_planning_time(0),
@@ -70,7 +71,10 @@ void AStarAlgorithm<NodeT>::initialize(
   _max_on_approach_iterations = max_on_approach_iterations;
   _terminal_checking_interval = terminal_checking_interval;
   _max_planning_time = max_planning_time;
-  NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
+  if(!_is_initialized) {
+    NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
+  }
+  _is_initialized = true;
   _dim3_size = dim_3_size;
   _expander = std::make_unique<AnalyticExpansion<NodeT>>(
     _motion_model, _search_info, _traverse_unknown, _dim3_size);

From fb5f29d34f687fa02858951c86e59e4dd5619389 Mon Sep 17 00:00:00 2001
From: Steve Macenski <stevenmacenski@gmail.com>
Date: Wed, 19 Jun 2024 11:48:16 -0700
Subject: [PATCH 14/18] Fix CI with BT test from btv4.X API update (#4452)

* fix CI with BT test from btv4.X API update

* Update test_bt_utils.cpp

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

---------

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
---
 nav2_behavior_tree/test/test_bt_utils.cpp | 55 +++--------------------
 1 file changed, 6 insertions(+), 49 deletions(-)

diff --git a/nav2_behavior_tree/test/test_bt_utils.cpp b/nav2_behavior_tree/test/test_bt_utils.cpp
index 1c008d6478..d495587955 100644
--- a/nav2_behavior_tree/test/test_bt_utils.cpp
+++ b/nav2_behavior_tree/test/test_bt_utils.cpp
@@ -59,13 +59,7 @@ TEST(PointPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::Point>>("PointPort");
-  auto tree = factory.createTreeFromText(xml_txt);
-
-  geometry_msgs::msg::Point value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   xml_txt =
     R"(
@@ -75,11 +69,7 @@ TEST(PointPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
 TEST(PointPortTest, test_correct_syntax)
@@ -115,14 +105,8 @@ TEST(QuaternionPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::Quaternion>>("QuaternionPort");
-  auto tree = factory.createTreeFromText(xml_txt);
 
-  geometry_msgs::msg::Quaternion value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
-  EXPECT_EQ(value.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   xml_txt =
     R"(
@@ -132,12 +116,7 @@ TEST(QuaternionPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(value.x, 0.0);
-  EXPECT_EQ(value.y, 0.0);
-  EXPECT_EQ(value.z, 0.0);
-  EXPECT_EQ(value.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
 TEST(QuaternionPortTest, test_correct_syntax)
@@ -174,19 +153,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax)
 
   BT::BehaviorTreeFactory factory;
   factory.registerNodeType<TestNode<geometry_msgs::msg::PoseStamped>>("PoseStampedPort");
-  auto tree = factory.createTreeFromText(xml_txt);
-
-  geometry_msgs::msg::PoseStamped value;
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
-  EXPECT_EQ(value.header.frame_id, "");
-  EXPECT_EQ(value.pose.position.x, 0.0);
-  EXPECT_EQ(value.pose.position.y, 0.0);
-  EXPECT_EQ(value.pose.position.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.x, 0.0);
-  EXPECT_EQ(value.pose.orientation.y, 0.0);
-  EXPECT_EQ(value.pose.orientation.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 
   xml_txt =
     R"(
@@ -196,17 +163,7 @@ TEST(PoseStampedPortTest, test_wrong_syntax)
         </BehaviorTree>
       </root>)";
 
-  tree = factory.createTreeFromText(xml_txt);
-  tree.rootNode()->getInput("test", value);
-  EXPECT_EQ(rclcpp::Time(value.header.stamp).nanoseconds(), 0);
-  EXPECT_EQ(value.header.frame_id, "");
-  EXPECT_EQ(value.pose.position.x, 0.0);
-  EXPECT_EQ(value.pose.position.y, 0.0);
-  EXPECT_EQ(value.pose.position.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.x, 0.0);
-  EXPECT_EQ(value.pose.orientation.y, 0.0);
-  EXPECT_EQ(value.pose.orientation.z, 0.0);
-  EXPECT_EQ(value.pose.orientation.w, 1.0);
+  EXPECT_THROW(factory.createTreeFromText(xml_txt), std::exception);
 }
 
 TEST(PoseStampedPortTest, test_correct_syntax)

From 43b238866655f749af5c6dcf6e1f2dff637fbce4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Carlos=20Garc=C3=ADa?=
 <40342885+jcarlosgm30@users.noreply.github.com>
Date: Thu, 20 Jun 2024 17:54:59 +0200
Subject: [PATCH 15/18] [Docking] Feature: support for not needing to set dock
 instances (#4442)

* feat: support for not needing to set dock_instances

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* feat: improve logs for dock_database

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* chore: comment the dock instances out from nav2_params

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* chore: dock database cleanup

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

* fix: update reloadDBService from DatabaseTests

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>

---------

Signed-off-by: josegarcia <josegarcia@pal-robotics.com>
Co-authored-by: josegarcia <josegarcia@pal-robotics.com>
---
 nav2_bringup/params/nav2_params.yaml          | 11 +++---
 .../opennav_docking/src/dock_database.cpp     | 37 ++++++++++++++-----
 .../test/test_dock_database.cpp               |  5 ++-
 3 files changed, 37 insertions(+), 16 deletions(-)

diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index b43b70318c..307eb2f37a 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -407,11 +407,12 @@ docking_server:
       filter_coef: 0.1
 
     # Dock instances
-    docks: ['home_dock']  # Input your docks here
-    home_dock:
-      type: 'simple_charging_dock'
-      frame: map
-      pose: [0.0, 0.0, 0.0]
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
 
     controller:
       k_phi: 3.0
diff --git a/nav2_docking/opennav_docking/src/dock_database.cpp b/nav2_docking/opennav_docking/src/dock_database.cpp
index 6774e93620..83ef42cd67 100644
--- a/nav2_docking/opennav_docking/src/dock_database.cpp
+++ b/nav2_docking/opennav_docking/src/dock_database.cpp
@@ -34,21 +34,32 @@ bool DockDatabase::initialize(
   node_ = parent;
   auto node = node_.lock();
 
-  if (getDockPlugins(node, tf) && getDockInstances(node)) {
-    RCLCPP_INFO(
+  if (!getDockPlugins(node, tf)) {
+    RCLCPP_ERROR(
       node->get_logger(),
-      "Docking Server has %u dock types and %u dock instances available.",
-      this->plugin_size(), this->instance_size());
-    return true;
+      "An error occurred while getting the dock plugins!");
+    return false;
+  }
+
+  if (!getDockInstances(node)) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "An error occurred while getting the dock instances!");
+    return false;
   }
 
+  RCLCPP_INFO(
+    node->get_logger(),
+    "Docking Server has %u dock types and %u dock instances available.",
+    this->plugin_size(), this->instance_size());
+
   reload_db_service_ = node->create_service<nav2_msgs::srv::ReloadDockDatabase>(
     "~/reload_database",
     std::bind(
       &DockDatabase::reloadDbCb, this,
       std::placeholders::_1, std::placeholders::_2));
 
-  return false;
+  return true;
 }
 
 void DockDatabase::activate()
@@ -71,10 +82,14 @@ void DockDatabase::reloadDbCb(
   const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
   std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response)
 {
+  auto node = node_.lock();
   DockMap dock_instances;
-  if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) {
+  if (utils::parseDockFile(request->filepath, node, dock_instances)) {
     dock_instances_ = dock_instances;
     response->success = true;
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Dock database reloaded from file %s.", request->filepath.c_str());
     return;
   }
   response->success = false;
@@ -194,10 +209,12 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share
     return utils::parseDockParams(docks_param, node, dock_instances_);
   }
 
-  RCLCPP_ERROR(
+  RCLCPP_WARN(
     node->get_logger(),
-    "Dock database filepath nor dock parameters set. Unable to perform docking actions.");
-  return false;
+    "Dock database filepath nor dock parameters set. "
+    "Docking actions can only be executed specifying the dock pose via the action request. "
+    "Or update the dock database via the reload_database service.");
+  return true;
 }
 
 unsigned int DockDatabase::plugin_size() const
diff --git a/nav2_docking/opennav_docking/test/test_dock_database.cpp b/nav2_docking/opennav_docking/test/test_dock_database.cpp
index 2753f34f1b..0285e30f4a 100644
--- a/nav2_docking/opennav_docking/test/test_dock_database.cpp
+++ b/nav2_docking/opennav_docking/test/test_dock_database.cpp
@@ -102,8 +102,11 @@ TEST(DatabaseTests, findTests)
 TEST(DatabaseTests, reloadDbService)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
-  std::vector<std::string> plugins{"dockv1", "dockv2"};
+  std::vector<std::string> plugins{"dockv1"};
   node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
   opennav_docking::DockDatabase db;
   db.initialize(node, nullptr);
 

From 90574567025cce3d7af0f0710bfabb4533c211fe Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor
 <61468301+stevedanomodolor@users.noreply.github.com>
Date: Thu, 20 Jun 2024 22:42:41 +0200
Subject: [PATCH 16/18] Port system tests in nav2_system_tests (#4440)

* wip, ported only test_bt_navigator

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* include test_bt_navigator_with dijlstra and test_bt_navigator_2

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* uncomment some lines

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* More tests

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* Include end of line

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* move gz_sim cleanup process to utils nav2_simple_commander

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* fix linter

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* cleanup

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* removed unused path

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* cleanup

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* more cleanup

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* reduce set initial pose time

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* remove repeated variable

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* Remove log

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* Remove todo

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* use robot publisher

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* use robot publisher

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* include copyright

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* correct year

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

---------

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
---
 .../nav2_simple_commander/utils.py            |  55 ++
 nav2_system_tests/CMakeLists.txt              |   7 +-
 nav2_system_tests/models/cardboard_box.sdf    |   9 +
 nav2_system_tests/package.xml                 |   2 +
 .../localization/test_localization_launch.py  |  37 +-
 nav2_system_tests/src/system/CMakeLists.txt   |  24 +-
 .../src/system/nav2_system_params.yaml        |  46 ++
 .../src/system/nav_to_pose_tester_node.py     |  14 +-
 .../src/system/test_system_launch.py          |  77 ++-
 .../test_system_with_obstacle_launch.py       | 190 ++++++
 .../src/system/test_wrong_init_pose_launch.py |  81 ++-
 nav2_system_tests/urdf/common_properties.urdf |  45 --
 nav2_system_tests/urdf/turtlebot3_burger.urdf | 155 -----
 nav2_system_tests/urdf/turtlebot3_waffle.urdf | 279 ---------
 .../worlds/turtlebot3_ros2_demo.world         | 497 ----------------
 .../worlds/turtlebot3_ros2_demo_gps.world     | 555 -----------------
 .../turtlebot3_ros2_demo_obstacle.world       | 558 ------------------
 nav2_system_tests/worlds/world_only.model     |  54 --
 18 files changed, 451 insertions(+), 2234 deletions(-)
 create mode 100644 nav2_simple_commander/nav2_simple_commander/utils.py
 create mode 100644 nav2_system_tests/models/cardboard_box.sdf
 create mode 100755 nav2_system_tests/src/system/test_system_with_obstacle_launch.py
 delete mode 100644 nav2_system_tests/urdf/common_properties.urdf
 delete mode 100644 nav2_system_tests/urdf/turtlebot3_burger.urdf
 delete mode 100644 nav2_system_tests/urdf/turtlebot3_waffle.urdf
 delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo.world
 delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world
 delete mode 100644 nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world
 delete mode 100644 nav2_system_tests/worlds/world_only.model

diff --git a/nav2_simple_commander/nav2_simple_commander/utils.py b/nav2_simple_commander/nav2_simple_commander/utils.py
new file mode 100644
index 0000000000..d363901020
--- /dev/null
+++ b/nav2_simple_commander/nav2_simple_commander/utils.py
@@ -0,0 +1,55 @@
+#! /usr/bin/env python3
+# Copyright 2024 Open Navigation LLC
+# Copyright 2024 Stevedan Ogochukwu Omodolor Omodia
+#
+# 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.
+
+"""General utility functions."""
+
+import os
+import signal
+import subprocess
+
+
+def find_os_processes(name):
+    """Find all the processes that are running gz sim."""
+    ps_output = subprocess.check_output(['ps', 'aux'], text=True)
+    ps_lines = ps_output.split('\n')
+    gz_sim_processes = []
+    for line in ps_lines:
+        if name in line:
+            columns = line.split()
+            pid = columns[1]
+            command = ' '.join(columns[10:])
+            if command.startswith(name):
+                gz_sim_processes.append((pid, command))
+    return gz_sim_processes
+
+
+def kill_process(pid):
+    """Kill a process with a given PID."""
+    try:
+        os.kill(int(pid), signal.SIGKILL)
+        print(f'Successfully killed process with PID: {pid}')
+    except Exception as e:
+        print(f'Failed to kill process with PID: {pid}. Error: {e}')
+
+
+def kill_os_processes(name):
+    """Kill all processes that are running with name."""
+    processes = find_os_processes(name)
+    if processes:
+        for pid, _ in processes:
+            kill_process(pid)
+    else:
+        print(f'No processes found starting with {name}')
diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index d94298da77..baa1256d52 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -28,6 +28,7 @@ find_package(navigation2)
 find_package(angles REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(nav2_minimal_tb3_sim REQUIRED)
 
 nav2_package()
 
@@ -113,9 +114,9 @@ if(BUILD_TESTING)
 
   add_subdirectory(src/behavior_tree)
   add_subdirectory(src/planning)
-  # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
   add_subdirectory(src/localization)
-  # add_subdirectory(src/system)
+  add_subdirectory(src/system)
+  # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
   # add_subdirectory(src/system_failure)
   # add_subdirectory(src/updown)
   # add_subdirectory(src/waypoint_follower)
@@ -127,7 +128,7 @@ if(BUILD_TESTING)
   # add_subdirectory(src/behaviors/assisted_teleop)
   # add_subdirectory(src/costmap_filters)
   add_subdirectory(src/error_codes)
-  install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+  install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME})
 
 endif()
 
diff --git a/nav2_system_tests/models/cardboard_box.sdf b/nav2_system_tests/models/cardboard_box.sdf
new file mode 100644
index 0000000000..97a40603c2
--- /dev/null
+++ b/nav2_system_tests/models/cardboard_box.sdf
@@ -0,0 +1,9 @@
+<sdf version="1.6">
+     <model name='cardboard_box'>
+      <include>
+      <uri>
+      https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box
+      </uri>
+      </include>
+    </model>
+</sdf>
\ No newline at end of file
diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml
index 5fdbc230b7..2f3f45d191 100644
--- a/nav2_system_tests/package.xml
+++ b/nav2_system_tests/package.xml
@@ -30,6 +30,7 @@
   <build_depend>launch_ros</build_depend>
   <build_depend>launch_testing</build_depend>
   <build_depend>nav2_planner</build_depend>
+  <build_depend>nav2_minimal_tb3_sim</build_depend>
 
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>launch_testing</exec_depend>
@@ -48,6 +49,7 @@
   <exec_depend>nav2_amcl</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>nav2_minimal_tb3_sim</exec_depend>
   <!-- <exec_depend>gazebo_ros_pkgs</exec_depend> Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 -->
   <exec_depend>navigation2</exec_depend>
   <exec_depend>lcov</exec_depend>
diff --git a/nav2_system_tests/src/localization/test_localization_launch.py b/nav2_system_tests/src/localization/test_localization_launch.py
index aedca4fcf7..e56ca77986 100755
--- a/nav2_system_tests/src/localization/test_localization_launch.py
+++ b/nav2_system_tests/src/localization/test_localization_launch.py
@@ -15,6 +15,7 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
@@ -26,6 +27,8 @@
 import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
+from nav2_simple_commander.utils import kill_os_processes
+
 
 def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
@@ -36,11 +39,19 @@ def main(argv=sys.argv[1:]):
     world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
     robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
 
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
     map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     set_env_vars_resources = AppendEnvironmentVariable(
         'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
     )
+    set_env_vars_resources2 = AppendEnvironmentVariable(
+        'GZ_SIM_RESOURCE_PATH',
+        str(Path(os.path.join(sim_dir)).parent.resolve())
+    )
 
     start_gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -64,18 +75,14 @@ def main(argv=sys.argv[1:]):
             'yaw': '0.0',
         }.items(),
     )
-
-    link_footprint = launch_ros.actions.Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        output='screen',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
-    )
-    footprint_scan = launch_ros.actions.Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
+    run_robot_state_publisher = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
         output='screen',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+        parameters=[
+            {'use_sim_time': True, 'robot_description': robot_description}
+        ],
     )
     run_map_server = launch_ros.actions.Node(
         package='nav2_map_server',
@@ -97,10 +104,10 @@ def main(argv=sys.argv[1:]):
     ld = LaunchDescription(
         [
             set_env_vars_resources,
+            set_env_vars_resources2,
             start_gazebo_server,
             spawn_robot,
-            link_footprint,
-            footprint_scan,
+            run_robot_state_publisher,
             run_map_server,
             run_amcl,
             run_lifecycle_manager,
@@ -115,7 +122,9 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code = lts.run(ls)
+    kill_os_processes('gz sim')
+    return return_code
 
 
 if __name__ == '__main__':
diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt
index 39afa461c2..34ebf0e30e 100644
--- a/nav2_system_tests/src/system/CMakeLists.txt
+++ b/nav2_system_tests/src/system/CMakeLists.txt
@@ -5,9 +5,6 @@ ament_add_test(test_bt_navigator
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=True
@@ -22,9 +19,6 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=True
@@ -39,9 +33,6 @@ ament_add_test(test_bt_navigator_with_dijkstra
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -56,9 +47,6 @@ ament_add_test(test_bt_navigator_2
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -68,14 +56,12 @@ ament_add_test(test_bt_navigator_2
 
 ament_add_test(test_dynamic_obstacle
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    ADD_OBSTACLE=True
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -85,14 +71,12 @@ ament_add_test(test_dynamic_obstacle
 
 ament_add_test(test_nav_through_poses
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_launch.py"
   WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    ADD_OBSTACLE=True
     BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
     TESTER=nav_through_poses_tester_node.py
     ASTAR=False
diff --git a/nav2_system_tests/src/system/nav2_system_params.yaml b/nav2_system_tests/src/system/nav2_system_params.yaml
index a98d340f21..5ee29e6536 100644
--- a/nav2_system_tests/src/system/nav2_system_params.yaml
+++ b/nav2_system_tests/src/system/nav2_system_params.yaml
@@ -317,3 +317,49 @@ collision_monitor:
       min_height: 0.15
       max_height: 2.0
       enabled: True
+
+docking_server:
+  ros__parameters:
+    controller_frequency: 50.0
+    initial_perception_timeout: 5.0
+    wait_charge_timeout: 5.0
+    dock_approach_timeout: 30.0
+    undock_linear_tolerance: 0.05
+    undock_angular_tolerance: 0.1
+    max_retries: 3
+    base_frame: "base_link"
+    fixed_frame: "odom"
+    dock_backwards: false
+    dock_prestaging_tolerance: 0.5
+
+    # Types of docks
+    dock_plugins: ['simple_charging_dock']
+    simple_charging_dock:
+      plugin: 'opennav_docking::SimpleChargingDock'
+      docking_threshold: 0.05
+      staging_x_offset: -0.7
+      use_external_detection_pose: true
+      use_battery_status: false # true
+      use_stall_detection: false # true
+
+      external_detection_timeout: 1.0
+      external_detection_translation_x: -0.18
+      external_detection_translation_y: 0.0
+      external_detection_rotation_roll: -1.57
+      external_detection_rotation_pitch: -1.57
+      external_detection_rotation_yaw: 0.0
+      filter_coef: 0.1
+
+    # Dock instances
+    docks: ['home_dock']  # Input your docks here
+    home_dock:
+      type: 'simple_charging_dock'
+      frame: map
+      pose: [0.0, 0.0, 0.0]
+
+    controller:
+      k_phi: 3.0
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      
\ No newline at end of file
diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py
index 8fd1bc6034..0eca068039 100755
--- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py
+++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py
@@ -59,6 +59,7 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ''):
         self.initial_pose_received = False
         self.initial_pose = initial_pose
         self.goal_pose = goal_pose
+        self.set_initial_pose_timeout = 15
         self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
 
     def info_msg(self, msg: str):
@@ -212,11 +213,21 @@ def shutdown(self):
 
     def wait_for_initial_pose(self):
         self.initial_pose_received = False
+        # If the initial pose is not received within 100 seconds, return False
+        # this is because when setting a wrong initial pose, amcl_pose is not received
+        # and the test will hang indefinitely
+        start_time = time.time()
+        duration = 0
         while not self.initial_pose_received:
             self.info_msg('Setting initial pose')
             self.setInitialPose()
             self.info_msg('Waiting for amcl_pose to be received')
+            duration = time.time() - start_time
+            if duration > self.set_initial_pose_timeout:
+                self.error_msg('Timeout waiting for initial pose to be set')
+                return False
             rclpy.spin_once(self, timeout_sec=1)
+        return True
 
 
 def test_RobotMovesToGoal(robot_tester):
@@ -231,7 +242,8 @@ def run_all_tests(robot_tester):
     result = True
     if result:
         robot_tester.wait_for_node_active('amcl')
-        robot_tester.wait_for_initial_pose()
+        result = robot_tester.wait_for_initial_pose()
+    if result:
         robot_tester.wait_for_node_active('bt_navigator')
         result = robot_tester.runNavigateAction()
 
diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py
index 2f0546d156..a644072ebb 100755
--- a/nav2_system_tests/src/system/test_system_launch.py
+++ b/nav2_system_tests/src/system/test_system_launch.py
@@ -16,6 +16,7 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
@@ -23,6 +24,7 @@
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import (
+    AppendEnvironmentVariable,
     ExecuteProcess,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
@@ -33,11 +35,22 @@
 from launch_testing.legacy import LaunchTestService
 
 from nav2_common.launch import RewrittenYaml
+from nav2_simple_commander.utils import kill_os_processes
 
 
 def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    world = os.getenv('TEST_WORLD')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
+
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
@@ -45,8 +58,6 @@ def generate_launch_description():
         os.getenv('BT_NAVIGATOR_XML'),
     )
 
-    bringup_dir = get_package_share_directory('nav2_bringup')
-
     # Use local param file
     launch_dir = os.path.dirname(os.path.realpath(__file__))
     params_file = os.path.join(launch_dir, 'nav2_system_params.yaml')
@@ -79,34 +90,46 @@ def generate_launch_description():
         [
             SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
             SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
-            # Launch gazebo server for simulation
-            ExecuteProcess(
-                cmd=[
-                    'gzserver',
-                    '-s',
-                    'libgazebo_ros_init.so',
-                    '--minimal_comms',
-                    world,
-                ],
-                output='screen',
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
             ),
-            # TODO(orduno) Launch the robot state publisher instead
-            #              using a local copy of TB3 urdf file
-            Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
             ),
             Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
                 output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
             ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-                    os.path.join(bringup_dir, 'launch', 'bringup_launch.py')
+                    os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
                 ),
                 launch_arguments={
                     'namespace': '',
@@ -145,7 +168,9 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code = lts.run(ls)
+    kill_os_processes('gz sim')
+    return return_code
 
 
 if __name__ == '__main__':
diff --git a/nav2_system_tests/src/system/test_system_with_obstacle_launch.py b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py
new file mode 100755
index 0000000000..fb4468afef
--- /dev/null
+++ b/nav2_system_tests/src/system/test_system_with_obstacle_launch.py
@@ -0,0 +1,190 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+# Copyright (c) 2020 Florian Gramss
+#
+# 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 pathlib import Path
+import sys
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import (
+    AppendEnvironmentVariable,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+)
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_testing.legacy import LaunchTestService
+
+from nav2_common.launch import RewrittenYaml
+from nav2_simple_commander.utils import kill_os_processes
+
+
+def generate_launch_description():
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
+    nav2_system_tests_dir = get_package_share_directory('nav2_system_tests')
+
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
+
+    bt_navigator_xml = os.path.join(
+        get_package_share_directory('nav2_bt_navigator'),
+        'behavior_trees',
+        os.getenv('BT_NAVIGATOR_XML'),
+    )
+
+    # Use local param file
+    launch_dir = os.path.dirname(os.path.realpath(__file__))
+    params_file = os.path.join(launch_dir, 'nav2_system_params.yaml')
+
+    # Replace the default parameter values for testing special features
+    # without having multiple params_files inside the nav2 stack
+    context = LaunchContext()
+    param_substitutions = {}
+
+    if os.getenv('ASTAR') == 'True':
+        param_substitutions.update({'use_astar': 'True'})
+
+    param_substitutions.update(
+        {'planner_server.ros__parameters.GridBased.plugin': os.getenv('PLANNER')}
+    )
+    param_substitutions.update(
+        {'controller_server.ros__parameters.FollowPath.plugin': os.getenv('CONTROLLER')}
+    )
+
+    configured_params = RewrittenYaml(
+        source_file=params_file,
+        root_key='',
+        param_rewrites=param_substitutions,
+        convert_types=True,
+    )
+
+    new_yaml = configured_params.perform(context)
+
+    cardbox_sdf = os.path.join(nav2_system_tests_dir, 'models', 'cardboard_box.sdf')
+
+    return LaunchDescription(
+        [
+            SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
+            SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
+            ),
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
+            ),
+            Node(
+                package='ros_gz_sim',
+                executable='create',
+                output='screen',
+                arguments=[
+                    '-entity', 'cardboard_box',
+                    '-file', cardbox_sdf,
+                    '-x', '-1.0', '-y', '0.6', '-z', '0.15',
+                    '-R', '0.0', '-P', '0.0', '-Y', '0.0',]
+            ),
+            Node(
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
+                output='screen',
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
+                ),
+                launch_arguments={
+                    'namespace': '',
+                    'use_namespace': 'False',
+                    'map': map_yaml_file,
+                    'use_sim_time': 'True',
+                    'params_file': new_yaml,
+                    'bt_xml_file': bt_navigator_xml,
+                    'use_composition': 'False',
+                    'autostart': 'True',
+                }.items(),
+            ),
+        ]
+    )
+
+
+def main(argv=sys.argv[1:]):
+    ld = generate_launch_description()
+
+    test1_action = ExecuteProcess(
+        cmd=[
+            os.path.join(os.getenv('TEST_DIR'), os.getenv('TESTER')),
+            '-r',
+            '-2.0',
+            '-0.5',
+            '0.0',
+            '2.0',
+            '-e',
+            'True',
+        ],
+        name='tester_node',
+        output='screen',
+    )
+
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    return_code = lts.run(ls)
+    kill_os_processes('gz sim')
+    return return_code
+
+
+if __name__ == '__main__':
+    sys.exit(main())
diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py
index f42d276fcc..b0e2322692 100755
--- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py
+++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py
@@ -16,13 +16,14 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import (
+    AppendEnvironmentVariable,
     ExecuteProcess,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
@@ -33,11 +34,22 @@
 from launch_testing.legacy import LaunchTestService
 
 from nav2_common.launch import RewrittenYaml
+from nav2_simple_commander.utils import kill_os_processes
 
 
 def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    world = os.getenv('TEST_WORLD')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
+
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
@@ -45,8 +57,9 @@ def generate_launch_description():
         os.getenv('BT_NAVIGATOR_XML'),
     )
 
-    bringup_dir = get_package_share_directory('nav2_bringup')
-    params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
+    # Use local param file
+    launch_dir = os.path.dirname(os.path.realpath(__file__))
+    params_file = os.path.join(launch_dir, 'nav2_system_params.yaml')
 
     # Replace the default parameter values for testing special features
     # without having multiple params_files inside the nav2 stack
@@ -76,34 +89,46 @@ def generate_launch_description():
         [
             SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
             SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
-            # Launch gazebo server for simulation
-            ExecuteProcess(
-                cmd=[
-                    'gzserver',
-                    '-s',
-                    'libgazebo_ros_init.so',
-                    '--minimal_comms',
-                    world,
-                ],
-                output='screen',
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
             ),
-            # TODO(orduno) Launch the robot state publisher instead
-            #              using a local copy of TB3 urdf file
-            Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
             ),
             Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
                 output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
             ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-                    os.path.join(bringup_dir, 'launch', 'bringup_launch.py')
+                    os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
                 ),
                 launch_arguments={
                     'namespace': '',
@@ -142,7 +167,9 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code = lts.run(ls)
+    kill_os_processes('gz sim')
+    return return_code
 
 
 if __name__ == '__main__':
diff --git a/nav2_system_tests/urdf/common_properties.urdf b/nav2_system_tests/urdf/common_properties.urdf
deleted file mode 100644
index 9d457ea1d6..0000000000
--- a/nav2_system_tests/urdf/common_properties.urdf
+++ /dev/null
@@ -1,45 +0,0 @@
-<?xml version="1.0" ?>
-
-<robot name="xacro_properties" 
-  xmlns:xacro="http://ros.org/wiki/xacro">
-
-  <!-- Init colour -->
-  <material name="black">
-    <color rgba="0.0 0.0 0.0 1.0"/>
-  </material>
-
-  <material name="dark">
-    <color rgba="0.3 0.3 0.3 1.0"/>
-  </material>
-  <material name="light_black">
-    <color rgba="0.4 0.4 0.4 1.0"/>
-  </material>
-
-  <material name="blue">
-    <color rgba="0.0 0.0 0.8 1.0"/>
-  </material>
-
-  <material name="green">
-    <color rgba="0.0 0.8 0.0 1.0"/>
-  </material>
-
-  <material name="grey">
-    <color rgba="0.5 0.5 0.5 1.0"/>
-  </material>
-
-  <material name="orange">
-    <color rgba="${255/255} ${108/255} ${10/255} 1.0"/>
-  </material>
-
-  <material name="brown">
-    <color rgba="${222/255} ${207/255} ${195/255} 1.0"/>
-  </material>
-
-  <material name="red">
-    <color rgba="0.8 0.0 0.0 1.0"/>
-  </material>
-
-  <material name="white">
-    <color rgba="1.0 1.0 1.0 1.0"/>
-  </material>
-</robot>
diff --git a/nav2_system_tests/urdf/turtlebot3_burger.urdf b/nav2_system_tests/urdf/turtlebot3_burger.urdf
deleted file mode 100644
index 90f34dbbb8..0000000000
--- a/nav2_system_tests/urdf/turtlebot3_burger.urdf
+++ /dev/null
@@ -1,155 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="turtlebot3_burger" 
-  xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/>
-
-  <link name="base_footprint"/>
-
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint"/>
-    <child link="base_link"/>
-    <origin xyz="0.0 0.0 0.010" rpy="0 0 0"/>
-  </joint>
-
-  <link name="base_link">
-    <visual>
-      <origin xyz="-0.032 0 0.0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_description/meshes/bases/burger_base.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="light_black"/>
-    </visual>
-
-    <collision>
-      <origin xyz="-0.032 0 0.070" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.140 0.140 0.143"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="8.2573504e-01"/>
-      <inertia ixx="2.2124416e-03" ixy="-1.2294101e-05" ixz="3.4938785e-05" iyy="2.1193702e-03" iyz="-5.0120904e-06" izz="2.0064271e-03" />
-    </inertial>
-  </link>
-
-  <joint name="wheel_left_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_left_link"/>
-    <origin xyz="0.0 0.08 0.023" rpy="-1.57 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-
-  <link name="wheel_left_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="1.57 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_description/meshes/wheels/left_tire.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="dark"/>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.018" radius="0.033"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="2.8498940e-02" />
-      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" iyy="1.1192413e-05" iyz="-1.4400107e-11" izz="2.0712558e-05" />
-    </inertial>
-  </link>
-
-  <joint name="wheel_right_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_right_link"/>
-    <origin xyz="0.0 -0.080 0.023" rpy="-1.57 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-
-  <link name="wheel_right_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="1.57 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_description/meshes/wheels/right_tire.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="dark"/>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.018" radius="0.033"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="2.8498940e-02" />
-      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09" iyy="1.1192413e-05" iyz="-1.4400107e-11" izz="2.0712558e-05" />
-    </inertial>
-  </link>
-
-  <joint name="caster_back_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="caster_back_link"/>
-    <origin xyz="-0.081 0 -0.004" rpy="-1.57 0 0"/>
-  </joint>
-
-  <link name="caster_back_link">
-    <collision>
-      <origin xyz="0 0.001 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.030 0.009 0.020"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="0.005" />
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
-    </inertial>
-  </link>
-
-  <joint name="imu_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="imu_link"/>
-    <origin xyz="-0.032 0 0.068" rpy="0 0 0"/>
-  </joint>
-
-  <link name="imu_link"/>
-
-  <joint name="scan_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="base_scan"/>
-    <origin xyz="-0.032 0 0.172" rpy="0 0 0"/>
-  </joint>
-
-  <link name="base_scan">
-    <visual>
-      <origin xyz="0 0 0.0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_description/meshes/sensors/lds.stl" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="dark"/>
-    </visual>
-
-    <collision>
-      <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.0315" radius="0.055"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <mass value="0.114" />
-      <origin xyz="0 0 0" />
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001" />
-    </inertial>
-  </link>
-
-</robot>
diff --git a/nav2_system_tests/urdf/turtlebot3_waffle.urdf b/nav2_system_tests/urdf/turtlebot3_waffle.urdf
deleted file mode 100644
index 9a5bca1141..0000000000
--- a/nav2_system_tests/urdf/turtlebot3_waffle.urdf
+++ /dev/null
@@ -1,279 +0,0 @@
-<?xml version="1.0" ?>
-<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.urdf"/>
-
-  <xacro:property name="r200_cam_rgb_px" value="0.005"/>
-  <xacro:property name="r200_cam_rgb_py" value="0.018"/>
-  <xacro:property name="r200_cam_rgb_pz" value="0.013"/>
-  <xacro:property name="r200_cam_depth_offset" value="0.01"/> -->
-
-  <!-- Init colour -->
-  <material name="black">
-      <color rgba="0.0 0.0 0.0 1.0"/>
-  </material>
-
-  <material name="dark">
-    <color rgba="0.3 0.3 0.3 1.0"/>
-  </material>
-
-  <material name="light_black">
-    <color rgba="0.4 0.4 0.4 1.0"/>
-  </material>
-
-  <material name="blue">
-    <color rgba="0.0 0.0 0.8 1.0"/>
-  </material>
-
-  <material name="green">
-    <color rgba="0.0 0.8 0.0 1.0"/>
-  </material>
-
-  <material name="grey">
-    <color rgba="0.5 0.5 0.5 1.0"/>
-  </material>
-
-  <material name="orange">
-    <color rgba="1.0 0.4235 0.0392 1.0"/>
-  </material>
-
-  <material name="brown">
-    <color rgba="0.8706 0.8118 0.7647 1.0"/>
-  </material>
-
-  <material name="red">
-    <color rgba="0.8 0.0 0.0 1.0"/>
-  </material>
-
-  <material name="white">
-    <color rgba="1.0 1.0 1.0 1.0"/>
-  </material>
-
-  <link name="base_footprint"/>
-
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint"/>
-    <child link="base_link" />
-    <origin xyz="0 0 0.010" rpy="0 0 0"/>
-  </joint>
-
-  <link name="base_link">
-    <visual>
-      <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.266 0.266 0.094"/>
-      </geometry>
-      <material name="blue"/>
-    </visual>
-
-    <collision>
-      <origin xyz="-0.064 0 0.047" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.266 0.266 0.094"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <mass value="1.3729096e+00"/>
-      <inertia ixx="8.7002718e-03" ixy="-4.7576583e-05" ixz="1.1160499e-04"
-               iyy="8.6195418e-03" iyz="-3.5422299e-06"
-               izz="1.4612727e-02" />
-    </inertial>
-  </link>
-
-  <joint name="wheel_left_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_left_link"/>
-    <origin xyz="0.0 0.144 0.023" rpy="-1.57 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-
-  <link name="wheel_left_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.018" radius="0.033"/>
-      </geometry>
-      <material name="light_black"/>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.018" radius="0.033"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="2.8498940e-02" />
-      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
-               iyy="1.1192413e-05" iyz="-1.4400107e-11"
-               izz="2.0712558e-05" />
-      </inertial>
-  </link>
-
-  <joint name="wheel_right_joint" type="continuous">
-    <parent link="base_link"/>
-    <child link="wheel_right_link"/>
-    <origin xyz="0.0 -0.144 0.023" rpy="-1.57 0 0"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-
-  <link name="wheel_right_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.018" radius="0.033"/>
-      </geometry>
-      <material name="light_black"/>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.018" radius="0.033"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="2.8498940e-02" />
-      <inertia ixx="1.1175580e-05" ixy="-4.2369783e-11" ixz="-5.9381719e-09"
-               iyy="1.1192413e-05" iyz="-1.4400107e-11"
-               izz="2.0712558e-05" />
-      </inertial>
-  </link>
-
-  <joint name="caster_back_right_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="caster_back_right_link"/>
-    <origin xyz="-0.177 -0.064 -0.004" rpy="-1.57 0 0"/>
-  </joint>
-
-  <link name="caster_back_right_link">
-    <collision>
-      <origin xyz="0 0.001 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.030 0.009 0.020"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="0.005" />
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
-               iyy="0.001" iyz="0.0"
-               izz="0.001" />
-    </inertial>
-  </link>
-
-  <joint name="caster_back_left_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="caster_back_left_link"/>
-    <origin xyz="-0.177 0.064 -0.004" rpy="-1.57 0 0"/>
-  </joint>
-
-  <link name="caster_back_left_link">
-    <collision>
-      <origin xyz="0 0.001 0" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.030 0.009 0.020"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <origin xyz="0 0 0" />
-      <mass value="0.005" />
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
-               iyy="0.001" iyz="0.0"
-               izz="0.001" />
-    </inertial>
-  </link>
-
-  <joint name="imu_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="imu_link"/>
-    <origin xyz="0.0 0 0.068" rpy="0 0 0"/>
-  </joint>
-
-  <link name="imu_link"/>
-
-  <joint name="scan_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="base_scan"/>
-    <origin xyz="-0.064 0 0.122" rpy="0 0 0"/>
-  </joint>
-
-  <link name="base_scan">
-    <collision>
-      <origin xyz="0.015 0 -0.0065" rpy="0 0 0"/>
-      <geometry>
-        <cylinder length="0.0315" radius="0.055"/>
-      </geometry>
-    </collision>
-
-    <inertial>
-      <mass value="0.114" />
-      <origin xyz="0 0 0" />
-      <inertia ixx="0.001" ixy="0.0" ixz="0.0"
-               iyy="0.001" iyz="0.0"
-               izz="0.001" />
-    </inertial>
-  </link>
-
-  <joint name="camera_joint" type="fixed">
-    <origin xyz="0.064 -0.065 0.094" rpy="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="camera_link"/>
-  </joint>
-
-  <link name="camera_link">
-    <collision>
-      <origin xyz="0.003 0.065 0.007" rpy="0 0 0"/>
-      <geometry>
-        <box size="0.012 0.132 0.020"/>
-      </geometry>
-    </collision>
-
-    <!-- This inertial field needs doesn't contain reliable data!! -->
-<!--   <inertial>
-      <mass value="0.564" />
-      <origin xyz="0 0 0" />
-      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0"
-               iyy="0.000498940" iyz="0.0"
-               izz="0.003879257" />
-    </inertial>-->
-  </link>
-
-  <joint name="camera_rgb_joint" type="fixed">
-    <origin xyz="0.005 0.018 0.013" rpy="-1.57 0 -1.57"/>
-    <!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py} ${r200_cam_rgb_pz}" rpy="0 0 0"/> -->
-    <parent link="camera_link"/>
-    <child link="camera_rgb_frame"/>
-  </joint>
-  <link name="camera_rgb_frame"/>
-
-  <joint name="camera_rgb_optical_joint" type="fixed">
-    <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
-    <parent link="camera_rgb_frame"/>
-    <child link="camera_rgb_optical_frame"/>
-  </joint>
-  <link name="camera_rgb_optical_frame"/>
-
-  <joint name="camera_depth_joint" type="fixed">
-    <origin xyz="0.005 0.028 0.013" rpy="-1.57 0 -1.57"/>
-    <!-- <origin xyz="${r200_cam_rgb_px} ${r200_cam_rgb_py + r200_cam_depth_offset} ${r200_cam_rgb_pz}" rpy="0 0 0"/> -->
-    <parent link="camera_link"/>
-    <child link="camera_depth_frame"/>
-  </joint>
-  <link name="camera_depth_frame"/>
-
-  <joint name="camera_depth_optical_joint" type="fixed">
-    <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
-    <parent link="camera_depth_frame"/>
-    <child link="camera_depth_optical_frame"/>
-  </joint>
-  <link name="camera_depth_optical_frame"/>
-
-</robot>
diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
deleted file mode 100644
index 9a9e1bdbe8..0000000000
--- a/nav2_system_tests/worlds/turtlebot3_ros2_demo.world
+++ /dev/null
@@ -1,497 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.6">
-  <world name="default">
-
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-
-    <physics type="ode">
-      <real_time_update_rate>1000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>150</iters>
-          <precon_iters>0</precon_iters>
-          <sor>1.400000</sor>
-          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0.00001</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
-          <contact_surface_layer>0.01000</contact_surface_layer>
-        </constraints>
-      </ode>
-    </physics>
-
-    <model name="turtlebot3_world">
-      <static>1</static>
-      <include>
-        <uri>model://turtlebot3_world</uri>
-      </include>
-    </model>
-
-    <model name="turtlebot3">
-      <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
-
-      <link name="base_footprint"/>
-
-      <link name="base_link">
-
-        <inertial>
-          <pose>-0.064 0 0.048 0 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>1.0</mass>
-        </inertial>
-
-        <collision name="base_collision">
-          <pose>-0.064 0 0.048 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.265 0.265 0.089</size>
-            </box>
-          </geometry>
-        </collision>
-
-        <visual name="base_visual">
-          <pose>-0.064 0 0 0 0 0</pose>
-          <geometry>
-            <mesh>
-              <uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri>
-              <scale>0.001 0.001 0.001</scale>
-            </mesh>
-          </geometry>
-        </visual>
-      </link>
-
-      <link name="imu_link">
-        <sensor name="tb3_imu" type="imu">
-          <always_on>true</always_on>
-          <update_rate>200</update_rate>
-          <imu>
-            <angular_velocity>
-              <x>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </x>
-              <y>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </y>
-              <z>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </z>
-            </angular_velocity>
-            <linear_acceleration>
-              <x>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </x>
-              <y>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </y>
-              <z>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </z>
-            </linear_acceleration>
-          </imu>
-          <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
-            <initial_orientation_as_reference>false</initial_orientation_as_reference>
-            <ros>
-              <!-- <namespace>/tb3</namespace> -->
-              <remapping>~/out:=imu</remapping>
-            </ros>
-          </plugin>
-        </sensor>
-      </link>
-
-      <link name="base_scan">
-        <inertial>
-          <pose>-0.052 0 0.111 0 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>0.125</mass>
-        </inertial>
-
-        <collision name="lidar_sensor_collision">
-            <pose>-0.064 0 0.121 0 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.0508</radius>
-                <length>0.055</length>
-              </cylinder>
-            </geometry>
-          </collision>
-
-          <visual name="lidar_sensor_visual">
-            <pose>-0.032 0 0.171 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/lds.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-
-          <sensor name="hls_lfcd_lds" type="ray">
-            <always_on>true</always_on>
-            <visualize>false</visualize>
-            <pose>-0.064 0 0.121 0 0 0</pose>
-            <update_rate>5</update_rate>
-            <ray>
-              <scan>
-                <horizontal>
-                  <samples>360</samples>
-                  <resolution>1.000000</resolution>
-                  <min_angle>0.000000</min_angle>
-                  <max_angle>6.280000</max_angle>
-                </horizontal>
-              </scan>
-              <range>
-                <min>0.120000</min>
-                <max>3.5</max>
-                <resolution>0.015000</resolution>
-              </range>
-              <noise>
-                <type>gaussian</type>
-                <mean>0.0</mean>
-                <stddev>0.01</stddev>
-              </noise>
-            </ray>
-            <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
-              <ros>
-                <!-- <namespace>/tb3</namespace> -->
-                <remapping>~/out:=scan</remapping>
-              </ros>
-              <output_type>sensor_msgs/LaserScan</output_type>
-            </plugin>
-          </sensor>
-        </link>
-
-        <link name="wheel_left_link">
-
-          <inertial>
-            <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-            <inertia>
-              <ixx>0.001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.001</izz>
-            </inertia>
-            <mass>0.1</mass>
-          </inertial>
-
-          <collision name="wheel_left_collision">
-            <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.033</radius>
-                <length>0.018</length>
-              </cylinder>
-            </geometry>
-            <surface>
-              <!-- This friction pamareter don't contain reliable data!! -->
-              <friction>
-                <ode>
-                  <mu>100000.0</mu>
-                  <mu2>100000.0</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0.0</slip1>
-                  <slip2>0.0</slip2>
-                </ode>
-              </friction>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-
-          <visual name="wheel_left_visual">
-            <pose>0.0 0.144 0.023 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-        </link>
-
-        <link name="wheel_right_link">
-
-          <inertial>
-            <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-            <inertia>
-              <ixx>0.001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.001</izz>
-            </inertia>
-            <mass>0.1</mass>
-          </inertial>
-
-          <collision name="wheel_right_collision">
-            <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.033</radius>
-                <length>0.018</length>
-              </cylinder>
-            </geometry>
-            <surface>
-              <!-- This friction pamareter don't contain reliable data!! -->
-              <friction>
-                <ode>
-                  <mu>100000.0</mu>
-                  <mu2>100000.0</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0.0</slip1>
-                  <slip2>0.0</slip2>
-                </ode>
-              </friction>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-
-          <visual name="wheel_right_visual">
-            <pose>0.0 -0.144 0.023 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-        </link>
-
-        <link name='caster_back_right_link'>
-          <pose>-0.177 -0.064 -0.004 0 0 0</pose>
-          <inertial>
-            <mass>0.001</mass>
-            <inertia>
-              <ixx>0.00001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.00001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.00001</izz>
-            </inertia>
-          </inertial>
-          <collision name='collision'>
-            <geometry>
-              <sphere>
-                <radius>0.005000</radius>
-              </sphere>
-            </geometry>
-            <surface>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-
-        <link name='caster_back_left_link'>
-          <pose>-0.177 0.064 -0.004 0 0 0</pose>
-          <inertial>
-            <mass>0.001</mass>
-            <inertia>
-              <ixx>0.00001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.00001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.00001</izz>
-            </inertia>
-          </inertial>
-          <collision name='collision'>
-            <geometry>
-              <sphere>
-                <radius>0.005000</radius>
-              </sphere>
-            </geometry>
-            <surface>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-
-        <joint name="base_joint" type="fixed">
-          <parent>base_footprint</parent>
-          <child>base_link</child>
-          <pose>0.0 0.0 0.010 0 0 0</pose>
-        </joint>
-
-        <joint name="wheel_left_joint" type="revolute">
-          <parent>base_link</parent>
-          <child>wheel_left_link</child>
-          <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <joint name="wheel_right_joint" type="revolute">
-          <parent>base_link</parent>
-          <child>wheel_right_link</child>
-          <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <joint name='caster_back_right_joint' type='ball'>
-          <parent>base_link</parent>
-          <child>caster_back_right_link</child>
-        </joint>
-
-        <joint name='caster_back_left_joint' type='ball'>
-          <parent>base_link</parent>
-          <child>caster_back_left_link</child>
-        </joint>
-
-        <joint name="imu_joint" type="fixed">
-          <parent>base_link</parent>
-          <child>imu_link</child>
-          <pose>-0.032 0 0.068 0 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <joint name="lidar_joint" type="fixed">
-          <parent>base_link</parent>
-          <child>base_scan</child>
-          <pose>-0.064 0 0.121 0 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
-
-          <ros>
-            <!-- <namespace>/tb3</namespace> -->
-            <remapping>/tf:=tf</remapping>
-          </ros>
-
-          <!-- wheels -->
-          <left_joint>wheel_left_joint</left_joint>
-          <right_joint>wheel_right_joint</right_joint>
-
-          <!-- kinematics -->
-          <wheel_separation>0.287</wheel_separation>
-          <wheel_diameter>0.066</wheel_diameter>
-
-          <!-- limits -->
-          <max_wheel_torque>20</max_wheel_torque>
-          <max_wheel_acceleration>1.0</max_wheel_acceleration>
-
-          <!-- output -->
-          <publish_odom>true</publish_odom>
-          <publish_odom_tf>true</publish_odom_tf>
-          <publish_wheel_tf>false</publish_wheel_tf>
-
-          <odometry_frame>odom</odometry_frame>
-          <robot_base_frame>base_footprint</robot_base_frame>
-
-        </plugin>
-
-        <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
-          <ros>
-            <!-- <namespace>/tb3</namespace> -->
-            <remapping>~/out:=joint_states</remapping>
-          </ros>
-          <update_rate>250</update_rate>
-          <joint_name>wheel_left_joint</joint_name>
-          <joint_name>wheel_right_joint</joint_name>
-        </plugin>
-        
-      </model>
-
-  </world>
-</sdf>
-
diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world
deleted file mode 100644
index 94b72499a2..0000000000
--- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world
+++ /dev/null
@@ -1,555 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.6">
-  <world name="default">
-
-    <spherical_coordinates>
-      <!-- currently gazebo has a bug: instead of outputing lat, long, altitude in ENU
-      (x = East, y = North and z = Up) as the default configurations, it's outputting (-E)(-N)U,
-      therefore we rotate the default frame 180 so that it would go back to ENU 
-      see: https://github.com/osrf/gazebo/issues/2022 --> 
-      <surface_model>EARTH_WGS84</surface_model>
-      <world_frame_orientation>ENU</world_frame_orientation>
-      <latitude_deg>55.944831</latitude_deg>
-      <longitude_deg>-3.186998</longitude_deg>
-      <elevation>0.0</elevation>
-      <heading_deg>180.0</heading_deg>
-    </spherical_coordinates>
-
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-
-    <physics type="ode">
-      <real_time_update_rate>1000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>150</iters>
-          <precon_iters>0</precon_iters>
-          <sor>1.400000</sor>
-          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0.00001</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
-          <contact_surface_layer>0.01000</contact_surface_layer>
-        </constraints>
-      </ode>
-    </physics>
-
-    <model name="turtlebot3">
-      <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
-
-      <link name="base_footprint"/>
-
-      <link name="base_link">
-
-        <inertial>
-          <pose>-0.064 0 0.048 0 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>1.0</mass>
-        </inertial>
-
-        <collision name="base_collision">
-          <pose>-0.064 0 0.048 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.265 0.265 0.089</size>
-            </box>
-          </geometry>
-        </collision>
-
-        <visual name="base_visual">
-          <pose>-0.064 0 0 0 0 0</pose>
-          <geometry>
-            <mesh>
-              <uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri>
-              <scale>0.001 0.001 0.001</scale>
-            </mesh>
-          </geometry>
-        </visual>
-      </link>
-
-      <link name="imu_link">
-        <sensor name="tb3_imu" type="imu">
-          <always_on>true</always_on>
-          <update_rate>200</update_rate>
-          <imu>
-            <angular_velocity>
-              <x>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </x>
-              <y>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </y>
-              <z>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </z>
-            </angular_velocity>
-            <linear_acceleration>
-              <x>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </x>
-              <y>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </y>
-              <z>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </z>
-            </linear_acceleration>
-          </imu>
-          <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
-            <initial_orientation_as_reference>false</initial_orientation_as_reference>
-            <ros>
-              <!-- <namespace>/tb3</namespace> -->
-              <remapping>~/out:=/imu/data</remapping>
-            </ros>
-          </plugin>
-        </sensor>
-      </link>
-
-      <link name="gps_link">
-      <!-- Some weird bug occurs if this joint gets the default inertia -->
-        <inertial>
-          <pose>-0.052 0 0.111 0 0 0</pose>
-          <inertia>
-            <mass>0.001</mass>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>0.125</mass>
-        </inertial>
-        <sensor name="tb3_gps" type="gps">
-          <always_on>true</always_on>
-          <update_rate>1</update_rate>
-          <pose>0 0 0 0 0 0</pose>
-          <gps>
-            <position_sensing>
-              <horizontal>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>0.1</stddev>
-                </noise>
-              </horizontal>
-              <vertical>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>0.0</stddev>
-                </noise>
-              </vertical>
-            </position_sensing>
-          </gps>
-          <plugin name="my_gps_plugin" filename="libgazebo_ros_gps_sensor.so">
-            <ros>
-              <remapping>~/out:=/gps/fix</remapping>
-            </ros>
-          </plugin>
-        </sensor>
-      </link>
-
-      <link name="base_scan">
-        <inertial>
-          <pose>-0.052 0 0.111 0 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>0.125</mass>
-        </inertial>
-
-        <collision name="lidar_sensor_collision">
-            <pose>-0.064 0 0.121 0 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.0508</radius>
-                <length>0.055</length>
-              </cylinder>
-            </geometry>
-          </collision>
-
-          <visual name="lidar_sensor_visual">
-            <pose>-0.032 0 0.171 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/lds.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-
-          <sensor name="hls_lfcd_lds" type="ray">
-            <always_on>true</always_on>
-            <visualize>false</visualize>
-            <pose>-0.064 0 0.121 0 0 0</pose>
-            <update_rate>5</update_rate>
-            <ray>
-              <scan>
-                <horizontal>
-                  <samples>360</samples>
-                  <resolution>1.000000</resolution>
-                  <min_angle>0.000000</min_angle>
-                  <max_angle>6.280000</max_angle>
-                </horizontal>
-              </scan>
-              <range>
-                <min>0.120000</min>
-                <max>3.5</max>
-                <resolution>0.015000</resolution>
-              </range>
-              <noise>
-                <type>gaussian</type>
-                <mean>0.0</mean>
-                <stddev>0.01</stddev>
-              </noise>
-            </ray>
-            <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
-              <ros>
-                <!-- <namespace>/tb3</namespace> -->
-                <remapping>~/out:=scan</remapping>
-              </ros>
-              <output_type>sensor_msgs/LaserScan</output_type>
-            </plugin>
-          </sensor>
-      </link>
-
-      <link name="wheel_left_link">
-
-        <inertial>
-          <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>0.1</mass>
-        </inertial>
-
-        <collision name="wheel_left_collision">
-          <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-          <geometry>
-            <cylinder>
-              <radius>0.033</radius>
-              <length>0.018</length>
-            </cylinder>
-          </geometry>
-          <surface>
-            <!-- This friction pamareter don't contain reliable data!! -->
-            <friction>
-              <ode>
-                <mu>100000.0</mu>
-                <mu2>100000.0</mu2>
-                <fdir1>0 0 0</fdir1>
-                <slip1>0.0</slip1>
-                <slip2>0.0</slip2>
-              </ode>
-            </friction>
-            <contact>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+5</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0.001</min_depth>
-              </ode>
-            </contact>
-          </surface>
-        </collision>
-
-        <visual name="wheel_left_visual">
-          <pose>0.0 0.144 0.023 0 0 0</pose>
-          <geometry>
-            <mesh>
-              <uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri>
-              <scale>0.001 0.001 0.001</scale>
-            </mesh>
-          </geometry>
-        </visual>
-      </link>
-
-      <link name="wheel_right_link">
-
-        <inertial>
-          <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>0.1</mass>
-        </inertial>
-
-        <collision name="wheel_right_collision">
-          <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-          <geometry>
-            <cylinder>
-              <radius>0.033</radius>
-              <length>0.018</length>
-            </cylinder>
-          </geometry>
-          <surface>
-            <!-- This friction pamareter don't contain reliable data!! -->
-            <friction>
-              <ode>
-                <mu>100000.0</mu>
-                <mu2>100000.0</mu2>
-                <fdir1>0 0 0</fdir1>
-                <slip1>0.0</slip1>
-                <slip2>0.0</slip2>
-              </ode>
-            </friction>
-            <contact>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+5</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0.001</min_depth>
-              </ode>
-            </contact>
-          </surface>
-        </collision>
-
-        <visual name="wheel_right_visual">
-          <pose>0.0 -0.144 0.023 0 0 0</pose>
-          <geometry>
-            <mesh>
-              <uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri>
-              <scale>0.001 0.001 0.001</scale>
-            </mesh>
-          </geometry>
-        </visual>
-      </link>
-
-      <link name='caster_back_right_link'>
-        <pose>-0.177 -0.064 -0.004 0 0 0</pose>
-        <inertial>
-          <mass>0.001</mass>
-          <inertia>
-            <ixx>0.00001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.00001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.00001</izz>
-          </inertia>
-        </inertial>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.005000</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <contact>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+5</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0.001</min_depth>
-              </ode>
-            </contact>
-          </surface>
-        </collision>
-      </link>
-
-      <link name='caster_back_left_link'>
-        <pose>-0.177 0.064 -0.004 0 0 0</pose>
-        <inertial>
-          <mass>0.001</mass>
-          <inertia>
-            <ixx>0.00001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.00001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.00001</izz>
-          </inertia>
-        </inertial>
-        <collision name='collision'>
-          <geometry>
-            <sphere>
-              <radius>0.005000</radius>
-            </sphere>
-          </geometry>
-          <surface>
-            <contact>
-              <ode>
-                <soft_cfm>0</soft_cfm>
-                <soft_erp>0.2</soft_erp>
-                <kp>1e+5</kp>
-                <kd>1</kd>
-                <max_vel>0.01</max_vel>
-                <min_depth>0.001</min_depth>
-              </ode>
-            </contact>
-          </surface>
-        </collision>
-      </link>
-
-      <joint name="base_joint" type="fixed">
-        <parent>base_footprint</parent>
-        <child>base_link</child>
-        <pose>0.0 0.0 0.010 0 0 0</pose>
-      </joint>
-
-      <joint name="wheel_left_joint" type="revolute">
-        <parent>base_link</parent>
-        <child>wheel_left_link</child>
-        <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-        <axis>
-          <xyz>0 0 1</xyz>
-        </axis>
-      </joint>
-
-      <joint name="wheel_right_joint" type="revolute">
-        <parent>base_link</parent>
-        <child>wheel_right_link</child>
-        <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-        <axis>
-          <xyz>0 0 1</xyz>
-        </axis>
-      </joint>
-
-      <joint name='caster_back_right_joint' type='ball'>
-        <parent>base_link</parent>
-        <child>caster_back_right_link</child>
-      </joint>
-
-      <joint name='caster_back_left_joint' type='ball'>
-        <parent>base_link</parent>
-        <child>caster_back_left_link</child>
-      </joint>
-
-      <joint name="imu_joint" type="fixed">
-        <parent>base_link</parent>
-        <child>imu_link</child>
-        <pose>-0.032 0 0.068 0 0 0</pose>
-        <axis>
-          <xyz>0 0 1</xyz>
-        </axis>
-      </joint>
-
-      <joint name="gps_sensor_joint" type="fixed">
-        <parent>base_link</parent>
-        <child>gps_link</child>
-        <pose>-0.05 0 0.05 0 0 0</pose>
-        <axis>
-          <xyz>0 0 1</xyz>
-        </axis>
-      </joint>
-
-      <joint name="lidar_joint" type="fixed">
-        <parent>base_link</parent>
-        <child>base_scan</child>
-        <pose>-0.064 0 0.121 0 0 0</pose>
-        <axis>
-          <xyz>0 0 1</xyz>
-        </axis>
-      </joint>
-
-      <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
-
-        <ros>
-          <!-- <namespace>/tb3</namespace> -->
-          <remapping>/tf:=tf</remapping>
-        </ros>
-
-        <!-- wheels -->
-        <left_joint>wheel_left_joint</left_joint>
-        <right_joint>wheel_right_joint</right_joint>
-
-        <!-- kinematics -->
-        <wheel_separation>0.287</wheel_separation>
-        <wheel_diameter>0.066</wheel_diameter>
-
-        <!-- limits -->
-        <max_wheel_torque>20</max_wheel_torque>
-        <max_wheel_acceleration>1.0</max_wheel_acceleration>
-
-        <!-- output -->
-        <publish_odom>true</publish_odom>
-        <publish_odom_tf>false</publish_odom_tf>
-        <publish_wheel_tf>false</publish_wheel_tf>
-
-        <odometry_frame>odom</odometry_frame>
-        <robot_base_frame>base_link</robot_base_frame>
-
-      </plugin>
-
-      <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
-        <ros>
-          <!-- <namespace>/tb3</namespace> -->
-          <remapping>~/out:=joint_states</remapping>
-        </ros>
-        <update_rate>250</update_rate>
-        <joint_name>wheel_left_joint</joint_name>
-        <joint_name>wheel_right_joint</joint_name>
-      </plugin>
-      
-    </model>
-
-  </world>
-</sdf>
-
diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world
deleted file mode 100644
index b8c0d0d128..0000000000
--- a/nav2_system_tests/worlds/turtlebot3_ros2_demo_obstacle.world
+++ /dev/null
@@ -1,558 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.6">
-  <world name="default">
-
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-
-    <physics type="ode">
-      <real_time_update_rate>1000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>150</iters>
-          <precon_iters>0</precon_iters>
-          <sor>1.400000</sor>
-          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0.00001</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
-          <contact_surface_layer>0.01000</contact_surface_layer>
-        </constraints>
-      </ode>
-    </physics>
-
-    <model name="turtlebot3_world">
-      <static>1</static>
-      <include>
-        <uri>model://turtlebot3_world</uri>
-      </include>
-    </model>
-
-    <model name="turtlebot3">
-      <pose>-2.0 -0.5 0.01 0.0 0.0 0.0</pose>
-
-      <link name="base_footprint"/>
-
-      <link name="base_link">
-
-        <inertial>
-          <pose>-0.064 0 0.048 0 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>1.0</mass>
-        </inertial>
-
-        <collision name="base_collision">
-          <pose>-0.064 0 0.048 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.265 0.265 0.089</size>
-            </box>
-          </geometry>
-        </collision>
-
-        <visual name="base_visual">
-          <pose>-0.064 0 0 0 0 0</pose>
-          <geometry>
-            <mesh>
-              <uri>model://turtlebot3_waffle/meshes/waffle_base.dae</uri>
-              <scale>0.001 0.001 0.001</scale>
-            </mesh>
-          </geometry>
-        </visual>
-      </link>
-
-      <link name="imu_link">
-        <sensor name="tb3_imu" type="imu">
-          <always_on>true</always_on>
-          <update_rate>200</update_rate>
-          <imu>
-            <angular_velocity>
-              <x>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </x>
-              <y>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </y>
-              <z>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>2e-4</stddev>
-                </noise>
-              </z>
-            </angular_velocity>
-            <linear_acceleration>
-              <x>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </x>
-              <y>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </y>
-              <z>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>1.7e-2</stddev>
-                </noise>
-              </z>
-            </linear_acceleration>
-          </imu>
-          <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
-            <initial_orientation_as_reference>false</initial_orientation_as_reference>
-            <ros>
-              <!-- <namespace>/tb3</namespace> -->
-              <remapping>~/out:=imu</remapping>
-            </ros>
-          </plugin>
-        </sensor>
-      </link>
-
-      <link name="base_scan">
-        <inertial>
-          <pose>-0.052 0 0.111 0 0 0</pose>
-          <inertia>
-            <ixx>0.001</ixx>
-            <ixy>0.000</ixy>
-            <ixz>0.000</ixz>
-            <iyy>0.001</iyy>
-            <iyz>0.000</iyz>
-            <izz>0.001</izz>
-          </inertia>
-          <mass>0.125</mass>
-        </inertial>
-
-        <collision name="lidar_sensor_collision">
-            <pose>-0.020 0 0.161 0 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.0508</radius>
-                <length>0.055</length>
-              </cylinder>
-            </geometry>
-          </collision>
-
-          <visual name="lidar_sensor_visual">
-            <pose>-0.064 0 0.121 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/lds.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-
-          <sensor name="hls_lfcd_lds" type="ray">
-            <always_on>true</always_on>
-            <visualize>false</visualize>
-            <pose>-0.064 0 0.121 0 0 0</pose>
-            <update_rate>5</update_rate>
-            <ray>
-              <scan>
-                <horizontal>
-                  <samples>360</samples>
-                  <resolution>1.000000</resolution>
-                  <min_angle>0.000000</min_angle>
-                  <max_angle>6.280000</max_angle>
-                </horizontal>
-              </scan>
-              <range>
-                <min>0.120000</min>
-                <max>3.5</max>
-                <resolution>0.015000</resolution>
-              </range>
-              <noise>
-                <type>gaussian</type>
-                <mean>0.0</mean>
-                <stddev>0.01</stddev>
-              </noise>
-            </ray>
-            <plugin name="laserscan" filename="libgazebo_ros_ray_sensor.so">
-              <ros>
-                <!-- <namespace>/tb3</namespace> -->
-                <remapping>~/out:=scan</remapping>
-              </ros>
-              <output_type>sensor_msgs/LaserScan</output_type>
-            </plugin>
-          </sensor>
-        </link>
-
-        <link name="wheel_left_link">
-
-          <inertial>
-            <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-            <inertia>
-              <ixx>0.001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.001</izz>
-            </inertia>
-            <mass>0.1</mass>
-          </inertial>
-
-          <collision name="wheel_left_collision">
-            <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.033</radius>
-                <length>0.018</length>
-              </cylinder>
-            </geometry>
-            <surface>
-              <!-- This friction pamareter don't contain reliable data!! -->
-              <friction>
-                <ode>
-                  <mu>100000.0</mu>
-                  <mu2>100000.0</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0.0</slip1>
-                  <slip2>0.0</slip2>
-                </ode>
-              </friction>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-
-          <visual name="wheel_left_visual">
-            <pose>0.0 0.144 0.023 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/left_tire.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-        </link>
-
-        <link name="wheel_right_link">
-
-          <inertial>
-            <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-            <inertia>
-              <ixx>0.001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.001</izz>
-            </inertia>
-            <mass>0.1</mass>
-          </inertial>
-
-          <collision name="wheel_right_collision">
-            <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-            <geometry>
-              <cylinder>
-                <radius>0.033</radius>
-                <length>0.018</length>
-              </cylinder>
-            </geometry>
-            <surface>
-              <!-- This friction pamareter don't contain reliable data!! -->
-              <friction>
-                <ode>
-                  <mu>100000.0</mu>
-                  <mu2>100000.0</mu2>
-                  <fdir1>0 0 0</fdir1>
-                  <slip1>0.0</slip1>
-                  <slip2>0.0</slip2>
-                </ode>
-              </friction>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-
-          <visual name="wheel_right_visual">
-            <pose>0.0 -0.144 0.023 0 0 0</pose>
-            <geometry>
-              <mesh>
-                <uri>model://turtlebot3_waffle/meshes/right_tire.dae</uri>
-                <scale>0.001 0.001 0.001</scale>
-              </mesh>
-            </geometry>
-          </visual>
-        </link>
-
-        <link name='caster_back_right_link'>
-          <pose>-0.177 -0.064 -0.004 0 0 0</pose>
-          <inertial>
-            <mass>0.001</mass>
-            <inertia>
-              <ixx>0.00001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.00001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.00001</izz>
-            </inertia>
-          </inertial>
-          <collision name='collision'>
-            <geometry>
-              <sphere>
-                <radius>0.005000</radius>
-              </sphere>
-            </geometry>
-            <surface>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-
-        <link name='caster_back_left_link'>
-          <pose>-0.177 0.064 -0.004 0 0 0</pose>
-          <inertial>
-            <mass>0.001</mass>
-            <inertia>
-              <ixx>0.00001</ixx>
-              <ixy>0.000</ixy>
-              <ixz>0.000</ixz>
-              <iyy>0.00001</iyy>
-              <iyz>0.000</iyz>
-              <izz>0.00001</izz>
-            </inertia>
-          </inertial>
-          <collision name='collision'>
-            <geometry>
-              <sphere>
-                <radius>0.005000</radius>
-              </sphere>
-            </geometry>
-            <surface>
-              <contact>
-                <ode>
-                  <soft_cfm>0</soft_cfm>
-                  <soft_erp>0.2</soft_erp>
-                  <kp>1e+5</kp>
-                  <kd>1</kd>
-                  <max_vel>0.01</max_vel>
-                  <min_depth>0.001</min_depth>
-                </ode>
-              </contact>
-            </surface>
-          </collision>
-        </link>
-
-        <joint name="base_joint" type="fixed">
-          <parent>base_footprint</parent>
-          <child>base_link</child>
-          <pose>0.0 0.0 0.010 0 0 0</pose>
-        </joint>
-
-        <joint name="wheel_left_joint" type="revolute">
-          <parent>base_link</parent>
-          <child>wheel_left_link</child>
-          <pose>0.0 0.144 0.023 -1.57 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <joint name="wheel_right_joint" type="revolute">
-          <parent>base_link</parent>
-          <child>wheel_right_link</child>
-          <pose>0.0 -0.144 0.023 -1.57 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <joint name='caster_back_right_joint' type='ball'>
-          <parent>base_link</parent>
-          <child>caster_back_right_link</child>
-        </joint>
-
-        <joint name='caster_back_left_joint' type='ball'>
-          <parent>base_link</parent>
-          <child>caster_back_left_link</child>
-        </joint>
-
-        <joint name="imu_joint" type="fixed">
-          <parent>base_link</parent>
-          <child>imu_link</child>
-          <pose>-0.032 0 0.068 0 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <joint name="lidar_joint" type="fixed">
-          <parent>base_link</parent>
-          <child>base_scan</child>
-          <pose>-0.064 0 0.121 0 0 0</pose>
-          <axis>
-            <xyz>0 0 1</xyz>
-          </axis>
-        </joint>
-
-        <plugin name="turtlebot3_diff_drive" filename="libgazebo_ros_diff_drive.so">
-
-          <ros>
-            <!-- <namespace>/tb3</namespace> -->
-            <remapping>/tf:=tf</remapping>
-          </ros>
-
-          <!-- wheels -->
-          <left_joint>wheel_left_joint</left_joint>
-          <right_joint>wheel_right_joint</right_joint>
-
-          <!-- kinematics -->
-          <wheel_separation>0.287</wheel_separation>
-          <wheel_diameter>0.066</wheel_diameter>
-
-          <!-- limits -->
-          <max_wheel_torque>20</max_wheel_torque>
-          <max_wheel_acceleration>1.0</max_wheel_acceleration>
-
-          <!-- output -->
-          <publish_odom>true</publish_odom>
-          <publish_odom_tf>true</publish_odom_tf>
-          <publish_wheel_tf>false</publish_wheel_tf>
-
-          <odometry_frame>odom</odometry_frame>
-          <robot_base_frame>base_footprint</robot_base_frame>
-
-        </plugin>
-
-        <plugin name="turtlebot3_joint_state" filename="libgazebo_ros_joint_state_publisher.so">
-          <ros>
-            <!-- <namespace>/tb3</namespace> -->
-            <remapping>~/out:=joint_states</remapping>
-          </ros>
-          <update_rate>250</update_rate>
-          <joint_name>wheel_left_joint</joint_name>
-          <joint_name>wheel_right_joint</joint_name>
-        </plugin>
-
-    </model>
-
-    <model name='cardboard_box'>
-      <pose frame=''>-1.0 0.6 0.15 0 -0 0</pose>
-
-      <link name='link'>
-        <inertial>
-          <mass>20</mass>
-          <inertia>
-            <ixx>0.0416667</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>0.0566667</iyy>
-            <iyz>0</iyz>
-            <izz>0.0683333</izz>
-          </inertia>
-          <pose frame=''>0 0 0 0 -0 0</pose>
-        </inertial>
-
-        <collision name='collision'>
-          <geometry>
-            <box>
-              <size>0.5 0.4 0.3</size>
-            </box>
-          </geometry>
-          <surface>
-            <friction>
-              <ode>
-                <mu>1</mu>
-                <mu2>1</mu2>
-              </ode>
-              <torsional>
-                <ode/>
-              </torsional>
-            </friction>
-            <contact>
-              <ode>
-                <kp>1e+07</kp>
-                <kd>1</kd>
-                <min_depth>0.001</min_depth>
-                <max_vel>0.1</max_vel>
-              </ode>
-            </contact>
-            <bounce/>
-          </surface>
-          <max_contacts>10</max_contacts>
-        </collision>
-
-        <visual name='visual'>
-          <pose frame=''>0 0 -0.15 0 -0 0</pose>
-          <geometry>
-            <mesh>
-              <uri>model://cardboard_box/meshes/cardboard_box.dae</uri>
-              <scale>1.25932 1.00745 0.755591</scale>
-            </mesh>
-          </geometry>
-        </visual>
-
-        <self_collide>0</self_collide>
-        <enable_wind>0</enable_wind>
-        <kinematic>0</kinematic>
-      </link>
-    </model>
-
-  </world>
-</sdf>
diff --git a/nav2_system_tests/worlds/world_only.model b/nav2_system_tests/worlds/world_only.model
deleted file mode 100644
index 4c45d4e2f9..0000000000
--- a/nav2_system_tests/worlds/world_only.model
+++ /dev/null
@@ -1,54 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.6">
-  <world name="default">
-
-    <include>
-      <uri>model://ground_plane</uri>
-    </include>
-
-    <include>
-      <uri>model://sun</uri>
-    </include>
-
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-    <gui fullscreen='0'>
-      <camera name='user_camera'>
-        <pose frame=''>0.319654 -0.235002 9.29441 0 1.5138 0.009599</pose>
-        <view_controller>orbit</view_controller>
-        <projection_type>perspective</projection_type>
-      </camera>
-    </gui>
-
-    <physics type="ode">
-      <real_time_update_rate>1000.0</real_time_update_rate>
-      <max_step_size>0.001</max_step_size>
-      <real_time_factor>1</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <iters>150</iters>
-          <precon_iters>0</precon_iters>
-          <sor>1.400000</sor>
-          <use_dynamic_moi_rescaling>1</use_dynamic_moi_rescaling>
-        </solver>
-        <constraints>
-          <cfm>0.00001</cfm>
-          <erp>0.2</erp>
-          <contact_max_correcting_vel>2000.000000</contact_max_correcting_vel>
-          <contact_surface_layer>0.01000</contact_surface_layer>
-        </constraints>
-      </ode>
-    </physics>
-
-    <model name="turtlebot3_world">
-      <static>1</static>
-      <include>
-        <uri>model://turtlebot3_world</uri>
-      </include>
-    </model>
-
-  </world>
-</sdf>

From a82090f5f1dde9caf9161dc77cbdcd4849a38691 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor
 <61468301+stevedanomodolor@users.noreply.github.com>
Date: Thu, 20 Jun 2024 23:26:24 +0200
Subject: [PATCH 17/18] Feat/port systems tests failure to new gazebo (#4453)

* migrate system failures test to new gazebo

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* cleanly kill gz sim in gazebo

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* use robot publisher and kill gz sim

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

---------

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
---
 nav2_system_tests/CMakeLists.txt              |  2 +-
 .../src/system_failure/CMakeLists.txt         |  3 -
 .../test_system_failure_launch.py             | 77 +++++++++++++------
 3 files changed, 53 insertions(+), 29 deletions(-)

diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index baa1256d52..acd3d0c2a7 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -116,8 +116,8 @@ if(BUILD_TESTING)
   add_subdirectory(src/planning)
   add_subdirectory(src/localization)
   add_subdirectory(src/system)
+  add_subdirectory(src/system_failure)
   # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
-  # add_subdirectory(src/system_failure)
   # add_subdirectory(src/updown)
   # add_subdirectory(src/waypoint_follower)
   # add_subdirectory(src/gps_navigation)
diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt
index 6bc4620535..babd49fd5b 100644
--- a/nav2_system_tests/src/system_failure/CMakeLists.txt
+++ b/nav2_system_tests/src/system_failure/CMakeLists.txt
@@ -5,8 +5,5 @@ ament_add_test(test_failure_navigator
   TIMEOUT 180
   ENV
     TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
-    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
-    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
 )
diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py
index 01a7d50bc9..052ec50b31 100755
--- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py
+++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py
@@ -16,6 +16,7 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
@@ -23,6 +24,7 @@
 from launch import LaunchDescription
 from launch import LaunchService
 from launch.actions import (
+    AppendEnvironmentVariable,
     ExecuteProcess,
     IncludeLaunchDescription,
     SetEnvironmentVariable,
@@ -33,11 +35,22 @@
 from launch_testing.legacy import LaunchTestService
 
 from nav2_common.launch import RewrittenYaml
+from nav2_simple_commander.utils import kill_os_processes
 
 
 def generate_launch_description():
-    map_yaml_file = os.getenv('TEST_MAP')
-    world = os.getenv('TEST_WORLD')
+    sim_dir = get_package_share_directory('nav2_minimal_tb3_sim')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    ros_gz_sim_dir = get_package_share_directory('ros_gz_sim')
+
+    world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
+    robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
+
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     bt_navigator_xml = os.path.join(
         get_package_share_directory('nav2_bt_navigator'),
@@ -45,8 +58,7 @@ def generate_launch_description():
         os.getenv('BT_NAVIGATOR_XML'),
     )
 
-    bringup_dir = get_package_share_directory('nav2_bringup')
-    params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
+    params_file = os.path.join(nav2_bringup_dir, 'params', 'nav2_params.yaml')
 
     # Replace the `use_astar` setting on the params file
     param_substitutions = {
@@ -66,33 +78,46 @@ def generate_launch_description():
             SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
             SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '1'),
             # Launch gazebo server for simulation
-            ExecuteProcess(
-                cmd=[
-                    'gzserver',
-                    '-s',
-                    'libgazebo_ros_init.so',
-                    '--minimal_comms',
-                    world,
-                ],
-                output='screen',
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
             ),
-            # TODO(orduno) Launch the robot state publisher instead
-            #              using a local copy of TB3 urdf file
-            Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
-                output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
+            AppendEnvironmentVariable(
+                'GZ_SIM_RESOURCE_PATH',
+                str(Path(os.path.join(sim_dir)).parent.resolve())
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py')
+                ),
+                launch_arguments={'gz_args': ['-r -s ', world_sdf_xacro]}.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')
+                ),
+                launch_arguments={
+                    'use_sim_time': 'True',
+                    'robot_sdf': robot_sdf,
+                    'x_pose': '-2.0',
+                    'y_pose': '-0.5',
+                    'z_pose': '0.01',
+                    'roll': '0.0',
+                    'pitch': '0.0',
+                    'yaw': '0.0',
+                }.items(),
             ),
             Node(
-                package='tf2_ros',
-                executable='static_transform_publisher',
+                package='robot_state_publisher',
+                executable='robot_state_publisher',
+                name='robot_state_publisher',
                 output='screen',
-                arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+                parameters=[
+                    {'use_sim_time': True, 'robot_description': robot_description}
+                ],
             ),
             IncludeLaunchDescription(
                 PythonLaunchDescriptionSource(
-                    os.path.join(bringup_dir, 'launch', 'bringup_launch.py')
+                    os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')
                 ),
                 launch_arguments={
                     'namespace': '',
@@ -129,7 +154,9 @@ def main(argv=sys.argv[1:]):
     lts.add_test_action(ld, test1_action)
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
-    return lts.run(ls)
+    return_code = lts.run(ls)
+    kill_os_processes('gz sim')
+    return return_code
 
 
 if __name__ == '__main__':

From dd7e1fc088f20501cac0241bb8fa3e09d38672ce Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor
 <61468301+stevedanomodolor@users.noreply.github.com>
Date: Thu, 20 Jun 2024 23:45:09 +0200
Subject: [PATCH 18/18] Feat/port upown tests to new gazebo (#4454)

* minor speeling check

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* minor changes

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

* minor changes

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

---------

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>
Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
---
 nav2_system_tests/CMakeLists.txt                   |  2 +-
 nav2_system_tests/src/updown/README.md             |  2 +-
 nav2_system_tests/src/updown/test_updown_launch.py | 11 +++++------
 3 files changed, 7 insertions(+), 8 deletions(-)

diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt
index acd3d0c2a7..13576a6cdd 100644
--- a/nav2_system_tests/CMakeLists.txt
+++ b/nav2_system_tests/CMakeLists.txt
@@ -117,8 +117,8 @@ if(BUILD_TESTING)
   add_subdirectory(src/localization)
   add_subdirectory(src/system)
   add_subdirectory(src/system_failure)
+  add_subdirectory(src/updown)
   # Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
-  # add_subdirectory(src/updown)
   # add_subdirectory(src/waypoint_follower)
   # add_subdirectory(src/gps_navigation)
   # add_subdirectory(src/behaviors/spin)
diff --git a/nav2_system_tests/src/updown/README.md b/nav2_system_tests/src/updown/README.md
index 6756d52465..ff84569c3a 100644
--- a/nav2_system_tests/src/updown/README.md
+++ b/nav2_system_tests/src/updown/README.md
@@ -14,7 +14,7 @@ If the test passes, you should see this comment in the output:
 
 To run the test in a loop 1000x, run the `test_updown_reliablity` script and log the output:
 ```
-./test_updown_reliablity |& tee /tmp/updown.log
+./test_updown_reliability |& tee /tmp/updown.log
 ```
 When the test is completed, pipe the log to the `updownresults.py` script to get a summary of the results:
 ```
diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py
index 458779411b..760f458d57 100755
--- a/nav2_system_tests/src/updown/test_updown_launch.py
+++ b/nav2_system_tests/src/updown/test_updown_launch.py
@@ -25,11 +25,9 @@
 def generate_launch_description():
 
     # Configuration parameters for the launch
-    launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
 
-    map_yaml_file = os.path.join(
-        get_package_share_directory('nav2_system_tests'), 'maps/map_circular.yaml'
-    )
+    map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     # Specify the actions
     start_tf_cmd_1 = Node(
@@ -61,7 +59,8 @@ def generate_launch_description():
     )
 
     nav2_bringup = launch.actions.IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')),
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py')),
         launch_arguments={
             'map': map_yaml_file,
             'use_sim_time': 'True',
@@ -77,7 +76,7 @@ def generate_launch_description():
                 'lib/nav2_system_tests/test_updown',
             )
         ],
-        cwd=[launch_dir],
+        cwd=[nav2_bringup_dir],
         output='screen',
     )