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