iterative_anchoring_smoother.cc

浏览该文件的文档.

/******************************************************************************
 * Copyright 2019 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
 
/*
 * @file
 */
 
#include "modules/planning/planning_open_space/trajectory_smoother/iterative_anchoring_smoother.h"
 
#include <algorithm>
#include <limits>
#include <random>
 
#include "modules/planning/planning_open_space/proto/planner_open_space_config.pb.h"
 
#include "cyber/common/log.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/math/math_utils.h"
#include "modules/planning/planning_base/common/util/print_debug_info.h"
#include "modules/planning/planning_base/math/discrete_points_math.h"
#include "modules/planning/planning_base/math/discretized_points_smoothing/fem_pos_deviation_smoother.h"
#include "modules/planning/planning_base/math/piecewise_jerk/piecewise_jerk_speed_problem.h"
 
namespace apollo {
namespace planning {
 
using apollo::common::PathPoint;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Box2d;
using apollo::common::math::LineSegment2d;
using apollo::common::math::NormalizeAngle;
using apollo::common::math::Vec2d;
 
IterativeAnchoringSmoother::IterativeAnchoringSmoother(
    const PlannerOpenSpaceConfig& planner_open_space_config) {
  // TODO(Jinyun, Yu): refactor after stabilized.
  const auto& vehicle_param =
      common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  ego_length_ = vehicle_param.length();
  ego_width_ = vehicle_param.width();
  center_shift_distance_ =
      ego_length_ / 2.0 - vehicle_param.back_edge_to_center();
  planner_open_space_config_ = planner_open_space_config;
  AINFO << "config:" << planner_open_space_config_.DebugString();
}
 
bool IterativeAnchoringSmoother::Smooth(
    const Eigen::MatrixXd& xWS, const double init_a, const double init_v,
    const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,
    DiscretizedTrajectory* discretized_trajectory) {
  if (xWS.cols() < 2) {
    AERROR << "reference points size smaller than two, smoother early "
              "returned";
    return false;
  }
  const auto start_timestamp = std::chrono::system_clock::now();
 
  // Set gear of the trajectory
  gear_ = CheckGear(xWS);
 
  // Set obstacle in form of linesegments
  std::vector<std::vector<LineSegment2d>> obstacles_linesegments_vec;
  for (const auto& obstacle_vertices : obstacles_vertices_vec) {
    size_t vertices_num = obstacle_vertices.size();
    std::vector<LineSegment2d> obstacle_linesegments;
    for (size_t i = 0; i + 1 < vertices_num; ++i) {
      LineSegment2d line_segment =
          LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]);
      obstacle_linesegments.emplace_back(line_segment);
    }
    obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  }
  obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);
 
  // Interpolate the traj
  DiscretizedPath warm_start_path;
  size_t xWS_size = xWS.cols();
  double accumulated_s = 0.0;
  Vec2d last_path_point(xWS(0, 0), xWS(1, 0));
  for (size_t i = 0; i < xWS_size; ++i) {
    Vec2d cur_path_point(xWS(0, i), xWS(1, i));
    accumulated_s += cur_path_point.DistanceTo(last_path_point);
    PathPoint path_point;
    path_point.set_x(xWS(0, i));
    path_point.set_y(xWS(1, i));
    path_point.set_theta(xWS(2, i));
    path_point.set_s(accumulated_s);
    warm_start_path.push_back(std::move(path_point));
    last_path_point = cur_path_point;
  }
 
  const double interpolated_delta_s =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .interpolated_delta_s();
  std::vector<std::pair<double, double>> interpolated_warm_start_point2ds;
  double path_length = warm_start_path.Length();
  double delta_s = path_length / std::ceil(path_length / interpolated_delta_s);
  path_length += delta_s * 1.0e-6;
  for (double s = 0; s < path_length; s += delta_s) {
    const auto point2d = warm_start_path.Evaluate(s);
    interpolated_warm_start_point2ds.emplace_back(point2d.x(), point2d.y());
  }
 
  const size_t interpolated_size = interpolated_warm_start_point2ds.size();
  if (interpolated_size < 4) {
    AERROR << "interpolated_warm_start_path smaller than 4, can't enforce "
              "heading continuity";
    return false;
  } else if (interpolated_size < 6) {
    ADEBUG << "interpolated_warm_start_path smaller than 4, can't enforce "
              "initial zero kappa";
    enforce_initial_kappa_ = false;
  } else {
    enforce_initial_kappa_ = true;
  }
 
  // Adjust heading to ensure heading continuity
  AdjustStartEndHeading(xWS, &interpolated_warm_start_point2ds);
 
  // Reset path profile by discrete point heading and curvature estimation
  DiscretizedPath interpolated_warm_start_path;
  if (!SetPathProfile(interpolated_warm_start_point2ds,
                      &interpolated_warm_start_path)) {
    AERROR << "Set path profile fails";
    return false;
  }
 
  // Generate feasible bounds for each path point
  std::vector<double> bounds;
  if (!GenerateInitialBounds(interpolated_warm_start_path, &bounds)) {
    AERROR << "Generate initial bounds failed, path point to close to obstacle";
    return false;
  }
 
  // Check initial path collision avoidance, if it fails, smoother assumption
  // fails. Try reanchoring
  input_colliding_point_index_.clear();
  if (!CheckCollisionAvoidance(interpolated_warm_start_path,
                               &input_colliding_point_index_)) {
    AERROR << "Interpolated input path points colliding with obstacle";
    // if (!ReAnchoring(colliding_point_index, &interpolated_warm_start_path)) {
    //   AERROR << "Fail to reanchor colliding input path points";
    //   return false;
    // }
  }
 
  const auto path_smooth_start_timestamp = std::chrono::system_clock::now();
 
  // Smooth path to have smoothed x, y, phi, kappa and s
  DiscretizedPath smoothed_path_points;
  if (!SmoothPath(interpolated_warm_start_path, bounds,
                  &smoothed_path_points)) {
    return false;
  }
  // PrintCurves print_curve;
  // for (size_t i = 0; i < smoothed_path_points.size(); i++) {
  //   print_curve.AddPoint("iter_smoothed_xy", smoothed_path_points[i].x(),
  //                        smoothed_path_points[i].y());
  //   print_curve.AddPoint("iter_smoothed_s-kappa",
  //   smoothed_path_points[i].s(),
  //                        smoothed_path_points[i].kappa());
  //   print_curve.AddPoint("iter_smoothed_s-theta",
  //   smoothed_path_points[i].s(),
  //                        smoothed_path_points[i].theta());
  // }
  // for (size_t i = 0; i < interpolated_warm_start_path.size(); i++) {
  //   print_curve.AddPoint("iter_warm_xy", interpolated_warm_start_path[i].x(),
  //                        interpolated_warm_start_path[i].y());
  //   print_curve.AddPoint("iter_warm_s-kappa",
  //                        interpolated_warm_start_path[i].s(),
  //                        interpolated_warm_start_path[i].kappa());
  //   print_curve.AddPoint("iter_warm_s-theta",
  //                        interpolated_warm_start_path[i].s(),
  //                        interpolated_warm_start_path[i].theta());
  // }
  // print_curve.PrintToLog();
  const auto path_smooth_end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> path_smooth_diff =
      path_smooth_end_timestamp - path_smooth_start_timestamp;
  ADEBUG << "iterative anchoring path smoother time: "
         << path_smooth_diff.count() * 1000.0 << " ms.";
 
  const auto speed_smooth_start_timestamp = std::chrono::system_clock::now();
 
  // Smooth speed to have smoothed v and a
  SpeedData smoothed_speeds;
  if (!SmoothSpeed(init_a, init_v, smoothed_path_points.Length(),
                   &smoothed_speeds)) {
    return false;
  }
 
  // TODO(Jinyun): Evaluate performance
  // SpeedData smoothed_speeds;
  // if (!GenerateStopProfileFromPolynomial(
  //         init_a, init_v, smoothed_path_points.Length(), &smoothed_speeds)) {
  //   return false;
  // }
 
  const auto speed_smooth_end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> speed_smooth_diff =
      speed_smooth_end_timestamp - speed_smooth_start_timestamp;
  ADEBUG << "iterative anchoring speed smoother time: "
         << speed_smooth_diff.count() * 1000.0 << " ms.";
 
  // Combine path and speed
  if (!CombinePathAndSpeed(smoothed_path_points, smoothed_speeds,
                           discretized_trajectory)) {
    return false;
  }
 
  AdjustPathAndSpeedByGear(discretized_trajectory);
 
  const auto end_timestamp = std::chrono::system_clock::now();
  std::chrono::duration<double> diff = end_timestamp - start_timestamp;
  ADEBUG << "iterative anchoring smoother total time: " << diff.count() * 1000.0
         << " ms.";
 
  ADEBUG << "discretized_trajectory size " << discretized_trajectory->size();
  return true;
}
 
