truck_nav2_params.yaml

# GPS WPF CHANGES:
# - amcl params where removed. They are not needed because global localization is provided
#   by an ekf node on robot_localization fusing gps data with local odometry sources
# - static layer is removed from both costmaps, in this tutorial we assume there is no map
#   of the environment
# - global costmap is set to be rolling to allow the robot to traverse big environment by
#   following successive relatively close waypoints that fit in a smaller rolling costmap

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    bt_loop_duration: 10
    default_server_timeout: 20
    navigators: ["navigate_to_pose", "navigate_through_poses"] #["navigate_through_poses"]
    navigate_to_pose:
      plugin: "nav2_bt_navigator/NavigateToPoseNavigator"
    navigate_through_poses:
      plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator"
    # '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_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_are_error_codes_active_condition_bt_node
      - nav2_would_a_controller_recovery_help_condition_bt_node
      - nav2_would_a_planner_recovery_help_condition_bt_node
      - nav2_would_a_smoother_recovery_help_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
    error_code_names:
      - compute_path_error_code
      - follow_path_error_code

# GPS WPF CHANGE: Remove static layer
local_costmap:
  local_costmap:
    ros__parameters:
      lethal_cost_threshold: 100
      update_frequency: 5.0 #20.0 #5.0
      transform_tolerance: 0.5
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_footprint
      rolling_window: true
      width: 100
      height: 50
      origin_x: 50.0  # Shifts the costmap back by 75 meters
      origin_y: 0.0  # No lateral shift
      track_unknown_space: true
      resolution:  1.0 #1.0 #0.05 Increased tge resoluiton to 1 meter for 1 grid, since the truck is much bigger than turtlebot size, and 0.1 was default for turtlebot.
      #robot_radius: 5.0 #0.22
      footprint:  "[[-4.8025, -1.739], [4.8025, -1.739], [4.8025, 1.739], [-4.8025, 1.739]]"
      plugins: ["voxel_layer", "inflation_layer"] # "range_sensor_layer"]
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0 #3.0 #Cost scale number 1 does not push robot against obstacle. Value of 2 create more stronger potential field for the planner deviate with higher offset...
        inflation_radius: 0.01 #10.0 # The inflation radius must be higher than robot's radius to create potential regions #0.5 #0.55 #the blue region is max with number 1 assigned, then with number 2 assigned starts red regions, and later other dark blue about number 5 assigned.
      # obstacle_layer:
      #   plugin: "nav2_costmap_2d::ObstacleLayer"
      #   enabled: True
      #   observation_sources: sick_scan_2D right_scan fused_scan  #left_scan
      #   sick_scan_2D:
      #     topic: /sick_scan_2d
      #     max_obstacle_height: 3.0
      #     clearing: True
      #     marking: True
      #     data_type: "LaserScan"
      #     raytrace_max_range: 50.0
      #     raytrace_min_range: 0.0
      #     obstacle_max_range: 25.0
      #     obstacle_min_range: 0.0
      #     # outdoors there will probably be more inf points
      #     inf_is_valid: true
      #     #OTHER SCAN SOURCE
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0 #2.0 #0.0
        z_resolution:  1.0 # 1.0 #0.05
        z_voxels: 16 #Only works with 16 not 32
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: sick_scan_2D #fused_scan left_scan right_scan #fused_scan
        sick_scan_2D:
          topic: /sick_scan_2D
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 50.0
          raytrace_min_range: 0.0
          obstacle_max_range: 50.0
          obstacle_min_range: 1.0
          marking_threshold: 100 # THIS SET ZONES AS LETHAL OBSTACLES

        # left_scan:
        #   topic: /left_scan
        #   max_obstacle_height: 2.0
        #   clearing: True
        #   marking: True
        #   data_type: "LaserScan"
        #   raytrace_max_range: 50.0
        #   raytrace_min_range: 0.0
        #   obstacle_max_range: 50.0
        #   obstacle_min_range: 1.0
        # right_scan:
        #   topic: /right_scan
        #   max_obstacle_height: 2.0
        #   clearing: True
        #   marking: True
        #   data_type: "LaserScan"
        #   raytrace_max_range: 50.0
        #   raytrace_min_range: 0.0
        #   obstacle_max_range: 50.0
        #   obstacle_min_range: 1.0
        fused_scan:
          topic: /fused_scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 25.0
          raytrace_min_range: 0.0
          obstacle_max_range: 25.0
          obstacle_min_range: 1.0
      # range_sensor_layer: #IT delays too much to send data, issues with planner and control when integrated
      #   plugin: "nav2_costmap_2d::RangeSensorLayer"
      #   enabled: True
      #   transform_tolerance: 0.3
      #   clear_on_max_reading: true
      #   topics: ["/ultrassonic/front_left"]
      #   clear_threshold: 0.2
      #   mark_threshold: 0.8
      #   no_readings_timeout: 5.0
      #   clear_after_reading: true
      #   max_distance: 5.0
      #   min_distance: 0.2
      #   clear_on_max_reading: true
      #   buffer_size: 10
      #   observation_persistence: 2.0
      #   inf_is_valid: false
      always_send_full_costmap: True



