From 3e9c4d3d8361a059f657bed395fbc9eceb2ca442 Mon Sep 17 00:00:00 2001
From: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
Date: Fri, 4 Jul 2025 20:11:04 +0200
Subject: [PATCH 1/6] Gps navigation Tutorial update

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
---
 .../config/dual_ekf_navsat_params.yaml        |   8 +-
 .../config/gps_wpf_demo.mvc                   |   7 +-
 .../config/nav2_no_map_params.yaml            | 369 ++++++++----
 .../launch/gazebo_gps_world.launch.py         | 117 ++--
 .../models/turtlebot_waffle_gps/model.config  |  19 -
 .../models/turtlebot_waffle_gps/model.sdf     | 561 ------------------
 .../urdf/turtlebot3_waffle_gps.urdf           | 305 ----------
 .../worlds/sonoma_raceway.world               |  61 --
 .../worlds/tb3_sonoma_raceway.sdf.xacro       | 103 ++++
 9 files changed, 437 insertions(+), 1113 deletions(-)
 delete mode 100755 nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.config
 delete mode 100755 nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.sdf
 delete mode 100644 nav2_gps_waypoint_follower_demo/urdf/turtlebot3_waffle_gps.urdf
 delete mode 100644 nav2_gps_waypoint_follower_demo/worlds/sonoma_raceway.world
 create mode 100644 nav2_gps_waypoint_follower_demo/worlds/tb3_sonoma_raceway.sdf.xacro

diff --git a/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml b/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml
index 5358f15..a53687e 100644
--- a/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml
+++ b/nav2_gps_waypoint_follower_demo/config/dual_ekf_navsat_params.yaml
@@ -23,7 +23,7 @@ ekf_filter_node_odom:
     odom0_differential: false
     odom0_relative: false
 
