config planner_controller

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: 1000 #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: 5.0 #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: 10 #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 #VALOR ALTO GERA ESPIRAIS # Minimum turning radius for the vehicle in meters. 11.6 official value given by manual
      reverse_penalty: 200.0  # Penalty for reversing. Higher values discourage reversing.
      change_penalty: 30.0  # Penalty for changing direction.
      non_straight_penalty: 1.5 #1.5 #1.2  # Penalty for non-straight motions. Encourages straighter paths.
      cost_penalty: 1000.0 #3.0  # Penalty for traversing high-cost cells. Higher values make the robot more cost-sensitive.
      retrospective_penalty: 1.0 #0.03
      lookup_table_size: 1.0 #PArece que auxilia na abertura pra recalculo usando valroes grandes#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: 1.0 #20.0  # Multiplier for how often analytic expansions are attempted.
      analytic_expansion_max_length: 120.0 #150.0 #3.0  # Maximum allowed length for an analytic expansion.
      analytic_expansion_max_cost: 10000 #200.0 #VALORES ALTOS AJUDA ELE OSCILAR MENOS # 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_3max_cost_override: True #false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
      smooth_path: False #True
      allow_primitive_interpolation: False #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: 10.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: 20.0 #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: 5.0 #10.0 # Tamanho minimo em metros que considera que o caminhão não esta progredindo senão andou isso.
      movement_time_allowance: 30.0 #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: 5.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.1 #0.2 # 0.25 rad = 14 degrees

    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      # MPPI specific parameters
      #current_horizon = 0.02 * 150 = 3.0 segundos# current_horizon = model_dt * time_steps
      time_steps: 60 #112 # new_time_steps = current_horizon / model_dt  # 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.05 #0.02  # Adjust to match your control loop timing (1/ controller_frequency) , frequency = 50, model_dt = 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 #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: 0.5 #10.0  # Set to your robot's max angular velocity clockwise
      wz_min: -0.5 #-10.0 # allow couterclockwise steering
      iteration_count: 10 #5  # Adjust based on the desired quality of the solution vs. computational time
      temperature: 0.05 # 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.02 #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: 100.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", "GoalCritic", "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: 50.0 #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: 2 #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: 100000.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: 30.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: 1 #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: 1 #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: 3 #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.