# GPS WPF CHANGE: Remove static layer
# GPS WPF CHANGE: Set rolling global costmap with 50x50 size. See note below
global_costmap:
  global_costmap:
    ros__parameters:
      #unknown_cost_value: 120 #255 #0# slightly less than 255 to be more explorative
      lethal_cost_threshold: 100
      update_frequency: 1.0
      publish_frequency: 1.0
      transform_tolerance: 0.5
      global_frame: map
      robot_base_frame: base_footprint
      use_sim_time: True
      #robot_radius: 5.0 #0.22
      footprint:  "[[-4.8025, -1.739], [4.8025, -1.739], [4.8025, 1.739], [-4.8025, 1.739]]"
      resolution: 1.0 #0.05 # 1.0 #0.05 # Increased tge resoluiton to 1 meter for 1 grid, since the truck is much bigger than turtlebot size, and 0.1 was default for turtlebot.
      # When using GPS navigation you will potentially traverse huge environments which are not practical to
      # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to
      # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174
      rolling_window: True
      width: 1000
      height: 100
      origin_x: 100.0  # Shifts the costmap back by 75 meters
      origin_y: 0.0  # No lateral shift
      track_unknown_space: true
      # no static map
      plugins: ["obstacle_layer", "inflation_layer"]
      #plugins: ["static_layer", "inflation_layer", "obstacle_layer"]  # We won't use static map
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: sick_scan_2D #right_scan #fused_scan left_scan right_scan sick_scan_2D
        # left_scan:
        #   topic: /left_scan
        #   max_obstacle_height: 2.0
        #   clearing: True
        #   marking: True
        #   data_type: "LaserScan"
        #   raytrace_max_range: 50.0
        #   raytrace_min_range: 0.0
        #   obstacle_max_range: 25.0
        #   obstacle_min_range: 0.0
        #   # outdoors there will probably be more inf points
        #   inf_is_valid: true
        #   #OTHER SCAN SOURCE
        # right_scan:
        #   topic: /right_scan
        #   max_obstacle_height: 2.0
        #   clearing: True
        #   marking: True
        #   data_type: "LaserScan"
        #   raytrace_max_range: 50.0
        #   raytrace_min_range: 0.0
        #   obstacle_max_range: 25.0
        #   obstacle_min_range: 1.0
        #   # outdoors there will probably be more inf points
        #   inf_is_valid: true
        fused_scan:
          topic: /fused_scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 90.0
          raytrace_min_range: 0.0
          obstacle_max_range: 90.0
          obstacle_min_range: 1.0
        sick_scan_2D:
          topic: /sick_scan_2D
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 50.0
          raytrace_min_range: 0.0
          obstacle_max_range: 50.0
          obstacle_min_range: 1.0
          marking_threshold: 100 # THIS SET ZONES AS LETHAL OBSTACLES
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 1.0 #3.0
        inflation_radius: 0.01 #10.0 # # The inflation radius must be higher than robot's radius to create potential regions#0.5 #0.55
      always_send_full_costmap: True





