nav2_params.yaml

amcl:
  ros__parameters:
    use_sim_time: True

    kld_err: 0.05 # Increasing this value allows the number of particles to fluctuate more freely, based on the Kullback-Leibler Distance (KLD). Higher values may be helpful for a large and complex environment.
    kld_z: 0.99 # This is the KLD update factor. It sets the speed at which AMCL updates the number of particles. A lower value means it updates more frequently. This might need to be lowered if your robot moves fast or if the environment changes often.
    alpha1: 0.9 #0.2 # or a bit higher depending on your odometry's noise
    alpha2: 0.9 #0.2
    alpha3: 0.9 #0.2
    alpha4: 0.9 #0.2
    alpha5: 0.9 #0.2
    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: 100.0
    laser_min_range: -1.0
    laser_model_type: "likelihood_field"
    max_beams: 60
    max_particles: 10000 #5000 # more particles might result in more accurate localization but consume more computational resources
    min_particles: 2000 #1000  # adjust based on your testing, might need to be raised if localization fails frequently
    odom_frame_id: "odom"
    pf_err: 0.2 #0.05  # increase the standard deviation for the error model
    pf_z: 0.99 # leave this as is, it's the weight for the most recent update
    recovery_alpha_fast: 0.001 #0.0 # This value determines how fast the slow recovery factor increases. If localization is lost often, you may need to increase this value to allow the filter to recover more quickly.
    recovery_alpha_slow: 0.1 #0.0 # This value is the fast recovery factor. Increase this value if localization is often lost, to allow the filter to recover more quickly.
    resample_interval: 1
    robot_model_type: "omnidirectional" #"differential" #Need to customize later an Ackermman plugin for  this according to the documentation: https://navigation.ros.org/configuration/packages/configuring-amcl.html
    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.25
    z_hit: 0.75 #0.5 # increase this to make the filter trust the measurements more
    z_max: 0.5 #0.05  # increase this to add more randomness to the filter
    z_rand: 0.5
    z_short: 0.05
    scan_topic: scan

amcl_map_client:
  ros__parameters:
    use_sim_time: True

amcl_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    enable_groot_monitoring: True
    groot_zmq_publisher_port: 1666
    groot_zmq_server_port: 1667
    # '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:
    - nav2_compute_path_to_pose_action_bt_node
    - nav2_compute_path_through_poses_action_bt_node
    - nav2_follow_path_action_bt_node
    - nav2_back_up_action_bt_node
    - nav2_spin_action_bt_node
    - nav2_wait_action_bt_node
    - nav2_clear_costmap_service_bt_node
    - nav2_is_stuck_condition_bt_node
    - nav2_goal_reached_condition_bt_node
    - nav2_goal_updated_condition_bt_node
    - nav2_initial_pose_received_condition_bt_node
    - nav2_reinitialize_global_localization_service_bt_node
    - nav2_rate_controller_bt_node
    - nav2_distance_controller_bt_node
    - nav2_speed_controller_bt_node
    - nav2_truncate_path_action_bt_node
    - nav2_goal_updater_node_bt_node
    - nav2_recovery_node_bt_node
    - nav2_pipeline_sequence_bt_node
    - nav2_round_robin_node_bt_node
    - nav2_transform_available_condition_bt_node
    - nav2_time_expired_condition_bt_node
    - nav2_distance_traveled_condition_bt_node
    - nav2_single_trigger_bt_node
    - nav2_is_battery_low_condition_bt_node
    - nav2_navigate_through_poses_action_bt_node
    - nav2_navigate_to_pose_action_bt_node
    - nav2_remove_passed_goals_action_bt_node
    - nav2_planner_selector_bt_node
    - nav2_controller_selector_bt_node
    - nav2_goal_checker_selector_bt_node

