2026-09-24-v2

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /fr_dd_odom
    bt_loop_duration: 10
    always_reload_bt_xml: true
    filter_duration: 0.3
    default_server_timeout: 200 # original is 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    service_introspection_mode: "disabled"
    navigators: ["navigate_to_pose", "navigate_through_poses", "navigate_route"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
      enable_groot_monitoring: false
      groot_server_port: 1667
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
      enable_groot_monitoring: false
      groot_server_port: 1669
    navigate_route:
      plugin: "nav2_bt_navigator::NavigateRouteNavigator"
      enable_groot_monitoring: true
      groot_server_port: 1671

    ## FIXME: remove hardcoded paths and use launch file remapping instead. The default BT XML files are in nav2_bt_navigator/behavior_trees.
    default_nav_to_pose_bt_xml: '/home/alex-beh/Desktop/standard_robot_ws/overlay_ws/src/hmgics_ws/hmc_bringup/params/navigate_on_route_graph_stop_and_wait.xml'
    default_nav_route_bt_xml: '/home/alex-beh/Desktop/standard_robot_ws/overlay_ws/src/hmgics_ws/hmc_bringup/params/navigate_on_route_graph_stop_and_wait.xml'

    # default_nav_to_pose_bt_xml: '/home/navifra/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml'
    # default_nav_to_pose_bt_xml: '/home/navifra/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml'
    # default_nav_route_bt_xml: "/home/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_w_recovery.xml"
    # default_nav_route_bt_xml: "/home/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_on_route_graph_stop_and_wait.xml"

    # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults:
    # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml
    # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml
    # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2.

    # plugin_lib_names is used to add custom BT plugins to the executor (vector of strings).
    # Built-in plugins are added automatically
    # plugin_lib_names: []

    error_code_name_prefixes:
      - assisted_teleop
      - backup
      - compute_path
      - compute_route
      - dock_robot
      - drive_on_heading
      - follow_object
      - follow_path
      - nav_thru_poses
      - nav_to_pose
      - route
      - spin
      - smoother
      - undock_robot
      - wait

controller_server:
  ros__parameters:
    controller_frequency: 20.0 ###################
    ## FIXME: enforce_path_inversion never reaches the critics
    enforce_path_inversion: True

    costmap_update_timeout: 0.30
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.05
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker", "BaseGoal"]
    # controller_plugins: ["FollowPath", "FollowPathWithBaseController"] ##############
    controller_plugins: ["FollowPath"]
    path_handler_plugins: ["PathHandler"]
    use_realtime_priority: false
    speed_limit_topic: "speed_limit"
    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::PoseProgressChecker"
      required_movement_radius: 0.5
      required_movement_angle: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    general_goal_checker:
      plugin: "nav2_controller::BidirectionalGoalChecker"
      xy_goal_tolerance: 0.05  ## aligned with FollowPath.vertex_tolerance
      yaw_goal_tolerance: 0.0524 ## in radian, [02sep26] 0.0524 = 3 degree
      path_length_tolerance: 1.0
      trans_stopped_velocity: 0.02   # m/s
      rot_stopped_velocity: 0.02
      stateful: True

      # [NOTE] Disable this during deployment
      bypass_y_axis: False
      bypass_y_axis_max_x_error: 0.05
      bypass_y_axis_max_y_error: 0.5
    PathHandler:
      plugin: "nav2_controller::FeasiblePathHandler"
      prune_distance: 5.0
      enforce_path_inversion: True
      enforce_path_rotation: False
      max_robot_pose_search_dist: 2.0
      inversion_xy_tolerance: 0.2
      inversion_yaw_tolerance: 0.4
      minimum_rotation_angle: 0.785
      reject_unit_path: False
    FollowPath:
      plugin: "nav2_precise_rotation_shim_controller::RotationShimController"

      # ------------------------------------------------------------------
      # Existing rotation shim parameters
      # ------------------------------------------------------------------
      static_handoff: true
      static_handoff_vel_threshold: 0.02

      # Legacy fallback only when rotation_settle_duration == 0.
      # At controller_frequency=20 Hz, 20 cycles = 1.00 s.
      static_handoff_settle_cycles: 20

      angular_dist_threshold: 0.90  #0.15 ## 18Sep Alex  ## 0.90
      angular_disengage_threshold: 0.035  #0.02 ## 18Sep Alex  ##0.035
      terminal_goal_approach_threshold: 0.05

      rotate_to_heading_angular_vel: 0.30
      max_angular_accel: 0.35

      # IMPORTANT:
      # Current code uses this as the braking model's assumed physical deceleration.
      # Keep 0.10 only if the fully-loaded robot can reliably achieve >= 0.10 rad/s^2.
      max_angular_decel: 0.10

      rotate_to_goal_heading: true
      rotate_to_heading_once: false

      # For vertex-approach velocity source.
      closed_loop: false

      # ------------------------------------------------------------------
      # precise-rotation parameters
      # ------------------------------------------------------------------

      # Conservative effective cmd -> physical motion / odom delay.
      # Start with 0.15 s for your ~0.10 s observed delay, then measure properly.
      rotation_latency: 0.15

      # Maximum rate at which BRAKING may pull the command down.
      # For initial testing I would keep this equal to max_angular_decel
      # until actual braking capability is measured.
      max_angular_brake_decel: 0.10

      # Additional angular stopping margin.
      rotation_stop_margin: 0.003

      # Look-back window for measured angular momentum.
      rotation_momentum_window: 0.15

      # Explicit time-based stop confirmation.
      # This overrides static_handoff_settle_cycles.
      rotation_settle_duration: 0.15

      # Maximum time allowed in settling / low-speed stall handling.
      rotation_settle_timeout: 3.0

      # Allow one reduced-speed correction after confirmed physical stop.
      rotation_max_corrections: 1
      rotation_correction_angular_vel: 0.08

      # Keep disabled initially.
      # Enable only if you confirm static-friction / breakaway problems.
      rotation_breakaway_angular_vel: 0.0
      rotation_breakaway_timeout: 0.5

      # Goal yaw acceptance = ratio × Nav2 goal checker yaw tolerance.
      # The goal tolerance is 0.0524 rad (~3°), so 0.5 means ~1.5°.
      goal_yaw_acceptance_ratio: 0.5

      # ------------------------------------------------------------------
      # Existing remaining shim configuration
      # ------------------------------------------------------------------
      goal_rotation_hysteresis: 1.5

      rotate_at_vertex: true
      vertex_search_distance: 3.5
      vertex_engage_distance: 1.2
      vertex_tolerance: 0.02
      vertex_decel: 0.5
      vertex_stop_vel_threshold: 0.02
      vertex_stop_settle_cycles: 20

      # [HMGICS Alex 17Sep] true: finish the rotation onto the path TANGENT (the route's poses carry
      # exact tangents), not onto the bearing to the pose forward_sampling_distance ahead. That
      # bearing left the robot aimed 0.8-1.8 deg across the path at every leg start
      # (nav2_full_20260917_212048: 1.0-1.4 cm offset / 0.5 m), which MPPI then steered out
      # (the +0.02-0.03 rad/s start bump) and which FollowPath.advanced.straight_wz_hold cannot
      # hold. Parallel hand-over + the hold: start wz exactly 0 offline.
      use_path_orientations: true
      forward_sampling_distance: 0.50

      bidirectional: true
      bidirectional_threshold: 1.50
      preferred_turn_direction: "left"
      preferred_turn_max_penalty: 0.10

      ## FIXME: enforce_path_inversion never reaches the critics
      enforce_path_inversion: True

      primary_controller: "nav2_mppi_controller::MPPIController"
      time_steps: 50
      model_dt: 0.05
      batch_size: 600 ## 1200

      model_delay_vx: 0.10
      model_delay_vy: 0.0
      model_delay_wz: 0.10

      ## acceleration
      ax_max: 2.0   #[15Sep Tete] #3.0
      ax_min: -2.0  #[15Sep Tete] #-2.0

      ## linear velocity x
      vx_std: 0.50 #### 0.65
      vx_max: 1.7
      vx_min: -1.7

      ## linear velocity y
      vy_std: 0.0
      vy_max: 0.0
      ay_max: 0.0
      ay_min: 0.0

      ## angular velocity
      wz_std: 0.25
      wz_max: 1.2 #### NOTE: min 0.9 to turn smoothly or 2.0 corner
      az_max: 3.5

      advanced:
        # [HMGICS Alex 17Sep] Samples drawn in pairs: same vx noise, opposite wz noise.
        # Removes the small wz wiggle on straight paths while accelerating / decelerating
        # (low speed -> critics can't see wz, only ~40-100 effective samples -> their random
        # wz leaks into the command). Offline: accel wz 0.098 -> 0.042 rad/s, decel
        # 0.042 -> 0.023, straight CTE mean halved, same accel / travel time. Needs an even
        # batch_size. Dynamic: `ros2 param set /controller_server
        # FollowPath.advanced.antithetic_wz_sampling false` to A/B live (triggers reset).
        antithetic_wz_sampling: true
        # [HMGICS Alex 17Sep] Straight wz hold (nav2_mppi_controller/tools/straight_wz_hold.hpp).
        # On a straight path, while the lateral offset (now, and 1 m ahead at the current heading
        # error) stays inside the band, every rollout keeps wz = 0: the command's wz is EXACTLY 0
        # and the critics only choose the speed. The offset is corrected in the next bend, where
        # the robot turns anyway. What remains after antithetic sampling is correction of real
        # error (hand-over 1.0-1.4 cm off the path, bend-exit residue), which a diff drive cannot
        # do without wz. Band 1 cm; 2 cm from the shim's hand-over until the first release, since
        # hand-over offset + the turn's 0.1-0.3 deg residual need ~2 cm over the 4 m start straight.
        # Released 2 s of travel before a bend, and by CostCritic wanting a steer.
        # Offline, route replay forward x10 seeds, hand-over 1.0 / 1.44 cm parallel, AMCL-level noise:
        #   start (first 3 s) wz rms 0.021-0.028 -> 0 (100% of cycles exactly 0)
        #   last 1.4 m: 81-95% of cycles exactly 0, rms 0.0031-0.0041 -> 0.0016-0.0035,
        #     remaining corrections peak 0.020-0.026 (off 0.012-0.022)
        #   max CTE straight 1.0-1.45 -> 1.0-1.64 cm, bend 2.2-2.3 -> 2.2-2.4, goal 0.3 -> 0.7-1.0
        #   route time unchanged. Logs [STRAIGHT_WZ_HOLD] engaged / released (reason).
        straight_wz_hold: true
        straight_hold_lateral_tolerance: 0.015          ## 18Sep Alex  ### original 0.01
        straight_hold_handover_lateral_tolerance: 0.050 ## 24Sep Alex: 0.015
        straight_hold_prediction_distance: 1.5          ## 24Sep Alex: 3.0
        straight_hold_filter_time: 0.3                  ## 18Sep Alex: original 0.3
        straight_hold_obstacle_influence: 1.0           ## 18Sep Alex  ### original 0.5
        straight_hold_lookahead_time: 2.0
        straight_hold_max_wz: 0.015

        # Shows the band the hold is judging against, the robot's offset from the path line, 
        # and the PREDICTED offset after straight_hold_prediction_distance of travel
        # which is what usually releases the hold on a long straight,
        # while the robot itself is still well inside.
        straight_hold_publish_debug_markers: true
        straight_hold_debug_text_size: 0.16
        straight_hold_debug_readout_offset_x: 0.0
        straight_hold_debug_readout_offset_y: -2.2

        wz_std_decay_strength: 2.0 ## original 2.0  ### set -1.0 to disable
        wz_std_decay_to: 0.06 # 0.06 for 1.0 m/s max

        # Terminal translation is governed by the latency-aware MPPI braking envelope below.
        near_goal_velocity_scaling: false
        near_goal_distance: 1.5 #1.0 for 1.0 m/s max
        near_goal_vx_max: 0.10
        near_goal_vx_min: -0.10
        near_goal_wz_max: 0.25
        # near_goal_vx_std / near_goal_vx_max == vx_std / vx_max
        # near_goal_vx_std: 0.033   # as-is: 0.5 * 0.10 / 1.5

        terminal_braking: true
        terminal_brake_decel: 0.5     # replace with loaded-robot measurement before final tuning
        terminal_brake_latency: 0.10
        terminal_stop_margin: 0.01
        terminal_heading_lookback: 0.10

        curvature_velocity_scaling: true
        max_lateral_accel: 0.17 # [HMGICS Alex 17Sep] 0.30 = faster arcs, max CTE 3.2 -> 3.8 cm offline
        # [HMGICS Alex 17Sep] 1.0 -> 0.3: start slowing ~2.5 m before an arc instead of ~0.7 m.
        # At 1.0 the robot reached the arc at 1.2-1.4 m/s; the rollouts (one speed cap for the
        # whole horizon) then crossed the arc at that speed, and PathFollow's progress reward
        # made cutting in cheapest: 10 cm off before the arc, 15 cm inside it. Offline on the
        # route from nav2_full_20260917_202927, with PathAlign 45 / PathFollow 3.5 below.
        curvature_decel: 0.3
        curvature_sample_distance: 0.2
        curvature_vx_min: 0.15

      iteration_count: 1
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      # motion_model: "DiffDrive"
      motion_model: "diff_drive"
      diff_drive:
        plugin: "mppi::DiffDriveMotionModel"
      visualize: false
      publish_optimal_trajectory: true
      publish_critics_stats: true
      # [DIAGNOSTIC] Draws the reference window the path-alignment critic can actually see
      # on ~/path_align_window, and adds the numbers to ~/critics_stats. PathAlignCritic now
      # uses the complete controller-local path, not only the furthest rollout projection.
      # Clamping should therefore occur only when that whole local path is shorter than a
      # rollout (normally near a goal or feasible-path cusp).
      #
      # Watch path_window_length / trajectory_reach_length. Near 1.0 is healthy. The RViz
      # marker is green at >= 80%, amber at 50-80%, and red below 50%; its readout also says
      # whether PathAlign contributed and how much of the rollout was clamped.
      publish_path_window: false       # [NOTE] For path_align_window, Disable this during deployment
      regenerate_noises: false
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      AckermannConstraints:
        min_turning_r: 0.2
      critics:
        [
          "ConstraintCritic",               #0
          "CostCritic",                     #1
          "GoalCritic",                     #2
          "GoalAngleCritic",                #3
          "TerminalGoalCritic",             #4
          "PathAlignCritic",                #5
          "CurvatureOnlyPathAlignCritic",   #6
          "PathFollowCritic",               #7
          "PathAngleCritic",                #8
          "PreferForwardCritic",            #9
          "TwirlingCritic"                  #10
        ]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        # [DISABLED] Superseded by TerminalGoalCritic, which now owns the whole band
        # below 1.4 m. Both are UPPER gates on local_path_length, so leaving this on
        # meant two position critics running together below 1.2 m. Three problems with
        # that, none of them a disagreement about the optimum -- measured, the two rank
        # rollouts identically in every case:
        #   1. Double counting. Each critic gets balanced to std/T 0.5-2.0 on its own,
        #      so two overlapping position terms put the COMBINED authority near 4 T.
        #   2. It dilutes the anisotropy. This critic is isotropic (euclidean), the
        #      terminal one weights lateral 1.5x because a diff drive cannot fix lateral
        #      error. Measured lateral/longitudinal penalty ratio: 1.87 terminal alone,
        #      1.16 this one alone, 1.65 for the sum -- pulled back toward isotropic.
        #   3. A second relay. Two gates on a jittery local_path_length means two step
        #      changes in total cost during the terminal approach, which is the same
        #      gating pattern blamed for the weave everywhere else in this file.
        #
        # What used to be given up: this critic scores the MEAN distance over the horizon,
        # so it rewards arriving early. TerminalGoalCritic now restores that property with
        # prompt_progress_weight, using time-averaged 2-D goal error rather than a second
        # independently gated position critic.
        #
        # To revert: set enabled back to true and drop TerminalGoalCritic's
        # threshold_to_consider to 1.2 -- but then tune the two AS A PAIR on their
        # combined costs_std, never separately.
        enabled: false
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        # [NOTE] This critic may be spending its weight on the wrong axis. Two facts:
        #
        # 1. This robot is NONHOLONOMIC -- motion_model is diff_drive and vy_max is 0.0,
        #    so DiffDriveMotionModel::isHolonomic() is false. It has two controls, v and
        #    w, and cannot translate sideways. The only ways it can change yaw are to
        #    curve (v != 0, w != 0), which moves x and y with it, or to stop and spin
        #    (v = 0), which surrenders all forward progress -- and under payload the
        #    wheel scrub of a spin drags the body anyway. There is no way for it to fix
        #    yaw for free. On an omni platform none of this would apply.
        #
        # 2. rotate_to_goal_heading is true, so the shim redoes terminal yaw regardless,
        #    and computeRotateToHeadingCommand() only ever sets twist.angular.z -- it
        #    never commands vx. Position error at that handover is therefore FROZEN.
        #
        # Together: this critic is active in the last 0.5 m while the robot is still
        # approaching, so the cheapest way for a rollout to reduce yaw error here is to
        # curve -- which displaces the very terminal position the goal tolerance is
        # measured on. It buys yaw, which the shim will redo anyway, with position,
        # which the shim cannot fix. TerminalGoalCritic carries a small yaw term of its
        # own (yaw_weight 0.15) for the one job that does matter: keeping the shim's
        # spin short, since a long spin drags xy through slip.
        #
        # With rotate_to_goal_heading enabled, the shim redoes terminal yaw
        # anyway, and on a diff drive robot this critic buys that yaw with POSITION error
        # -- which is the one thing the shim cannot fix, because it commands wz only.
        # TerminalGoalCritic carries a small yaw term of its own for the same job.
        # Once TerminalGoalCritic is tuned, try dropping this to ~1.5 or disabling it
        # and compare goal xy p95; do NOT change both in the same run.
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 1.5
        symmetric_yaw_tolerance: true
        # [HMGICS Alex 17Sep] Score the path's arrival heading instead of the goal yaw when they
        # differ by more than this (rad). Same switch as TerminalGoalCritic below; the shim
        # turns onto the goal yaw in place after arrival. Offline with a goal yaw 90 deg off
        # the final straight, this critic plus the terminal yaw term turned the chassis up to
        # 90 deg over the last 0.5 m (14.5 cm off the path, 10 s to arrive); with both
        # switched: 0.4 cm, 0.6 deg, 3.9 s. 0.05 (~ yaw_goal_tolerance) also keeps a 15 deg
        # goal from pre-rotating (2.5 -> 0.5 cm). -1 disables.
        path_heading_yaw_threshold: 0.05
      TerminalGoalCritic:
        # Parks the robot ON the goal over the last metre.
        #
        # [!] cost_weight below is a STARTING POINT, not a tuned value. Balance it the
        # way every other critic here was: read costs_std off critics_stats, divide by
        # cost_weight to get the raw-unit spread, and pick the weight that lands
        #   std/T = cost_weight * raw_spread / 0.3
        # in the 0.5-2.0 band. This critic only scores the last 0.25 s of each rollout,
        # so its raw spread will NOT resemble GoalCritic's -- measure, do not scale.
        #
        # This fork also publishes effective sample size on critics_stats. Watch it
        # during the terminal approach: collapsing toward 1 means the softmax has
        # narrowed onto a single sampled rollout and the command is a fresh random draw
        # each cycle (jitter at the goal); sitting near batch_size means nothing is
        # steering. That is the fastest signal that this weight is too high or too low.
        enabled: true
        cost_power: 1
        cost_weight: 8.0

        # Legacy fallback for older TerminalGoalCritic binaries. The explicit activation
        # band below is used by this source version.
        threshold_to_consider: 1.4
        # Fade terminal position/yaw in while PathFollow is still active, then reach full
        # authority exactly where PathFollow gates off (1.4 m). This overlap removes the
        # one-cycle critic handoff without leaving a goal-seeking gap.
        activation_start: 2.5  ## Beh: 1.8  ## Kelvin: 2.5 
        activation_full: 1.4

        # Physical time, independent of model_dt. Position/yaw use a centred window;
        # velocity uses a forward-only window beginning at closest approach.
        terminal_window_time: 0.50
        # Do not reward crawling from 1.4 m away. The stop/dwell term fades in only over
        # the final 0.6 m and reaches full strength at the goal.
        stop_ramp_distance: 1.0  #0.6  ##1.0 kelvin
        # Lateral > longitudinal: a diff drive drives longitudinal error out on the
        # spot, but lateral error needs a manoeuvre it has no room for this close in.
        lateral_weight: 1.0  ##1.5
        longitudinal_weight: 1.0
        # Deliberately small -- the rotation shim owns terminal yaw. This only keeps
        # the shim's in-place spin short, because that spin drags xy through slip.
        yaw_weight: 0.15  ## 18Sep Alex ## original 1.0
        braking_vx_weight: 1.0
        # Prevent receding-horizon procrastination: reducing 2-D goal error early is cheaper
        # than scheduling the same arrival at the horizon end. The 2-D form stays active for
        # lateral and already-past recovery. This is not a minimum-speed target; braking and
        # dwell costs still own the smooth stop.
        prompt_progress_weight: 1.0
        # What separates "park here" from "pass through here". Units are m/s and rad/s
        # against metres of position error; at the near-goal caps (vx 0.20, wz 0.25)
        # they are worth about 0.10 m and 0.05 m of equivalent position error.
        stop_vx_weight: 0.15
        stop_wz_weight: 0.2
        # Charges the part of each rollout PAST the goal, along the path's incoming
        # direction, averaged over the whole horizon (metres, like position error).
        # Nothing above looks beyond a 0.25 s window at closest approach, so the optimal
        # path ran 0.34-0.38 m past the goal with the robot still 7-8 cm short
        # (nav2_full_20260915_072506). At 1.0, sitting 1 cm past costs the same as
        # stopping 1 cm short. Tune on the goal checker's x_error: trending negative
        # (parking short) -> lower; plan still running past the goal -> raise. 0 disables.
        overshoot_weight: 1.0
        # Match BidirectionalGoalChecker and the shim's selectBidirectionalHeading.
        symmetric_yaw_tolerance: true
        # [HMGICS Alex 17Sep] Yaw term scores the path's arrival heading when the goal yaw is
        # more than this (rad) off it. See GoalAngleCritic above; set both together. -1 disables.
        path_heading_yaw_threshold: 0.05
        # path_follow_debug_markers: true # [NOTE] Disable this during deployment
        publish_debug_markers: false  # [NOTE] Disable this during deployment
        # Large saturated text so the terminal diagnostics remain legible over a white map.
        debug_readout_offset_x: -1.5
        debug_readout_offset_y: -1.0
        debug_readout_text_size: 0.22
        # Once XY tolerance is reached, keep the complete final marker frame indefinitely.
        # Use a positive duration for timed expiry; -1 means no automatic RViz expiry.
        # A later inactive/new-goal update can still clear it explicitly with DELETEALL.
        debug_reached_hold_time: -1.0
      PreferForwardCritic:
        enabled: false
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        near_collision_cost: 253
        critical_cost: 60.0
        consider_footprint: false
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        ## Default value
        # cost_weight: 14.0
        # max_path_occupancy_ratio: 0.05
        # threshold_to_consider: 0.5
        # offset_from_furthest: 20
        enabled: true
        cost_power: 1
        # Balance critics on the SPREAD of their cost across the batch (critics_stats
        # costs_std), not costs_sum: MPPI softmaxes over (cost - min), so a term that shifts
        # every trajectory equally has no effect. Target std ~= 0.5-2.0 x temperature (0.3).
        #
        # Clean post-fix A/B with PathFollow=1.7:
        #   weight 23: influence 2.51, CTE 2.35 cm RMS / 3.59 cm p95 / 15.6 s
        #   weight 14: influence 1.44, CTE 4.45 cm RMS / 9.99 cm p95 / 12.2 s
        # Influence is magnitude, not correctness: 14 looks more conventionally balanced but
        # cuts the curve badly. The precision-first requirement therefore keeps 23.
        #
        # [HMGICS Alex 17Sep] 23 -> 45, with PathFollow 5.0 -> 3.5 and curvature_decel 0.3.
        # Offline, real MPPI on the Gazebo route (2 x 90 deg arcs R 2.06 m, reverse), 6 seeds,
        # max CTE: arc 15.0 -> 3.2 cm, curve entry 10.0 -> 1.3, exit 6.4 -> 2.5, straight
        # 1.3 -> 0.7; route time 37.3 -> 33.7 s (goal-yaw fix included). Arc n_eff 37 -> 136.
        # PathFollow kept at 5.0 with this weight was not robust (one seed cut 10.8 cm, arc
        # n_eff 27). Re-check live: CTE by section with tools/analyze_wz.py.
        cost_weight: 30.0 ## 18Sep Alex  ### original 45.0
        max_path_occupancy_ratio: 0.10
        trajectory_point_step: 4
        # threshold_to_consider: 0.15
        threshold_to_consider: 1.4
        # NOTE: this is a GATE, not an offset: the critic returns early when
        # furthest_reached_path_point < offset_from_furthest. That index was measured
        # oscillating around 20 at 10 Hz, so at 20 a weight-24.8 term switched fully on and
        # off every control cycle. Keep it well below the usual value of furthest.
        offset_from_furthest: 4
        use_path_orientations: false
      CurvatureOnlyPathAlignCritic:
        enabled: false
        cost_power: 1
        # Measured raw-unit spread 0.0339, so std/T = cost_weight * 0.0339 / 0.3.
        # At 3.0 this is 0.34, close to the ~0.2 floor below which a critic stops
        # influencing the result at all. 8.0 -> 0.90.
        cost_weight: 8.0
        max_path_occupancy_ratio: 0.07
        trajectory_point_step: 2
        threshold_to_consider: 0.5
        offset_from_furthest: 8
        use_path_orientations: false
        curvature_gain: 2.0
        max_curvature: 1.5
        max_curvature_extra: 2.0
        curvature_influence_distance: 2.4851
      PathFollowCritic:
        ## Default value
        # cost_weight: 5.0
        # offset_from_furthest: 5
        enabled: true
        publish_debug_markers: false # [NOTE] Disable this during deployment
        cost_power: 1
        # The only critic that rewards making progress along the path, so it must stay strong
        # enough that standing still is never the cheapest option. But it only constrains the
        # trajectory's END POINT -- it says nothing about the shape in between, so letting it
        # dominate makes the trajectory cut corners and drift off the path.
        #
        # Clean node18 -> node12 -> node11 live A/B (same route and old loaded PathAlign binary):
        #   weight 4.5: 5.09 cm RMS / 10.95 cm p95 / 9.30 s
        #   weight 3.0: 3.90 cm RMS /  8.42 cm p95 / 11.25 s
        #   weight 1.7: 2.25 cm RMS /  3.64 cm p95 / 15.75 s
        # The stated requirement prioritises sticking to the path, so keep 1.7. A post-restart
        # full-reference run measured 2.35 cm RMS / 3.59 cm p95 at PathAlign=23; the time remained
        # about 15.6 s, so repeat both directions before accepting the throughput trade-off.
        # [HMGICS Alex 17Sep] 5.0 -> 3.5 together with PathAlign 45 (see there). 1.7 tracks best
        # (max CTE 5.3 cm with PathAlign 23) but is ~25% slower; 3.5 / 45 is tighter and faster.
        cost_weight: 3.5
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        ## Default value
        # offset_from_furthest: 4
        # threshold_to_consider: 0.5
        # max_angle_to_furthest: 1.0
        # The only critic that reads trajectory yaws. Without it, at v ~ 0 every remaining
        # critic gives an identical trajectory for w = -1.0 and w = +1.0, so the cost is flat
        # in angular velocity and the robot has no gradient to rotate itself out of a stall.
        enabled: true
        cost_power: 1
        # Corrective, not tracking -- keep low. Its cost is an angle in radians and the batch
        # terminal-yaw spread is ~0.3-0.5 rad, so 2.0 already lands at std/T ~1.3-2.2.
        # Do NOT raise above ~3: at 5 it saturates the softmax and drives the oscillation.
        cost_weight: 2.0
        # The gate below is evaluated once at the robot's CURRENT pose, so this critic is
        # all-on or all-off for the whole batch and behaves as a relay with no hysteresis --
        # the oscillation source. Pushing the target point further out (12 -> 25 indices, so
        # ~0.6 m -> ~1.25 m past furthest_reached_path_point at 0.05 m path density) makes the
        # bearing far less sensitive to that index's jitter, which is what chatters the relay.
        offset_from_furthest: 4 #### 12
        threshold_to_consider: 0.5
        # Do NOT widen to quiet the oscillation: at the measured stall the robot-to-target
        # angle was 0.68 rad, so at 0.6 the critic fires and at 0.8 it would never engage.
        max_angle_to_furthest: 0.6
        mode: 1 #### 2: consider feasible path orientation
      TwirlingCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0 ## 24Sep Alex: 25.0
    
    ## ============================================
    ##           Docking Related (FollowPathWithBaseController) ## 
    ## ============================================
    BaseGoal:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.001
      yaw_goal_tolerance: 0.001
      stateful: True

    FollowPathWithBaseController:
      plugin: "nav2_hmc_controller/Nav2ControllerROS"

      # GA_MPC Base
      v_max: 0.3 # 0.8 1.0
      w_max: 0.4 ##### 0.24
      v_min: 0.1 # 0.18 0.3
      v_acc: 0.1 # 2.05 # 0.15 # 0.085 # 0.06 # 0.09 0.4
      w_acc: 0.5 # 2.05 # 0.15 # 0.085 # 0.06 # 0.09 0.4 ##### 0.025
      v_deacc: 0.35
      projection_time_max: 1.2
      learning_mlp: False
      fixed_reverse: True # False
      # fixed_reverse_value: True

      # GA_MPC Monte Carlo Sampling
      time_steps: 150 # 150
      model_dt: 0.02
      batch_size: 2000 # 2000
      iteration_count: 1
      ax_max: 3.0
      ax_min: -3.0
      vx_max: 1.0 # 1.0
      vx_min: 0.0 # -1.0
      vx_std: 0.1 # 0.45 0.05 0.1
      wz_std: 0.1 # 0.18 0.05 0.1
      wz_max: 2.0

      vx_acc_limit: 1.0 ##### lift up 0.5 ##### down 1.0
      vx_decel_limit: 0.7 ##### lift up 0.3 ##### down 0.7

      wz_acc_limit: 1.8 ##### lift up 0.9 ##### down 1.8
      wz_decel_limit: 1.8 ##### lift up 0.9 ##### down 1.8

      acceleration_limited_sampling: true

      temperature: 0.6 # 0.2 0.4
      gamma: 0.015 # 0.015
      # This controller embeds the nav2_hmc_mppi_controller optimizer, whose
      # setMotionModel() takes a fixed string ("DiffDrive" / "Omni" /
      # "Ackermann") and throws on anything else. The pluginlib namespace form
      # ("diff_drive" + a diff_drive.plugin block) belongs to the *upstream*
      # nav2_mppi_controller only, and was previously set here by mistake.
      motion_model: "DiffDrive"
      mode : "GA-MPC" # SMOOTH, SMOOTH-PID, GA-MPC, PID, MPPI,
      visualize: true
      reset_period: 1.0 # (only in Humble)
      regenerate_noises: false
      adaptive_exploration: false

      # GA_MPC Kinematics Based Geometry
      fixed_smooth: false # mode must be smooth_mppi
      transform_tolerance: 0.1
      motion_target_dist: 50.0 #4.0
      initial_rotation: true
      initial_rotation_min_angle: 6.28
      lateral_constraint_dist: 0.5
      final_rotation: true
      k_phi: 6.0 # 8.0 6.0 9.0 6.0 7.0 6.0 8.0 6.0
      k_delta: 2.0 # 2.0 6.0 4.0 1.0 2.0
      beta: 0.2
      lambda: 1.5
      v_linear_min: 0.1 # 0.05
      v_linear_max: 1.0 # 0.6 0.8 0.6
      v_angular_max: 2.0 # 2.0 1.6
      slowdown_radius: 0.1 # 3.0 4.0
      qr_marker_width: 0.05 ##### 0.05 # 0.1
      qr_marker_length: 1.5 ##### 2.4 # 1.5
      static_wall: 50.2 ##### 2.3 2.2

      # GA_MPC PID
      pid_vel: 0.1 # 0.35 0.5 0.4 0.3 0.2
      pid_thres: 0.01
      angular_heading_dist: 0.3
      lookahead_nav_factor: 2.0
      lookahead_dock_factor: 3.0 # 2.0
      goal_dist_thres: 0.00
      kp_angular: 2.4 # 0.1 0.05 0.8 0.1 3.6
      ki_angular: 0.3 # 0.3
      kd_angular: 0.1 # 1.4 0.1
      kp_lookahead: 2.4 # 1.0 0.8 1.2 1.4 2.0 2.8 5.0 1.4 4.2 1.4 2.4
      ki_lookahead: 0.3 # 0.3 2.3
      kd_lookahead: 2.1 # 1.4 4.0 2.4 2.8 1.0 0.1 2.1

      # GA_MPC Critics
      critics: ["PathAlignCritic", "CurvatureAwareCritic", "PathAngleCritic"] ##### GoalTargetVelocityCritic
      PayloadJerkCritic:
        enabled: true
        cost_weight: 0.1
        cost_power: 1

        weight_v: 0.8
        weight_y: 0.2
        weight_w: 0.2

        robot_mass: 1.0 # amr weight : 100kg
        robot_inertia: 1.0

        # 25 * 0.02 = 0.5 sec
        peak_window_steps: 25

        # secondary critic cap
        max_payload_cost: 1.0

        smooth_v_threshold: 0.02
        smooth_w_threshold: 0.02

        force_rate_x_ref: 1.0
        force_rate_y_ref: 1.0
        torque_rate_ref: 1.0

        model_dt: 0.02
        use_dt_normalization: false
        use_squared_cost: false

      CurvatureAwareCritic:
        enabled: true
        cost_weight: 2.0 # 1.0
        cost_power: 1
        v_max: 0.7 ##### lift up 0.5 ##### lift down  0.7
        kappa_threshold: 0.0
        preview_distance: 2.0
        mean_cost_scale: 180.0
        # 0.25 = 현재 kappa_future_raw 25%, 이전 smoothed 75%
        kappa_future_smoothing_alpha: 0.5

      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 100.0
        trajectory_point_step: 4 # 4 ##### 10
        threshold_to_consider: 0.05 ###### 0.05
        offset_from_furthest: 1

      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 6.0 # 18.0   # ↓ 회전비중 조정 (경로 정렬은 유지)
        offset_from_furthest: 4 # 4
        threshold_to_consider: 0.05 # 0.18 0.1 ##### 0.08
        max_angle_to_furthest: 0.05 # 0.18 0.1 ##### 0.08
        forward_preference: true

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

      GoalTargetVelocityCritic:
        enabled: true
        cost_weight: 1.0
        cost_power: 1

        # 1.2m 안쪽에서 유지할 목표 속도
        target_v: 0.2

        # 1.2m부터 goal까지는 0.1 유지
        slow_radius: 1.2 # 1.2

        # 1.2m 바깥쪽에서는 이 거리부터 overspeed cost가 점점 생김
        outer_radius: 4.0

        # 이 거리 밖에서는 critic 비활성
        goal_active_radius: 4.0

        # 0.0이면 horizon 전체 평가
        preview_distance: 0.0

        # 현재 raw cost가 작으면 20~50까지 올려도 됨
        mean_cost_scale: 35.0

        max_velocity_error: 1.0
        use_squared_error: false

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 10.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      map_topic: /map_nav
      rolling_window: true
      width: 12
      height: 12
      resolution: 0.05
      # [from lidar calib]
      # footprint: "[[0.594907, -0.440983], [0.594907, 0.359017], [-0.545093, 0.359017], [-0.545093, -0.440983]]"
      # [from CAD]
      footprint: "[[0.572, -0.38], [0.572, 0.38], [-0.572, 0.38], [-0.572, -0.38]]"
      footprint_padding: 0.05
      # plugins: ["obstacle_layer", "nonpersisting_obstacle_layer", "inflation_layer"]
      plugins: ["obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        # enabled: KEEPOUT_ZONE_ENABLED
        enabled: False
        filter_info_topic: "keepout_costmap_filter_info"
        override_lethal_cost: True
        lethal_override_cost: 200
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0 #### 3.0
        inflation_radius: 1.7 #### 1.0 [no payload]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: scan
        scan:
          topic: "/scan"
          sensor_frame: "base_link"
          inf_is_valid: True
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          obstacle_max_range: 8.0
          obstacle_min_range: 0.0
          raytrace_max_range: 10.0 # increase this => trace longer range for local cost map clearing
          raytrace_min_range: 0.0
          observation_persistence: 0.2 # keep obstacle for 0.2 sec if not seen 
      nonpersisting_obstacle_layer:
        plugin: "nav2_costmap_2d/NonPersistentVoxelLayer"
        enabled:              true
        track_unknown_space:  true
        max_obstacle_height:  3.0
        unknown_threshold:    15
        mark_threshold:       2
        combination_method:   1
        obstacle_range: 2.0
        origin_z: 0.
        z_resolution: 0.05
        z_voxels: 16
        publish_voxel_map: false
        observation_sources: depth
        depth:
          data_type: PointCloud2
          topic: /amr_vision/residual_cell_points
          marking: true
          min_obstacle_height: 0.12
          max_obstacle_height: 2.8
      
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      always_send_full_costmap: True
      service_introspection_mode: "disabled"

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      # footprint: "[[0.594907, -0.440983], [0.594907, 0.359017], [-0.545093, 0.359017], [-0.545093, -0.440983]]"
      footprint: "[[0.572, -0.38], [0.572, 0.38], [-0.572, 0.38], [-0.572, -0.38]]"
      footprint_padding: 0.05
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        # enabled: KEEPOUT_ZONE_ENABLED
        enabled: False
        filter_info_topic: "keepout_costmap_filter_info"
        override_lethal_cost: True
        lethal_override_cost: 200
      speed_filter:
        plugin: "nav2_costmap_2d::SpeedFilter"
        enabled: SPEED_ZONE_ENABLED
        filter_info_topic: "speed_costmap_filter_info"
        speed_limit_topic: "speed_limit"
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: "/scan"
          inf_is_valid: True         # [16July] default false
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 9.0    # [16July] 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 8.0    # [16July] 2.5
          obstacle_min_range: 0.0
      static_layer:
        topic: "/map_nav"
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 5.0 #### 3.0
        inflation_radius: 1.7 #### 1.0 [no payload]
      always_send_full_costmap: True
      service_introspection_mode: "disabled"

keepout_filter_mask_server:
  ros__parameters:
    topic_name: "keepout_filter_mask"
    # yaml_filename: ""

keepout_costmap_filter_info_server:
  ros__parameters:
    type: 0
    filter_info_topic: "keepout_costmap_filter_info"
    mask_topic: "keepout_filter_mask"
    base: 0.0
    multiplier: 1.0

speed_filter_mask_server:
  ros__parameters:
    topic_name: "speed_filter_mask"
    # yaml_filename: ""

speed_costmap_filter_info_server:
  ros__parameters:
    type: 1
    filter_info_topic: "speed_costmap_filter_info"
    mask_topic: "speed_filter_mask"
    base: 100.0
    multiplier: -1.0

planner_server:
  ros__parameters:
    expected_planner_frequency: 20.0
    planner_plugins: ["GridBased", "AstarGridBased"]
                  # , "GridBasedArc"]
    costmap_update_timeout: 1.0
    service_introspection_mode: "disabled"
    AstarGridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.05
      use_astar: true
      allow_unknown: true
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid"
      downsample_costmap: false           # whether or not to downsample the map
      downsampling_factor: 1              # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm)
      tolerance: 0.10                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      allow_unknown: true                 # allow traveling in unknown space
      max_iterations: 1000000             # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable
      max_on_approach_iterations: 1000    # Maximum number of iterations after within tolerances to continue to try to find exact solution
      max_planning_time: 5.0              # max time in s for planner to plan, smooth
      motion_model_for_search: "REEDS_SHEPP"    # Hybrid-A* Dubin, Redds-Shepp
      angle_quantization_bins: 72         # Number of angle bins for search
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      analytic_expansion_max_cost: 200.0  # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal
      analytic_expansion_max_cost_override: false  #  Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      # minimum_turning_radius: 0.40        # minimum turning radius in m of path / vehicle
      minimum_turning_radius: 0.80
      reverse_penalty: 1.2                # Penalty to apply if motion is reversing, must be => 1
      # change_penalty: 0.2                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      change_penalty: 0.4
      # non_straight_penalty: 1.2           # Penalty to apply if motion is non-straight, must be => 1
      non_straight_penalty: 1.5
      cost_penalty: 2.0                   # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable.
      retrospective_penalty: 0.015
      lookup_table_size: 20.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      cache_heuristic_lookup_table: true  # Persist the dubin/reeds-sheep distance window to disk so restarts skip the precompute. Not tied to the map; invalidated by costmap resolution, minimum_turning_radius, lookup_table_size, angle_quantization_bins or motion_model_for_search.
      heuristic_cache_dir: "/tmp"             # Where to keep that cache. Empty -> $ROS_HOME/nav2_smac_planner (i.e. ~/.ros/nav2_smac_planner).
      cache_obstacle_heuristic: false     # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static.
      debug_visualizations: false         # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
      use_quadratic_cost_penalty: False
      downsample_obstacle_heuristic: True
      allow_primitive_interpolation: False
      coarse_search_resolution: 4         # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode.
      goal_heading_mode: "DEFAULT"        # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION
      smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path

      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2

    ## ============================================
    ##           Docking Related (GridBasedArc)  ## 
    ## ============================================
    GridBasedArc:
      plugin: "nav2_hmc_planner/Nav2PlannerROS"
      step_arc_length: 0.025
      step_str_length: 0.025
      const_path_length: 2.0
      arc_radius: 0.1 # 0.3 # 1.5
      sc_arc_goal: 0.1 # 0.5
      sc_str_goal: 0.1 # 0.5
      pr_goal: 0.5 # 0.1
      goal_checker: 0.5
      lift_stop_sec: 4.0
      extension_dist: 0.3
      use_reverse: true
      dock_registry_path: ""

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother", "route_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      do_refinement: True
      max_its: 1000
      w_data: 0.3   #path data weight
      w_smooth: 0.2   #path를 smooth할 때 weight
    # route_smoother:
    #   plugin: "nav2_smoother::SimpleSmoother"
    #   tolerance: 1.0e-10
    #   max_its: 1000
    #   refinement_num: 5
    #   # False for Route Server or others where smoothing out sharp corners is OK
    #   enforce_path_inversion: False
    #   do_refinement: True
    route_smoother:
      # [NOTE] CornerFilletSmoother rounds every corner of the final concatenated
      # path to smoothing_radius, independent of the route window. This is why
      # route_server smooth_corners is false: corner rounding is owned here (always
      # runs on SmoothPath) instead of in the route server (only fillets junctions
      # interior to the sliding base window, so corners flicker as the robot turns).
      plugin: "nav2_smoother::CornerFilletSmoother"
      smoothing_radius: 2.0      # match route_server smoothing_radius
      density: 0.05              # arc/segment sample spacing (m)
      simplify_tolerance: 0.10   # RDP epsilon; raise if the A* first-mile staircase makes fake corners
      min_turn_angle: 0.35       # skip fillet below ~20 deg turns

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    global_costmap_topic: global_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins:
      ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    spin:
      plugin: "nav2_behaviors::Spin"
    backup:
      plugin: "nav2_behaviors::BackUp"
      acceleration_limit: 2.5
      deceleration_limit: -2.5
      minimum_speed: 0.10
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
      acceleration_limit: 2.5
      deceleration_limit: -2.5
      minimum_speed: 0.10
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

route_server:
  ros__parameters:
    debug_traversability_rejections: true
    # The graph_filepath does not need to be specified since it going to be set by defaults in launch.
    # If you'd rather set it in the yaml, remove the default "graph" value in the launch file(s).
    # file & provide full path to map below. If graph config or launch default is provided, it is used
    # graph_filepath: $(find-pkg-share nav2_route)/graphs/aws_graph.geojson
    boundary_radius_to_achieve_node: 1.0
    radius_to_achieve_node: 2.0
    smoothing_radius: 2.0 ## 1.5
    smooth_corners: false
    nn_search_use_route_cost: false
    nn_search_route_cost_margin: 1.0
    # operations: ["AdjustSpeedLimit", "ReroutingService", "CollisionMonitor"]
    operations: ["AdjustSpeedLimit"]
    ReroutingService:
      plugin: "nav2_route::ReroutingService"
    AdjustSpeedLimit:
      plugin: "nav2_route::AdjustSpeedLimit"
    CollisionMonitor:
      plugin: "nav2_route::CollisionMonitor"
      max_collision_dist: 3.0
    # edge_cost_functions: ["DistanceScorer", "CostmapScorer"]
    edge_cost_functions: ["DistanceScorer"]
    DistanceScorer:
      plugin: "nav2_route::DistanceScorer"
    CostmapScorer:
      plugin: "nav2_route::CostmapScorer"

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [1.5, 0.0, 1.2]
    min_velocity: [-1.5, 0.0, -1.2]
    max_accel: [2.0, 0.0, 3.5]   #[3.0, 0.0, 3.5]
    max_decel: [-2.0, 0.0, -3.5] #[-3.0, 0.0, -3.5]
    odom_topic: "/odom"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

collision_monitor:
  ros__parameters:
    base_frame_id: "base_link"
    odom_frame_id: "odom"
    cmd_vel_in_topic: "cmd_vel_smoothed"
    cmd_vel_out_topic: "cmd_vel"
    state_topic: "collision_monitor_state"
    transform_tolerance: 0.5
    source_timeout: 1.0
    base_shift_correction: True
    stop_pub_timeout: 2.0
    # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
    # and robot footprint for "approach" action type.
    polygons: ["PathCorridorStop", "FootprintApproach"]
    PathCorridorStop:
      enabled: False
      type: "path_polygon"
      action_type: "stop"
      path_topic: "/controller_server/transformed_global_plan"
      corridor_width: 2.0
      lookahead_distance: 3.5
      path_timeout: 1.0
      centerline_sample_spacing: 0.1
      max_polygon_points: 100
      min_path_length: 0.2
      near_goal_distance: 2.0  # disable corridor stop within 2.0 m of goal; 0.0 = off (must be < prune_distance)
      min_points: 3
      visualize: True
      polygon_pub_topic: "path_corridor_stop_zone"
      # sources_names: ["scan","pointcloud"]
      sources_names: ["scan"]
    FootprintApproach:
      enabled: False
      type: "polygon"
      action_type: "approach"
      footprint_topic: "local_costmap/published_footprint"
      time_before_collision: 1.8 ## original: 1.2
      simulation_time_step: 0.1
      min_points: 6
      visualize: True
    # observation_sources: ["scan","pointcloud"]
    observation_sources: ["scan"]
    scan:
      type: "scan"
      topic: "/scan"
      min_height: 0.15
      max_height: 2.0
      enabled: True
    pointcloud: 
      type: "pointcloud"
      topic: "/amr_vision/residual_cell_points"
      transport_type: "raw"
      min_height: 0.12
      max_height: 2.0
      min_range: 0.2
      enabled: True