amcl:
  ros__parameters:
    # use_sim_time: False
    alpha1: 0.1
    alpha2: 0.1
    alpha3: 0.1
    alpha4: 0.1
    alpha5: 0.1
    base_frame_id: "base_footprint"
    beam_skip_distance: 0.5
    beam_skip_error_threshold: 0.9
    beam_skip_threshold: 0.3
    do_beamskip: false
    global_frame_id: "map"
    lambda_short: 0.1
    laser_likelihood_max_dist: 2.0
    laser_max_range: 10.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 2000
    min_particles: 500
    odom_frame_id: "odom"
    pf_err: 0.05
    pf_z: 0.99
    recovery_alpha_fast: 0.0
    recovery_alpha_slow: 0.0
    resample_interval: 1
    robot_model_type: "nav2_amcl::OmniMotionModel"
    save_pose_rate: 0.5
    sigma_hit: 0.2
    tf_broadcast: true
    transform_tolerance: 1.0
    update_min_a: 0.2
    update_min_d: 0.1
    z_hit: 0.5
    z_rand: 0.5
    z_max: 0.0
    z_short: 0.0
    scan_topic: scan
    set_initial_pose: true
    initial_pose:
      x: 0.0
      y: 0.0
      yaw: 0.0   

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    transform_tolerance: 0.5
    filter_duration: 0.3
    default_nav_to_pose_bt_xml: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml" # behavior trees location.
    default_nav_through_poses_bt_xml: "$(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml"
    always_reload_bt_xml: false
    goal_blackboard_id: goal
    goals_blackboard_id: goals
    path_blackboard_id: path
    navigators: ['navigate_to_pose', 'navigate_through_poses']
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator"
    error_code_name_prefixes:
      - assisted_teleop
      - backup
      - compute_path
      - dock_robot
      - drive_on_heading
      - follow_path
      - nav_thru_poses
      - nav_to_pose
      - spin
      - route
      - undock_robot
      - wait

docking_server:
  ros__parameters:
    dock_plugins: ['nova_carter_dock']
    nova_carter_dock:
      plugin: 'opennav_docking::SimpleChargingDock'
    docks: ['home_dock','flex_dock1', 'flex_dock2']
    home_dock:
      type: 'nova_carter_dock'
      frame: map
      pose: [0.0, 0.0, 0.0]
    flex_dock1:
      type: 'nova_carter_dock'
      frame: map
      pose: [10.0, 10.0, 0.0]
    flex_dock2:
      type: 'nova_carter_dock'
      frame: map
      pose: [30.0, 30.0, 0.0]
    enable_stamped_cmd_vel: false

controller_server:
  ros__parameters:
    controller_frequency: 40.0
    costmap_update_timeout: 0.30
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]
    use_realtime_priority: false

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    # Goal checker parameters
    #precise_goal_checker:
    #  plugin: "nav2_controller::SimpleGoalChecker"
    #  xy_goal_tolerance: 0.25
    #  yaw_goal_tolerance: 0.25
    #  stateful: True
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.25
      yaw_goal_tolerance: 0.25
    FollowPath:
      plugin: "nav2_frenet_ilqr_controller::FrenetILQRController"
      time_discretization: 0.05
      frenet_trajectory_planner:
        min_lateral_distance: -1.0
        max_lateral_distance: 1.0
        step_lateral_distance: 0.5
        min_longtitutal_velocity: 0.0
        max_longtitutal_velocity: 2.0
        step_longtitutal_velocity: 0.1
        time_interval: 1.0
        max_state_in_trajectory: 40
      ilqr_trajectory_tracker:
        iteration_number: 20
        alpha: 1.0
        input_limits_min: [-0.5, -0.5, -1.5]
        input_limits_max: [0.5, 0.5, 1.5]
      cost_checker_plugins: ["LateralDistanceCostChecker", "LongtitutalVelocityCostChecker"]
      LateralDistanceCostChecker:
        plugin: "nav2_frenet_ilqr_controller::costs::LateralDistanceCost"
        K_lateral_distance: 10.0
      LongtitutalVelocityCostChecker:
        plugin: "nav2_frenet_ilqr_controller::costs::LongtitutalVelocityCost"
        K_longtitutal_velocity: 5.0
        desired_velocity: 0.5

      policy_plugins: ["ObstaclePolicy"]
      ObstaclePolicy:
        plugin: "nav2_frenet_ilqr_controller::policies::ObstaclePolicy"
    enable_stamped_cmd_vel: false

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 2
      height: 2
      resolution: 0.05
      robot_radius: 0.1
      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.3
        cost_scaling_factor: 2.0
      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"
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        publish_voxel_map: true
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: true
      always_send_full_costmap: true

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      transform_tolerance: 0.5
      robot_radius: 0.1
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "voxel_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"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: true
        publish_voxel_map: true
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: true
          marking: true
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: true
        transform_tolerance: 0.1
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.3
        cost_scaling_factor: 5.0
      always_send_full_costmap: true

