nav2_params.yaml

livox_ros_driver2:
  ros__parameters:
    xfer_format: 4                                      # 0-PointCloud2Msg(PointXYZRTL), 1-LivoxCustomMsg, 2-PclPxyziMsg, 3-LivoxImuMsg, 4-AllMsg
    multi_topic: 0                                      # 0-All LiDARs share the same topic, 1-One LiDAR one topic
    data_src: 0                                         # 0-lidar, others-Invalid data src
    publish_freq: 20.0                                  # frequency of publish, 5.0, 10.0, 20.0, 50.0, etc.
    output_data_type: 0
    frame_id: front_mid360
    user_config_path: $(find-pkg-share pb2025_nav_bringup)/config/reality/mid360_user_config.json
    cmdline_input_bd_code: livox0000000001
    lvx_file_path: ""

point_lio:
  ros__parameters:
    use_imu_as_input: False                             # Change to True to use IMU as input of Point-LIO
    prop_at_freq_of_imu: True
    check_satu: True
    init_map_size: 10
    point_filter_num: 8                                 # Options: 4, 3
    space_down_sample: True
    filter_size_surf: 0.2                               # Options: 0.5, 0.3, 0.2, 0.15, 0.1
    filter_size_map: 0.2                                # Options: 0.5, 0.3, 0.15, 0.1
    ivox_nearby_type: 18                                # Options: 0, 6, 18, 26
    runtime_pos_log_enable: False                       # Option: True

    common:
      lid_topic: "livox/lidar"
      imu_topic: "livox/imu"
      con_frame: False                                  # True: if you need to combine several LiDAR frames into one
      con_frame_num: 1                                  # the number of frames combined
      cut_frame: False                                  # True: if you need to cut one LiDAR frame into several subframes
      cut_frame_time_interval: 0.05                     # should be integral fraction of 1 / LiDAR frequency
      time_diff_lidar_to_imu: 0.0                       # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)

    prior_pcd:
      enable: False
      # NOTE: `prior_pcd_map_path` will be provided in the launch file
      # prior_pcd_map_path: ""
      init_pose: [ 0.0, 0.0, 0.0 ]

    preprocess:
      lidar_type: 1                                     # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR
      scan_line: 4
      timestamp_unit: 3                                 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
      blind: 0.5

    mapping:
      imu_en: True
      extrinsic_est_en: False                           # for aggressive motion, set this variable False
      imu_time_inte: 0.005                              # = 1 / frequency of IMU
      lidar_time_inte: 0.1
      satu_acc: 4.0                                     # the saturation value of IMU's acceleration. not related to the units
      satu_gyro: 35.0                                   # the saturation value of IMU's angular velocity. not related to the units
      acc_norm: 1.0                                     # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
      lidar_meas_cov: 0.01                              # 0.001
      acc_cov_output: 500.0
      gyr_cov_output: 1000.0
      b_acc_cov: 0.0001
      b_gyr_cov: 0.0001
      imu_meas_acc_cov: 0.1
      imu_meas_omg_cov: 0.1
      gyr_cov_input: 0.1                                # for IMU as input model
      acc_cov_input: 0.1                                # for IMU as input model
      plane_thr: 0.1                                    # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
      match_s: 81.0
      ivox_grid_resolution: 0.5
      gravity: [0.0, 0.0, -9.8]                         # gravity to be aligned
      gravity_init: [0.0, 0.0, -9.8]                    # preknown gravity in the first IMU body frame, use when imu_en is False or start from a non-stationary state
      extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
      extrinsic_R: [ 1.0, 0.0, 0.0,
                     0.0, 1.0, 0.0,
                     0.0, 0.0, 1.0 ]

    odometry:
      publish_odometry_without_downsample: True

    publish:
      path_en: False                                    # False: close the path output
      scan_publish_en: True                             # False: close all the point cloud output
      scan_bodyframe_pub_en: False                      # True: output the point cloud scans in IMU-body-frame
      tf_send_en: False                                 # True: send transform from 'camera_init' to 'aft_mapped'

    pcd_save:
      pcd_save_en: False
      interval: -1                                      # how many LiDAR frames saved in each pcd file;
                                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

