Merge pull request #139 from Immortals-Robotics/dev/zones

Dev/zones

source/soccer/CMakeLists.txt

@@ -97,7 +97,11 @@ set(SOURCE_FILES
         skills/running_def.cpp
         skills/attacker.cpp
 
-        robot/robot.cpp)
+        robot/robot.cpp
+
+        zone/pos_attack.cpp
+        zone/pos_defence.cpp
+        zone/zone.cpp)
 
 add_library(${PROJECT_NAME}
         ${SOURCE_FILES} ${HEADER_FILES})

source/soccer/ai.cpp

@@ -31,6 +31,24 @@ Ai::Ai()
 
     m_current_play = &Ai::haltAll;
 
+    // create zones
+    const unsigned zone_count_x = 2 * (Common::field().width / Zone::kZoneWidth);
+    const unsigned zone_count_y = 2 * (Common::field().height / Zone::kZoneHeight);
+
+    for (unsigned i = 0; i < zone_count_x; i++)
+    {
+        for (unsigned j = 0; j < zone_count_y; j++)
+        {
+            const Common::Vec2 field_min = Common::Vec2(-Common::field().width, -Common::field().height);
+            const Common::Vec2 zone_min  = field_min + Common::Vec2(i * Zone::kZoneWidth, j * Zone::kZoneHeight);
+
+            Zone zone{};
+
+            zone.rect = Common::Rect(zone_min, Zone::kZoneWidth, Zone::kZoneHeight);
+            m_zones.push_back(zone);
+        }
+    }
+
     m_mark_map[&m_mid1] = -1;
     m_mark_map[&m_mid2] = -1;
     m_mark_map[&m_mid3] = -1;
@@ -42,9 +60,14 @@ Ai::Ai()
     m_prioritized_mids.push_back(&m_mid5);
     m_prioritized_mids.push_back(&m_mid1);
     m_prioritized_mids.push_back(&m_mid2);
-
+    m_prioritized_mids.push_back(&m_mid3);
+    m_prioritized_mids.push_back(&m_mid4);
+    m_prioritized_mids.push_back(&m_mid6);
+    m_prioritized_mids.push_back(&m_mid7);
+
     m_ids = {&m_gk, &m_def1, &m_def2, &m_mid1, &m_mid2, &m_mid3, &m_mid4, &m_mid5, &m_mid6, &m_mid7, &m_attack};
-    m_strategy_ids = {&m_gk, &m_def1, &m_mid5, &m_mid1, &m_mid2, &m_attack, &m_def2, &m_mid3, &m_mid4, &m_mid6, &m_mid7};
+    m_strategy_ids = {&m_gk,   &m_def1, &m_mid5, &m_mid1, &m_mid2, &m_attack,
+                      &m_def2, &m_mid3, &m_mid4, &m_mid6, &m_mid7};
 
     for (int i = 0; i < Common::Config::Common::kMaxRobots; i++)
     {
@@ -56,8 +79,6 @@ Ai::Ai()
 
         m_one_touch_type[i]      = OneTouchType::OneTouch;
         m_one_touch_type_used[i] = false;
-
-        m_allaf_pos[i] = Common::Vec2();
     }
 
     const auto strategy_path = std::filesystem::path(DATA_DIR) / "strategy.ims";

source/soccer/ai.h

@@ -78,13 +78,36 @@ class Ai
 
     BallTrajectory m_ball_trajectory;
 
+    // static pos for mids
+    struct Zone
+    {
+        static constexpr float kZoneWidth  = 2000.0f;
+        static constexpr float kZoneHeight = 3000.0f;
+
+        Common::Rect rect;
+
+        Common::Vec2 best_pos;
+        float        score;
+    };
+
+    std::vector<Zone>         m_zones;
+    std::vector<const Zone *> m_sorted_zones;
+
+    std::unordered_map<int *, Common::Vec2> m_mids_static_pos;
+
+    void staticZoneScore(Zone &t_zone) const;
+
+    float staticPosScoreDefence(Common::Vec2 t_pos) const;
+    float staticPosScoreAttack(Common::Vec2 t_pos) const;
+
     // boz ha
 
     void calcIsDefending();
 
