changed recovery goal back to circle dist from path dist

ut-amrl · Apr 29, 2024 · 8d35e6b · 8d35e6b
1 parent ffe1298
commit 8d35e6b
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 12 deletions.
diff --git a/config/navigation.lua b/config/navigation.lua
@@ -43,7 +43,7 @@ NavigationParameters = {
   intermediate_goal_dist = 10; 
   max_inflation_radius = 1;
   min_inflation_radius = 0.3;
-  local_costmap_resolution = 0.2;
+  local_costmap_resolution = 0.05;
   local_costmap_size = 20;
   global_costmap_resolution = 0.1;
   global_costmap_size_x = 100;
@@ -56,7 +56,7 @@ NavigationParameters = {
   object_lifespan = 15;
   inflation_coeff = 8;
   distance_weight = 3;
-  recovery_carrot_dist = 0.5;
+  recovery_carrot_dist = 0.7;
 };
 
 AckermannSampler = {

diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -929,22 +929,15 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) {
     if (debug) printf("No best path found\n");
     // No valid path found!
     Eigen::Vector2f prev_local_target = local_target_;
-    // Eigen::Vector2f temp_target = GetPathGoal(params_.carrot_dist/2);
-    Eigen::Vector2f temp_target = GetPathGoal(params_.recovery_carrot_dist);
+    Eigen::Vector2f temp_target;
+    GetCarrot(temp_target, false, params_.recovery_carrot_dist);
+    // Eigen::Vector2f temp_target = GetPathGoal(params_.recovery_carrot_dist);
     local_target_ = Rotation2Df(-robot_angle_) * (temp_target - robot_loc_);
     TurnInPlace(vel_cmd, ang_vel_cmd);
     local_target_ = prev_local_target;
-    // Halt(vel_cmd, ang_vel_cmd);
     return;
   }
 
-  // else if(best_path->FPL() < params_.target_dist_tolerance/2){
-  //   Eigen::Vector2f prev_local_target = local_target_;
-  //   Eigen::Vector2f temp_target = GetPathGoal(params_.carrot_dist/2);
-  //   local_target_ = Rotation2Df(-robot_angle_) * (temp_target - robot_loc_);
-  //   TurnInPlace(vel_cmd, ang_vel_cmd);
-  //   local_target_ = prev_local_target;
-  // }
 
   ang_vel_cmd = 0;
   vel_cmd = {0, 0};