bool SpeedBoundsDecider::Init(
    const std::string &config_dir, const std::string &name,
    const std::shared_ptr<DependencyInjector> &injector) {
  if (!Decider::Init(config_dir, name, injector)) {
    return false;
  }
  AINFO << "config_dir:" << config_dir << " " << "name:" << name;
  // Load the config this task.
  return Decider::LoadConfig<SpeedBoundsDeciderConfig>(&config_);
}

 STBoundaryMapper boundary_mapper(config_, reference_line, path_data,
path_data.discretized_path().Length(),
config_.total_time(), injector_);

  if (!FLAGS_use_st_drivable_boundary) {
    path_decision->EraseStBoundaries();
  }

Status STBoundaryMapper::ComputeSTBoundary(PathDecision* path_decision)

    if (!ptr_obstacle->HasLongitudinalDecision()) {
      ComputeSTBoundary(ptr_obstacle);
      continue;
    }

const auto* planning_status = injector_->planning_context()
                                    ->mutable_planning_status()
                                    ->mutable_change_lane();

  double l_buffer =
      planning_status->status() == ChangeLaneStatus::IN_CHANGE_LANE
          ? speed_bounds_config_.lane_change_obstacle_nudge_l_buffer()
          : FLAGS_nonstatic_obstacle_nudge_l_buffer;

