[Improvement] Added exponential clearance separation for trajectory s…

…election

config/navigation.lua

@@ -16,8 +16,8 @@ NavigationParameters = {
     -- "/kinect_laserscan",
   };
   laser_frame = "base_link";
-  odom_topic = "/jackal_velocity_controller/odom";
-  -- odom_topic = "/odometry/filtered";
+  -- odom_topic = "/jackal_velocity_controller/odom";
+  odom_topic = "/odometry/filtered";
   localization_topic = "localization";
   image_topic = "/camera/rgb/image_raw/compressed";
   init_topic = "initialpose";
@@ -35,9 +35,10 @@ NavigationParameters = {
   num_options = 31;
   robot_width = 0.44;
   robot_length = 0.5;
+  robot_wheelbase = 0.26;
   base_link_offset = 0.1;
-  max_free_path_length = 10.0;
-  max_clearance = 1.0;
+  max_free_path_length = 5.0;
+  max_clearance = 1.0; -- was 1.0
   can_traverse_stairs = false;
   use_map_speed = true;
   target_dist_tolerance = 0.1;
@@ -69,5 +70,11 @@ NavigationParameters = {
 
 AckermannSampler = {
   max_curvature = 2.5;
-  clearance_path_clip_fraction = 0.8;
+  clearance_path_clip_fraction = 0.05;
 };
+
+-- LinearEvaluator = {
+--   distance_weight = 1.0;
+--   free_path_weight = 1.0;
+--   clearance_weight = 1.0;
+-- }

src/navigation/ackermann_motion_primitives.cc

@@ -44,7 +44,6 @@ using namespace math_util;
 
 CONFIG_FLOAT(max_curvature, "AckermannSampler.max_curvature");
 CONFIG_FLOAT(clearance_clip, "AckermannSampler.clearance_path_clip_fraction");
-CONFIG_FLOAT(clearance_padding, "AckermannSampler.clearance_padding");
 
 namespace {
 // Epsilon value for handling limited numerical precision.
@@ -226,13 +225,16 @@ void AckermannSampler::CheckObstacles(ConstantCurvatureArc* path_ptr) {
     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) {
+    // if (theta < CONFIG_clearance_clip * angle_min && theta > 0.0) {
+    if (theta > 0.0) {
       const float r = (p - c).norm();
       const float current_clearance = fabs(r - fabs(path_radius));
       if (path.clearance > current_clearance) {
         path.clearance = current_clearance;
       }
     }
+
+    // }
   }
   path.clearance = max(0.0f, path.clearance);
   // printf("%7.3f %7.3f %7.3f \n",

src/navigation/ackermann_motion_primitives.h

@@ -19,9 +19,6 @@
 */
 //========================================================================
 
-#ifndef ACKERMANN_MOTION_PRIMITIVES_H
-#define ACKERMANN_MOTION_PRIMITIVES_H
-
 #include <memory>
 #include <vector>
 
@@ -30,6 +27,9 @@
 #include "math/poses_2d.h"
 #include "motion_primitives.h"
 
+#ifndef ACKERMANN_MOTION_PRIMITIVES_H
+#define ACKERMANN_MOTION_PRIMITIVES_H
+
 namespace motion_primitives {
 
 // Path rollout sampler.

src/navigation/constant_curvature_arcs.cc

@@ -19,18 +19,18 @@
 */
 //========================================================================
 
+#include "constant_curvature_arcs.h"
+
 #include <float.h>
 
 #include <memory>
 #include <vector>
 
-#include "math/poses_2d.h"
-#include "math/math_util.h"
 #include "eigen3/Eigen/Dense"
-
+#include "math/math_util.h"
+#include "math/poses_2d.h"
 #include "motion_primitives.h"
 #include "navigation_parameters.h"
-#include "constant_curvature_arcs.h"
 
 using Eigen::Vector2f;
 using pose_2d::Pose2Df;
@@ -39,32 +39,33 @@ using navigation::MotionLimits;
 
 namespace motion_primitives {
 
-float ConstantCurvatureArc::Length() const {
-  return length;
-}
+float ConstantCurvatureArc::Length() const { return length; }
 
-float ConstantCurvatureArc::FPL() const {
-  return fpl;
-}
+float ConstantCurvatureArc::FPL() const { return fpl; }
 
-float ConstantCurvatureArc::AngularLength() const {
-  return angular_length;
-}
+float ConstantCurvatureArc::AngularLength() const { return angular_length; }
 
-float ConstantCurvatureArc::Clearance() const {
-  return clearance;
+float ConstantCurvatureArc::Clearance() const { return clearance; }
+
+void ConstantCurvatureArc::SetLength(const float& new_length) {
+  length = new_length;
+}
+void ConstantCurvatureArc::SetFPL(const float& new_fpl) { fpl = new_fpl; }
+void ConstantCurvatureArc::SetAngularLength(const float& new_angular_length) {
+  angular_length = new_angular_length;
+}
+void ConstantCurvatureArc::SetClearance(const float& new_clearance) {
+  clearance = new_clearance;
 }
 
 void ConstantCurvatureArc::GetControls(const MotionLimits& linear_limits,
                                        const MotionLimits& angular_limits,
-                                       const float dt,
-                                       const Vector2f& vel,
-                                       const float ang_vel,
-                                       Vector2f& vel_cmd,
+                                       const float dt, const Vector2f& vel,
+                                       const float ang_vel, Vector2f& vel_cmd,
                                        float& ang_vel_cmd) const {
   vel_cmd.y() = 0;
-  vel_cmd.x() = Run1DTimeOptimalControl(
-      linear_limits, 0, vel.x(), length, 0, dt);
+  vel_cmd.x() =
+      Run1DTimeOptimalControl(linear_limits, 0, vel.x(), length, 0, dt);
   ang_vel_cmd = vel_cmd.x() * curvature;
 }
 

src/navigation/constant_curvature_arcs.h

@@ -19,13 +19,11 @@
 */
 //========================================================================
 
-
 #include <memory>
 #include <vector>
 
-#include "math/poses_2d.h"
 #include "eigen3/Eigen/Dense"
-
+#include "math/poses_2d.h"
 #include "motion_primitives.h"
 
 #ifndef CONSTANT_CURVATURE_ARCS_H
@@ -48,12 +46,18 @@ struct ConstantCurvatureArc : PathRolloutBase {
   // Clearance along path.
   float Clearance() const override;
 
+  // Setters
+  void SetLength(const float& new_length) override;
+  void SetFPL(const float& new_fpl) override;
+  void SetAngularLength(const float& new_angular_length) override;
+  void SetClearance(const float& new_clearance) override;
+
   // Default constructor.
   ConstantCurvatureArc() : curvature(0), length(0), angular_length(0) {}
 
   // Explicit constructor from curvature.
-  explicit ConstantCurvatureArc(float curvature) : 
-      curvature(curvature), length(0), angular_length(0) {}
+  explicit ConstantCurvatureArc(float curvature)
+      : curvature(curvature), length(0), angular_length(0) {}
 
   // The pose of the robot at the end of the path rollout.
   pose_2d::Pose2Df EndPoint() const override;
@@ -63,10 +67,8 @@ struct ConstantCurvatureArc : PathRolloutBase {
   // The pose of the robot at the end of the path rollout.
   void GetControls(const navigation::MotionLimits& linear_limits,
                    const navigation::MotionLimits& angular_limits,
-                   const float dt,
-                   const Eigen::Vector2f& linear_vel,
-                   const float angular_vel,
-                   Eigen::Vector2f& vel_cmd,
+                   const float dt, const Eigen::Vector2f& linear_vel,
+                   const float angular_vel, Eigen::Vector2f& vel_cmd,
                    float& ang_vel_cmd) const override;
 
   float curvature;

src/navigation/linear_evaluator.cc

@@ -48,10 +48,24 @@ using std::vector;
 using namespace geometry;
 using namespace math_util;
 
-DEFINE_double(dw, 1, "Distance weight");
-DEFINE_double(cw, -0.5, "Clearance weight");
-DEFINE_double(fw, -1, "Free path weight");
-DEFINE_double(subopt, 1.5, "Max path increase for clearance");
+// DEFINE_double(dw, 1, "Distance weight");
+// DEFINE_double(cw, -3.0, "Clearance weight");
+// DEFINE_double(fw, -1, "Free path weight");
+// DEFINE_double(subopt, 1.5, "Max path increase for clearance");
+
+// // Somewhat good paramters!
+// DEFINE_double(dw, 1.0, "Distance weight");
+// DEFINE_double(cw, 1.5, "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");
+
+// Somewhat good paramters!
+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");
 
 namespace motion_primitives {
 
@@ -95,15 +109,15 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
   }
 
   // Next try to find better paths.
-  float best_cost = FLAGS_dw * (FLAGS_subopt * best_path_length) +
-                    FLAGS_fw * best->Length() + FLAGS_cw * best->Clearance();
+  float best_cost = FLAGS_dw * (best_path_length) + FLAGS_fw * best->Length() +
+                    FLAGS_cw * ClearanceCost(best);
   for (size_t i = 0; i < paths.size(); ++i) {
     if (paths[i]->Length() <= 0.0f) continue;
     const float path_length =
         (path_to_goal_exists ? (paths[i]->Length() + dist_to_goal[i])
                              : dist_to_goal[i]);
-    const float cost = FLAGS_dw * path_length + FLAGS_fw * paths[i]->Length() +
-                       FLAGS_cw * paths[i]->Clearance();
+    const float cost = FLAGS_cw * ClearanceCost(paths[i]) +
+                       FLAGS_dw * path_length + FLAGS_fw * paths[i]->Length();
     if (cost < best_cost) {
       best = paths[i];
       best_cost = cost;
@@ -112,6 +126,10 @@ shared_ptr<PathRolloutBase> LinearEvaluator::FindBest(
   return best;
 }
 
+float LinearEvaluator::ClearanceCost(const shared_ptr<PathRolloutBase> &path) {
+  return FLAGS_cw * exp(-FLAGS_cw_beta * path->Clearance());
+}
+
 void LinearEvaluator::SetClearanceWeight(const float &weight) {
   FLAGS_cw = weight;
 }