From 4e6dfb5a769455f3c19574054b3b62240a5d3f5c Mon Sep 17 00:00:00 2001
From: abco20 <meijizifuyong@gmail.com>
Date: Thu, 27 Feb 2025 14:51:02 +0900
Subject: [PATCH] move `getStopLinePolygon` method

---
 .../traffic_simulator/hdmap_utils/hdmap_utils.hpp |  2 --
 .../lanelet_wrapper/lanelet_map.hpp               |  2 ++
 .../src/hdmap_utils/hdmap_utils.cpp               | 15 ---------------
 .../src/lanelet_wrapper/lanelet_map.cpp           |  6 ++++++
 .../test/src/hdmap_utils/test_hdmap_utils.cpp     |  6 ++++--
 5 files changed, 12 insertions(+), 19 deletions(-)

diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
index ecba6c46fbd..dba499ade63 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
@@ -213,8 +213,6 @@ class HdMapUtils
                             traffic_simulator::RoutingConfiguration().routing_graph_type) const
     -> double;
 
-  auto getStopLinePolygon(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
-
   auto getTangentVector(const lanelet::Id, const double s) const
     -> std::optional<geometry_msgs::msg::Vector3>;
 
diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp
index 26ab3e94596..08d093cd99c 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp
+++ b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_map.hpp
@@ -96,6 +96,8 @@ auto leftBound(const lanelet::Id lanelet_id) -> std::vector<Point>;
 auto rightBound(const lanelet::Id lanelet_id) -> std::vector<Point>;
 
 // Polygons
+auto stopLinePolygon(const lanelet::Id lanelet_id) -> std::vector<Point>;
+
 auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector<Point>;
 
 // Objects on path
diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
index b9039755ab0..43b5b586798 100644
--- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
+++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
@@ -1285,21 +1285,6 @@ auto HdMapUtils::getTrafficLightStopLinesPoints(const lanelet::Id traffic_light_
   return ret;
 }
 
-auto HdMapUtils::getStopLinePolygon(const lanelet::Id lanelet_id) const
-  -> std::vector<geometry_msgs::msg::Point>
-{
-  std::vector<geometry_msgs::msg::Point> points;
-  const auto stop_line = lanelet_map_ptr_->lineStringLayer.get(lanelet_id);
-  for (const auto & point : stop_line) {
-    geometry_msgs::msg::Point p;
-    p.x = point.x();
-    p.y = point.y();
-    p.z = point.z();
-    points.emplace_back(p);
-  }
-  return points;
-}
-
 auto HdMapUtils::getTrafficLightIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids
 {
   lanelet::Ids ids;
diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp
index 7ddcec65762..017e867b5f5 100644
--- a/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp
+++ b/simulation/traffic_simulator/src/lanelet_wrapper/lanelet_map.cpp
@@ -248,6 +248,12 @@ auto rightBound(const lanelet::Id lanelet_id) -> std::vector<Point>
 }
 
 // Polygons
+auto stopLinePolygon(const lanelet::Id lanelet_id) -> std::vector<Point>
+{
+  /// @todo here you should probably add a verify if the passed lanelet_id is indeed a stop_line
+  return toPolygon(LaneletWrapper::map()->lineStringLayer.get(lanelet_id));
+}
+
 auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector<Point>
 {
   std::vector<Point> points;
diff --git a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp
index e7c0a20a2ab..62c13f8d399 100644
--- a/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp
+++ b/simulation/traffic_simulator/test/src/hdmap_utils/test_hdmap_utils.cpp
@@ -2418,7 +2418,8 @@ TEST_F(
  */
 TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getStopLinePolygon_stopLine)
 {
-  const auto result_stoplines_points = hdmap_utils.getStopLinePolygon(lanelet::Id{120663});
+  const auto result_stoplines_points =
+    traffic_simulator::lanelet_wrapper::lanelet_map::stopLinePolygon(lanelet::Id{120663});
   const auto actual_stoplines_points = std::vector<geometry_msgs::msg::Point>{
     makePoint(3768.5, 73737.5, -0.5), makePoint(3765.5, 73735.5, -0.5)};
 
@@ -2433,7 +2434,8 @@ TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getStopLinePolygon_stopLine)
  */
 TEST_F(HdMapUtilsTest_CrossroadsWithStoplinesMap, getStopLinePolygon_invalidLaneletId)
 {
-  EXPECT_THROW(hdmap_utils.getStopLinePolygon(1000039), std::runtime_error);
+  EXPECT_THROW(
+    traffic_simulator::lanelet_wrapper::lanelet_map::stopLinePolygon(1000039), std::runtime_error);
 }
 
 /**