Localizer working with DEM and gps_goal added to launch file

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
+    origin_x: -400.0
+    origin_y: -400.0

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([

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");
         }

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()

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>

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>

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>

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>

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:

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 

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,
        )
    )

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),

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>