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

use enemy pos to now where to go

Browse Source
This commit is contained in:
acki
2026-01-28 17:28:11 +01:00
parent 272294bc88
commit a1a229f163
2 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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