bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_footprint
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    wait_for_service_timeout: 1000
    action_server_result_timeout: 900.0
    navigators: ["navigate_to_pose"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator::NavigateToPoseNavigator"
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code


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"
    drive_on_heading:
      plugin: "nav2_behaviors::DriveOnHeading"
    wait:
      plugin: "nav2_behaviors::Wait"
    assisted_teleop:
      plugin: "nav2_behaviors::AssistedTeleop"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_footprint
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.1
    rotational_acc_lim: 3.2

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
      robot_radius: 0.18
      track_unknown_space: false
      rolling_window: false
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        footprint_clearing_enabled: false
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        footprint_clearing_enabled: false
        cost_scaling_factor: 12.0
        inflation_radius: 0.40
      always_send_full_costmap: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      footprint: "[[-0.13, 0.16], [0.13, 0.16], [0.13, -0.16], [-0.13, -0.16]]"
      footprint_padding: 0.02
      #robot_radius: 0.18
      global_frame: map
      robot_base_frame: base_footprint
      rolling_window: true
      width: 1
      height: 1
      resolution: 0.01
      introspection_mode: "disabled"
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 10.0
        inflation_radius: 0.45

planner_server:
  ros__parameters:
    allow_partial_planning: false
    expected_planner_frequency: 20.0
    costmap_update_timeout: 1.0
    introspection_mode: "disabled"
    planner_plugins: ['GridBased']
    GridBased:
      plugin: 'nav2_navfn_planner::NavfnPlanner'

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    costmap_update_timeout: 0.3
    failure_tolerance: 0.3
    odom_topic: "odom"
    odom_duration: 0.3
    progress_checker_plugins: ["progress_checker"] 
    goal_checker_plugins: ["goal_checker"]
    controller_plugins: ["FollowPath"]
    enable_stamped_cmd_vel: True
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.05
      movement_time_allowance: 5.0
    goal_checker:
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.10
      yaw_goal_tolerance: 0.10
      stateful: True
    FollowPath:
#      plugin: "dwb_core::DWBLocalPlanner"
#      debug_trajectory_details: True
#      min_vel_x: 0.0
#      min_vel_y: 0.0
#      max_vel_x: 0.26
#      max_vel_y: 0.0
#      max_vel_theta: 1.0
#      min_speed_xy: 0.0
#      max_speed_xy: 0.26
#      min_speed_theta: 0.0
#      acc_lim_x: 2.5
#      acc_lim_y: 0.0
#      acc_lim_theta: 3.2
#      decel_lim_x: -2.5
#      decel_lim_y: 0.0
#      decel_lim_theta: -3.2
#      vx_samples: 20
#      vtheta_samples: 20
#      sim_time: 1.7
#      linear_granularity: 0.05
#      angular_granularity: 0.025
#      transform_tolerance: 0.2
#      xy_goal_tolerance: 0.10
#      trans_stopped_velocity: 0.25
#      short_circuit_trajectory_evaluation: True
#      limit_vel_cmd_in_traj: False
#      stateful: True
#      critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
#      ObstacleFootprint.scale: 1.00
#      PathAlign.scale: 32.0
#      GoalAlign.scale: 24.0
#      PathAlign.forward_point_distance: 0.05
#      GoalAlign.forward_point_distance: 0.05
#      PathDist.scale: 32.0
#      GoalDist.scale: 24.0
#      RotateToGoal.scale: 32.0
#      RotateToGoal.slowing_factor: 5.0
#      RotateToGoal.lookahead_time: -1.0
      plugin: "nav2_mppi_controller::MPPIController"
      time_steps: 56
      model_dt: 0.05
      batch_size: 2000
      ax_max: 3.0
      ax_min: -3.0
      ay_max: 3.0
      ay_min: -3.0
      az_max: 3.5
      vx_std: 0.2
      vy_std: 0.2
      wz_std: 0.4
      vx_max: 0.5
      vx_min: -0.35
      vy_max: 0.5
      wz_max: 1.9
      iteration_count: 1
      prune_distance: 1.7
      transform_tolerance: 0.1
      temperature: 0.3
      gamma: 0.015
      motion_model: "DiffDrive"
      visualize: true
      publish_optimal_trajectory: true
      regenerate_noises: true
      TrajectoryVisualizer:
        trajectory_step: 5
        time_step: 3
      TrajectoryValidator:
        # The validator can be configured with additional parameters if needed.
        plugin: "mppi::DefaultOptimalTrajectoryValidator"
        collision_lookahead_time: 1.0  # Time to look ahead to check for collisions
        consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
      AckermannConstraints:
        min_turning_r: 0.2
      critics:
        [
          "ConstraintCritic",
          "CostCritic",
          "GoalCritic",
          "GoalAngleCritic",
          "PathAlignCritic",
          "PathFollowCritic",
          "PathAngleCritic",
          "PreferForwardCritic",
        ]
      ConstraintCritic:
        enabled: true
        cost_power: 1
        cost_weight: 4.0
      GoalCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 1.4
      GoalAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.0
        threshold_to_consider: 0.5
      PreferForwardCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        threshold_to_consider: 0.5
      CostCritic:
        enabled: true
        cost_power: 1
        cost_weight: 3.81
        near_collision_cost: 253
        critical_cost: 300.0
        consider_footprint: true
        collision_cost: 1000000.0
        near_goal_distance: 1.0
        trajectory_point_step: 2
      PathAlignCritic:
        enabled: true
        cost_power: 1
        cost_weight: 14.0
        max_path_occupancy_ratio: 0.05
        trajectory_point_step: 4
        threshold_to_consider: 0.5
        offset_from_furthest: 20
        use_path_orientations: false
      PathFollowCritic:
        enabled: true
        cost_power: 1
        cost_weight: 5.0
        offset_from_furthest: 5
        threshold_to_consider: 1.4
      PathAngleCritic:
        enabled: true
        cost_power: 1
        cost_weight: 2.0
        offset_from_furthest: 4
        threshold_to_consider: 0.5
        max_angle_to_furthest: 1.0
        mode: 0

lifecycle_manager:
  ros__parameters:
    autostart: true
    node_names: ['controller_server','planner_server','behavior_server','map_server','bt_navigator' ]


controller_manager:
  ros__parameters:
    update_rate: 20  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

diffbot_base_controller:
  ros__parameters:
    type: diff_drive_controller/DiffDriveController

    left_wheel_names: ["drivewhl_l_joint"]
    right_wheel_names: ["drivewhl_r_joint"]

    wheel_separation: 0.29

    wheel_radius: 0.04
    use_stamped_vel: false

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_footprint

    open_loop: true
    enable_odom_tf: true
    publish_limited_velocity: true

    cmd_vel_timeout: 1.0