void IterativeAnchoringSmoother::AdjustStartEndHeading(
    const Eigen::MatrixXd& xWS,
    std::vector<std::pair<double, double>>* const point2d) {
  // Sanity check
  CHECK_NOTNULL(point2d);
  CHECK_GT(xWS.cols(), 1);
  CHECK_GT(point2d->size(), 3U);
 
  // Set initial heading and bounds
  const double initial_heading = xWS(2, 0);
  const double end_heading = xWS(2, xWS.cols() - 1);
 
  // Adjust the point position to have heading by finite element difference of
  // the point and the other point equal to the given warm start initial or end
  // heading
  const double first_to_second_dx = point2d->at(1).first - point2d->at(0).first;
  const double first_to_second_dy =
      point2d->at(1).second - point2d->at(0).second;
  const double first_to_second_s =
      std::sqrt(first_to_second_dx * first_to_second_dx +
                first_to_second_dy * first_to_second_dy);
  Vec2d first_point(point2d->at(0).first, point2d->at(0).second);
  Vec2d initial_vec(first_to_second_s, 0);
  initial_vec.SelfRotate(gear_ ? initial_heading
                               : NormalizeAngle(initial_heading + M_PI));
  initial_vec += first_point;
  point2d->at(1) = std::make_pair(initial_vec.x(), initial_vec.y());
 
  const size_t path_size = point2d->size();
  const double second_last_to_last_dx =
      point2d->at(path_size - 1).first - point2d->at(path_size - 2).first;
  const double second_last_to_last_dy =
      point2d->at(path_size - 1).second - point2d->at(path_size - 2).second;
  const double second_last_to_last_s =
      std::sqrt(second_last_to_last_dx * second_last_to_last_dx +
                second_last_to_last_dy * second_last_to_last_dy);
  Vec2d last_point(point2d->at(path_size - 1).first,
                   point2d->at(path_size - 1).second);
  Vec2d end_vec(second_last_to_last_s, 0);
  end_vec.SelfRotate(gear_ ? NormalizeAngle(end_heading + M_PI) : end_heading);
  end_vec += last_point;
  point2d->at(path_size - 2) = std::make_pair(end_vec.x(), end_vec.y());
}
 