-    void                 markManager();
+    void markManager();
+
     std::map<int *, int> m_mark_map;
-    std::vector<int*> m_prioritized_mids;
+    std::vector<int *>   m_prioritized_mids;
 
     Common::Vec2  m_predicted_ball;
     bool          m_circle_reached_behind_ball = false;
@@ -134,6 +157,8 @@ class Ai
     inline Common::Vec2        oppGoalPostTop() const;
     inline Common::Vec2        oppGoalPostBottom() const;
     inline Common::LineSegment oppGoalLine() const;
+    inline Common::Rect        ownPenaltyArea() const;
+    inline Common::Rect        oppPenaltyArea() const;
     inline bool                isOut(Common::Vec2 t_point, float t_margin = 0.0f) const;
 
     // Navigation
@@ -197,8 +222,8 @@ class Ai
     // Strategy
     int strategyWeight();
 
-    PlayBook     m_playbook;
-    Common::Vec2 m_allaf_pos[Common::Config::Common::kMaxRobots];
+    PlayBook                                m_playbook;
+    std::unordered_map<int *, Common::Vec2> m_allaf_pos;
 
     // FSM
     int m_func_state = 0;
@@ -267,7 +292,8 @@ class Ai
     void createMidAssignments();
 
     // this uses the last target position as the target point
-    void createStaticAssignment(int *t_role, Assignment::Priority t_priority, bool t_shoot = false, bool t_chip = false);
+    void createStaticAssignment(int *t_role, Assignment::Priority t_priority, bool t_shoot = false,
+                                bool t_chip = false);
 
     std::vector<Assignment> m_assignments;
 

source/soccer/assignment/cost_static.cpp

@@ -14,6 +14,8 @@ int Ai::staticRoleCost(const int t_robot_idx, const Assignment &t_assignment) co
     if (t_assignment.needs_chip != physical_status.has_chip_kick)
         caps_cost += 5000;
 
-    return dis_cost + caps_cost;
+    int switch_cost = t_robot_idx == *t_assignment.role ? 0 : 500;
+
+    return dis_cost + caps_cost + switch_cost;
 }
 } // namespace Tyr::Soccer

source/soccer/assignment/create_mids.cpp

@@ -6,7 +6,8 @@ void Ai::createMidAssignments()
 {
     std::vector<std::pair<int, float>> crunchingOpps;
 
-    if (m_is_defending)
+    const bool state_needs_marks = m_ref_state.stop() || m_ref_state.running() || m_ref_state.theirFreeKick();
+    if (m_is_defending && state_needs_marks)
     {
         for (int i = 0; i < Common::Config::Common::kMaxRobots; i++)
         {
@@ -75,10 +76,5 @@ void Ai::createMidAssignments()
             createStaticAssignment(role, Assignment::Priority::Low, shoot, chip);
         }
     }
-
-    createStaticAssignment(&m_mid3, Assignment::Priority::None);
-    createStaticAssignment(&m_mid4, Assignment::Priority::None);
-    createStaticAssignment(&m_mid6, Assignment::Priority::None);
-    createStaticAssignment(&m_mid7, Assignment::Priority::None);
 }
 } // namespace Tyr::Soccer

source/soccer/helpers/field.inl

@@ -42,6 +42,30 @@ inline Common::LineSegment Ai::oppGoalLine() const
     return {oppGoalPostBottom(), oppGoalPostTop()};
 }
 
