HdMapUtils refactor (PR 2/6) - extend lanelet_wrapper: add ::distance and necessary parts of ::route, ::lanelet_map, ::traffic_lights

mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp

@@ -51,7 +51,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar
         return traffic_simulator::distance::lateralDistance(
           from_entity->getCanonicalizedLaneletPose().value(),
           to_entity->getCanonicalizedLaneletPose().value(),
-          traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils());
+          traffic_simulator::RoutingConfiguration());
       }
     }
     return std::nullopt;
@@ -68,7 +68,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar
         if (from_entity_lanelet_pose && to_entity_lanelet_pose) {
           return traffic_simulator::distance::lateralDistance(
             from_entity_lanelet_pose.value(), to_entity_lanelet_pose.value(),
-            traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils());
+            traffic_simulator::RoutingConfiguration());
         }
       }
     }
@@ -86,7 +86,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar
         return traffic_simulator::distance::longitudinalDistance(
           from_entity->getCanonicalizedLaneletPose().value(),
           to_entity->getCanonicalizedLaneletPose().value(), false, false,
-          traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils());
+          traffic_simulator::RoutingConfiguration());
       }
     }
     return std::nullopt;

mock/cpp_mock_scenarios/src/measurement/get_distance_to_lane_bound.cpp

@@ -46,8 +46,7 @@ class GetDistanceToLaneBoundScenario : public cpp_mock_scenarios::CppScenarioNod
     }
     if (auto ego_entity = api_.getEntity("ego")) {
       const auto distance = traffic_simulator::distance::distanceToLaneBound(
-        ego_entity->getMapPose(), ego_entity->getBoundingBox(), ego_entity->getRouteLanelets(),
-        api_.getHdmapUtils());
+        ego_entity->getMapPose(), ego_entity->getBoundingBox(), ego_entity->getRouteLanelets());
       // LCOV_EXCL_START
       if (distance <= 0.4 && distance >= 0.52) {
         stop(cpp_mock_scenarios::Result::FAILURE);

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -182,7 +182,7 @@ class SimulatorCore
       routing_configuration.allow_lane_change =
         (routing_algorithm == RoutingAlgorithm::value_type::shortest);
       return traffic_simulator::pose::relativeLaneletPose(
-        from_lanelet_pose, to_lanelet_pose, routing_configuration, core->getHdmapUtils());
+        from_lanelet_pose, to_lanelet_pose, routing_configuration);
     }
 
     static auto makeNativeBoundingBoxRelativeLanePosition(
@@ -230,7 +230,7 @@ class SimulatorCore
         (routing_algorithm == RoutingAlgorithm::value_type::shortest);
       return traffic_simulator::pose::boundingBoxRelativeLaneletPose(
         from_lanelet_pose, from_bounding_box, to_lanelet_pose, to_bounding_box,
-        routing_configuration, core->getHdmapUtils());
+        routing_configuration);
     }
 
     static auto makeNativeBoundingBoxRelativeWorldPosition(

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -55,20 +55,17 @@ 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>;
   auto getRightOfWayEntities() const -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto getRightOfWayEntities(const lanelet::Ids & following_lanelets) const
     -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto getYieldStopDistance(const lanelet::Ids & following_lanelets) const -> std::optional<double>;
-  auto getOtherEntityStatus(lanelet::Id lanelet_id) const
-    -> std::vector<traffic_simulator::CanonicalizedEntityStatus>;
   auto stopEntity() const -> void;
   auto getHorizon() const -> double;
+  auto getOtherEntitiesCanonicalizedLaneletPoses() const
+    -> std::vector<traffic_simulator::CanonicalizedLaneletPose>;
 
   /// throws if the derived class return RUNNING.
   auto executeTick() -> BT::NodeStatus override;

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -90,6 +90,18 @@ auto ActionNode::getBlackBoardValues() -> void
   }
 }
 
+auto ActionNode::getOtherEntitiesCanonicalizedLaneletPoses() const
+  -> std::vector<traffic_simulator::CanonicalizedLaneletPose>
+{
+  std::vector<traffic_simulator::CanonicalizedLaneletPose> other_canonicalized_lanelet_poses;
+  for (const auto & [name, status] : other_entity_status) {
+    if (auto const canonicalized_lanelet_pose = status.getCanonicalizedLaneletPose()) {
+      other_canonicalized_lanelet_poses.push_back(canonicalized_lanelet_pose.value());
+    }
+  }
+  return other_canonicalized_lanelet_poses;
+}
+
 auto ActionNode::getHorizon() const -> double
 {
   return std::clamp(canonicalized_entity_status->getTwist().linear.x * 5.0, 20.0, 50.0);
@@ -109,43 +121,16 @@ auto ActionNode::setCanonicalizedEntityStatus(const traffic_simulator::EntitySta
     entity_status, default_matching_distance_for_lanelet_pose_calculation);
 }
 