bt_navigator_rclcpp_node:
  ros__parameters:
    use_sim_time: True

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0 #5.0 #20.0 # Lowering frequency if you experience lag or latency issues
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugin: "progress_checker"
    goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 1.0 #0.5 # Adjust based on robot size and environment
      movement_time_allowance: 15.0 #10.0 # Adjust based on average time robot needs to move
    # 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.9 #0.25 # This is the tolerance for reaching the goal, in meters. Increase this value if your robot is larger and needs a larger area to consider that it has reached its goal.
      yaw_goal_tolerance: 0.75 #0.25  # This is the tolerance for the orientation at the goal, in radians. Increase this if your robot has a larger acceptable margin of error for its final orientation.
    # DWB parameters
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      debug_trajectory_details: True
      min_vel_x: 0.0 # This is the minimum translational velocity. Increase this if your robot has a higher minimum speed.
      min_vel_y: 0.0 # This is the minimum rotational velocity. Increase this if your robot has a higher minimum rotational speed.
      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
      # Add high threshold velocity for turtlebot 3 issue.
      # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
      acc_lim_x: 2.5  # Adjust based on your robot's actual acceleration limit
      acc_lim_y: 0.0
      acc_lim_theta: 3.2 # Adjust based on your robot's actual angular acceleration limit
      decel_lim_x: -2.5
      decel_lim_y: 0.0
      decel_lim_theta: -3.2
      vx_samples: 20
      vy_samples: 5
      vtheta_samples: 20
      sim_time: 1.7
      linear_granularity: 0.05
      angular_granularity: 0.025
      transform_tolerance: 0.2
      xy_goal_tolerance: 0.9 #0.25 #This is the tolerance for reaching the goal, in meters. Increase this value if your robot is larger and needs a larger area to consider that it has reached its goal.
      trans_stopped_velocity: 0.25
      short_circuit_trajectory_evaluation: True
      stateful: True
      critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
      BaseObstacle.scale: 0.02
      PathAlign.scale: 32.0
      PathAlign.forward_point_distance: 0.1
      GoalAlign.scale: 24.0
      GoalAlign.forward_point_distance: 0.1
      PathDist.scale: 32.0
      GoalDist.scale: 24.0
      RotateToGoal.scale: 32.0
      RotateToGoal.slowing_factor: 5.0
      RotateToGoal.lookahead_time: -1.0

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      static_map: false
      use_sim_time: True
      rolling_window: true
      width: 30
      height: 30
      #origin_x: -5.0
      #origin_y: -5.0
      resolution: 0.05
      footprint: "[ [-4.15, -1.3025], [4.15, -1.3025], [4.15, 1.3025], [-4.15, 1.3025] ]" # Put an offste of 0.1meters each side (corner points)
      #of chassis: Length: 8.2m (total robot length) + 0.1m (margin offset) = 8.3m | Width: 2.505m (total robot width) + 0.1m (margin offset) = 2.605m
      #robot_radius: 4.5 # 4.3411 real value but put a margin
      plugins: ["voxel_layer", "inflation_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      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: 5.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 5.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 12.0
          raytrace_min_range: 0.0
          obstacle_max_range: 12.0
          obstacle_min_range: 0.0
      static_layer:
        map_subscribe_transient_local: True
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

# local_costmap:
#   local_costmap:
#     ros__parameters:
#       update_frequency: 5.0
#       publish_frequency: 2.0
#       global_frame: odom
#       robot_base_frame: base_link
#       use_sim_time: False
#       rolling_window: true
#       width: 30
#       height: 30
#       resolution: 0.05
#       footprint: "[ [-4.15, -1.3025], [4.15, -1.3025], [4.15, 1.3025], [-4.15, 1.3025] ]"
#       #robot_radius: 5.0
#       plugins: ["obstacle_layer", "inflation_layer"]
#       inflation_layer:
#         plugin: "nav2_costmap_2d::InflationLayer"
#         cost_scaling_factor: 3.0
#         inflation_radius: 0.55
#       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"
#       static_layer:
#         map_subscribe_transient_local: True
#       always_send_full_costmap: True
#   local_costmap_client:
#     ros__parameters:
#       use_sim_time: False
#   local_costmap_rclcpp_node:
#     ros__parameters:
#       use_sim_time: False


global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      #width: 50
      #height: 50
      origin_x: -25.0
      origin_y: -25.0
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [-4.15, -1.3025], [4.15, -1.3025], [4.15, 1.3025], [-4.15, 1.3025] ]"
      #robot_radius: 4.5
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 5.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 20.0
          raytrace_min_range: 0.0
          obstacle_max_range: 20.0
          obstacle_min_range: 0.0
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True
  global_costmap_client:
    ros__parameters:
      use_sim_time: True
  global_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

map_server:
  ros__parameters:
    use_sim_time: True
    yaml_filename: "fixed_map.yaml"

map_saver:
  ros__parameters:
    use_sim_time: True
    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: 20.0
    use_sim_time: True
    planner_plugins: ["GridBased"]

    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      downsample_costmap: false
      downsampling_factor: 1
      tolerance: 0.5 #0.25 # Adjust based on how much error you can tolerate in the planned path
      allow_unknown: true
      max_iterations: 1000000
      max_on_approach_iterations: 1000
      max_planning_time: 5.0  # If you find planning takes too long, you might want to lower this
      motion_model_for_search: "DUBIN"
      angle_quantization_bins: 72
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 3.0
      minimum_turning_radius: 1.0 # Adjust based on your robot's actual turning radius
      reverse_penalty: 2.0
      change_penalty: 0.0
      non_straight_penalty: 1.2
      cost_penalty: 2.0
      retrospective_penalty: 0.015
      lookup_table_size: 20.0
      cache_obstacle_heuristic: false
      smooth_path: True # If your robot can handle sharp turns, you might not need path smoothing
      costmap:
        resolution: 0.5
        width: 25 #150
        height: 25 #150
        footprint: "[ [-4.15, -1.3025], [4.15, -1.3025], [4.15, 1.3025], [-4.15, 1.3025] ]"

      smoother:
        max_iterations: 1000
        w_smooth: 0.5 # 0.3 # Adjust based on how much smoothing you want (higher value means more smoothing)
        w_data: 0.5 #0.2 # Adjust based on how closely you want the smoothed path to adhere to the original path
        tolerance: 1.0e-10
        do_refinement: true
        refinement_num: 2


planner_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True

recoveries_server:
  ros__parameters:
    costmap_topic: local_costmap/costmap_raw
    footprint_topic: local_costmap/published_footprint
    cycle_frequency: 5.0 #10.0 # If your robot can safely attempt recoveries less frequently, you can lower this
    recovery_plugins: ["spin", "backup", "wait"]
    spin:
      plugin: "nav2_recoveries/Spin"
    backup:
      plugin: "nav2_recoveries/BackUp"
    wait:
      plugin: "nav2_recoveries/Wait"
    global_frame: odom
    robot_base_frame: base_link
    transform_timeout: 0.1
    use_sim_time: true
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2

robot_state_publisher:
  ros__parameters:
    use_sim_time: True

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