loam_interface:
  ros__parameters:
    use_sim_time: false
    state_estimation_topic: "aft_mapped_to_init"
    registered_scan_topic: "cloud_registered"
    odom_frame: "odom"
    base_frame: "base_footprint"
    lidar_frame: "front_mid360"

sensor_scan_generation:
  ros__parameters:
    use_sim_time: false
    lidar_frame: "front_mid360"
    base_frame: "base_footprint"
    robot_base_frame: "gimbal_yaw"

terrain_analysis:
  ros__parameters:
    sensor_frame: "front_mid360"
    scan_voxel_size: 0.05               # 点云下采样
    decay_time: 0.5                     # 点云时间差阈值 大于则不会被处理
    no_decay_dis: 0.0                   # 点云距离阈值 小于该阈值不考虑时间差
    clearing_dis: 0.0                   # 该距离外的点会被清除
    use_sorting: True                   # 这个参数开起来就能上坡了；如果 use_sorting,那么就以周围点中的分位点作为地面点，否则以最低点作为地面点；周围其余点相对于地面点的高度作为通过代价，所以对于坡面，有的点的代价是正，有的是负，就相互抵消了；不要与 consider_drop 同时开启，计算上会有冲突
    quantile_z: 0.2                     # useful if use_sorting is enabled.
    consider_drop: False                # 考虑凹下去的地面,开启则将相对于地面点的高度取绝对值
    limit_ground_lift: False            # useful if use_sorting is enabled
    max_ground_lift: 0.3                # useful if use_sorting is enabled
    clear_dy_obs: True                  # 清除动态物体的话，有动态物体走过的地方就走不了了
    min_dy_obs_dis: 0.3                 # 以下参数都是在 clear_dy_obs 的情况下才有用的
    min_dy_obs_angle: 0.0
    min_dy_obs_rel_z: -0.3
    abs_dy_obs_rel_z_thre: 0.2
    min_dy_obs_vfov: -28.0
    max_dy_obs_vfov: 33.0
    min_dy_obs_point_num: 1
    no_data_obstacle: False             # treat no data as obstacle
    no_data_block_skip_num: 0
    min_block_point_num: 10
    vehicle_height: 0.5                 # 小于车辆高度的点才会进行处理
    voxel_point_update_thre: 100
    voxel_time_update_thre: 1.0
    min_rel_z: -1.5                     # min_rel_z 以及 max_rel_z 限制了有效点云点的Z值范围，用于对天花板和地板进行处理
    max_rel_z: 0.5
    dis_ratio_z: 0.2                    # Z 值的最大最小范围由这两个值决定，但是并不等于这两个值，是有一个比例关系的，距离机器人越远，限定范围就越大，这其中考虑了坡度

fake_vel_transform:
  ros__parameters:
    use_sim_time: false
    odom_topic: "odometry"
    robot_base_frame: "gimbal_yaw"
    fake_robot_base_frame: "gimbal_yaw_fake"
    input_cmd_vel_topic: "cmd_vel_nav2_result"
    output_cmd_vel_topic: "cmd_vel"
    cmd_spin_topic: "cmd_spin"
    init_spin_speed: 3.14

small_gicp_relocalization:
  ros__parameters:
    use_sim_time: false
    num_threads: 4
    num_neighbors: 20
    global_leaf_size: 0.15
    registered_leaf_size: 0.05
    max_dist_sq: 3.0
    map_frame: "map"
    odom_frame: "odom"
    base_frame: "base_footprint"
    robot_base_frame: "gimbal_yaw"
    lidar_frame: "front_mid360"
    # The prior_pcd_file does not need to be specified since it going to be set by defaults in launch.
    # prior_pcd_file: ""

pointcloud_to_laserscan:
  ros__parameters:
    use_sim_time: false
    target_frame: base_footprint
    min_height: -0.5
    max_height: 6.0
    min_intensity: 0.1
    max_intensity: 2.0
    angle_min: -3.1415926                   # -M_PI/2
    angle_max: 3.1415926                    # M_PI/2
    angle_increment: 0.00872665             # M_PI/360.0
    scan_time: 0.05
    range_min: 0.3
    range_max: 10.0
    use_inf: true