# 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_saver:
  ros__parameters:
    save_map_timeout: 5.0
    free_thresh_default: 0.25
    occupied_thresh_default: 0.65
    map_subscribe_transient_local: True

# map_server:
#   ros__parameters:
#     use_sim_time: True
#     yaml_filename: "turtlebot3.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

#GLOBAL PLANNER
planner_server:
  ros__parameters:
    expected_planner_frequency: 5.0  # Expected frequency for the planner to run in Hz.
    planner_patience: 50.0  # Maximum time in seconds the planner waits for a valid plan


    #use_sim_time: True  # If simulation time should be used.
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlannerHybrid"  # The plugin used for planning, here Hybrid-A*.
      downsample_costmap: false  #switched to true to see if reach greater paths # Whether the costmap should be downsampled.
      transform_tolerance: 2.0 #0.3 # Increased over 3 times the time tolerance to calculate a path, otherwise just close waypoints will produce a path, since it take time to generate a path for large areas
      downsampling_factor: 1
      allow_unknown: true  # If planning through unknown space is allowed.
      tolerance: 2.0  # Tolerance for planning in meters: Given the size of truck 1 meter is not so huge...
      max_iterations: 1000000 #1000000  # Max iterations the planner can take before stopping.
      max_on_approach_iterations: 1000  # Iterations to try to find an exact solution once within goal tolerance.
      max_planning_time: 50.0 #30.0 #15.0  # Max planning time in seconds.# I have doubled the time to allow computate far waypoitns, wich require more time to produce a bigger path (greater lenght)
      motion_model_for_search: "DUBIN"  # The motion model used for planning. "DUBIN" for non-holonomic robots without reverse gear, "REEDS_SHEPP" for those with.
      angle_quantization_bins: 72 #36 # Number of bins for quantizing angles. More bins, the finer the search . Reduced to provide more straight paths instead the curvy ones being produced which is not compatible for aligned waypoints of GPS.
      minimum_turning_radius: 11.6 # Minimum turning radius for the vehicle in meters. 11.6 official value given by manual
      reverse_penalty: 2.0  # Penalty for reversing. Higher values discourage reversing.
      change_penalty: 1.0  # Penalty for changing direction.
      non_straight_penalty: 1.0 #1.5 #1.2  # Penalty for non-straight motions. Encourages straighter paths.
      cost_penalty: 1.0 #3.0  # Penalty for traversing high-cost cells. Higher values make the robot more cost-sensitive.
      retrospective_penalty: 0.03
      lookup_table_size: 25.0 # Increased for complex environment handling
      cache_obstacle_heuristic: True  # Caches the heuristic calculations for obstacle-laden areas to speed up planning.
      debug_visualizations: True  # Enables debug visualizations in RViz.
      analytic_expansion_ratio: 20.0  # Multiplier for how often analytic expansions are attempted.
      analytic_expansion_max_length: 100.0 #3.0  # Maximum allowed length for an analytic expansion.
      analytic_expansion_max_cost: 800 #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: True #false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      smooth_path: True
      allow_primitive_interpolation: True #This adds additional motion primitives that can help create more direct paths between points.
# Each of these parameters allows for fine-tuning the planner's behavior to suit the specific needs of your application and the characteristics of the environment.
# The analytic_expansion_ratio and analytic_expansion_max_length, for example, are particularly useful in complex or highly constrained environments where the standard grid-based search might struggle to find an optimal path.
      costmap:
        resolution: 1.0 # #1.0 # 0.05  # Smaller  captures more detail, useful for larger areas
        width: 30 #150
        height: 30 #150
        #radius: 0.22
        footprint:  "[[-4.8025, -1.739], [4.8025, -1.739], [4.8025, 1.739], [-4.8025, 1.739]]"


