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

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -64,6 +64,8 @@ class ActionNode : public BT::ActionNodeBase
   auto getYieldStopDistance(const lanelet::Ids & following_lanelets) const -> std::optional<double>;
   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

@@ -114,6 +114,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);
@@ -133,43 +145,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 =
-          traffic_simulator::lanelet_wrapper::distance::longitudinalDistance(
-            lanelet_pose, traffic_simulator::helper::constructLaneletPose(lanelet, 0));
-        const auto distance_backward =
-          traffic_simulator::lanelet_wrapper::distance::longitudinalDistance(
-            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;
@@ -235,7 +220,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());
         }
       }
@@ -331,17 +316,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 -> longitudinal_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(
@@ -353,9 +338,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;

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

@@ -106,22 +106,6 @@ class HdMapUtils
                             traffic_simulator::RoutingConfiguration().routing_graph_type) const
     -> lanelet::Ids;
 
-  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,

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

@@ -46,6 +46,30 @@ auto distanceToStopLine(
 
 auto distanceToStopLine(const std::vector<Point> & route_waypoints, const lanelet::Id stop_line_id)
   -> std::optional<double>;
+
+// TrafficLightStopLine
+auto distanceToTrafficLightStopLine(
+  const SplineInterface & route_spline, const lanelet::Id traffic_light_id)
+  -> std::optional<double>;
+
+auto distanceToTrafficLightStopLine(
+  const std::vector<Point> & route_waypoints, const lanelet::Id traffic_light_id)
+  -> std::optional<double>;
+
+auto distanceToTrafficLightStopLine(
+  const lanelet::Ids & route_lanelets, const SplineInterface & route_spline)
+  -> std::optional<double>;
+
+auto distanceToTrafficLightStopLine(
+  const lanelet::Ids & route_lanelets, const std::vector<Point> & route_waypoints)
+  -> std::optional<double>;
+
+// Crosswalk
+auto distanceToCrosswalk(const std::vector<Point> & route_waypoints, const lanelet::Id crosswalk_id)
+  -> std::optional<double>;
+
+auto distanceToCrosswalk(const SplineInterface & route_spline, const lanelet::Id crosswalk_id)
+  -> std::optional<double>;
 }  // namespace distance
 }  // namespace lanelet_wrapper
 }  // namespace traffic_simulator

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