bool IterativeAnchoringSmoother::ReAnchoring(
    const std::vector<size_t>& colliding_point_index,
    DiscretizedPath* path_points) {
  CHECK_NOTNULL(path_points);
  if (colliding_point_index.empty()) {
    ADEBUG << "no point needs to be re-anchored";
    return true;
  }
 
  CHECK_GT(path_points->size(),
           *(std::max_element(colliding_point_index.begin(),
                              colliding_point_index.end())));
 
  for (const auto index : colliding_point_index) {
    if (index == 0 || index == path_points->size() - 1) {
      AERROR << "Initial and end points collision avoid condition failed.";
      return false;
    }
    if (index == 1 || index == path_points->size() - 2) {
      AERROR << "second to last point or second point pos reanchored. Heading "
                "discontinuity might "
                "happen";
    }
  }
 
  // TODO(Jinyun): move to confs
  const size_t reanchoring_trails_num = static_cast<size_t>(
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .reanchoring_trails_num());
  const double reanchoring_pos_stddev =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .reanchoring_pos_stddev();
  const double reanchoring_length_stddev =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .reanchoring_length_stddev();
  std::random_device rd;
  std::default_random_engine gen = std::default_random_engine(rd());
  std::normal_distribution<> pos_dis{0, reanchoring_pos_stddev};
  std::normal_distribution<> length_dis{0, reanchoring_length_stddev};
 
  for (const auto index : colliding_point_index) {
    bool reanchoring_success = false;
    for (size_t i = 0; i < reanchoring_trails_num; ++i) {
      // Get ego box for collision check on collision point index
      bool is_colliding = false;
      for (size_t j = index - 1; j < index + 2; ++j) {
        const double heading =
            gear_ ? path_points->at(j).theta()
                  : NormalizeAngle(path_points->at(j).theta() + M_PI);
        Box2d ego_box({path_points->at(j).x() +
                           center_shift_distance_ * std::cos(heading),
                       path_points->at(j).y() +
                           center_shift_distance_ * std::sin(heading)},
                      heading, ego_length_, ego_width_);
        for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
          for (const LineSegment2d& linesegment : obstacle_linesegments) {
            if (ego_box.HasOverlap(linesegment)) {
              is_colliding = true;
              break;
            }
          }
          if (is_colliding) {
            break;
          }
        }
        if (is_colliding) {
          break;
        }
      }
 
      if (is_colliding) {
        // Adjust the point by randomly move around the original points
        if (index == 1) {
          const double adjust_theta = path_points->at(index - 1).theta();
          const double delta_s = std::abs(path_points->at(index).s() -
                                          path_points->at(index - 1).s());
          double rand_dev = common::math::Clamp(length_dis(gen), 0.8, -0.8);
          double adjusted_delta_s = delta_s * (1.0 + rand_dev);
          path_points->at(index).set_x(path_points->at(index - 1).x() +
                                       adjusted_delta_s *
                                           std::cos(adjust_theta));
          path_points->at(index).set_y(path_points->at(index - 1).y() +
                                       adjusted_delta_s *
                                           std::sin(adjust_theta));
        } else if (index == path_points->size() - 2) {
          const double adjust_theta =
              NormalizeAngle(path_points->at(index + 1).theta() + M_PI);
          const double delta_s = std::abs(path_points->at(index + 1).s() -
                                          path_points->at(index).s());
          double rand_dev = common::math::Clamp(length_dis(gen), 0.8, -0.8);
          double adjusted_delta_s = delta_s * (1.0 + rand_dev);
          path_points->at(index).set_x(path_points->at(index + 1).x() +
                                       adjusted_delta_s *
                                           std::cos(adjust_theta));
          path_points->at(index).set_y(path_points->at(index + 1).y() +
                                       adjusted_delta_s *
                                           std::sin(adjust_theta));
        } else {
          double rand_dev_x =
              common::math::Clamp(pos_dis(gen), 2.0 * reanchoring_pos_stddev,
                                  -2.0 * reanchoring_pos_stddev);
          double rand_dev_y =
              common::math::Clamp(pos_dis(gen), 2.0 * reanchoring_pos_stddev,
                                  -2.0 * reanchoring_pos_stddev);
          path_points->at(index).set_x(path_points->at(index).x() + rand_dev_x);
          path_points->at(index).set_y(path_points->at(index).y() + rand_dev_y);
        }
 
        // Adjust heading accordingly
        // TODO(Jinyun): refactor into math module
        // current point heading adjustment
        for (size_t i = index - 1; i < index + 2; ++i) {
          path_points->at(i).set_theta(CalcHeadings(*path_points, i));
        }
      } else {
        reanchoring_success = true;
        break;
      }
    }
 
    if (!reanchoring_success) {
      AERROR << "interpolated points at index " << index
             << "can't be successfully reanchored";
      return false;
    }
  }
  return true;
}
 
