changed a* heuristic to octile distance and added recovery carrot param

config/navigation.lua

@@ -21,7 +21,7 @@ NavigationParameters = {
   max_angular_accel = 0.5;
   max_angular_decel = 0.5;
   max_angular_speed = 1.0;
-  carrot_dist = 1;
+  carrot_dist = 1.5;
   system_latency = 0.24;
   obstacle_margin = 0.15;
   num_options = 31;
@@ -42,8 +42,8 @@ NavigationParameters = {
   evaluator_type = "linear";
   intermediate_goal_dist = 10; 
   max_inflation_radius = 1;
-  min_inflation_radius = 0.2;
-  local_costmap_resolution = 0.1;
+  min_inflation_radius = 0.3;
+  local_costmap_resolution = 0.2;
   local_costmap_size = 20;
   global_costmap_resolution = 0.1;
   global_costmap_size_x = 100;
@@ -52,10 +52,11 @@ NavigationParameters = {
   global_costmap_origin_y = 0;
   lidar_range_min = 0.1;
   lidar_range_max = 10.0;
-  replan_carrot_dist = 2;
+  replan_dist = 2;
   object_lifespan = 15;
   inflation_coeff = 8;
   distance_weight = 3;
+  recovery_carrot_dist = 0.5;
 };
 
 AckermannSampler = {

src/navigation/navigation.cc

@@ -634,7 +634,13 @@ vector<GraphDomain::State> Navigation::Plan(const Vector2f& initial,
         if(cost.count(neighbor_index) == 0 || new_cost < cost[neighbor_index]){
           cost[neighbor_index] = new_cost;
           parent[neighbor_index] = current_index;
-          float heuristic_cost = (Vector2f(goal_map_x, goal_map_y) - Vector2f(new_row, new_col)).norm();
+
+          float dx = abs(goal_map_x - new_row);
+          float dy = abs(goal_map_y - new_col);
+
+          float heuristic_cost = max(dx, dy) + (sqrt(2.0) - 1.0) * min(dx, dy);
+
+          // float heuristic_cost = (Vector2f(goal_map_x, goal_map_y) - Vector2f(new_row, new_col)).norm();
           intermediate_queue.Push(neighbor_index, -(new_cost + heuristic_cost));
         }
       }   
@@ -713,7 +719,7 @@ bool Navigation::IntermediatePlanStillValid(){
 
   Vector2f global_carrot;
   GetGlobalCarrot(global_carrot);
-  if((intermediate_goal_ - global_carrot).norm() > params_.replan_carrot_dist){
+  if((intermediate_goal_ - global_carrot).norm() > params_.replan_dist){
     return false;
   }
 
@@ -903,17 +909,17 @@ void Navigation::RunObstacleAvoidance(Vector2f& vel_cmd, float& ang_vel_cmd) {
   }
   if (paths.size() == 0) {
     // No options, just stop.
-    // Halt(vel_cmd, ang_vel_cmd);
+    Halt(vel_cmd, ang_vel_cmd);
     if (debug) printf("No paths found\n");
     return;
   }
   auto best_path = evaluator_->FindBest(paths);
   if (best_path == nullptr) {
     if (debug) printf("No best path found\n");
     // No valid path found!
-    // TODO: Change this to rotate instead of halt
     Eigen::Vector2f prev_local_target = local_target_;
-    Eigen::Vector2f temp_target = GetPathGoal(params_.carrot_dist/2);
+    // Eigen::Vector2f temp_target = GetPathGoal(params_.carrot_dist/2);
+    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;

src/navigation/navigation_main.cc

@@ -799,10 +799,11 @@ void LoadConfig(navigation::NavigationParameters* params) {
   REAL_PARAM(carrot_dist);
   REAL_PARAM(lidar_range_min);
   REAL_PARAM(lidar_range_max);
-  REAL_PARAM(replan_carrot_dist);
+  REAL_PARAM(replan_dist);
   REAL_PARAM(object_lifespan);
   REAL_PARAM(inflation_coeff);
   REAL_PARAM(distance_weight);
+  REAL_PARAM(recovery_carrot_dist);
 
   config_reader::ConfigReader reader({FLAGS_robot_config});
   params->dt = CONFIG_dt;
@@ -844,10 +845,11 @@ void LoadConfig(navigation::NavigationParameters* params) {
   params->carrot_dist = CONFIG_carrot_dist;
   params->lidar_range_min = CONFIG_lidar_range_min;
   params->lidar_range_max = CONFIG_lidar_range_max;
-  params->replan_carrot_dist = CONFIG_replan_carrot_dist;
+  params->replan_dist = CONFIG_replan_dist;
   params->object_lifespan = CONFIG_object_lifespan;
   params->inflation_coeff = CONFIG_inflation_coeff;
   params->distance_weight = CONFIG_distance_weight;
+  params->recovery_carrot_dist = CONFIG_recovery_carrot_dist;
 
   // TODO Rather than loading camera homography from a file, compute it from camera transformation info
   LoadCameraCalibrationCV(CONFIG_camera_calibration_path, &params->K, &params->D, &params->H);

src/navigation/navigation_parameters.h

@@ -120,14 +120,17 @@ struct NavigationParameters {
   float lidar_range_max;
 
   // Distance between intermediate goal and global carrot before replanning
-  float replan_carrot_dist;
+  float replan_dist;
 
   // How long an object should stay in the costmap if not continuously observed
   float object_lifespan;
 
+  // Coefficient for exponential inflation cost
   float inflation_coeff;
-
+  // Weight for distance cost vs. inflation cost
   float distance_weight;
+  // Distance of carrot when using turn in place recovery
+  float recovery_carrot_dist;
 
 
   cv::Mat K;
@@ -167,10 +170,11 @@ struct NavigationParameters {
       global_costmap_origin_y(-50),
       lidar_range_min(0.1),
       lidar_range_max(10),
-      replan_carrot_dist(2),
+      replan_dist(2),
       object_lifespan(5),
       inflation_coeff(5),
-      distance_weight(2){
+      distance_weight(2),
+      recovery_carrot_dist(0.5){
       }
 };
 }  // namespace navigation