-    imu0: imu
+    imu0: imu/data
     imu0_config: [false, false, false,
                   false,  false,  true,
                   false, false, false,
@@ -85,7 +85,7 @@ ekf_filter_node_map:
     odom1_differential: false
     odom1_relative: false
 
-    imu0: imu
+    imu0: imu/data
     imu0_config: [false, false, false,
                   false,  false,  true,
                   false, false, false,
@@ -121,7 +121,7 @@ navsat_transform:
     magnetic_declination_radians: 0.0
     yaw_offset: 0.0
     zero_altitude: true
-    broadcast_utm_transform: true
+    broadcast_cartesian_transform: true
     publish_filtered_gps: true
     use_odometry_yaw: true
-    wait_for_datum: false 
+    wait_for_datum: false
diff --git a/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc b/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
index bb578c7..85a6402 100644
--- a/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
+++ b/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
@@ -22,9 +22,12 @@ displays:
       visible: true
       collapsed: false
       custom_sources:
-        []
+        - base_url: https://tiles.stadiamaps.com/tiles/alidade_satellite/{level}/{x}/{y}.png?api_key=d82ece0f-e8ff-4d1d-b61c-4724c3a6e09d
+          max_zoom: 15
+          name: Stadia (alidade_satellite)
+          type: wmts
       bing_api_key: ""
-      source: Bing Maps (terrain)
+      source: Stadia (alidade_satellite)
   - type: mapviz_plugins/point_click_publisher
     name: new display
     config:
diff --git a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
index ec47f72..b889be8 100644
--- a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
+++ b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
@@ -13,80 +13,24 @@ bt_navigator:
     odom_topic: /odom
     bt_loop_duration: 10
     default_server_timeout: 20
+    wait_for_service_timeout: 1000
     navigators: ["navigate_to_pose", "navigate_through_poses"]
     navigate_to_pose:
-      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
+      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
     navigate_through_poses:
-      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
+      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:
-      - nav2_compute_path_to_pose_action_bt_node
-      - nav2_compute_path_through_poses_action_bt_node
-      - nav2_smooth_path_action_bt_node
-      - nav2_follow_path_action_bt_node
-      - nav2_spin_action_bt_node
-      - nav2_wait_action_bt_node
-      - nav2_assisted_teleop_action_bt_node
-      - nav2_back_up_action_bt_node
-      - nav2_drive_on_heading_bt_node
-      - nav2_clear_costmap_service_bt_node
-      - nav2_is_stuck_condition_bt_node
-      - nav2_goal_reached_condition_bt_node
-      - nav2_goal_updated_condition_bt_node
-      - nav2_globally_updated_goal_condition_bt_node
-      - nav2_is_path_valid_condition_bt_node
-      - nav2_are_error_codes_active_condition_bt_node
-      - nav2_would_a_controller_recovery_help_condition_bt_node
-      - nav2_would_a_planner_recovery_help_condition_bt_node
-      - nav2_would_a_smoother_recovery_help_condition_bt_node
-      - nav2_initial_pose_received_condition_bt_node
-      - nav2_reinitialize_global_localization_service_bt_node
-      - nav2_rate_controller_bt_node
-      - nav2_distance_controller_bt_node
-      - nav2_speed_controller_bt_node
-      - nav2_truncate_path_action_bt_node
-      - nav2_truncate_path_local_action_bt_node
-      - nav2_goal_updater_node_bt_node
-      - nav2_recovery_node_bt_node
-      - nav2_pipeline_sequence_bt_node
-      - nav2_round_robin_node_bt_node
-      - nav2_transform_available_condition_bt_node
-      - nav2_time_expired_condition_bt_node
-      - nav2_path_expiring_timer_condition
-      - nav2_distance_traveled_condition_bt_node
-      - nav2_single_trigger_bt_node
-      - nav2_goal_updated_controller_bt_node
-      - nav2_is_battery_low_condition_bt_node
-      - nav2_navigate_through_poses_action_bt_node
-      - nav2_navigate_to_pose_action_bt_node
-      - nav2_remove_passed_goals_action_bt_node
-      - nav2_planner_selector_bt_node
-      - nav2_controller_selector_bt_node
-      - nav2_goal_checker_selector_bt_node
-      - nav2_controller_cancel_bt_node
-      - nav2_path_longer_on_approach_bt_node
-      - nav2_wait_cancel_bt_node
-      - nav2_spin_cancel_bt_node
-      - nav2_back_up_cancel_bt_node
-      - nav2_assisted_teleop_cancel_bt_node
-      - nav2_drive_on_heading_cancel_bt_node
-      - nav2_is_battery_charging_condition_bt_node
+
+    # 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_name_prefixes:
-      - assisted_teleop
-      - backup
       - compute_path
-      - dock_robot
-      - drive_on_heading
       - follow_path
-      - nav_thru_poses
-      - nav_to_pose
-      - route
-      - spin
-      - undock_robot
-      - wait
 
 controller_server:
   ros__parameters:
@@ -117,48 +61,103 @@ controller_server:
       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
-      # Add high threshold velocity for turtlebot 3 issue.
-      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
-      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
+      plugin: "nav2_mppi_controller::MPPIController"
+      time_steps: 56
+      model_dt: 0.05
+      batch_size: 2000
+      ax_max: 3.0
+      ax_min: -3.0
+      ay_max: 3.0
+      ay_min: -3.0
+      az_max: 3.5
+      vx_std: 0.2
+      vy_std: 0.2
+      wz_std: 0.4
+      vx_max: 0.5
+      vx_min: -0.35
+      vy_max: 0.5
+      wz_max: 1.9
+      iteration_count: 1
+      prune_distance: 1.7
+      transform_tolerance: 0.1
+      temperature: 0.3
+      gamma: 0.015
+      motion_model: "DiffDrive"
+      visualize: true
+      regenerate_noises: true
+      TrajectoryVisualizer:
+        trajectory_step: 5
+        time_step: 3
+      AckermannConstraints:
+        min_turning_r: 0.2
+      critics:
+        [
+          "ConstraintCritic",
+          "CostCritic",
+          "GoalCritic",
+          "GoalAngleCritic",
+          "PathAlignCritic",
+          "PathFollowCritic",
+          "PathAngleCritic",
+          "PreferForwardCritic",
+        ]
+      ConstraintCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 4.0
+      GoalCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 1.4
+      GoalAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.0
+        threshold_to_consider: 0.5
+      PreferForwardCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        threshold_to_consider: 0.5
+      CostCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 3.81
+        near_collision_cost: 253
+        critical_cost: 300.0
+        consider_footprint: false
+        collision_cost: 1000000.0
+        near_goal_distance: 1.0
+        trajectory_point_step: 2
+      PathAlignCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 14.0
+        max_path_occupancy_ratio: 0.05
+        trajectory_point_step: 4
+        threshold_to_consider: 0.5
+        offset_from_furthest: 20
+        use_path_orientations: false
+      PathFollowCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 5.0
+        offset_from_furthest: 5
+        threshold_to_consider: 1.4
+      PathAngleCritic:
+        enabled: true
+        cost_power: 1
+        cost_weight: 2.0
+        offset_from_furthest: 4
+        threshold_to_consider: 0.5
+        max_angle_to_furthest: 1.0
+        mode: 0
+      # TwirlingCritic:
+      #   enabled: true
+      #   twirling_cost_power: 1
+      #   twirling_cost_weight: 10.0
 
-# GPS WPF CHANGE: Remove static layer
 local_costmap:
   local_costmap:
     ros__parameters:
@@ -196,10 +195,11 @@ local_costmap:
           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
 
-# GPS WPF CHANGE: Remove static layer
-# GPS WPF CHANGE: Set rolling global costmap with 50x50 size. See note below
 global_costmap:
   global_costmap:
     ros__parameters:
@@ -215,6 +215,8 @@ global_costmap:
       rolling_window: True
       width: 50
       height: 50
+      # origin_x: 0.0
+      # origin_y: 0.0
       track_unknown_space: true
       # no static map
       plugins: ["obstacle_layer", "inflation_layer"]
@@ -247,19 +249,48 @@ global_costmap:
 #   ros__parameters:
 #     yaml_filename: ""
 
+keepout_filter_mask_server:
+  ros__parameters:
+    topic_name: "keepout_filter_mask"
+    # yaml_filename: ""
+
+keepout_costmap_filter_info_server:
+  ros__parameters:
+    type: 0
+    filter_info_topic: "keepout_costmap_filter_info"
+    mask_topic: "keepout_filter_mask"
+    base: 0.0
+    multiplier: 1.0
+
+speed_filter_mask_server:
+  ros__parameters:
+    topic_name: "speed_filter_mask"
+    # yaml_filename: ""
+
+speed_costmap_filter_info_server:
+  ros__parameters:
+    type: 1
+    filter_info_topic: "speed_costmap_filter_info"
+    mask_topic: "speed_filter_mask"
+    base: 100.0
+    multiplier: -1.0
+
 map_saver:
   ros__parameters:
     save_map_timeout: 5.0
     free_thresh_default: 0.25
     occupied_thresh_default: 0.65
     map_subscribe_transient_local: True
+    service_introspection_mode: "disabled"
 
 planner_server:
   ros__parameters:
     expected_planner_frequency: 20.0
     planner_plugins: ["GridBased"]
+    costmap_update_timeout: 1.0
+    service_introspection_mode: "disabled"
     GridBased:
-      plugin: "nav2_navfn_planner/NavfnPlanner"
+      plugin: "nav2_navfn_planner::NavfnPlanner"
       tolerance: 0.5
       use_astar: false
       allow_unknown: true
@@ -271,6 +302,9 @@ smoother_server:
       plugin: "nav2_smoother::SimpleSmoother"
       tolerance: 1.0e-10
       max_its: 1000
+      refinement_num: 2
+      # True for Hybrid A* or State Lattice to not smooth over directional changes
+      enforce_path_inversion: True
       do_refinement: True
 
 behavior_server:
@@ -280,17 +314,24 @@ behavior_server:
     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"]
+    behavior_plugins:
+      ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
     spin:
-      plugin: "nav2_behaviors/Spin"
+      plugin: "nav2_behaviors::Spin"
     backup:
-      plugin: "nav2_behaviors/BackUp"
+      plugin: "nav2_behaviors::BackUp"
+      acceleration_limit: 2.5
+      deceleration_limit: -2.5
+      minimum_speed: 0.10
     drive_on_heading:
-      plugin: "nav2_behaviors/DriveOnHeading"
+      plugin: "nav2_behaviors::DriveOnHeading"
+      acceleration_limit: 2.5
+      deceleration_limit: -2.5
+      minimum_speed: 0.10
     wait:
-      plugin: "nav2_behaviors/Wait"
+      plugin: "nav2_behaviors::Wait"
     assisted_teleop:
-      plugin: "nav2_behaviors/AssistedTeleop"
+      plugin: "nav2_behaviors::AssistedTeleop"
     local_frame: odom
     global_frame: map
     robot_base_frame: base_link
@@ -304,6 +345,7 @@ waypoint_follower:
   ros__parameters:
     loop_rate: 20
     stop_on_failure: false
+    service_introspection_mode: "disabled"
     waypoint_task_executor_plugin: "wait_at_waypoint"
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
@@ -323,3 +365,102 @@ velocity_smoother:
     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: "scan"
+      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_prestaging_tolerance: 0.5
+    service_introspection_mode: "disabled"
+
+    # 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
+      dock_direction: "forward"
+
+      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
+    # 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
+      k_delta: 2.0
+      v_linear_min: 0.15
+      v_linear_max: 0.15
+      use_collision_detection: true
+      costmap_topic: "/local_costmap/costmap_raw"
+      footprint_topic: "/local_costmap/published_footprint"
+      transform_tolerance: 0.1
+      projection_time: 5.0
+      simulation_step: 0.1
+      dock_collision_threshold: 0.3
+
+loopback_simulator:
+  ros__parameters:
+    base_frame_id: "base_footprint"
+    odom_frame_id: "odom"
+    map_frame_id: "map"
+    scan_frame_id: "base_scan" # tb4_loopback_simulator.launch.py remaps to 'rplidar_link'
+    update_duration: 0.02
+    scan_range_min: 0.05
+    scan_range_max: 30.0
+    scan_angle_min: -3.1415
+    scan_angle_max: 3.1415
+    scan_angle_increment: 0.02617
+    scan_use_inf: true
\ No newline at end of file
diff --git a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
index 4bf4449..8a9bfe9 100644
--- a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
+++ b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
@@ -1,5 +1,5 @@
-# Copyright (c) 2018 Intel Corporation
-#
+# Copyright (C) 2023 Open Source Robotics Foundation
+# Copyright (C) 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
@@ -16,67 +16,90 @@
 
 from ament_index_python.packages import get_package_share_directory
 
+
 from launch import LaunchDescription
-from launch.actions import ExecuteProcess, SetEnvironmentVariable
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    ExecuteProcess,
+)
+
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
 from launch_ros.actions import Node
 
 
 def generate_launch_description():
-    # Get the launch directory
-    gps_wpf_dir = get_package_share_directory(
-        "nav2_gps_waypoint_follower_demo")
-    launch_dir = os.path.join(gps_wpf_dir, 'launch')
-    world = os.path.join(gps_wpf_dir, "worlds", "sonoma_raceway.world")
-
-    urdf = os.path.join(gps_wpf_dir, 'urdf', 'turtlebot3_waffle_gps.urdf')
-    with open(urdf, 'r') as infp:
-        robot_description = infp.read()
+    sim_dir = get_package_share_directory("nav2_minimal_tb3_sim")
+    tutorial_dir = get_package_share_directory("nav2_gps_waypoint_follower_demo")
 
-    models_dir = os.path.join(gps_wpf_dir, "models")
-    models_dir += os.pathsep + \
-        f"/opt/ros/{os.getenv('ROS_DISTRO')}/share/turtlebot3_gazebo/models"
-    set_gazebo_model_path_cmd = None
+    # Create the launch configuration variables
+    use_sim_time = LaunchConfiguration("use_sim_time")
 
-    if 'GAZEBO_MODEL_PATH' in os.environ:
-        gazebo_model_path = os.environ['GAZEBO_MODEL_PATH'] + \
-            os.pathsep + models_dir
-        set_gazebo_model_path_cmd = SetEnvironmentVariable(
-            "GAZEBO_MODEL_PATH", gazebo_model_path)
-    else:
-        set_gazebo_model_path_cmd = SetEnvironmentVariable(
-            "GAZEBO_MODEL_PATH", models_dir)
+    # Decalre the launch arguments
+    declare_use_sim_time_cmd = DeclareLaunchArgument(
+        "use_sim_time",
+        default_value="True",
+        description="Use simulation (Gazebo) clock if true",
+    )
 
-    set_tb3_model_cmd = SetEnvironmentVariable("TURTLEBOT3_MODEL", "waffle")
+    world_sdf = os.path.join(tutorial_dir, "worlds", "tb3_sonoma_raceway.sdf.xacro")
+    robot_sdf = os.path.join(sim_dir, "urdf", "gz_waffle_gps.sdf.xacro")
 
-    # Specify the actions
-    start_gazebo_server_cmd = ExecuteProcess(
-        cmd=['gzserver', '-s', 'libgazebo_ros_init.so',
-             '-s', 'libgazebo_ros_factory.so', world],
-        cwd=[launch_dir], output='both')
+    urdf = os.path.join(sim_dir, "urdf", "turtlebot3_waffle_gps.urdf")
+    with open(urdf, "r") as infp:
+        robot_description = infp.read()
 
-    start_gazebo_client_cmd = ExecuteProcess(
-        cmd=['gzclient'],
-        cwd=[launch_dir], output='both')
+    gazebo_server = ExecuteProcess(
+        cmd=["gz", "sim", "-r", "-s", world_sdf],
+        output="screen",
+    )
+
+    gazebo_client = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("ros_gz_sim"), "launch", "gz_sim.launch.py"
+            )
+        ),
+        launch_arguments={"gz_args": ["-v4 -g "]}.items(),
+    )
+
+    gz_robot = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(sim_dir, "launch", "spawn_tb3_gps.launch.py")
+        ),
+        launch_arguments={
+            "use_sim_time": use_sim_time,
+            "robot_sdf": robot_sdf,
+            "x_pose": "2.0",
+            "y_pose": "-2.5",
+            "z_pose": "0.3",
+            "roll": "0.0",
+            "pitch": "0.0",
+            "yaw": "0.0",
+        }.items(),
+    )
 
     start_robot_state_publisher_cmd = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='both',
