提交 9f1b8658 编写于 作者: Y Yajia Zhang

planning: renamed STBoundary; minor code refactor

上级 27606d24
......@@ -330,7 +330,7 @@ void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
STPoint(end_s - adc_start_s, 0.0));
point_pairs.emplace_back(STPoint(start_s - adc_start_s, FLAGS_st_max_t),
STPoint(end_s - adc_start_s, FLAGS_st_max_t));
reference_line_st_boundary_ = StBoundary(point_pairs);
reference_line_st_boundary_ = STBoundary(point_pairs);
} else {
if (BuildTrajectoryStBoundary(reference_line, adc_start_s,
&reference_line_st_boundary_)) {
......@@ -347,7 +347,7 @@ void Obstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line,
bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
const double adc_start_s,
StBoundary* const st_boundary) {
STBoundary* const st_boundary) {
if (!IsValidObstacle(perception_obstacle_)) {
AERROR << "Fail to build trajectory st boundary because object is not "
"valid. PerceptionObstacle: "
......@@ -499,7 +499,7 @@ bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
});
polygon_points.erase(last, polygon_points.end());
if (polygon_points.size() > 2) {
*st_boundary = StBoundary(polygon_points);
*st_boundary = STBoundary(polygon_points);
}
} else {
return false;
......@@ -507,11 +507,11 @@ bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
return true;
}
const StBoundary& Obstacle::reference_line_st_boundary() const {
const STBoundary& Obstacle::reference_line_st_boundary() const {
return reference_line_st_boundary_;
}
const StBoundary& Obstacle::st_boundary() const { return st_boundary_; }
const STBoundary& Obstacle::st_boundary() const { return st_boundary_; }
const std::vector<std::string>& Obstacle::decider_tags() const {
return decider_tags_;
......@@ -686,27 +686,27 @@ const SLBoundary& Obstacle::PerceptionSLBoundary() const {
return sl_boundary_;
}
void Obstacle::SetStBoundary(const StBoundary& boundary) {
void Obstacle::SetStBoundary(const STBoundary& boundary) {
st_boundary_ = boundary;
}
void Obstacle::SetStBoundaryType(const StBoundary::BoundaryType type) {
void Obstacle::SetStBoundaryType(const STBoundary::BoundaryType type) {
st_boundary_.SetBoundaryType(type);
}
void Obstacle::EraseStBoundary() { st_boundary_ = StBoundary(); }
void Obstacle::EraseStBoundary() { st_boundary_ = STBoundary(); }
void Obstacle::SetReferenceLineStBoundary(const StBoundary& boundary) {
void Obstacle::SetReferenceLineStBoundary(const STBoundary& boundary) {
reference_line_st_boundary_ = boundary;
}
void Obstacle::SetReferenceLineStBoundaryType(
const StBoundary::BoundaryType type) {
const STBoundary::BoundaryType type) {
reference_line_st_boundary_.SetBoundaryType(type);
}
void Obstacle::EraseReferenceLineStBoundary() {
reference_line_st_boundary_ = StBoundary();
reference_line_st_boundary_ = STBoundary();
}
bool Obstacle::IsValidObstacle(
......
......@@ -142,9 +142,9 @@ class Obstacle {
const SLBoundary& PerceptionSLBoundary() const;
const StBoundary& reference_line_st_boundary() const;
const STBoundary& reference_line_st_boundary() const;
const StBoundary& st_boundary() const;
const STBoundary& st_boundary() const;
const std::vector<std::string>& decider_tags() const;
......@@ -157,15 +157,15 @@ class Obstacle {
const ObjectDecisionType& decision);
bool HasLateralDecision() const;
void SetStBoundary(const StBoundary& boundary);
void SetStBoundary(const STBoundary& boundary);
void SetStBoundaryType(const StBoundary::BoundaryType type);
void SetStBoundaryType(const STBoundary::BoundaryType type);
void EraseStBoundary();
void SetReferenceLineStBoundary(const StBoundary& boundary);
void SetReferenceLineStBoundary(const STBoundary& boundary);
void SetReferenceLineStBoundaryType(const StBoundary::BoundaryType type);
void SetReferenceLineStBoundaryType(const STBoundary::BoundaryType type);
void EraseReferenceLineStBoundary();
......@@ -223,7 +223,7 @@ class Obstacle {
bool BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
const double adc_start_s,
StBoundary* const st_boundary);
STBoundary* const st_boundary);
bool IsValidObstacle(
const perception::PerceptionObstacle& perception_obstacle);
......@@ -243,8 +243,8 @@ class Obstacle {
std::vector<std::string> decider_tags_;
SLBoundary sl_boundary_;
StBoundary reference_line_st_boundary_;
StBoundary st_boundary_;
STBoundary reference_line_st_boundary_;
STBoundary st_boundary_;
ObjectDecisionType lateral_decision_;
ObjectDecisionType longitudinal_decision_;
......
......@@ -41,7 +41,7 @@ const Obstacle *PathDecision::Find(const std::string &object_id) const {
}
void PathDecision::SetStBoundary(const std::string &id,
const StBoundary &boundary) {
const STBoundary &boundary) {
auto *obstacle = obstacles_.Find(id);
if (!obstacle) {
......
......@@ -54,7 +54,7 @@ class PathDecision {
Obstacle *Find(const std::string &object_id);
void SetStBoundary(const std::string &id, const StBoundary &boundary);
void SetStBoundary(const std::string &id, const STBoundary &boundary);
void EraseStBoundaries();
MainStop main_stop() const { return main_stop_; }
double stop_reference_line_s() const { return stop_reference_line_s_; }
......
......@@ -29,6 +29,7 @@ cc_library(
"//cyber/common:log",
"//modules/planning/proto:planning_proto",
"@gtest",
"//modules/planning/common:planning_gflags",
],
)
......
......@@ -22,6 +22,7 @@
#include "cyber/common/log.h"
#include "modules/common/math/math_utils.h"
#include "modules/planning/common/planning_gflags.h"
namespace apollo {
namespace planning {
......@@ -29,7 +30,7 @@ namespace planning {
using common::math::LineSegment2d;
using common::math::Vec2d;
StBoundary::StBoundary(
STBoundary::STBoundary(
const std::vector<std::pair<STPoint, STPoint>>& point_pairs) {
CHECK(IsValid(point_pairs)) << "The input point_pairs are NOT valid";
......@@ -62,12 +63,12 @@ StBoundary::StBoundary(
max_t_ = lower_points_.back().t();
}
bool StBoundary::IsPointNear(const common::math::LineSegment2d& seg,
bool STBoundary::IsPointNear(const common::math::LineSegment2d& seg,
const Vec2d& point, const double max_dist) {
return seg.DistanceSquareTo(point) < max_dist * max_dist;
}
std::string StBoundary::TypeName(BoundaryType type) {
std::string STBoundary::TypeName(BoundaryType type) {
if (type == BoundaryType::FOLLOW) {
return "FOLLOW";
} else if (type == BoundaryType::KEEP_CLEAR) {
......@@ -86,7 +87,7 @@ std::string StBoundary::TypeName(BoundaryType type) {
return "UNKNOWN";
}
void StBoundary::RemoveRedundantPoints(
void STBoundary::RemoveRedundantPoints(
std::vector<std::pair<STPoint, STPoint>>* point_pairs) {
if (!point_pairs || point_pairs->size() <= 2) {
return;
......@@ -114,7 +115,7 @@ void StBoundary::RemoveRedundantPoints(
point_pairs->resize(i + 1);
}
bool StBoundary::IsValid(
bool STBoundary::IsValid(
const std::vector<std::pair<STPoint, STPoint>>& point_pairs) const {
if (point_pairs.size() < 2) {
AERROR << "point_pairs.size() must > 2. current point_pairs.size() = "
......@@ -154,7 +155,7 @@ bool StBoundary::IsValid(
return true;
}
bool StBoundary::IsPointInBoundary(const STPoint& st_point) const {
bool STBoundary::IsPointInBoundary(const STPoint& st_point) const {
if (st_point.t() <= min_t_ || st_point.t() >= max_t_) {
return false;
}
......@@ -172,29 +173,29 @@ bool StBoundary::IsPointInBoundary(const STPoint& st_point) const {
return (check_upper * check_lower < 0);
}
STPoint StBoundary::UpperLeftPoint() const {
STPoint STBoundary::upper_left_point() const {
DCHECK(!upper_points_.empty()) << "StBoundary has zero points.";
return upper_points_.front();
}
STPoint StBoundary::UpperRightPoint() const {
STPoint STBoundary::upper_right_point() const {
DCHECK(!upper_points_.empty()) << "StBoundary has zero points.";
return upper_points_.back();
}
STPoint StBoundary::BottomLeftPoint() const {
STPoint STBoundary::bottom_left_point() const {
DCHECK(!lower_points_.empty()) << "StBoundary has zero points.";
return lower_points_.front();
}
STPoint StBoundary::BottomRightPoint() const {
STPoint STBoundary::bottom_right_point() const {
DCHECK(!lower_points_.empty()) << "StBoundary has zero points.";
return lower_points_.back();
}
StBoundary StBoundary::ExpandByS(const double s) const {
STBoundary STBoundary::ExpandByS(const double s) const {
if (lower_points_.empty()) {
return StBoundary();
return STBoundary();
}
std::vector<std::pair<STPoint, STPoint>> point_pairs;
for (size_t i = 0; i < lower_points_.size(); ++i) {
......@@ -202,13 +203,13 @@ StBoundary StBoundary::ExpandByS(const double s) const {
STPoint(lower_points_[i].y() - s, lower_points_[i].x()),
STPoint(upper_points_[i].y() + s, upper_points_[i].x()));
}
return StBoundary(std::move(point_pairs));
return STBoundary(std::move(point_pairs));
}
StBoundary StBoundary::ExpandByT(const double t) const {
STBoundary STBoundary::ExpandByT(const double t) const {
if (lower_points_.empty()) {
AERROR << "The current st_boundary has NO points.";
return StBoundary();
return STBoundary();
}
std::vector<std::pair<STPoint, STPoint>> point_pairs;
......@@ -252,34 +253,34 @@ StBoundary StBoundary::ExpandByT(const double t) const {
std::fmax(point_pairs.back().second.s(),
point_pairs.back().first.s() + kMinSEpsilon));
return StBoundary(std::move(point_pairs));
return STBoundary(std::move(point_pairs));
}
StBoundary::BoundaryType StBoundary::boundary_type() const {
STBoundary::BoundaryType STBoundary::boundary_type() const {
return boundary_type_;
}
void StBoundary::SetBoundaryType(const BoundaryType& boundary_type) {
void STBoundary::SetBoundaryType(const BoundaryType& boundary_type) {
boundary_type_ = boundary_type;
}
const std::string& StBoundary::id() const { return id_; }
const std::string& STBoundary::id() const { return id_; }
void StBoundary::set_id(const std::string& id) { id_ = id; }
void STBoundary::set_id(const std::string& id) { id_ = id; }
double StBoundary::characteristic_length() const {
double STBoundary::characteristic_length() const {
return characteristic_length_;
}
void StBoundary::SetCharacteristicLength(const double characteristic_length) {
void STBoundary::SetCharacteristicLength(const double characteristic_length) {
characteristic_length_ = characteristic_length;
}
bool StBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
bool STBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
double* s_lower) const {
CHECK_NOTNULL(s_upper);
CHECK_NOTNULL(s_lower);
*s_upper = s_high_limit_;
*s_upper = FLAGS_decision_horizon;
*s_lower = 0.0;
if (curr_time < min_t_ || curr_time > max_t_) {
return true;
......@@ -315,7 +316,7 @@ bool StBoundary::GetUnblockSRange(const double curr_time, double* s_upper,
return true;
}
bool StBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
bool STBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
double* s_lower) const {
CHECK_NOTNULL(s_upper);
CHECK_NOTNULL(s_lower);
......@@ -337,17 +338,17 @@ bool StBoundary::GetBoundarySRange(const double curr_time, double* s_upper,
*s_lower = lower_points_[left].s() +
r * (lower_points_[right].s() - lower_points_[left].s());
*s_upper = std::fmin(*s_upper, s_high_limit_);
*s_upper = std::fmin(*s_upper, FLAGS_decision_horizon);
*s_lower = std::fmax(*s_lower, 0.0);
return true;
}
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_; }
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_; }
bool StBoundary::GetIndexRange(const std::vector<STPoint>& points,
bool STBoundary::GetIndexRange(const std::vector<STPoint>& points,
const double t, size_t* left,
size_t* right) const {
CHECK_NOTNULL(left);
......@@ -370,11 +371,11 @@ bool StBoundary::GetIndexRange(const std::vector<STPoint>& points,
return true;
}
StBoundary StBoundary::GenerateStBoundary(
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();
return STBoundary();
}
std::vector<std::pair<STPoint, STPoint>> point_pairs;
......@@ -383,10 +384,10 @@ StBoundary StBoundary::GenerateStBoundary(
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);
return STBoundary(point_pairs);
}
StBoundary StBoundary::CutOffByT(const double t) const {
STBoundary STBoundary::CutOffByT(const double t) const {
std::vector<STPoint> lower_points;
std::vector<STPoint> upper_points;
for (size_t i = 0; i < lower_points_.size() && i < upper_points_.size();
......@@ -397,22 +398,22 @@ StBoundary StBoundary::CutOffByT(const double t) const {
lower_points.push_back(lower_points_[i]);
upper_points.push_back(upper_points_[i]);
}
return GenerateStBoundary(lower_points, upper_points);
return CreateInstance(lower_points, upper_points);
}
void StBoundary::set_upper_left_point(STPoint st_point) {
void STBoundary::set_upper_left_point(STPoint st_point) {
upper_left_point_ = std::move(st_point);
}
void StBoundary::set_upper_right_point(STPoint st_point) {
void STBoundary::set_upper_right_point(STPoint st_point) {
upper_right_point_ = std::move(st_point);
}
void StBoundary::set_bottom_left_point(STPoint st_point) {
void STBoundary::set_bottom_left_point(STPoint st_point) {
bottom_left_point_ = std::move(st_point);
}
void StBoundary::set_bottom_right_point(STPoint st_point) {
void STBoundary::set_bottom_right_point(STPoint st_point) {
bottom_right_point_ = std::move(st_point);
}
......
......@@ -21,6 +21,7 @@
#pragma once
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
......@@ -37,34 +38,42 @@
namespace apollo {
namespace planning {
class StBoundary : public common::math::Polygon2d {
class STBoundary : public common::math::Polygon2d {
public:
StBoundary() = default;
STBoundary() = default;
explicit StBoundary(
explicit STBoundary(
const std::vector<std::pair<STPoint, STPoint>>& point_pairs);
explicit StBoundary(const common::math::Box2d& box) = delete;
explicit StBoundary(std::vector<common::math::Vec2d> points) = delete;
explicit STBoundary(const common::math::Box2d& box) = delete;
~StBoundary() = default;
explicit STBoundary(std::vector<common::math::Vec2d> points) = delete;
static STBoundary CreateInstance(
const std::vector<STPoint>& lower_points,
const std::vector<STPoint>& upper_points);
static std::unique_ptr<STBoundary> CreateInstance(
const std::vector<std::pair<STPoint, STPoint>>& point_pairs);
~STBoundary() = default;
bool IsEmpty() const { return lower_points_.empty(); }
bool IsPointInBoundary(const STPoint& st_point) const;
STPoint UpperLeftPoint() const;
STPoint UpperRightPoint() const;
STPoint upper_left_point() const;
STPoint upper_right_point() const;
STPoint BottomLeftPoint() const;
STPoint BottomRightPoint() const;
STPoint bottom_left_point() const;
STPoint bottom_right_point() const;
void set_upper_left_point(STPoint st_point);
void set_upper_right_point(STPoint st_point);
void set_bottom_left_point(STPoint st_point);
void set_bottom_right_point(STPoint st_point);
StBoundary ExpandByS(const double s) const;
StBoundary ExpandByT(const double t) const;
STBoundary ExpandByS(const double s) const;
STBoundary ExpandByT(const double t) const;
// if you need to add boundary type, make sure you modify
// GetUnblockSRange accordingly.
......@@ -103,11 +112,7 @@ class StBoundary : public common::math::Polygon2d {
std::vector<STPoint> upper_points() const { return upper_points_; }
std::vector<STPoint> lower_points() const { return lower_points_; }
static StBoundary GenerateStBoundary(
const std::vector<STPoint>& lower_points,
const std::vector<STPoint>& upper_points);
StBoundary CutOffByT(const double t) const;
STBoundary CutOffByT(const double t) const;
private:
bool IsValid(
......@@ -134,7 +139,6 @@ class StBoundary : public common::math::Polygon2d {
std::string id_;
double characteristic_length_ = 1.0;
double s_high_limit_ = 200.0;
double min_s_ = std::numeric_limits<double>::max();
double max_s_ = std::numeric_limits<double>::lowest();
double min_t_ = std::numeric_limits<double>::max();
......
......@@ -38,10 +38,10 @@ TEST(StBoundaryTest, basic_test) {
point_pairs.emplace_back(lower_points[0], upper_points[0]);
point_pairs.emplace_back(lower_points[1], upper_points[1]);
StBoundary boundary(point_pairs);
STBoundary boundary(point_pairs);
EXPECT_EQ(boundary.id(), "");
EXPECT_EQ(boundary.boundary_type(), StBoundary::BoundaryType::UNKNOWN);
EXPECT_EQ(boundary.boundary_type(), STBoundary::BoundaryType::UNKNOWN);
EXPECT_DOUBLE_EQ(0.0, boundary.min_s());
EXPECT_DOUBLE_EQ(5.0, boundary.max_s());
EXPECT_DOUBLE_EQ(0.0, boundary.min_t());
......@@ -62,9 +62,9 @@ TEST(StBoundaryTest, boundary_range) {
point_pairs.emplace_back(lower_points[0], upper_points[0]);
point_pairs.emplace_back(lower_points[1], upper_points[1]);
StBoundary boundary(point_pairs);
STBoundary boundary(point_pairs);
boundary.SetBoundaryType(StBoundary::BoundaryType::YIELD);
boundary.SetBoundaryType(STBoundary::BoundaryType::YIELD);
double t = -10.0;
const double dt = 0.01;
while (t < 10.0) {
......@@ -103,7 +103,7 @@ TEST(StBoundaryTest, get_index_range) {
point_pairs.emplace_back(lower_points[0], upper_points[0]);
point_pairs.emplace_back(lower_points[1], upper_points[1]);
StBoundary boundary(point_pairs);
STBoundary boundary(point_pairs);
size_t left = 0;
size_t right = 0;
......@@ -136,7 +136,7 @@ TEST(StBoundaryTest, remove_redundant_points) {
EXPECT_EQ(points.size(), 5);
StBoundary st_boundary;
STBoundary st_boundary;
st_boundary.RemoveRedundantPoints(&points);
EXPECT_EQ(points.size(), 2);
......
......@@ -203,13 +203,13 @@ STPoint PathTimeGraph::SetPathTimePoint(const std::string& obstacle_id,
return path_time_point;
}
const std::vector<StBoundary>& PathTimeGraph::GetPathTimeObstacles()
const std::vector<STBoundary>& PathTimeGraph::GetPathTimeObstacles()
const {
return path_time_obstacles_;
}
bool PathTimeGraph::GetPathTimeObstacle(const std::string& obstacle_id,
StBoundary* path_time_obstacle) {
STBoundary* path_time_obstacle) {
if (path_time_obstacle_map_.find(obstacle_id) ==
path_time_obstacle_map_.end()) {
return false;
......@@ -226,13 +226,14 @@ std::vector<std::pair<double, double>> PathTimeGraph::GetPathBlockingIntervals(
if (t > pt_obstacle.max_t() || t < pt_obstacle.min_t()) {
continue;
}
double s_upper = lerp(pt_obstacle.UpperLeftPoint().s(),
pt_obstacle.UpperLeftPoint().t(), pt_obstacle.UpperRightPoint().s(),
pt_obstacle.UpperRightPoint().t(), t);
double s_upper = lerp(pt_obstacle.upper_left_point().s(),
pt_obstacle.upper_left_point().t(), pt_obstacle.upper_right_point().s(),
pt_obstacle.upper_right_point().t(), t);
double s_lower = lerp(pt_obstacle.BottomLeftPoint().s(),
pt_obstacle.BottomLeftPoint().t(), pt_obstacle.BottomRightPoint().s(),
pt_obstacle.BottomRightPoint().t(), t);
double s_lower = lerp(pt_obstacle.bottom_left_point().s(),
pt_obstacle.bottom_left_point().t(),
pt_obstacle.bottom_right_point().s(),
pt_obstacle.bottom_right_point().t(), t);
intervals.emplace_back(s_lower, s_upper);
}
......@@ -276,17 +277,17 @@ std::vector<STPoint> PathTimeGraph::GetObstacleSurroundingPoints(
double t0 = 0.0;
double t1 = 0.0;
if (s_dist > 0.0) {
s0 = pt_obstacle.UpperLeftPoint().s();
s1 = pt_obstacle.UpperRightPoint().s();
s0 = pt_obstacle.upper_left_point().s();
s1 = pt_obstacle.upper_right_point().s();
t0 = pt_obstacle.UpperLeftPoint().t();
t1 = pt_obstacle.UpperRightPoint().t();
t0 = pt_obstacle.upper_left_point().t();
t1 = pt_obstacle.upper_right_point().t();
} else {
s0 = pt_obstacle.BottomLeftPoint().s();
s1 = pt_obstacle.BottomRightPoint().s();
s0 = pt_obstacle.bottom_left_point().s();
s1 = pt_obstacle.bottom_right_point().s();
t0 = pt_obstacle.BottomLeftPoint().t();
t1 = pt_obstacle.BottomRightPoint().t();
t0 = pt_obstacle.bottom_left_point().t();
t1 = pt_obstacle.bottom_right_point().t();
}
double time_gap = t1 - t0;
......
......@@ -47,10 +47,10 @@ class PathTimeGraph {
const double s_start, const double s_end, const double t_start,
const double t_end, const std::array<double, 3>& init_d);
const std::vector<StBoundary>& GetPathTimeObstacles() const;
const std::vector<STBoundary>& GetPathTimeObstacles() const;
bool GetPathTimeObstacle(const std::string& obstacle_id,
StBoundary* path_time_obstacle);
STBoundary* path_time_obstacle);
std::vector<std::pair<double, double>> GetPathBlockingIntervals(
const double t) const;
......@@ -102,8 +102,8 @@ class PathTimeGraph {
const ReferenceLineInfo* ptr_reference_line_info_;
std::array<double, 3> init_d_;
std::unordered_map<std::string, StBoundary> path_time_obstacle_map_;
std::vector<StBoundary> path_time_obstacles_;
std::unordered_map<std::string, STBoundary> path_time_obstacle_map_;
std::vector<STBoundary> path_time_obstacles_;
std::vector<SLBoundary> static_obs_sl_boundaries_;
};
......
......@@ -126,8 +126,8 @@ bool DeciderCreep::CheckCreepDone(const Frame& frame,
creep_config.min_boundary_t()) {
const double kepsilon = 1e-6;
double obstacle_traveled_s =
obstacle->reference_line_st_boundary().BottomLeftPoint().s() -
obstacle->reference_line_st_boundary().BottomRightPoint().s();
obstacle->reference_line_st_boundary().bottom_left_point().s() -
obstacle->reference_line_st_boundary().bottom_right_point().s();
ADEBUG << "obstacle[" << obstacle->Id()
<< "] obstacle_st_min_t["
<< obstacle->reference_line_st_boundary().min_t()
......
......@@ -59,7 +59,7 @@ void DpStCost::AddToKeepClearRange(
continue;
}
if (obstacle->st_boundary().boundary_type() !=
StBoundary::BoundaryType::KEEP_CLEAR) {
STBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
......
......@@ -44,11 +44,11 @@ namespace {
constexpr double kInf = std::numeric_limits<double>::infinity();
bool CheckOverlapOnDpStGraph(const std::vector<const StBoundary*>& boundaries,
bool CheckOverlapOnDpStGraph(const std::vector<const STBoundary*>& boundaries,
const StGraphPoint& p1, const StGraphPoint& p2) {
const common::math::LineSegment2d seg(p1.point(), p2.point());
for (const auto* boundary : boundaries) {
if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary->boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
if (boundary->HasOverlap(seg)) {
......@@ -82,7 +82,7 @@ DpStGraph::DpStGraph(const StGraphData& st_graph_data,
Status DpStGraph::Search(SpeedData* const speed_data) {
constexpr double kBounadryEpsilon = 1e-2;
for (const auto& boundary : st_graph_data_.st_boundaries()) {
if (boundary->boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary->boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
if (boundary->IsPointInBoundary({0.0, 0.0}) ||
......
......@@ -109,9 +109,9 @@ TEST_F(DpStGraphTest, simple) {
point_pairs.emplace_back(lower_points[0], upper_points[0]);
point_pairs.emplace_back(lower_points[1], upper_points[1]);
obstacle_list_.back().SetStBoundary(StBoundary(point_pairs));
obstacle_list_.back().SetStBoundary(STBoundary(point_pairs));
std::vector<const StBoundary*> boundaries;
std::vector<const STBoundary*> boundaries;
boundaries.push_back(&(obstacles_.back()->st_boundary()));
init_point_.mutable_path_point()->set_x(0.0);
......
......@@ -51,12 +51,12 @@ bool DpStSpeedOptimizer::SearchStGraph(
const SpeedLimitDecider& speed_limit_decider, const PathData& path_data,
SpeedData* speed_data, PathDecision* path_decision,
STGraphDebug* st_graph_debug) const {
std::vector<const StBoundary*> boundaries;
std::vector<const STBoundary*> boundaries;
for (auto* obstacle : path_decision->obstacles().Items()) {
auto id = obstacle->Id();
if (!obstacle->st_boundary().IsEmpty()) {
if (obstacle->st_boundary().boundary_type() ==
StBoundary::BoundaryType::KEEP_CLEAR) {
STBoundary::BoundaryType::KEEP_CLEAR) {
path_decision->Find(id)->SetBlockingObstacle(false);
} else {
path_decision->Find(id)->SetBlockingObstacle(true);
......
......@@ -100,7 +100,7 @@ bool PathDecider::MakeStaticObstacleDecision(
continue;
}
if (obstacle->reference_line_st_boundary().boundary_type() ==
StBoundary::BoundaryType::KEEP_CLEAR) {
STBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
......
......@@ -187,24 +187,24 @@ void PolyVTSpeedOptimizer::RecordSTGraphDebug(
auto boundary_debug = st_graph_debug->add_boundary();
boundary_debug->set_name(boundary->id());
switch (boundary->boundary_type()) {
case StBoundary::BoundaryType::FOLLOW:
case STBoundary::BoundaryType::FOLLOW:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_FOLLOW);
break;
case StBoundary::BoundaryType::OVERTAKE:
case STBoundary::BoundaryType::OVERTAKE:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_OVERTAKE);
break;
case StBoundary::BoundaryType::STOP:
case STBoundary::BoundaryType::STOP:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_STOP);
break;
case StBoundary::BoundaryType::UNKNOWN:
case STBoundary::BoundaryType::UNKNOWN:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_UNKNOWN);
break;
case StBoundary::BoundaryType::YIELD:
case STBoundary::BoundaryType::YIELD:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_YIELD);
break;
case StBoundary::BoundaryType::KEEP_CLEAR:
case STBoundary::BoundaryType::KEEP_CLEAR:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_KEEP_CLEAR);
break;
......
......@@ -123,7 +123,7 @@ Status QpPiecewiseStGraph::Search(
Status QpPiecewiseStGraph::AddConstraint(
const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
const std::vector<const StBoundary*>& boundaries,
const std::vector<const STBoundary*>& boundaries,
const std::pair<double, double>& accel_bound) {
auto* constraint = generator_->mutable_constraint();
// position, velocity, acceleration
......@@ -212,7 +212,7 @@ Status QpPiecewiseStGraph::AddConstraint(
}
Status QpPiecewiseStGraph::AddKernel(
const std::vector<const StBoundary*>& boundaries,
const std::vector<const STBoundary*>& boundaries,
const SpeedLimit& speed_limit) {
auto* kernel = generator_->mutable_kernel();
DCHECK_NOTNULL(kernel);
......@@ -284,7 +284,7 @@ Status QpPiecewiseStGraph::AddCruiseReferenceLineKernel(
}
Status QpPiecewiseStGraph::AddFollowReferenceLineKernel(
const std::vector<const StBoundary*>& boundaries, const double weight) {
const std::vector<const STBoundary*>& boundaries, const double weight) {
auto* follow_kernel = generator_->mutable_kernel();
std::vector<double> ref_s;
std::vector<double> filtered_evaluate_t;
......@@ -294,7 +294,7 @@ Status QpPiecewiseStGraph::AddFollowReferenceLineKernel(
double s_min = std::numeric_limits<double>::infinity();
bool success = false;
for (const auto* boundary : boundaries) {
if (boundary->boundary_type() != StBoundary::BoundaryType::FOLLOW) {
if (boundary->boundary_type() != STBoundary::BoundaryType::FOLLOW) {
continue;
}
if (curr_t < boundary->min_t() || curr_t > boundary->max_t()) {
......@@ -336,12 +336,12 @@ Status QpPiecewiseStGraph::AddFollowReferenceLineKernel(
}
Status QpPiecewiseStGraph::GetSConstraintByTime(
const std::vector<const StBoundary*>& boundaries, const double time,
const std::vector<const 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 StBoundary* boundary : boundaries) {
for (const STBoundary* boundary : boundaries) {
double s_upper = 0.0;
double s_lower = 0.0;
......@@ -349,12 +349,12 @@ Status QpPiecewiseStGraph::GetSConstraintByTime(
continue;
}
if (boundary->boundary_type() == StBoundary::BoundaryType::STOP ||
boundary->boundary_type() == StBoundary::BoundaryType::FOLLOW ||
boundary->boundary_type() == StBoundary::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() == StBoundary::BoundaryType::OVERTAKE);
DCHECK(boundary->boundary_type() == STBoundary::BoundaryType::OVERTAKE);
*s_lower_bound = std::fmax(*s_lower_bound, s_lower);
}
}
......
......@@ -56,11 +56,11 @@ class QpPiecewiseStGraph {
// Add st graph constraint
common::Status AddConstraint(const common::TrajectoryPoint& init_point,
const SpeedLimit& speed_limit,
const std::vector<const StBoundary*>& boundaries,
const std::vector<const STBoundary*>& boundaries,
const std::pair<double, double>& accel_bound);
// Add objective function
common::Status AddKernel(const std::vector<const StBoundary*>& boundaries,
common::Status AddKernel(const std::vector<const STBoundary*>& boundaries,
const SpeedLimit& speed_limit);
// solve
......@@ -68,7 +68,7 @@ class QpPiecewiseStGraph {
// extract upper lower bound for constraint;
common::Status GetSConstraintByTime(
const std::vector<const StBoundary*>& boundaries, const double time,
const std::vector<const STBoundary*>& boundaries, const double time,
const double total_path_s, double* const s_upper_bound,
double* const s_lower_bound) const;
......@@ -78,7 +78,7 @@ class QpPiecewiseStGraph {
const double weight);
common::Status AddFollowReferenceLineKernel(
const std::vector<const StBoundary*>& boundaries, const double weight);
const std::vector<const STBoundary*>& boundaries, const double weight);
common::Status EstimateSpeedUpperBound(
const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
......
......@@ -156,7 +156,7 @@ Status QpSplineStGraph::Search(const StGraphData& st_graph_data,
Status QpSplineStGraph::AddConstraint(
const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
const std::vector<const StBoundary*>& boundaries,
const std::vector<const STBoundary*>& boundaries,
const std::pair<double, double>& accel_bound) {
Spline1dConstraint* constraint = spline_solver_->mutable_spline_constraint();
......@@ -253,7 +253,7 @@ Status QpSplineStGraph::AddConstraint(
bool has_follow = false;
double delta_s = 1.0;
for (const auto* boundary : boundaries) {
if (boundary->boundary_type() == StBoundary::BoundaryType::FOLLOW) {
if (boundary->boundary_type() == STBoundary::BoundaryType::FOLLOW) {
has_follow = true;
delta_s = std::fmin(
delta_s, boundary->min_s() - fabs(boundary->characteristic_length()));
......@@ -280,7 +280,7 @@ Status QpSplineStGraph::AddConstraint(
}
Status QpSplineStGraph::AddKernel(
const std::vector<const StBoundary*>& boundaries,
const std::vector<const STBoundary*>& boundaries,
const SpeedLimit& speed_limit) {
Spline1dKernel* spline_kernel = spline_solver_->mutable_spline_kernel();
......@@ -369,7 +369,7 @@ Status QpSplineStGraph::AddCruiseReferenceLineKernel(const double weight) {
}
Status QpSplineStGraph::AddFollowReferenceLineKernel(
const std::vector<const StBoundary*>& boundaries, const double weight) {
const std::vector<const STBoundary*>& boundaries, const double weight) {
auto* spline_kernel = spline_solver_->mutable_spline_kernel();
std::vector<double> ref_s;
std::vector<double> filtered_evaluate_t;
......@@ -378,7 +378,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
double s_min = std::numeric_limits<double>::infinity();
bool success = false;
for (const auto* boundary : boundaries) {
if (boundary->boundary_type() != StBoundary::BoundaryType::FOLLOW) {
if (boundary->boundary_type() != STBoundary::BoundaryType::FOLLOW) {
continue;
}
if (curr_t < boundary->min_t() || curr_t > boundary->max_t()) {
......@@ -421,7 +421,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel(
}
Status QpSplineStGraph::AddYieldReferenceLineKernel(
const std::vector<const StBoundary*>& boundaries, const double weight) {
const std::vector<const STBoundary*>& boundaries, const double weight) {
auto* spline_kernel = spline_solver_->mutable_spline_kernel();
std::vector<double> ref_s;
std::vector<double> filtered_evaluate_t;
......@@ -430,7 +430,7 @@ Status QpSplineStGraph::AddYieldReferenceLineKernel(
double s_min = std::numeric_limits<double>::infinity();
bool success = false;
for (const auto* boundary : boundaries) {
if (boundary->boundary_type() != StBoundary::BoundaryType::YIELD) {
if (boundary->boundary_type() != STBoundary::BoundaryType::YIELD) {
continue;
}
if (curr_t < boundary->min_t() || curr_t > boundary->max_t()) {
......@@ -484,12 +484,12 @@ bool QpSplineStGraph::AddDpStReferenceKernel(const double weight) const {
}
Status QpSplineStGraph::GetSConstraintByTime(
const std::vector<const StBoundary*>& boundaries, const double time,
const std::vector<const 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 StBoundary* boundary : boundaries) {
for (const STBoundary* boundary : boundaries) {
double s_upper = 0.0;
double s_lower = 0.0;
......@@ -497,16 +497,16 @@ Status QpSplineStGraph::GetSConstraintByTime(
continue;
}
if (boundary->boundary_type() == StBoundary::BoundaryType::STOP ||
boundary->boundary_type() == StBoundary::BoundaryType::FOLLOW ||
boundary->boundary_type() == StBoundary::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 if (boundary->boundary_type() ==
StBoundary::BoundaryType::OVERTAKE) {
STBoundary::BoundaryType::OVERTAKE) {
*s_lower_bound = std::fmax(*s_lower_bound, s_lower);
} else {
AWARN << "Unhandled boundary type: "
<< StBoundary::TypeName(boundary->boundary_type());
<< STBoundary::TypeName(boundary->boundary_type());
}
}
......
......@@ -59,11 +59,11 @@ class QpSplineStGraph {
// Add st graph constraint
common::Status AddConstraint(const common::TrajectoryPoint& init_point,
const SpeedLimit& speed_limit,
const std::vector<const StBoundary*>& boundaries,
const std::vector<const STBoundary*>& boundaries,
const std::pair<double, double>& accel_bound);
// Add objective function
common::Status AddKernel(const std::vector<const StBoundary*>& boundaries,
common::Status AddKernel(const std::vector<const STBoundary*>& boundaries,
const SpeedLimit& speed_limit);
// solve
......@@ -71,7 +71,7 @@ class QpSplineStGraph {
// extract upper lower bound for constraint;
common::Status GetSConstraintByTime(
const std::vector<const StBoundary*>& boundaries, const double time,
const std::vector<const STBoundary*>& boundaries, const double time,
const double total_path_s, double* const s_upper_bound,
double* const s_lower_bound) const;
......@@ -80,11 +80,11 @@ class QpSplineStGraph {
// follow line kernel
common::Status AddFollowReferenceLineKernel(
const std::vector<const StBoundary*>& boundaries, const double weight);
const std::vector<const STBoundary*>& boundaries, const double weight);
// yield line kernel
common::Status AddYieldReferenceLineKernel(
const std::vector<const StBoundary*>& boundaries, const double weight);
const std::vector<const STBoundary*>& boundaries, const double weight);
const SpeedData GetHistorySpeed() const;
common::Status EstimateSpeedUpperBound(
......
......@@ -89,7 +89,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
"Mapping obstacle for qp st speed optimizer failed!");
}
std::vector<const StBoundary*> boundaries;
std::vector<const STBoundary*> boundaries;
for (auto* obstacle : path_decision->obstacles().Items()) {
auto id = obstacle->Id();
if (!obstacle->st_boundary().IsEmpty()) {
......
......@@ -62,7 +62,7 @@ common::Status SpeedDecider::Execute(Frame* frame,
SpeedDecider::StPosition SpeedDecider::GetStPosition(
const PathDecision* const path_decision, const SpeedData& speed_profile,
const StBoundary& st_boundary) const {
const STBoundary& st_boundary) const {
StPosition st_position = BELOW;
if (st_boundary.IsEmpty()) {
return st_position;
......@@ -86,7 +86,7 @@ SpeedDecider::StPosition SpeedDecider::GetStPosition(
ADEBUG << "speed profile cross st_boundaries.";
st_position = CROSS;
if (st_boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (st_boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
if (!CheckKeepClearCrossable(path_decision, speed_profile,
st_boundary)) {
st_position = BELOW;
......@@ -111,7 +111,7 @@ SpeedDecider::StPosition SpeedDecider::GetStPosition(
bool SpeedDecider::CheckKeepClearCrossable(
const PathDecision* const path_decision, const SpeedData& speed_profile,
const StBoundary& keep_clear_st_boundary) const {
const STBoundary& keep_clear_st_boundary) const {
bool keep_clear_crossable = true;
const auto& last_speed_point = speed_profile.back();
......@@ -205,7 +205,7 @@ Status SpeedDecider::MakeObjectDecision(
}
auto position = GetStPosition(path_decision, speed_profile, boundary);
if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
if (CheckKeepClearBlocked(path_decision, *obstacle)) {
position = BELOW;
}
......@@ -218,7 +218,7 @@ Status SpeedDecider::MakeObjectDecision(
switch (position) {
case BELOW:
if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
ObjectDecisionType stop_decision;
if (CreateStopDecision(*mutable_obstacle, &stop_decision, 0.0)) {
mutable_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear",
......@@ -254,7 +254,7 @@ Status SpeedDecider::MakeObjectDecision(
}
break;
case ABOVE:
if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
ObjectDecisionType ignore;
ignore.mutable_ignore();
mutable_obstacle->AddLongitudinalDecision("dp_st_graph", ignore);
......@@ -308,7 +308,7 @@ bool SpeedDecider::CreateStopDecision(const Obstacle& obstacle,
const auto& boundary = obstacle.st_boundary();
double fence_s = adc_sl_boundary_.end_s() + boundary.min_s() + stop_distance;
if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
fence_s = obstacle.PerceptionSLBoundary().start_s();
}
const double main_stop_s =
......@@ -329,7 +329,7 @@ bool SpeedDecider::CreateStopDecision(const Obstacle& obstacle,
stop_point->set_z(0.0);
stop->set_stop_heading(fence_point.heading());
if (boundary.boundary_type() == StBoundary::BoundaryType::KEEP_CLEAR) {
if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
stop->set_reason_code(StopReasonCode::STOP_REASON_CLEAR_ZONE);
}
......@@ -465,8 +465,8 @@ bool SpeedDecider::CreateOvertakeDecision(
return true;
}
bool SpeedDecider::CheckIsFollowByT(const StBoundary& boundary) const {
if (boundary.BottomLeftPoint().s() > boundary.BottomRightPoint().s()) {
bool SpeedDecider::CheckIsFollowByT(const STBoundary& boundary) const {
if (boundary.bottom_left_point().s() > boundary.bottom_right_point().s()) {
return false;
}
constexpr double kFollowTimeEpsilon = 1e-3;
......
......@@ -44,12 +44,12 @@ class SpeedDecider : public Task {
StPosition GetStPosition(const PathDecision* const path_decision,
const SpeedData& speed_profile,
const StBoundary& st_boundary) const;
const STBoundary& st_boundary) const;
bool CheckKeepClearCrossable(
const PathDecision* const path_decision,
const SpeedData& speed_profile,
const StBoundary& keep_clear_st_boundary) const;
const STBoundary& keep_clear_st_boundary) const;
bool CheckKeepClearBlocked(
const PathDecision* const path_decision,
......@@ -62,7 +62,7 @@ class SpeedDecider : public Task {
* @return true if the ADC believe it should follow the obstacle, and
* false otherwise.
**/
bool CheckIsFollowByT(const StBoundary& boundary) const;
bool CheckIsFollowByT(const STBoundary& boundary) const;
bool CreateStopDecision(const Obstacle& obstacle,
ObjectDecisionType* const stop_decision,
......
......@@ -66,24 +66,24 @@ void SpeedOptimizer::RecordSTGraphDebug(const StGraphData& st_graph_data,
auto boundary_debug = st_graph_debug->add_boundary();
boundary_debug->set_name(boundary->id());
switch (boundary->boundary_type()) {
case StBoundary::BoundaryType::FOLLOW:
case STBoundary::BoundaryType::FOLLOW:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_FOLLOW);
break;
case StBoundary::BoundaryType::OVERTAKE:
case STBoundary::BoundaryType::OVERTAKE:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_OVERTAKE);
break;
case StBoundary::BoundaryType::STOP:
case STBoundary::BoundaryType::STOP:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_STOP);
break;
case StBoundary::BoundaryType::UNKNOWN:
case STBoundary::BoundaryType::UNKNOWN:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_UNKNOWN);
break;
case StBoundary::BoundaryType::YIELD:
case STBoundary::BoundaryType::YIELD:
boundary_debug->set_type(StGraphBoundaryDebug::ST_BOUNDARY_TYPE_YIELD);
break;
case StBoundary::BoundaryType::KEEP_CLEAR:
case STBoundary::BoundaryType::KEEP_CLEAR:
boundary_debug->set_type(
StGraphBoundaryDebug::ST_BOUNDARY_TYPE_KEEP_CLEAR);
break;
......
......@@ -300,8 +300,8 @@ bool StBoundaryMapper::MapStopDecision(
point_pairs.emplace_back(
STPoint(s_min, planning_time_),
STPoint(s_max + st_boundary_config_.boundary_buffer(), planning_time_));
auto boundary = StBoundary(point_pairs);
boundary.SetBoundaryType(StBoundary::BoundaryType::STOP);
auto boundary = STBoundary(point_pairs);
boundary.SetBoundaryType(STBoundary::BoundaryType::STOP);
boundary.SetCharacteristicLength(st_boundary_config_.boundary_buffer());
boundary.set_id(stop_obstacle->Id());
stop_obstacle->SetStBoundary(boundary);
......@@ -317,7 +317,7 @@ Status StBoundaryMapper::MapWithoutDecision(Obstacle* obstacle) const {
return Status::OK();
}
auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
auto boundary = STBoundary::CreateInstance(lower_points, upper_points)
.ExpandByS(boundary_s_buffer)
.ExpandByT(boundary_t_buffer);
boundary.set_id(obstacle->Id());
......@@ -496,24 +496,24 @@ Status StBoundaryMapper::MapWithDecision(
lower_points.emplace_back(extend_lower_s, planning_time_);
}
auto boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
auto boundary = STBoundary::CreateInstance(lower_points, upper_points)
.ExpandByS(boundary_s_buffer)
.ExpandByT(boundary_t_buffer);
// get characteristic_length and boundary_type.
StBoundary::BoundaryType b_type = StBoundary::BoundaryType::UNKNOWN;
STBoundary::BoundaryType b_type = STBoundary::BoundaryType::UNKNOWN;
double characteristic_length = 0.0;
if (decision.has_follow()) {
characteristic_length = std::fabs(decision.follow().distance_s());
b_type = StBoundary::BoundaryType::FOLLOW;
b_type = STBoundary::BoundaryType::FOLLOW;
} else if (decision.has_yield()) {
characteristic_length = std::fabs(decision.yield().distance_s());
boundary = StBoundary::GenerateStBoundary(lower_points, upper_points)
boundary = STBoundary::CreateInstance(lower_points, upper_points)
.ExpandByS(characteristic_length);
b_type = StBoundary::BoundaryType::YIELD;
b_type = STBoundary::BoundaryType::YIELD;
} else if (decision.has_overtake()) {
characteristic_length = std::fabs(decision.overtake().distance_s());
b_type = StBoundary::BoundaryType::OVERTAKE;
b_type = STBoundary::BoundaryType::OVERTAKE;
} else {
DCHECK(false) << "Obj decision should be either yield or overtake: "
<< decision.DebugString();
......
......@@ -25,7 +25,7 @@ namespace planning {
using apollo::common::TrajectoryPoint;
StGraphData::StGraphData(const std::vector<const StBoundary*>& st_boundaries,
StGraphData::StGraphData(const std::vector<const STBoundary*>& st_boundaries,
const TrajectoryPoint& init_point,
const SpeedLimit& speed_limit,
const double path_data_length)
......@@ -34,7 +34,7 @@ StGraphData::StGraphData(const std::vector<const StBoundary*>& st_boundaries,
speed_limit_(speed_limit),
path_data_length_(path_data_length) {}
const std::vector<const StBoundary*>& StGraphData::st_boundaries() const {
const std::vector<const STBoundary*>& StGraphData::st_boundaries() const {
return st_boundaries_;
}
......
......@@ -33,12 +33,12 @@ namespace planning {
class StGraphData {
public:
StGraphData(const std::vector<const StBoundary*>& st_boundaries,
StGraphData(const std::vector<const STBoundary*>& st_boundaries,
const apollo::common::TrajectoryPoint& init_point,
const SpeedLimit& speed_limit, const double path_data_length);
StGraphData() = default;
const std::vector<const StBoundary*>& st_boundaries() const;
const std::vector<const STBoundary*>& st_boundaries() const;
const apollo::common::TrajectoryPoint& init_point() const;
......@@ -47,7 +47,7 @@ class StGraphData {
double path_data_length() const;
private:
std::vector<const StBoundary*> st_boundaries_;
std::vector<const STBoundary*> st_boundaries_;
apollo::common::TrajectoryPoint init_point_;
SpeedLimit speed_limit_;
......
......@@ -25,8 +25,8 @@ namespace apollo {
namespace planning {
TEST(StGraphDataTest, basic_test) {
std::vector<const StBoundary*> boundary_vec;
auto boundary = StBoundary();
std::vector<const STBoundary*> boundary_vec;
auto boundary = STBoundary();
boundary_vec.push_back(&boundary);
apollo::common::TrajectoryPoint traj_point;
traj_point.mutable_path_point()->set_x(1.1);
......
......@@ -176,7 +176,7 @@ bool KeepClear::BuildKeepClearObstacle(
return false;
}
path_obstacle->SetReferenceLineStBoundaryType(
StBoundary::BoundaryType::KEEP_CLEAR);
STBoundary::BoundaryType::KEEP_CLEAR);
return true;
}
......
......@@ -158,13 +158,13 @@ void AutotuningRawFeatureGenerator::GenerateSTBoundaries(
}
void AutotuningRawFeatureGenerator::ConvertToDiscretizedBoundaries(
const StBoundary& boundary, const double speed) {
const STBoundary& boundary, const double speed) {
for (size_t i = 0; i < eval_time_.size(); ++i) {
double upper = 0.0;
double lower = 0.0;
bool suc = boundary.GetBoundarySRange(eval_time_[i], &upper, &lower);
if (suc) {
if (boundary.boundary_type() == StBoundary::BoundaryType::STOP) {
if (boundary.boundary_type() == STBoundary::BoundaryType::STOP) {
stop_boundaries_[i].push_back({{lower, upper, speed}});
} else {
obs_boundaries_[i].push_back({{lower, upper, speed}});
......
......@@ -75,7 +75,7 @@ class AutotuningRawFeatureGenerator {
/**
* Convert st boundaries to discretized boundaries
*/
void ConvertToDiscretizedBoundaries(const StBoundary& boundary,
void ConvertToDiscretizedBoundaries(const STBoundary& boundary,
const double speed);
common::Status EvaluateSpeedPoint(const common::SpeedPoint& speed_point,
......@@ -88,7 +88,7 @@ class AutotuningRawFeatureGenerator {
const ReferenceLineInfo& reference_line_info_;
const Frame& frame_;
const SpeedLimit& speed_limit_;
std::vector<const StBoundary*> boundaries_;
std::vector<const STBoundary*> boundaries_;
// covers the boundary info lower s upper s as well as the speed of obs
std::vector<std::vector<std::array<double, 3>>> obs_boundaries_;
......
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册