From 60f542851928e485f4ce0162bc36c787a15ee4de Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Tue, 27 May 2025 21:43:59 -0400
Subject: [PATCH] enemy close

---
 src/modelec_strat/data/config.xml       |  2 +-
 src/modelec_strat/src/enemy_manager.cpp | 32 +++++++++++++++++++++++--
 2 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml
index 17b81b2..8889096 100644
--- a/src/modelec_strat/data/config.xml
+++ b/src/modelec_strat/data/config.xml
@@ -9,7 +9,7 @@
             <min_move_threshold_mm>50</min_move_threshold_mm>
             <refresh_rate>0.5</refresh_rate>
             <max_stationary_time_s>5</max_stationary_time_s>
-            <min_emergency_distance_mm>100</min_emergency_distance_mm>
+            <min_emergency_distance_mm>500</min_emergency_distance_mm>
         </detection>
         <factor_close_enemy>-0.3</factor_close_enemy>
     </enemy>
diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp
index 9bb17b0..12b56c6 100644
--- a/src/modelec_strat/src/enemy_manager.cpp
+++ b/src/modelec_strat/src/enemy_manager.cpp
@@ -124,9 +124,37 @@ namespace Modelec
                 continue;
             }
 
-            if (range < robot_radius_)
+            double range_mm = range * 1000.0;  // conversion en mm
+
+            if (range_mm < robot_radius_)
             {
-                RCLCPP_INFO(this->get_logger(), "Ignoring Lidar point too close to robot: range=%.2f", range);
+                RCLCPP_INFO(this->get_logger(), "Ignoring Lidar point too close to robot body: range=%.2f mm", range_mm);
+                angle += msg->angle_increment;
+                continue;
+            }
+
+            // EMERGENCY detection: obstacle very close but not the robot itself
+            if (range_mm < min_emergency_distance_)
+            {
+                // Convert to local robot frame
+                double x_local = range * std::cos(angle) * 1000.0; // meters -> mm
+                double y_local = range * std::sin(angle) * 1000.0; // meters -> mm
+
+                // Rotate + translate into global frame
+                double x_global = robot_x + (x_local * std::cos(robot_theta) - y_local * std::sin(robot_theta));
+                double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
+
+                // Check if in bounds
+                if (x_global >= 0 && x_global <= map_width_ && y_global >= 0 && y_global <= map_height_)
+                {
+                    modelec_interfaces::msg::OdometryPos emergency_msg;
+                    emergency_msg.x = x_global;
+                    emergency_msg.y = y_global;
+                    emergency_msg.theta = 0.0;
+
+                    close_enemy_pos_pub_->publish(emergency_msg);
+                    RCLCPP_WARN(this->get_logger(), "EMERGENCY CLOSE OBJECT DETECTED at x=%.2f y=%.2f (%.1f mm)", x_global, y_global, range_mm);
+                }
 
                 angle += msg->angle_increment;
                 continue;