bool IterativeAnchoringSmoother::GenerateInitialBounds(
    const DiscretizedPath& path_points, std::vector<double>* initial_bounds) {
  CHECK_NOTNULL(initial_bounds);
  initial_bounds->clear();
 
  const bool estimate_bound =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .estimate_bound();
  const double default_bound =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .default_bound();
  const double vehicle_shortest_dimension =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .vehicle_shortest_dimension();
  const double kEpislon = 1e-8;
 
  if (!estimate_bound) {
    std::vector<double> default_bounds(path_points.size(), default_bound);
    *initial_bounds = std::move(default_bounds);
    return true;
  }
 
  // TODO(Jinyun): refine obstacle formulation and speed it up
  for (const auto& path_point : path_points) {
    double min_bound = std::numeric_limits<double>::infinity();
    for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
      for (const LineSegment2d& linesegment : obstacle_linesegments) {
        min_bound =
            std::min(min_bound,
                     linesegment.DistanceTo({path_point.x(), path_point.y()}));
      }
    }
    min_bound -= vehicle_shortest_dimension;
    min_bound = min_bound < kEpislon ? 0.0 : min_bound;
    initial_bounds->push_back(min_bound);
  }
  return true;
}
 
bool IterativeAnchoringSmoother::SmoothPath(
    const DiscretizedPath& raw_path_points,
    const std::vector<double>& bounds,
    DiscretizedPath* smoothed_path_points,
    std::vector<std::vector<common::math::Vec2d>> point_box) {
  std::vector<std::pair<double, double>> raw_point2d;
  std::vector<double> flexible_bounds;
  for (const auto& path_point : raw_path_points) {
    raw_point2d.emplace_back(path_point.x(), path_point.y());
  }
  flexible_bounds = bounds;
 
  FemPosDeviationSmoother fem_pos_smoother(
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .fem_pos_deviation_smoother_config());
 
  // TODO(Jinyun): move to confs
  const size_t max_iteration_num = 50;
 
  bool is_collision_free = false;
  std::vector<size_t> colliding_point_index;
  std::vector<std::pair<double, double>> smoothed_point2d;
  size_t counter = 0;
 
  while (!is_collision_free) {
    if (counter > max_iteration_num) {
      AERROR << "Maximum iteration reached, path smoother early stops";
      return true;
    }
 
    AdjustPathBounds(colliding_point_index, &flexible_bounds);
 
    std::vector<double> opt_x;
    std::vector<double> opt_y;
    if (!fem_pos_smoother.Solve(raw_point2d, flexible_bounds, &opt_x, &opt_y)) {
      AERROR << "Smoothing path fails";
      return false;
    }
 
    if (opt_x.size() < 2 || opt_y.size() < 2) {
      AERROR << "Return by fem_pos_smoother is wrong. Size smaller than 2 ";
      return false;
    }
 
    CHECK_EQ(opt_x.size(), opt_y.size());
 
    size_t point_size = opt_x.size();
    smoothed_point2d.clear();
    for (size_t i = 0; i < point_size; ++i) {
      smoothed_point2d.emplace_back(opt_x[i], opt_y[i]);
    }
 
    if (!SetPathProfile(smoothed_point2d, smoothed_path_points)) {
      AERROR << "Set path profile fails";
      return false;
    }
 
    is_collision_free =
        CheckCollisionAvoidance(*smoothed_path_points, &colliding_point_index);
 
    ADEBUG << "loop iteration number is " << counter;
    ++counter;
  }
  return true;
}
 
