apollo::planning::DpStCost类 参考

#include <dp_st_cost.h>

apollo::planning::DpStCost 的协作图:

Public 成员函数
	DpStCost (const DpStSpeedOptimizerConfig &config, const double total_t, const double total_s, const std::vector< const Obstacle * > &obstacles, const STDrivableBoundary &st_drivable_boundary, const common::TrajectoryPoint &init_point)
double	GetObstacleCost (const StGraphPoint &point)
double	GetSpatialPotentialCost (const StGraphPoint &point)
double	GetReferenceCost (const STPoint &point, const STPoint &reference_point) const
double	GetSpeedCost (const STPoint &first, const STPoint &second, const double speed_limit, const double cruise_speed) const
double	GetAccelCostByTwoPoints (const double pre_speed, const STPoint &first, const STPoint &second)
double	GetAccelCostByThreePoints (const STPoint &first, const STPoint &second, const STPoint &third)
double	GetJerkCostByTwoPoints (const double pre_speed, const double pre_acc, const STPoint &pre_point, const STPoint &curr_point)
double	GetJerkCostByThreePoints (const double first_speed, const STPoint &first_point, const STPoint &second_point, const STPoint &third_point)
double	GetJerkCostByFourPoints (const STPoint &first, const STPoint &second, const STPoint &third, const STPoint &fourth)

详细描述

在文件 dp_st_cost.h 第 39 行定义.

构造及析构函数说明

DpStCost()

apollo::planning::DpStCost::DpStCost	(	const DpStSpeedOptimizerConfig &	config,
		const double	total_t,
		const double	total_s,
		const std::vector< const Obstacle * > &	obstacles,
		const STDrivableBoundary &	st_drivable_boundary,
		const common::TrajectoryPoint &	init_point
	)

在文件 dp_st_cost.cc 第 36 行定义.

    : config_(config),
      obstacles_(obstacles),
      st_drivable_boundary_(st_drivable_boundary),
      init_point_(init_point),
      unit_t_(config.unit_t()),
      total_s_(total_s) {
  int index = 0;
  for (const auto& obstacle : obstacles) {
    boundary_map_[obstacle->path_st_boundary().id()] = index++;
  }
 
  AddToKeepClearRange(obstacles);
 
  const auto dimension_t =
      static_cast<uint32_t>(std::ceil(total_t / static_cast<double>(unit_t_))) +
      1;
  boundary_cost_.resize(obstacles_.size());
  for (auto& vec : boundary_cost_) {
    vec.resize(dimension_t, std::make_pair(-1.0, -1.0));
  }
  accel_cost_.fill(-1.0);
  jerk_cost_.fill(-1.0);
}

成员函数说明

GetAccelCostByThreePoints()

double apollo::planning::DpStCost::GetAccelCostByThreePoints	(	const STPoint &	first,
		const STPoint &	second,
		const STPoint &	third
	)

在文件 dp_st_cost.cc 第 274 行定义.

                                                                 {
  double accel = (first.s() + third.s() - 2 * second.s()) / (unit_t_ * unit_t_);
  return GetAccelCost(accel);
}

GetAccelCostByTwoPoints()

double apollo::planning::DpStCost::GetAccelCostByTwoPoints	(	const double	pre_speed,
		const STPoint &	first,
		const STPoint &	second
	)

在文件 dp_st_cost.cc 第 281 行定义.

                                                                    {
  double current_speed = (curr_point.s() - pre_point.s()) / unit_t_;
  double accel = (current_speed - pre_speed) / unit_t_;
  return GetAccelCost(accel);
}

GetJerkCostByFourPoints()

double apollo::planning::DpStCost::GetJerkCostByFourPoints	(	const STPoint &	first,
		const STPoint &	second,
		const STPoint &	third,
		const STPoint &	fourth
	)

在文件 dp_st_cost.cc 第 314 行定义.

                                                                {
  double jerk = (fourth.s() - 3 * third.s() + 3 * second.s() - first.s()) /
                (unit_t_ * unit_t_ * unit_t_);
  return JerkCost(jerk);
}

GetJerkCostByThreePoints()

double apollo::planning::DpStCost::GetJerkCostByThreePoints	(	const double	first_speed,
		const STPoint &	first_point,
		const STPoint &	second_point,
		const STPoint &	third_point
	)

在文件 dp_st_cost.cc 第 333 行定义.

                                                                {
  const double pre_speed = (second.s() - first.s()) / unit_t_;
  const double pre_acc = (pre_speed - first_speed) / unit_t_;
  const double curr_speed = (third.s() - second.s()) / unit_t_;
  const double curr_acc = (curr_speed - pre_speed) / unit_t_;
  const double jerk = (curr_acc - pre_acc) / unit_t_;
  return JerkCost(jerk);
}

GetJerkCostByTwoPoints()

double apollo::planning::DpStCost::GetJerkCostByTwoPoints	(	const double	pre_speed,
		const double	pre_acc,
		const STPoint &	pre_point,
		const STPoint &	curr_point
	)

在文件 dp_st_cost.cc 第 323 行定义.

                                                                   {
  const double curr_speed = (curr_point.s() - pre_point.s()) / unit_t_;
  const double curr_accel = (curr_speed - pre_speed) / unit_t_;
  const double jerk = (curr_accel - pre_acc) / unit_t_;
  return JerkCost(jerk);
}

