diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 6667368a014..8f87db3f764 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -16,8 +16,67 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id) : Position(r_id) { std::optional 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; // (robotId, yPosition) + + std::vector 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(robot_id_); + } + } else { + if (current_position_->get_name() != "Offense") { + current_position_ = std::make_unique(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(robot_id_); + } + } else { + if (current_position_->get_name() != "Offense") { + current_position_ = std::make_unique(robot_id_); + } + } + } + return current_position_->get_task(world_state, field_dimensions); }