bool IterativeAnchoringSmoother::CheckCollisionAvoidance(
    const DiscretizedPath& path_points,
    std::vector<size_t>* colliding_point_index) {
  CHECK_NOTNULL(colliding_point_index);
 
  colliding_point_index->clear();
  size_t path_points_size = path_points.size();
  for (size_t i = 0; i < path_points_size; ++i) {
    // Skip checking collision for thoese points colliding originally
    bool skip_checking = false;
    for (const auto index : input_colliding_point_index_) {
      if (i == index) {
        skip_checking = true;
        break;
      }
    }
    if (skip_checking) {
      continue;
    }
 
    const double heading = gear_
                               ? path_points[i].theta()
                               : NormalizeAngle(path_points[i].theta() + M_PI);
    Box2d ego_box(
        {path_points[i].x() + center_shift_distance_ * std::cos(heading),
         path_points[i].y() + center_shift_distance_ * std::sin(heading)},
        heading, ego_length_, ego_width_);
 
    bool is_colliding = false;
    for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
      for (const LineSegment2d& linesegment : obstacle_linesegments) {
        if (ego_box.HasOverlap(linesegment)) {
          colliding_point_index->push_back(i);
          ADEBUG << "point at " << i << "collied with LineSegment "
                 << linesegment.DebugString();
          is_colliding = true;
          break;
        }
      }
      if (is_colliding) {
        break;
      }
    }
  }
 
  if (!colliding_point_index->empty()) {
    return false;
  }
  return true;
}
 