+inline Common::Rect Ai::ownPenaltyArea() const
+{
+    const float penalty_area_half_width = Common::field().penalty_area_width / 2.0f;
+
+    const Common::Vec2 start{m_side * Common::field().width, -penalty_area_half_width};
+
+    const float w = -m_side * Common::field().penalty_area_depth;
+    const float h = Common::field().penalty_area_width;
+
+    return {start, w, h};
+}
+
+inline Common::Rect Ai::oppPenaltyArea() const
+{
+    const float penalty_area_half_width = Common::field().penalty_area_width / 2.0f;
+
+    const Common::Vec2 start{-m_side * Common::field().width, -penalty_area_half_width};
+
+    const float w = m_side * Common::field().penalty_area_depth;
+    const float h = Common::field().penalty_area_width;
+
+    return {start, w, h};
+}
+
 inline bool Ai::isOut(const Common::Vec2 t_point, const float t_margin) const
 {
     return std::fabs(t_point.x) > Common::field().width + t_margin ||

source/soccer/internal_process_data.cpp

@@ -16,5 +16,34 @@ void Ai::internalProcessData()
         m_side = 1;
     else
         m_side = -1;
+
+    if (m_ref_state.stop() || m_ref_state.halt())
+        m_allaf_pos.clear();
+
+    for (auto &zone : m_zones)
+    {
+        staticZoneScore(zone);
+    }
+
+    m_sorted_zones.clear();
+    for (const auto &zone : m_zones)
+    {
+        if (zone.score < 0)
+            continue;
+
+        m_sorted_zones.push_back(&zone);
+
+        if (!Common::almostEqual(zone.score, 0.0f))
+        {
+            Common::Color color = Common::Color::lerp(Common::Color::black(), Common::Color::blue(), zone.score);
+            color.a             = 0.5f;
+
+            Common::debug().draw(zone.rect, color, true);
+        }
+    }
+
+    std::sort(m_sorted_zones.begin(), m_sorted_zones.end(),
+              [](const Zone *a, const Zone *b) -> bool { return a->score > b->score; });
+    Common::logInfo("Found {} valid zones", m_sorted_zones.size());
 }
 } // namespace Tyr::Soccer

source/soccer/plays/kickoff_their_one_wall.cpp

