提交 7e184a45 编写于 作者: D Dong Li 提交者: lianglia-apollo

planning: rename st_graph_boundary to st_boundary

* prepare to migrate st_boundary to path_obstacle.
上级 317c692e
......@@ -55,7 +55,7 @@ cc_library(
"//modules/planning/common:speed_limit",
"//modules/planning/common/path:path_data",
"//modules/planning/common/speed:speed_data",
"//modules/planning/tasks/st_graph:st_graph_boundary",
"//modules/planning/tasks/st_graph:st_boundary",
"//modules/planning/tasks/st_graph:st_graph_point",
"@eigen//:eigen",
],
......
......@@ -14,7 +14,7 @@ cc_library(
"//modules/common/proto:pnc_point_proto",
"//modules/planning/common:frame",
"//modules/planning/math:double",
"//modules/planning/tasks/st_graph:st_graph_boundary",
"//modules/planning/tasks/st_graph:st_boundary",
"//modules/planning/proto:dp_st_speed_config_proto",
],
)
......
......@@ -38,9 +38,9 @@ DpStCost::DpStCost(const DpStSpeedConfig& dp_st_speed_config)
// TODO(all): normalize cost with time
double DpStCost::GetObstacleCost(
const STPoint& point,
const std::vector<StGraphBoundary>& st_graph_boundaries) const {
const std::vector<StBoundary>& st_graph_boundaries) const {
double total_cost = 0.0;
for (const StGraphBoundary& boundary : st_graph_boundaries) {
for (const StBoundary& boundary : st_graph_boundaries) {
if (point.s() < 0 || boundary.IsPointInBoundary(point)) {
total_cost = std::numeric_limits<double>::infinity();
break;
......
......@@ -27,7 +27,7 @@
#include "modules/planning/proto/dp_st_speed_config.pb.h"
#include "modules/planning/common/speed/st_point.h"
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
namespace apollo {
namespace planning {
......@@ -38,7 +38,7 @@ class DpStCost {
double GetObstacleCost(
const STPoint& point,
const std::vector<StGraphBoundary>& st_graph_boundaries) const;
const std::vector<StBoundary>& st_graph_boundaries) const;
double GetReferenceCost(const STPoint& point,
const STPoint& reference_point) const;
......
......@@ -42,7 +42,7 @@ using Vec2d = apollo::common::math::Vec2d;
namespace {
bool CheckOverlapOnDpStGraph(const std::vector<StGraphBoundary> boundaries,
bool CheckOverlapOnDpStGraph(const std::vector<StBoundary> boundaries,
const StGraphPoint& p1, const StGraphPoint& p2) {
for (const auto& boundary : boundaries) {
common::math::LineSegment2d seg(p1.point(), p2.point());
......@@ -135,7 +135,7 @@ Status DpStGraph::InitCostTable() {
}
void DpStGraph::CalculatePointwiseCost(
const std::vector<StGraphBoundary>& boundaries) {
const std::vector<StBoundary>& boundaries) {
// TODO(all): extract reference line from decision first
std::vector<STPoint> reference_points;
double curr_t = 0.0;
......@@ -533,7 +533,7 @@ bool DpStGraph::CreateFollowDecision(
}
bool DpStGraph::CreateYieldDecision(
const StGraphBoundary& boundary,
const StBoundary& boundary,
ObjectDecisionType* const yield_decision) const {
DCHECK_NOTNULL(yield_decision);
......@@ -561,7 +561,7 @@ bool DpStGraph::CreateYieldDecision(
}
bool DpStGraph::CreateOvertakeDecision(
const PathObstacle& path_obstacle, const StGraphBoundary& boundary,
const PathObstacle& path_obstacle, const StBoundary& boundary,
ObjectDecisionType* const overtake_decision) const {
DCHECK_NOTNULL(overtake_decision);
......@@ -631,7 +631,7 @@ double DpStGraph::CalculateEdgeCostForThirdCol(const uint32_t curr_row,
dp_st_cost_.GetJerkCostByThreePoints(init_speed, first, second, third);
}
bool DpStGraph::CheckIsFollowByT(const StGraphBoundary& boundary) const {
bool DpStGraph::CheckIsFollowByT(const StBoundary& boundary) const {
DCHECK_EQ(boundary.points().size(), 4);
if (boundary.BottomLeftPoint().s() > boundary.BottomRightPoint().s()) {
......
......@@ -50,7 +50,7 @@ class DpStGraph {
private:
apollo::common::Status InitCostTable();
void CalculatePointwiseCost(const std::vector<StGraphBoundary>& boundaries);
void CalculatePointwiseCost(const std::vector<StBoundary>& boundaries);
apollo::common::Status RetrieveSpeedProfile(
SpeedData* const speed_data) const;
......@@ -76,12 +76,12 @@ class DpStGraph {
/**
* @brief check if the ADC should follow an obstacle by examing the
*StGraphBoundary of the obstacle.
*StBoundary of the obstacle.
* @param boundary The boundary of the obstacle.
* @return true if the ADC believe it should follow the obstacle, and
* false otherwise.
**/
bool CheckIsFollowByT(const StGraphBoundary& boundary) const;
bool CheckIsFollowByT(const StBoundary& boundary) const;
/**
* @brief create follow decision based on the boundary
......@@ -97,7 +97,7 @@ class DpStGraph {
* @return true if the yield decision is created successfully, and
* false otherwise.
**/
bool CreateYieldDecision(const StGraphBoundary& boundary,
bool CreateYieldDecision(const StBoundary& boundary,
ObjectDecisionType* const yield_decision) const;
/**
......@@ -106,7 +106,7 @@ class DpStGraph {
* false otherwise.
**/
bool CreateOvertakeDecision(
const PathObstacle& path_obstacle, const StGraphBoundary& boundary,
const PathObstacle& path_obstacle, const StBoundary& boundary,
ObjectDecisionType* const overtake_decision) const;
void GetRowRange(const uint32_t curr_row, const uint32_t curr_col,
......
......@@ -71,7 +71,7 @@ Status DpStSpeedOptimizer::Process(const PathData& path_data,
dp_st_speed_config_.total_time());
// step 1 get boundaries
std::vector<StGraphBoundary> boundaries;
std::vector<StBoundary> boundaries;
if (boundary_mapper.GetGraphBoundary(*path_decision, &boundaries).code() ==
ErrorCode::PLANNING_ERROR) {
const std::string msg =
......
......@@ -125,7 +125,7 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
Status QpSplineStGraph::ApplyConstraint(
const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
const std::vector<StGraphBoundary>& boundaries) {
const std::vector<StBoundary>& boundaries) {
Spline1dConstraint* constraint =
spline_generator_->mutable_spline_constraint();
// position, velocity, acceleration
......@@ -223,7 +223,7 @@ Status QpSplineStGraph::ApplyConstraint(
}
Status QpSplineStGraph::ApplyKernel(
const std::vector<StGraphBoundary>& boundaries,
const std::vector<StBoundary>& boundaries,
const SpeedLimit& speed_limit) {
Spline1dKernel* spline_kernel = spline_generator_->mutable_spline_kernel();
......@@ -297,7 +297,7 @@ Status QpSplineStGraph::AddCruiseReferenceLineKernel(
Status QpSplineStGraph::AddFollowReferenceLineKernel(
const std::vector<double>& evaluate_t,
const std::vector<StGraphBoundary>& boundaries, const double weight) {
const std::vector<StBoundary>& boundaries, const double weight) {
auto* spline_kernel = spline_generator_->mutable_spline_kernel();
std::vector<double> ref_s;
std::vector<double> filtered_evaluate_t;
......@@ -305,7 +305,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
double s_min = std::numeric_limits<double>::infinity();
bool success = false;
for (const auto& boundary : boundaries) {
if (boundary.boundary_type() != StGraphBoundary::BoundaryType::FOLLOW) {
if (boundary.boundary_type() != StBoundary::BoundaryType::FOLLOW) {
continue;
}
double s_upper = 0.0;
......@@ -336,12 +336,12 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
}
Status QpSplineStGraph::GetSConstraintByTime(
const std::vector<StGraphBoundary>& boundaries, const double time,
const std::vector<StBoundary>& boundaries, const double time,
const double total_path_s, double* const s_upper_bound,
double* const s_lower_bound) const {
*s_upper_bound = total_path_s;
for (const StGraphBoundary& boundary : boundaries) {
for (const StBoundary& boundary : boundaries) {
double s_upper = 0.0;
double s_lower = 0.0;
......@@ -349,13 +349,13 @@ Status QpSplineStGraph::GetSConstraintByTime(
continue;
}
if (boundary.boundary_type() == StGraphBoundary::BoundaryType::STOP ||
boundary.boundary_type() == StGraphBoundary::BoundaryType::FOLLOW ||
boundary.boundary_type() == StGraphBoundary::BoundaryType::YIELD) {
if (boundary.boundary_type() == StBoundary::BoundaryType::STOP ||
boundary.boundary_type() == StBoundary::BoundaryType::FOLLOW ||
boundary.boundary_type() == StBoundary::BoundaryType::YIELD) {
*s_upper_bound = std::fmin(*s_upper_bound, s_upper);
} else {
DCHECK(boundary.boundary_type() ==
StGraphBoundary::BoundaryType::OVERTAKE);
StBoundary::BoundaryType::OVERTAKE);
*s_lower_bound = std::fmax(*s_lower_bound, s_lower);
}
}
......
......@@ -34,7 +34,7 @@
#include "modules/planning/common/planning_util.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/math/smoothing_spline/spline_1d_generator.h"
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
#include "modules/planning/tasks/st_graph/st_graph_data.h"
namespace apollo {
......@@ -54,10 +54,10 @@ class QpSplineStGraph {
// apply st graph constraint
common::Status ApplyConstraint(
const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
const std::vector<StGraphBoundary>& boundaries);
const std::vector<StBoundary>& boundaries);
// apply objective function
common::Status ApplyKernel(const std::vector<StGraphBoundary>& boundaries,
common::Status ApplyKernel(const std::vector<StBoundary>& boundaries,
const SpeedLimit& speed_limit);
// solve
......@@ -65,7 +65,7 @@ class QpSplineStGraph {
// extract upper lower bound for constraint;
common::Status GetSConstraintByTime(
const std::vector<StGraphBoundary>& boundaries, const double time,
const std::vector<StBoundary>& boundaries, const double time,
const double total_path_s, double* const s_upper_bound,
double* const s_lower_bound) const;
......@@ -77,7 +77,7 @@ class QpSplineStGraph {
common::Status AddFollowReferenceLineKernel(
const std::vector<double>& evaluate_t,
const std::vector<StGraphBoundary>& boundaries, const double weight);
const std::vector<StBoundary>& boundaries, const double weight);
common::Status EstimateSpeedUpperBound(
const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
......
......@@ -75,7 +75,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
DCHECK(path_obstacle->HasLongitudinalDecision());
}
// step 1 get boundaries
std::vector<StGraphBoundary> boundaries;
std::vector<StBoundary> boundaries;
if (boundary_mapper.GetGraphBoundary(*path_decision, &boundaries).code() ==
ErrorCode::PLANNING_ERROR) {
return Status(ErrorCode::PLANNING_ERROR,
......@@ -84,7 +84,7 @@ Status QpSplineStSpeedOptimizer::Process(const PathData& path_data,
for (const auto& boundary : boundaries) {
ADEBUG << "QPST mapped boundary: " << boundary.DebugString() << std::endl;
DCHECK(boundary.boundary_type() != StGraphBoundary::BoundaryType::UNKNOWN);
DCHECK(boundary.boundary_type() != StBoundary::BoundaryType::UNKNOWN);
}
SpeedLimit speed_limits;
......
......@@ -49,7 +49,7 @@ void SpeedOptimizer::RecordDebugInfo(const SpeedData& speed_data) {
}
void SpeedOptimizer::RecordSTGraphDebug(
const std::vector<StGraphBoundary>& boundaries,
const std::vector<StBoundary>& boundaries,
const SpeedLimit& speed_limits, const SpeedData& speed_data) {
if (!FLAGS_enable_record_debug) {
ADEBUG << "Skip record debug info";
......
......@@ -27,7 +27,7 @@
#include "modules/common/status/status.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
#include "modules/planning/tasks/task.h"
namespace apollo {
......@@ -46,7 +46,7 @@ class SpeedOptimizer : public Task {
const ReferenceLine& reference_line, PathDecision* const path_decision,
SpeedData* const speed_data) = 0;
void RecordSTGraphDebug(const std::vector<StGraphBoundary>& boundaries,
void RecordSTGraphDebug(const std::vector<StBoundary>& boundaries,
const SpeedLimit& speed_limits,
const SpeedData& speed_data);
......
......@@ -17,12 +17,12 @@ cc_library(
)
cc_library(
name = "st_graph_boundary",
name = "st_boundary",
srcs = [
"st_graph_boundary.cc",
"st_boundary.cc",
],
hdrs = [
"st_graph_boundary.h",
"st_boundary.h",
],
deps = [
":st_graph_point",
......@@ -41,7 +41,7 @@ cc_library(
"st_graph_data.h",
],
deps = [
":st_graph_boundary",
":st_boundary",
"//modules/planning/common:planning_gflags",
"//modules/planning/common:speed_limit",
],
......@@ -56,7 +56,7 @@ cc_library(
"st_boundary_mapper.h",
],
deps = [
":st_graph_boundary",
":st_boundary",
"//modules/common/configs:vehicle_config_helper",
"//modules/common/configs/proto:vehicle_config_proto",
"//modules/common/proto:pnc_point_proto",
......@@ -98,13 +98,13 @@ cc_test(
)
cc_test(
name = "st_graph_boundary_test",
name = "st_boundary_test",
size = "small",
srcs = [
"st_graph_boundary_test.cc",
"st_boundary_test.cc",
],
deps = [
":st_graph_boundary",
":st_boundary",
"//modules/common:log",
"//modules/common/time",
"//modules/common/util",
......@@ -119,7 +119,7 @@ cc_test(
"st_graph_data_test.cc",
],
deps = [
":st_graph_boundary",
":st_boundary",
":st_graph_data",
"//modules/common:log",
"//modules/common/time",
......
......@@ -15,10 +15,10 @@
*****************************************************************************/
/**
* @file: obstacle_st_boundary.cc
* @file
**/
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
#include <algorithm>
......@@ -29,10 +29,10 @@ namespace planning {
using Vec2d = common::math::Vec2d;
StGraphBoundary::StGraphBoundary(const std::vector<STPoint>& points)
StBoundary::StBoundary(const std::vector<STPoint>& points)
: Polygon2d(std::vector<Vec2d>(points.begin(), points.end())) {
CHECK_EQ(points.size(), 4)
<< "StGraphBoundary must have exactly four points. Input points size: "
<< "StBoundary must have exactly four points. Input points size: "
<< points.size();
for (const auto& point : points) {
min_s_ = std::fmin(min_s_, point.s());
......@@ -42,11 +42,10 @@ StGraphBoundary::StGraphBoundary(const std::vector<STPoint>& points)
}
}
StGraphBoundary::StGraphBoundary(
const std::vector<::apollo::common::math::Vec2d>& points)
StBoundary::StBoundary(const std::vector<::apollo::common::math::Vec2d>& points)
: Polygon2d(points) {
CHECK_EQ(points.size(), 4)
<< "StGraphBoundary must have exactly four points. Input points size: "
<< "StBoundary must have exactly four points. Input points size: "
<< points.size();
for (const auto& point : points) {
min_s_ = std::fmin(min_s_, point.y());
......@@ -56,57 +55,55 @@ StGraphBoundary::StGraphBoundary(
}
}
bool StGraphBoundary::IsPointInBoundary(
const StGraphPoint& st_graph_point) const {
bool StBoundary::IsPointInBoundary(const StGraphPoint& st_graph_point) const {
return IsPointInBoundary(st_graph_point.point());
}
bool StGraphBoundary::IsPointInBoundary(const STPoint& st_point) const {
bool StBoundary::IsPointInBoundary(const STPoint& st_point) const {
return IsPointIn(st_point);
}
STPoint StGraphBoundary::BottomLeftPoint() const {
DCHECK(!points_.empty()) << "StGraphBoundary has zero points.";
STPoint StBoundary::BottomLeftPoint() const {
DCHECK(!points_.empty()) << "StBoundary has zero points.";
return STPoint(points_.at(0).y(), points_.at(0).x());
}
STPoint StGraphBoundary::BottomRightPoint() const {
DCHECK(!points_.empty()) << "StGraphBoundary has zero points.";
STPoint StBoundary::BottomRightPoint() const {
DCHECK(!points_.empty()) << "StBoundary has zero points.";
return STPoint(points_.at(1).y(), points_.at(1).x());
}
STPoint StGraphBoundary::TopRightPoint() const {
DCHECK(!points_.empty()) << "StGraphBoundary has zero points.";
STPoint StBoundary::TopRightPoint() const {
DCHECK(!points_.empty()) << "StBoundary has zero points.";
return STPoint(points_.at(2).y(), points_.at(2).x());
}
STPoint StGraphBoundary::TopLeftPoint() const {
DCHECK(!points_.empty()) << "StGraphBoundary has zero points.";
STPoint StBoundary::TopLeftPoint() const {
DCHECK(!points_.empty()) << "StBoundary has zero points.";
return STPoint(points_.at(3).y(), points_.at(3).x());
}
StGraphBoundary::BoundaryType StGraphBoundary::boundary_type() const {
StBoundary::BoundaryType StBoundary::boundary_type() const {
return boundary_type_;
}
void StGraphBoundary::SetBoundaryType(const BoundaryType& boundary_type) {
void StBoundary::SetBoundaryType(const BoundaryType& boundary_type) {
boundary_type_ = boundary_type;
}
const std::string& StGraphBoundary::id() const { return id_; }
const std::string& StBoundary::id() const { return id_; }
void StGraphBoundary::SetId(const std::string& id) { id_ = id; }
void StBoundary::SetId(const std::string& id) { id_ = id; }
double StGraphBoundary::characteristic_length() const {
double StBoundary::characteristic_length() const {
return characteristic_length_;
}
void StGraphBoundary::SetCharacteristicLength(
const double characteristic_length) {
void StBoundary::SetCharacteristicLength(const double characteristic_length) {
characteristic_length_ = characteristic_length;
}
bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
double* s_lower) const {
bool StBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
double* s_lower) const {
const common::math::LineSegment2d segment = {Vec2d(curr_time, 0.0),
Vec2d(curr_time, s_high_limit_)};
*s_upper = s_high_limit_;
......@@ -135,8 +132,8 @@ bool StGraphBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
return true;
}
bool StGraphBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
double* s_lower) const {
bool StBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
double* s_lower) const {
const common::math::LineSegment2d segment = {Vec2d(curr_time, 0.0),
Vec2d(curr_time, s_high_limit_)};
*s_upper = s_high_limit_;
......@@ -154,10 +151,10 @@ bool StGraphBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
return true;
}
double StGraphBoundary::min_s() const { return min_s_; }
double StGraphBoundary::min_t() const { return min_t_; }
double StGraphBoundary::max_s() const { return max_s_; }
double StGraphBoundary::max_t() const { return max_t_; }
double StBoundary::min_s() const { return min_s_; }
double StBoundary::min_t() const { return min_t_; }
double StBoundary::max_s() const { return max_s_; }
double StBoundary::max_t() const { return max_t_; }
} // namespace planning
} // namespace apollo
......@@ -15,7 +15,7 @@
*****************************************************************************/
/**
* @file: st_graph_boundary.h
* @file
**/
#ifndef MODULES_PLANNING_TASKS_ST_GRAPH_ST_GRAPH_BOUNDARY_H_
......@@ -34,16 +34,16 @@
namespace apollo {
namespace planning {
class StGraphBoundary : public common::math::Polygon2d {
class StBoundary : public common::math::Polygon2d {
public:
StGraphBoundary() = default;
StBoundary() = default;
// boundary points go counter clockwise.
explicit StGraphBoundary(const std::vector<STPoint>& points);
explicit StBoundary(const std::vector<STPoint>& points);
// boundary points go counter clockwise.
explicit StGraphBoundary(const std::vector<common::math::Vec2d>& points);
~StGraphBoundary() = default;
explicit StBoundary(const std::vector<common::math::Vec2d>& points);
~StBoundary() = default;
// if you need to add boundary type, make sure you modify
// GetUnblockSRange accordingly.
......
......@@ -79,7 +79,7 @@ StBoundaryMapper::StBoundaryMapper(const StBoundaryConfig& config,
Status StBoundaryMapper::GetGraphBoundary(
const PathDecision& path_decision,
std::vector<StGraphBoundary>* const st_graph_boundaries) const {
std::vector<StBoundary>* const st_graph_boundaries) const {
const auto& path_obstacles = path_decision.path_obstacles();
if (st_graph_boundaries == nullptr) {
const std::string msg = "st_graph_boundaries is NULL.";
......@@ -110,7 +110,7 @@ Status StBoundaryMapper::GetGraphBoundary(
for (const auto* path_obstacle : path_obstacles.Items()) {
if (!path_obstacle->HasLongitudinalDecision()) {
StGraphBoundary boundary;
StBoundary boundary;
const auto ret = MapWithoutDecision(*path_obstacle, &boundary);
if (!ret.ok()) {
std::string msg = common::util::StrCat(
......@@ -123,7 +123,7 @@ Status StBoundaryMapper::GetGraphBoundary(
}
const auto& decision = path_obstacle->LongitudinalDecision();
if (decision.has_follow()) {
StGraphBoundary follow_boundary;
StBoundary follow_boundary;
const auto ret =
MapFollowDecision(*path_obstacle, decision, &follow_boundary);
if (!ret.ok()) {
......@@ -152,7 +152,7 @@ Status StBoundaryMapper::GetGraphBoundary(
stop_decision = decision;
}
} else if (decision.has_overtake() || decision.has_yield()) {
StGraphBoundary boundary;
StBoundary boundary;
const auto ret =
MapWithPredictionTrajectory(*path_obstacle, decision, &boundary);
if (!ret.ok()) {
......@@ -168,7 +168,7 @@ Status StBoundaryMapper::GetGraphBoundary(
}
if (stop_obstacle) {
StGraphBoundary stop_boundary;
StBoundary stop_boundary;
bool success =
MapStopDecision(*stop_obstacle, stop_decision, &stop_boundary);
if (!success) {
......@@ -178,16 +178,16 @@ Status StBoundaryMapper::GetGraphBoundary(
}
AppendBoundary(stop_boundary, st_graph_boundaries);
}
for (const auto& st_graph_boundary : *st_graph_boundaries) {
DCHECK_EQ(st_graph_boundary.points().size(), 4);
DCHECK_NE(st_graph_boundary.id().length(), 0);
for (const auto& st_boundary : *st_graph_boundaries) {
DCHECK_EQ(st_boundary.points().size(), 4);
DCHECK_NE(st_boundary.id().length(), 0);
}
return Status::OK();
}
bool StBoundaryMapper::MapStopDecision(const PathObstacle& stop_obstacle,
const ObjectDecisionType& stop_decision,
StGraphBoundary* const boundary) const {
StBoundary* const boundary) const {
CHECK_NOTNULL(boundary);
DCHECK(stop_decision.has_stop()) << "Must have stop decision";
......@@ -222,15 +222,15 @@ bool StBoundaryMapper::MapStopDecision(const PathObstacle& stop_obstacle,
planning_time_);
boundary_points.emplace_back(s_max, 0.0);
*boundary = StGraphBoundary(boundary_points);
boundary->SetBoundaryType(StGraphBoundary::BoundaryType::STOP);
*boundary = StBoundary(boundary_points);
boundary->SetBoundaryType(StBoundary::BoundaryType::STOP);
boundary->SetCharacteristicLength(st_boundary_config_.boundary_buffer());
boundary->SetId(stop_obstacle.Id());
return true;
}
Status StBoundaryMapper::MapWithoutDecision(
const PathObstacle& path_obstacle, StGraphBoundary* const boundary) const {
const PathObstacle& path_obstacle, StBoundary* const boundary) const {
std::vector<STPoint> lower_points;
std::vector<STPoint> upper_points;
......@@ -257,7 +257,7 @@ Status StBoundaryMapper::MapWithoutDecision(
AWARN << "lower/upper points are reversed.";
}
*boundary = StGraphBoundary(boundary_points);
*boundary = StBoundary(boundary_points);
boundary->SetId(path_obstacle.Obstacle()->Id());
}
return Status::OK();
......@@ -344,7 +344,7 @@ bool StBoundaryMapper::GetOverlapBoundaryPoints(
Status StBoundaryMapper::MapWithPredictionTrajectory(
const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
StGraphBoundary* const boundary) const {
StBoundary* const boundary) const {
DCHECK_NOTNULL(boundary);
DCHECK(obj_decision.has_follow() || obj_decision.has_yield() ||
obj_decision.has_overtake())
......@@ -387,8 +387,8 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
}
// change boundary according to obj_decision.
StGraphBoundary::BoundaryType b_type =
StGraphBoundary::BoundaryType::UNKNOWN;
StBoundary::BoundaryType b_type =
StBoundary::BoundaryType::UNKNOWN;
double characteristic_length = 0.0;
constexpr double kBoundaryEpsilon = 1e-3;
if (obj_decision.has_follow()) {
......@@ -404,7 +404,7 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
boundary_points.at(0).set_s(boundary_points.at(0).s());
boundary_points.at(1).set_s(boundary_points.at(1).s());
boundary_points.at(3).set_t(-1.0);
b_type = StGraphBoundary::BoundaryType::FOLLOW;
b_type = StBoundary::BoundaryType::FOLLOW;
} else if (obj_decision.has_yield()) {
const double dis = std::fabs(obj_decision.yield().distance_s());
characteristic_length = dis;
......@@ -422,7 +422,7 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
boundary_points.at(1).set_s(boundary_points.at(1).s() - dis);
}
boundary_points.at(3).set_t(-kBoundaryEpsilon);
b_type = StGraphBoundary::BoundaryType::YIELD;
b_type = StBoundary::BoundaryType::YIELD;
} else if (obj_decision.has_overtake()) {
const double dis = std::fabs(obj_decision.overtake().distance_s());
......@@ -433,12 +433,12 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
const double time_buffer = obj_decision.overtake().time_buffer();
boundary_points.at(3).set_t(boundary_points.at(3).t() - time_buffer);
b_type = StGraphBoundary::BoundaryType::OVERTAKE;
b_type = StBoundary::BoundaryType::OVERTAKE;
} else {
DCHECK(false) << "Obj decision should be either yield or overtake: "
<< obj_decision.DebugString();
}
*boundary = StGraphBoundary(boundary_points);
*boundary = StBoundary(boundary_points);
boundary->SetBoundaryType(b_type);
boundary->SetId(path_obstacle.Obstacle()->Id());
boundary->SetCharacteristicLength(characteristic_length);
......@@ -448,7 +448,7 @@ Status StBoundaryMapper::MapWithPredictionTrajectory(
Status StBoundaryMapper::MapFollowDecision(
const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
StGraphBoundary* const boundary) const {
StBoundary* const boundary) const {
DCHECK_NOTNULL(boundary);
DCHECK(obj_decision.has_follow())
<< "Map obstacle without prediction trajectory is ONLY supported when "
......@@ -513,7 +513,7 @@ Status StBoundaryMapper::MapFollowDecision(
boundary_points.emplace_back(s_max_upper, planning_time_);
boundary_points.emplace_back(s_min_upper, 0.0);
*boundary = StGraphBoundary(boundary_points);
*boundary = StBoundary(boundary_points);
const double characteristic_length =
std::fabs(obj_decision.follow().distance_s()) +
......@@ -521,7 +521,7 @@ Status StBoundaryMapper::MapFollowDecision(
boundary->SetCharacteristicLength(characteristic_length);
boundary->SetId(obstacle->Id());
boundary->SetBoundaryType(StGraphBoundary::BoundaryType::FOLLOW);
boundary->SetBoundaryType(StBoundary::BoundaryType::FOLLOW);
return Status::OK();
}
......@@ -574,8 +574,8 @@ Status StBoundaryMapper::GetSpeedLimits(
}
void StBoundaryMapper::AppendBoundary(
const StGraphBoundary& boundary,
std::vector<StGraphBoundary>* st_graph_boundaries) const {
const StBoundary& boundary,
std::vector<StBoundary>* st_graph_boundaries) const {
if (Double::Compare(boundary.area(), 0.0) <= 0) {
return;
}
......
......@@ -30,7 +30,7 @@
#include "modules/planning/common/path/path_data.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
#include "modules/planning/reference_line/reference_line.h"
namespace apollo {
......@@ -45,7 +45,7 @@ class StBoundaryMapper {
apollo::common::Status GetGraphBoundary(
const PathDecision& path_decision,
std::vector<StGraphBoundary>* const boundary) const;
std::vector<StBoundary>* const boundary) const;
virtual apollo::common::Status GetSpeedLimits(
SpeedLimit* const speed_limit_data) const;
......@@ -62,22 +62,22 @@ class StBoundaryMapper {
std::vector<STPoint>* lower_points) const;
apollo::common::Status MapWithoutDecision(
const PathObstacle& path_obstacle, StGraphBoundary* const boundary) const;
const PathObstacle& path_obstacle, StBoundary* const boundary) const;
bool MapStopDecision(const PathObstacle& stop_obstacle,
const ObjectDecisionType& stop_decision,
StGraphBoundary* const boundary) const;
StBoundary* const boundary) const;
apollo::common::Status MapWithPredictionTrajectory(
const PathObstacle& path_obstacle, const ObjectDecisionType& obj_decision,
StGraphBoundary* const boundary) const;
StBoundary* const boundary) const;
apollo::common::Status MapFollowDecision(
const PathObstacle& obstacle, const ObjectDecisionType& obj_decision,
StGraphBoundary* const boundary) const;
StBoundary* const boundary) const;
void AppendBoundary(const StGraphBoundary& boundary,
std::vector<StGraphBoundary>* st_graph_boundaries) const;
void AppendBoundary(const StBoundary& boundary,
std::vector<StBoundary>* st_graph_boundaries) const;
private:
StBoundaryConfig st_boundary_config_;
......
......@@ -14,7 +14,7 @@
* limitations under the License.
*****************************************************************************/
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
#include <algorithm>
#include <cmath>
......@@ -27,30 +27,30 @@
namespace apollo {
namespace planning {
TEST(StGraphBoundaryTest, basic_test) {
TEST(StBoundaryTest, basic_test) {
std::vector<STPoint> st_points;
st_points.emplace_back(0.0, 0.0);
st_points.emplace_back(0.0, 10.0);
st_points.emplace_back(5.0, 10.0);
st_points.emplace_back(5.0, 0.0);
StGraphBoundary boundary(st_points);
StBoundary boundary(st_points);
EXPECT_EQ(boundary.id(), "");
EXPECT_EQ(boundary.boundary_type(), StGraphBoundary::BoundaryType::UNKNOWN);
EXPECT_EQ(boundary.boundary_type(), StBoundary::BoundaryType::UNKNOWN);
EXPECT_FLOAT_EQ(0.0, boundary.min_s());
EXPECT_FLOAT_EQ(5.0, boundary.max_s());
EXPECT_FLOAT_EQ(0.0, boundary.min_t());
EXPECT_FLOAT_EQ(10.0, boundary.max_t());
}
TEST(StGraphBoundaryTest, boundary_range) {
TEST(StBoundaryTest, boundary_range) {
std::vector<STPoint> st_points;
st_points.emplace_back(1.0, 0.0);
st_points.emplace_back(1.0, 10.0);
st_points.emplace_back(5.0, 10.0);
st_points.emplace_back(5.0, 0.0);
StGraphBoundary boundary(st_points);
boundary.SetBoundaryType(StGraphBoundary::BoundaryType::YIELD);
StBoundary boundary(st_points);
boundary.SetBoundaryType(StBoundary::BoundaryType::YIELD);
double t = -10.0;
const double dt = 0.01;
while (t < 10.0) {
......
......@@ -26,7 +26,7 @@ namespace planning {
using apollo::common::TrajectoryPoint;
StGraphData::StGraphData(
const std::vector<StGraphBoundary>& st_graph_boundaries,
const std::vector<StBoundary>& st_graph_boundaries,
const TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
const double path_data_length)
: st_graph_boundaries_(st_graph_boundaries),
......@@ -34,7 +34,7 @@ StGraphData::StGraphData(
speed_limit_(speed_limit),
path_data_length_(path_data_length) {}
const std::vector<StGraphBoundary>& StGraphData::st_graph_boundaries() const {
const std::vector<StBoundary>& StGraphData::st_graph_boundaries() const {
return st_graph_boundaries_;
}
......
......@@ -27,7 +27,7 @@
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/speed_limit.h"
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
namespace apollo {
namespace planning {
......@@ -36,11 +36,11 @@ class StGraphData {
public:
StGraphData() = default;
StGraphData(const std::vector<StGraphBoundary>& st_graph_boundaries,
StGraphData(const std::vector<StBoundary>& st_graph_boundaries,
const apollo::common::TrajectoryPoint& init_point,
const SpeedLimit& speed_limit, const double path_data_length);
const std::vector<StGraphBoundary>& st_graph_boundaries() const;
const std::vector<StBoundary>& st_graph_boundaries() const;
const apollo::common::TrajectoryPoint& init_point() const;
......@@ -49,7 +49,7 @@ class StGraphData {
double path_data_length() const;
private:
std::vector<StGraphBoundary> st_graph_boundaries_;
std::vector<StBoundary> st_graph_boundaries_;
apollo::common::TrajectoryPoint init_point_;
SpeedLimit speed_limit_;
......
......@@ -23,7 +23,7 @@
#include "gtest/gtest.h"
#include "modules/common/log.h"
#include "modules/planning/tasks/st_graph/st_graph_boundary.h"
#include "modules/planning/tasks/st_graph/st_boundary.h"
namespace apollo {
namespace planning {
......@@ -43,8 +43,8 @@ TEST(StGraphDataTest, basic_test) {
EXPECT_DOUBLE_EQ(st_graph_data.init_point().relative_time(), 0.0);
EXPECT_DOUBLE_EQ(st_graph_data.path_data_length(), 0.0);
std::vector<StGraphBoundary> boundary_vec;
boundary_vec.push_back(StGraphBoundary());
std::vector<StBoundary> boundary_vec;
boundary_vec.push_back(StBoundary());
apollo::common::TrajectoryPoint traj_point;
traj_point.mutable_path_point()->set_x(1.1);
traj_point.mutable_path_point()->set_y(2.1);
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册