Planning: a bug fix on traffic_decider/change_lane.cc

modules/planning/tasks/speed_optimizer.cc

@@ -20,7 +20,6 @@
 
 #include "modules/planning/common/planning_gflags.h"
 #include "modules/planning/common/speed_limit.h"
-#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
 #include "modules/planning/tasks/speed_optimizer.h"
 
 namespace apollo {
@@ -109,8 +108,9 @@ SpeedData SpeedOptimizer::GenerateStopProfile(const double init_speed,
 
 SpeedData SpeedOptimizer::GenerateStopProfileFromPolynomial(
     const double init_speed, const double init_acc) const {
-  constexpr double kMaxT = 3.0;
-  for (double t = 2.0; t < kMaxT; t += 0.5) {
+  AERROR << "Slowing down the car with polynomial.";
+  constexpr double kMaxT = 4.0;
+  for (double t = 2.0; t <= kMaxT; t += 0.5) {
     for (double s = 0.0; s < 50.0; s += 1.0) {
       QuinticPolynomialCurve1d curve(0.0, init_speed, init_acc, s, 0.0, 0.0, t);
       if (!IsValidProfile(curve)) {
@@ -134,13 +134,12 @@ SpeedData SpeedOptimizer::GenerateStopProfileFromPolynomial(
 
 bool SpeedOptimizer::IsValidProfile(
     const QuinticPolynomialCurve1d& curve) const {
-  for (double evaluate_t = 0.1; evaluate_t <= t; evaluate_t += 0.2) {
-    const double v = curve.Evaluate(1, t);
-    const double a = curve.Evaluate(2, t);
-    if (v < 0) {
-      return false;
-    }
-    if (a < -5.0) {
+  for (double evaluate_t = 0.1; evaluate_t <= curve.ParamLength();
+       evaluate_t += 0.2) {
+    const double v = curve.Evaluate(1, evaluate_t);
+    const double a = curve.Evaluate(2, evaluate_t);
+    constexpr double kEpsilon = 1e-3;
+    if (v < -kEpsilon || a < -5.0) {
       return false;
     }
   }

modules/planning/tasks/speed_optimizer.h

@@ -26,6 +26,7 @@
 
 #include "modules/common/status/status.h"
 #include "modules/planning/common/speed/speed_data.h"
+#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"
 #include "modules/planning/tasks/st_graph/st_graph_data.h"
 #include "modules/planning/tasks/task.h"
 

modules/planning/tasks/traffic_decider/change_lane.cc

@@ -22,6 +22,7 @@
 
 #include <algorithm>
 
+#include "modules/common/util/util.h"
 #include "modules/planning/common/planning_gflags.h"
 
 namespace apollo {
@@ -112,14 +113,23 @@ bool ChangeLane::CreateGuardObstacle(
   double ref_s = sl_point.s() + kStepDistance;
   for (double t = last_point.relative_time() + time_delta; ref_s < end_s;
        ref_s += kStepDistance, s += kStepDistance, t += time_delta) {
-    auto ref_point = reference_line.GetNearestReferencepoint(s);
+    auto ref_point = reference_line.GetNearestReferencepoint(ref_s);
+
+    Vec2d xy_point;
+    if (!reference_line.SLToXY(common::util::MakeSLPoint(ref_s, sl_point.l()),
+                               &xy_point)) {
+      return false;
+    }
+
     auto* tp = obstacle->AddTrajectoryPoint();
     tp->set_a(0.0);
     tp->set_v(extend_v);
     tp->set_relative_time(t);
-    tp->mutable_path_point()->set_x(ref_point.x());
-    tp->mutable_path_point()->set_y(ref_point.y());
+    tp->mutable_path_point()->set_x(xy_point.x());
+    tp->mutable_path_point()->set_y(xy_point.y());
     tp->mutable_path_point()->set_theta(ref_point.heading());
+
+    // this is an approximate estimate since we do not use it.
     tp->mutable_path_point()->set_s(s);
     tp->mutable_path_point()->set_kappa(ref_point.kappa());
   }