Position FSM (#2173)

* Initial fsm

* Adding possession as a consideration

* Style fixes and minor changes

* Adding todo about race condition

* Fix Code Style On position-fsm (#2174)

automated style fixes

Co-authored-by: rishiso <[email protected]>

* Update soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp

Co-authored-by: Sid Parikh <[email protected]>

* Working on PR changes (not finished)

* More fixes (not finished)

* Hardcoding number of robots

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: rishiso <[email protected]>
Co-authored-by: Sid Parikh <[email protected]>

soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp

@@ -16,8 +16,67 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) {
 
 std::optional<RobotIntent> RobotFactoryPosition::get_task(WorldState& world_state,
                                                           FieldDimensions& field_dimensions) {
-    // This is where the current_position can be reassigned based on the
-    // PlayState
+    // If keeper, make no changes
+    if (robot_id_ == 0) {
+        return current_position_->get_task(world_state, field_dimensions);
+    }
+
+    // TODO (Rishi and Jack): Make this synchronized across all robots to avoid race conditions
+
+    // Get sorted positions of all friendly robots
+    using RobotPos = std::pair<int, double>;  // (robotId, yPosition)
+
+    std::vector<RobotPos> robots_copy;
+    for (int i = 0; i < 6; i++) {
+        // Ignore goalie
+        if (i == 0) {
+            continue;
+        }
+        robots_copy.emplace_back(i, world_state.our_robots[i].pose.position().y());
+    }
+
+    std::sort(robots_copy.begin(), robots_copy.end(),
+              [](RobotPos const& a, RobotPos const& b) { return a.second < b.second; });
+
+    // Find relative location of current robot
+    int i = 0;
+    for (RobotPos r : robots_copy) {
+        if (r.first == robot_id_) {
+            break;
+        }
+        i++;
+    }
+
+    // Assigning new position
+    // Checking whether we have possesion or if the ball is on their half (using 1.99 to avoid
+    // rounding issues on midline)
+    if (Position::our_possession_ ||
+        world_state.ball.position.y() > field_dimensions.length() / 1.99) {
+        // Offensive mode
+        // Closest 2 robots on defense, rest on offense
+        if (i <= 1) {
+            if (current_position_->get_name() != "Defense") {
+                current_position_ = std::make_unique<Defense>(robot_id_);
+            }
+        } else {
+            if (current_position_->get_name() != "Offense") {
+                current_position_ = std::make_unique<Offense>(robot_id_);
+            }
+        }
+    } else {
+        // Defensive mode
+        // Closest 4 robots on defense, rest on offense
+        if (i <= 3) {
+            if (current_position_->get_name() != "Defense") {
+                current_position_ = std::make_unique<Defense>(robot_id_);
+            }
+        } else {
+            if (current_position_->get_name() != "Offense") {
+                current_position_ = std::make_unique<Offense>(robot_id_);
+            }
+        }
+    }
+
     return current_position_->get_task(world_state, field_dimensions);
 }