diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 449f097..1df0646 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -95,7 +95,7 @@ namespace Modelec bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg); - bool Replan(bool force = true); + bool Replan(bool force = false); void SetTeamId(int id); diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 955d1c9..6f7a90b 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -587,7 +587,15 @@ namespace Modelec if (EnemyOnPath(*msg)) { RCLCPP_INFO(node_->get_logger(), "Enemy is blocking the path, replanning..."); - Replan(); + if (!Replan()) + { + RCLCPP_WARN(node_->get_logger(), "Replanning failed, stopping odometry..."); + std_msgs::msg::Bool start_odo_msg; + start_odo_msg.data = false; + start_odo_pub_->publish(start_odo_msg); + + last_was_close_enemy_ = true; + } } }