New custom bt logic and athena_map fixes

src/subsystems/navigation/athena_map/CMakeLists.txt

@@ -46,6 +46,10 @@ install(DIRECTORY maps/
 install(DIRECTORY launch/
   DESTINATION share/${PROJECT_NAME}/launch)
 
+# Install config directory
+install(DIRECTORY config/
+  DESTINATION share/${PROJECT_NAME}/config)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

src/subsystems/navigation/athena_map/config/dem_costmap.yaml

@@ -1,6 +1,6 @@
 dem_costmap_converter:
   ros__parameters:
-    dem_file_path: "maps/north_site800m.tif"                  # override via launch-arg if you like
+    dem_file_path: "src/subsystems/navigation/athena_map/maps/north_site800m.tif"                  # override via launch-arg if you like
     map_resolution: 1.0
     max_passable_slope_degrees: 15.0
     output_frame: map

src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py

@@ -11,24 +11,14 @@ def generate_launch_description():
     pkg_dir = get_package_share_directory('athena_map')
     config_file = os.path.join(pkg_dir, 'config', 'dem_costmap.yaml')
 
-    # you can still expose the DEM path (if you ever want to override it)
-    dem_file_arg = DeclareLaunchArgument(
-        'dem_file_path',
-        default_value='',
-        description='Path to the DEM TIFF file'
-    )
-
     dem_node = Node(
         package='athena_map',
         executable='map_node',
         name='dem_costmap_converter',
         output='screen',
-        parameters=[config_file,  # load everything from YAML
-                    { 'dem_file_path': LaunchConfiguration('dem_file_path') }  # override just this one
-                   ]
+        parameters=[config_file]
     )
 
     return LaunchDescription([
-        dem_file_arg,
         dem_node
     ])

src/subsystems/navigation/athena_planner/behavior_trees/navigate_w_replanning_time.xml

@@ -0,0 +1,14 @@
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz.
+-->
+
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <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/config/nav2_params.yaml

@@ -3,8 +3,8 @@ bt_navigator:
     use_sim_time: true
     global_frame: map
     robot_base_frame: base_link
-    default_bt_xml_filename: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_w_replanning.xml"
-
+    # Note the bt xml file is set up in navigation.launch.py
+    
 planner_server:
   ros__parameters:
     use_sim_time: true

src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py

@@ -0,0 +1,207 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node, SetParameter
+
+
+
+
+def generate_launch_description():
+   use_sim_time = LaunchConfiguration('use_sim_time')
+   autostart = LaunchConfiguration('autostart')
+   params_file = LaunchConfiguration('params_file')
+   use_respawn = LaunchConfiguration('use_respawn')
+   log_level = LaunchConfiguration('log_level')
+
+
+   default_bt_xml_path = PathJoinSubstitution([
+       FindPackageShare('athena_planner'),
+       'behavior_trees',
+       'navigate_w_replanning_time.xml'
+   ])
+
+
+   lifecycle_nodes = [
+       'controller_server',
+       'smoother_server',
+       'planner_server',
+       'behavior_server',
+       'bt_navigator',
+       'waypoint_follower',
+       'velocity_smoother',
+   ]
+
+
+   remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')]
+
+
+   declare_use_sim_time_cmd = DeclareLaunchArgument(
+       'use_sim_time',
+       default_value='false',
+       description='Use simulation clock if true',
+   )
+
+
+   declare_params_file_cmd = DeclareLaunchArgument(
+       'params_file',
+       description='Full path to the ROS2 parameters file',
+   )
+
+
+   declare_autostart_cmd = DeclareLaunchArgument(
+       'autostart',
+       default_value='true',
+       description='Automatically startup the nav2 stack',
+   )
+
+
+   declare_use_respawn_cmd = DeclareLaunchArgument(
+       'use_respawn',
+       default_value='False',
+       description='Whether to respawn if a node crashes',
+   )
+
+
+   declare_log_level_cmd = DeclareLaunchArgument(
+       'log_level', default_value='info', description='log level'
+   )
+
+
+   ld = LaunchDescription()
+
+
+   ld.add_action(declare_use_sim_time_cmd)
+   ld.add_action(declare_params_file_cmd)
+   ld.add_action(declare_autostart_cmd)
+   ld.add_action(declare_use_respawn_cmd)
+   ld.add_action(declare_log_level_cmd)
+
+
+   ld.add_action(SetParameter('use_sim_time', use_sim_time))
+
+
+   ld.add_action(
+       Node(
+               package='nav2_controller',
+               executable='controller_server',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[params_file],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_smoother',
+               executable='smoother_server',
+               name='smoother_server',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[params_file],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings,
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_planner',
+               executable='planner_server',
+               name='planner_server',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[params_file],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings,
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_behaviors',
+               executable='behavior_server',
+               name='behavior_server',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[params_file],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_bt_navigator',
+               executable='bt_navigator',
+               name='bt_navigator',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[
+                   params_file,
+                   {'default_nav_to_pose_bt_xml': default_bt_xml_path}
+               ],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings,
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_waypoint_follower',
+               executable='waypoint_follower',
+               name='waypoint_follower',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[params_file],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings,
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_velocity_smoother',
+               executable='velocity_smoother',
+               name='velocity_smoother',
+               output='screen',
+               respawn=use_respawn,
+               respawn_delay=2.0,
+               parameters=[params_file],
+               arguments=['--ros-args', '--log-level', log_level],
+               remappings=remappings + [('cmd_vel', 'cmd_vel_nav')],
+       )
+   )
+
+
+   ld.add_action(
+       Node(
+               package='nav2_lifecycle_manager',
+               executable='lifecycle_manager',
+               name='lifecycle_manager_navigation',
+               output='screen',
+               arguments=['--ros-args', '--log-level', log_level],
+               parameters=[{'autostart': autostart}, {'node_names': lifecycle_nodes}],
+       )
+   )
+
+
+   return ld
+
+
+
+

src/subsystems/navigation/athena_planner/launch/nav2.launch.py → src/subsystems/navigation/athena_planner/launch/navigation.launch.py

@@ -13,26 +13,26 @@ def generate_launch_description():
     athena_map_share = get_package_share_directory('athena_map')
     dem_launch = os.path.join(athena_map_share, 'launch', 'dem_costmap.launch.py')
 
-    nav2_bringup_share = get_package_share_directory('nav2_bringup')
-    nav2_nav = os.path.join(nav2_bringup_share, 'launch', 'navigation_launch.py')
+    athena_planner_share = get_package_share_directory('athena_planner')
+    nav2_nav = os.path.join(athena_planner_share, 'launch', 'nav2_nodes.launch.py')
 
     default_params = PathJoinSubstitution([
         FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml'
     ])
     params_file = LaunchConfiguration('params_file')
 
-    # --- Frame names (override if your robot uses different ones) ---
     map_frame  = LaunchConfiguration('map_frame')
     odom_frame = LaunchConfiguration('odom_frame')
     base_frame = LaunchConfiguration('base_frame')
 
-    # --- Static TFs (identity transforms for bring-up/testing) ---
-    # map -> odom
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+
     static_map_to_odom = Node(
         package='tf2_ros',
         executable='static_transform_publisher',
         name='static_map_to_odom',
-        # args: x y z roll pitch yaw parent child
         arguments=['400', '400', '0', '0', '0', '0', map_frame, odom_frame],
         output='screen',
     )
@@ -56,22 +56,26 @@ def generate_launch_description():
         DeclareLaunchArgument('map_frame',  default_value='map'),
         DeclareLaunchArgument('odom_frame', default_value='odom'),
         DeclareLaunchArgument('base_frame', default_value='base_link'),
+        DeclareLaunchArgument('use_respawn', default_value='False',
+            description='Whether to respawn if a node crashes'),
+        DeclareLaunchArgument('log_level', default_value='info',
+            description='Log level for nav2 nodes'),
 
         SetRemap(src='cmd_vel', dst='/cmd_vel_nav2'),
 
-        # Static TFs (identity by default)
         static_map_to_odom,
         twist_stamper_node,
 
         IncludeLaunchDescription(PythonLaunchDescriptionSource(dem_launch)),
 
-        # Nav2 core stack (planner/controller/smoother/behavior/BT nav + lifecycle)
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(nav2_nav),
             launch_arguments={
                 'params_file': params_file,
                 'use_sim_time': 'true',
-                'autostart': 'true'
+                'autostart': 'true',
+                'use_respawn': use_respawn,
+                'log_level': log_level
             }.items()
         ),
     ])