From a1a229f1639bc9fb9faa07dd4d10d49d56939a44 Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Wed, 28 Jan 2026 17:28:11 +0100
Subject: [PATCH] use enemy pos to now where to go

---
 .../include/modelec_strat/navigation_helper.hpp          | 9 +++++++++
 src/modelec_strat/src/navigation_helper.cpp              | 2 ++
 2 files changed, 11 insertions(+)

diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
index eabaa3a..e7e0101 100644
--- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
+++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp
@@ -166,6 +166,7 @@ namespace Modelec
         rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
 
         modelec_interfaces::msg::OdometryPos last_enemy_pos_;
+        bool has_enemy_ = false;
 
         bool await_rotate_ = false;
         std::vector<Waypoint> send_back_waypoints_;
@@ -182,6 +183,7 @@ namespace Modelec
     {
         std::shared_ptr<T> closest_obstacle = nullptr;
         auto robotPos = Point(pos->x, pos->y, pos->theta);
+        auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta);
         float distance = std::numeric_limits<float>::max();
 
         for (const auto& obstacle : GetPathfinding()->GetObstacles())
@@ -191,6 +193,13 @@ namespace Modelec
                 if (!obs->IsAtObjective())
                 {
                     auto dist = Point::distance(robotPos, obs->GetPosition());
+
+                    if (has_enemy_)
+                    {
+                        auto enemyDist = Point::distance(enemyPos, obs->GetPosition());
+                        dist *= (1.0f + factor_close_enemy_ * std::exp(-enemyDist / enemy_emergency_distance_));
+                    }
+
                     if (dist < distance)
                     {
                         distance = dist;
diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp
index 021fe26..c219c29 100644
--- a/src/modelec_strat/src/navigation_helper.cpp
+++ b/src/modelec_strat/src/navigation_helper.cpp
@@ -552,6 +552,8 @@ namespace Modelec
 
     void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
     {
+        if (!has_enemy_) has_enemy_ = true;
+
         pathfinding_->OnEnemyPosition(msg);
         last_enemy_pos_ = *msg;