@@ -4,7 +4,7 @@ namespace Tyr::Soccer
 {
 void Ai::kickoffTheirOneWall()
 {
-    m_is_defending = false;
+    m_is_defending = true;
 
     m_assignments.clear();
     createGkAssignment();
@@ -16,11 +16,7 @@ void Ai::kickoffTheirOneWall()
     gkHi(m_gk);
     defHi(m_def1, m_def2, nullptr);
 
-    m_own_robot[m_mid5].face(m_world_state.ball.position);
-    navigate(m_mid5,
-             m_world_state.ball.position.pointOnConnectingLine(
-                 ownGoal(), m_world_state.ball.position.distanceTo(ownGoal()) / 2.0f),
-             VelocityProfile::mamooli());
+
 
     int indexP = -1;
     int indexN = -1;
@@ -42,74 +38,26 @@ void Ai::kickoffTheirOneWall()
 
     if (indexN != -1)
     {
-        if (m_side == -1)
-        {
-            m_own_robot[m_mid1].face(oppGoal());
-            navigate(m_mid1, m_world_state.opp_robot[indexN].position.pointOnConnectingLine(
-                               ownGoal(), (std::fabs(m_world_state.opp_robot[indexN].position.x) + 14) * 1.5));
-            m_mark_map[&m_mid1] = indexN;
-        }
-        else
-        {
-            m_own_robot[m_mid2].face(oppGoal());
-            navigate(m_mid2, m_world_state.opp_robot[indexN].position.pointOnConnectingLine(
-                               ownGoal(), (std::fabs(m_world_state.opp_robot[indexN].position.x) + 14) * 1.5));
-            m_mark_map[&m_mid2] = indexN;
-        }
-    }
-    else
-    {
-        if (m_side == -1)
-        {
-            m_own_robot[m_mid1].face(m_world_state.ball.position);
-            navigate(m_mid1,
-                     m_world_state.ball.position.circleAroundPoint(
-                         Common::Angle::fromDeg(20.0f) + m_world_state.ball.position.angleWith(ownGoal()), 790.0f));
-            m_mark_map[&m_mid1] = -1;
-        }
-        else
-        {
-            m_own_robot[m_mid2].face(m_world_state.ball.position);
-            navigate(m_mid2,
-                     m_world_state.ball.position.circleAroundPoint(
-                         Common::Angle::fromDeg(-20.0f) + m_world_state.ball.position.angleWith(ownGoal()), 790.0f));
-            m_mark_map[&m_mid2] = -1;
-        }
+        m_own_robot[m_mid5].face(oppGoal());
+        navigate(m_mid5, m_world_state.opp_robot[indexN].position.pointOnConnectingLine(
+                             ownGoal(), (std::fabs(m_world_state.opp_robot[indexN].position.x) + 14) * 1.5));
     }
-
     if (indexP != -1)
     {
-        if (m_side == 1)
-        {
-            m_own_robot[m_mid1].face(oppGoal());
-            navigate(m_mid1, m_world_state.opp_robot[indexP].position.pointOnConnectingLine(
-                               ownGoal(), (std::fabs(m_world_state.opp_robot[indexP].position.x) + 14) * 1.5));
-            m_mark_map[&m_mid1] = indexP;
-        }
-        else
-        {
-            m_own_robot[m_mid2].face(oppGoal());
-            navigate(m_mid2, m_world_state.opp_robot[indexP].position.pointOnConnectingLine(
-                               ownGoal(), (std::fabs(m_world_state.opp_robot[indexP].position.x) + 14) * 1.5));
-            m_mark_map[&m_mid2] = indexP;
-        }
+        m_own_robot[m_mid1].face(oppGoal());
+        navigate(m_mid1, m_world_state.opp_robot[indexP].position.pointOnConnectingLine(
+                             ownGoal(), (std::fabs(m_world_state.opp_robot[indexP].position.x) + 14) * 1.5));
     }
-    else
+
+    int zone_idx = 0;
+    for (const auto& mid : m_prioritized_mids)
     {
-        if (m_side == 1)
-        {
-            m_own_robot[m_mid1].face(m_world_state.ball.position);
-            navigate(m_mid1, m_world_state.ball.position.circleAroundPoint(
-                               Common::Angle::fromDeg(20) + m_world_state.ball.position.angleWith(ownGoal()), 790));
-            m_mark_map[&m_mid1] = -1;
-        }
-        else
-        {
-            m_own_robot[m_mid2].face(m_world_state.ball.position);
-            navigate(m_mid2, m_world_state.ball.position.circleAroundPoint(
-                               Common::Angle::fromDeg(-20) + m_world_state.ball.position.angleWith(ownGoal()), 790));
-            m_mark_map[&m_mid2] = -1;
-        }
+        if (m_own_robot[*mid].navigated())
+            continue;
+
+        m_own_robot[*mid].face(m_world_state.ball.position);
+        navigate(*mid, m_sorted_zones[zone_idx]->best_pos, VelocityProfile::mamooli());
+        ++zone_idx;
     }
 
     defenceWall(m_attack, true);

source/soccer/plays/kickoff_us_chip.cpp

@@ -38,6 +38,17 @@ void Ai::kickoffUsChip()
     navigate(m_mid1, Common::Vec2(m_world_state.ball.position.x + m_side * 150, -(Common::field().height - 300)),
              VelocityProfile::mamooli(), NavigationFlagsForceBallObstacle);
 
+    int zone_idx = 0;
+    for (const auto& mid : m_prioritized_mids)
+    {
+        if (m_own_robot[*mid].navigated())
+            continue;
+
+        m_own_robot[*mid].face(m_world_state.ball.position);
+        navigate(*mid, m_sorted_zones[zone_idx]->best_pos, VelocityProfile::mamooli());
+        ++zone_idx;
+    }
+
     Common::Vec2 chip_target = Common::Vec2(-m_side * 2000, 0);
     Common::logDebug("can kick ball: {}", m_ref_state.canKickBall());
     if (m_ref_state.canKickBall())