diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
index 9018f938855..80172c65b1c 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp
+++ b/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(
diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp
index 8969a818308..0d08992f75c 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_spline_interface.hpp
+++ b/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
diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp
index 22827657f2f..10a085932a2 100644
--- a/common/math/geometry/include/geometry/spline/catmull_rom_subspline.hpp
+++ b/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_;
diff --git a/common/math/geometry/src/spline/catmull_rom_subspline.cpp b/common/math/geometry/src/spline/catmull_rom_subspline.cpp
index 8cfadb4ad24..0c80d3ac7c3 100644
--- a/common/math/geometry/src/spline/catmull_rom_subspline.cpp
+++ b/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
diff --git a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp b/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp
index 3c71568a435..58c4199af50 100644
--- a/simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp
+++ b/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
diff --git a/simulation/behavior_tree_plugin/src/action_node.cpp b/simulation/behavior_tree_plugin/src/action_node.cpp
index 57d5a45c966..5cfcd2266f2 100644
--- a/simulation/behavior_tree_plugin/src/action_node.cpp
+++ b/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>
@@ -231,11 +233,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
@@ -244,7 +246,7 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
   std::vector<double> distances;
   std::vector<std::string> entities;
   for (const auto & [name, status] : other_entity_status) {
-    const auto distance = getDistanceToTargetEntityPolygon(spline, name);
+    const auto distance = getDistanceToTargetEntity(spline, status);
     const auto quat = math::geometry::getRotation(
       canonicalized_entity_status->getMapPose().orientation,
       other_entity_status.at(name).getMapPose().orientation);
@@ -292,34 +294,81 @@ 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>
-{
-  if (const auto & status = getEntityStatus(target_name); status.isInLanelet()) {
-    return getDistanceToTargetEntityPolygon(
-      spline, status, width_extension_right, width_extension_left, length_extension_front,
-      length_extension_rear);
-  }
-  return std::nullopt;
-}
-
-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 (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) 
+ *    -> 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 (status.isInLanelet() && 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 {
+  if (
+    !status.isInLanelet() || !canonicalized_entity_status->isInLanelet() ||
+    !isOtherEntityAtConsideredAltitude(status)) {
     return std::nullopt;
   }
+  /** 
+      * 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();
+
+  if (const auto & target_lanelet_pose =
+        traffic_simulator::pose::findRoutableAlternativeLaneletPoseFrom(
+          canonicalized_entity_status->getLaneletId(), status.getCanonicalizedLaneletPose().value(),
+          target_bounding_box, hdmap_utils);
+      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);
+        !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);
+               !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(
@@ -348,9 +397,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()) {
diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp
index 2697bd8bdb1..71e8a78cdca 100644
--- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_front_entity_action.cpp
+++ b/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);
   }
diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp
index f33648f9db9..624f59cb07e 100644
--- a/simulation/behavior_tree_plugin/src/vehicle/follow_lane_sequence/follow_lane_action.cpp
+++ b/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() <=
diff --git a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp
index 278dbee3c47..99d85f2c1fe 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/lanelet_wrapper/pose.hpp
+++ b/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,
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp
index 7919a140491..5a43dd030dc 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/distance.hpp
+++ b/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_
diff --git a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp b/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
index c2c6553e79f..ac269269542 100644
--- a/simulation/traffic_simulator/include/traffic_simulator/utils/pose.hpp
+++ b/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 findRoutableAlternativeLaneletPoseFrom(
+  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(
diff --git a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp
index a0688cd5055..aa98e68373a 100644
--- a/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp
+++ b/simulation/traffic_simulator/src/lanelet_wrapper/pose.cpp
@@ -407,10 +407,12 @@ 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) {
     auto relative_hull = lanelet::matching::Hull2d{
@@ -427,34 +429,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 (!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 (!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;
   }
 }
 
diff --git a/simulation/traffic_simulator/src/utils/distance.cpp b/simulation/traffic_simulator/src/utils/distance.cpp
index cb2f1169d70..880a9cbfa5e 100644
--- a/simulation/traffic_simulator/src/utils/distance.cpp
+++ b/simulation/traffic_simulator/src/utils/distance.cpp
@@ -311,5 +311,47 @@ auto distanceToStopLine(
     return spline.getCollisionPointIn2D(polygon);
   }
 }
+
+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
+{
+  /*
+  * Convergence threshold for binary search.
+  * The search stops when the interval between `s_start` and `s_end` is below this value.
+  * The value 0.05 was chosen empirically to balance accuracy and performance.
+  * A smaller value improves precision but increases computation time.
+  */
+  constexpr double distance_accuracy{0.05};
+
+  const auto bounding_box_map_points =
+    math::geometry::transformPoints(map_pose, math::geometry::getPointsFromBbox(bounding_box));
+  const auto bounding_box_diagonal_length =
+    math::geometry::getDistance(bounding_box_map_points[0], bounding_box_map_points[2]);
+
+  /// @note it may be a good idea to develop spline.getSquaredDistanceIn2D(point, s_start, s_end);
+  std::vector<double> distances;
+  for (const auto & point : bounding_box_map_points) {
+    auto s_start = s_reference - bounding_box_diagonal_length / 2;
+    auto s_end = s_reference + bounding_box_diagonal_length / 2;
+    auto s_start_distance = spline.getSquaredDistanceIn2D(point, s_start);
+    auto s_end_distance = spline.getSquaredDistanceIn2D(point, s_end);
+
+    while (std::abs(s_start - s_end) > distance_accuracy) {
+      double s_mid = s_start + (s_end - s_start) / 2;
+      double s_mid_distance = spline.getSquaredDistanceIn2D(point, s_mid);
+      if (s_start_distance > s_end_distance) {
+        s_start = s_mid;
+        s_start_distance = s_mid_distance;
+      } else {
+        s_end = s_mid;
+        s_end_distance = s_mid_distance;
+      }
+    }
+    distances.push_back(std::min(s_start_distance, s_end_distance));
+  }
+  return std::sqrt(*std::min_element(distances.begin(), distances.end()));
+}
 }  // namespace distance
 }  // namespace traffic_simulator
diff --git a/simulation/traffic_simulator/src/utils/pose.cpp b/simulation/traffic_simulator/src/utils/pose.cpp
index 2ad05436e74..b04df57b577 100644
--- a/simulation/traffic_simulator/src/utils/pose.cpp
+++ b/simulation/traffic_simulator/src/utils/pose.cpp
@@ -328,6 +328,63 @@ auto isAtEndOfLanelets(
          lanelet_wrapper::lanelet_map::laneletLength(lanelet_pose.lanelet_id) <= lanelet_pose.s;
 }
 
+auto findRoutableAlternativeLaneletPoseFrom(
+  const lanelet::Id from_lanelet_id, const CanonicalizedLaneletPose & to_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>
+{
+  /// @note search_distance should be minimal, just to check nearest neighbour lanelets
+  constexpr auto search_distance{3.0};
+  /// @note default_match_to_lane_reduction_ratio is constant described in hdmap_utils.hpp
+  constexpr auto default_match_to_lane_reduction_ratio{0.8};
+  constexpr auto include_crosswalk{false};
+  /** 
+    * @note hdmap_utils_ptr->getRoute requires routing_configuration, 
+    * 'allow_lane_change = true' is needed to check distances to entities on neighbour lanelets 
+    */
+  RoutingConfiguration routing_configuration;
+  routing_configuration.allow_lane_change = true;
+
+  /// @note if there is already a route from_lanelet_id->to_lanelet_id, return it
+  /// if not, transform the 'to_lanelet_id' position into the nearby lanelets and search for a route in relation to them
+  if (const auto to_lanelet_id = to_canonicalized_lanelet_pose.getLaneletPose().lanelet_id;
+      !hdmap_utils_ptr->getRoute(from_lanelet_id, to_lanelet_id, routing_configuration).empty()) {
+    return to_canonicalized_lanelet_pose;
+  } else if (const auto nearby_lanelet_ids = lanelet_wrapper::pose::findMatchingLanes(
+               static_cast<geometry_msgs::msg::Pose>(to_canonicalized_lanelet_pose),
+               to_bounding_box, include_crosswalk, search_distance,
+               default_match_to_lane_reduction_ratio, routing_configuration.routing_graph_type);
+             nearby_lanelet_ids.has_value()) {
+    std::vector<std::pair<CanonicalizedLaneletPose, lanelet::Ids>> routes;
+    for (const auto & [distance, lanelet_id] : nearby_lanelet_ids.value()) {
+      if (auto route =
+            hdmap_utils_ptr->getRoute(from_lanelet_id, lanelet_id, routing_configuration);
+          lanelet_id == to_lanelet_id || route.empty()) {
+        continue;
+      } else if (const auto lanelet_pose = lanelet_wrapper::pose::toLaneletPose(
+                   static_cast<geometry_msgs::msg::Pose>(to_canonicalized_lanelet_pose), lanelet_id,
+                   search_distance);
+                 !lanelet_pose) {
+        continue;
+      } else if (auto canonicalized = toCanonicalizedLaneletPose(lanelet_pose.value());
+                 canonicalized) {
+        routes.emplace_back(std::move(canonicalized.value()), std::move(route));
+      }
+    }
+    if (!routes.empty()) {
+      /// @note we want to have alternative lanelet pose with shortest route so we search for minimum
+      return std::min_element(
+               routes.cbegin(), routes.cend(),
+               [](const auto & a, const auto & b) { return a.second.size() < b.second.size(); })
+        ->first;
+    } else {
+      return std::nullopt;
+    }
+  }
+  return std::nullopt;
+}
+
 namespace pedestrian
 {
 /*