Added non-working terrain evaluator, fpl seems wrong

config/navigation_jackal_classifier.lua

@@ -17,7 +17,7 @@ NavigationParameters = {
   max_linear_accel = 0.5;
   max_linear_decel = 0.5;
   -- max_linear_speed = 1.4;
-  max_linear_speed = 1.5;
+  max_linear_speed = .8;
   max_angular_accel = 0.5;
   max_angular_decel = 0.5;
   max_angular_speed = 1.0;
@@ -63,9 +63,9 @@ TerrainEvaluator = {
   model_path = "../terrain_models/classifier_model.pt";
 
   -- dist_to_goal_weight = -0.2;
-  -- dist_to_goal_weight = -0.7;
+  dist_to_goal_weight = -0.7;
   -- dist_to_goal_weight = -2.0;
-  dist_to_goal_weight = 0;
+  --  dist_to_goal_weight = 0;
 
   clearance_weight = 0; -- -0.25;
   fpl_weight = 0; -- -0.75;

config/navigation_jackal_icl.lua

@@ -3,37 +3,38 @@ function deg2rad(deg)
 end
 
 NavigationParameters = {
-  laser_topic = "velodyne_2dscan_highbeams";
+  laser_topic = "/velodyne_2dscan_lowbeam";
   odom_topic = "/odometry/filtered";
   localization_topic = "localization";
   image_topic = "/bev/single/compressed";
   init_topic = "initialpose";
   enable_topic = "autonomy_arbiter/enabled";
   laser_loc = {
-    x = 0.065;
-    y = 0;
+    x = 0.095;
+    y = 0.1;
   };
   dt = 0.025;
-  max_linear_accel = 0.5;
-  max_linear_decel = 0.5;
+  max_linear_accel = 0.3;
+  max_linear_decel = 0.3;
   -- max_linear_speed = 1.4;
-  max_linear_speed = 1.5;
+  max_linear_speed = 1.0;
   max_angular_accel = 0.5;
-  max_angular_decel = 0.5;
+  max_angular_decel = 0.3; -- 0.5
   max_angular_speed = 1.0;
   carrot_dist = 250;  -- large carrot distance so the goal does not latch onto the map
-  system_latency = 0.24;
-  obstacle_margin = 0.15;
-  num_options = 31;
+  system_latency = 0.24; -- 0.24
+  obstacle_margin = 0.2; -- 0.15
+  num_options = 21;
   -- num_options = 15;
-  robot_width = 0.44;
-  robot_length = 0.5;
+  robot_width = 0.2;
+  robot_length = 0.4;
   base_link_offset = 0;
-  max_free_path_length = 4.0;
+  max_free_path_length = 7.5;
   -- max_free_path_length = 3.5;  -- ahg demo
   -- max_free_path_length = 1;  -- ahg demo
   max_clearance = 1.0;
   can_traverse_stairs = false;
+  -- evaluator_type = "linear";
   evaluator_type = "terrain2";
   camera_calibration_path = "config/camera_calibration_kinect.yaml";
   model_path = "";
@@ -46,29 +47,28 @@ NavigationParameters = {
 };
 
 AckermannSampler = {
-  max_curvature = 2.5;
+  max_curvature = 0.5;
   -- max_curvature = 5;
   clearance_path_clip_fraction = 0.8;
 };
 
+Context = {
+  context_path="../terrain_models/realistic_context.pt";
+}
+
 TerrainEvaluator = {
   patch_size_pixels = 1;
   bev_pixels_per_meter = 150;
   min_cost = 0.0;
-  max_cost = 1;
+  max_cost = 1.0;
   discount_factor = 0.9;
   -- discount_factor = 0.8; -- ahg demo
   rollout_density = 20;
 
   model_path = "../terrain_models/model.pt";
-  context_path="../terrain_models/context.pt";
-
-  -- dist_to_goal_weight = -0.2;
-  -- dist_to_goal_weight = -0.7;
-  -- dist_to_goal_weight = -2.0;
-  dist_to_goal_weight = 0;
 
-  clearance_weight = 0; -- -0.25;
-  fpl_weight = 0; -- -0.75;
-  terrain_weight = 4.0;
+  dist_to_goal_weight = 0.0;
+  clearance_weight = -0.25; -- [0, max_clearance] 
+  fpl_weight = -0.1; -- [0, max_free_path_length] -> [0, -0.75]
+  terrain_weight = 0.0; -- [0, 1] Usually <1 due to discounting factor + norm cost
 }

config/navigation_jackal_sterling.lua

@@ -17,7 +17,7 @@ NavigationParameters = {
   max_linear_accel = 0.5;
   max_linear_decel = 0.5;
   -- max_linear_speed = 1.4;
-  max_linear_speed = 1.5;
+  max_linear_speed = 0.8;
   max_angular_accel = 0.5;
   max_angular_decel = 0.5;
   max_angular_speed = 1.0;
@@ -63,9 +63,9 @@ TerrainEvaluator = {
   model_path = "../terrain_models/sterling_model.pt";
 
   -- dist_to_goal_weight = -0.2;
-  -- dist_to_goal_weight = -0.7;
+  dist_to_goal_weight = -0.7;
   -- dist_to_goal_weight = -2.0;
-  dist_to_goal_weight = 0;
+  --  dist_to_goal_weight = 0;
 
   clearance_weight = 0; -- -0.25;
   fpl_weight = 0; -- -0.75;

src/navigation/ackermann_motion_primitives.cc

@@ -19,26 +19,26 @@
 */
 //========================================================================
 
+#include "ackermann_motion_primitives.h"
+
 #include <math.h>
 
 #include <algorithm>
 #include <memory>
 #include <vector>
 
-#include "math/poses_2d.h"
+#include "config_reader/config_reader.h"
 #include "eigen3/Eigen/Dense"
 #include "eigen3/Eigen/Geometry"
-#include "config_reader/config_reader.h"
+#include "math/poses_2d.h"
 #include "motion_primitives.h"
-#include "constant_curvature_arcs.h"
-#include "ackermann_motion_primitives.h"
 
-using std::min;
+using Eigen::Vector2f;
+using pose_2d::Pose2Df;
 using std::max;
-using std::vector;
+using std::min;
 using std::shared_ptr;
-using pose_2d::Pose2Df;
-using Eigen::Vector2f;
+using std::vector;
 using namespace math_util;
 
 CONFIG_FLOAT(max_curvature, "AckermannSampler.max_curvature");
@@ -51,8 +51,7 @@ const float kEpsilon = 1e-5;
 
 namespace motion_primitives {
 
-AckermannSampler::AckermannSampler() {
-}
+AckermannSampler::AckermannSampler() {}
 
 void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) {
   ConstantCurvatureArc& path = *path_ptr;
@@ -63,29 +62,26 @@ void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) {
     // path.length = min(nav_params.max_free_path_length, local_target.x());
     path.fpl = path.length;
     return;
-  } 
+  }
   const float turn_radius = 1.0f / path.curvature;
   const float quarter_circle_dist = fabs(turn_radius) * M_PI_2;
   const Vector2f turn_center(0, turn_radius);
   const Vector2f target_radial = local_target - turn_center;
-  const Vector2f middle_radial =
-      fabs(turn_radius) * target_radial.normalized();
+  const Vector2f middle_radial = fabs(turn_radius) * target_radial.normalized();
   const float middle_angle =
       atan2(fabs(middle_radial.x()), fabs(middle_radial.y()));
   const float dist_closest_to_goal = middle_angle * fabs(turn_radius);
-  path.fpl = min<float>({
-      nav_params.max_free_path_length, 
-      quarter_circle_dist});
+  path.fpl = min<float>({nav_params.max_free_path_length, quarter_circle_dist});
 
   // eyang/terrain: for terrain-based navigation, do not restrict
-  // the path length to only make progress towards the goal 
+  // the path length to only make progress towards the goal
   path.length = path.fpl;
   std::ignore = dist_closest_to_goal;  // silence compiler warning
   // path.length = min<float>({
   //   path.fpl,
   //   dist_closest_to_goal
   // });
-  const float stopping_dist = 
+  const float stopping_dist =
       Sq(vel.x()) / (2.0 * nav_params.linear_limits.max_deceleration);
   // eyang/terrain: since the robot is now allowed to move past the goal,
   // sometimes the stopping distance makes the curve too long.
@@ -97,29 +93,27 @@ vector<shared_ptr<PathRolloutBase>> AckermannSampler::GetSamples(int n) {
   vector<shared_ptr<PathRolloutBase>> samples;
   if (false) {
     samples = {
-      shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(-0.1)),
-      shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0)),
-      shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0.1)),
+        shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(-0.1)),
+        shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0)),
+        shared_ptr<PathRolloutBase>(new ConstantCurvatureArc(0.1)),
     };
     return samples;
   }
