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

@@ -49,8 +49,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar
     if (from_entity.isInLanelet() && to_entity.isInLanelet()) {
       return traffic_simulator::distance::lateralDistance(
         from_entity.getCanonicalizedLaneletPose().value(),
-        to_entity.getCanonicalizedLaneletPose().value(), traffic_simulator::RoutingConfiguration(),
-        api_.getHdmapUtils());
+        to_entity.getCanonicalizedLaneletPose().value(), traffic_simulator::RoutingConfiguration());
     }
     return std::nullopt;
   };
@@ -66,7 +65,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());
     }
     return std::nullopt;
   };
@@ -81,7 +80,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;
   }

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

@@ -93,6 +93,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);
@@ -112,41 +124,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 & [name, status] : other_entity_status) {
-    if (status.isInLanelet() && traffic_simulator::isSameLaneletId(status, lanelet_id)) {
-      ret.emplace_back(status);
-    }
-  }
-  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->isInLanelet()) {
-        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;
@@ -212,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());
         }
       }
@@ -224,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>
 {
@@ -297,17 +277,17 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const
 
 /**
  * @note getDistanceToTargetEntity working schematics
- * 
- * 1. Check if route to target entity from reference entity exists, if not try to transform pose to other 
+ *
+ * 1. Check if route to target entity from reference entity exists, if not try to transform pose to other
  *    routable lanelet, within matching distance (findRoutableAlternativeLaneletPoseFrom).
  * 2. Calculate longitudinal distance between entities bounding boxes -> bounding_box_distance.
  * 3. Calculate longitudinal distance between entities poses -> position_distance.
- * 4. Calculate target entity bounding box distance to reference entity spline (minimal distance from all corners) 
+ * 4. Calculate target entity bounding box distance to reference entity spline (minimal distance from all corners)
  *    -> target_to_spline_distance.
  * 5. If target_to_spline_distance is less than half width of reference entity target entity is conflicting.
  * 6. Check corner case where target entity width is bigger than width of entity and target entity
  *    is exactly on the spline -> spline.getCollisionPointIn2D
- * 7. If target entity is conflicting return bounding_box_distance enlarged by half of the entity 
+ * 7. If target entity is conflicting return bounding_box_distance enlarged by half of the entity
  *    length.
  */
 auto ActionNode::getDistanceToTargetEntity(
@@ -319,9 +299,9 @@ auto ActionNode::getDistanceToTargetEntity(
     !isOtherEntityAtConsideredAltitude(status)) {
     return std::nullopt;
   }
-  /** 
-      * boundingBoxLaneLongitudinalDistance requires routing_configuration, 
-      * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets 
+  /**
+      * boundingBoxLaneLongitudinalDistance requires routing_configuration,
+      * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets
       */
   traffic_simulator::RoutingConfiguration routing_configuration;
   routing_configuration.allow_lane_change = true;
@@ -341,13 +321,12 @@ auto ActionNode::getDistanceToTargetEntity(
     if (const auto bounding_box_distance =
           traffic_simulator::distance::boundingBoxLaneLongitudinalDistance(
             *from_lanelet_pose, from_bounding_box, *target_lanelet_pose, target_bounding_box,
-            include_adjacent_lanelet, include_opposite_direction, routing_configuration,
-            hdmap_utils);
+            include_adjacent_lanelet, include_opposite_direction, routing_configuration);
         !bounding_box_distance || bounding_box_distance.value() < 0.0) {
       return std::nullopt;
     } else if (const auto position_distance = traffic_simulator::distance::longitudinalDistance(
                  *from_lanelet_pose, *target_lanelet_pose, include_adjacent_lanelet,
-                 include_opposite_direction, routing_configuration, hdmap_utils);
+                 include_opposite_direction, routing_configuration);
                !position_distance) {
       return std::nullopt;
     } else {

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

@@ -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/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

@@ -49,11 +49,13 @@ 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
   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/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,18 +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 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;
@@ -213,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>;
 
@@ -350,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>>;