HdMapUtils refactor lanelet_wrapper::distance::distanceToStopLine

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

@@ -49,6 +49,7 @@ ament_auto_add_library(traffic_simulator SHARED
   src/helper/ostream_helpers.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>;
@@ -213,12 +205,6 @@ class HdMapUtils
                             traffic_simulator::RoutingConfiguration().routing_graph_type) const
     -> double;
 
-  auto getStopLineIds() const -> lanelet::Ids;
-
-  auto getStopLineIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids;
-
-  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>;
 
@@ -350,10 +336,6 @@ class HdMapUtils
     const traffic_simulator::lane_change::TrajectoryShape,
     const double tangent_vector_size = 100) const -> math::geometry::HermiteCurve;
 
-  auto getStopLines() const -> lanelet::ConstLineStrings3d;
-
-  auto getStopLinesOnPath(const lanelet::Ids &) const -> lanelet::ConstLineStrings3d;
-
   auto getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids &) const
     -> std::vector<std::shared_ptr<const lanelet::autoware::AutowareTrafficLight>>;
 

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_map.hpp

@@ -96,7 +96,17 @@ 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
+auto trafficSignsOnPath(const lanelet::Ids & lanelet_ids)
+  -> std::vector<std::shared_ptr<const lanelet::TrafficSign>>;
+
+auto stopLinesOnPath(const lanelet::Ids & lanelet_ids) -> lanelet::ConstLineStrings3d;
+
+auto stopLineIdsOnPath(const lanelet::Ids & lanelet_ids) -> lanelet::Ids;
 }  // namespace lanelet_map
 }  // namespace lanelet_wrapper
 }  // namespace traffic_simulator

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,7 +15,9 @@
 #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/lanelet_wrapper/distance.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.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

@@ -1229,51 +1229,6 @@ auto HdMapUtils::getTrafficLightRegulatoryElementsOnPath(const lanelet::Ids & la
   return ret;
 }
 
-auto HdMapUtils::getStopLines() const -> lanelet::ConstLineStrings3d
-{
-  lanelet::ConstLineStrings3d ret;
-  for (const auto & traffic_sign : getTrafficSignRegulatoryElements()) {
-    if (traffic_sign->type() == "stop_sign") {
-      for (const auto & stop_line : traffic_sign->refLines()) {
-        ret.emplace_back(stop_line);
-      }
-    }
-  }
-  return ret;
-}
-
-auto HdMapUtils::getStopLinesOnPath(const lanelet::Ids & lanelet_ids) const
-  -> lanelet::ConstLineStrings3d
-{
-  lanelet::ConstLineStrings3d ret;
-  for (const auto & traffic_sign : getTrafficSignRegulatoryElementsOnPath(lanelet_ids)) {
-    if (traffic_sign->type() == "stop_sign") {
-      for (const auto & stop_line : traffic_sign->refLines()) {
-        ret.emplace_back(stop_line);
-      }
-    }
-  }
-  return ret;
-}
-
-auto HdMapUtils::getStopLineIds() const -> lanelet::Ids
-{
-  lanelet::Ids stop_line_ids;
-  for (const auto & ret : getStopLines()) {
-    stop_line_ids.push_back(ret.id());
-  }
-  return stop_line_ids;
-}
-
-auto HdMapUtils::getStopLineIdsOnPath(const lanelet::Ids & route_lanelets) const -> lanelet::Ids
-{
-  lanelet::Ids stop_line_ids;
-  for (const auto & ret : getStopLinesOnPath(route_lanelets)) {
-    stop_line_ids.push_back(ret.id());
-  }
-  return stop_line_ids;
-}
-
 auto HdMapUtils::getTrafficLights(const lanelet::Id traffic_light_id) const
   -> std::vector<lanelet::AutowareTrafficLightConstPtr>
 {
@@ -1330,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;
@@ -1437,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>
 {