-        parameters=[{'robot_description': robot_description}])
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="screen",
+        parameters=[
+            {"use_sim_time": use_sim_time, "robot_description": robot_description}
+        ],
+    )
 
-    # Create the launch description and populate
     ld = LaunchDescription()
 
-    # Set gazebo up to find models properly
-    ld.add_action(set_gazebo_model_path_cmd)
-    ld.add_action(set_tb3_model_cmd)
+    # Declare the launch options
+    ld.add_action(declare_use_sim_time_cmd)
 
-    # simulator launch
-    ld.add_action(start_gazebo_server_cmd)
-    ld.add_action(start_gazebo_client_cmd)
+    ld.add_action(gz_robot)
+    ld.add_action(gazebo_server)
+    ld.add_action(gazebo_client)
 
-    # robot state publisher launch
     ld.add_action(start_robot_state_publisher_cmd)
-
     return ld
diff --git a/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.config b/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.config
deleted file mode 100755
index 6543f64..0000000
--- a/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.config
+++ /dev/null
@@ -1,19 +0,0 @@
-<?xml version="1.0"?>
-
-<model>
-  <name>turtlebot_waffle_gps</name>
-  <version>2.0</version>
-  <sdf version="1.5">model.sdf</sdf>
-
-  <author>
-    <name>Taehun Lim(Darby)</name>
-    <email>thlim@robotis.com</email>
-
-    <name>Melih Erdogan(mlherd)</name>
-    <email>h.meliherdogan@gmail.com</email>
-  </author>
-
-  <description>
-    TurtleBot3 Waffle with a gps sensor
-  </description>
-</model>
diff --git a/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.sdf b/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.sdf
deleted file mode 100755
index 2479342..0000000
--- a/nav2_gps_waypoint_follower_demo/models/turtlebot_waffle_gps/model.sdf
+++ /dev/null
@@ -1,561 +0,0 @@
-<!-- This is the same turtlebot model in nav2 with the following changes -->
-<!-- 1. A GPS sensor is added -->
-<!-- 2. A link for the GPS sensor is added -->
-<!-- 3. The diff drive plugin does not publish the tf between odom->base_footprint, which is done by RL -->
-
-<?xml version="1.0"?>
-<sdf version="1.6">
-    <model name="turtlebot_waffle_gps"> 
-      <pose>0.0 0.0 0.0 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_common/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="turtlebot3_imu" 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="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.01</stddev>
-                </noise>
-              </horizontal>
-              <vertical>
-                <noise type="gaussian">
-                  <mean>0.0</mean>
-                  <stddev>0.01</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.064 0 0.121 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.052 0 0.111 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_common/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.15 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.00000</min>
-              <max>20.0</max>
-              <resolution>0.015000</resolution>
-            </range>
-            <noise>
-              <type>gaussian</type>
-              <mean>0.0</mean>
-              <stddev>0.01</stddev>
-            </noise>
-          </ray>
-          <plugin name="turtlebot3_laserscan" filename="libgazebo_ros_ray_sensor.so">
-            <ros>
-                <!-- <namespace>/tb3</namespace> -->
-                <remapping>~/out:=scan</remapping>
-            </ros>
-            <output_type>sensor_msgs/LaserScan</output_type>
-            <frame_name>base_scan</frame_name>
-          </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_common/meshes/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_common/meshes/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>
-
-      <link name="camera_link">
-        <inertial>
-          <pose>0.069 -0.047 0.107 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.035</mass>
-        </inertial>
-        <collision name="collision">
-          <pose>0 0.047 -0.005 0 0 0</pose>
-          <geometry>
-            <box>
-              <size>0.008 0.130 0.022</size>
-            </box>
-          </geometry>
-        </collision>
-
-        <pose>0.069 -0.047 0.107 0 0 0</pose>
-
-        <sensor name="intel_realsense_r200_depth" type="depth">
-          <always_on>1</always_on>
-          <update_rate>5</update_rate>
-          <pose>0.064 -0.047 0.107 0 0 0</pose>
-          <camera name="realsense_depth_camera">
-          </camera>
-          <plugin name="intel_realsense_r200_depth_driver" filename="libgazebo_ros_camera.so">
-            <ros>
-            </ros>
-            <camera_name>intel_realsense_r200_depth</camera_name>
-            <frame_name>camera_depth_frame</frame_name>
-            <hack_baseline>0.07</hack_baseline>
-            <min_depth>0.001</min_depth>
-            <max_depth>5.0</max_depth>
-          </plugin>
-        </sensor>
-      </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="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_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="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="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>
-
-      <joint name="camera_joint" type="fixed">
-        <parent>base_link</parent>
-        <child>camera_link</child>
-        <pose>0.064 -0.065 0.094 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> -->
-          <!--since gazebo_plugins publish tf topic in global namespace /tf only and it
-            cannot be remapped like (namespace/tf) through launch arguments -->
-          <remapping>/tf:=tf</remapping>
-        </ros>
-
-        <update_rate>30</update_rate>
-
-        <!-- 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>
-
-        <command_topic>cmd_vel</command_topic>
-
-        <!-- output -->
-        <!-- The odom -> base link transform is published by the ekf in robot localization -->
-        <publish_odom>true</publish_odom>
-        <publish_odom_tf>false</publish_odom_tf>
-        <publish_wheel_tf>false</publish_wheel_tf>
-
-        <odometry_topic>odom</odometry_topic>
-        <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>30</update_rate>
-        <joint_name>wheel_left_joint</joint_name>
-        <joint_name>wheel_right_joint</joint_name>
-      </plugin>
-
-    </model>
-</sdf>
\ No newline at end of file
diff --git a/nav2_gps_waypoint_follower_demo/urdf/turtlebot3_waffle_gps.urdf b/nav2_gps_waypoint_follower_demo/urdf/turtlebot3_waffle_gps.urdf
deleted file mode 100644
index 8fa7b2c..0000000
--- a/nav2_gps_waypoint_follower_demo/urdf/turtlebot3_waffle_gps.urdf
+++ /dev/null
@@ -1,305 +0,0 @@
-<!-- This is the same turtlebot urdf in nav2 with the following changes -->
-<!-- 1. A link for the GPS sensor is added -->
-
-<?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.0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/waffle_base.dae" scale="0.001 0.001 0.001"/>
-      </geometry>
-      <material name="light_black"/>
-    </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="1.57 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/tire.dae" 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.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="1.57 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/tire.dae" 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_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"/>
-
-  <!-- Add the gps link so we get a static transform for the GPS -->
-  <joint name="gps_joint" type="fixed">
-    <parent link="base_link"/>
-    <child link="gps_link"/>
-    <origin xyz="0 0 0" rpy="0 0 0"/>
-  </joint>
-
-  <link name="gps_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">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/lds.dae" 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>
-
-  <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">
-    <visual>
-     <origin xyz="0 0 0" rpy="1.57 0 1.57"/>
-      <geometry>
-        <mesh filename="package://turtlebot3_gazebo/models/turtlebot3_common/meshes/r200.dae" />
-      </geometry>
-    </visual>
-    <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="0 0 0"/>
-    <!-- <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="0 0 0"/>
-    <parent link="camera_depth_frame"/>
-    <child link="camera_depth_optical_frame"/>
-  </joint>
-  <link name="camera_depth_optical_frame"/>
-
-</robot>
\ No newline at end of file
diff --git a/nav2_gps_waypoint_follower_demo/worlds/sonoma_raceway.world b/nav2_gps_waypoint_follower_demo/worlds/sonoma_raceway.world
deleted file mode 100644
index 9a08cbd..0000000
--- a/nav2_gps_waypoint_follower_demo/worlds/sonoma_raceway.world
+++ /dev/null
@@ -1,61 +0,0 @@
-<?xml version="1.0" ?>
-<sdf version="1.6">
-  <world name="default">
-    <!-- A global light source -->
-    <scene>
-      <ambient>0.95 0.95 0.95 1</ambient>
-      <background>0.3 0.3 0.3 1</background>
-      <shadows>true</shadows>
-      <sky>
-        <clouds>
-          <speed>3</speed>
-        </clouds>
-      </sky>
-    </scene>
-    <include>
-      <uri>model://sun</uri>
-    </include>
-    <include>
-      <uri>model://sonoma_raceway</uri>
-      <pose>-287.5 143.5 -7 0 0 0</pose>
-    </include>
-    <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>
-      <latitude_deg>38.161479</latitude_deg>
-      <longitude_deg>-122.454630</longitude_deg>
-      <elevation>488.0</elevation>
-      <heading_deg>180</heading_deg>
-    </spherical_coordinates>
-    
-    <physics type="ode">
-      <real_time_update_rate>100.0</real_time_update_rate>
-      <max_step_size>0.01</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>
-
-    <include>
-      <uri>model://turtlebot_waffle_gps</uri>
-      <pose>2 -2.5 0.3 0 0 0</pose>
-    </include>
-
-  </world>
-</sdf>
\ No newline at end of file
diff --git a/nav2_gps_waypoint_follower_demo/worlds/tb3_sonoma_raceway.sdf.xacro b/nav2_gps_waypoint_follower_demo/worlds/tb3_sonoma_raceway.sdf.xacro
new file mode 100644
index 0000000..df1f6a0
--- /dev/null
+++ b/nav2_gps_waypoint_follower_demo/worlds/tb3_sonoma_raceway.sdf.xacro
@@ -0,0 +1,103 @@
+<?xml version="1.0"?>
+<sdf version="1.6" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:arg name="headless" default="true"/>
+
+  <world name="default">
+    <plugin
+      filename="gz-sim-physics-system"
+      name="gz::sim::systems::Physics">
+    </plugin>
+    <plugin
+      filename="gz-sim-user-commands-system"
+      name="gz::sim::systems::UserCommands">
+    </plugin>
+    <plugin
+      filename="gz-sim-navsat-system"
+      name="gz::sim::systems::NavSat">
+    </plugin>
+  <xacro:unless value="$(arg headless)">
+    <plugin
+      filename="gz-sim-scene-broadcaster-system"
+      name="gz::sim::systems::SceneBroadcaster">
+    </plugin>
+  </xacro:unless>
+    <plugin
+      filename="gz-sim-sensors-system"
+      name="gz::sim::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="gz-sim-imu-system"
+      name="gz::sim::systems::Imu">
+    </plugin>
+
+    <!-- Set the coordinates for the world origin-->
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <world_frame_orientation>ENU</world_frame_orientation>
+      <latitude_deg>38.161479</latitude_deg>
+      <longitude_deg>-122.454630</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+
+    <light name='sun' type='directional'>
+        <cast_shadows>0</cast_shadows>
+        <pose>0 0 10 0 0 0</pose>
+        <diffuse>0.8 0.8 0.8 1</diffuse>
+        <specular>0.8 0.8 0.8 1</specular>
+        <attenuation>
+            <range>1000</range>
+            <constant>0.9</constant>
+            <linear>0.01</linear>
+            <quadratic>0.001</quadratic>
+        </attenuation>
+        <direction>-0.5 0.1 -0.9</direction>
+    </light>
+    <model name='ground_plane'>
+        <static>1</static>
+        <link name='link'>
+            <collision name='collision'>
+                <geometry>
+                    <plane>
+                        <normal>0 0 1</normal>
+                        <size>100 100</size>
+                    </plane>
+                </geometry>
+                <max_contacts>10</max_contacts>
+            </collision>
+            <visual name='visual'>
+                <geometry>
+                    <plane>
+                        <normal>0 0 1</normal>
+                        <size>100 100</size>
+                    </plane>
+                </geometry>
+                <material>
+                    <ambient>0.8 0.8 0.8 1</ambient>
+                    <diffuse>0.8 0.8 0.8 1</diffuse>
+                    <specular>0.8 0.8 0.8 1</specular>
+                </material>
+            </visual>
+        </link>
+    </model>
+
+    <scene>
+      <shadows>0</shadows>
+    </scene>
+
+    <physics name='3ms' type='ode'>
+        <max_step_size>0.003</max_step_size>
+        <real_time_factor>1</real_time_factor>
+    </physics>
+
+    <include>
+        <uri>
+            https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sonoma Raceway
+        </uri>
+        <name>sonoma_raceway</name>
+        <pose>-287.5 143.5 -7 0 0 0</pose>
+    </include>
+
+  </world>
+</sdf>

