planning: renamed ConstantAccelerationTrajectory1d to...

planning: renamed ConstantAccelerationTrajectory1d to PiecewiseAccelerationTrajectory1d; fixed evaluation bug in PiecewiseAcclerationTrajectory1d

modules/planning/lattice/trajectory1d/piecewise_acceleration_trajectory1d.cc

@@ -30,7 +30,7 @@
 namespace apollo {
 namespace planning {
 
-ConstantAccelerationTrajectory1d::ConstantAccelerationTrajectory1d(
+PiecewiseAccelerationTrajectory1d::PiecewiseAccelerationTrajectory1d(
     const double start_s, const double start_v) {
   s_.push_back(start_s);
   v_.push_back(start_v);
@@ -38,7 +38,7 @@ ConstantAccelerationTrajectory1d::ConstantAccelerationTrajectory1d(
   t_.push_back(0.0);
 }
 
-void ConstantAccelerationTrajectory1d::AppendSegment(
+void PiecewiseAccelerationTrajectory1d::AppendSegment(
     const double a, const double t_duration) {
   double s0 = s_.back();
   double v0 = v_.back();
@@ -59,7 +59,7 @@ void ConstantAccelerationTrajectory1d::AppendSegment(
   t_.push_back(t1);
 }
 
-void ConstantAccelerationTrajectory1d::PopSegment() {
+void PiecewiseAccelerationTrajectory1d::PopSegment() {
   if (a_.size() > 0) {
     s_.pop_back();
     v_.pop_back();
@@ -68,12 +68,12 @@ void ConstantAccelerationTrajectory1d::PopSegment() {
   }
 }
 
-double ConstantAccelerationTrajectory1d::ParamLength() const {
+double PiecewiseAccelerationTrajectory1d::ParamLength() const {
   CHECK_GT(t_.size(), 1);
   return t_.back() - t_.front();
 }
 
-std::string ConstantAccelerationTrajectory1d::ToString() const {
+std::string PiecewiseAccelerationTrajectory1d::ToString() const {
   return apollo::common::util::StrCat(apollo::common::util::PrintIter(s_, "\t"),
                                       apollo::common::util::PrintIter(t_, "\t"),
                                       apollo::common::util::PrintIter(v_, "\t"),
@@ -81,7 +81,7 @@ std::string ConstantAccelerationTrajectory1d::ToString() const {
                                       "\n");
 }
 
-double ConstantAccelerationTrajectory1d::Evaluate(const std::uint32_t order,
+double PiecewiseAccelerationTrajectory1d::Evaluate(const std::uint32_t order,
                                                   const double param) const {
   CHECK_GT(t_.size(), 1);
   CHECK(t_.front() <= param && param <= t_.back());
@@ -99,7 +99,7 @@ double ConstantAccelerationTrajectory1d::Evaluate(const std::uint32_t order,
   return 0.0;
 }
 
-double ConstantAccelerationTrajectory1d::Evaluate_s(const double t) const {
+double PiecewiseAccelerationTrajectory1d::Evaluate_s(const double t) const {
   auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
   auto index = std::distance(t_.begin(), it_lower);
 
@@ -107,39 +107,39 @@ double ConstantAccelerationTrajectory1d::Evaluate_s(const double t) const {
   double v0 = v_[index - 1];
   double t0 = t_[index - 1];
 
-  double v1 = v_.back();
-  double t1 = t_.back();
+  double v1 = v_[index];
+  double t1 = t_[index];
 
   double v = common::math::lerp(v0, t0, v1, t1, t);
   double s = (v0 + v) * (t - t0) * 0.5 + s0;
   return s;
 }
 
-double ConstantAccelerationTrajectory1d::Evaluate_v(const double t) const {
+double PiecewiseAccelerationTrajectory1d::Evaluate_v(const double t) const {
   auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
   auto index = std::distance(t_.begin(), it_lower);
 
   double v0 = v_[index - 1];
   double t0 = t_[index - 1];
 
-  double v1 = v_.back();
-  double t1 = t_.back();
+  double v1 = v_[index];
+  double t1 = t_[index];
 
   double v = apollo::common::math::lerp(v0, t0, v1, t1, t);
   return v;
 }
 
-double ConstantAccelerationTrajectory1d::Evaluate_a(const double t) const {
+double PiecewiseAccelerationTrajectory1d::Evaluate_a(const double t) const {
   auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
   auto index = std::distance(t_.begin(), it_lower);
   return a_[index - 1];
 }
 
-double ConstantAccelerationTrajectory1d::Evaluate_j(const double t) const {
+double PiecewiseAccelerationTrajectory1d::Evaluate_j(const double t) const {
   return 0.0;
 }
 
-std::array<double, 4> ConstantAccelerationTrajectory1d::Evaluate(
+std::array<double, 4> PiecewiseAccelerationTrajectory1d::Evaluate(
     const double t) const {
   CHECK_GT(t_.size(), 1);
   CHECK(t_.front() <= t && t <= t_.back());
@@ -151,8 +151,8 @@ std::array<double, 4> ConstantAccelerationTrajectory1d::Evaluate(
   double v0 = v_[index - 1];
   double t0 = t_[index - 1];
 
-  double v1 = v_.back();
-  double t1 = t_.back();
+  double v1 = v_[index];
+  double t1 = t_[index];
 
   double v = common::math::lerp(v0, t0, v1, t1, t);
   double s = (v0 + v) * (t - t0) * 0.5 + s0;

modules/planning/lattice/trajectory1d/piecewise_acceleration_trajectory1d.h

@@ -30,11 +30,11 @@
 namespace apollo {
 namespace planning {
 
-class ConstantAccelerationTrajectory1d : public Curve1d {
+class PiecewiseAccelerationTrajectory1d : public Curve1d {
  public:
-  ConstantAccelerationTrajectory1d(const double start_s, const double start_v);
+  PiecewiseAccelerationTrajectory1d(const double start_s, const double start_v);
 
-  virtual ~ConstantAccelerationTrajectory1d() = default;
+  virtual ~PiecewiseAccelerationTrajectory1d() = default;
 
   void AppendSegment(const double a, const double t_duration);
 

modules/planning/lattice/trajectory_generation/piecewise_braking_trajectory_generator.cc

@@ -32,8 +32,8 @@ std::shared_ptr<Curve1d> PiecewiseBrakingTrajectoryGenerator::Generate(
     const double v_curr, const double a_comfort, const double d_comfort,
     const double max_time) {
 
-  std::shared_ptr<ConstantAccelerationTrajectory1d> ptr_trajectory =
-      std::make_shared<ConstantAccelerationTrajectory1d>(s_curr, v_curr);
+  std::shared_ptr<PiecewiseAccelerationTrajectory1d> ptr_trajectory =
+      std::make_shared<PiecewiseAccelerationTrajectory1d>(s_curr, v_curr);
 
   double s_dist = s_target - s_curr;
 

modules/planning/lattice/trajectory_generation/trajectory_evaluator.cc

@@ -521,7 +521,7 @@ std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
   double cruise_v = planning_target.cruise_speed();
 
   if (!planning_target.has_stop_point()) {
-    ConstantAccelerationTrajectory1d lon_traj(init_s_[0], cruise_v);
+    PiecewiseAccelerationTrajectory1d lon_traj(init_s_[0], cruise_v);
     lon_traj.AppendSegment(0.0,
         FLAGS_trajectory_time_length + + FLAGS_lattice_epsilon);
 
@@ -532,7 +532,7 @@ std::vector<double> TrajectoryEvaluator::ComputeLongitudinalGuideVelocity(
   } else {
     double dist_s = planning_target.stop_point().s() - init_s_[0];
     if (dist_s < FLAGS_lattice_epsilon) {
-      ConstantAccelerationTrajectory1d lon_traj(init_s_[0], 0.0);
+      PiecewiseAccelerationTrajectory1d lon_traj(init_s_[0], 0.0);
       lon_traj.AppendSegment(0.0,
           FLAGS_trajectory_time_length + FLAGS_lattice_epsilon);