-  const float max_domega = 
+  const float max_domega =
       nav_params.dt * nav_params.angular_limits.max_acceleration;
-  const float max_dv = 
+  const float max_dv =
       nav_params.dt * nav_params.linear_limits.max_acceleration;
   const float robot_speed = fabs(vel.x());
   float c_min = -CONFIG_max_curvature;
   float c_max = CONFIG_max_curvature;
   if (robot_speed > max_dv + kEpsilon) {
-    c_min = max<float>(
-        c_min, (ang_vel - max_domega) / (robot_speed - max_dv));
-    c_max = min<float>(
-        c_max, (ang_vel + max_domega) / (robot_speed - max_dv));
+    c_min = max<float>(c_min, (ang_vel - max_domega) / (robot_speed - max_dv));
+    c_max = min<float>(c_max, (ang_vel + max_domega) / (robot_speed - max_dv));
   }
   const float dc = (c_max - c_min) / static_cast<float>(n - 1);
   // printf("Options: %6.2f : %6.2f : %6.2f\n", c_min, dc, c_max);
   if (false) {
-    for (float c = c_min; c <= c_max; c+= dc) {
+    for (float c = c_min; c <= c_max; c += dc) {
       auto sample = new ConstantCurvatureArc(c);
       SetMaxPathLength(sample);
       CheckObstacles(sample);
@@ -128,22 +122,23 @@ vector<shared_ptr<PathRolloutBase>> AckermannSampler::GetSamples(int n) {
     }
   } else {
     const float dc = (2.0f * CONFIG_max_curvature) / static_cast<float>(n - 1);
-    for (float c = -CONFIG_max_curvature; c <= CONFIG_max_curvature; c+= dc) {
+    for (float c = -CONFIG_max_curvature; c <= CONFIG_max_curvature; c += dc) {
       auto sample = new ConstantCurvatureArc(c);
       SetMaxPathLength(sample);
       CheckObstacles(sample);
       sample->angular_length = fabs(sample->length * c);
       samples.push_back(shared_ptr<PathRolloutBase>(sample));
     }
   }
-  
+
   return samples;
 }
 
 void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
   ConstantCurvatureArc& path = *path_ptr;
   // How much the robot's body extends in front of its base link frame.
-  const float l = 0.5 * nav_params.robot_length - nav_params.base_link_offset + nav_params.obstacle_margin;
+  const float l = 0.5 * nav_params.robot_length - nav_params.base_link_offset +
+                  nav_params.obstacle_margin;
   // The robot's half-width.
   const float w = 0.5 * nav_params.robot_width + nav_params.obstacle_margin;
   if (fabs(path.curvature) < kEpsilon) {
@@ -160,16 +155,16 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     path.fpl = max(0.0f, path.fpl);
     path.length = min(path.fpl, path.length);
 
-    const float stopping_dist = 
-      vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration);
+    const float stopping_dist =
+        vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration);
     if (path.fpl < stopping_dist) {
       path.length = 0;
     }
-    
+
     return;
   }
   const float path_radius = 1.0 / path.curvature;