From 7916b4e69cdab515b7f723fa85b2305c535786d8 Mon Sep 17 00:00:00 2001
From: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
Date: Fri, 4 Jul 2025 20:43:28 +0200
Subject: [PATCH 2/6] remove key

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
---
 nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc b/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
index 85a6402..e97245c 100644
--- a/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
+++ b/nav2_gps_waypoint_follower_demo/config/gps_wpf_demo.mvc
@@ -22,7 +22,7 @@ displays:
       visible: true
       collapsed: false
       custom_sources:
-        - base_url: https://tiles.stadiamaps.com/tiles/alidade_satellite/{level}/{x}/{y}.png?api_key=d82ece0f-e8ff-4d1d-b61c-4724c3a6e09d
+        - base_url: https://tiles.stadiamaps.com/tiles/alidade_satellite/{level}/{x}/{y}.png?api_key=api_key
           max_zoom: 15
           name: Stadia (alidade_satellite)
           type: wmts

From d7d41b7da24945e5395f4f945e260ccf376f662a Mon Sep 17 00:00:00 2001
From: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
Date: Wed, 16 Jul 2025 05:43:44 +0200
Subject: [PATCH 3/6] spawn at highe location

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
---
 .../launch/gazebo_gps_world.launch.py                           | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
index 8a9bfe9..021463c 100644
--- a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
+++ b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
@@ -75,7 +75,7 @@ def generate_launch_description():
             "robot_sdf": robot_sdf,
             "x_pose": "2.0",
             "y_pose": "-2.5",
