RJD-1489 Change detection of front entity on path of NPC

common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp

@@ -46,7 +46,7 @@ class CatmullRomSpline : public CatmullRomSplineInterface
   auto getSValue(const geometry_msgs::msg::Pose & pose, double threshold_distance = 3.0) const
     -> std::optional<double>;
   auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const
-    -> double;
+    -> double override;
   auto getSquaredDistanceVector(const geometry_msgs::msg::Point & point, const double s) const
     -> geometry_msgs::msg::Vector3;
   auto getCollisionPointsIn2D(

common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp

@@ -34,6 +34,8 @@ class CatmullRomSplineInterface
   virtual std::optional<double> getCollisionPointIn2D(
     const std::vector<geometry_msgs::msg::Point> & polygon,
     const bool search_backward = false) const = 0;
+  virtual double getSquaredDistanceIn2D(
+    const geometry_msgs::msg::Point & point, const double s) const = 0;
 };
 }  // namespace geometry
 }  // namespace math

common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp

@@ -46,6 +46,9 @@ class CatmullRomSubspline : public CatmullRomSplineInterface
     const std::vector<geometry_msgs::msg::Point> & polygon,
     const bool search_backward = false) const override;
 
+  auto getSquaredDistanceIn2D(const geometry_msgs::msg::Point & point, const double s) const
+    -> double override;
+
 private:
   std::shared_ptr<math::geometry::CatmullRomSpline> spline_;
   double start_s_;

common/math/geometry/src/spline/catmull_rom_subspline.cpp

@@ -61,5 +61,11 @@ std::optional<double> CatmullRomSubspline::getCollisionPointIn2D(
   }
   return *begin - start_s_;
 }
+
+auto CatmullRomSubspline::getSquaredDistanceIn2D(
+  const geometry_msgs::msg::Point & point, const double s) const -> double
+{
+  return spline_->getSquaredDistanceIn2D(point, start_s_ + s);
+}
 }  // namespace geometry
 }  // namespace math

simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp

@@ -99,11 +99,6 @@ class ActionNode : public BT::ActionNodeBase
   virtual auto getBlackBoardValues() -> void;
   auto getEntityStatus(const std::string & target_name) const
     -> const traffic_simulator::CanonicalizedEntityStatus &;
