map_server:
  ros__parameters:
    yaml_filename: "/home/yoo/fori_map.yaml"
    use_sim_time: False
amcl:
  ros__parameters:
    use_sim_time: False
    transform_tolerance: 1.0
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    base_frame_id: "base_link"
    global_frame_id: "map"
    odom_frame_id: "camera_init" # FAST-LIO2의 오도메트리 프레임
    scan_topic: "scan"
    map_topic: "map"
    set_initial_pose: true

behavior_server:
  ros__parameters:
    local_frame: base_link
    global_frame: map
    odom_frame: camera_init  # <--- 이 부분이 odom 에러를 해결합니다.
    use_sim_time: False
    device_id: "robot"
    simulate_ahead_time: 2.0

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /aft_mapped_to_init # FAST-LIO2의 위치 토픽

controller_server:
  ros__parameters:
    use_sim_time: False
    controller_frequency: 20.0
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.1
      movement_time_allowance: 30.0
    general_goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.30
      yaw_goal_tolerance: 0.35
    FollowPath:
      plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
      desired_linear_vel: 0.25
      lookahead_dist: 0.6
      min_lookahead_dist: 0.3
      max_lookahead_dist: 0.9
      use_rotate_to_heading: True
      rotate_to_heading_min_angle: 0.20
      max_angular_vel: 0.5
      min_tc_velocity: 0.1
      regulated_linear_scaling_min_speed: 0.15

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: camera_init # 지역 지도는 오도메트리 기준
      robot_base_frame: base_link
      use_sim_time: False
      transform_tolerance: 1.0
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.38
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.15

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map # 전역 지도는 지도 기준
      robot_base_frame: base_link
      use_sim_time: False
      transform_tolerance: 1.0
      robot_radius: 0.38
      resolution: 0.05
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        observation_sources: scan
        scan:
          topic: /scan
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.15

planner_server:
  ros__parameters:
    use_sim_time: False
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.5
      downsample_costmap: false
      allow_unknown: true
      max_iterations: 1000000
      max_planning_time: 2.0
      motion_model_for_search: "2D"
      angle_quantization_binned: 72
      minimum_turning_radius: 0.0
