update strat

simulated_pcb/alim.py

@@ -4,7 +4,7 @@ import time
 import serial
 
 # Set the parameters for the serial connection
-serial_port = '/dev/pts/12'  # Modify this to your serial port (e.g., 'COM3' on Windows)
+serial_port = '/dev/pts/9'  # Modify this to your serial port (e.g., 'COM3' on Windows)
 baud_rate = 115200  # Modify this to your baud rate
 
 # Open the serial connection

src/modelec_com/src/pcb_alim_interface.cpp

@@ -10,7 +10,7 @@ namespace Modelec
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
         request->name = "pcb_alim";
         request->bauds = 115200;
-        request->serial_port = "/dev/pts/13"; // TODO : check the real serial port
+        request->serial_port = "/dev/pts/10"; // TODO : check the real serial port
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))
         {

src/modelec_strat/include/modelec_strat/navigation_helper.hpp

@@ -101,5 +101,7 @@ namespace Modelec {
         rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
 
         rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
+
+        modelec_interfaces::msg::OdometryPos last_enemy_pos_;
     };
 }

src/modelec_strat/src/missions/prepare_concert_mission.cpp

@@ -10,6 +10,7 @@ namespace Modelec
 
     void PrepareConcertMission::start(rclcpp::Node::SharedPtr node)
     {
+        node_ = node;
 
         if (!nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), nav_->GetTeamId()))
         {
@@ -17,24 +18,35 @@ namespace Modelec
             return;
         }
 
-        if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos()))
+        int timeout = 0;
+        std::vector<int> blacklistId = {};
+        bool canGo = false;
+
+        while (!canGo && timeout < 10)
         {
-            column_ = col;
-        } else
-        {
-            status_ = MissionStatus::FINISH_ALL;
-            return;
+            if (auto col = nav_->GetPathfinding()->GetClosestColumn(nav_->GetCurrentPos(), blacklistId))
+            {
+                column_ = col;
+
+                auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
+
+                auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL);
+                if (res != Pathfinding::FREE)
+                {
+                    blacklistId.push_back(column_->id());
+                }
+                else
+                {
+                    canGo = true;
+                }
+            }
+
+            timeout++;
         }
 
-        node_ = node;
-
-        auto pos = column_->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition();
-
-        auto res = nav_->GoTo(pos.x, pos.y, pos.theta, false, Pathfinding::FREE | Pathfinding::WALL);
-        if (res != Pathfinding::FREE)
+        if (!canGo)
         {
-            RCLCPP_WARN(node_->get_logger(), "Can't go to column %d", column_->id());
-            status_ = MissionStatus::FAILED;
+            status_ = MissionStatus::FINISH_ALL;
             return;
         }
 

src/modelec_strat/src/navigation_helper.cpp

@@ -277,17 +277,22 @@ namespace Modelec {
 
     std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, const std::vector<int>& blacklistedId)
     {
+        // TODO : score
         std::shared_ptr<DepositeZone> closest_zone = nullptr;
-        double min_distance = std::numeric_limits<double>::max();
+        double score = std::numeric_limits<double>::max();
         auto posPoint = Point(pos->x, pos->y, pos->theta);
+        auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta);
+
         for (const auto& [id, zone] : deposite_zones_)
         {
             if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(blacklistedId.begin(), blacklistedId.end(), id))
             {
                 double distance = Point::distance(posPoint, zone->GetPosition());
-                if (distance < min_distance)
+                double enemy_distance = Point::distance(enemyPos, zone->GetPosition());
+                double s = distance * 2 + enemy_distance;
+                if (s < score)
                 {
-                    min_distance = distance;
+                    score = s;
                     closest_zone = zone;
                 }
             }
@@ -318,6 +323,7 @@ namespace Modelec {
     void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
     {
         pathfinding_->OnEnemyPosition(msg);
+        last_enemy_pos_ = *msg;
 
         if (EnemyOnPath(*msg))
         {

src/modelec_strat/src/pathfinding.cpp

@@ -539,9 +539,11 @@ namespace Modelec
     std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos,
                                                                   const std::vector<int>& blacklistedId)
     {
+        // TODO score (count dist and dist with enemy)
         std::shared_ptr<ColumnObstacle> closest_obstacle = nullptr;
         auto robotPos = Point(pos->x, pos->y, pos->theta);
-        float distance = std::numeric_limits<float>::max();
+        auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta);
+        float score = std::numeric_limits<float>::max();
 
         for (const auto& [id, obstacle] : obstacle_map_)
         {
@@ -553,9 +555,13 @@ namespace Modelec
                     for (auto possiblePos : obs->GetAllPossiblePositions())
                     {
                         auto dist = Point::distance(robotPos, possiblePos);
-                        if (dist < distance)
+                        auto distEnemy = Point::distance(enemyPos, possiblePos);
+
+                        auto s = dist * 2 + distEnemy;
+
+                        if (s < score)
                         {
-                            distance = dist;
+                            score = s;
                             closest_obstacle = obs;
                         }
                     }