#LOCAL PLANNER
controller_server:
  ros__parameters:
    controller_frequency: 50.0  #20.0
    min_x_velocity_threshold: 0.01
    min_y_velocity_threshold: 0.5
    min_theta_velocity_threshold: 0.01
    failure_tolerance: 1.0 #0.3
    progress_checker_plugins: ["progress_checker"]
    #goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
    goal_checker_plugins: ["front_cabin_goal_checker"] # I have developed this one
    front_cabin_goal_checker:
     plugin: "nav2_bringup/FrontCabinGoalChecker"
# Adjust as necessary # It is not working here. CHANGE IN CPP CODE THE TOLERANCES: front_cabin_goal_checker.cpp and recompile
    #  front_offset: 5.0
    #  xy_goal_tolerance: 1.0  # Meters
    #  yaw_goal_tolerance: 1.25  # Radians

    controller_plugins: ["FollowPath"]

    # Progress checker parameters
    progress_checker:
      plugin: "nav2_controller::SimpleProgressChecker"
      required_movement_radius: 10.0 # Tamanho minimo em metros que considera que o caminhão não esta progredindo senão andou isso.
      movement_time_allowance: 15.0

    # 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: 1.0 #1.5 # meters wE CANNOT LET IT DEVIATE TO MUCH FROM CENTER OF PLANNED TRAJECTORY, IT MUST TRACK AND FOLLOW IT KEEPING  ON CENTER
    #   yaw_goal_tolerance: 0.2 # 0.25 rad = 14 degrees

    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      # MPPI specific parameters
      time_steps: 112 #30  # Increasing this value consumer more computer resources, however is needed to increase time horizon and get more smooth possible paths, increaseing time-horizon
      model_dt: 0.02  # Adjust to match your control loop timing , frequency = 50, dt = 150 = 0.02
      #TIME HORIZON = model_dt * time_steps --> predicted horizon is about 2.24 seconds x 10 m/s (Transboard vel) = 22.4 meters < 50 meters of local costmap size (Ok is under the costmap size)
      batch_size: 1000 #1000  # Lowered to reduce computational load
      vx_std: 0.1 #0.2  # Adjust based on expected velocity deviations
      #vy_std: 0.2  # Set to 0 as lateral movement is not applicable
      wz_std: 1.0 #0.0175 #0.05  # Adjust based on expected angular velocity deviations
      vx_max: 5.0 #8.5 # 5.0  # Set to your robot's max forward velocity= 8.5 m/s = 30 km/h (Transbord Operation)
      vx_min: -2.25  # Set to your robot's max reverse velocity if applicable, else set to 0
      vy_max: 0.0  # Set to 0 as lateral movement is not applicable
      wz_max: 10.0  # Set to your robot's max angular velocity clockwise
      wz_min: -10.0 # allow couterclockwise steering
      iteration_count: 10 #5  # Adjust based on the desired quality of the solution vs. computational time
      temperature: 0.15 #0.2 # Lowered to encourage less exploration in the control space, following the optimal path, avoiding unncessary exploitation and selection of suboptimal paths (offset, paralle routes), focusing on optimum path and smooth trajectories.
      gamma: 0.03 #0.015  # Adjust based on your system's response to control inputs. I have reduced from 0.015 to 0.01 to favor smoother trajectory following.
      motion_model: "Ackermann"  # Changed to Ackermann to match your robot's model
      retry_attempt_limit: 10  # Sets the number of attempts to find a feasible trajectory before failure. Higher values increase resilience to transient navigation difficulties.
      reset_period: 2.0  # Time of inactivity required to reset the optimizer, helping recover from bad states. Relevant for maintaining consistent controller performance.
      regenerate_noises: false
      #TRAJECTORY VISUALIZATION
      visualize: true  # Enable to visualize sampled trajectories in RViz. Useful for debugging and tuning, but may impact performance due to data volume.
      trajectory_step: 5  # Determines the step between visualized trajectories. A value of 5 means every 5th trajectory is visualized, reducing data volume for clarity.
      time_step: 3  # Sets the step between points on visualized trajectories. A value of 3 means every 3rd point is shown, balancing detail with visualization manageability.
      #PATH HANDLER PARAMS
      transform_tolerance: 1.0 #0.3  # Sets the tolerance for data transformations in seconds. A value of 0.3 allows for a broader window to accommodate potential delays in receiving GPS data and other sensor inputs, ensuring effective synchronization of data from different sources. This is particularly important in off-road conditions where GPS signal delays or inconsistencies might occur, helping to maintain accurate and reliable navigation despite these challenges.
      prune_distance: 120.0 # Balanced foresight for smoother, efficient navigation # I am setting to be greater than twice of the truck size
