From 7504c15643b77af3a44dcfa22d047b1327cd76ef Mon Sep 17 00:00:00 2001
From: Dong Li <lidong.main@gmail.com>
Date: Mon, 7 Aug 2017 16:19:03 -0700
Subject: [PATCH] planning added min_s, min_t, max_s, max_t function in st
 graph. which is used to calculate the fence point in st graph

---
 .../optimizer/st_graph/st_graph_boundary.cc     | 17 +++++++++++++++++
 .../optimizer/st_graph/st_graph_boundary.h      |  9 +++++++++
 .../st_graph/st_graph_boundary_test.cc          |  4 ++++
 3 files changed, 30 insertions(+)

diff --git a/modules/planning/optimizer/st_graph/st_graph_boundary.cc b/modules/planning/optimizer/st_graph/st_graph_boundary.cc
index 3f14ca17a3..8549594343 100644
--- a/modules/planning/optimizer/st_graph/st_graph_boundary.cc
+++ b/modules/planning/optimizer/st_graph/st_graph_boundary.cc
@@ -46,6 +46,12 @@ StGraphBoundary::StGraphBoundary(const PathObstacle* path_obstacle,
   CHECK_EQ(points.size(), 4)
       << "StGraphBoundary must have exactly four points. Input points size: "
       << points.size();
+  for (const auto& point : points) {
+    min_s_ = std::fmin(min_s_, point.s());
+    min_t_ = std::fmin(min_t_, point.t());
+    max_s_ = std::fmax(max_s_, point.s());
+    max_t_ = std::fmax(max_t_, point.t());
+  }
 }
 
 StGraphBoundary::StGraphBoundary(
@@ -56,6 +62,12 @@ StGraphBoundary::StGraphBoundary(
   CHECK_EQ(points.size(), 4)
       << "StGraphBoundary must have exactly four points. Input points size: "
       << points.size();
+  for (const auto& point : points) {
+    min_s_ = std::fmin(min_s_, point.y());
+    min_t_ = std::fmin(min_t_, point.x());
+    max_s_ = std::fmax(max_s_, point.y());
+    max_t_ = std::fmax(max_t_, point.x());
+  }
 }
 
 bool StGraphBoundary::IsPointInBoundary(
@@ -153,5 +165,10 @@ void StGraphBoundary::GetBoundaryTimeScope(double* start_t,
   *end_t = right.t();
 }
 
+double StGraphBoundary::min_s() const { return min_s_; }
+double StGraphBoundary::min_t() const { return min_t_; }
+double StGraphBoundary::max_s() const { return max_s_; }
+double StGraphBoundary::max_t() const { return max_t_; }
+
 }  // namespace planning
 }  // namespace apollo
diff --git a/modules/planning/optimizer/st_graph/st_graph_boundary.h b/modules/planning/optimizer/st_graph/st_graph_boundary.h
index d756c46adf..02f817b4ce 100644
--- a/modules/planning/optimizer/st_graph/st_graph_boundary.h
+++ b/modules/planning/optimizer/st_graph/st_graph_boundary.h
@@ -79,12 +79,21 @@ class StGraphBoundary : public common::math::Polygon2d {
 
   const PathObstacle* path_obstacle() const;
 
+  double min_s() const;
+  double min_t() const;
+  double max_s() const;
+  double max_t() const;
+
  private:
   const PathObstacle* path_obstacle_ = nullptr;
   BoundaryType boundary_type_ = BoundaryType::UNKNOWN;
   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();
+  double max_t_ = std::numeric_limits<double>::lowest();
 };
 
 }  // namespace planning
diff --git a/modules/planning/optimizer/st_graph/st_graph_boundary_test.cc b/modules/planning/optimizer/st_graph/st_graph_boundary_test.cc
index ab02c65e7a..c4f34971f0 100644
--- a/modules/planning/optimizer/st_graph/st_graph_boundary_test.cc
+++ b/modules/planning/optimizer/st_graph/st_graph_boundary_test.cc
@@ -45,6 +45,10 @@ TEST(StGraphBoundaryTest, basic_test) {
   boundary.GetBoundaryTimeScope(&left_t, &right_t);
   EXPECT_DOUBLE_EQ(left_t, 0.0);
   EXPECT_DOUBLE_EQ(right_t, 10.0);
+  EXPECT_FLOAT_EQ(0.0, boundary.min_s());
+  EXPECT_FLOAT_EQ(5.0, boundary.max_s());
+  EXPECT_FLOAT_EQ(0.0, boundary.min_t());
+  EXPECT_FLOAT_EQ(10.0, boundary.max_t());
 }
 
 }  // namespace planning
-- 
GitLab