@@ -26,6 +26,7 @@ namespace lanelet_wrapper
 {
 namespace lanelet_map
 {
+// Basics
 auto isInLanelet(const lanelet::Id lanelet_id, const double lanelet_pose_s) -> bool;
 
 auto isInLanelet(const lanelet::Id lanelet_id, const Point point) -> bool;
@@ -52,12 +53,14 @@ auto nearbyLaneletIds(
   const Point & point, const double distance_threshold, const bool include_crosswalk,
   const std::size_t search_count) -> lanelet::Ids;
 
+// Center points
 auto centerPoints(const lanelet::Ids & lanelet_ids) -> std::vector<Point>;
 
 auto centerPoints(const lanelet::Id lanelet_id) -> std::vector<Point>;
 
 auto centerPointsSpline(const lanelet::Id lanelet_id) -> std::shared_ptr<Spline>;
 
+// Next lanelet
 auto nextLaneletIds(
   const lanelet::Id lanelet_id,
   const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids;
@@ -74,6 +77,7 @@ auto nextLaneletIds(
   const lanelet::Ids & lanelet_ids, std::string_view turn_direction,
   const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids;
 
+// Previous lanelet
 auto previousLaneletIds(
   const lanelet::Id lanelet_id,
   const RoutingGraphType type = RoutingConfiguration().routing_graph_type) -> lanelet::Ids;
@@ -96,10 +100,18 @@ auto leftBound(const lanelet::Id lanelet_id) -> std::vector<Point>;
 auto rightBound(const lanelet::Id lanelet_id) -> std::vector<Point>;
 
 // Polygons
+auto laneletPolygon(const lanelet::Id lanelet_id) -> std::vector<Point>;
+
 auto stopLinePolygon(const lanelet::Id lanelet_id) -> std::vector<Point>;
 
 auto toPolygon(const lanelet::ConstLineString3d & line_string) -> std::vector<Point>;
 
+// Relations
+auto rightOfWayLaneletIds(const lanelet::Ids & lanelet_ids)
+  -> std::unordered_map<lanelet::Id, lanelet::Ids>;
+
+auto rightOfWayLaneletIds(const lanelet::Id lanelet_id) -> lanelet::Ids;
+
 // Objects on path
 auto trafficSignsOnPath(const lanelet::Ids & lanelet_ids)
   -> std::vector<std::shared_ptr<const lanelet::TrafficSign>>;

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

@@ -66,6 +66,7 @@ using Point = geometry_msgs::msg::Point;
 using Pose = geometry_msgs::msg::Pose;
 using PoseStamped = geometry_msgs::msg::PoseStamped;
 using Spline = math::geometry::CatmullRomSpline;
+using SplineInterface = math::geometry::CatmullRomSplineInterface;
 using Vector3 = geometry_msgs::msg::Vector3;
 using SplineInterface = math::geometry::CatmullRomSplineInterface;
 
@@ -159,7 +160,7 @@ class CenterPointsCache
   }
 
   auto getCenterPoints(const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map)
-    -> std::vector<Point>
+    -> decltype(auto)
   {
     if (!exists(lanelet_id)) {
       appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map));
@@ -168,8 +169,7 @@ class CenterPointsCache
   }
 
   auto getCenterPointsSpline(
-    const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map)
-    -> std::shared_ptr<Spline>
+    const lanelet::Id lanelet_id, const lanelet::LaneletMapPtr & lanelet_map) -> decltype(auto)
   {
     if (!exists(lanelet_id)) {
       appendData(lanelet_id, centerPoints(lanelet_id, lanelet_map));
@@ -184,7 +184,7 @@ class CenterPointsCache
     return data_.find(lanelet_id) != data_.end();
   }
 
-  auto readData(const lanelet::Id lanelet_id) -> std::vector<Point>
+  auto readData(const lanelet::Id lanelet_id) -> const std::vector<Point> &
   {
     std::lock_guard lock(mutex_);
     return data_.at(lanelet_id);
@@ -233,7 +233,7 @@ class CenterPointsCache
 class LaneletLengthCache
 {
 public:
-  auto getLength(lanelet::Id lanelet_id)
+  auto getLength(lanelet::Id lanelet_id) -> double
   {
     if (!exists(lanelet_id)) {
       THROW_SIMULATION_ERROR("length of : ", lanelet_id, " does not exists on route cache.");

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

@@ -16,8 +16,10 @@
 #define TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_
 
 #include <geometry/spline/catmull_rom_spline_interface.hpp>
+#include <traffic_simulator/data_type/entity_status.hpp>
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
 #include <traffic_simulator/lanelet_wrapper/distance.hpp>
+#include <traffic_simulator_msgs/msg/bounding_box.hpp>
 #include <traffic_simulator_msgs/msg/waypoints_array.hpp>
 
 namespace traffic_simulator
@@ -106,17 +108,29 @@ auto distanceToRightLaneBound(
   -> double;
 
 // Other objects
-auto distanceToCrosswalk(
-  const traffic_simulator_msgs::msg::WaypointsArray & waypoints_array,
-  const lanelet::Id target_crosswalk_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)...);
 }
 
+template <typename... Ts>
+auto distanceToTrafficLightStopLine(Ts &&... xs)
+{
+  return lanelet_wrapper::distance::distanceToTrafficLightStopLine(
+    std::forward<decltype(xs)>(xs)...);
+}
+
+template <typename... Ts>
+auto distanceToCrosswalk(Ts &&... xs)
+{
+  return lanelet_wrapper::distance::distanceToCrosswalk(std::forward<decltype(xs)>(xs)...);
+}
+
+auto distanceToYieldStop(
+  const CanonicalizedLaneletPose & reference_pose, const lanelet::Ids & following_lanelets,
+  const std::vector<CanonicalizedLaneletPose> & other_poses) -> std::optional<double>;
+
 // spline
 auto distanceToSpline(
   const geometry_msgs::msg::Pose & map_pose,

simulation/traffic_simulator/src/behavior/follow_trajectory.cpp

@@ -82,8 +82,6 @@ auto makeUpdatedStatus(
     using geometry_msgs::msg::Pose;
     using geometry_msgs::msg::Vector3;
 
-    const RoutingConfiguration routing_configuration{allow_lane_change};
-
     const auto quaternion = convertDirectionToQuaternion(
       geometry_msgs::build<Vector3>().x(to.x - from.x).y(to.y - from.y).z(to.z - from.z));
     const auto from_pose = geometry_msgs::build<Pose>().position(from).orientation(quaternion);
@@ -94,24 +92,12 @@ auto makeUpdatedStatus(
       if (
         const auto to_canonicalized_lanelet_pose = pose::toCanonicalizedLaneletPose(
           to_pose, entity_status.bounding_box, include_crosswalk, matching_distance)) {
-        if (const auto longitudinal_distance = distance::longitudinalDistance(
-              from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(),
-              include_adjacent_lanelet, include_opposite_direction, routing_configuration);
-            longitudinal_distance.has_value()
-            /**
-             * DIRTY HACK!
-             * Negative longitudinal distance (calculated along lanelet in opposite direction)
-             * causes some scenarios to fail because of an unrelated issue with lanelet matching.
-             * The issue is caused by wrongly matched lanelet poses and thus wrong distances.
-             * When lanelet matching errors are fixed, this dirty hack can be removed.
-             */
-            and longitudinal_distance.value() >= 0.0) {
-          if (
-            const auto lateral_distance = distance::lateralDistance(
-              from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(),
-              routing_configuration)) {
-            return std::hypot(longitudinal_distance.value(), lateral_distance.value());
-          }
+        if (
+          const auto longitudinal_distance = distance::longitudinalDistance(
+            from_canonicalized_lanelet_pose.value(), to_canonicalized_lanelet_pose.value(),
+            include_adjacent_lanelet, include_opposite_direction,
+            RoutingConfiguration(allow_lane_change))) {
+          return longitudinal_distance.value();
         }
       }
     }
@@ -588,7 +574,7 @@ auto makeUpdatedStatus(
         return entity_status.pose.orientation;
       } else {
         /// @note if there is a designed_velocity vector, set the orientation in the direction of it
-        return math::geometry::convertDirectionToQuaternion(desired_velocity);
+        return convertDirectionToQuaternion(desired_velocity);
       }
     }();