map_server:
  ros__parameters:
    yaml_filename: "map.yaml"

map_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: true

planner_server:
  ros__parameters:
    expected_planner_frequency: 10.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_navfn_planner::NavfnPlanner"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

behavior_server:
  ros__parameters:
    local_costmap_topic: local_costmap/costmap_raw
    local_footprint_topic: local_costmap/published_footprint
    global_costmap_topic: global_costmap/costmap_raw
    global_footprint_topic: global_costmap/published_footprint
    cycle_frequency: 10.0
    behavior_plugins: ["spin", "backup", "drive_on_heading", "wait", "assisted_teleop"]
    spin:
      plugin: "nav2_behaviors::Spin" # In Iron and older versions, "/" was used instead of "::"
    backup:
      plugin: "nav2_behaviors::BackUp" # In Iron and older versions, "/" was used instead of "::"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading" # In Iron and older versions, "/" was used instead of "::"
    wait:
      plugin: "nav2_behaviors::Wait" # In Iron and older versions, "/" was used instead of "::"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop" # In Iron and older versions, "/" was used instead of "::"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_link
    transform_timeout: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2
    enable_stamped_cmd_vel: false

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

collision_monitor:
  ros__parameters:
    base_frame_id: "base_footprint"
    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: 5.0
    base_shift_correction: true
    stop_pub_timeout: 2.0
    enable_stamped_cmd_vel: false
    use_realtime_priority: false
    polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"]
    PolygonStop:
      type: "circle"
      radius: 0.1
      action_type: "stop"
      min_points: 4
      visualize: true
      polygon_pub_topic: "polygon_stop"
      enabled: true
    PolygonSlow:
      type: "polygon"
      points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]"
      action_type: "slowdown"
      min_points: 4
      slowdown_ratio: 0.3
      visualize: true
      polygon_pub_topic: "polygon_slowdown"
      enabled: true
    PolygonLimit:
      type: "polygon"
      points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]"
      action_type: "limit"
      min_points: 4
      linear_limit: 0.4
      angular_limit: 0.5
      visualize: true
      polygon_pub_topic: "polygon_limit"
      enabled: true
    FootprintApproach:
      type: "polygon"
      action_type: "approach"
      footprint_topic: "/local_costmap/published_footprint"
      time_before_collision: 2.0
      simulation_time_step: 0.02
      min_points: 6
      visualize: False
      enabled: true
    VelocityPolygonStop:
      type: "velocity_polygon"
      action_type: "stop"
      min_points: 6
      visualize: true
      enabled: False
      polygon_pub_topic: "velocity_polygon_stop"
      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
      holonomic: false
      rotation:
        points: "[[0.3, 0.3], [0.3, -0.3], [-0.3, -0.3], [-0.3, 0.3]]"
        linear_min: 0.0
        linear_max: 0.05
        theta_min: -1.0
        theta_max: 1.0
      translation_forward:
        points: "[[0.35, 0.3], [0.35, -0.3], [-0.2, -0.3], [-0.2, 0.3]]"
        linear_min: 0.0
        linear_max: 1.0
        theta_min: -1.0
        theta_max: 1.0
      translation_backward:
        points: "[[0.2, 0.3], [0.2, -0.3], [-0.35, -0.3], [-0.35, 0.3]]"
        linear_min: -1.0
        linear_max: 0.0
        theta_min: -1.0
        theta_max: 1.0
      stopped:
        points: "[[0.25, 0.25], [0.25, -0.25], [-0.25, -0.25], [-0.25, 0.25]]"
        linear_min: -1.0
        linear_max: 1.0
        theta_min: -1.0
        theta_max: 1.0
    observation_sources: ["scan"]
    scan:
      source_timeout: 0.3
      type: "scan"
      topic: "/scan"
      enabled: true

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: false
    feedback: "OPEN_LOOP"
    max_velocity: [0.5, 0.5, 2.5]
    min_velocity: [-0.5, -0.5, -2.5]
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0
    max_accel: [2.5, 2.5, 3.2]
    max_decel: [-2.5, -2.5, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    use_realtime_priority: false
    enable_stamped_cmd_vel: false