## Distance ahead of the nearest path point to the truck to prune the path.
# Determines the length of the path segment considered for current navigation decisions.
# Increasing this distance allows for more foresighted planning, enabling smoother maneuvers and better anticipation of upcoming path changes.
# Too high a value might overload the system with distant path details, while too low could lead to short-sighted navigation,
# potentially missing out on efficient route adjustments or preparation for future obstacles and turns.
      max_robot_pose_search_dist: 25.0   # Don't increase too much otherwise the truck will move just straight thinking it can readapt to a curve later
## Max integrated distance ahead of robot pose to search for the nearest path point.
# A higher value allows for a broader search area to reacquire the path after deviations,
# especially useful in complex or looping paths. It ensures the truck can find its way back to the path efficiently,
# enhancing navigation robustness in dynamic environments or after unexpected detours.
# However, setting it too high may increase computational load without proportional benefits.
      enforce_path_inversion: false  # Disables strict adherence to path inversions to promote smoother driving. Useful for avoiding unnecessary and potentially sharp direction changes that can disrupt passenger comfort. Set to true only if precise path following, including exact direction changes, is critical for operation.
      inversion_xy_tolerance: 0.2  # Adjusts how closely the truck needs to get to a path inversion point before considering it reached, in meters. A larger value smooths out navigation by reducing the need for precise alignment with path inversion points, minimizing abrupt steering adjustments. Ideal for ensuring passenger comfort by prioritizing smoother transitions over strict path fidelity.
      inversion_yaw_tolerance: 0.4  # Sets the angular tolerance for considering a path inversion point achieved, in radians. Increasing this tolerance allows for smoother turns by reducing the precision required in aligning with the path's intended direction changes. This helps in maintaining a comfortable ride by avoiding sharp or oscillatory steering behaviors.

      AckermannConstraints:
        min_turning_r: 11.6 # Adjust to match your vehicle's minimum turning radius, 11.6 official value given by manual

      critics: ["PathFollowCritic", "PathAlignCritic", "PathAngleCritic", "PreferForwardCritic"] #, "ObstaclesCritic", , ] #, "GoalCritic", GoalAngleCritic]
      #critics: ["ObstaclesCritic", "GoalCritic", "PathAlignCritic", "PathFollowCritic", "GoalAngleCritic", "PathAngleCritic", "PreferForwardCritic", "TwirlingCritic"] #"CostCritic"
      # # Critics configuration (adjust weights and parameters based on your environment and requirements)
      PathFollowCritic:
        enabled: true
        cost_weight: 60.0 # Prioritizes progression along the path, key for efficient navigation in variable off-road conditions.
        cost_power: 5 #2  # Quadratic cost scaling to deter deviations from the path, ensuring diligent path tracking.
        threshold_to_consider: 0.5  # Adjusted for the truck's dimensions, allowing path follow behavior up until closer proximity to the goal.
        offset_from_furthest: 1  # Optimized to keep the vehicle aligned with the path's most relevant segments, enhancing navigational accuracy.

      # ObstaclesCritic:
      #   enabled: true
      #   critical_weight: 35.0             # Increases penalty for near-collision scenarios, crucial for a large off-road truck to maintain safe distances from obstacles.
      #   repulsion_weight: 1.5             # Enhances general obstacle avoidance, providing a balance between safety and efficiency in dynamic environments.
      #   cost_power: 1                     # Quadratic scaling of obstacle costs, promoting safer routes by penalizing closer obstacles more heavily.
      #   consider_footprint: true          # Essential for accurate obstacle avoidance, considering the truck's entire physical footprint.
      #   collision_cost: 250000.0          # Sets a high penalty for collisions, reflecting the greater impact of collisions for larger vehicles.
      #   collision_margin_distance: 12.0   # A wider margin to severely penalize near-collision paths, suitable for the truck's size.
      #   near_goal_distance: 2.0           # Allows smoother goal convergence by relaxing obstacle avoidance near the goal, accommodating the truck's navigation needs.
      #   cost_scaling_factor: 3.0         # Consistent with the inflation layer, ensuring coherent obstacle cost scaling.
      #   inflation_radius: 5.0             # A larger radius around obstacles, providing a safer navigation buffer for the truck.
      #   inflation_layer_name: ""          # Specify if using multiple inflation layers and a particular one is needed for obstacle inflation.

      # GoalAngleCritic:
      #   enabled: true
      #   cost_weight: 2.0
      #   cost_power: 2
      #   threshold_to_consider: 1.0  # # Tighter orientation alignment required
      # GoalCritic:
      #   enabled: true
      #   threshold_to_consider: 1.0 #2.0  # Increased to 2 meters to start considering goal distance cost from a greater distance, accommodating the truck's larger size and ensuring smoother transitions to goal-directed movement.
      #   cost_power: 2 #2    # Set to 2 to apply a quadratic scaling of goal distance cost, encouraging more direct approaches as the truck gets closer to the goal.
      #   cost_weight: 15.0 #7.0  # Increase to place a higher emphasis on moving directly towards the goal when in proximity, suitable for ensuring efficient navigation in off-road conditions.
      # # CostCritic: # For some reason using this set of parameters below avoid tuck start the gps following feature...
      # #   enabled: true
      #   # cost_weight: 4.0  # Higher weight prioritizes avoidance of high-cost areas, crucial for off-road navigation where obstacles can be unpredictable.
      #   # cost_power: 2     # Quadratic scaling increases the penalty for nearing obstacles, encouraging safer routes.
      #   # consider_footprint: true  # Utilizes the full vehicle footprint for calculating costs, essential for accurately assessing the truck's interaction with obstacles.
      #   # collision_cost: 1000000.0  # A very high cost for collisions emphasizes the importance of avoiding obstacles entirely.
      #   # critical_cost: 500.0       # Increases the cost for being in the inflated space around obstacles, encouraging a safer distance from potential hazards.
      #   # near_goal_distance: 1.0    # Expands the distance near the goal where obstacle costs are less emphasized, allowing for smoother goal convergence.
      #   # inflation_layer_name: ""   # Specify if using multiple inflation layers and a particular one is needed for cost calculations.
      PathAlignCritic:
        enabled: true
        cost_weight: 35.0 #12.0  # Higher weight on path alignment to ensure efficient navigation along the planned global path in off-road terrain.
        cost_power: 5 #2     # Quadratic cost scaling to more heavily penalize deviations from the path, promoting tighter adherence to the route.
        threshold_to_consider: 1.0  # Increased distance for path alignment focus, accommodating the truck's size and the navigational challenges of off-road terrain.
        offset_from_furthest: 2  # Adjusted to ensure path alignment is considered earlier, aiding in consistent path tracking in variable off-road conditions.
        max_path_occupancy_ratio: 1.0 #0.1  # A slightly higher tolerance for path occupancy, allowing for dynamic obstacle negotiation without losing path alignment.
        use_path_orientations: true  # Enables consideration of path orientations, enhancing directional fidelity to the planned path, essential for complex off-road routes.
      PathAngleCritic:
        enabled: true
        cost_weight: 35.0 #3.6  # Enhances path angle alignment in complex off-road terrains, aiding in precise maneuvering and turns.
        cost_power: 5     # Applies quadratic scaling to penalize significant deviations from the path's angle, promoting closer adherence.
        threshold_to_consider: 1.0  # Allows for path angle adjustments from a greater distance, facilitating smoother approach and alignment.
        offset_from_furthest: 2  # Ensures path angle considerations are applied sooner, enhancing the vehicle's ability to align with upcoming turns.
        max_angle_to_furthest: 0.5  # Adopts a more lenient angle threshold for considering path alignment, accommodating the need for wider turns in off-road environments.
        mode: 2  # Prioritizes following the path in the requested direction, essential for effective navigation in off-road settings with specific directional requirements.
      PreferForwardCritic:
        enabled: true
        cost_weight: 50.0 #6.0  # Enhances the preference for forward movement, crucial for a truck's navigation efficiency and safety in off-road environments.
        cost_power: 2     # Implements quadratic cost scaling to more significantly penalize reversing, promoting a forward direction of travel.
        threshold_to_consider: 10.0  # Ensures forward movement is prioritized until the truck is within a practical distance to the goal, suitable for the vehicle's larger turning requirements.
      #TwirlingCritic: #DISABLE IT AFTER REMOVING SPINNING FROM 'RECOVERY BEHAVIORS' since nonholomic robots do not SPIN!
      #   enabled: true  # Enabled to apply a penalty for twirling movements, with the aim of discouraging spinning.
      #   cost_weight: 20.0  # High weight to significantly penalize spinning movements during path planning.
      #   cost_power: 1      # Linear penalty to ensure consistent discouragement of unnecessary rotations.


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

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: 10.0
    behavior_plugins: ["backup", "drive_on_heading", "assisted_teleop", "wait"]
    #behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"]
    # 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"
    local_frame: odom
    global_frame: map
    robot_base_frame: base_footprint
    transform_tolerance: 0.1
    simulate_ahead_time: 2.0
    max_rotational_vel: 10.0 #1.0
    min_rotational_vel: 0.4
    rotational_acc_lim: 10.0 #3.2

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

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0  # This is the frequency at which the smoother operates. 20 Hz is a reasonable starting point.
    scale_velocities: False  # Keeps scaling behavior off, which can be useful for debugging.
    feedback: "OPEN_LOOP"  # Uses the open-loop smoothing method. This is suitable if closed-loop feedback isn't necessary.
    max_velocity: [8.33, 0.0, 0.72]  # Max linear velocity set to ~8.33 m/s (~30 km/h), and angular velocity to ~0.72 rad/s.
    min_velocity: [-8.33, 0.0, -0.72]  # Min linear velocity for reverse movement at the same max speed, and angular velocity for reverse turning.
    max_accel: [3.0, 0.0, 1.0]  # Increases max linear and angular accelerations to 5.0 m/s² and 5.0 rad/s², respectively.
    max_decel: [-3.0, 0.0, -1.0]  # Increases max linear and angular decelerations to -5.0 m/s² and -5.0 rad/s².
    odom_topic: "odom"  # Confirms the odometry topic for velocity feedback, if used.
    odom_duration: 0.1  # Sets the duration to consider for odometry feedback.
    deadband_velocity: [0.0, 0.0, 0.0]  # Sets the deadband for velocity to zero, ensuring no minimum threshold.
    velocity_timeout: 1.0  # Sets the timeout for velocity commands to 1 second.


