2026-09-08-v2

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /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/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmc_bringup/params/navigate_on_route_graph_stop_and_wait.xml'
    # default_nav_to_pose_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_to_pose_bt_xml: '/home/hyundai-motor/standard_amr_ws/overlay_ws/src/hmgics_ws/hmgics_navigation2/nav2_bt_navigator/behavior_trees/navigate_w_routing_global_planning_and_control_w_recovery.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:
    use_sim_time: False
    controller_frequency: 20.0
    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 ## in meter
      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: 4.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_rotation_shim_controller::RotationShimController"
      static_handoff: true

      # --- shim ---
      angular_dist_threshold: 0.785
      angular_disengage_threshold: 0.3925
      forward_sampling_distance: 0.5
      rotate_to_heading_angular_vel: 0.25  # 0.6 if robot stuck at inplace rotation
      max_angular_accel: 0.35 # 1.0 if robot stuck at inplace rotation
      max_cost_threshold: 254.0
      simulate_ahead_time: 3.0
      rotate_to_goal_heading: true
      rotate_to_heading_once: false
      closed_loop: false
      use_path_orientations: false
      bidirectional: true          # vx_min is -1.0, PathAngleCritic mode is 1
      # [HMGICS]
      goal_rotation_hysteresis: 1.5

      primary_controller: "nav2_mppi_controller::MPPIController"
      # --- mppi ---
      time_steps: 50 # 70 for 1m/s max
      model_dt: 0.05
      model_delay_vx: 0.10
      model_delay_vy: 0.0
      model_delay_wz: 0.10
      batch_size: 1600
      ## acceleration
      ax_max: 3.0
      ax_min: -2.0

      ## linear velocity x
      vx_std: 0.5 #### 0.65
      vx_max: 1.5
      vx_min: -1.5

      ## 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:
        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

        near_goal_velocity_scaling: true
        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

        curvature_velocity_scaling: true
        max_lateral_accel: 0.17
        curvature_decel: 1.0
        curvature_sample_distance: 0.2
        curvature_vx_min: 0.15

      iteration_count: 1
      transform_tolerance: 0.1
      temperature:  0.45
      gamma: 0.015
      # motion_model: "DiffDrive"
      motion_model: "diff_drive"
      diff_drive:
        plugin: "mppi::DiffDriveMotionModel"
      visualize: true
      publish_optimal_trajectory: true
      publish_critics_stats: true
      # [DIAGNOSTIC] Draws the reference window the path-alignment critics can actually see
      # on ~/path_align_window, and adds the numbers to ~/critics_stats. PathAlignCritic
      # builds its reference only out to furthest_reached_path_point and clamps every
      # trajectory sample past it onto that one point, so when the green window is short
      # against the batch's reach the critic stops scoring alignment and starts scoring
      # SPEED -- measured, a 1.0 m window against a 3.85 m reach makes rollouts that are all
      # perfectly on the path spread ~44x more than the cost of being 10 cm off it, and the
      # cheapest trajectory in the batch becomes the slowest one.
      #
      # Watch path_window_length / trajectory_reach_length. Near 1.0 is healthy; the marker
      # label says DEGRADED below 0.8.
      publish_path_window: true # [NOTE] 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
        ]
      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 is given up: this critic scores the MEAN distance over the horizon, so it
        # rewards arriving EARLY, which the terminal critic is indifferent to (measured
        # flat, 0.045 vs 0.050, across every cruise speed that still parks). The cost is
        # well under a second per goal over the last 1.4 m, and creeping is still
        # penalised by the terminal critic (2.12 T worse than parking).
        #
        # 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: false
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
        symmetric_yaw_tolerance: true
      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.4608
        # 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
        # 1.4 m is where PathFollowCritic gates OFF, so this is a single clean handover:
        # one critic owns longitudinal progress above it, one owns parking below it, and
        # exactly one gate fires. Do not lower this while GoalCritic is disabled -- it
        # would leave a band with no goal-seeking term at all.
        threshold_to_consider: 1.4
        # 5 x model_dt 0.05 = the last 0.25 s of the rollout.
        terminal_window: 5
        # 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.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.80
        # 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
        # Match BidirectionalGoalChecker and the shim's selectBidirectionalHeading.
        symmetric_yaw_tolerance: true
      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.4608).
        #
        # Measured raw-unit spread (costs_std / cost_weight) at 1.1 m/s: 0.0382. So
        #   std/T = cost_weight * 0.0382 / 0.4608
        cost_weight: 23.0
        max_path_occupancy_ratio: 0.10
        trajectory_point_step: 4
        threshold_to_consider: 0.15
        # 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.4608.
        # At 3.0 this sat at 0.22, close to the ~0.2 floor below which a critic stops
        # influencing the result at all. 8.0 -> 0.59.
        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
        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.
        #
        # Measured raw-unit spread 0.2069 (5.4x PathAlignCritic's -- which is why its weight
        # is numerically the smallest of the three while still being influential):
        #   std/T = cost_weight * 0.2069 / 0.4608
        cost_weight: 4.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: false
        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: 25 #### 12
        threshold_to_consider: 0.6 #### 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
      #   twirling_cost_power: 1
      #   twirling_cost_weight: 10.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.1 # 0.1
      qr_marker_length: 1.4 # 1.5
      static_wall: 2.5 ##### 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
      # These resolve through the nav2_hmc_mppi_controller critic ClassLoader,
      # which is where CurvatureAwareCritic / PayloadJerkCritic /
      # GoalTargetVelocityCritic live. They do NOT exist in the upstream
      # nav2_mppi_controller shipped by hmgics_navigation2.
      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 ##### 0.3
        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: 15
      height: 15
      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
          topic: /scan_filtered
          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
          topic: /scan_filtered
          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: "/home/hyundai-motor/standard_amr_ws"             # 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:
    use_sim_time: False
    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_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: [3.0, 0.0, 3.5]
    max_decel: [-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"
      topic: /scan_filtered
      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