bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom_custom
    bt_loop_duration: 20
    default_server_timeout: 2000
    wait_for_service_timeout: 4000

    default_nav_to_pose_bt_xml: "navigate_to_pose_w_replanning_and_recovery.xml"
    default_nav_through_poses_bt_xml: "navigate_through_poses_w_replanning_and_recovery.xml"

    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_compute_path_through_poses_action_bt_node
      - nav2_smooth_path_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_assisted_teleop_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_drive_on_heading_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_globally_updated_goal_condition_bt_node
      - nav2_is_path_valid_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_truncate_path_local_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_path_expiring_timer_condition
      - nav2_distance_traveled_condition_bt_node
      - nav2_single_trigger_bt_node
      - nav2_goal_updated_controller_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
      - nav2_controller_cancel_bt_node
      - nav2_path_longer_on_approach_bt_node
      - nav2_wait_cancel_bt_node
      - nav2_spin_cancel_bt_node
      - nav2_back_up_cancel_bt_node
      - nav2_assisted_teleop_cancel_bt_node
      - nav2_drive_on_heading_cancel_bt_node
      - nav2_is_battery_charging_condition_bt_node

bt_navigator_navigate_through_poses_rclcpp_node:
  ros__parameters:
    use_sim_time: True

bt_navigator_navigate_to_pose_rclcpp_node:
  ros__parameters:
    use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      transform_tolerance: 1.0

      update_frequency: 0.5
      publish_frequency: 0.5
      global_frame: map
      robot_base_frame: base_link
      use_sim_time: True
      footprint: "[ [0.35, 0.28], [0.35, -0.28], [-0.35, -0.28], [-0.35, 0.28] ]"
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]

      obstacle_layer:
        min_obstacle_height: 0.1
        max_obstacle_height: 2.5

        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: lidar3d
        lidar3d:
          topic: /lidar_filtered

          min_obstacle_height: 0.1
          max_obstacle_height: 2.5

          clearing: True
          marking: True
          data_type: "PointCloud2"
          obstacle_min_range: 0.0
          obstacle_max_range: 15.0
          raytrace_min_range: 0.0
          raytrace_max_range: 15.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.02
      always_send_full_costmap: True

planner_server:
  ros__parameters:
    expected_planner_frequency: 2.0
    use_sim_time: True
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.5
      allow_unknown: true
      downsample_costmap: true
      downsampling_factor: 2
      max_iterations: 3000000
      max_on_approach_iterations: 5000
      max_planning_time: 10.0
      motion_model_for_search: "DUBIN"
      angle_quantization_bins: 144
      analytic_expansion_ratio: 3.5
      analytic_expansion_max_length: 5.0
      minimum_turning_radius: 1.1
      reverse_penalty: 2.1
      change_penalty: 0.0
      non_straight_penalty: 1.5
      cost_penalty: 5.0
      retrospective_penalty: 0.1
      lookup_table_size: 20.0
      cache_obstacle_heuristic: true
      smooth_path: true

smoother_server:
  ros__parameters:
    use_sim_time: True
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      do_refinement: True

robot_state_publisher:
  ros__parameters:
    use_sim_time: True
    
waypoint_follower:
  ros__parameters:
    use_sim_time: True
    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