From 9f1b865892d30246824b076add025bb43a7fa0d5 Mon Sep 17 00:00:00 2001 From: Yajia Zhang Date: Fri, 8 Feb 2019 12:49:54 -0800 Subject: [PATCH] planning: renamed STBoundary; minor code refactor --- modules/planning/common/obstacle.cc | 22 ++--- modules/planning/common/obstacle.h | 18 ++--- modules/planning/common/path_decision.cc | 2 +- modules/planning/common/path_decision.h | 2 +- modules/planning/common/speed/BUILD | 1 + modules/planning/common/speed/st_boundary.cc | 81 ++++++++++--------- modules/planning/common/speed/st_boundary.h | 40 ++++----- .../planning/common/speed/st_boundary_test.cc | 12 +-- .../lattice/behavior/path_time_graph.cc | 33 ++++---- .../lattice/behavior/path_time_graph.h | 8 +- .../planning/tasks/deciders/decider_creep.cc | 4 +- .../optimizers/dp_st_speed/dp_st_cost.cc | 2 +- .../optimizers/dp_st_speed/dp_st_graph.cc | 6 +- .../dp_st_speed/dp_st_graph_test.cc | 4 +- .../dp_st_speed/dp_st_speed_optimizer.cc | 4 +- .../optimizers/path_decider/path_decider.cc | 2 +- .../poly_vt_speed/poly_vt_speed_optimizer.cc | 12 +-- .../qp_piecewise_st_graph.cc | 20 ++--- .../qp_piecewise_st_graph.h | 8 +- .../qp_spline_st_speed/qp_spline_st_graph.cc | 28 +++---- .../qp_spline_st_speed/qp_spline_st_graph.h | 10 +-- .../qp_spline_st_speed_optimizer.cc | 2 +- .../optimizers/speed_decider/speed_decider.cc | 20 ++--- .../optimizers/speed_decider/speed_decider.h | 6 +- .../tasks/optimizers/speed_optimizer.cc | 12 +-- .../optimizers/st_graph/st_boundary_mapper.cc | 18 ++--- .../optimizers/st_graph/st_graph_data.cc | 4 +- .../tasks/optimizers/st_graph/st_graph_data.h | 6 +- .../optimizers/st_graph/st_graph_data_test.cc | 4 +- modules/planning/traffic_rules/keep_clear.cc | 2 +- .../autotuning_raw_feature_generator.cc | 4 +- .../tuning/autotuning_raw_feature_generator.h | 4 +- 32 files changed, 204 insertions(+), 197 deletions(-) diff --git a/modules/planning/common/obstacle.cc b/modules/planning/common/obstacle.cc index a123c1ac0d..f831a8ebcb 100644 --- a/modules/planning/common/obstacle.cc +++ b/modules/planning/common/obstacle.cc @@ -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& 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( diff --git a/modules/planning/common/obstacle.h b/modules/planning/common/obstacle.h index bbdaad7aa4..bf5f6ebad1 100644 --- a/modules/planning/common/obstacle.h +++ b/modules/planning/common/obstacle.h @@ -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& 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 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_; diff --git a/modules/planning/common/path_decision.cc b/modules/planning/common/path_decision.cc index 36861233df..f779e3fd58 100644 --- a/modules/planning/common/path_decision.cc +++ b/modules/planning/common/path_decision.cc @@ -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) { diff --git a/modules/planning/common/path_decision.h b/modules/planning/common/path_decision.h index 1a6c36c101..e57bc82ede 100644 --- a/modules/planning/common/path_decision.h +++ b/modules/planning/common/path_decision.h @@ -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_; } diff --git a/modules/planning/common/speed/BUILD b/modules/planning/common/speed/BUILD index 164a37de95..9824c4b845 100644 --- a/modules/planning/common/speed/BUILD +++ b/modules/planning/common/speed/BUILD @@ -29,6 +29,7 @@ cc_library( "//cyber/common:log", "//modules/planning/proto:planning_proto", "@gtest", + "//modules/planning/common:planning_gflags", ], ) diff --git a/modules/planning/common/speed/st_boundary.cc b/modules/planning/common/speed/st_boundary.cc index 8af7e6d3e8..92d027c787 100644 --- a/modules/planning/common/speed/st_boundary.cc +++ b/modules/planning/common/speed/st_boundary.cc @@ -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>& 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>* 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>& 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> 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> 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& points, +bool STBoundary::GetIndexRange(const std::vector& points, const double t, size_t* left, size_t* right) const { CHECK_NOTNULL(left); @@ -370,11 +371,11 @@ bool StBoundary::GetIndexRange(const std::vector& points, return true; } -StBoundary StBoundary::GenerateStBoundary( +STBoundary STBoundary::CreateInstance( const std::vector& lower_points, const std::vector& upper_points) { if (lower_points.size() != upper_points.size() || lower_points.size() < 2) { - return StBoundary(); + return STBoundary(); } std::vector> 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 lower_points; std::vector 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); } diff --git a/modules/planning/common/speed/st_boundary.h b/modules/planning/common/speed/st_boundary.h index b91e4764f1..f8b3210466 100644 --- a/modules/planning/common/speed/st_boundary.h +++ b/modules/planning/common/speed/st_boundary.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include #include @@ -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>& point_pairs); - explicit StBoundary(const common::math::Box2d& box) = delete; - explicit StBoundary(std::vector points) = delete; + explicit STBoundary(const common::math::Box2d& box) = delete; - ~StBoundary() = default; + explicit STBoundary(std::vector points) = delete; + + static STBoundary CreateInstance( + const std::vector& lower_points, + const std::vector& upper_points); + + static std::unique_ptr CreateInstance( + const std::vector>& 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 upper_points() const { return upper_points_; } std::vector lower_points() const { return lower_points_; } - static StBoundary GenerateStBoundary( - const std::vector& lower_points, - const std::vector& 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::max(); double max_s_ = std::numeric_limits::lowest(); double min_t_ = std::numeric_limits::max(); diff --git a/modules/planning/common/speed/st_boundary_test.cc b/modules/planning/common/speed/st_boundary_test.cc index 5d278b715e..75a49e92eb 100644 --- a/modules/planning/common/speed/st_boundary_test.cc +++ b/modules/planning/common/speed/st_boundary_test.cc @@ -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); diff --git a/modules/planning/lattice/behavior/path_time_graph.cc b/modules/planning/lattice/behavior/path_time_graph.cc index f4aaef45ac..250b275cad 100644 --- a/modules/planning/lattice/behavior/path_time_graph.cc +++ b/modules/planning/lattice/behavior/path_time_graph.cc @@ -203,13 +203,13 @@ STPoint PathTimeGraph::SetPathTimePoint(const std::string& obstacle_id, return path_time_point; } -const std::vector& PathTimeGraph::GetPathTimeObstacles() +const std::vector& 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> 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 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; diff --git a/modules/planning/lattice/behavior/path_time_graph.h b/modules/planning/lattice/behavior/path_time_graph.h index edd0237c44..aefead2de9 100644 --- a/modules/planning/lattice/behavior/path_time_graph.h +++ b/modules/planning/lattice/behavior/path_time_graph.h @@ -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& init_d); - const std::vector& GetPathTimeObstacles() const; + const std::vector& GetPathTimeObstacles() const; bool GetPathTimeObstacle(const std::string& obstacle_id, - StBoundary* path_time_obstacle); + STBoundary* path_time_obstacle); std::vector> GetPathBlockingIntervals( const double t) const; @@ -102,8 +102,8 @@ class PathTimeGraph { const ReferenceLineInfo* ptr_reference_line_info_; std::array init_d_; - std::unordered_map path_time_obstacle_map_; - std::vector path_time_obstacles_; + std::unordered_map path_time_obstacle_map_; + std::vector path_time_obstacles_; std::vector static_obs_sl_boundaries_; }; diff --git a/modules/planning/tasks/deciders/decider_creep.cc b/modules/planning/tasks/deciders/decider_creep.cc index 3e0ca05b3a..018f23f01a 100644 --- a/modules/planning/tasks/deciders/decider_creep.cc +++ b/modules/planning/tasks/deciders/decider_creep.cc @@ -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() diff --git a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc index e0c2b5cef4..87f1947a0f 100644 --- a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc +++ b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_cost.cc @@ -59,7 +59,7 @@ void DpStCost::AddToKeepClearRange( continue; } if (obstacle->st_boundary().boundary_type() != - StBoundary::BoundaryType::KEEP_CLEAR) { + STBoundary::BoundaryType::KEEP_CLEAR) { continue; } diff --git a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc index c0de8a4e48..1d06b27fde 100644 --- a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc +++ b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph.cc @@ -44,11 +44,11 @@ namespace { constexpr double kInf = std::numeric_limits::infinity(); -bool CheckOverlapOnDpStGraph(const std::vector& boundaries, +bool CheckOverlapOnDpStGraph(const std::vector& 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}) || diff --git a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc index 04f140d787..e541bc5d8e 100644 --- a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc +++ b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_graph_test.cc @@ -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 boundaries; + std::vector boundaries; boundaries.push_back(&(obstacles_.back()->st_boundary())); init_point_.mutable_path_point()->set_x(0.0); diff --git a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc index 3bc9fcc414..057ef8513b 100644 --- a/modules/planning/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc +++ b/modules/planning/tasks/optimizers/dp_st_speed/dp_st_speed_optimizer.cc @@ -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 boundaries; + std::vector 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); diff --git a/modules/planning/tasks/optimizers/path_decider/path_decider.cc b/modules/planning/tasks/optimizers/path_decider/path_decider.cc index 5070d3d5ce..affdf4fc33 100644 --- a/modules/planning/tasks/optimizers/path_decider/path_decider.cc +++ b/modules/planning/tasks/optimizers/path_decider/path_decider.cc @@ -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; } diff --git a/modules/planning/tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc b/modules/planning/tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc index f228d90b9a..de4516fde9 100644 --- a/modules/planning/tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc +++ b/modules/planning/tasks/optimizers/poly_vt_speed/poly_vt_speed_optimizer.cc @@ -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; diff --git a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc index 70a41a849b..f5f17e35bc 100644 --- a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc +++ b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.cc @@ -123,7 +123,7 @@ Status QpPiecewiseStGraph::Search( Status QpPiecewiseStGraph::AddConstraint( const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit, - const std::vector& boundaries, + const std::vector& boundaries, const std::pair& accel_bound) { auto* constraint = generator_->mutable_constraint(); // position, velocity, acceleration @@ -212,7 +212,7 @@ Status QpPiecewiseStGraph::AddConstraint( } Status QpPiecewiseStGraph::AddKernel( - const std::vector& boundaries, + const std::vector& 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& boundaries, const double weight) { + const std::vector& boundaries, const double weight) { auto* follow_kernel = generator_->mutable_kernel(); std::vector ref_s; std::vector filtered_evaluate_t; @@ -294,7 +294,7 @@ Status QpPiecewiseStGraph::AddFollowReferenceLineKernel( double s_min = std::numeric_limits::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& boundaries, const double time, + const std::vector& 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); } } diff --git a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h index e995cad800..3332b13f0a 100644 --- a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h +++ b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_piecewise_st_graph.h @@ -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& boundaries, + const std::vector& boundaries, const std::pair& accel_bound); // Add objective function - common::Status AddKernel(const std::vector& boundaries, + common::Status AddKernel(const std::vector& boundaries, const SpeedLimit& speed_limit); // solve @@ -68,7 +68,7 @@ class QpPiecewiseStGraph { // extract upper lower bound for constraint; common::Status GetSConstraintByTime( - const std::vector& boundaries, const double time, + const std::vector& 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& boundaries, const double weight); + const std::vector& boundaries, const double weight); common::Status EstimateSpeedUpperBound( const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit, diff --git a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc index 99e215032b..b357e53887 100644 --- a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc +++ b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.cc @@ -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& boundaries, + const std::vector& boundaries, const std::pair& 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& boundaries, + const std::vector& 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& boundaries, const double weight) { + const std::vector& boundaries, const double weight) { auto* spline_kernel = spline_solver_->mutable_spline_kernel(); std::vector ref_s; std::vector filtered_evaluate_t; @@ -378,7 +378,7 @@ Status QpSplineStGraph::AddFollowReferenceLineKernel( double s_min = std::numeric_limits::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& boundaries, const double weight) { + const std::vector& boundaries, const double weight) { auto* spline_kernel = spline_solver_->mutable_spline_kernel(); std::vector ref_s; std::vector filtered_evaluate_t; @@ -430,7 +430,7 @@ Status QpSplineStGraph::AddYieldReferenceLineKernel( double s_min = std::numeric_limits::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& boundaries, const double time, + const std::vector& 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()); } } diff --git a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h index 260f3fe7fb..e14327c625 100644 --- a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h +++ b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_graph.h @@ -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& boundaries, + const std::vector& boundaries, const std::pair& accel_bound); // Add objective function - common::Status AddKernel(const std::vector& boundaries, + common::Status AddKernel(const std::vector& boundaries, const SpeedLimit& speed_limit); // solve @@ -71,7 +71,7 @@ class QpSplineStGraph { // extract upper lower bound for constraint; common::Status GetSConstraintByTime( - const std::vector& boundaries, const double time, + const std::vector& 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& boundaries, const double weight); + const std::vector& boundaries, const double weight); // yield line kernel common::Status AddYieldReferenceLineKernel( - const std::vector& boundaries, const double weight); + const std::vector& boundaries, const double weight); const SpeedData GetHistorySpeed() const; common::Status EstimateSpeedUpperBound( diff --git a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc index 8f355c3ad0..728762b5ba 100644 --- a/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc +++ b/modules/planning/tasks/optimizers/qp_spline_st_speed/qp_spline_st_speed_optimizer.cc @@ -89,7 +89,7 @@ Status QpSplineStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary, "Mapping obstacle for qp st speed optimizer failed!"); } - std::vector boundaries; + std::vector boundaries; for (auto* obstacle : path_decision->obstacles().Items()) { auto id = obstacle->Id(); if (!obstacle->st_boundary().IsEmpty()) { diff --git a/modules/planning/tasks/optimizers/speed_decider/speed_decider.cc b/modules/planning/tasks/optimizers/speed_decider/speed_decider.cc index 46e3df5e01..4fbe6ab20c 100644 --- a/modules/planning/tasks/optimizers/speed_decider/speed_decider.cc +++ b/modules/planning/tasks/optimizers/speed_decider/speed_decider.cc @@ -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; diff --git a/modules/planning/tasks/optimizers/speed_decider/speed_decider.h b/modules/planning/tasks/optimizers/speed_decider/speed_decider.h index bb7dc79370..7d2d66ffd1 100644 --- a/modules/planning/tasks/optimizers/speed_decider/speed_decider.h +++ b/modules/planning/tasks/optimizers/speed_decider/speed_decider.h @@ -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, diff --git a/modules/planning/tasks/optimizers/speed_optimizer.cc b/modules/planning/tasks/optimizers/speed_optimizer.cc index e50601678f..c5f7f9e777 100644 --- a/modules/planning/tasks/optimizers/speed_optimizer.cc +++ b/modules/planning/tasks/optimizers/speed_optimizer.cc @@ -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; diff --git a/modules/planning/tasks/optimizers/st_graph/st_boundary_mapper.cc b/modules/planning/tasks/optimizers/st_graph/st_boundary_mapper.cc index a533715157..a7d49618e9 100644 --- a/modules/planning/tasks/optimizers/st_graph/st_boundary_mapper.cc +++ b/modules/planning/tasks/optimizers/st_graph/st_boundary_mapper.cc @@ -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(); diff --git a/modules/planning/tasks/optimizers/st_graph/st_graph_data.cc b/modules/planning/tasks/optimizers/st_graph/st_graph_data.cc index 1f0a882ece..0d9b39c16f 100644 --- a/modules/planning/tasks/optimizers/st_graph/st_graph_data.cc +++ b/modules/planning/tasks/optimizers/st_graph/st_graph_data.cc @@ -25,7 +25,7 @@ namespace planning { using apollo::common::TrajectoryPoint; -StGraphData::StGraphData(const std::vector& st_boundaries, +StGraphData::StGraphData(const std::vector& st_boundaries, const TrajectoryPoint& init_point, const SpeedLimit& speed_limit, const double path_data_length) @@ -34,7 +34,7 @@ StGraphData::StGraphData(const std::vector& st_boundaries, speed_limit_(speed_limit), path_data_length_(path_data_length) {} -const std::vector& StGraphData::st_boundaries() const { +const std::vector& StGraphData::st_boundaries() const { return st_boundaries_; } diff --git a/modules/planning/tasks/optimizers/st_graph/st_graph_data.h b/modules/planning/tasks/optimizers/st_graph/st_graph_data.h index d5a7df2231..8c4fd03b7f 100644 --- a/modules/planning/tasks/optimizers/st_graph/st_graph_data.h +++ b/modules/planning/tasks/optimizers/st_graph/st_graph_data.h @@ -33,12 +33,12 @@ namespace planning { class StGraphData { public: - StGraphData(const std::vector& st_boundaries, + StGraphData(const std::vector& st_boundaries, const apollo::common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit, const double path_data_length); StGraphData() = default; - const std::vector& st_boundaries() const; + const std::vector& st_boundaries() const; const apollo::common::TrajectoryPoint& init_point() const; @@ -47,7 +47,7 @@ class StGraphData { double path_data_length() const; private: - std::vector st_boundaries_; + std::vector st_boundaries_; apollo::common::TrajectoryPoint init_point_; SpeedLimit speed_limit_; diff --git a/modules/planning/tasks/optimizers/st_graph/st_graph_data_test.cc b/modules/planning/tasks/optimizers/st_graph/st_graph_data_test.cc index bffb2d1a94..84093ac34e 100644 --- a/modules/planning/tasks/optimizers/st_graph/st_graph_data_test.cc +++ b/modules/planning/tasks/optimizers/st_graph/st_graph_data_test.cc @@ -25,8 +25,8 @@ namespace apollo { namespace planning { TEST(StGraphDataTest, basic_test) { - std::vector boundary_vec; - auto boundary = StBoundary(); + std::vector boundary_vec; + auto boundary = STBoundary(); boundary_vec.push_back(&boundary); apollo::common::TrajectoryPoint traj_point; traj_point.mutable_path_point()->set_x(1.1); diff --git a/modules/planning/traffic_rules/keep_clear.cc b/modules/planning/traffic_rules/keep_clear.cc index e875bfe526..e3d50e791a 100644 --- a/modules/planning/traffic_rules/keep_clear.cc +++ b/modules/planning/traffic_rules/keep_clear.cc @@ -176,7 +176,7 @@ bool KeepClear::BuildKeepClearObstacle( return false; } path_obstacle->SetReferenceLineStBoundaryType( - StBoundary::BoundaryType::KEEP_CLEAR); + STBoundary::BoundaryType::KEEP_CLEAR); return true; } diff --git a/modules/planning/tuning/autotuning_raw_feature_generator.cc b/modules/planning/tuning/autotuning_raw_feature_generator.cc index 0e23b0cebf..a4e8632071 100644 --- a/modules/planning/tuning/autotuning_raw_feature_generator.cc +++ b/modules/planning/tuning/autotuning_raw_feature_generator.cc @@ -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}}); diff --git a/modules/planning/tuning/autotuning_raw_feature_generator.h b/modules/planning/tuning/autotuning_raw_feature_generator.h index f8f3813281..48eb7a1826 100644 --- a/modules/planning/tuning/autotuning_raw_feature_generator.h +++ b/modules/planning/tuning/autotuning_raw_feature_generator.h @@ -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 boundaries_; + std::vector boundaries_; // covers the boundary info lower s upper s as well as the speed of obs std::vector>> obs_boundaries_; -- GitLab