diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index a8d6ab6..a36574e 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -9,7 +9,7 @@ 50 3 0.5 - 500 + 370 50 -0.3 diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index ddd549a..449f097 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -129,6 +129,8 @@ namespace Modelec float factor_close_enemy_ = 0; + int enemy_emergency_distance_ = 0; + bool last_was_close_enemy_ = false; std::vector waypoints_; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 930a362..955d1c9 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -16,6 +16,8 @@ namespace Modelec factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); + enemy_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 350); + SetupSpawn(); waypoint_reach_sub_ = node_->create_subscription( @@ -76,7 +78,7 @@ namespace Modelec ask_spawn_srv_ = node->create_service( "/nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr, - const std_srvs::srv::Empty::Response::SharedPtr) + const std_srvs::srv::Empty::Response::SharedPtr) { for (auto& ys : spawn_yellow_) { @@ -160,22 +162,22 @@ namespace Modelec waypoint_queue_.pop(); } -/* void NavigationHelper::SendWaypoint() const - { - for (auto& w : waypoints_) + /* void NavigationHelper::SendWaypoint() const { - waypoint_pub_->publish(w.ToMsg()); + for (auto& w : waypoints_) + { + waypoint_pub_->publish(w.ToMsg()); + } } - } - void NavigationHelper::SendWaypoint(const std::vector& waypoints) const - { - for (auto& w : waypoints) + void NavigationHelper::SendWaypoint(const std::vector& waypoints) const { - waypoint_pub_->publish(w); + for (auto& w : waypoints) + { + waypoint_pub_->publish(w); + } } - } -*/ + */ void NavigationHelper::AddWaypoint(const PosMsg& pos, int index) { @@ -543,14 +545,16 @@ namespace Modelec home->y = Config::get("config.home.blue@y", 0); home->theta = Config::get("config.home.blue@theta", 0); } - return home; } + return home; + } void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { pathfinding_->OnEnemyPosition(msg); last_enemy_pos_ = *msg; - if (Point::distance(Point(msg->x, msg->y, msg->theta), Point(current_pos_->x, current_pos_->y, current_pos_->theta)) < 600) + if (Point::distance(Point(msg->x, msg->y, msg->theta), + Point(current_pos_->x, current_pos_->y, current_pos_->theta)) < enemy_emergency_distance_) { RCLCPP_INFO(node_->get_logger(), "Enemy is close, stopping odometry..."); @@ -589,7 +593,6 @@ namespace Modelec void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { - if (!last_was_close_enemy_) { RCLCPP_INFO(node_->get_logger(), "Enemy is close, replanning..."); @@ -633,7 +636,8 @@ namespace Modelec bool NavigationHelper::EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg) { - if (!current_pos_) { + if (!current_pos_) + { return false; }