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

enemy close

Browse Source
This commit is contained in:
acki
2025-05-27 21:48:34 -04:00
parent 60f5428519
commit 660aa8ef69
2 changed files with 23 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

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

30
src/modelec_strat/src/navigation_helper.cpp
View File

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