slam_toolbox:
  ros__parameters:
    # https://github.com/SteveMacenski/slam_toolbox/tree/humble?#configuration
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: obstacle_scan
    use_map_saver: true
    mode: mapping

    # if you'd like to immediately start continuing a map at a given pose
    # or at the dock, but they are mutually exclusive, if pose is given
    # will use pose
    # map_file_name: test_steve
    # map_start_pose: [0.0, 0.0, 0.0]
    # map_start_at_dock: true

    debug_logging: false
    throttle_scans: 1
    scan_queue_size: 10
    transform_publish_period: 0.0                # if 0 never publishes odometry
    map_update_interval: 1.0
    resolution: 0.05
    min_laser_range: 0.3                         # for rastering images
    max_laser_range: 10.0                         # for rastering images
    minimum_time_interval: 0.5
    transform_timeout: 0.2
    tf_buffer_duration: 10.
    stack_size_to_use: 40000000                   # program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: false
    use_scan_barycenter: true
    minimum_travel_distance: 0.5
    minimum_travel_heading: 0.3
    scan_buffer_size: 10
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: false
    loop_match_minimum_chain_size: 10
    loop_match_maximum_variance_coarse: 3.0
    loop_match_minimum_response_coarse: 0.35
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    distance_variance_penalty: 5.5
    angle_variance_penalty: 5.0

    fine_search_angle_offset: 0.00349
    coarse_search_angle_offset: 0.349
    coarse_angle_resolution: 0.0349
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

bt_navigator:
  ros__parameters:
    use_sim_time: false
    global_frame: map
    robot_base_frame: gimbal_yaw_fake
    odom_topic: odometry
    bt_loop_duration: 10
    default_server_timeout: 100
    wait_for_service_timeout: 1000
    default_nav_to_pose_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
    default_nav_through_poses_bt_xml: $(find-pkg-share pb2025_nav_bringup)/behavior_trees/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

controller_server:
  ros__parameters:
    use_sim_time: false
    odom_topic: odometry
    controller_frequency: 20.0
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.001
    failure_tolerance: 0.3
    progress_checker_plugins: ["progress_checker"]
    goal_checker_plugins: ["general_goal_checker"]
    controller_plugins: ["FollowPath"]
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 0.5
      movement_time_allowance: 10.0
    general_goal_checker:
      stateful: True
      plugin: "nav2_controller::SimpleGoalChecker"
      xy_goal_tolerance: 0.15
      yaw_goal_tolerance: 6.28
    FollowPath:
      plugin: "pb_omni_pid_pursuit_controller::OmniPidPursuitController"
      translation_kp: 3.0
      translation_ki: 0.1
      translation_kd: 0.3
      enable_rotation: false
      rotation_kp: 3.0
      rotation_ki: 0.1
      rotation_kd: 0.3
      transform_tolerance: 0.1
      min_max_sum_error: 1.0
      lookahead_dist: 2.0
      use_velocity_scaled_lookahead_dist: true
      lookahead_time: 1.0
      min_lookahead_dist: 0.5
      max_lookahead_dist: 1.0
      use_interpolation: false
      use_rotate_to_heading: false
      use_rotate_to_heading_treshold: 0.1
      min_approach_linear_velocity: 0.5
      approach_velocity_scaling_dist: 1.0
      v_linear_min: -2.5
      v_linear_max: 2.5
      v_angular_min: -3.0
      v_angular_max: 3.0
      curvature_min: 0.4                                # 低曲率阈值，低于此值不减速。
      curvature_max: 0.7                                # 高曲率阈值，高于此值大幅减速。
      reduction_ratio_at_high_curvature: 0.5            # 高曲率时的速度降低比例。 0.5 (即减速 50%)
      curvature_forward_dist: 0.7                       # 前向距离，用于曲率计算
      curvature_backward_dist: 0.3                      # 后向距离，用于曲率计算
      max_velocity_scaling_factor_rate: 0.9             # 速度缩放因子的最大变化率

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: true
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: gimbal_yaw_fake
      rolling_window: true
      width: 5
      height: 5
      resolution: 0.05
      robot_radius: 0.2
      plugins: ["static_layer", "intensity_voxel_layer", "inflation_layer"]
      intensity_voxel_layer:
        plugin: pb_nav2_costmap_2d::IntensityVoxelLayer
        enabled: true
        track_unknown_space: true
        footprint_clearing_enabled: true
        publish_voxel_map: false
        combination_method: 1
        mark_threshold: 0
        origin_z: 0.0
        unknown_threshold: 5
        z_resolution: 0.05
        z_voxels: 16
        min_obstacle_height: 0.0
        max_obstacle_height: 2.0
        min_obstacle_intensity: 0.1
        max_obstacle_intensity: 2.0
        observation_sources: terrain_map
        terrain_map:
          data_type: PointCloud2
          # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
          # It doesn't need to start with '/'
          topic: <robot_namespace>/terrain_map
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          obstacle_max_range: 5.0
          obstacle_min_range: 0.2
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.0
        inflation_radius: 0.7
      always_send_full_costmap: False

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: true
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: map
      robot_base_frame: gimbal_yaw_fake
      robot_radius: 0.2
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "intensity_voxel_layer", "inflation_layer"]
      intensity_voxel_layer:
        plugin: pb_nav2_costmap_2d::IntensityVoxelLayer
        enabled: true
        track_unknown_space: true
        footprint_clearing_enabled: true
        publish_voxel_map: false
        combination_method: 1
        mark_threshold: 0
        origin_z: 0.0
        unknown_threshold: 5
        z_resolution: 0.05
        z_voxels: 16
        min_obstacle_height: 0.0
        max_obstacle_height: 2.0
        min_obstacle_intensity: 0.1
        max_obstacle_intensity: 2.0
        observation_sources: terrain_map
        terrain_map:
          data_type: PointCloud2
          # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
          # It doesn't need to start with '/'
          topic: <robot_namespace>/terrain_map
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          obstacle_max_range: 10.0
          obstacle_min_range: 0.2
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.0
        inflation_radius: 0.7
      always_send_full_costmap: False