if (trajectory.trajectory_point().empty()) {
    bool box_check_collision = false;
    if (!obstacle.IsStatic()) {
      AWARN << "Non-static obstacle[" << obstacle.Id()
            << "] has NO prediction trajectory."
            << obstacle.Perception().ShortDebugString();
    }

const Box2d& obs_box = obstacle.PerceptionBoundingBox();

perception_bounding_box_({perception_obstacle_.position().x(),
                                perception_obstacle_.position().y()},
                               perception_obstacle_.theta(),
                               perception_obstacle_.length(),
                               perception_obstacle_.width())

    for (const auto& curr_point_on_path : path_points) {
      if (curr_point_on_path.s() > planning_max_distance_) {
        break;
      }
      if (CheckOverlap(curr_point_on_path, obs_box, l_buffer)) {
        box_check_collision = true;
        break;
      }
    }

Vec2d ego_center_map_frame((vehicle_param_.front_edge_to_center() -
                              vehicle_param_.back_edge_to_center()) *
                                 0.5,
                             (vehicle_param_.left_edge_to_center() -
                              vehicle_param_.right_edge_to_center()) *
                                 0.5);

void Vec2d::SelfRotate(const double angle) {
  double tmp_x = x_;
  x_ = x_ * cos(angle) - y_ * sin(angle);
  y_ = tmp_x * sin(angle) + y_ * cos(angle);
}

ego_center_map_frame.set_x(ego_center_map_frame.x() + path_point.x());
ego_center_map_frame.set_y(ego_center_map_frame.y() + path_point.y());

Box2d adc_box(ego_center_map_frame, path_point.theta(),
                vehicle_param_.length(), vehicle_param_.width() + l_buffer * 2);

obs_box.HasOverlap(adc_box);

if (box_check_collision) {
      // const double backward_distance =
      // -vehicle_param_.front_edge_to_center();
      const double backward_distance = 0;
      const double forward_distance = obs_box.length();

      for (const auto& curr_point_on_path : path_points) {
        if (curr_point_on_path.s() > planning_max_distance_) {
          break;
        }
        const Polygon2d& obs_polygon = obstacle.PerceptionPolygon();

Polygon2d ego_collision_polygon;
        if (CheckOverlap(curr_point_on_path, obs_polygon, l_buffer, &ego_collision_polygon))

Polygon2d adc_polygon(adc_box);
  if (obs_polygon.HasOverlap(adc_polygon)) {
    if (collision_ego_polygon != nullptr) {
      *collision_ego_polygon = adc_polygon;
    }
    return true;
  }

Polygon2d::Polygon2d(const Box2d &box) {
  box.GetAllCorners(&points_);
  BuildFromPoints();
}

void Polygon2d::BuildFromPoints(bool check_area) {
  num_points_ = static_cast<int>(points_.size());
  CHECK_GE(num_points_, 3);

  // Make sure the points are in ccw order.
  area_ = 0.0;
  for (int i = 1; i < num_points_; ++i) {
    area_ += CrossProd(points_[0], points_[i - 1], points_[i]);
  }

if (area_ < 0) {
    area_ = -area_;
    std::reverse(points_.begin(), points_.end());
  }

area_ /= 2.0;

  line_segments_.reserve(num_points_);
  for (int i = 0; i < num_points_; ++i) {
    line_segments_.emplace_back(points_[i], points_[Next(i)]);
  }

is_convex_ = true;
  for (int i = 0; i < num_points_; ++i) {
    if (CrossProd(points_[Prev(i)], points_[i], points_[Next(i)]) <=
        -kMathEpsilon) {
      is_convex_ = false;
      break;
    }
  }

  min_x_ = points_[0].x();
  max_x_ = points_[0].x();
  min_y_ = points_[0].y();
  max_y_ = points_[0].y();
  for (const auto &point : points_) {
    min_x_ = std::min(min_x_, point.x());
    max_x_ = std::max(max_x_, point.x());
    min_y_ = std::min(min_y_, point.y());
    max_y_ = std::max(max_y_, point.y());
  }

Polygon2d adc_polygon(adc_box);
  if (obs_polygon.HasOverlap(adc_polygon)) {
    if (collision_ego_polygon != nullptr) {
      *collision_ego_polygon = adc_polygon;
    }
    return true;
  }

  if (polygon.max_x() < min_x() || polygon.min_x() > max_x() ||
      polygon.max_y() < min_y() || polygon.min_y() > max_y()) {
    return false;
  }

bool Polygon2d::IsPointOnBoundary(const Vec2d &point) const {
  CHECK_GE(points_.size(), 3U);
  return std::any_of(
      line_segments_.begin(), line_segments_.end(),
      [&](const LineSegment2d &poly_seg) { return poly_seg.IsPointIn(point); });
}

  if (length_ <= kMathEpsilon) {
    return std::abs(point.x() - start_.x()) <= kMathEpsilon &&
           std::abs(point.y() - start_.y()) <= kMathEpsilon;
  }

const double prod = CrossProd(point, start_, end_);
  if (std::abs(prod) > kMathEpsilon) {
    return false;
  }

  return IsWithin(point.x(), start_.x(), end_.x()) &&
         IsWithin(point.y(), start_.y(), end_.y());

bool IsWithin(double val, double bound1, double bound2) {
  if (bound1 > bound2) {
    std::swap(bound1, bound2);
  }
  return val >= bound1 - kMathEpsilon && val <= bound2 + kMathEpsilon;
}

  int j = num_points_ - 1;
  int c = 0;
  for (int i = 0; i < num_points_; ++i) {
    if ((points_[i].y() > point.y()) != (points_[j].y() > point.y())) {
      const double side = CrossProd(point, points_[i], points_[j]);
      if (points_[i].y() < points_[j].y() ? side > 0.0 : side < 0.0) {
        ++c;
      }
    }
    j = i;
  }
  return c & 1;

if (points_[i].y() < points_[j].y() ? side > 0.0 : side < 0.0) {
        ++c;
      }

return c & 1;

  for (int i = 0; i < polygon.num_points(); ++i) {
    if (HasOverlap(polygon.line_segments()[i])) {
      return true;
    }
  }

  if ((line_segment.start().x() < min_x_ && line_segment.end().x() < min_x_) ||
      (line_segment.start().x() > max_x_ && line_segment.end().x() > max_x_) ||
      (line_segment.start().y() < min_y_ && line_segment.end().y() < min_y_) ||
      (line_segment.start().y() > max_y_ && line_segment.end().y() > max_y_)) {
    return false;
  }

  if (std::any_of(line_segments_.begin(), line_segments_.end(),
                  [&](const LineSegment2d &poly_seg) {
                    return poly_seg.HasIntersect(line_segment);
                  })) {
    return true;
  }

bool LineSegment2d::HasIntersect(const LineSegment2d &other_segment) const {
  Vec2d point;
  return GetIntersect(other_segment, &point);
}

if (IsPointIn(other_segment.start())) {
    *point = other_segment.start();
    return true;
  }
  if (IsPointIn(other_segment.end())) {
    *point = other_segment.end();
    return true;
  }

if (other_segment.IsPointIn(start_)) {
    *point = start_;
    return true;
  }
  if (other_segment.IsPointIn(end_)) {
    *point = end_;
    return true;
  }

const double cc1 = CrossProd(start_, end_, other_segment.start());
  const double cc2 = CrossProd(start_, end_, other_segment.end());
  if (cc1 * cc2 >= -kMathEpsilon) {
    return false;
  }

  const double cc3 =
      CrossProd(other_segment.start(), other_segment.end(), start_);
  const double cc4 =
      CrossProd(other_segment.start(), other_segment.end(), end_);
  if (cc3 * cc4 >= -kMathEpsilon) {
    return false;
  }

const double ratio = cc4 / (cc4 - cc3);
  *point = Vec2d(start_.x() * ratio + end_.x() * (1.0 - ratio),
                 start_.y() * ratio + end_.y() * (1.0 - ratio));

double low_s = std::fmax(0.0, curr_point_on_path.s() + backward_distance);
double high_s = std::fmin(planning_max_distance_, curr_point_on_path.s() + forward_distance);

lower_points->emplace_back(low_s - speed_bounds_config_.point_extension(), 0.0);
          lower_points->emplace_back(low_s - speed_bounds_config_.point_extension(), planning_max_time_);
          upper_points->emplace_back(high_s + speed_bounds_config_.point_extension(), 0.0);
          upper_points->emplace_back(high_s + speed_bounds_config_.point_extension(), planning_max_time_);

    const int default_num_point = 50;
    DiscretizedPath discretized_path;
    if (path_points.size() > 2 * default_num_point) {
      const auto ratio = path_points.size() / default_num_point;
      std::vector<PathPoint> sampled_path_points;
      for (size_t i = 0; i < path_points.size(); ++i) {
        if (i % ratio == 0) {
          sampled_path_points.push_back(path_points[i]);
        }
      }
      discretized_path = DiscretizedPath(std::move(sampled_path_points));
    } else {
      discretized_path = DiscretizedPath(path_points);
    }

double trajectory_time_interval =
        obstacle.Trajectory().trajectory_point()[1].relative_time();

int trajectory_step = std::min(FLAGS_trajectory_check_collision_time_step,
                      std::max(vehicle_param_.width() / obstacle.speed() /
                      trajectory_time_interval,
                      1.0));
bool trajectory_point_collision_status = false;
int previous_index = 0;

for (int i = 0; i < trajectory.trajectory_point_size();
         i = std::min(i + trajectory_step,
                      trajectory.trajectory_point_size() - 1)) {
      const auto& trajectory_point = trajectory.trajectory_point(i);
      Polygon2d obstacle_shape =
          obstacle.GetObstacleTrajectoryPolygon(trajectory_point);

common::math::Polygon2d Obstacle::GetObstacleTrajectoryPolygon(
    const common::TrajectoryPoint& point) const {
  double delta_heading =
      point.path_point().theta() - perception_obstacle_.theta();

double cos_delta_heading = cos(delta_heading);
  double sin_delta_heading = sin(delta_heading);
  std::vector<common::math::Vec2d> polygon_point;
  polygon_point.reserve(perception_polygon_.points().size());

  for (auto& iter : perception_polygon_.points()) {
    double relative_x = iter.x() - perception_obstacle_.position().x();
    double relative_y = iter.y() - perception_obstacle_.position().y();

double x = relative_x * cos_delta_heading - relative_y * sin_delta_heading +
               point.path_point().x();
    double y = relative_x * sin_delta_heading + relative_y * cos_delta_heading +
               point.path_point().y();

common::math::Polygon2d trajectory_point_polygon(polygon_point);

      bool collision = CheckOverlapWithTrajectoryPoint(
          discretized_path, obstacle_shape, upper_points, lower_points,
          l_buffer, default_num_point, obstacle_length, obstacle_width,
          trajectory_point_time);

const double step_length = vehicle_param_.front_edge_to_center();
  auto path_len = std::min(speed_bounds_config_.max_trajectory_len(),
                           discretized_path.Length());
  // Go through every point of the ADC's path.
  for (double path_s = 0.0; path_s < path_len; path_s += step_length) 

const auto curr_adc_path_point =
        discretized_path.Evaluate(path_s + discretized_path.front().s());

PathPoint DiscretizedPath::Evaluate(const double path_s) const {
  ACHECK(!empty());
  auto it_lower = QueryLowerBound(path_s);
  if (it_lower == begin()) {
    return front();
  }
  if (it_lower == end()) {
    return back();
  }
  return common::math::InterpolateUsingLinearApproximation(*(it_lower - 1),
                                                           *it_lower, path_s);
}

std::vector<PathPoint>::const_iterator DiscretizedPath::QueryLowerBound(
    const double path_s) const {
  auto func = [](const PathPoint &tp, const double path_s) {
    return tp.s() < path_s;
  };
  return std::lower_bound(begin(), end(), path_s, func);
}

  double s0 = p0.s();
  double s1 = p1.s();

PathPoint path_point;
  double weight = (s - s0) / (s1 - s0);
  double x = (1 - weight) * p0.x() + weight * p1.x();
  double y = (1 - weight) * p0.y() + weight * p1.y();

double theta = slerp(p0.theta(), p0.s(), p1.theta(), p1.s(), s);

  if (std::abs(t1 - t0) <= kMathEpsilon) {
    ADEBUG << "input time difference is too small";
    return NormalizeAngle(a0);
  }

const double a0_n = NormalizeAngle(a0);
  const double a1_n = NormalizeAngle(a1);
  double d = a1_n - a0_n;

  if (d > M_PI) {
    d = d - 2 * M_PI;
  } else if (d < -M_PI) {
    d = d + 2 * M_PI;
  }

const double r = (t - t0) / (t1 - t0);
  const double a = a0_n + d * r;
  return NormalizeAngle(a);

double theta = slerp(p0.theta(), p0.s(), p1.theta(), p1.s(), s);

  double kappa = (1 - weight) * p0.kappa() + weight * p1.kappa();
  double dkappa = (1 - weight) * p0.dkappa() + weight * p1.dkappa();
  double ddkappa = (1 - weight) * p0.ddkappa() + weight * p1.ddkappa();

  path_point.set_x(x);
  path_point.set_y(y);
  path_point.set_theta(theta);
  path_point.set_kappa(kappa);
  path_point.set_dkappa(dkappa);
  path_point.set_ddkappa(ddkappa);
  path_point.set_s(s);
  return path_point;

if (CheckOverlap(curr_adc_path_point, obstacle_shape, l_buffer))
const double backward_distance = 0.0;
      const double forward_distance = vehicle_param_.length() +
                                      vehicle_param_.width() + obstacle_length +
                                      obstacle_width;
      const double default_min_step = 0.1;  // in meters
      const double fine_tuning_step_length = std::fmin(
          default_min_step, discretized_path.Length() / default_num_point);

      bool find_low = false;
      bool find_high = false;
      double low_s = std::fmax(0.0, path_s + backward_distance);
      double high_s =
          std::fmin(discretized_path.Length(), path_s + forward_distance);

      while (low_s < high_s) {
        if (find_low && find_high) {
          break;
        }
        if (!find_low) {
          const auto& point_low =
              discretized_path.Evaluate(low_s + discretized_path.front().s());
          if (!CheckOverlap(point_low, obstacle_shape, l_buffer)) {
            low_s += fine_tuning_step_length;
          } else {
            find_low = true;
          }
        }
        if (!find_high) {
          const auto& point_high =
              discretized_path.Evaluate(high_s + discretized_path.front().s());
          if (!CheckOverlap(point_high, obstacle_shape, l_buffer)) {
            high_s -= fine_tuning_step_length;
          } else {
            find_high = true;
          }
        }
      }

 if (find_high && find_low) {
   lower_points->emplace_back(
       low_s - speed_bounds_config_.point_extension(),
       trajectory_point_time);
   upper_points->emplace_back(
       high_s + speed_bounds_config_.point_extension(),
       trajectory_point_time);
 }

      if ((trajectory_point_collision_status ^ collision) && i != 0) {
        // Start retracing track points forward
        int index = i - 1;
        while ((trajectory_point_collision_status ^ collision) &&
               index > previous_index) {
          const auto& point = trajectory.trajectory_point(index);
          trajectory_point_time = point.relative_time();
          obstacle_shape = obstacle.GetObstacleTrajectoryPolygon(point);
          collision = CheckOverlapWithTrajectoryPoint(
              discretized_path, obstacle_shape, upper_points, lower_points,
              l_buffer, default_num_point, obstacle_length, obstacle_width,
              trajectory_point_time);
          index--;
        }
        trajectory_point_collision_status = !trajectory_point_collision_status;
      }

  std::sort(lower_points->begin(), lower_points->end(),
            [](const STPoint& a, const STPoint& b) { return a.t() < b.t(); });
  std::sort(upper_points->begin(), upper_points->end(),
            [](const STPoint& a, const STPoint& b) { return a.t() < b.t(); });

  auto boundary = STBoundary::CreateInstance(lower_points, upper_points);

boundary.set_id(obstacle->Id());

const auto& prev_st_boundary = obstacle->path_st_boundary();
const auto& ref_line_st_boundary = obstacle->reference_line_st_boundary();

if (!prev_st_boundary.IsEmpty()) {
  boundary.SetBoundaryType(prev_st_boundary.boundary_type());
} else if (!ref_line_st_boundary.IsEmpty()) {
  boundary.SetBoundaryType(ref_line_st_boundary.boundary_type());
}

obstacle->set_path_st_boundary(boundary);

const auto& decision = ptr_obstacle->LongitudinalDecision();
    if (decision.has_stop()) {
      // 1. Store the closest stop fence info.
      // TODO(all): store ref. s value in stop decision; refine the code then.
      common::SLPoint stop_sl_point;
      reference_line_.XYToSL(decision.stop().stop_point(), &stop_sl_point);
      const double stop_s = stop_sl_point.s();

      if (stop_s < min_stop_s) {
        stop_obstacle = ptr_obstacle;
        min_stop_s = stop_s;
        stop_decision = decision;
      }
    }

else if (decision.has_follow() || decision.has_overtake() ||
               decision.has_yield()) {
      // 2. Depending on the longitudinal overtake/yield decision,
      //    fine-tune the upper/lower st-boundary of related obstacles.
      ComputeSTBoundaryWithDecision(ptr_obstacle, decision);
    }

if (FLAGS_use_st_drivable_boundary &&
      obstacle->is_path_st_boundary_initialized()) {
    const auto& path_st_boundary = obstacle->path_st_boundary();
    lower_points = path_st_boundary.lower_points();
    upper_points = path_st_boundary.upper_points();
  } 

const auto& path_st_boundary = obstacle->path_st_boundary();
lower_points = path_st_boundary.lower_points();
upper_points = path_st_boundary.upper_points();

else {
   if (!GetOverlapBoundaryPoints(path_data_.discretized_path(), *obstacle,
                                 &upper_points, &lower_points)) 
   {
     return;
   }
  }

auto boundary = STBoundary::CreateInstance(lower_points, upper_points);

STBoundary STBoundary::CreateInstance(
    const std::vector<STPoint>& lower_points,
    const std::vector<STPoint>& upper_points) {
  if (lower_points.size() != upper_points.size() || lower_points.size() < 2) {
    return STBoundary();
  }

std::vector<std::pair<STPoint, STPoint>> point_pairs;
for (size_t i = 0; i < lower_points.size(); ++i) {
  point_pairs.emplace_back(
      STPoint(lower_points.at(i).s(), lower_points.at(i).t()),
      STPoint(upper_points.at(i).s(), upper_points.at(i).t()));
}

return STBoundary(point_pairs);

STBoundary::STBoundary(
    const std::vector<std::pair<STPoint, STPoint>>& point_pairs,
    bool is_accurate_boundary) {

void STBoundary::RemoveRedundantPoints(
    std::vector<std::pair<STPoint, STPoint>>* point_pairs)

 while (i < point_pairs->size() && j + 1 < point_pairs->size()) {

LineSegment2d lower_seg(point_pairs->at(i).first,
                            point_pairs->at(j + 1).first);

LineSegment2d::LineSegment2d(const Vec2d &start, const Vec2d &end)
    : start_(start), end_(end) {

const double dx = end_.x() - start_.x();
const double dy = end_.y() - start_.y();

length_ = hypot(dx, dy);

unit_direction_ =
      (length_ <= kMathEpsilon ? Vec2d(0, 0)
                               : Vec2d(dx / length_, dy / length_));

heading_ = unit_direction_.Angle();

double Vec2d::Angle() const { return std::atan2(y_, x_); }

LineSegment2d upper_seg(point_pairs->at(i).second,
                            point_pairs->at(j + 1).second);

IsPointNear(lower_seg, point_pairs->at(j).first, kMaxDist)

bool STBoundary::IsPointNear(const common::math::LineSegment2d& seg,
                             const Vec2d& point, const double max_dist) {
  return seg.DistanceSquareTo(point) < max_dist * max_dist;
}

double LineSegment2d::DistanceSquareTo(const Vec2d &point) const {

  if (length_ <= kMathEpsilon) {
    return point.DistanceSquareTo(start_);
  }

const double x0 = point.x() - start_.x();
const double y0 = point.y() - start_.y();
const double proj = x0 * unit_direction_.x() + y0 * unit_direction_.y();

  if (proj <= 0.0) {
    return Square(x0) + Square(y0);
  }

  if (proj >= length_) {
    return point.DistanceSquareTo(end_);
  }

return Square(x0 * unit_direction_.y() - y0 * unit_direction_.x());

IsPointNear(upper_seg, point_pairs->at(j).second, kMaxDist)

    if (!IsPointNear(lower_seg, point_pairs->at(j).first, kMaxDist) ||
        !IsPointNear(upper_seg, point_pairs->at(j).second, kMaxDist)) {
      ++i;
      if (i != j) {
        point_pairs->at(i) = point_pairs->at(j);
      }
    }
    ++j;

point_pairs->at(++i) = point_pairs->back();
point_pairs->resize(i + 1);

  for (const auto& item : reduced_pairs) {
    // use same t for both points
    const double t = item.first.t();
    lower_points_.emplace_back(item.first.s(), t);
    upper_points_.emplace_back(item.second.s(), t);
  }

  for (const auto& point : lower_points_) {
    points_.emplace_back(point.t(), point.s());
  }

  for (auto rit = upper_points_.rbegin(); rit != upper_points_.rend(); ++rit) {
    points_.emplace_back(rit->t(), rit->s());
  }

  for (const auto& point : lower_points_) {
    min_s_ = std::fmin(min_s_, point.s());
  }

for (const auto& point : upper_points_) {
  max_s_ = std::fmax(max_s_, point.s());
}

min_t_ = lower_points_.front().t();
max_t_ = lower_points_.back().t();

STBoundary::CreateInstance(lower_points, upper_points)
                   .ExpandByS(characteristic_length)

  boundary.SetBoundaryType(b_type);
  boundary.set_id(obstacle->Id());
  boundary.SetCharacteristicLength(characteristic_length);
  obstacle->set_path_st_boundary(boundary);

  if (stop_obstacle) {
    bool success = MapStopDecision(stop_obstacle, stop_decision);
    if (!success) {
      const std::string msg = "Fail to MapStopDecision.";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }

common::SLPoint stop_sl_point;
reference_line_.XYToSL(stop_decision.stop().stop_point(), &stop_sl_point);

const double stop_ref_s =
      stop_sl_point.s() - vehicle_param_.front_edge_to_center();

if (stop_ref_s > path_data_.frenet_frame_path().back().s()) {
    st_stop_s = path_data_.discretized_path().back().s() +
                (stop_ref_s - path_data_.frenet_frame_path().back().s());
  }

  for (uint32_t i = 0; i + 1 < frenet_path_.size(); ++i) {
    if (fabs(ref_s - frenet_path_.at(i).s()) < kDistanceEpsilon) {
      path_point->CopyFrom(discretized_path_.at(i));
      return true;
    }
    if (frenet_path_.at(i).s() < ref_s && ref_s <= frenet_path_.at(i + 1).s()) {
      index = i;
      break;
    }
  }

double r = (ref_s - frenet_path_.at(index).s()) /
             (frenet_path_.at(index + 1).s() - frenet_path_.at(index).s());
 const double discretized_path_s = discretized_path_.at(index).s() +
                                    r * (discretized_path_.at(index + 1).s() -
                                         discretized_path_.at(index).s());
path_point->CopyFrom(discretized_path_.Evaluate(discretized_path_s));

st_stop_s = stop_point.s();

 const double s_min = std::fmax(0.0, st_stop_s);
  const double s_max = std::fmax(
      s_min, std::fmax(planning_max_distance_, reference_line_.Length()));

  point_pairs.emplace_back(STPoint(s_min, 0.0), STPoint(s_max, 0.0));
  point_pairs.emplace_back(
      STPoint(s_min, planning_max_time_),
      STPoint(s_max + speed_bounds_config_.boundary_buffer(),
              planning_max_time_));
  auto boundary = STBoundary(point_pairs);

  boundary.SetBoundaryType(STBoundary::BoundaryType::STOP);
  boundary.SetCharacteristicLength(speed_bounds_config_.boundary_buffer());
  boundary.set_id(stop_obstacle->Id());
  stop_obstacle->set_path_st_boundary(boundary);

  std::vector<const STBoundary *> boundaries;
  for (auto *obstacle : path_decision->obstacles().Items()) {
    const auto &id = obstacle->Id();
    const auto &st_boundary = obstacle->path_st_boundary();
    if (!st_boundary.IsEmpty()) {
      if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
        path_decision->Find(id)->SetBlockingObstacle(false);
      } else {
        path_decision->Find(id)->SetBlockingObstacle(true);
      }
      st_boundary.PrintDebug("_obs_st_bounds");
      boundaries.push_back(&st_boundary);
    }
  }

const double min_s_on_st_boundaries = SetSpeedFallbackDistance(path_decision);

double SpeedBoundsDecider::SetSpeedFallbackDistance(
    PathDecision *const path_decision) {

for (auto *obstacle : path_decision->obstacles().Items()) {
    const auto &st_boundary = obstacle->path_st_boundary();

    if (st_boundary.IsEmpty()) {
      continue;
    }

const auto left_bottom_point_s = st_boundary.bottom_left_point().s();
const auto right_bottom_point_s = st_boundary.bottom_right_point().s();
const auto lowest_s = std::min(left_bottom_point_s, right_bottom_point_s);

if (left_bottom_point_s - right_bottom_point_s > kEpsilon) {
      if (min_s_reverse > lowest_s) {
        min_s_reverse = lowest_s;
      }
    }

else if (min_s_non_reverse > lowest_s) {
      min_s_non_reverse = lowest_s;
    }

in_s_reverse = std::max(min_s_reverse, 0.0);
min_s_non_reverse = std::max(min_s_non_reverse, 0.0);
return min_s_non_reverse > min_s_reverse ? 0.0 : min_s_non_reverse;

SpeedLimitDecider speed_limit_decider(config_, reference_line, path_data);

SpeedLimit speed_limit;
  if (!speed_limit_decider
      .GetSpeedLimits(path_decision->obstacles(), &speed_limit)
      .ok())

Status SpeedLimitDecider::GetSpeedLimits(
    const IndexedList<std::string, Obstacle>& obstacles,
    SpeedLimit* const speed_limit_data) const {
  CHECK_NOTNULL(speed_limit_data);

  const auto& discretized_path = path_data_.discretized_path();
  const auto& frenet_path = path_data_.frenet_frame_path();

for (uint32_t i = 0; i < discretized_path.size(); ++i) {
    const double path_s = discretized_path.at(i).s();
    const double reference_line_s = frenet_path.at(i).s();

double speed_limit_from_reference_line =
        reference_line_.GetSpeedLimitFromS(reference_line_s);

double ReferenceLine::GetSpeedLimitFromS(const double s) const {
  for (const auto& speed_limit : speed_limit_) {
    if (s >= speed_limit.start_s && s <= speed_limit.end_s) {
      return speed_limit.speed_limit;
    }
  }

const auto& map_path_point = GetReferencePoint(s);

  double speed_limit = FLAGS_planning_upper_speed_limit;
  bool speed_limit_found = false;
  for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
    if (lane_waypoint.lane == nullptr) {
      AWARN << "lane_waypoint.lane is nullptr.";
      continue;
    }
    speed_limit_found = true;
    speed_limit =
        std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
  }

  if (!speed_limit_found) {
    // use default speed limit based on road_type
    speed_limit = FLAGS_default_city_road_speed_limit;
    hdmap::Road::Type road_type = GetRoadType(s);
    if (road_type == hdmap::Road::HIGHWAY) {
      speed_limit = FLAGS_default_highway_speed_limit;
    }
  }

const double speed_limit_from_centripetal_acc =
 std::sqrt(speed_bounds_config_.max_centric_acceleration_limit() /
           std::fmax(std::fabs(discretized_path.at(i).kappa()),
                     speed_bounds_config_.minimal_kappa()));

double speed_limit_from_nearby_obstacles =
        std::numeric_limits<double>::max();

const double collision_safety_range =
        speed_bounds_config_.collision_safety_range();

for (const auto* ptr_obstacle : obstacles.Items()) {
      if (ptr_obstacle->IsVirtual()) {
        continue;
      }
      if (!ptr_obstacle->LateralDecision().has_nudge()) {
        continue;
      }

const double vehicle_front_s =
          reference_line_s + vehicle_param_.front_edge_to_center();
      const double vehicle_back_s =
          reference_line_s - vehicle_param_.back_edge_to_center();

const double obstacle_front_s =
          ptr_obstacle->PerceptionSLBoundary().end_s();
      const double obstacle_back_s =
          ptr_obstacle->PerceptionSLBoundary().start_s();

if (vehicle_front_s < obstacle_back_s ||
          vehicle_back_s > obstacle_front_s) {
        continue;
      }

bool is_close_on_left =
    (nudge_decision.type() == ObjectNudge::LEFT_NUDGE) &&
    (frenet_point_l - vehicle_param_.right_edge_to_center() -
         collision_safety_range <
     ptr_obstacle->PerceptionSLBoundary().end_l());

bool is_close_on_right =
          (nudge_decision.type() == ObjectNudge::RIGHT_NUDGE) &&
          (ptr_obstacle->PerceptionSLBoundary().start_l() -
               collision_safety_range <
           frenet_point_l + vehicle_param_.left_edge_to_center());

 if (is_close_on_left || is_close_on_right) {
   double nudge_speed_ratio = 1.0;
   if (ptr_obstacle->IsStatic()) {
     nudge_speed_ratio =
         speed_bounds_config_.static_obs_nudge_speed_ratio();
   } else {
     nudge_speed_ratio =
         speed_bounds_config_.dynamic_obs_nudge_speed_ratio();
   }
   speed_limit_from_nearby_obstacles =
       nudge_speed_ratio * speed_limit_from_reference_line;
   break;
 }

if (speed_bounds_config_.enable_nudge_slowdown()) {
  curr_speed_limit =
      std::fmax(speed_bounds_config_.lowest_speed(),
                std::min({speed_limit_from_reference_line,
                          speed_limit_from_centripetal_acc,
                          speed_limit_from_nearby_obstacles}));
}

else {
curr_speed_limit =
std::fmax(speed_bounds_config_.lowest_speed(),
std::min({speed_limit_from_reference_line,
speed_limit_from_centripetal_acc}));
}

speed_limit_data->AppendSpeedLimit(path_s, curr_speed_limit);

const double path_data_length = path_data.discretized_path().Length();

const double total_time_by_conf = config_.total_time();

StGraphData *st_graph_data = reference_line_info_->mutable_st_graph_data();

  auto *debug = reference_line_info_->mutable_debug();
  STGraphDebug *st_graph_debug = debug->mutable_planning_data()->add_st_graph();

st_graph_data->LoadData(boundaries, min_s_on_st_boundaries, init_point,
                          speed_limit, reference_line_info->GetCruiseSpeed(),
                          path_data_length, total_time_by_conf, st_graph_debug);

void StGraphData::LoadData(const std::vector<const STBoundary*>& st_boundaries,
                           const double min_s_on_st_boundaries,
                           const apollo::common::TrajectoryPoint& init_point,
                           const SpeedLimit& speed_limit,
                           const double cruise_speed,
                           const double path_data_length,
                           const double total_time_by_conf,
                           planning_internal::STGraphDebug* st_graph_debug) {
  init_ = true;
  st_boundaries_ = st_boundaries;
  min_s_on_st_boundaries_ = min_s_on_st_boundaries;
  init_point_ = init_point;
  speed_limit_ = speed_limit;
  cruise_speed_ = cruise_speed;
  path_data_length_ = path_data_length;
  total_time_by_conf_ = total_time_by_conf;
  st_graph_debug_ = st_graph_debug;
}