modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

ignore point too close

Browse Source
This commit is contained in:
acki
2025-05-27 15:05:48 -04:00
parent 6bfe5bb65f
commit 4fd8a936ba
2 changed files with 19 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/modelec_strat/include/modelec_strat/enemy_manager.hpp
View File

@@ -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;
};
}

15
src/modelec_strat/src/enemy_manager.cpp
View File

@@ -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;