# collision_monitor:
#   ros__parameters:
#     base_frame_id: "base_footprint"
#     odom_frame_id: "odom"
#     cmd_vel_in_topic: "cmd_vel_smoothed"
#     cmd_vel_out_topic: "cmd_vel"
#     state_topic: "collision_monitor_state"
#     transform_tolerance: 0.2
#     source_timeout: 1.0
#     base_shift_correction: True
#     stop_pub_timeout: 2.0
#     # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types,
#     # and robot footprint for "approach" action type.
#     polygons: ["FootprintApproach"]
#     FootprintApproach:
#       type: "polygon"
#       action_type: "approach"
#       footprint_topic: "/local_costmap/published_footprint"
#       time_before_collision: 2.0
#       simulation_time_step: 0.1
#       min_points: 10
#       visualize: True
#       enabled: True
#     observation_sources: ["/sick_scan_2D"]
#     scan:
#       type: "scan"
#       topic: "/sick_scan_2D"
#       min_height: 0.15
#       max_height: 2.0
#       enabled: False #True


# collision_monitor:
#   ros__parameters:
#     use_sim_time: True
#     base_frame_id: "base_footprint"
#     odom_frame_id: "odom"
#     cmd_vel_in_topic: "cmd_vel_raw"
#     cmd_vel_out_topic: "cmd_vel"
#     transform_tolerance: 0.5
#     source_timeout: 5.0
#     stop_pub_timeout: 2.0
#     polygons: ["PolygonStop", "PolygonSlow"]
#     PolygonStop:
#       type: "polygon"
#       points: [0.4, 0.3, 0.4, -0.3, 0.0, -0.3, 0.0, 0.3]
#       action_type: "stop"
#       min_points: 4  # max_points: 3 for Humble
#       visualize: True
#       polygon_pub_topic: "polygon_stop"
#     PolygonSlow:
#       type: "polygon"
#       points: [0.6, 0.4, 0.6, -0.4, 0.0, -0.4, 0.0, 0.4]
#       action_type: "slowdown"
#       min_points: 4  # max_points: 3 for Humble
#       slowdown_ratio: 0.3
#       visualize: True
#       polygon_pub_topic: "polygon_slowdown"
#     observation_sources: ["sick_scan_2D"]
#     scan:
#       type: "scan"
#       topic: "sick_scan_2D"