void IterativeAnchoringSmoother::AdjustPathBounds(
    const std::vector<size_t>& colliding_point_index,
    std::vector<double>* bounds) {
  CHECK_NOTNULL(bounds);
 
  const double collision_decrease_ratio =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .collision_decrease_ratio();
 
  for (const auto index : colliding_point_index) {
    bounds->at(index) *= collision_decrease_ratio;
  }
 
  // Anchor the end points to enforce the initial end end heading continuity and
  // zero kappa
  bounds->at(0) = 0.0;
  bounds->at(1) = 0.0;
  bounds->at(bounds->size() - 1) = 0.0;
  bounds->at(bounds->size() - 2) = 0.0;
  if (enforce_initial_kappa_) {
    bounds->at(2) = 0.0;
  }
}
 
bool IterativeAnchoringSmoother::SetPathProfile(
    const std::vector<std::pair<double, double>>& point2d,
    DiscretizedPath* raw_path_points) {
  CHECK_NOTNULL(raw_path_points);
  raw_path_points->clear();
  // Compute path profile
  std::vector<double> headings;
  std::vector<double> kappas;
  std::vector<double> dkappas;
  std::vector<double> accumulated_s;
  if (!DiscretePointsMath::ComputePathProfile(
          point2d, &headings, &accumulated_s, &kappas, &dkappas)) {
    return false;
  }
  CHECK_EQ(point2d.size(), headings.size());
  CHECK_EQ(point2d.size(), kappas.size());
  CHECK_EQ(point2d.size(), dkappas.size());
  CHECK_EQ(point2d.size(), accumulated_s.size());
 
  // Load into path point
  size_t points_size = point2d.size();
  for (size_t i = 0; i < points_size; ++i) {
    PathPoint path_point;
    path_point.set_x(point2d[i].first);
    path_point.set_y(point2d[i].second);
    path_point.set_theta(headings[i]);
    path_point.set_s(accumulated_s[i]);
    path_point.set_kappa(kappas[i]);
    path_point.set_dkappa(dkappas[i]);
    raw_path_points->push_back(std::move(path_point));
  }
  return true;
}
 
bool IterativeAnchoringSmoother::CheckGear(const Eigen::MatrixXd& xWS) {
  CHECK_GT(xWS.size(), 1);
  double init_heading_angle = xWS(2, 0);
  const Vec2d init_tracking_vector(xWS(0, 1) - xWS(0, 0),
                                   xWS(1, 1) - xWS(1, 0));
  double init_tracking_angle = init_tracking_vector.Angle();
  return std::abs(NormalizeAngle(init_tracking_angle - init_heading_angle)) <
         M_PI_2;
}
 
