From a833307be576962b56c5820956a19b999641fecf Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 24 Mar 2026 01:20:19 -0400 Subject: [PATCH] added minimal nav2 config --- .../behavior_trees/navigate_minimal.xml | 18 ++ .../config/nav2_params_minimal.yaml | 155 ++++++++++++++++++ .../launch/navigation.launch.py | 16 +- 3 files changed, 188 insertions(+), 1 deletion(-) create mode 100644 src/subsystems/navigation/athena_planner/behavior_trees/navigate_minimal.xml create mode 100644 src/subsystems/navigation/athena_planner/config/nav2_params_minimal.yaml diff --git a/src/subsystems/navigation/athena_planner/behavior_trees/navigate_minimal.xml b/src/subsystems/navigation/athena_planner/behavior_trees/navigate_minimal.xml new file mode 100644 index 00000000..50ec2e74 --- /dev/null +++ b/src/subsystems/navigation/athena_planner/behavior_trees/navigate_minimal.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + diff --git a/src/subsystems/navigation/athena_planner/config/nav2_params_minimal.yaml b/src/subsystems/navigation/athena_planner/config/nav2_params_minimal.yaml new file mode 100644 index 00000000..d0de6ae5 --- /dev/null +++ b/src/subsystems/navigation/athena_planner/config/nav2_params_minimal.yaml @@ -0,0 +1,155 @@ +bt_navigator: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_footprint + odom_topic: /localization/odom + bt_loop_duration: 100 + default_server_timeout: 20 + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_rate_controller_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_wait_action_bt_node + - nav2_recovery_node_bt_node + +planner_server: + ros__parameters: + use_sim_time: false + expected_planner_frequency: 1.0 + planner_plugins: ["GridBased"] + + GridBased: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + tolerance: 0.5 + downsample_costmap: false + downsampling_factor: 1 + allow_unknown: true + max_iterations: 1000000 + max_planning_time: 7.5 + + motion_model_for_search: "REEDS_SHEPP" + angle_quantization_bins: 72 + analytic_expansion_ratio: 3.5 + analytic_expansion_max_length: 3.0 + minimum_turning_radius: 1.5 + reverse_penalty: 2.5 + change_penalty: 0.5 + non_straight_penalty: 1.4 + cost_penalty: 2.0 + retrospective_penalty: 0.1 + + lookup_table_size: 20.0 + cache_obstacle_heuristic: false + smooth_path: true + +controller_server: + ros__parameters: + use_sim_time: false + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + odom_topic: /localization/odom + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + + general_goal_checker: + stateful: true + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.5 + yaw_goal_tolerance: 0.3 + + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 1.0 + lookahead_dist: 2.5 + min_lookahead_dist: 1.5 + max_lookahead_dist: 4.0 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 0.3 + transform_tolerance: 0.5 + use_velocity_scaled_lookahead_dist: true + min_approach_linear_velocity: 0.15 + approach_velocity_scaling_dist: 1.5 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 1.5 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: false + allow_reversing: true + rotate_to_heading_min_angle: 0.785 + max_linear_accel: 1.0 + max_linear_decel: 1.5 + max_angular_accel: 1.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: true + +behavior_server: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.5 + simulate_ahead_time: 2.0 + max_rotational_vel: 0.3 + min_rotational_vel: 0.05 + rotational_acc_lim: 0.8 + + behavior_plugins: ["wait"] + wait: + plugin: "nav2_behaviors/Wait" + +global_costmap: + global_costmap: + ros__parameters: + use_sim_time: false + robot_base_frame: base_footprint + global_frame: map + update_frequency: 5.0 + publish_frequency: 2.0 + resolution: 0.1 + width: 100 + height: 100 + rolling_window: true + track_unknown_space: false + always_send_full_costmap: true + footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]" + plugins: [] + +local_costmap: + local_costmap: + ros__parameters: + use_sim_time: false + global_frame: map + robot_base_frame: base_footprint + update_frequency: 10.0 + publish_frequency: 5.0 + rolling_window: true + width: 8 + height: 8 + resolution: 0.05 + transform_tolerance: 0.3 + footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]" + plugins: [] + +lifecycle_manager: + ros__parameters: + use_sim_time: false + autostart: true + node_names: + - controller_server + - planner_server + - behavior_server + - bt_navigator diff --git a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py index da50e1a5..4d55704d 100644 --- a/src/subsystems/navigation/athena_planner/launch/navigation.launch.py +++ b/src/subsystems/navigation/athena_planner/launch/navigation.launch.py @@ -33,8 +33,17 @@ def generate_launch_description(): default_params = PathJoinSubstitution([ FindPackageShare('athena_planner'), 'config', 'nav2_params.yaml' ]) + minimal_params = PathJoinSubstitution([ + FindPackageShare('athena_planner'), 'config', 'nav2_params_minimal.yaml' + ]) sim = LaunchConfiguration('sim') + use_minimal = LaunchConfiguration('use_minimal') + + selected_params = PythonExpression([ + "'", minimal_params, "' if '", use_minimal, "' == 'true' else '", default_params, "'" + ]) + params_file = LaunchConfiguration('params_file') use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') @@ -129,7 +138,12 @@ def generate_launch_description(): description='Set true when running in Gazebo simulation (enables sim time and sensor bridges)', ), DeclareLaunchArgument( - 'params_file', default_value=default_params, + 'use_minimal', default_value='false', + choices=['true', 'false'], + description='Use minimal Nav2 config for ackermann testing (empty costmaps, basic BT)', + ), + DeclareLaunchArgument( + 'params_file', default_value=selected_params, description='Full path to the Nav2 params YAML', ), DeclareLaunchArgument('map_frame', default_value='map'),