diff --git a/tmp/lanelet2_extension/lib/query.cpp b/tmp/lanelet2_extension/lib/query.cpp
index 0d181451..212e0768 100644
--- a/tmp/lanelet2_extension/lib/query.cpp
+++ b/tmp/lanelet2_extension/lib/query.cpp
@@ -775,6 +775,11 @@ bool query::getClosestLanelet(
     }
   }
 
+  if (candidate_lanelets.size() == 1) {
+    *closest_lanelet_ptr = candidate_lanelets.at(0);
+    return found;
+  }
+
   // find by angle
   {
     double min_angle = std::numeric_limits<double>::max();