bool IterativeAnchoringSmoother::SmoothSpeed(const double init_a,
                                             const double init_v,
                                             const double path_length,
                                             SpeedData* smoothed_speeds) {
  const double max_forward_v =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .max_forward_v();
  const double max_reverse_v =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .max_reverse_v();
  const double max_forward_acc =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .max_forward_acc();
  const double max_reverse_acc =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .max_reverse_acc();
  const double max_acc_jerk =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .max_acc_jerk();
  const double delta_t =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .delta_t();
 
  const double total_t = 2 * path_length / max_reverse_acc * 10;
  ADEBUG << "total_t is : " << total_t << "path_length" << path_length;
  const size_t num_of_knots = static_cast<size_t>(total_t / delta_t) + 1;
 
  PiecewiseJerkSpeedProblem piecewise_jerk_problem(
      num_of_knots, delta_t, {0.0, std::abs(init_v), 0.0});
 
  auto s_curve_config =
      planner_open_space_config_.iterative_anchoring_smoother_config()
          .s_curve_config();
 
  // set end constraints
  std::vector<std::pair<double, double>> x_bounds(num_of_knots,
                                                  {0.0, path_length});
 
  const double max_v = gear_ ? max_forward_v : max_reverse_v;
  const double max_acc = gear_ ? max_forward_acc : max_reverse_acc;
 
  const auto upper_dx = std::fmax(max_v, std::abs(init_v));
  std::vector<std::pair<double, double>> dx_bounds(num_of_knots,
                                                   {0.0, upper_dx});
  std::vector<std::pair<double, double>> ddx_bounds(num_of_knots,
                                                    {-max_acc, max_acc});
 
  x_bounds[num_of_knots - 1] = std::make_pair(path_length, path_length);
  dx_bounds[num_of_knots - 1] = std::make_pair(0.0, 0.0);
  ddx_bounds[num_of_knots - 1] = std::make_pair(0.0, 0.0);
 
  std::vector<double> x_ref(num_of_knots, path_length);
  piecewise_jerk_problem.set_x_ref(s_curve_config.ref_s_weight(),
                                   std::move(x_ref));
  piecewise_jerk_problem.set_weight_ddx(s_curve_config.acc_weight());
  piecewise_jerk_problem.set_weight_dddx(s_curve_config.jerk_weight());
  piecewise_jerk_problem.set_x_bounds(x_bounds);
  piecewise_jerk_problem.set_dx_bounds(dx_bounds);
  piecewise_jerk_problem.set_ddx_bounds(ddx_bounds);
  piecewise_jerk_problem.set_dddx_bound(max_acc_jerk);
 
  // Solve the problem
  if (!piecewise_jerk_problem.Optimize()) {
    AERROR << "Piecewise jerk speed optimizer failed!";
    AERROR << "init v" << init_v << ",a" << init_a << "jerk bound"
           << max_acc_jerk;
    AERROR << "xbounds; dxbounds;ddxbounds";
    for (size_t i = 0; i < num_of_knots; i++) {
      AERROR << x_bounds[i].first << ", " << x_bounds[i].second << ";"
             << dx_bounds[i].first << "," << dx_bounds[i].second << ";"
             << ddx_bounds[i].first << "," << ddx_bounds[i].second;
    }
    return false;
  }
 
  // Extract output
  const std::vector<double>& s = piecewise_jerk_problem.opt_x();
  const std::vector<double>& ds = piecewise_jerk_problem.opt_dx();
  const std::vector<double>& dds = piecewise_jerk_problem.opt_ddx();
 
  // Assign speed point by gear
  smoothed_speeds->AppendSpeedPoint(s[0], 0.0, ds[0], dds[0], 0.0);
  const double kEpislon = 1.0e-4;
  const double sEpislon = 1.0e-4;
  for (size_t i = 1; i < num_of_knots; ++i) {
    if (s[i - 1] - s[i] > kEpislon) {
      AERROR << "unexpected decreasing s in speed smoothing at time "
             << static_cast<double>(i) * delta_t << "with total time "
             << total_t;
      return false;
    }
    smoothed_speeds->AppendSpeedPoint(s[i], delta_t * static_cast<double>(i),
                                      ds[i], dds[i],
                                      (dds[i] - dds[i - 1]) / delta_t);
    // Cut the speed data when it is about to meet end condition
    if (path_length - s[i] < sEpislon) {
      break;
    }
  }
  return true;
}
 