-auto ActionNode::getOtherEntityStatus(lanelet::Id lanelet_id) const
-  -> std::vector<traffic_simulator::CanonicalizedEntityStatus>
-{
-  std::vector<traffic_simulator::CanonicalizedEntityStatus> ret;
-  for (const auto & status : other_entity_status) {
-    if (
-      status.second.laneMatchingSucceed() &&
-      traffic_simulator::isSameLaneletId(status.second, lanelet_id)) {
-      ret.emplace_back(status.second);
-    }
-  }
-  return ret;
-}
-
 auto ActionNode::getYieldStopDistance(const lanelet::Ids & following_lanelets) const
   -> std::optional<double>
 {
-  std::set<double> distances;
-  for (const auto & lanelet : following_lanelets) {
-    const auto right_of_way_ids = hdmap_utils->getRightOfWayLaneletIds(lanelet);
-    for (const auto right_of_way_id : right_of_way_ids) {
-      const auto other_status = getOtherEntityStatus(right_of_way_id);
-      if (!other_status.empty() && canonicalized_entity_status->laneMatchingSucceed()) {
-        const auto lanelet_pose = canonicalized_entity_status->getLaneletPose();
-        const auto distance_forward = hdmap_utils->getLongitudinalDistance(
-          lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0));
-        const auto distance_backward = hdmap_utils->getLongitudinalDistance(
-          traffic_simulator::helper::constructLaneletPose(lanelet, 0), lanelet_pose);
-        if (distance_forward) {
-          distances.insert(distance_forward.value());
-        } else if (distance_backward) {
-          distances.insert(-distance_backward.value());
-        }
-      }
-    }
-    if (distances.size() != 0) {
-      return *distances.begin();
+  if (
+    const auto canonicalized_lanelet_pose =
+      canonicalized_entity_status->getCanonicalizedLaneletPose()) {
+    if (const auto other_canonicalized_lanelet_poses = getOtherEntitiesCanonicalizedLaneletPoses();
+        !other_canonicalized_lanelet_poses.empty()) {
+      traffic_simulator::distance::distanceToYieldStop(
+        canonicalized_lanelet_pose.value(), following_lanelets, other_canonicalized_lanelet_poses);
     }
   }
   return std::nullopt;
@@ -214,7 +199,7 @@ auto ActionNode::getDistanceToTrafficLightStopLine(
       if (traffic_lights->isRequiredStopTrafficLightState(traffic_light_id)) {
         if (
           const auto collision_point =
-            hdmap_utils->getDistanceToTrafficLightStopLine(spline, traffic_light_id)) {
+            traffic_simulator::distance::distanceToTrafficLightStopLine(spline, traffic_light_id)) {
           collision_points.insert(collision_point.value());
         }
       }
@@ -226,13 +211,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/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -77,7 +77,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
   } else if (
     const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
-      *polyline_trajectory, behavior_parameter, hdmap_utils, step_time,
+      *polyline_trajectory, behavior_parameter, step_time,
       default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) {
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());

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

@@ -105,7 +105,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/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp

@@ -77,7 +77,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
   } else if (
     const auto entity_status_updated = traffic_simulator::follow_trajectory::makeUpdatedStatus(
       static_cast<traffic_simulator::EntityStatus>(*canonicalized_entity_status),
-      *polyline_trajectory, behavior_parameter, hdmap_utils, step_time,
+      *polyline_trajectory, behavior_parameter, step_time,
       default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed())) {
     setCanonicalizedEntityStatus(entity_status_updated.value());
     setOutput("waypoints", calculateWaypoints());

simulation/traffic_simulator/CMakeLists.txt

@@ -48,10 +48,13 @@ 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
   src/lanelet_wrapper/pose.cpp
+  src/lanelet_wrapper/route.cpp
+  src/lanelet_wrapper/traffic_lights.cpp
   src/lanelet_wrapper/traffic_rules.cpp
   src/simulation_clock/simulation_clock.cpp
   src/traffic/traffic_controller.cpp

simulation/traffic_simulator/include/traffic_simulator/behavior/follow_trajectory.hpp

@@ -29,8 +29,7 @@ namespace follow_trajectory
 auto makeUpdatedStatus(
   const traffic_simulator_msgs::msg::EntityStatus &,
   traffic_simulator_msgs::msg::PolylineTrajectory &,
-  const traffic_simulator_msgs::msg::BehaviorParameter &,
-  const std::shared_ptr<hdmap_utils::HdMapUtils> &, double, double,
+  const traffic_simulator_msgs::msg::BehaviorParameter &, double, double,
   std::optional<double> target_speed = std::nullopt) -> std::optional<EntityStatus>;
 }  // namespace follow_trajectory
 }  // namespace traffic_simulator