-  const Vector2f c(0, path_radius );
+  const Vector2f c(0, path_radius);
   const float s = ((path_radius > 0.0) ? 1.0 : -1.0);
   const Vector2f inner_front_corner(l, s * w);
   const Vector2f outer_front_corner(l, -s * w);
@@ -179,11 +174,11 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
   const float r3_sq = (outer_front_corner - c).squaredNorm();
   float angle_min = M_PI;
   path.obstruction = Vector2f(-nav_params.max_free_path_length, 0);
-  // printf("%7.3f %7.3f %7.3f %7.3f\n", 
+  // printf("%7.3f %7.3f %7.3f %7.3f\n",
   //     path.curvature, sqrt(r1_sq), sqrt(r2_sq), sqrt(r3_sq));
   // The x-coordinate of the rear margin.
-  const float x_min = -0.5 * nav_params.robot_length + 
-      nav_params.base_link_offset - nav_params.obstacle_margin;
+  const float x_min = -0.5 * nav_params.robot_length +
+                      nav_params.base_link_offset - nav_params.obstacle_margin;
   using std::isfinite;
   for (const Vector2f& p : point_cloud) {
     if (!isfinite(p.x()) || !isfinite(p.y()) || p.x() < 0.0f) continue;
@@ -200,9 +195,9 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     //        path.curvature, sqrt(r_sq), r1, sqrt(r2_sq), sqrt(r3_sq));
     if (r_sq < r1_sq || r_sq > r3_sq) continue;
     const float r = sqrt(r_sq);
-    const float theta = ((path.curvature > 0.0f) ?
-      atan2<float>(p.x(), path_radius - p.y()) :
-      atan2<float>(p.x(), p.y() - path_radius));
+    const float theta =
+        ((path.curvature > 0.0f) ? atan2<float>(p.x(), path_radius - p.y())
+                                 : atan2<float>(p.x(), p.y() - path_radius));
     float alpha;
     if (r_sq < r2_sq) {
       // Point will hit the side of the robot first.
@@ -232,18 +227,17 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
       angle_min = theta;
     }
   }
-  const float stopping_dist = 
+  const float stopping_dist =
       vel.squaredNorm() / (2.0 * nav_params.linear_limits.max_deceleration);
   if (path.length < stopping_dist) path.length = 0;
   path.length = max(0.0f, path.length);
   angle_min = min<float>(angle_min, path.length * fabs(path.curvature));
   path.clearance = nav_params.max_clearance;
 
-
   for (const Vector2f& p : point_cloud) {
-    const float theta = ((path.curvature > 0.0f) ?
-        atan2<float>(p.x(), path_radius - p.y()) :
-        atan2<float>(p.x(), p.y() - path_radius));
+    const float theta =
+        ((path.curvature > 0.0f) ? atan2<float>(p.x(), path_radius - p.y())
+                                 : atan2<float>(p.x(), p.y() - path_radius));
     if (theta < CONFIG_clearance_clip * angle_min && theta > 0.0) {
       const float r = (p - c).norm();
       const float current_clearance = fabs(r - fabs(path_radius));
@@ -253,7 +247,7 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     }
   }
   path.clearance = max(0.0f, path.clearance);
-  // printf("%7.3f %7.3f %7.3f \n", 
+  // printf("%7.3f %7.3f %7.3f \n",
   //     path.curvature, path.length, path.clearance);
 }