From b851ed1784ced12f92d1ef62b9c6e9b36b7ddb3d Mon Sep 17 00:00:00 2001
From: pimpest <82343504+Pimpest@users.noreply.github.com>
Date: Sun, 23 Nov 2025 13:59:52 +0100
Subject: [PATCH] Set up mppi

---
 .../params/toid_general_params.yaml           | 86 +++++++++++++++----
 1 file changed, 71 insertions(+), 15 deletions(-)

diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml
index 5a11780..9fc95f6 100644
--- a/toid_navigation/params/toid_general_params.yaml
+++ b/toid_navigation/params/toid_general_params.yaml
@@ -14,6 +14,7 @@ bt_navigator:
       - compute_path_error_code
       - follow_path_error_code
 
+
 behavior_server:
   ros__parameters:
     local_costmap_topic: local_costmap/costmap_raw
@@ -48,17 +49,19 @@ global_costmap:
       publish_frequency: 1.0
       global_frame: map
       robot_base_frame: base_footprint
-      robot_radius: 0.17
+      robot_radius: 0.18
       track_unknown_space: false
       rolling_window: false
       plugins: ["static_layer", "inflation_layer"]
       static_layer:
         plugin: "nav2_costmap_2d::StaticLayer"
+        footprint_clearing_enabled: false
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.25
+        footprint_clearing_enabled: false
+        cost_scaling_factor: 12.0
+        inflation_radius: 0.40
       always_send_full_costmap: True
 
 local_costmap:
@@ -67,12 +70,14 @@ local_costmap:
       update_frequency: 5.0
       publish_frequency: 2.0
       footprint: "[[-0.13, 0.16], [0.13, 0.16], [0.13, -0.16], [-0.13, -0.16]]"
-      global_frame: odom
+      footprint_padding: 0.02
+      #robot_radius: 0.18
+      global_frame: map
       robot_base_frame: base_footprint
       rolling_window: true
-      width: 2
-      height: 2
-      resolution: 0.025
+      width: 1
+      height: 1
+      resolution: 0.01
       introspection_mode: "disabled"
       plugins: ["static_layer", "inflation_layer"]
       static_layer:
@@ -80,8 +85,8 @@ local_costmap:
         map_subscribe_transient_local: True
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
-        cost_scaling_factor: 3.0
-        inflation_radius: 0.25
+        cost_scaling_factor: 10.0
+        inflation_radius: 0.45
 
 planner_server:
   ros__parameters:
@@ -107,13 +112,51 @@ controller_server:
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
       required_movement_radius: 0.05
-      movement_time_allowance: 10.0
+      movement_time_allowance: 5.0
     goal_checker:
       plugin: "nav2_controller::SimpleGoalChecker"
       xy_goal_tolerance: 0.10
       yaw_goal_tolerance: 0.10
       stateful: True
     FollowPath:
+#      plugin: "dwb_core::DWBLocalPlanner"
+#      debug_trajectory_details: True
+#      min_vel_x: 0.0
+#      min_vel_y: 0.0
+#      max_vel_x: 0.26
+#      max_vel_y: 0.0
+#      max_vel_theta: 1.0
+#      min_speed_xy: 0.0
+#      max_speed_xy: 0.26
+#      min_speed_theta: 0.0
+#      acc_lim_x: 2.5
+#      acc_lim_y: 0.0
+#      acc_lim_theta: 3.2
+#      decel_lim_x: -2.5
+#      decel_lim_y: 0.0
+#      decel_lim_theta: -3.2
+#      vx_samples: 20
+#      vtheta_samples: 20
+#      sim_time: 1.7
+#      linear_granularity: 0.05
+#      angular_granularity: 0.025
+#      transform_tolerance: 0.2
+#      xy_goal_tolerance: 0.10
+#      trans_stopped_velocity: 0.25
+#      short_circuit_trajectory_evaluation: True
+#      limit_vel_cmd_in_traj: False
+#      stateful: True
+#      critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+#      ObstacleFootprint.scale: 1.00
+#      PathAlign.scale: 32.0
+#      GoalAlign.scale: 24.0
+#      PathAlign.forward_point_distance: 0.05
+#      GoalAlign.forward_point_distance: 0.05
+#      PathDist.scale: 32.0
+#      GoalDist.scale: 24.0
+#      RotateToGoal.scale: 32.0
+#      RotateToGoal.slowing_factor: 5.0
+#      RotateToGoal.lookahead_time: -1.0
       plugin: "nav2_mppi_controller::MPPIController"
       time_steps: 56
       model_dt: 0.05
@@ -137,16 +180,29 @@ controller_server:
       gamma: 0.015
       motion_model: "DiffDrive"
       visualize: true
+      publish_optimal_trajectory: true
       regenerate_noises: true
       TrajectoryVisualizer:
         trajectory_step: 5
         time_step: 3
+      TrajectoryValidator:
+        # The validator can be configured with additional parameters if needed.
+        plugin: "mppi::DefaultOptimalTrajectoryValidator"
+        collision_lookahead_time: 1.0  # Time to look ahead to check for collisions
+        consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks
       AckermannConstraints:
         min_turning_r: 0.2
-      critics: [
-        "ConstraintCritic", "CostCritic", "GoalCritic",
-        "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic",
-        "PathAngleCritic", "PreferForwardCritic"]
+      critics:
+        [
+          "ConstraintCritic",
+          "CostCritic",
+          "GoalCritic",
+          "GoalAngleCritic",
+          "PathAlignCritic",
+          "PathFollowCritic",
+          "PathAngleCritic",
+          "PreferForwardCritic",
+        ]
       ConstraintCritic:
         enabled: true
         cost_power: 1
@@ -172,7 +228,7 @@ controller_server:
         cost_weight: 3.81
         near_collision_cost: 253
         critical_cost: 300.0
-        consider_footprint: false
+        consider_footprint: true
         collision_cost: 1000000.0
         near_goal_distance: 1.0
         trajectory_point_step: 2