local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 4.0
      publish_frequency: 1.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.03
      footprint: "[[-0.275, -0.33], [-0.275, 0.33], [0.275, 0.33], [0.275, -0.33]]"
      #robot_radius: 0.22
      plugins: ["obstacle_layer", "inflation_layer"]
      filters: ["keepout_filter"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: False
        filter_info_topic: "keepout_costmap_filter_info"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.80
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: <obstacle_scan_topic>
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          inf_is_valid: 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
      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: <robot_prefix>base_link
      #robot_radius: 0.22
      footprint: "[[-0.275, -0.33], [-0.275, 0.33], [0.275, 0.33], [0.275, -0.33]]"
      resolution: 0.03
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer", "speed_filter"]
      filters: ["keepout_filter"]
      keepout_filter:
        plugin: "nav2_costmap_2d::KeepoutFilter"
        enabled: False
        filter_info_topic: "keepout_costmap_filter_info"
      speed_filter:
        plugin: "nav2_costmap_2d::SpeedFilter"
        enabled: <speed_enabled>
        filter_info_topic: "speed_costmap_filter_info"
        speed_limit_topic: <speed_limit_topic>
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: <obstacle_scan_topic>
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          inf_is_valid: 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
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.80
      always_send_full_costmap: True
      service_introspection_mode: "disabled"

planner_server:
  ros__parameters:
    expected_planner_frequency: 25.0
    planner_plugins: ["GridBased"]
    costmap_update_timeout: 1.0
    service_introspection_mode: "disabled" 
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerHybrid" # In Iron and older versions, "/" was used instead of "::"
      downsample_costmap: False           # whether or not to downsample the map
      downsampling_factor: 2              # 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: False                 # 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: 60         # 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: 1.5  # 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.5        # minimum turning radius in m of path / vehicle
      reverse_penalty: 10.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.25                 # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.3           # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 3.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: 3.0             # Size of the dubin/reeds-sheep distance window to cache, in meters.
      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: True         # 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: False
      allow_primitive_interpolation: True
      coarse_search_resolution: 2         # 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: False                   # 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

smoother_server:
  ros__parameters:
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      refinement_num: 2
      # True for Hybrid A* or State Lattice to not smooth over directional changes
      enforce_path_inversion: True
      do_refinement: True