# ============================================================================
# nav2_params_basic_tracking.yaml
#
# Basic stable trajectory-following profile for obstacle_nav2.
# This file is intentionally conservative: low speed, longer MPPI horizon,
# smoother acceleration, moderate path tracking weights, and Ackermann limits.
#
# It does not replace nav2_params.yaml. To use it, launch Nav2 with this params
# file or add a launch argument later.
# ============================================================================

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: odom
    robot_base_frame: base_footprint
    odom_topic: /odom_combined
    bt_loop_duration: 50
    default_server_timeout: 20
    default_nav_to_pose_bt_xml: ""
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_truncate_path_action_bt_node
      - nav2_truncate_path_local_action_bt_node
      - nav2_goal_updater_node_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_is_battery_low_condition_bt_node
      - nav2_navigate_through_poses_action_bt_node
      - nav2_navigate_to_pose_action_bt_node
      - nav2_remove_passed_goals_action_bt_node
      - nav2_planner_selector_bt_node
      - nav2_controller_selector_bt_node
      - nav2_goal_checker_selector_bt_node
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: False

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0

    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"

      # Longer horizon and moderate sampling make turns easier to discover.
      time_steps: 48
      model_dt: 0.05
      batch_size: 1000
      iteration_count: 1

      # Conservative velocity exploration. Speed is deliberately not optimized.
      vx_std: 0.12
      vy_std: 0.0
      wz_std: 0.85
      vx_max: 0.25
      vx_min: -0.15
      vy_max: 0.0
      wz_max: 1.8

      # Lower temperature makes selection steadier; gamma damps rough controls.
      temperature: 0.25
      gamma: 0.02

      motion_model: "Ackermann"
      visualize: false
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.60

      critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]

      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0

      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.5
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 0.8
        trajectory_point_step: 2

      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        threshold_to_consider: 1.0

      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        threshold_to_consider: 0.45

      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 10.0
        max_path_occupancy_ratio: 0.07
        trajectory_point_step: 3
        threshold_to_consider: 0.45
        offset_from_furthest: 12
        use_path_orientations: false

      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 8.0
        offset_from_furthest: 6
        threshold_to_consider: 1.0

      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 8.0
        offset_from_furthest: 5
        threshold_to_consider: 0.45
        max_angle_to_furthest: 1.2
        forward_preference: false

      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        threshold_to_consider: 0.4

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 8.0
      publish_frequency: 4.0
      transform_tolerance: 0.5
      global_frame: odom
      robot_base_frame: base_footprint
      use_sim_time: False
      rolling_window: true
      width: 4
      height: 4
      resolution: 0.05
      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
      footprint_padding: 0.02
      track_unknown_space: false
      plugins: ["obstacle_array_layer", "inflation_layer"]
      obstacle_array_layer:
        plugin: "obstacle_nav2::ObstacleArrayLayer"
        enabled: true
        topic: /obstacles
        obstacle_timeout: 0.5
        transform_tolerance: 0.2
        default_obstacle_radius: 0.05
        minimum_obstacle_radius: 0.02
        maximum_obstacle_radius: 0.06
        extra_inflation: 0.02
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.25
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: False
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 2.0
      publish_frequency: 1.0
      transform_tolerance: 0.5
      global_frame: odom
      robot_base_frame: base_footprint
      use_sim_time: False
      rolling_window: true
      width: 10
      height: 10
      resolution: 0.05
      footprint: "[[0.14, 0.085], [0.14, -0.085], [-0.14, -0.085], [-0.14, 0.085]]"
      footprint_padding: 0.02
      track_unknown_space: false
      plugins: ["obstacle_array_layer", "inflation_layer"]
      obstacle_array_layer:
        plugin: "obstacle_nav2::ObstacleArrayLayer"
        enabled: true
        topic: /obstacles
        obstacle_timeout: 0.5
        transform_tolerance: 0.2
        default_obstacle_radius: 0.05
        minimum_obstacle_radius: 0.02
        maximum_obstacle_radius: 0.50
        extra_inflation: 0.02
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.45
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: False
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: False

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    use_sim_time: False
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      downsample_costmap: false
      downsampling_factor: 1
      tolerance: 0.20
      allow_unknown: false
      max_iterations: 1000000
      max_on_approach_iterations: 1000
      max_planning_time: 5.0
      motion_model_for_search: "REEDS_SHEPP"
      angle_quantization_bins: 72
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 2.0
      minimum_turning_radius: 0.60
      reverse_penalty: 2.2
      change_penalty: 0.5
      non_straight_penalty: 0.85
      cost_penalty: 4.5
      retrospective_penalty: 0.015
      lookup_table_size: 5.0
      cache_obstacle_heuristic: false
      viz_expansions: false
      smooth_path: True
      smoother:
        max_iterations: 1000
        w_smooth: 0.35
        w_data: 0.25
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2

planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: False

smoother_server:
  ros__parameters:
    use_sim_time: False
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

behavior_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_behaviors/Spin"
    backup:
      plugin: "nav2_behaviors/BackUp"
      backup_dist: 0.30
      backup_speed: 0.08
    wait:
      plugin: "nav2_behaviors/Wait"
      wait_duration: 0.5
    global_frame: odom
    robot_base_frame: base_footprint
    transform_tolerance: 0.5
    use_sim_time: False
    simulate_ahead_time: 2.0
    max_rotational_vel: 0.8
    min_rotational_vel: 0.2
    rotational_acc_lim: 1.5

robot_state_publisher:
  ros__parameters:
    use_sim_time: False

waypoint_follower:
  ros__parameters:
    loop_rate: 20
    use_sim_time: False
    stop_on_failure: false
    waypoint_task_executor_plugin: "wait_at_waypoint"
    wait_at_waypoint:
      plugin: "nav2_waypoint_follower::WaitAtWaypoint"
      enabled: True
      waypoint_pause_duration: 200

velocity_smoother:
  ros__parameters:
    use_sim_time: False
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [0.25, 0.0, 1.8]
    min_velocity: [-0.15, 0.0, -1.8]
    max_accel: [0.50, 0.0, 1.50]
    max_decel: [-0.60, 0.0, -1.80]
    odom_topic: /odom_combined
    odom_duration: 0.1
    deadband_velocity: [0.02, 0.0, 0.02]
    velocity_timeout: 1.0