bool IterativeAnchoringSmoother::CombinePathAndSpeed(
    const DiscretizedPath& path_points, const SpeedData& speed_points,
    DiscretizedTrajectory* discretized_trajectory) {
  CHECK_NOTNULL(discretized_trajectory);
  discretized_trajectory->clear();
  // TODO(Jinyun): move to confs
  const double kDenseTimeResolution = 0.1;
  const double time_horizon =
      speed_points.TotalTime() + kDenseTimeResolution * 1.0e-6;
  if (path_points.empty()) {
    AERROR << "path data is empty";
    return false;
  }
  ADEBUG << "speed_points.TotalTime() " << speed_points.TotalTime();
  for (double cur_rel_time = 0.0; cur_rel_time < time_horizon;
       cur_rel_time += kDenseTimeResolution) {
    common::SpeedPoint speed_point;
    if (!speed_points.EvaluateByTime(cur_rel_time, &speed_point)) {
      AERROR << "Fail to get speed point with relative time " << cur_rel_time;
      return false;
    }
 
    if (speed_point.s() > path_points.Length()) {
      break;
    }
 
    common::PathPoint path_point = path_points.Evaluate(speed_point.s());
 
    common::TrajectoryPoint trajectory_point;
    trajectory_point.mutable_path_point()->CopyFrom(path_point);
    trajectory_point.set_v(speed_point.v());
    trajectory_point.set_a(speed_point.a());
    trajectory_point.set_relative_time(speed_point.t());
    discretized_trajectory->AppendTrajectoryPoint(trajectory_point);
  }
  ADEBUG << "path length before combine " << path_points.Length();
  ADEBUG << "trajectory length after combine "
         << discretized_trajectory->GetSpatialLength();
  return true;
}
 
void IterativeAnchoringSmoother::AdjustPathAndSpeedByGear(
    DiscretizedTrajectory* discretized_trajectory) {
  if (gear_) {
    return;
  }
  std::for_each(
      discretized_trajectory->begin(), discretized_trajectory->end(),
      [](TrajectoryPoint& trajectory_point) {
        trajectory_point.mutable_path_point()->set_theta(
            NormalizeAngle(trajectory_point.path_point().theta() + M_PI));
        trajectory_point.mutable_path_point()->set_s(
            -1.0 * trajectory_point.path_point().s());
        trajectory_point.mutable_path_point()->set_kappa(
            -1.0 * trajectory_point.path_point().kappa());
        // dkappa stays the same as direction of both kappa and s are reversed
        trajectory_point.set_v(-1.0 * trajectory_point.v());
        trajectory_point.set_a(-1.0 * trajectory_point.a());
      });
}
 
bool IterativeAnchoringSmoother::GenerateStopProfileFromPolynomial(
    const double init_acc, const double init_speed, const double stop_distance,
    SpeedData* smoothed_speeds) {
  static constexpr double kMaxT = 8.0;
  static constexpr double kUnitT = 0.2;
  for (double t = 2.0; t <= kMaxT; t += kUnitT) {
    QuinticPolynomialCurve1d curve(0.0, init_speed, init_acc, stop_distance,
                                   0.0, 0.0, t);
    if (!IsValidPolynomialProfile(curve)) {
      continue;
    }
    for (double curve_t = 0.0; curve_t <= t; curve_t += kUnitT) {
      const double curve_s = curve.Evaluate(0, curve_t);
      const double curve_v = curve.Evaluate(1, curve_t);
      const double curve_a = curve.Evaluate(2, curve_t);
      const double curve_da = curve.Evaluate(3, curve_t);
      smoothed_speeds->AppendSpeedPoint(curve_s, curve_t, curve_v, curve_a,
                                        curve_da);
    }
    return true;
  }
  AERROR << "GenerateStopProfileFromPolynomial fails.";
  return false;
}
 
bool IterativeAnchoringSmoother::IsValidPolynomialProfile(
    const QuinticPolynomialCurve1d& curve) {
  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);
    static constexpr double kEpsilon = 1e-3;
    if (v < -kEpsilon || a > 1.0) {
      return false;
    }
  }
  return true;
}
 
double IterativeAnchoringSmoother::CalcHeadings(
    const DiscretizedPath& path_points, const size_t index) {
  CHECK_GT(path_points.size(), 2U);
  double dx = 0.0;
  double dy = 0.0;
  if (index == 0) {
    dx = path_points[index + 1].x() - path_points[index].x();
    dy = path_points[index + 1].y() - path_points[index].y();
  } else if (index == path_points.size() - 1) {
    dx = path_points[index].x() - path_points[index - 1].x();
    dy = path_points[index].y() - path_points[index - 1].y();
  } else {
    dx = 0.5 * (path_points[index + 1].x() - path_points[index - 1].x());
    dy = 0.5 * (path_points[index + 1].y() - path_points[index - 1].y());
  }
  return std::atan2(dy, dx);
}
}  // namespace planning
}  // namespace apollo