-            "z_pose": "0.3",
+            "z_pose": "0.33",
             "roll": "0.0",
             "pitch": "0.0",
             "yaw": "0.0",

From fd521ea5a04418d8a265ed30ecb6cd07d40dcc01 Mon Sep 17 00:00:00 2001
From: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
Date: Wed, 16 Jul 2025 05:46:23 +0200
Subject: [PATCH 4/6] changes based on pr

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
---
 .../config/nav2_no_map_params.yaml            | 29 -------------------
 1 file changed, 29 deletions(-)

diff --git a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
index b889be8..f75fc7c 100644
--- a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
+++ b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
@@ -195,9 +195,6 @@ local_costmap:
           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:
@@ -249,32 +246,6 @@ global_costmap:
 #   ros__parameters:
 #     yaml_filename: ""
 
-keepout_filter_mask_server:
-  ros__parameters:
-    topic_name: "keepout_filter_mask"
-    # yaml_filename: ""
-
-keepout_costmap_filter_info_server:
-  ros__parameters:
-    type: 0
-    filter_info_topic: "keepout_costmap_filter_info"
-    mask_topic: "keepout_filter_mask"
-    base: 0.0
-    multiplier: 1.0
-
-speed_filter_mask_server:
-  ros__parameters:
-    topic_name: "speed_filter_mask"
-    # yaml_filename: ""
-
-speed_costmap_filter_info_server:
-  ros__parameters:
-    type: 1
-    filter_info_topic: "speed_costmap_filter_info"
-    mask_topic: "speed_filter_mask"
-    base: 100.0
-    multiplier: -1.0
-
 map_saver:
   ros__parameters:
     save_map_timeout: 5.0

