diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 7b52919..58d61f2 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -8,7 +8,9 @@ 50 0.5 + 10 + -0.3 diff --git a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp index 5889fa4..a86a725 100644 --- a/src/modelec_strat/include/modelec_strat/enemy_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/enemy_manager.hpp @@ -3,6 +3,7 @@ #include #include +#include #include namespace Modelec @@ -23,9 +24,13 @@ namespace Modelec rclcpp::Subscription::SharedPtr current_pos_sub_; rclcpp::Subscription::SharedPtr laser_scan_sub_; + rclcpp::Subscription::SharedPtr state_sub_; rclcpp::Publisher::SharedPtr enemy_pos_pub_; + rclcpp::Publisher::SharedPtr enemy_long_time_pub_; rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Time last_movement_time_; + modelec_interfaces::msg::OdometryPos current_pos_; modelec_interfaces::msg::OdometryPos last_enemy_pos_; @@ -34,5 +39,8 @@ namespace Modelec float min_move_threshold_mm_ = 0.0f; float refresh_rate_s_ = 0.0f; + + bool game_stated_ = false; + float max_stationary_time_s_ = 10.0f; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp index 17fb1ec..032fe75 100644 --- a/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/go_home_mission.hpp @@ -19,6 +19,7 @@ namespace Modelec private: enum Step { + ROTATE_TO_HOME, GO_HOME, GO_CLOSE, DONE diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index c040e9d..37b0e1c 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -41,10 +41,18 @@ namespace Modelec { bool HasArrived() const; + bool RotateTo(const PosMsg::SharedPtr& pos); + bool RotateTo(const Point& pos); + void Rotate(double angle); + int GoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); int GoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); int GoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); + int GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); + int GoToRotateFirst(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); + int GoToRotateFirst(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); + int CanGoTo(const PosMsg::SharedPtr& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); int CanGoTo(int x, int y, double theta, bool isClose = false, int collisionMask = Pathfinding::FREE); int CanGoTo(const Point& goal, bool isClose = false, int collisionMask = Pathfinding::FREE); @@ -88,6 +96,8 @@ namespace Modelec { int team_id_ = YELLOW; + float factor_close_enemy_ = 0; + std::vector waypoints_; PosMsg::SharedPtr current_pos_; @@ -103,5 +113,8 @@ namespace Modelec { rclcpp::Subscription::SharedPtr enemy_pos_sub_; modelec_interfaces::msg::OdometryPos last_enemy_pos_; + + bool await_rotate_ = false; + std::vector send_back_waypoints_; }; } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 3340db8..8a68fe1 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -58,6 +58,7 @@ namespace Modelec { int enemy_length_mm_ = 0; int enemy_margin_mm_ = 0; int margin_mm_ = 0; + float factor_close_enemy_ = 0; Pathfinding(); diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 8ff4039..9806e45 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -17,6 +17,8 @@ namespace Modelec refresh_rate_s_ = Config::get("config.enemy.detection.refresh_rate", 1.0); + max_stationary_time_s_ = Config::get("config.enemy.detection.max_stationary_time_s", 10.0); + current_pos_sub_ = this->create_subscription( "odometry/position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { @@ -32,6 +34,18 @@ namespace Modelec enemy_pos_pub_ = this->create_publisher( "enemy/position", 10); + state_sub_ = create_subscription("/strat/state", 10, + [this](const modelec_interfaces::msg::StratState::SharedPtr msg) + { + if (!game_stated_ && msg->state == modelec_interfaces::msg::StratState::SELECT_MISSION) + { + game_stated_ = true; + } + }); + + enemy_long_time_pub_ = this->create_publisher( + "enemy/long-time", 10); + timer_ = this->create_wall_timer( std::chrono::seconds(1), std::bind(&EnemyManager::TimerCallback, this) @@ -128,9 +142,21 @@ namespace Modelec { last_enemy_pos_ = enemy_pos; last_publish_time_ = this->now(); + last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement enemy_pos_pub_->publish(enemy_pos); RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%ld, y=%ld", enemy_pos.x, enemy_pos.y); } + else + { + auto now = this->now(); + if ((now - last_movement_time_).seconds() > max_stationary_time_s_) + { + enemy_long_time_pub_->publish(last_enemy_pos_); + RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%ld y=%ld", last_enemy_pos_.x, last_enemy_pos_.y); + + last_movement_time_ = now; + } + } } else { diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index bfcc579..7b0a13c 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -2,7 +2,7 @@ namespace Modelec { - GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : step_(GO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) + GoHomeMission::GoHomeMission(const std::shared_ptr& nav, const rclcpp::Time& start_time) : step_(ROTATE_TO_HOME), status_(MissionStatus::READY), nav_(nav), start_time_(start_time) { } @@ -12,7 +12,15 @@ namespace Modelec auto pos = nav_->GetHomePosition(); home_point_ = Point(pos->x, pos->y, pos->theta); - nav_->GoTo(home_point_.GetTakeBasePosition()); + if (nav_->CanGoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) + { + if (nav_->CanGoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } + } + nav_->RotateTo(home_point_); status_ = MissionStatus::RUNNING; } @@ -23,6 +31,20 @@ namespace Modelec switch (step_) { + case ROTATE_TO_HOME: + { + if (nav_->GoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) + { + if (nav_->GoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } + } + } + + step_ = GO_HOME; + break; case GO_HOME: if ((node_->now() - start_time_).seconds() < 94) { diff --git a/src/modelec_strat/src/missions/prepare_concert_mission.cpp b/src/modelec_strat/src/missions/prepare_concert_mission.cpp index 2f4d64d..9104b9c 100644 --- a/src/modelec_strat/src/missions/prepare_concert_mission.cpp +++ b/src/modelec_strat/src/missions/prepare_concert_mission.cpp @@ -30,7 +30,7 @@ namespace Modelec auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL); + auto res = nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL); if (res != Pathfinding::FREE) { blacklistId.push_back(column_->id()); @@ -64,7 +64,7 @@ namespace Modelec case GO_TO_COLUMN: { auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); - nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL); + nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL); } step_ = GO_CLOSE_TO_COLUMN; @@ -86,16 +86,16 @@ namespace Modelec closestDepoZonePoint_ = closestDepoZone_->GetNextPotPos(); auto p = closestDepoZonePoint_.GetTakeBasePosition(); - auto res = nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL); + auto res = nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL); if (res != Pathfinding::FREE) { auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - nav_->GoTo(pos.x, pos.y, pos.theta, true, Pathfinding::FREE | Pathfinding::WALL); + nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::WALL); step_ = GO_BACK; } else { - nav_->GoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL); + nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL); closestDepoZone_->ValidNextPotPos(); step_ = GO_TO_PLATFORM; } @@ -113,13 +113,13 @@ namespace Modelec closestDepoZonePoint_ = closestDepoZone_->ValidNextPotPos(); auto p = closestDepoZonePoint_.GetTakeBasePosition(); - if (nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) + if (nav_->CanGoTo(p, false, Pathfinding::FREE | Pathfinding::WALL) != Pathfinding::FREE) { - nav_->GoTo(p.x, p.y, p.theta, true, Pathfinding::FREE | Pathfinding::WALL); + nav_->GoToRotateFirst(p, true, Pathfinding::FREE | Pathfinding::WALL); } else { - nav_->CanGoTo(p.x, p.y, p.theta, false, Pathfinding::FREE | Pathfinding::WALL); + nav_->GoToRotateFirst(p, false, Pathfinding::FREE | Pathfinding::WALL); } } diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index d4baa26..df57caf 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -13,6 +13,8 @@ namespace Modelec { { pathfinding_ = std::make_shared(node); + factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); + waypoint_reach_sub_ = node_->create_subscription( "odometry/waypoint_reach", 10, [this](const WaypointReachMsg::SharedPtr msg) { @@ -180,6 +182,45 @@ namespace Modelec { return waypoints_.back().reached; } + bool NavigationHelper::RotateTo(const PosMsg::SharedPtr& pos) + { + double angle = std::atan2(pos->y - current_pos_->y, pos->x - current_pos_->x); + + if (std::abs(angle - current_pos_->theta) > M_PI / 3) + { + Rotate(angle); + return true; + } + return false; + } + + bool NavigationHelper::RotateTo(const Point& pos) + { + double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); + + if (std::abs(angle - current_pos_->theta) > M_PI / 3) + { + Rotate(angle); + return true; + } + return false; + } + + void NavigationHelper::Rotate(double angle) + { + waypoints_.clear(); + + WaypointMsg startAngle; + startAngle.x = current_pos_->x; + startAngle.y = current_pos_->y; + startAngle.theta = angle; + startAngle.id = 0; + startAngle.is_end = true; + + waypoints_.emplace_back(startAngle); + SendWaypoint(); + } + int NavigationHelper::GoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { last_go_to_ = {goal, isClose, collisionMask}; @@ -217,6 +258,58 @@ namespace Modelec { return GoTo(goal.x, goal.y, goal.theta, isClose, collisionMask); } + int NavigationHelper::GoToRotateFirst(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) + { + last_go_to_ = {goal, isClose, collisionMask}; + + auto [res, wl] = FindPath(goal, isClose, collisionMask); + + if (wl.empty() || res != Pathfinding::FREE) + { + return res; + } + + auto p = Point(wl[0].x, wl[0].y, wl[0].theta); + if (RotateTo(p)) + { + await_rotate_ = true; + + send_back_waypoints_.clear(); + + for (auto & w : wl) + { + send_back_waypoints_.emplace_back(w); + } + } + else + { + waypoints_.clear(); + + for (auto & w : wl) + { + waypoints_.emplace_back(w); + } + + SendWaypoint(); + } + + return res; + } + + int NavigationHelper::GoToRotateFirst(int x, int y, double theta, bool isClose, int collisionMask) + { + PosMsg::SharedPtr goal = std::make_shared(); + goal->x = x; + goal->y = y; + goal->theta = theta; + return GoToRotateFirst(goal, isClose, collisionMask); + } + + int NavigationHelper::GoToRotateFirst(const Point& goal, bool isClose, int collisionMask) + { + return GoToRotateFirst(goal.x, goal.y, goal.theta, isClose, collisionMask); + } + int NavigationHelper::CanGoTo(const PosMsg::SharedPtr& goal, bool isClose, int collisionMask) { return FindPath(goal, isClose, collisionMask).first; @@ -271,7 +364,7 @@ namespace Modelec { deposite_zones_[obs->GetId()] = obs; } - RCLCPP_INFO(node_->get_logger(), "Loaded %zu obstacles from XML", deposite_zones_.size()); + RCLCPP_INFO(node_->get_logger(), "Loaded %zu zone from XML", deposite_zones_.size()); return true; } @@ -289,7 +382,7 @@ namespace Modelec { { double distance = Point::distance(posPoint, zone->GetPosition()); double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); - double s = distance * 2 + enemy_distance; + double s = distance + enemy_distance * factor_close_enemy_; if (s < score) { score = s; @@ -381,6 +474,18 @@ namespace Modelec { if (waypoint.id == msg->id) { waypoint.reached = true; + + if (await_rotate_) + { + await_rotate_ = false; + + waypoints_.clear(); + for (auto & w : send_back_waypoints_) + { + waypoints_.emplace_back(w); + } + SendWaypoint(); + } } } } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 8157976..68e737d 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -42,6 +42,8 @@ namespace Modelec enemy_margin_mm_ = Config::get("config.enemy.size.margin_mm", 50); + factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); + grid_width_ = Config::get("config.map.size.grid_width", 300); grid_height_ = Config::get("config.map.size.grid_height", 200); @@ -498,18 +500,6 @@ namespace Modelec return {FREE, waypoints}; } - /*void Pathfinding::SetStartAndGoal(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal) - { - current_start_ = start; - current_goal_ = goal; - current_waypoints_ = FindPath(start, goal); - - for (const auto& wp : current_waypoints_) - { - waypoint_pub_->publish(wp); - } - }*/ - void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr& pos) { current_start_ = pos; @@ -557,7 +547,7 @@ namespace Modelec auto dist = Point::distance(robotPos, possiblePos); auto distEnemy = Point::distance(enemyPos, possiblePos); - auto s = dist * 2 + distEnemy; + auto s = dist + distEnemy * factor_close_enemy_; if (s < score) {