# The yaml_filename does not need to be specified since it going to be set by defaults in launch.
# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py
# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used.
map_server:
  ros__parameters:
    yaml_filename: ""

map_saver:
  ros__parameters:
    use_sim_time: false
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

planner_server:
  ros__parameters:
    use_sim_time: false
    expected_planner_frequency: 10.0
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"
      tolerance: 0.5                      # 如果无法到达精确位置的规划容差，单位：米
      allow_unknown: true                 # 允许在未知空间中行驶
      downsample_costmap: false           # 是否对地图进行下采样
      downsampling_factor: 1              # 代价地图层分辨率的倍数 (e.g. 2 on a 5cm costmap would be 10cm)
      max_iterations: 1000000             # 搜索的最大总迭代次数（如果无法到达），设置为-1以禁用
      max_on_approach_iterations: 1000    # 一旦在容差范围内，尝试到达目标的最大迭代次数
      max_planning_time: 3.5              # 规划、平滑和上采样的最大时间（秒）。将根据规划后的剩余时间缩放最大平滑和上采样时间。
      cost_travel_multiplier: 2.0         # For 2D: 应用于搜索的代价乘数，以避开高代价区域。较大的值将更精确地放置在通道的中心（如果存在非“FREE”代价势场），但计算时间稍长。为了优化速度，1.0是合理的值。合理的折衷值是2.0。0.0的值有效地禁用避开障碍物的功能，像一个简单的二进制搜索A*。
      motion_model_for_search: "DUBIN"    # For Hybrid Dubin, Redds-Shepp
      angle_quantization_bins: 64         # For Hybrid nodes: 搜索的角度桶数，对于2D节点必须为1（无角度搜索）
      analytic_expansion_ratio: 3.5       # For Hybrid/Lattice nodes: 在搜索过程中尝试解析扩展的比例，用于最终接近。
      analytic_expansion_max_length: 3.0  # For Hybrid/Lattice nodes: 解析扩展被认为有效的最大长度，以防止不安全的捷径（单位：米）。这应该与最小转弯半径成比例，不应小于最小半径的4-5倍。
      minimum_turning_radius: 0.05        # For Hybrid/Lattice nodes: 路径/车辆的最小转弯半径，单位：米
      retrospective_penalty: 0.025        # For Hybrid/Lattice nodes: 优先考虑路径上较晚的机动而不是较早的机动。节省搜索时间，因为较早的节点不会被扩展，直到有必要。必须 >= 0.0且 <= 1.0
      reverse_penalty: 1.0                # For Reeds-Shepp model: 应用于倒车的惩罚，必须 >= 1
      change_penalty: 0.0                 # For Hybrid nodes: 应用于改变方向的惩罚，必须 >= 0
      non_straight_penalty: 1.20          # For Hybrid nodes: 应用于非直线运动的惩罚，必须 >= 1
      cost_penalty: 2.0                   # For Hybrid nodes: 在将更高代价区域添加到障碍物地图动态规划距离扩展启发式时应用的惩罚。这将驱动机器人更靠近通道的中心。1.3 - 3.5 之间的值是合理的。
      rotation_penalty: 5.0               # For Lattice node: 仅在使用包含原地旋转原语的最小控制集时应用于纯原地旋转命令的惩罚。除非严格必要用于避开障碍物，否则应始终设置足够高的权重，以避免此操作，否则可能会在路径中频繁出现不连续的情况，要求机器人原地旋转以节省路径距离。
      lookup_table_size: 20.0             # For Hybrid nodes: 缓存的dubin/reeds-sheep距离窗口的大小，单位：米。
      cache_obstacle_heuristic: True      # For Hybrid nodes: 在同一目标位置的后续重新规划之间缓存障碍物地图动态规划距离扩展启发式。如果代价地图基本静态，则显著加快重新规划性能（40倍）。
      allow_reverse_expansion: False      # For Lattice node: 是否在前向原语或反向扩展状态格子图中扩展，将使每一步的分支因子加倍。
      smooth_path: True                   # For Lattice/Hybrid nodes: 是否平滑路径，对于2D节点始终为 true。
      smoother:
        max_iterations: 1000
        w_smooth: 0.3
        w_data: 0.2
        tolerance: 1.0e-10
        do_refinement: true

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