simulation/traffic_simulator/include/traffic_simulator/data_type/lanelet_pose.hpp

@@ -49,7 +49,7 @@ class CanonicalizedLaneletPose
   auto alignOrientationToLanelet() -> void;
   auto hasAlternativeLaneletPose() const -> bool { return lanelet_poses_.size() > 1; }
   auto getAlternativeLaneletPoseBaseOnShortestRouteFrom(
-    LaneletPose from, const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
+    LaneletPose from,
     const RoutingConfiguration & routing_configuration = RoutingConfiguration()) const
     -> std::optional<LaneletPose>;
 

simulation/traffic_simulator/include/traffic_simulator/data_type/routing_configuration.hpp

@@ -22,6 +22,10 @@ namespace traffic_simulator
 {
 struct RoutingConfiguration
 {
+  RoutingConfiguration() = default;
+  explicit RoutingConfiguration(const bool allow_lane_change)
+  : allow_lane_change(allow_lane_change){};
+
   bool allow_lane_change = false;
   traffic_simulator::RoutingGraphType routing_graph_type =
     traffic_simulator::RoutingGraphType::VEHICLE_WITH_ROAD_SHOULDER;

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

@@ -106,30 +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>;
-
-  auto getDistanceToTrafficLightStopLine(
-    const lanelet::Ids & route_lanelets,
-    const std::vector<geometry_msgs::msg::Point> & waypoints) const -> std::optional<double>;
-
-  auto getDistanceToTrafficLightStopLine(
-    const math::geometry::CatmullRomSplineInterface & spline,
-    const lanelet::Id traffic_light_id) const -> std::optional<double>;
-
-  auto getDistanceToTrafficLightStopLine(
-    const std::vector<geometry_msgs::msg::Point> & waypoints,
-    const lanelet::Id traffic_light_id) const -> std::optional<double>;
-
   auto getFollowingLanelets(
     const lanelet::Id current_lanelet_id, const lanelet::Ids & route, const double horizon = 100,
     const bool include_current_lanelet_id = true,
@@ -173,20 +149,6 @@ class HdMapUtils
 
   auto getLanelets(const lanelet::Ids &) const -> lanelet::Lanelets;
 
-  auto getLateralDistance(
-    const traffic_simulator_msgs::msg::LaneletPose & from,
-    const traffic_simulator_msgs::msg::LaneletPose & to,
-    const traffic_simulator::RoutingConfiguration & routing_configuration =
-      traffic_simulator::RoutingConfiguration()) const -> std::optional<double>;
-
-  auto getLeftBound(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
-
-  auto getLongitudinalDistance(
-    const traffic_simulator_msgs::msg::LaneletPose & from_pose,
-    const traffic_simulator_msgs::msg::LaneletPose & to_pose,
-    const traffic_simulator::RoutingConfiguration & routing_configuration =
-      traffic_simulator::RoutingConfiguration()) const -> std::optional<double>;
-
   auto getNearbyLaneletIds(
     const geometry_msgs::msg::Point &, const double distance_threshold,
     const bool include_crosswalk, const std::size_t search_count = 5) const -> lanelet::Ids;
@@ -200,8 +162,6 @@ class HdMapUtils
     const traffic_simulator::RoutingGraphType type =
       traffic_simulator::RoutingConfiguration().routing_graph_type) const -> lanelet::Ids;
 
-  auto getRightBound(const lanelet::Id) const -> std::vector<geometry_msgs::msg::Point>;
-
   auto getRightOfWayLaneletIds(const lanelet::Ids &) const
     -> std::unordered_map<lanelet::Id, lanelet::Ids>;
 
@@ -217,12 +177,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>;
 
@@ -354,10 +308,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>>;