GetObstacleCost()

double apollo::planning::DpStCost::GetObstacleCost

(

const StGraphPoint &

point

)

在文件 dp_st_cost.cc 第 113 行定义.

                                                                   {
  const double s = st_graph_point.point().s();
  const double t = st_graph_point.point().t();
 
  double cost = 0.0;
 
  if (FLAGS_use_st_drivable_boundary) {
    // TODO(Jiancheng): move to configs
    static constexpr double boundary_resolution = 0.1;
    int index = static_cast<int>(t / boundary_resolution);
    const double lower_bound =
        st_drivable_boundary_.st_boundary(index).s_lower();
    const double upper_bound =
        st_drivable_boundary_.st_boundary(index).s_upper();
 
    if (s > upper_bound || s < lower_bound) {
      return kInf;
    }
  }
 
  for (const auto* obstacle : obstacles_) {
    // Not applying obstacle approaching cost to virtual obstacle like created
    // stop fences
    if (obstacle->IsVirtual()) {
      continue;
    }
 
    // Stop obstacles are assumed to have a safety margin when mapping them out,
    // so repelling force in dp st is not needed as it is designed to have adc
    // stop right at the stop distance we design in prior mapping process
    if (obstacle->LongitudinalDecision().has_stop()) {
      continue;
    }
 
    auto boundary = obstacle->path_st_boundary();
 
    if (boundary.min_s() > FLAGS_speed_lon_decision_horizon) {
      continue;
    }
    if (t < boundary.min_t() || t > boundary.max_t()) {
      continue;
    }
    if (boundary.IsPointInBoundary(st_graph_point.point())) {
      return kInf;
    }
    double s_upper = 0.0;
    double s_lower = 0.0;
 
    int boundary_index = boundary_map_[boundary.id()];
    if (boundary_cost_[boundary_index][st_graph_point.index_t()].first < 0.0) {
      boundary.GetBoundarySRange(t, &s_upper, &s_lower);
      boundary_cost_[boundary_index][st_graph_point.index_t()] =
          std::make_pair(s_upper, s_lower);
    } else {
      s_upper = boundary_cost_[boundary_index][st_graph_point.index_t()].first;
      s_lower = boundary_cost_[boundary_index][st_graph_point.index_t()].second;
    }
    if (s < s_lower) {
      const double follow_distance_s = config_.safe_distance();
      if (s + follow_distance_s < s_lower) {
        continue;
      } else {
        auto s_diff = follow_distance_s - s_lower + s;
        cost += config_.obstacle_weight() * config_.default_obstacle_cost() *
                s_diff * s_diff;
      }
    } else if (s > s_upper) {
      const double overtake_distance_s =
          StGapEstimator::EstimateSafeOvertakingGap();
      if (s > s_upper + overtake_distance_s) {  // or calculated from velocity
        continue;
      } else {
        auto s_diff = overtake_distance_s + s_upper - s;
        cost += config_.obstacle_weight() * config_.default_obstacle_cost() *
                s_diff * s_diff;
      }
    }
  }
  return cost * unit_t_;
}

GetReferenceCost()

double apollo::planning::DpStCost::GetReferenceCost	(	const STPoint &	point,
		const STPoint &	reference_point
	)		const

在文件 dp_st_cost.cc 第 198 行定义.

                                                                        {
  return config_.reference_weight() * (point.s() - reference_point.s()) *
         (point.s() - reference_point.s()) * unit_t_;
}

GetSpatialPotentialCost()

double apollo::planning::DpStCost::GetSpatialPotentialCost

(

const StGraphPoint &

point

)

在文件 dp_st_cost.cc 第 194 行定义.

                                                                  {
  return (total_s_ - point.point().s()) * config_.spatial_potential_penalty();
}

GetSpeedCost()

double apollo::planning::DpStCost::GetSpeedCost	(	const STPoint &	first,
		const STPoint &	second,
		const double	speed_limit,
		const double	cruise_speed
	)		const

在文件 dp_st_cost.cc 第 204 行定义.

                                                               {
  double cost = 0.0;
  const double speed = (second.s() - first.s()) / unit_t_;
  if (speed < 0) {
    return kInf;
  }
 
  const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()
                                        ->GetConfig()
                                        .vehicle_param()
                                        .max_abs_speed_when_stopped();
  if (speed < max_adc_stop_speed && InKeepClearRange(second.s())) {
    // first.s in range
    cost += config_.keep_clear_low_speed_penalty() * unit_t_ *
            config_.default_speed_cost();
  }
 
  double det_speed = (speed - speed_limit) / speed_limit;
  if (det_speed > 0) {
    cost += config_.exceed_speed_penalty() * config_.default_speed_cost() *
            (det_speed * det_speed) * unit_t_;
  } else if (det_speed < 0) {
    cost += config_.low_speed_penalty() * config_.default_speed_cost() *
            -det_speed * unit_t_;
  }
 
  if (config_.enable_dp_reference_speed()) {
    double diff_speed = speed - cruise_speed;
    cost += config_.reference_speed_penalty() * config_.default_speed_cost() *
            fabs(diff_speed) * unit_t_;
  }
 
  return cost;
}

---

该类的文档由以下文件生成:

• modules/planning/tasks/path_time_heuristic/dp_st_cost.h
• modules/planning/tasks/path_time_heuristic/dp_st_cost.cc