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: 50.0
    behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX"]
    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"
    rotate:
      plugin: "toid::SimpleRotateBehavior"
      max_angular_accel: 4.0
      max_angular_decel: 1.0
    translateX:
      plugin: "toid::SimpleTranslateXBehavior"
    local_frame: map
    global_frame: map
    robot_base_frame: base_footprint
    enable_stamped_cmd_vel: true
    transform_tolerance: 0.1
    simulate_ahead_time: 0.2
    max_rotational_vel: 1.0
    min_rotational_vel: 0.1
    rotational_acc_lim: 3.2

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_footprint
      footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
      footprint_padding: 0.02
      track_unknown_space: false
      rolling_window: false
      plugins: ["static_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        footprint_clearing_enabled: true
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        footprint_clearing_enabled: true
        cost_scaling_factor: 6.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.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
      footprint_padding: 0.01
      #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"
        footprint_clearing_enabled: true
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.40

planner_server:
  ros__parameters:
    allow_partial_planning: false
    expected_planner_frequency: 0.02
    costmap_update_timeout: 1.0
    introspection_mode: "disabled"
    planner_plugins: ['GridBased']
    GridBased:
      plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::"
      allow_unknown: true                 # Allow traveling in unknown space
      tolerance: 0.05                     # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found.
      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
      analytic_expansion_ratio: 3.5       # The ratio to attempt analytic expansions during search for final approach.
      analytic_expansion_max_length: 0.1  # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting
      analytic_expansion_max_cost: 50.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)
      downsample_obstacle_heuristic: true   # Downsample the obstacle map dynamic programming distance expansion heuristic to speed up search at the cost of some path quality
      use_quadratic_cost_penalty: false   # Use quadratic cost penalty for traversal and heuristic cost computations rather than linear
      reverse_penalty: 1.0                # Penalty to apply if motion is reversing, must be => 1
      change_penalty: 0.00                # Penalty to apply if motion is changing directions (L to R), must be >= 0
      non_straight_penalty: 1.2          # Penalty to apply if motion is non-straight, must be => 1
      cost_penalty: 2.5                   # 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.
      rotation_penalty: 4.0               # Penalty to apply to in-place rotations, if minimum control set contains them
      retrospective_penalty: 0.015
      lookup_table_size: 1.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.
      allow_reverse_expansion: true     # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse).
      coarse_search_resolution: 1         # 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: True                   # 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


controller_server:
  ros__parameters:
    controller_frequency: 100.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.04
      yaw_goal_tolerance: 0.10
      stateful: True
    FollowPath:
      plugin: "toid::SpinnerController"
      primary_controller:
        plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
        desired_linear_vel: 0.50
        lookahead_dist: 0.1
        min_lookahead_dist: 0.08
        max_lookahead_dist: 0.15
        lookahead_time: 0.2
        rotate_to_heading_angular_vel: 0.8
        use_velocity_scaled_lookahead_dist: true
        min_approach_linear_velocity: 0.1
        approach_velocity_scaling_dist: 0.6
        use_collision_detection: true
        max_allowed_time_to_collision_up_to_carrot: 0.3
        use_regulated_linear_velocity_scaling: true
        use_fixed_curvature_lookahead: false
        curvature_lookahead_dist: 0.1
        use_cost_regulated_linear_velocity_scaling: true
        cost_scaling_dist: 0.1
        cost_scaling_gain: 1.5
        regulated_linear_scaling_min_radius: 0.9
        regulated_linear_scaling_min_speed: 0.25
        use_rotate_to_heading: false
        allow_reversing: true
        max_angular_accel: 3.2
        min_distance_to_obstacle: 0.0
        max_robot_pose_search_dist: 0.5
        use_cancel_deceleration: true
        stateful: true

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