diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
index 4b15416..2c60451 100644
--- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
+++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
@@ -89,7 +89,7 @@ namespace Modelec
 
         bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg);
 
-        void Replan();
+        bool Replan(bool force = true);
 
         void SetTeamId(int id);
 
diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index f9da2e6..412d2a8 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -492,10 +492,20 @@ namespace Modelec
         pathfinding_->OnEnemyPosition(msg);
         last_enemy_pos_ = *msg;
 
-        if (EnemyOnPath(*msg))
+        if (last_was_close_enemy_)
         {
             RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning...");
-            Replan();
+            if (Replan(false)) {
+                last_was_close_enemy_ = false;
+            }
+        }
+        else
+        {
+            if (EnemyOnPath(*msg))
+            {
+                RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning...");
+                Replan();
+            }
         }
     }
 
@@ -506,8 +516,6 @@ namespace Modelec
         pathfinding_->OnEnemyPosition(msg);
         last_enemy_pos_ = *msg;
 
-        // TODO : try to replan next frame
-
         waypoints_.clear();
 
         WaypointMsg emptyWaypoint;
@@ -582,7 +590,7 @@ namespace Modelec
         return dist < radius;
     }
 
-    void NavigationHelper::Replan()
+    bool NavigationHelper::Replan(bool force)
     {
         if (last_go_to_.goal)
         {
@@ -590,12 +598,18 @@ namespace Modelec
             {
                 if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE)
                 {
-                    GoTo(current_pos_, true,
-                         Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY);
-                    // TODO : Handle case where no path is found
+                    if (!force) return false;
+
+                    if (GoTo(current_pos_, true,
+                         Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY) != Pathfinding::FREE)
+                    {
+                        return false;
+                    }
                 }
             }
         }
+
+        return true;
     }
 
     void NavigationHelper::SetTeamId(int id)