Debugging why collision avoidance is getting too close to obstacles

config/navigation.lua

@@ -17,6 +17,7 @@ NavigationParameters = {
   };
   laser_frame = "base_link";
   odom_topic = "/jackal_velocity_controller/odom";
+  -- odom_topic = "/odometry/filtered";
   localization_topic = "localization";
   image_topic = "/camera/rgb/image_raw/compressed";
   init_topic = "initialpose";
@@ -31,10 +32,10 @@ NavigationParameters = {
   carrot_dist = 20.0;
   system_latency = 0.24;
   obstacle_margin = 0.15;
-  num_options = 62;
+  num_options = 31;
   robot_width = 0.44;
   robot_length = 0.5;
-  base_link_offset = 0;
+  base_link_offset = 0.1;
   max_free_path_length = 10.0;
   max_clearance = 1.0;
   can_traverse_stairs = false;

src/navigation/ackermann_motion_primitives.cc

@@ -19,30 +19,32 @@
 */
 //========================================================================
 
+#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 "constant_curvature_arcs.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");
 CONFIG_FLOAT(clearance_clip, "AckermannSampler.clearance_path_clip_fraction");
+CONFIG_FLOAT(clearance_padding, "AckermannSampler.clearance_padding");
 
 namespace {
 // Epsilon value for handling limited numerical precision.
@@ -51,33 +53,26 @@ const float kEpsilon = 1e-5;
 
 namespace motion_primitives {
 
-AckermannSampler::AckermannSampler() {
-}
+AckermannSampler::AckermannSampler() {}
 
 void AckermannSampler::SetMaxPathLength(ConstantCurvatureArc* path_ptr) {
   ConstantCurvatureArc& path = *path_ptr;
   if (fabs(path.curvature) < kEpsilon) {
     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.length = min<float>({
-    path.fpl,
-    dist_closest_to_goal
-  });
-  const float stopping_dist = 
+  path.fpl = min<float>({nav_params.max_free_path_length, quarter_circle_dist});
+  path.length = min<float>({path.fpl, dist_closest_to_goal});
+  const float stopping_dist =
       Sq(vel.x()) / (2.0 * nav_params.linear_limits.max_deceleration);
   path.length = max(path.length, stopping_dist);
 }
@@ -86,29 +81,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);
@@ -117,22 +110,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) {
@@ -149,16 +143,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);
@@ -168,11 +162,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;
@@ -189,9 +183,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.
@@ -221,18 +215,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));
@@ -242,7 +235,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);
 }
 

src/navigation/linear_evaluator.cc

@@ -48,9 +48,9 @@ using std::vector;
 using namespace geometry;
 using namespace math_util;
 
-DEFINE_double(dw, 0.5, "Distance weight");
+DEFINE_double(dw, 1, "Distance weight");
 DEFINE_double(cw, -0.5, "Clearance weight");
-DEFINE_double(fw, 0.5, "Free path weight");
+DEFINE_double(fw, -1, "Free path weight");
 DEFINE_double(subopt, 1.5, "Max path increase for clearance");
 
 namespace motion_primitives {
@@ -128,4 +128,4 @@ void LinearEvaluator::SetSubOpt(const float &threshold) {
   FLAGS_subopt = threshold;
 }
 
-}  // namespace motion_primitives
+}  // namespace motion_primitives

src/navigation/linear_evaluator.h