From 266b24b3b8c651614f2231178eed256e68a2fa27 Mon Sep 17 00:00:00 2001
From: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
Date: Wed, 16 Jul 2025 05:47:46 +0200
Subject: [PATCH 5/6] changes based on pr

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
---
 nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
index f75fc7c..63099a0 100644
--- a/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
+++ b/nav2_gps_waypoint_follower_demo/config/nav2_no_map_params.yaml
@@ -158,6 +158,7 @@ controller_server:
       #   twirling_cost_power: 1
       #   twirling_cost_weight: 10.0
 
+# GPS WPF CHANGE: Remove static layer
 local_costmap:
   local_costmap:
     ros__parameters:
@@ -197,6 +198,7 @@ local_costmap:
           obstacle_min_range: 0.0
       always_send_full_costmap: True
 
+# GPS WPF CHANGE: Remove static layer
 global_costmap:
   global_costmap:
     ros__parameters:

From 114b265fb9394a7185a0e76dfce590795dfdb9e0 Mon Sep 17 00:00:00 2001
From: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
Date: Wed, 16 Jul 2025 21:52:26 +0200
Subject: [PATCH 6/6] fix licence

Signed-off-by: Stevedan Omodolor <stevedan.o.omodolor@gmail.com>
---
 .../launch/gazebo_gps_world.launch.py                           | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
index 021463c..e14f83c 100644
--- a/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
+++ b/nav2_gps_waypoint_follower_demo/launch/gazebo_gps_world.launch.py
@@ -1,4 +1,4 @@
-# Copyright (C) 2023 Open Source Robotics Foundation
+# Copyright (c) 2018 Intel Corporation
 # Copyright (C) 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.