Tuned linear evaluator code for outdoor deployments and refactored na…

…v state machine for readability

config/navigation.lua

@@ -37,14 +37,14 @@ NavigationParameters = {
   robot_length = 0.5;
   robot_wheelbase = 0.26;
   base_link_offset = 0.1;
-  max_free_path_length = 5.0;
+  max_free_path_length = 10.0;
   max_clearance = 1.0; -- was 1.0
   can_traverse_stairs = false;
   use_map_speed = true;
   target_dist_tolerance = 0.1;
   target_vel_tolerance = 0.1;
   target_angle_tolerance = 0.05;
-  local_fov = deg2rad(150);
+  local_fov = deg2rad(180);
   use_kinect = true;
   camera_calibration_path = "config/camera_calibration_kinect.yaml";
   model_path = "../preference_learning_models/jit_cost_model_outdoor_6dim.pt";

src/navigation/linear_evaluator.cc

@@ -60,10 +60,16 @@ using namespace math_util;
 // DEFINE_double(subopt, 0.0, "Max path increase for clearance");
 // DEFINE_double(cw_beta, 5.0, "Clearance weight beta");
 
-// Somewhat good paramters!
-DEFINE_double(dw, 1.0, "Distance weight");
+// Somewhat good paramters! 12/10
+// DEFINE_double(dw, 1.0, "Distance weight");
+// DEFINE_double(cw, 6.0, "Clearance weight");
+// DEFINE_double(fw, -2.0, "Free path weight");
+// DEFINE_double(subopt, 0.0, "Max path increase for clearance");
+// DEFINE_double(cw_beta, 5.0, "Clearance weight beta");
+
+DEFINE_double(dw, 2.0, "Distance weight");
 DEFINE_double(cw, 6.0, "Clearance weight");
-DEFINE_double(fw, -2.0, "Free path weight");
+DEFINE_double(fw, -20.0, "Free path weight");
 DEFINE_double(subopt, 0.0, "Max path increase for clearance");
 DEFINE_double(cw_beta, 5.0, "Clearance weight beta");
 

src/navigation/navigation.cc

@@ -82,11 +82,12 @@ DEFINE_bool(test_toc, false, "Run 1D time-optimal controller test");
 DEFINE_bool(test_obstacle, false, "Run obstacle detection test");
 DEFINE_bool(test_avoidance, false, "Run obstacle avoidance test");
 DEFINE_bool(test_osm_planner, false, "Run OSM planner test");
-DEFINE_bool(test_gps_planner, true, "Run GPS planner test");
+DEFINE_bool(test_gps_planner, false, "Run GPS planner test");
 DEFINE_bool(test_planner, false, "Run navigation planner test");
 DEFINE_bool(test_latency, false, "Run Latency test");
 DEFINE_double(test_dist, 0.5, "Test distance");
 DEFINE_string(test_log_file, "", "Log test results to file");
+DEFINE_string(nav_frame, "utm", "Map Reference frame");
 DEFINE_int32(max_unsolvable_iterations, 3,
              "Max unsolvable obstacle avoidance iterations");
 
@@ -207,7 +208,7 @@ void Navigation::Initialize(const NavigationParameters& params,
   // planning_domain_ = GraphDomain(map_file, &params_);
 
   // LoadVectorMap(map_file);
-  gps_translator_.SetReferenceFrame("utm");
+  gps_translator_.SetReferenceFrame(FLAGS_nav_frame);
 
   initialized_ = true;
   sampler_->SetNavParams(params);
@@ -1453,6 +1454,9 @@ int Navigation::GetNextGPSGlobalGoal(int start_goal_index) {
 }
 
 bool Navigation::IsGoalInFOV(const Vector2f& local_goal) {
+  if (local_goal.x() == 0 and local_goal.y() == 0) return true;
+  // Compute angle between two vectors
+  // const Vector2f v1 = robot_loc_.normalize()
   const float angle_to_goal = atan2(local_goal.y(), local_goal.x());
   const float min_angle = -params_.local_fov / 2;  // in radians
   const float max_angle = params_.local_fov / 2;   // in radians
@@ -1719,7 +1723,8 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
       osm_planner_.IsGoalReached(gps_nav_goals_loc_.back(),
                                  params_.intermediate_goal_tolerance);
   bool isNavComplete = gps_nav_goals_loc_.empty() || isLastGoalReached;
-  bool isGoalInFOV = IsGoalInFOV(nav_goal_loc_);
+  bool isGoalInFOV = true;
+  // IsGoalInFOV(nav_goal_loc_);
 
   if (kDebug)
     printf("Run() isLastGoalReached %d isNavComplete %d isGoalInFOV %d\n",
@@ -1785,7 +1790,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
     } else if (nav_state_ == NavigationState::kTurnInPlace &&
                AngleDist(robot_angle_, nav_goal_angle_) <
                    params_.target_angle_tolerance) {
-      nav_state_ = NavigationState::kStopped;
+      nav_state_ = NavigationState::kGoto;
     }
   } while (prev_state != nav_state_);
 

src/navigation/navigation_main.cc

@@ -116,7 +116,7 @@ const string kOpenCVWindow = "Image window";
 DEFINE_string(robot_config, "config/navigation.lua", "Robot config file");
 DEFINE_string(maps_dir, kAmrlMapsDir, "Directory containing AMRL maps");
 DEFINE_bool(no_joystick, true, "Whether to use a joystick or not");
-DEFINE_bool(no_intermed, false,
+DEFINE_bool(no_intermed, true,
             "Whether to disable intermediate planning (will use legacy "
             "obstacle avoidance planner)");