behavior_server:
  ros__parameters:
    use_sim_time: false
    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: "pb_nav2_behaviors/BackUpFreeSpace"
    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: gimbal_yaw_fake
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 3.2
    # params for pb_nav2_behaviors/BackUpFreeSpace
    service_name: "global_costmap/get_costmap"
    max_radius: 2.0
    visualize: false

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

velocity_smoother:
  ros__parameters:
    use_sim_time: false
    smoothing_frequency: 20.0
    scale_velocities: False
    feedback: "OPEN_LOOP"
    max_velocity: [2.5, 2.5, 3.0]
    min_velocity: [-2.5, -2.5, -3.0]
    max_accel: [4.5, 4.5, 5.0]
    max_decel: [-4.5, -4.5, -5.0]
    odom_topic: "odometry"
    odom_duration: 0.1
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0

pb_teleop_twist_joy_node:
  ros__parameters:
    use_sim_time: false
    robot_base_frame: gimbal_yaw
    control_mode: auto_control    # Option: auto_control, manual_control

    require_enable_button: true
    enable_button: 4              # L1 shoulder button
    enable_turbo_button: 5        # R1 shoulder button

    axis_chassis:
      x: 1                        # Left thumb stick vertical
      y: 0                        # Left thumb stick horizontal
      yaw: 6                      # button_left and button_right
    scale_chassis:
      x: 2.5
      y: 2.5
      yaw: 3.0
    scale_chassis_turbo:
      x: 4.0
      y: 4.0
      yaw: 6.0

    axis_gimbal:
      roll: -1                    # Disable
      pitch: 4                    # Right thumb stick vertical
      yaw: 3                      # Right thumb stick horizontal
    scale_gimbal:
      roll: 0.0
      pitch: -1.0
      yaw: 2.5
    scale_gimbal_turbo:
      roll: 0.0
      pitch: -1.5
      yaw: 3.5