From 4fd8a936ba99949068bc7822a7f2355d75ba8def Mon Sep 17 00:00:00 2001
From: acki <ackimixs@protonmail.com>
Date: Tue, 27 May 2025 15:05:48 -0400
Subject: [PATCH] ignore point too close

---
 .../include/modelec_strat/enemy_manager.hpp       |  5 +++++
 src/modelec_strat/src/enemy_manager.cpp           | 15 ++++++++++++++-
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
index 760870a..c5a8efc 100644
--- a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
+++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp
@@ -41,5 +41,10 @@ namespace Modelec
 
         bool game_stated_ = false;
         float max_stationary_time_s_ = 10.0f;
+        float map_width_ = 0;
+        float map_height_ = 0;
+        float robot_width_ = 0;
+        float robot_length_ = 0;
+        double robot_radius_ = 0;
     };
 }
diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp
index 75763da..c7b73ec 100644
--- a/src/modelec_strat/src/enemy_manager.cpp
+++ b/src/modelec_strat/src/enemy_manager.cpp
@@ -19,6 +19,13 @@ namespace Modelec
 
         max_stationary_time_s_ = Config::get<float>("config.enemy.detection.max_stationary_time_s", 10.0);
 
+        map_width_ = Config::get<float>("config.map.size.map_width_mm", 3000.0);
+        map_height_ = Config::get<float>("config.map.size.map_height_mm", 2000.0);
+
+        robot_width_ = Config::get<float>("config.robot.size.width_mm", 500.0);
+        robot_length_ = Config::get<float>("config.robot.size.length_mm", 500.0);
+        robot_radius_ = std::max(robot_width_, robot_length_) / 2.0;
+
         current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
             "odometry/position", 10,
             [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
@@ -105,12 +112,18 @@ namespace Modelec
             double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
 
             // Ignore points outside of the table
-            if (x_global < 0 || x_global > 3000 || y_global < 0 || y_global > 2000)
+            if (x_global < 0 || x_global > map_width_ || y_global < 0 || y_global > map_height_)
             {
                 angle += msg->angle_increment;
                 continue;
             }
 
+            if (std::hypot(x_global - robot_x, y_global - robot_y) < robot_radius_)
+            {
+                angle += msg->angle_increment;
+                continue; // Ignore points too close to the robot
+            }
+
             if (range < min_distance)
             {
                 min_distance = range;