# collision_monitor:
#   ros__parameters:
#     base_frame_id: "base_footprint"
#     odom_frame_id: "odometry/local"  # Adjusted to your specific odometry topic
#     cmd_vel_in_topic: "cmd_vel_smoothed"
#     cmd_vel_out_topic: "cmd_vel"
#     state_topic: "collision_monitor_state"
#     transform_tolerance: 0.5
#     source_timeout: 5.0
#     base_shift_correction: True
#     stop_pub_timeout: 2.0
#     polygons: ["PolygonStop", "PolygonSlow", "FootprintApproach"]
#     PolygonStop:
#       type: "circle"
#       radius: 2.0  # Increased radius suitable for truck size
#       action_type: "stop"
#       visualize: True
#       polygon_pub_topic: "polygon_stop"
#       enabled: True
#     PolygonSlow:
#       type: "polygon"
#       points: "[[2.0, 2.0], [2.0, -2.0], [-1.0, -2.0], [-1.0, 2.0]]"  # Adjusted dimensions for truck
#       action_type: "slowdown"
#       slowdown_ratio: 0.3
#       visualize: True
#       polygon_pub_topic: "polygon_slowdown"
#       enabled: True
#     FootprintApproach:
#       type: "polygon"
#       action_type: "approach"
#       footprint_topic: "/local_costmap/published_footprint"
#       time_before_collision: 3.0  # Increased time for truck's longer stopping distance
#       simulation_time_step: 0.02
#       visualize: False
#       enabled: True
#     observation_sources: ["scan"]
#     scan:
#       type: "sick_scan_2D"
#       topic: "/sick_scan_2D"
#       min_height: 0.1
#       max_height: 2.0  # Adjusted for truck's height
#       enabled: True