diff --git a/src/subsystems/navigation/athena_map/config/dem_costmap.yaml b/src/subsystems/navigation/athena_map/config/dem_costmap.yaml
index 40bd445..ca198ef 100644
--- a/src/subsystems/navigation/athena_map/config/dem_costmap.yaml
+++ b/src/subsystems/navigation/athena_map/config/dem_costmap.yaml
@@ -1,8 +1,8 @@
 dem_costmap_converter:
   ros__parameters:
-    dem_file_path: "src/subsystems/navigation/athena_map/maps/north_site800m.tif"                  # override via launch-arg if you like
+    dem_file_path: ""
     map_resolution: 1.0
     max_passable_slope_degrees: 15.0
     output_frame: map
-    origin_x: 518370.0
-    origin_y: 4253272.0
\ No newline at end of file
+    origin_x: -400.0
+    origin_y: -400.0
\ No newline at end of file
diff --git a/src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py b/src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py
index a835c67..49de6d4 100644
--- a/src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py
+++ b/src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py
@@ -10,13 +10,19 @@
 def generate_launch_description():
     pkg_dir = get_package_share_directory('athena_map')
     config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml')
+    dem_file = os.path.join(pkg_dir, 'maps', 'north_site800m.tif')
 
     dem_node = Node(
         package='athena_map',
         executable='map_node',
         name='dem_costmap_converter',
         output='screen',
-        parameters=[config_file]
+        parameters=[
+            config_file,
+            {
+                'dem_file_path': dem_file
+            }
+        ]
     )
 
     return LaunchDescription([
diff --git a/src/subsystems/navigation/athena_map/src/map_node.cpp b/src/subsystems/navigation/athena_map/src/map_node.cpp
index 3200077..b59e567 100644
--- a/src/subsystems/navigation/athena_map/src/map_node.cpp
+++ b/src/subsystems/navigation/athena_map/src/map_node.cpp
@@ -209,9 +209,6 @@ class DEMCostmapConverter : public rclcpp::Node
         if (costmap_ready_) {
             costmap_msg_.header.stamp = this->get_clock()->now();
             costmap_publisher_->publish(costmap_msg_);
-            RCLCPP_INFO(this->get_logger(), "Published costmap: %dx%d, resolution: %.2f, origin: (%.2f, %.2f)", 
-                        costmap_msg_.info.width, costmap_msg_.info.height, costmap_msg_.info.resolution,
-                        costmap_msg_.info.origin.position.x, costmap_msg_.info.origin.position.y);
         } else {
             RCLCPP_DEBUG(this->get_logger(), "Costmap not ready, skipping publish");
         }
diff --git a/src/subsystems/navigation/athena_planner/CMakeLists.txt b/src/subsystems/navigation/athena_planner/CMakeLists.txt
index 34b648f..0f79e07 100644
--- a/src/subsystems/navigation/athena_planner/CMakeLists.txt
+++ b/src/subsystems/navigation/athena_planner/CMakeLists.txt
@@ -1,15 +1,45 @@
 cmake_minimum_required(VERSION 3.8)
 project(athena_planner)
 
-# Warnings
 if(CMAKE_CXX_COMPILER_ID MATCHES "GNU|Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# Buildtool dep only (no code targets here)
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(nav2_util REQUIRED)  
+
+add_library(bt_nodes SHARED
+  plugins/nav_selector_node.cpp
+)
+
+target_compile_definitions(bt_nodes PRIVATE BT_PLUGIN_EXPORT)
+
+target_include_directories(bt_nodes PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/plugins>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(bt_nodes
+  rclcpp
+  std_msgs
+  behaviortree_cpp_v3
+  nav2_util  
+)
+
+install(TARGETS bt_nodes
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include
+)
 
-# ---- Install runtime assets ----
 install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME}/launch
 )
@@ -18,17 +48,16 @@ install(DIRECTORY config/
   DESTINATION share/${PROJECT_NAME}/config
 )
 
-# (Optional) install behavior trees if the folder exists
-if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/behavior_trees)
-  install(DIRECTORY behavior_trees/
-    DESTINATION share/${PROJECT_NAME}/behavior_trees
-  )
-endif()
+install(DIRECTORY behavior_trees/
+  DESTINATION share/${PROJECT_NAME}/behavior_trees
+)
+
+ament_export_libraries(bt_nodes)
+ament_export_dependencies(rclcpp std_msgs behaviortree_cpp_v3 nav2_util)
+ament_export_include_directories(include)
 
-# ---- Lint / tests (still fine even with no targets) ----
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
-  # Mark as found to avoid needing templates
   set(ament_cmake_cpplint_FOUND TRUE)
   set(ament_cmake_copyright_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
diff --git a/src/subsystems/navigation/athena_planner/behavior_trees/aruco_detection_navigation.xml b/src/subsystems/navigation/athena_planner/behavior_trees/aruco_detection_navigation.xml
new file mode 100644
index 0000000..551e893
--- /dev/null
+++ b/src/subsystems/navigation/athena_planner/behavior_trees/aruco_detection_navigation.xml
@@ -0,0 +1,7 @@
+<root>
+  <BehaviorTree ID="ArucoDetectionNavigation">
+    <ForceSuccess>
+      <Wait wait_duration="0.01"/>
+    </ForceSuccess>
+  </BehaviorTree>
+</root>
diff --git a/src/subsystems/navigation/athena_planner/behavior_trees/gnss_navigation.xml b/src/subsystems/navigation/athena_planner/behavior_trees/gnss_navigation.xml
new file mode 100644
index 0000000..08ac25e
--- /dev/null
+++ b/src/subsystems/navigation/athena_planner/behavior_trees/gnss_navigation.xml
@@ -0,0 +1,10 @@
+<root>
+  <BehaviorTree ID="GNSSNavigation">
+    <PipelineSequence name="NavigateWithReplanning">
+      <RateController hz="1.0">
+        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+      </RateController>
+      <FollowPath path="{path}" controller_id="FollowPath"/>
+    </PipelineSequence>
+  </BehaviorTree>
+</root>
diff --git a/src/subsystems/navigation/athena_planner/behavior_trees/main_router.xml b/src/subsystems/navigation/athena_planner/behavior_trees/main_router.xml
new file mode 100644
index 0000000..db473ad
--- /dev/null
+++ b/src/subsystems/navigation/athena_planner/behavior_trees/main_router.xml
@@ -0,0 +1,36 @@
+<root main_tree_to_execute="MainTree">
+
+  <!-- Bring in the other trees -->
+  <include ros_pkg="athena_planner"  path="behavior_trees/gnss_navigation.xml"/>
+  <include ros_pkg="athena_planner"  path="behavior_trees/object_detection_navigation.xml"/>
+  <include ros_pkg="athena_planner"  path="behavior_trees/aruco_detection_navigation.xml"/>
+
+  <BehaviorTree ID="MainTree">
+    <PipelineSequence name="ModeRouter">
+
+      <NavSelector topic_name="/nav_mode" default_nav_mode="GNSS" nav_mode="{nav_mode}" />
+
+      <ReactiveFallback name="SelectMode">
+
+        <BlackboardCheckString value_A="{nav_mode}" value_B="GNSS" return_on_mismatch="FAILURE">
+          <SubTree ID="GNSSNavigation" goal="{goal}" path="{path}" __shared_blackboard="true"/>
+        </BlackboardCheckString>
+
+        <BlackboardCheckString value_A="{nav_mode}" value_B="ObjectDetection" return_on_mismatch="FAILURE">
+          <SubTree ID="ObjectDetectionNavigation" __shared_blackboard="true" />
+        </BlackboardCheckString>
+
+        <BlackboardCheckString value_A="{nav_mode}" value_B="ArucoDetection" return_on_mismatch="FAILURE">
+          <SubTree ID="ArucoDetectionNavigation" __shared_blackboard="true" />
+        </BlackboardCheckString>
+
+        <!-- Default fallback -->
+        <SubTree ID="GNSSNavigation" goal="{goal}" path="{path}" __shared_blackboard="true" />
+
+      </ReactiveFallback>
+
+
+
+    </PipelineSequence>
+  </BehaviorTree>
+</root>
diff --git a/src/subsystems/navigation/athena_planner/behavior_trees/object_detection_navigation.xml b/src/subsystems/navigation/athena_planner/behavior_trees/object_detection_navigation.xml
new file mode 100644
index 0000000..034fc36
--- /dev/null
+++ b/src/subsystems/navigation/athena_planner/behavior_trees/object_detection_navigation.xml
@@ -0,0 +1,7 @@
+<root>
+  <BehaviorTree ID="ObjectDetectionNavigation">
+    <ForceSuccess>
+      <Wait wait_duration="0.01"/>
+    </ForceSuccess>
+  </BehaviorTree>
+</root>
diff --git a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml
index a157bea..ecb1cc4 100644
--- a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml
+++ b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml
@@ -4,9 +4,8 @@ bt_navigator:
     global_frame: map
     robot_base_frame: base_footprint
     odom_topic: /localization/odom
-    bt_loop_duration: 10
+    bt_loop_duration: 100
     default_server_timeout: 20
-    default_bt_xml_filename: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_w_replanning_and_recovery.xml"
     plugin_lib_names:
       - nav2_compute_path_to_pose_action_bt_node
       - nav2_compute_path_through_poses_action_bt_node
@@ -40,6 +39,7 @@ bt_navigator:
       - nav2_goal_updater_node_bt_node
       - nav2_remove_passed_goals_action_bt_node
       - nav2_reinitialize_global_localization_service_bt_node
+      - bt_nodes
 
 planner_server:
   ros__parameters:
diff --git a/src/subsystems/navigation/athena_planner/include/athena_planner/nav_selector_node.hpp b/src/subsystems/navigation/athena_planner/include/athena_planner/nav_selector_node.hpp
new file mode 100644
index 0000000..086457f
--- /dev/null
+++ b/src/subsystems/navigation/athena_planner/include/athena_planner/nav_selector_node.hpp
@@ -0,0 +1,61 @@
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAV_SELECTOR_NODE_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__NAV_SELECTOR_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "behaviortree_cpp_v3/action_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_nodes
+{
+
+class NavSelector : public BT::SyncActionNode
+{
+public:
+ 
+  NavSelector(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  static BT::PortsList providedPorts()
+  {
+    return {
+      BT::InputPort<std::string>(
+        "default_nav_mode",
+        "the default nav mode to use if there is not any external topic message received."),
+
+      BT::InputPort<std::string>(
+        "topic_name",
+        "nav_selector",
+        "the input topic name to select the nav mode"),
+
+      BT::OutputPort<std::string>(
+        "nav_mode",
+        "Selected nav mode by subscription")
+    };
+  }
+
+private:
+  
+  BT::NodeStatus tick() override;
+
+  void callbackNavSelect(const std_msgs::msg::String::SharedPtr msg);
+
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr nav_selector_sub_;
+
+  std::string last_selected_nav_;
+
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+
+  std::string topic_name_;
+};
+
+}  
+
+#endif 
diff --git a/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py b/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py
index d7c5e8f..a107a9d 100644
--- a/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py
+++ b/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py
@@ -14,13 +14,15 @@ def generate_launch_description():
    use_respawn = LaunchConfiguration('use_respawn')
    log_level = LaunchConfiguration('log_level')
 
+   bt_dir = PathJoinSubstitution([
+        FindPackageShare('athena_planner'), 
+        'behavior_trees'
+    ])
 
    default_bt_xml_path = PathJoinSubstitution([
-       FindPackageShare('athena_planner'),
-       'behavior_trees',
-       'navigate_w_replanning_time.xml'
-   ])
-
+        bt_dir,
+        'main_router.xml'
+    ])
 
    lifecycle_nodes = [
        'controller_server',
@@ -150,9 +152,10 @@ def generate_launch_description():
                respawn_delay=2.0,
                parameters=[
                    params_file,
-                   {'default_nav_to_pose_bt_xml': default_bt_xml_path}
+                   {'default_nav_to_pose_bt_xml': default_bt_xml_path},
                ],
                arguments=['--ros-args', '--log-level', log_level],
+               cwd=bt_dir,
                remappings=remappings,
        )
    )
