move getDistanceToStopLine method

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -55,9 +55,6 @@ class ActionNode : public BT::ActionNodeBase
     -> double;
   auto getDistanceToFrontEntity(const math::geometry::CatmullRomSplineInterface & spline) const
     -> std::optional<double>;
-  auto getDistanceToStopLine(
-    const lanelet::Ids & route_lanelets,
-    const std::vector<geometry_msgs::msg::Point> & waypoints) const -> std::optional<double>;
   auto getDistanceToTrafficLightStopLine(
     const lanelet::Ids & route_lanelets,
     const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>;

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -224,13 +224,6 @@ auto ActionNode::getDistanceToTrafficLightStopLine(
   return std::nullopt;
 }
 
-auto ActionNode::getDistanceToStopLine(
-  const lanelet::Ids & route_lanelets,
-  const std::vector<geometry_msgs::msg::Point> & waypoints) const -> std::optional<double>
-{
-  return hdmap_utils->getDistanceToStopLine(route_lanelets, waypoints);
-}
-
 auto ActionNode::getDistanceToFrontEntity(
   const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>
 {

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp

@@ -90,7 +90,8 @@ BT::NodeStatus FollowFrontEntityAction::tick()
   if (trajectory == nullptr) {
     return BT::NodeStatus::FAILURE;
   }
-  auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory);
+  auto distance_to_stopline =
+    traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory);
   auto distance_to_conflicting_entity = getDistanceToConflictingEntity(route_lanelets, *trajectory);
   const auto front_entity_name = getFrontEntityName(*trajectory);
   if (!front_entity_name) {

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp

@@ -108,7 +108,8 @@ BT::NodeStatus FollowLaneAction::tick()
         return BT::NodeStatus::FAILURE;
       }
     }
-    auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory);
+    auto distance_to_stopline =
+      traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory);
     auto distance_to_conflicting_entity =
       getDistanceToConflictingEntity(route_lanelets, *trajectory);
     if (distance_to_stopline) {

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_crossing_entity_action.cpp

@@ -112,7 +112,8 @@ BT::NodeStatus StopAtCrossingEntityAction::tick()
     return BT::NodeStatus::FAILURE;
   }
   distance_to_stop_target_ = getDistanceToConflictingEntity(route_lanelets, *trajectory);
