Position FSM

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

@@ -14,10 +14,69 @@
     }
 }
 
 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);
 }