diff --git a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py
index a3b357b..5a69db4 100644
--- a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py
+++ b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py
@@ -17,9 +17,15 @@ def generate_launch_description():
     athena_planner_share = get_package_share_directory('athena_planner')
     nav2_nav = os.path.join(athena_planner_share, 'launch', 'nav2_nodes.launch.py')
     
+    athena_map_share = get_package_share_directory('athena_map')
+    dem_costmap_launch_file = os.path.join(athena_map_share, 'launch', 'dem_costmap.launch.py')
+    
     localizer_share = get_package_share_directory('localizer')
     localizer_launch_file = os.path.join(localizer_share, 'launch', 'localizer.launch.py')
     
+    gps_goal_share = get_package_share_directory('gps_goal')
+    gps_goal_launch_file = os.path.join(gps_goal_share, 'launch', 'gps_goal_server.launch.py')
+    
     default_params = PathJoinSubstitution([
         FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml'
     ])
@@ -39,12 +45,21 @@ def generate_launch_description():
         ],
     )
     
+    dem_costmap_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(dem_costmap_launch_file)
+    )
+    
     localizer_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(localizer_launch_file),
         launch_arguments={
             'use_sim_time': 'true',
         }.items()
     )
+    
+    gps_goal_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(gps_goal_launch_file)
+    )
+    
 
     return LaunchDescription([
         DeclareLaunchArgument(
@@ -60,9 +75,9 @@ def generate_launch_description():
             description='Log level for nav2 nodes'),
 
         twist_stamper_node,
+        dem_costmap_launch, 
         localizer_launch,
-
-       #IncludeLaunchDescription(PythonLaunchDescriptionSource(dem_launch)),
+        gps_goal_launch,
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(nav2_nav),
diff --git a/src/subsystems/navigation/athena_planner/package.xml b/src/subsystems/navigation/athena_planner/package.xml
index cd1cb09..2920daf 100644
--- a/src/subsystems/navigation/athena_planner/package.xml
+++ b/src/subsystems/navigation/athena_planner/package.xml
@@ -17,6 +17,9 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tf2_geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>behaviortree_cpp_v3</depend>
+  <depend>nav2_util</depend>
 
   <exec_depend>nav2_planner</exec_depend>
   <exec_depend>nav2_costmap_2d</exec_depend>
diff --git a/src/subsystems/navigation/athena_planner/plugins/nav_selector_node.cpp b/src/subsystems/navigation/athena_planner/plugins/nav_selector_node.cpp
new file mode 100644
index 0000000..ab3924a
--- /dev/null
+++ b/src/subsystems/navigation/athena_planner/plugins/nav_selector_node.cpp
@@ -0,0 +1,74 @@
+
+#include <string>
+#include <memory>
+
+#include "std_msgs/msg/string.hpp"
+
+#include "athena_planner/nav_selector_node.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+namespace bt_nodes
+{
+
+using std::placeholders::_1;
+
+NavSelector::NavSelector(
+  const std::string & name,
+  const BT::NodeConfiguration & conf)
+: BT::SyncActionNode(name, conf)
+{
+  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+  callback_group_ = node_->create_callback_group(
+    rclcpp::CallbackGroupType::MutuallyExclusive,
+    false);
+  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
+
+  getInput("topic_name", topic_name_);
+
+  rclcpp::QoS qos(rclcpp::KeepLast(1));
+  qos.transient_local().reliable();
+
+  rclcpp::SubscriptionOptions sub_option;
+  sub_option.callback_group = callback_group_;
+  nav_selector_sub_ = node_->create_subscription<std_msgs::msg::String>(
+    topic_name_,
+    qos,
+    std::bind(&NavSelector::callbackNavSelect, this, _1),
+    sub_option);
+}
+
+BT::NodeStatus NavSelector::tick()
+{
+  callback_group_executor_.spin_some();
+
+  
+  if (last_selected_nav_.empty()) {
+    std::string default_nav_mode;
+    getInput("default_nav_mode", default_nav_mode);
+    if (default_nav_mode.empty()) {
+      return BT::NodeStatus::FAILURE;
+    } else {
+      last_selected_nav_ = default_nav_mode;
+    }
+  }
+
+  setOutput("nav_mode", last_selected_nav_);
+
+  return BT::NodeStatus::SUCCESS;
+}
+
+void
+NavSelector::callbackNavSelect(const std_msgs::msg::String::SharedPtr msg)
+{
+  last_selected_nav_ = msg->data;
+}
+
+}  
+
+BT_REGISTER_NODES(factory)
+{
+  factory.registerNodeType<bt_nodes::NavSelector>("NavSelector");
+}