-  auto distance_to_stopline = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory);
+  auto distance_to_stopline =
+    traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory);
   const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
   if (!distance_to_stop_target_) {
     in_stop_sequence_ = false;

simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/stop_at_stop_line_action.cpp

@@ -108,7 +108,8 @@ BT::NodeStatus StopAtStopLineAction::tick()
   if (trajectory == nullptr) {
     return BT::NodeStatus::FAILURE;
   }
-  distance_to_stopline_ = hdmap_utils->getDistanceToStopLine(route_lanelets, *trajectory);
+  distance_to_stopline_ =
+    traffic_simulator::distance::distanceToStopLine(route_lanelets, *trajectory);
   const auto distance_to_stop_target = getDistanceToConflictingEntity(route_lanelets, *trajectory);
   const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
   if (!distance_to_stopline_) {

simulation/traffic_simulator/CMakeLists.txt

@@ -48,6 +48,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/helper/helper.cpp
   src/job/job.cpp
   src/job/job_list.cpp
+  src/lanelet_wrapper/distance.cpp
   src/lanelet_wrapper/lanelet_loader.cpp
   src/lanelet_wrapper/lanelet_map.cpp
   src/lanelet_wrapper/lanelet_wrapper.cpp

simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp

@@ -106,14 +106,6 @@ class HdMapUtils
                             traffic_simulator::RoutingConfiguration().routing_graph_type) const
     -> lanelet::Ids;
 
-  auto getDistanceToStopLine(
-    const lanelet::Ids & route_lanelets,
-    const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>;
-
-  auto getDistanceToStopLine(
-    const lanelet::Ids & route_lanelets,
-    const std::vector<geometry_msgs::msg::Point> & waypoints) const -> std::optional<double>;
-
   auto getDistanceToTrafficLightStopLine(
     const lanelet::Ids & route_lanelets,
     const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>;

simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/distance.hpp

@@ -0,0 +1,42 @@
+
+// Copyright 2015 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_
+#define TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_
+
+#include <lanelet2_core/geometry/Lanelet.h>
+
+#include <traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp>
+
+namespace traffic_simulator
+{
+namespace lanelet_wrapper
+{
+namespace distance
+{
+// StopLine
+auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline)
+  -> std::optional<double>;
+
+auto distanceToStopLine(
+  const lanelet::Ids & route_lanelets, const std::vector<Point> & route_waypoints)
+  -> std::optional<double>;
+
+auto distanceToStopLine(const std::vector<Point> & route_waypoints, const lanelet::Id stop_line_id)
+  -> std::optional<double>;
+}  // namespace distance
+}  // namespace lanelet_wrapper
+}  // namespace traffic_simulator
+#endif  // TRAFFIC_SIMULATOR__LANELET_WRAPPER_DISTANCE_HPP_

simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/lanelet_wrapper.hpp

@@ -22,6 +22,7 @@
 #include <autoware_lanelet2_extension/utility/utilities.hpp>
 #include <filesystem>
 #include <geometry/spline/catmull_rom_spline.hpp>
+#include <geometry/spline/catmull_rom_spline_interface.hpp>
 #include <geometry_msgs/msg/point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -66,6 +67,7 @@ using Pose = geometry_msgs::msg::Pose;
 using PoseStamped = geometry_msgs::msg::PoseStamped;
 using Spline = math::geometry::CatmullRomSpline;
 using Vector3 = geometry_msgs::msg::Vector3;
+using SplineInterface = math::geometry::CatmullRomSplineInterface;
 
 class RouteCache
 {

simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp

@@ -15,8 +15,10 @@
 #ifndef TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_
 #define TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_
 
+#include <geometry/spline/catmull_rom_spline_interface.hpp>
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
+#include <traffic_simulator/lanelet_wrapper/distance.hpp>
 
 namespace traffic_simulator
 {
@@ -108,10 +110,11 @@ auto distanceToCrosswalk(
   const lanelet::Id target_crosswalk_id,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
 
-auto distanceToStopLine(
-  const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array,
-  const lanelet::Id target_stop_line_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>;
+template <typename... Ts>
+auto distanceToStopLine(Ts &&... xs)
+{
+  return lanelet_wrapper::distance::distanceToStopLine(std::forward<decltype(xs)>(xs)...);
+}
 
 // spline
 auto distanceToSpline(

simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp

@@ -1377,68 +1377,6 @@ auto HdMapUtils::getDistanceToTrafficLightStopLine(
   return std::nullopt;
 }
 
-auto HdMapUtils::getDistanceToStopLine(
-  const lanelet::Ids & route_lanelets,
-  const std::vector<geometry_msgs::msg::Point> & waypoints) const -> std::optional<double>
-{
-  if (waypoints.empty()) {
-    return std::nullopt;
-  }
-  std::set<double> collision_points;
-  if (waypoints.empty()) {
-    return std::nullopt;
-  }
-  math::geometry::CatmullRomSpline spline(waypoints);
-  const auto stop_lines = getStopLinesOnPath({route_lanelets});
-  for (const auto & stop_line : stop_lines) {
-    std::vector<geometry_msgs::msg::Point> stop_line_points;
-    for (const auto & point : stop_line) {
-      geometry_msgs::msg::Point p;
-      p.x = point.x();
-      p.y = point.y();
-      p.z = point.z();
-      stop_line_points.emplace_back(p);
-    }
-    const auto collision_point = spline.getCollisionPointIn2D(stop_line_points);
-    if (collision_point) {
-      collision_points.insert(collision_point.value());
-    }
-  }
-  if (collision_points.empty()) {
-    return std::nullopt;
-  }
-  return *collision_points.begin();
-}
-
-auto HdMapUtils::getDistanceToStopLine(
-  const lanelet::Ids & route_lanelets,
-  const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>
-{
-  if (spline.getLength() <= 0) {
-    return std::nullopt;
-  }
-  std::set<double> collision_points;
-  const auto stop_lines = getStopLinesOnPath({route_lanelets});
-  for (const auto & stop_line : stop_lines) {
-    std::vector<geometry_msgs::msg::Point> stop_line_points;
-    for (const auto & point : stop_line) {
-      geometry_msgs::msg::Point p;
-      p.x = point.x();
-      p.y = point.y();
-      p.z = point.z();
-      stop_line_points.emplace_back(p);
-    }
-    const auto collision_point = spline.getCollisionPointIn2D(stop_line_points);
-    if (collision_point) {
-      collision_points.insert(collision_point.value());
-    }
-  }
-  if (collision_points.empty()) {
-    return std::nullopt;
-  }
-  return *collision_points.begin();
-}
-
 auto HdMapUtils::calculateSegmentDistances(const lanelet::ConstLineString3d & line_string) const
   -> std::vector<double>
 {

simulation/traffic_simulator/src/lanelet_wrapper/distance.cpp

@@ -0,0 +1,72 @@
+// Copyright 2015 Tier IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <traffic_simulator/helper/helper.hpp>
+#include <traffic_simulator/lanelet_wrapper/distance.hpp>
+#include <traffic_simulator/lanelet_wrapper/lanelet_map.hpp>
+#include <traffic_simulator/lanelet_wrapper/pose.hpp>
+#include <traffic_simulator/lanelet_wrapper/route.hpp>
+
+namespace traffic_simulator
+{
+namespace lanelet_wrapper
+{
+namespace distance
+{
+// StopLine
+auto distanceToStopLine(const lanelet::Ids & route_lanelets, const SplineInterface & route_spline)
+  -> std::optional<double>
+{
+  if (route_spline.getLength() <= 0.0) {
+    return std::nullopt;
+  } else {
+    std::optional<double> min_distance{std::nullopt};
+    const auto & stop_lines = lanelet_wrapper::lanelet_map::stopLinesOnPath({route_lanelets});
+    for (const auto & stop_line : stop_lines) {
+      const auto & stop_line_points = lanelet_wrapper::lanelet_map::toPolygon(stop_line);
+      if (const auto & collision_point = route_spline.getCollisionPointIn2D(stop_line_points)) {
+        if (not min_distance.has_value() or collision_point.value() < min_distance.value()) {
+          min_distance = collision_point;
+        }
+      }
+    }
+    return min_distance;
+  }
+}
+
+auto distanceToStopLine(
+  const lanelet::Ids & route_lanelets, const std::vector<Point> & route_waypoints)
+  -> std::optional<double>
+{
+  if (route_waypoints.empty()) {
+    return std::nullopt;
+  } else {
+    return distanceToStopLine(route_lanelets, Spline{route_waypoints});
+  }
+}
+
+auto distanceToStopLine(const std::vector<Point> & route_waypoints, const lanelet::Id stop_line_id)
+  -> std::optional<double>
+{
+  if (route_waypoints.empty()) {
+    return std::nullopt;
+  } else {
+    const Spline route_spline{route_waypoints};
+    return route_spline.getCollisionPointIn2D(
+      lanelet_wrapper::lanelet_map::stopLinePolygon(stop_line_id));
+  }
+}
+}  // namespace distance
+}  // namespace lanelet_wrapper
+}  // namespace traffic_simulator

simulation/traffic_simulator/src/utils/distance.cpp

@@ -305,20 +305,6 @@ auto distanceToCrosswalk(
   }
 }
 
-auto distanceToStopLine(
-  const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array,
-  const lanelet::Id target_stop_line_id,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> std::optional<double>
-{
-  if (waypoints_array.waypoints.empty()) {
-    return std::nullopt;
-  } else {
-    const math::geometry::CatmullRomSpline spline(waypoints_array.waypoints);
-    const auto polygon = hdmap_utils_ptr->getStopLinePolygon(target_stop_line_id);
-    return spline.getCollisionPointIn2D(polygon);
-  }
-}
-
 auto distanceToSpline(
   const geometry_msgs::msg::Pose & map_pose,
   const traffic_simulator_msgs::msg::BoundingBox & bounding_box,