From 6fc8f92d2ed2bc2d9ab741f33a0aa69460667879 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 5 Jan 2026 20:24:41 -0500 Subject: [PATCH 1/3] added new behavior tree structure --- .../navigation/athena_map/CMakeLists.txt | 4 + .../athena_map/config/dem_costmap.yaml | 2 +- .../athena_map/launch/dem_costmap.launch.py | 12 +- .../navigate_w_replanning_time.xml | 14 ++ .../launch/nav2_nodes.launch.py | 207 ++++++++++++++++++ .../{nav2.launch.py => navigation.launch.py} | 22 +- 6 files changed, 240 insertions(+), 21 deletions(-) create mode 100644 src/subsystems/navigation/athena_planner/behavior_trees/navigate_w_replanning_time.xml create mode 100644 src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py rename src/subsystems/navigation/athena_planner/launch/{nav2.launch.py => navigation.launch.py} (79%) diff --git a/src/subsystems/navigation/athena_map/CMakeLists.txt b/src/subsystems/navigation/athena_map/CMakeLists.txt index 5a87b14..5547621 100644 --- a/src/subsystems/navigation/athena_map/CMakeLists.txt +++ b/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 diff --git a/src/subsystems/navigation/athena_map/config/dem_costmap.yaml b/src/subsystems/navigation/athena_map/config/dem_costmap.yaml index dfd01f1..40bd445 100644 --- a/src/subsystems/navigation/athena_map/config/dem_costmap.yaml +++ b/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 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 f585b79..a835c67 100644 --- a/src/subsystems/navigation/athena_map/launch/dem_costmap.launch.py +++ b/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 ]) diff --git a/src/subsystems/navigation/athena_planner/behavior_trees/navigate_w_replanning_time.xml b/src/subsystems/navigation/athena_planner/behavior_trees/navigate_w_replanning_time.xml new file mode 100644 index 0000000..b2bed58 --- /dev/null +++ b/src/subsystems/navigation/athena_planner/behavior_trees/navigate_w_replanning_time.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py b/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py new file mode 100644 index 0000000..06d0612 --- /dev/null +++ b/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 + + + + diff --git a/src/subsystems/navigation/athena_planner/launch/nav2.launch.py b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py similarity index 79% rename from src/subsystems/navigation/athena_planner/launch/nav2.launch.py rename to src/subsystems/navigation/athena_planner/launch/navigation.launch.py index 98f399b..c1c163b 100644 --- a/src/subsystems/navigation/athena_planner/launch/nav2.launch.py +++ b/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() ), ]) From e59861cb0fdaae6f4b2d48016d2c417b5035f1b4 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 5 Jan 2026 20:31:15 -0500 Subject: [PATCH 2/3] added note on xml file and fixed log level --- .../navigation/athena_planner/config/nav2_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml index ec0f8e9..729302d 100644 --- a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml +++ b/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 From f11921cb2940ab1203089751072f419ecc47651d Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Mon, 5 Jan 2026 20:31:28 -0500 Subject: [PATCH 3/3] additional fixes --- .../navigation/athena_planner/launch/nav2_nodes.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 06d0612..d7c5e8f 100644 --- a/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py +++ b/src/subsystems/navigation/athena_planner/launch/nav2_nodes.launch.py @@ -152,7 +152,7 @@ def generate_launch_description(): params_file, {'default_nav_to_pose_bt_xml': default_bt_xml_path} ], - arguments=['--ros-args', '--log-level', "log_level"], + arguments=['--ros-args', '--log-level', log_level], remappings=remappings, ) )