@@ -28,10 +28,11 @@
 
 namespace motion_primitives {
 
-struct LinearEvaluator :  PathEvaluatorBase {
+struct LinearEvaluator : PathEvaluatorBase {
   // Return the best path rollout from the provided set of paths.
   std::shared_ptr<PathRolloutBase> FindBest(
       const std::vector<std::shared_ptr<PathRolloutBase>>& paths) override;
+  float GetPathCost(const std::shared_ptr<PathRolloutBase>& path);
   void SetClearanceWeight(const float& weight);
   void SetDistanceWeight(const float& weight);
   void SetFreePathWeight(const float& weight);
@@ -40,5 +41,4 @@ struct LinearEvaluator :  PathEvaluatorBase {
 
 }  // namespace motion_primitives
 
-
 #endif  // LINEAR_EVALUATOR_H

src/navigation/navigation.cc

@@ -80,7 +80,7 @@ using namespace motion_primitives;
 // Special test modes.
 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_avoidance, true, "Run obstacle avoidance test");
 DEFINE_bool(test_osm_planner, false, "Run OSM planner test");
 DEFINE_bool(test_planner, false, "Run navigation planner test");
 DEFINE_bool(test_latency, false, "Run Latency test");
@@ -940,19 +940,22 @@ bool Navigation::PlanStillValid() {
 }
 
 bool Navigation::IntermediatePlanStillValid() {
+  const bool kDebug = FLAGS_v > 1;
   if (plan_path_.size() < 2 || !intermediate_path_found_) return false;
 
   // TODO: Add parameter for when to look for new goal or use different
   // heuristic, may not be necessary with carrot replan
   if ((nav_goal_loc_ - plan_path_[0].loc).norm() >
           sqrt(2 * params_.local_costmap_resolution) / 2 &&
       (robot_loc_ - plan_path_[0].loc).norm() < 1) {
+    if (kDebug) printf("IntermediatePlanStillValid(): Goal too far\n");
     return false;
   }
 
   Vector2f global_carrot;
   GetGlobalCarrot(global_carrot);
   if ((intermediate_goal_ - global_carrot).norm() > params_.replan_dist) {
+    if (kDebug) printf("IntermediatePlanStillValid(): Carrot too far\n");
     return false;
   }
 
@@ -964,6 +967,7 @@ bool Navigation::IntermediatePlanStillValid() {
     if (in_map &&
         (costmap_.getCost(mx, my) == costmap_2d::LETHAL_OBSTACLE ||
          costmap_.getCost(mx, my) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE)) {
+      if (kDebug) printf("IntermediatePlanStillValid(): Obstacle\n");
       return false;
     }
   }
@@ -1382,7 +1386,7 @@ Eigen::Vector2f Navigation::GetIntermediateGoal() { return intermediate_goal_; }
 int Navigation::GetNextGPSGlobalGoal(int start_goal_index) {
   if (gps_nav_goals_loc_.empty() || !gps_initialized_) return -1;
 
-  const bool kDebug = FLAGS_v > 0;
+  const bool kDebug = FLAGS_v > 1;
   // Never go back, Never surrender
   const auto& final_goal = gps_nav_goals_loc_.back();
   for (int i = start_goal_index + 1; i < int(gps_nav_goals_loc_.size()); ++i) {
@@ -1713,7 +1717,7 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
       if (gps_goal_index_ + 1 < int(gps_nav_goals_loc_.size())) {
         if (kDebug) printf("Switching to next GPS Goal\n");
         printf("Switching to next GPS Goal\n");
-        ReplanAndSetNextNavGoal(true);
+        ReplanAndSetNextNavGoal(false);
       } else {
         nav_state_ = NavigationState::kStopped;
       }
@@ -1760,7 +1764,6 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
 
   // Switch between navigation states.
   NavigationState prev_state = nav_state_;
-  bool did_state_change = prev_state != nav_state_;
   do {
     prev_state = nav_state_;
     if (nav_state_ == NavigationState::kGoto &&
@@ -1776,16 +1779,16 @@ bool Navigation::Run(const double& time, Vector2f& cmd_vel,
 
   switch (nav_state_) {
     case NavigationState::kStopped: {
-      if (kDebug && did_state_change) printf("\nNav complete\n");
+      if (kDebug) printf("\nNav complete\n");
     } break;
     case NavigationState::kPaused: {
       if (kDebug) printf("\nNav paused\n");
     } break;
     case NavigationState::kGoto: {
-      if (kDebug && did_state_change) printf("\nNav Goto\n");
+      if (kDebug) printf("\nNav Goto\n");
     } break;
     case NavigationState::kTurnInPlace: {
-      if (kDebug && did_state_change) printf("\nNav TurnInPlace\n");
+      if (kDebug) printf("\nNav TurnInPlace\n");
     } break;
     case NavigationState::kOverride: {
       if (kDebug) printf("\nNav override\n");

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, true,
+DEFINE_bool(no_intermed, false,
             "Whether to disable intermediate planning (will use legacy "
             "obstacle avoidance planner)");
 
@@ -206,7 +206,7 @@ void OdometryCallback(const nav_msgs::Odometry& msg) {
   odom_ = OdomHandler(msg);
   navigation_.UpdateOdometry(odom_);
   // Compute global coordinate offset from odometry t'' to odometry t'
-  // navigation_.UpdateRobotLocFromOdom(odom_);
+  navigation_.UpdateRobotLocFromOdom(odom_);
 }
 
 void GPSCallback(const std_msgs::Float64MultiArray& msg) {
@@ -483,7 +483,7 @@ void SendCommand(Eigen::Vector2f vel, float ang_vel) {
     vel.setZero();
     ang_vel = 0;
   }
-
+  printf("Sending command: (%f, %f) %f\n", vel.x(), vel.y(), ang_vel);
   drive_msg.twist.angular.x = 0;
   drive_msg.twist.angular.y = 0;
   drive_msg.twist.angular.z = ang_vel;