-  auto getDistanceToTargetEntityPolygon(
-    const math::geometry::CatmullRomSplineInterface & spline, const std::string target_name,
-    double width_extension_right = 0.0, double width_extension_left = 0.0,
-    double length_extension_front = 0.0, double length_extension_rear = 0.0) const
-    -> std::optional<double>;
 
   auto setCanonicalizedEntityStatus(const traffic_simulator::EntityStatus & entity_status) -> void;
   auto calculateUpdatedEntityStatus(
@@ -125,15 +120,14 @@ class ActionNode : public BT::ActionNodeBase
   EntityStatusDict other_entity_status;
   lanelet::Ids route_lanelets;
 
+  auto getDistanceToTargetEntity(
+    const math::geometry::CatmullRomSplineInterface & spline,
+    const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>;
+
 private:
   auto getDistanceToTargetEntityOnCrosswalk(
     const math::geometry::CatmullRomSplineInterface & spline,
     const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>;
-  auto getDistanceToTargetEntityPolygon(
-    const math::geometry::CatmullRomSplineInterface & spline,
-    const traffic_simulator::CanonicalizedEntityStatus & status, double width_extension_right = 0.0,
-    double width_extension_left = 0.0, double length_extension_front = 0.0,
-    double length_extension_rear = 0.0) const -> std::optional<double>;
   auto getConflictingEntityStatus(const lanelet::Ids & following_lanelets) const
     -> std::optional<traffic_simulator::CanonicalizedEntityStatus>;
   auto getConflictingEntityStatusOnCrossWalk(const lanelet::Ids & route_lanelets) const

simulation/behavior_tree_plugin/src/action_node.cpp

@@ -15,6 +15,7 @@
 #include <algorithm>
 #include <behavior_tree_plugin/action_node.hpp>
 #include <geometry/bounding_box.hpp>
+#include <geometry/distance.hpp>
 #include <geometry/quaternion/euler_to_quaternion.hpp>
 #include <geometry/quaternion/get_rotation.hpp>
 #include <geometry/quaternion/get_rotation_matrix.hpp>
@@ -28,6 +29,7 @@
 #include <traffic_simulator/behavior/longitudinal_speed_planning.hpp>
 #include <traffic_simulator/helper/helper.hpp>
 #include <traffic_simulator/lanelet_wrapper/pose.hpp>
+#include <traffic_simulator/utils/pose.hpp>
 #include <unordered_map>
 #include <utility>
 #include <vector>
@@ -236,11 +238,11 @@ auto ActionNode::getDistanceToStopLine(
 auto ActionNode::getDistanceToFrontEntity(
   const math::geometry::CatmullRomSplineInterface & spline) const -> std::optional<double>
 {
-  auto name = getFrontEntityName(spline);
-  if (!name) {
+  if (const auto entity_name = getFrontEntityName(spline)) {
+    return getDistanceToTargetEntity(spline, getEntityStatus(entity_name.value()));
+  } else {
     return std::nullopt;
   }
-  return getDistanceToTargetEntityPolygon(spline, name.value());
 }
 
 auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const
@@ -249,7 +251,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
   std::vector<double> distances;
   std::vector<std::string> entities;
   for (const auto & each : other_entity_status) {
-    const auto distance = getDistanceToTargetEntityPolygon(spline, each.first);
+    const auto distance = getDistanceToTargetEntity(spline, each.second);
     const auto quat = math::geometry::getRotation(
       canonicalized_entity_status->getMapPose().orientation,
       other_entity_status.at(each.first).getMapPose().orientation);
@@ -297,31 +299,80 @@ auto ActionNode::getEntityStatus(const std::string & target_name) const
   }
 }
 
-auto ActionNode::getDistanceToTargetEntityPolygon(
-  const math::geometry::CatmullRomSplineInterface & spline, const std::string target_name,
-  double width_extension_right, double width_extension_left, double length_extension_front,
-  double length_extension_rear) const -> std::optional<double>
-{
-  return getDistanceToTargetEntityPolygon(
-    spline, getEntityStatus(target_name), width_extension_right, width_extension_left,
-    length_extension_front, length_extension_rear);
-}
-
-auto ActionNode::getDistanceToTargetEntityPolygon(
+/**
+ * @note getDistanceToTargetEntity working schematics
+ * 
+ * 1. Check if route to target entity from reference entity exists, if not try to transform pose to other 
+ *    routable lanelet, within matching distance (transformToRoutableCanonicalizedLaneletPose).
+ * 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) 
+ *    -> 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 
+ *    length.
+ */
+auto ActionNode::getDistanceToTargetEntity(
   const math::geometry::CatmullRomSplineInterface & spline,
-  const traffic_simulator::CanonicalizedEntityStatus & status, double width_extension_right,
-  double width_extension_left, double length_extension_front, double length_extension_rear) const
-  -> std::optional<double>
+  const traffic_simulator::CanonicalizedEntityStatus & status) const -> std::optional<double>
 {
-  if (isOtherEntityAtConsideredAltitude(status)) {
-    const auto polygon = math::geometry::transformPoints(
-      status.getMapPose(), math::geometry::getPointsFromBbox(
-                             status.getBoundingBox(), width_extension_right, width_extension_left,
-                             length_extension_front, length_extension_rear));
-    return spline.getCollisionPointIn2D(polygon, false);
-  } else {
-    return std::nullopt;
+  if (
+    status.laneMatchingSucceed() && canonicalized_entity_status->laneMatchingSucceed() &&
+    isOtherEntityAtConsideredAltitude(status)) {
+    /** 
+      * 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;
+    constexpr bool include_adjacent_lanelet{false};
+    constexpr bool include_opposite_direction{true};
+    constexpr bool search_backward{false};
+
+    const auto & target_bounding_box = status.getBoundingBox();
+    const auto & target_lanelet_pose =
+      traffic_simulator::pose::transformToRoutableCanonicalizedLaneletPose(
+        canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(),
+        target_bounding_box, hdmap_utils);
+
+    if (target_lanelet_pose) {
+      const auto & from_lanelet_pose = canonicalized_entity_status->getCanonicalizedLaneletPose();
+      const auto & from_bounding_box = canonicalized_entity_status->getBoundingBox();
+      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);
+          not 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);
+                 not position_distance) {
+        return std::nullopt;
+      } else {
+        const auto target_bounding_box_distance =
+          bounding_box_distance.value() + from_bounding_box.dimensions.x / 2.0;
+
+        /// @note if the distance of the target entity to the spline is smaller than the width of the reference entity
+        if (const auto target_to_spline_distance = traffic_simulator::distance::distanceToSpline(
+              static_cast<geometry_msgs::msg::Pose>(*target_lanelet_pose), target_bounding_box,
+              spline, position_distance.value());
+            target_to_spline_distance <= from_bounding_box.dimensions.y / 2.0) {
+          return target_bounding_box_distance;
+        }
+        /// @note if the distance of the target entity to the spline cannot be calculated because a collision occurs
+        else if (const auto target_polygon = math::geometry::transformPoints(
+                   status.getMapPose(), math::geometry::getPointsFromBbox(target_bounding_box));
+                 spline.getCollisionPointIn2D(target_polygon, search_backward)) {
+          return target_bounding_box_distance;
+        }
+      }
+    }
   }
+  return std::nullopt;
 }
 
 auto ActionNode::isOtherEntityAtConsideredAltitude(
@@ -350,9 +401,8 @@ auto ActionNode::getDistanceToConflictingEntity(
     }
   }
   for (const auto & status : lane_entity_status) {
-    const auto s = getDistanceToTargetEntityPolygon(spline, status, 0.0, 0.0, 0.0, 1.0);
-    if (s) {
-      distances.insert(s.value());
+    if (const auto distance_to_entity = getDistanceToTargetEntity(spline, status)) {
+      distances.insert(distance_to_entity.value());
     }
   }
   if (distances.empty()) {

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

@@ -96,8 +96,8 @@ BT::NodeStatus FollowFrontEntityAction::tick()
   if (!front_entity_name) {
     return BT::NodeStatus::FAILURE;
   }
-  distance_to_front_entity_ =
-    getDistanceToTargetEntityPolygon(*trajectory, front_entity_name.value());
+  const auto & front_entity_status = getEntityStatus(front_entity_name.value());
+  distance_to_front_entity_ = getDistanceToTargetEntity(*trajectory, front_entity_status);
   if (!distance_to_front_entity_) {
     return BT::NodeStatus::FAILURE;
   }
@@ -111,7 +111,6 @@ BT::NodeStatus FollowFrontEntityAction::tick()
       return BT::NodeStatus::FAILURE;
     }
   }
-  const auto & front_entity_status = getEntityStatus(front_entity_name.value());
   if (!target_speed) {
     target_speed = hdmap_utils->getSpeedLimit(route_lanelets);
   }

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

@@ -92,7 +92,7 @@ BT::NodeStatus FollowLaneAction::tick()
     if (trajectory == nullptr) {
       return BT::NodeStatus::FAILURE;
     }
-    auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
+    const auto distance_to_front_entity = getDistanceToFrontEntity(*trajectory);
     if (distance_to_front_entity) {
       if (
         distance_to_front_entity.value() <=

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

@@ -92,6 +92,14 @@ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose)
 auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Ids & route_lanelets)
   -> std::tuple<std::optional<LaneletPose>, std::optional<lanelet::Id>>;
 
+auto findMatchingLanes(
+  const geometry_msgs::msg::Pose &, const traffic_simulator_msgs::msg::BoundingBox &,
+  const bool include_crosswalk, const double matching_distance = 1.0,
+  const double reduction_ratio = DEFAULT_MATCH_TO_LANE_REDUCTION_RATIO,
+  const traffic_simulator::RoutingGraphType type =
+    traffic_simulator::RoutingConfiguration().routing_graph_type)
+  -> std::optional<std::set<std::pair<double, lanelet::Id>>>;
+
 auto matchToLane(
   const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk,
   const double matching_distance = 1.0,

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

@@ -112,6 +112,12 @@ 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>;
+
+// spline
+auto distanceToSpline(
+  const geometry_msgs::msg::Pose & map_pose,
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box,
+  const math::geometry::CatmullRomSplineInterface & spline, const double s_reference) -> double;
 }  // namespace distance
 }  // namespace traffic_simulator
 #endif  // TRAFFIC_SIMULATOR__UTILS__DISTANCE_HPP_

simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp

@@ -106,6 +106,12 @@ auto isAtEndOfLanelets(
   const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr) -> bool;
 
+auto transformToRoutableCanonicalizedLaneletPose(
+  const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & canonicalized_lanelet_pose,
+  const traffic_simulator_msgs::msg::BoundingBox & to_bounding_box,
+  const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils_ptr)
+  -> std::optional<traffic_simulator::CanonicalizedLaneletPose>;
+
 namespace pedestrian
 {
 auto transformToCanonicalizedLaneletPose(

simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp

@@ -407,12 +407,15 @@ auto canonicalizeLaneletPose(const LaneletPose & lanelet_pose, const lanelet::Id
   return {canonicalized_lanelet_pose, std::nullopt};
 }
 
-auto matchToLane(
-  const Pose & map_pose, const BoundingBox & bounding_box, const bool include_crosswalk,
-  const double matching_distance, const double reduction_ratio, const RoutingGraphType type)
-  -> std::optional<lanelet::Id>
+auto findMatchingLanes(
+  const geometry_msgs::msg::Pose & map_pose,
+  const traffic_simulator_msgs::msg::BoundingBox & bounding_box, const bool include_crosswalk,
+  const double matching_distance, const double reduction_ratio,
+  const traffic_simulator::RoutingGraphType type)
+  -> std::optional<std::set<std::pair<double, lanelet::Id>>>
 {
-  const auto absoluteHullPolygon = [&reduction_ratio, &bounding_box](const auto & pose) {
+  const auto absoluteHullPolygon = [&reduction_ratio,
+                                    &bounding_box](const auto & pose) -> lanelet::BasicPolygon2d {
     auto relative_hull = lanelet::matching::Hull2d{
       lanelet::BasicPoint2d{
         bounding_box.center.x + bounding_box.dimensions.x * 0.5 * reduction_ratio,
@@ -427,34 +430,47 @@ auto matchToLane(
     }
     return absolute_hull_polygon;
   };
-  /// @note prepare object for matching
+  // prepare object for matching
   const auto yaw = math::geometry::convertQuaternionToEulerAngle(map_pose.orientation).z;
   lanelet::matching::Object2d bounding_box_object;
   bounding_box_object.pose.translation() =
     lanelet::BasicPoint2d(map_pose.position.x, map_pose.position.y);
   bounding_box_object.pose.linear() = Eigen::Rotation2D<double>(yaw).matrix();
   bounding_box_object.absoluteHull = absoluteHullPolygon(bounding_box_object.pose);
-  /// @note find matches and optionally filter
+  // find matches and optionally filter
   auto matches = lanelet::matching::getDeterministicMatches(
     *LaneletWrapper::map(), bounding_box_object, matching_distance);
   if (!include_crosswalk) {
     matches =
       lanelet::matching::removeNonRuleCompliantMatches(matches, LaneletWrapper::trafficRules(type));
   }
-  /// @note find best match (minimize offset)
-  if (matches.empty()) {
-    return std::nullopt;
-  } else {
-    std::optional<std::pair<lanelet::Id, double>> min_pair_id_offset;
+  if (not matches.empty()) {
+    std::set<std::pair<double, lanelet::Id>> distances_with_ids;
     for (const auto & match : matches) {
-      if (const auto lanelet_pose =
-            pose::toLaneletPose(map_pose, match.lanelet.id(), matching_distance);
-          lanelet_pose &&
-          (!min_pair_id_offset || lanelet_pose->offset < min_pair_id_offset->second)) {
-        min_pair_id_offset = std::make_pair(lanelet_pose->lanelet_id, lanelet_pose->offset);
+      if (
+        const auto lanelet_pose = toLaneletPose(map_pose, match.lanelet.id(), matching_distance)) {
+        distances_with_ids.emplace(lanelet_pose->offset, lanelet_pose->lanelet_id);
       }
     }
-    return min_pair_id_offset ? std::optional(min_pair_id_offset->first) : std::nullopt;
+    if (not distances_with_ids.empty()) {
+      return distances_with_ids;
+    }
+  }
+  return std::nullopt;
+}
+
+auto matchToLane(
+  const geometry_msgs::msg::Pose & pose, const traffic_simulator_msgs::msg::BoundingBox & bbox,
+  const bool include_crosswalk, const double matching_distance, const double reduction_ratio,
+  const traffic_simulator::RoutingGraphType type) -> std::optional<lanelet::Id>
+{
+  /// @note findMatchingLanes returns a container sorted by distance - increasing, return the nearest id
+  if (
+    const auto matching_lanes =
+      findMatchingLanes(pose, bbox, include_crosswalk, matching_distance, reduction_ratio, type)) {
+    return matching_lanes->begin()->second;
+  } else {
+    return std::nullopt;
   }
 }