diff --git a/roboteam_ai/include/roboteam_ai/stp/PlayDecider.hpp b/roboteam_ai/include/roboteam_ai/stp/PlayDecider.hpp index b54aa0d71..aa13f793f 100644 --- a/roboteam_ai/include/roboteam_ai/stp/PlayDecider.hpp +++ b/roboteam_ai/include/roboteam_ai/stp/PlayDecider.hpp @@ -40,6 +40,11 @@ class PlayDecider { */ static void lockPlay(const std::optional playName); + /** + * @brief Unlocks the play + */ + static void unlockPlay(); + /** * @brief This function checks if there is a locked play. If there is, pick that play. * If there isn't, pick the play with the highest score (either a locked play through the interface or just the highest scored play) diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h index d32948f63..98da29ade 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h @@ -58,12 +58,15 @@ class PassComputations { * @param point the point to check for validity * @param ballLocation the current ball location * @param possibleReceiverLocations the locations of all robots that could receive the ball + * @param possibleReceiverVelocities the velocities of all robots that could receive the ball * @param passerLocation the location of the robot that will pass the ball + * @param passerVelocity the velocity of the robot that will pass the ball * @param field the current field * @param world the current world * @return bool indicating whether this point is (likely) possible to pass to */ - static bool pointIsValidPassLocation(Vector2 point, Vector2 ballLocation, const std::vector& possibleReceiverLocations, Vector2 passerLocation, const Field& field, + static bool pointIsValidPassLocation(Vector2 point, Vector2 ballLocation, const std::vector& possibleReceiverLocations, + const std::vector& possibleReceiverVelocities, Vector2 passerLocation, Vector2 passerVelocity, const Field& field, const world::World* world); /** @@ -87,19 +90,21 @@ class PassComputations { /** * @brief Approximate the time it takes a robot to reach a point * @param robotPosition current position of robot + * @param robotVelocity current velocity of robot * @param targetPosition position to calculate travel time to * @return approximated time to reach target from position */ - static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 targetPosition); + static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition); /** * Approximate the time it takes the ball to reach a point * @param ballLocation current location of the ball * @param passerLocation current location of the passer + * @param robotVelocity current velocity of robot * @param targetPosition position to calculate travel time to * @return approximated time for ball to reach target position */ - static double calculateBallTravelTime(Vector2 ballLocation, Vector2 passerLocation, Vector2 targetPosition); + static double calculateBallTravelTime(Vector2 ballLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition); }; } // namespace rtt::ai::stp::computations #endif // RTT_PASSCOMPUTATIONS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index fdb807394..fa89f2c92 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h @@ -31,9 +31,9 @@ struct EnemyInfo { int id; }; -struct HarasserInfo { - int harasserId; - double timeToBall; +struct InterceptInfo { + Vector2 interceptLocation; + int interceptId = -1; }; /** @@ -94,20 +94,20 @@ class PositionComputations { static Vector2 calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field); /** - * @brief Calculates info for the keeper - * @param stpInfos The current stpInfos - * @param field The current field + * @brief Calculates the id of the harasser * @param world The current world + * @param field The current field + * @return HarasserInfo with the id and the time to the ball */ - static void calculateInfoForKeeper(std::unordered_map &stpInfos, const Field &field, world::World *world) noexcept; + static InterceptInfo calculateHarasserId(rtt::world::World *world, const Field &field) noexcept; /** - * @brief Calculates the id of the harasser - * @param world The current world + * @brief Calculates the position for an intercept * @param field The current field - * @return HarasserInfo with the id and the time to the ball + * @param world The current world + * @param interceptId The Id of the robot which will intercept */ - static HarasserInfo calculateHarasserId(rtt::world::World *world, const Field &field) noexcept; + static InterceptInfo calculateInterceptionInfo(const Field &field, rtt::world::World *world, int interceptId) noexcept; /** * @brief Calculates info for the harasser role @@ -115,10 +115,10 @@ class PositionComputations { * @param roles The current roles * @param field The current field * @param world The current world - * @param timeToBall The time to the ball + * @param interceptionLocation The location where the harasser should go to */ static void calculateInfoForHarasser(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, - const Field &field, world::World *world, double timeToBall) noexcept; + const Field &field, world::World *world, Vector2 interceptionLocation) noexcept; /** * @brief Calculates info for the defenders diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h index b5ee45897..0f471632e 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h @@ -52,8 +52,8 @@ constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot // Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.04; /**< Angle error for a robot to be considered to have reached a position */ // Maximum inaccuracy during ballplacement -constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS; /**< Distance error for the ball to be considered to have reached the ball placement position*/ -constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9; /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */ +constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.02; /**< Distance error for the ball to be considered to have reached the ball placement position*/ +constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9; /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */ /// Invariant constants constexpr uint8_t FUZZY_TRUE = 255; /**< Value at which the fuzzy logic is considered 100% true */ diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h index 16011d279..288bed047 100644 --- a/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h +++ b/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h @@ -56,7 +56,7 @@ struct ScoredPosition { * They consist of a generalized weight combination. */ -constexpr ScoreProfile AttackingPass = {0.5, 1, 1}; /**< Scoring weights for Attacking Pass */ +constexpr ScoreProfile AttackingPass = {0.5, 1, 2}; /**< Scoring weights for Attacking Pass */ constexpr ScoreProfile SafePass = {1, 1, 0}; /**< Scoring weights for Safe Pass, used by the keeper */ constexpr ScoreProfile LineOfSight = {0, 1, 0}; /**< Scoring weights for Line of Sight score, only used for testing minimum line of sight */ constexpr ScoreProfile GoalShot = {0, 0, 1}; /**< Scoring weights for Goal Shot Score, a position where we can shoot at goal */ diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/position/GoalShotEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/position/GoalShotEvaluation.h index f6685ddf2..3e2a5c725 100644 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/position/GoalShotEvaluation.h +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/position/GoalShotEvaluation.h @@ -16,10 +16,11 @@ class GoalShotEvaluation { /** * @brief Score ability to make a goal from position based on how much of the goal is visible and the angular size of the goal * @param goalVisibility % visibility of the goal (taking in account other robots) - * @param goalAngle angular size of the goal (relative to the goal angle from the penalty point) + * @param goalAngle angular size of the goal (relative to the goal angle from the best position) + * @param normalizedDistanceToGoal normalized distance to the goal (0 = closest, 1 = furthest) * @return uint8_t score */ - [[nodiscard]] static uint8_t metricCheck(double goalVisibility, double goalAngle) noexcept; + [[nodiscard]] static uint8_t metricCheck(double goalVisibility, double goalAngle, double normalizedDistanceToGoal) noexcept; }; } // namespace rtt::ai::stp::evaluation #endif // RTT_GOALSHOTEVALUATION_H diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h index 4b9ea6469..92f1e8225 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h @@ -45,7 +45,7 @@ class DefendPass : public Play { */ const char* getName() const override; - HarasserInfo harasserInfo; + InterceptInfo harasserInfo; }; } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h index 8754a3b51..f1f80890f 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h @@ -44,7 +44,7 @@ class DefendShot : public Play { */ const char* getName() const override; - HarasserInfo harasserInfo; + InterceptInfo harasserInfo; }; } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h index 5b1fcb947..0cb3d7896 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/Dealer.h +++ b/roboteam_ai/include/roboteam_ai/utilities/Dealer.h @@ -142,11 +142,12 @@ class Dealer { /** * @brief Calculates the cost of travelling a certain distance - * @param distance The distance to travel - * @param fieldHeight The height of the field + * @param robot the robot for which the cost will be determined + * @param target_position the position the robot will be travelling to + * @param MaxRobotVelocity the maximum velocity a robot can have * @return Cost of travelling that distance */ - static double costForDistance(double distance, double fieldHeight); + static double costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity); /** * @brief Calculates the cost of a property of a robot diff --git a/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h b/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h index 92eeccf46..b2841e411 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h +++ b/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h @@ -39,6 +39,7 @@ enum class RefCommand { BALL_PLACEMENT_US_DIRECT = 22, // Ball placement before a direct free kick DIRECT_FREE_US_STOP = 23, // Direct free kick us directly after a stop DIRECT_FREE_THEM_STOP = 24, // Direct free kick them directly after a stop + PREPARE_FORCED_START = 25, // The state before a forced start, after ball placement them UNDEFINED = -1 }; @@ -115,6 +116,9 @@ inline std::ostream &operator<<(std::ostream &os, const RefCommand &command) { case RefCommand::PENALTY_THEM: os << "PENALTY_THEM"; break; + case RefCommand::PREPARE_FORCED_START: + os << "PREPARE_FORCED_START"; + break; case RefCommand::UNDEFINED: os << "UNDEFINED"; break; diff --git a/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h b/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h index cbfbeb737..91552c5c0 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h +++ b/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h @@ -73,6 +73,7 @@ class StrategyManager { GameState(RefCommand::PREPARE_KICKOFF_THEM, Constants::RULESET_STOP(), false, RefCommand::KICKOFF_THEM), GameState(RefCommand::PREPARE_PENALTY_US, Constants::RULESET_STOP(), false, RefCommand::PENALTY_US), GameState(RefCommand::PREPARE_PENALTY_THEM, Constants::RULESET_STOP(), false, RefCommand::PENALTY_THEM), + GameState(RefCommand::PREPARE_FORCED_START, Constants::RULESET_STOP()), GameState(RefCommand::DIRECT_FREE_THEM, Constants::RULESET_DEFAULT()), GameState(RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()), diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp index 0b0702124..e5ef42f76 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp @@ -27,7 +27,8 @@ std::optional WorldObjects::getFirstCollision(const rtt::world::W if (avoidObjects.shouldAvoidDefenseArea) { calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } - if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) { + if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM || + rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) { calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]); } if (avoidObjects.shouldAvoidBall) { diff --git a/roboteam_ai/src/gui/networking/InterfaceSubscriber.cpp b/roboteam_ai/src/gui/networking/InterfaceSubscriber.cpp index e2f961b79..0e92b5ea8 100644 --- a/roboteam_ai/src/gui/networking/InterfaceSubscriber.cpp +++ b/roboteam_ai/src/gui/networking/InterfaceSubscriber.cpp @@ -30,6 +30,9 @@ void InterfaceSubscriber::onMessage(const proto::MsgFromInterface&& message) { case proto::MsgFromInterface::kSetRuntimeConfig: { RuntimeConfig::useReferee = message.set_runtime_config().use_referee(); RuntimeConfig::ignoreInvariants = message.set_runtime_config().ignore_invariants(); + if (RuntimeConfig::useReferee) { + stp::PlayDecider::unlockPlay(); + } } break; case proto::MsgFromInterface::kSetGameSettings: { diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index c1871a0e5..64d18c200 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -17,6 +17,7 @@ void Play::initialize() noexcept { for (auto &role : roles) { if (role == nullptr) continue; stpInfos[role->getName()].setShouldAvoidBall(FieldComputations::getBallAvoidance()); + stpInfos[role->getName()].setMaxRobotVelocity(control::ControlUtils::getMaxVelocity(false)); } calculateInfoForRoles(); distributeRoles(); @@ -184,22 +185,25 @@ uint8_t Play::getLastScore() const { return lastScore.value_or(0); } bool Play::shouldEndPlay() noexcept { return false; } void Play::DrawMargins() noexcept { - std::array ball = {world->getWorld()->getBall()->get()->position}; + RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); + RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); std::array rightDefenseAreaMargin = {field.rightDefenseArea.topRight() + Vector2(0.0, 0.2), field.rightDefenseArea.topLeft() + Vector2(-0.2, 0.2), field.rightDefenseArea.bottomLeft() + Vector2(-0.2, -0.2), field.rightDefenseArea.bottomRight() + Vector2(0.0, -0.2)}; std::array leftDefenseAreaMargin = {field.leftDefenseArea.topLeft() + Vector2(0.0, 0.2), field.leftDefenseArea.topRight() + Vector2(0.2, 0.2), field.leftDefenseArea.bottomRight() + Vector2(0.2, -0.2), field.leftDefenseArea.bottomLeft() + Vector2(0.0, -0.2)}; std::array cardId = {rtt::Vector2(0.0, -field.playArea.height() / 2)}; - std::array pathToBallPlacement = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; - - RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); - RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); - auto color = proto::Drawing::BLUE; + std::array placementLocation = {rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; + std::array pathToPlacementLocation = {world->getWorld()->getBall()->get()->position, world->getWorld()->getBall()->get()->position}; + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || + currentGameState == RefCommand::PREPARE_FORCED_START) + pathToPlacementLocation = {world->getWorld()->getBall()->get()->position, rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; // Drawing all figures regarding states robots have to avoid certain area's (stop, ball placement, free kick, kick off) if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM_STOP || - currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM) { - if (currentGameState != RefCommand::BALL_PLACEMENT_THEM && currentGameState != RefCommand::BALL_PLACEMENT_US && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { + currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM || + currentGameState == RefCommand::PREPARE_FORCED_START) { + if (currentGameState != RefCommand::BALL_PLACEMENT_THEM && currentGameState != RefCommand::BALL_PLACEMENT_US && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT && + currentGameState != RefCommand::PREPARE_FORCED_START) { rtt::ai::gui::Out::draw( { .label = "Left defense area to avoid", @@ -221,41 +225,42 @@ void Play::DrawMargins() noexcept { }, rightDefenseAreaMargin); } - - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) + auto color = proto::Drawing::BLUE; + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM || + currentGameState == RefCommand::PREPARE_FORCED_START) color = GameSettings::isYellow() ? proto::Drawing::YELLOW : proto::Drawing::BLUE; else if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || currentGameState == RefCommand::DIRECT_FREE_US || - currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) + currentGameState == RefCommand::KICKOFF_US) color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW; else color = proto::Drawing::GREEN; + for (auto method : {proto::Drawing::CIRCLES, proto::Drawing::LINES_CONNECTED}) { + rtt::ai::gui::Out::draw( + { + .label = method == proto::Drawing::CIRCLES ? "Ball area to avoid" : "Path to placement location", + .color = method == proto::Drawing::CIRCLES ? color : proto::Drawing::BLACK, + .method = method, + .category = proto::Drawing::MARGINS, + .size = method == proto::Drawing::CIRCLES ? 52 : 8, + .thickness = method == proto::Drawing::CIRCLES ? 4 : 8, + }, + pathToPlacementLocation); + } + } + // Drawing all figures regarding ball placement location and the path towards it + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || + currentGameState == RefCommand::PREPARE_FORCED_START) { rtt::ai::gui::Out::draw( { - .label = "Ball area to avoid", - .color = color, + .label = "Placement location", + .color = proto::Drawing::BLACK, .method = proto::Drawing::CIRCLES, .category = proto::Drawing::MARGINS, - .size = 53, - .thickness = 3, + .size = 17, + .thickness = 4, }, - ball); - } - - // Drawing all figures regarding ball placement location and the path towards it - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { - for (auto method : {proto::Drawing::CROSSES, proto::Drawing::LINES_CONNECTED}) { - RTT_INFO(method) - rtt::ai::gui::Out::draw( - { - .label = method == proto::Drawing::CROSSES ? "Placement location" : "Path to placement location ", - .color = proto::Drawing::BLACK, - .method = method, - .size = method == proto::Drawing::CROSSES ? 10 : 8, - .thickness = method == proto::Drawing::CROSSES ? 5 : 8, - }, - pathToBallPlacement); - } + placementLocation); } if (GameStateManager::getCurrentGameState().cardId != -1) { @@ -266,10 +271,27 @@ void Play::DrawMargins() noexcept { .method = proto::Drawing::CIRCLES, .category = proto::Drawing::MARGINS, .size = 15, - .thickness = 10, + .thickness = 7, }, cardId); } + std::array names = {"harasser", "passer", "receiver", "striker"}; + std::array colors = {proto::Drawing::RED, proto::Drawing::WHITE, proto::Drawing::MAGENTA, proto::Drawing::WHITE}; + for (int i = 0; i < names.size(); i++) { + if (stpInfos[names[i]].getRobot()) { + std::array position = {stpInfos[names[i]].getRobot()->get()->getPos()}; + rtt::ai::gui::Out::draw( + { + .label = names[i], + .color = colors[i], + .method = proto::Drawing::CIRCLES, + .category = proto::Drawing::DEBUG, + .size = 15, + .thickness = 7, + }, + position); + } + } } } // namespace rtt::ai::stp diff --git a/roboteam_ai/src/stp/PlayDecider.cpp b/roboteam_ai/src/stp/PlayDecider.cpp index 0e41aa8dd..8a0bce65c 100644 --- a/roboteam_ai/src/stp/PlayDecider.cpp +++ b/roboteam_ai/src/stp/PlayDecider.cpp @@ -50,6 +50,12 @@ void PlayDecider::lockPlay(const std::optional playName) { playLock.didChange = true; } +void PlayDecider::unlockPlay() { + std::scoped_lock lock(playLock.lock); + playLock.isSet = false; + playLock.didChange = true; +} + Play* PlayDecider::getPlayForName(std::string name, const std::vector>& plays) { auto found = std::find_if(plays.begin(), plays.end(), [&](auto& play) { return play->getName() == name; }); if (found == plays.end()) { diff --git a/roboteam_ai/src/stp/computations/PassComputations.cpp b/roboteam_ai/src/stp/computations/PassComputations.cpp index 9dabeb97f..37a0e13c6 100644 --- a/roboteam_ai/src/stp/computations/PassComputations.cpp +++ b/roboteam_ai/src/stp/computations/PassComputations.cpp @@ -8,8 +8,8 @@ #include #include "control/ControlUtils.h" -#include "roboteam_utils/Tube.h" #include "gui/Out.h" +#include "roboteam_utils/Tube.h" #include "stp/constants/ControlConstants.h" namespace rtt::ai::stp::computations { @@ -36,9 +36,11 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w auto passerIt = std::find_if(us.begin(), us.end(), [passInfo](auto& bot) { return bot->getId() == passInfo.passerId; }); Vector2 passerLocation; + Vector2 passerVelocity; // there should always be a valid passer, since we know there are >2 robots (or 2 robots where the keeper can pass/receive), but check just in case something goes wrong if (passerIt != us.end()) { passerLocation = passerIt->get()->getPos(); + passerVelocity = passerIt->get()->getVel(); us.erase(passerIt); } else { // If we could not find a passer, we return an empty passInfo @@ -47,9 +49,11 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w // This is a vector with the locations of all robots that could act as a receiver (ie all robots except the keeper and the passer) std::vector possibleReceiverLocations; + std::vector possibleReceiverVelocities; // Add all robots that can also kick (nice for kicking at goal or passing further) for (const auto& robot : us) { if (Constants::ROBOT_HAS_KICKER(robot->getId())) possibleReceiverLocations.push_back(robot->getPos()); + if (Constants::ROBOT_HAS_KICKER(robot->getId())) possibleReceiverVelocities.push_back(robot->getPos()); } // If there are no other robots that can kick, add every other robots if (possibleReceiverLocations.empty()) { @@ -58,6 +62,12 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w possibleReceiverLocations.push_back(robot->getPos()); } } + if (possibleReceiverVelocities.empty()) { + possibleReceiverVelocities.reserve(us.size()); + for (const auto& robot : us) { + possibleReceiverVelocities.push_back(robot->getPos()); + } + } // Now find out the best pass location and corresponding info auto possiblePassLocationsVector = getPassGrid(field).getPoints(); @@ -66,17 +76,17 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w for (auto& point : pointVector) { numberOfPoints++; std::array pointToPassTo = {point}; - if (pointIsValidPassLocation(point, ballLocation, possibleReceiverLocations, passerLocation, field, world)) { + if (pointIsValidPassLocation(point, ballLocation, possibleReceiverLocations, possibleReceiverVelocities, passerLocation, passerVelocity, field, world)) { gen::ScoredPosition scoredPosition = PositionScoring::scorePosition(point, profile, field, world); rtt::ai::gui::Out::draw( { .label = "pass_location" + std::to_string(numberOfPoints), - .color = passInfo.passScore >= 245 ? proto::Drawing::GREEN : proto::Drawing::MAGENTA, + .color = scoredPosition.score >= 245 ? proto::Drawing::GREEN : proto::Drawing::MAGENTA, .method = proto::Drawing::PLUSES, .category = proto::Drawing::DEBUG, .forRobotId = passInfo.passerId, - .size = pow(passInfo.passScore/40, 2) + 4, - .thickness = pow(passInfo.passScore/80, 2) + 3, + .size = static_cast(pow(scoredPosition.score / 40, 2) + 4), + .thickness = static_cast(pow(scoredPosition.score / 80, 2) + 3), }, pointToPassTo); if (scoredPosition.score > passInfo.passScore) { @@ -104,17 +114,20 @@ Grid PassComputations::getPassGrid(const Field& field) { return Grid(-gridWidth / 2, -gridHeight / 2, gridWidth, gridHeight, numPoints, numPoints); // 81 points spread over the whole field } -bool PassComputations::pointIsValidPassLocation(Vector2 point, Vector2 ballLocation, const std::vector& possibleReceiverLocations, Vector2 passerLocation, - const Field& field, const world::World* world) { +bool PassComputations::pointIsValidPassLocation(Vector2 point, Vector2 ballLocation, const std::vector& possibleReceiverLocations, + const std::vector& possibleReceiverVelocities, Vector2 passerLocation, Vector2 passerVelocity, const Field& field, + const world::World* world) { constexpr double MINIMUM_PASS_DISTANCE = 2.0; // This can be dribbled instead of passed if (point.dist(ballLocation) < MINIMUM_PASS_DISTANCE) return false; constexpr double MINIMUM_LINE_OF_SIGHT = 10.0; // The minimum LoS to be a valid pass, otherwise, the pass will go into an enemy robot if (PositionScoring::scorePosition(point, gen::LineOfSight, field, world).score < MINIMUM_LINE_OF_SIGHT) return false; if (!FieldComputations::pointIsValidPosition(field, point)) return false; // Pass is valid if the above conditions are met and there is a robot whose travel time is smaller than the balls travel time (i.e. the robot can actually receive the ball) - auto ballTravelTime = calculateBallTravelTime(ballLocation, passerLocation, point); - return std::any_of(possibleReceiverLocations.begin(), possibleReceiverLocations.end(), - [&](auto& robotPos) { return calculateRobotTravelTime(robotPos, point) < ballTravelTime; }); + auto ballTravelTime = calculateBallTravelTime(ballLocation, passerLocation, passerVelocity, point); + for (int i = 0; i < possibleReceiverLocations.size(); i++) { + if (calculateRobotTravelTime(possibleReceiverLocations[i], possibleReceiverVelocities[i], point) < ballTravelTime) return true; + } + return false; } int PassComputations::getPasserId(Vector2 ballLocation, const std::vector& ourRobots, const world::World* world) { @@ -155,13 +168,13 @@ int PassComputations::getKeeperId(const std::vector& pos return -1; // Should never reach this point unless there are no robots } -// TODO: use BBT for this -double PassComputations::calculateRobotTravelTime(Vector2 robotPosition, Vector2 targetPosition) { return robotPosition.dist(targetPosition) * 2; } +double PassComputations::calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition) { + return Trajectory2D(robotPosition, robotVelocity, targetPosition, control::ControlUtils::getMaxVelocity(false), ai::Constants::MAX_ACC_UPPER()).getTotalTime() * 1.1; +} -// TODO: create better approximation -double PassComputations::calculateBallTravelTime(Vector2 ballPosition, Vector2 passerLocation, Vector2 targetPosition) { - auto travelTime = calculateRobotTravelTime(passerLocation, ballPosition - (passerLocation - ballPosition).stretchToLength(control_constants::ROBOT_RADIUS)); - auto rotateTime = (ballPosition - passerLocation).toAngle().shortestAngleDiff(targetPosition - ballPosition) / (control_constants::ANGLE_RATE * 2); +double PassComputations::calculateBallTravelTime(Vector2 ballPosition, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition) { + auto travelTime = calculateRobotTravelTime(passerLocation, passerVelocity, ballPosition - (passerLocation - ballPosition).stretchToLength(control_constants::ROBOT_RADIUS)); + auto rotateTime = (ballPosition - passerLocation).toAngle().shortestAngleDiff(targetPosition - ballPosition) / (M_PI); double ballSpeed = control::ControlUtils::determineKickForce(ballPosition.dist(targetPosition), ShotType::PASS); auto ballTime = ballPosition.dist(targetPosition) / ballSpeed; return travelTime + rotateTime + ballTime; diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 5d596b5e3..d740d5e09 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -55,7 +55,8 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie Vector2 ballPos; // Calculate the position of the ball, projected onto the field - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || + currentGameState == RefCommand::PREPARE_FORCED_START) { ballPos = GameStateManager::getRefereeDesignatedPosition(); } else { ballPos = FieldComputations::projectPointInField(field, world->getWorld().value().getBall()->get()->position); @@ -216,7 +217,8 @@ Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, std::unique_ptr avoidShape; - if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { + if (currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || + currentGameState == RefCommand::PREPARE_FORCED_START) { avoidShape = std::make_unique( Tube(ballPosition, GameStateManager::getRefereeDesignatedPosition(), control_constants::AVOID_BALL_DISTANCE + control_constants::GO_TO_POS_ERROR_MARGIN)); } else { @@ -250,23 +252,49 @@ Vector2 PositionComputations::calculatePositionOutsideOfShape(Vector2 targetPosi return targetPosition; } -void PositionComputations::calculateInfoForKeeper(std::unordered_map &stpInfos, const Field &field, world::World *world) noexcept { - stpInfos["keeper"].setPositionToMoveTo(field.leftGoalArea.rightLine().center()); - stpInfos["keeper"].setEnemyRobot(world->getWorld()->getRobotClosestToBall(world::them)); +InterceptInfo PositionComputations::calculateHarasserId(world::World *world, const Field &field) noexcept { + InterceptInfo interceptionInfo; + // If the ball is moving, we will try to intercept. Otherwise, the harasser will go to the ball. + if ((world->getWorld()->getBall()->get()->velocity).length() >= control_constants::BALL_STILL_VEL) { + interceptionInfo = PositionComputations::calculateInterceptionInfo(field, world, -1); + } else + interceptionInfo.interceptLocation = world->getWorld()->getBall()->get()->position; + return interceptionInfo; } -HarasserInfo PositionComputations::calculateHarasserId(world::World *world, const Field &field) noexcept { +InterceptInfo PositionComputations::calculateInterceptionInfo(const Field &field, world::World *world, int interceptId) noexcept { auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); + double minTimeToTarget = std::numeric_limits::max(); int keeperId = GameStateManager::getCurrentGameState().keeperId; - double maximumTimeToIntercept = 1; + int interceptScore = 50; + int minTimeRobotId = -1; + Vector2 ballPosition = world->getWorld()->getBall()->get()->position; + Vector2 interceptLocation = ballPosition; Vector2 newBallPos; + // We want the keeper to take less risk, so the LOS score has to be higher + if (interceptId == keeperId) interceptScore = 80; + auto interceptRobot = + std::find_if(world->getWorld()->getUs().begin(), world->getWorld()->getUs().end(), [interceptId](const auto &robot) { return robot->getId() == interceptId; }); for (double loopTime = 0; loopTime < 1; loopTime += 0.1) { newBallPos = FieldComputations::getBallPositionAtTime(*(world->getWorld()->getBall()->get()), loopTime); - if (!field.playArea.contains(newBallPos, control_constants::BALL_RADIUS)) { - maximumTimeToIntercept = loopTime; - break; + // If the line of sight score is too low or the ball is out of field, we don't intercept, we go to the ball + if (interceptId != keeperId && PositionScoring::scorePosition(newBallPos, gen::LineOfSight, field, world).score < interceptScore || + !field.playArea.contains(newBallPos, control_constants::BALL_RADIUS)) { + return {interceptLocation, minTimeRobotId}; + } + // If the robot with interceptId is already close to the line, project it's position onto the line + if (interceptId != -1 && interceptId != keeperId && + LineSegment(ballPosition, newBallPos).distanceToLine(interceptRobot->get()->getPos()) < 1.5 * control_constants::ROBOT_RADIUS) { + return {LineSegment(ballPosition, newBallPos).project(interceptRobot->get()->getPos()), interceptId}; } + // Projecting the interception location outside of the defense area if (field.leftDefenseArea.contains(newBallPos)) { + // If the new position of the ball is in the defense area, the keeper will intercept + if (interceptId == keeperId && + Trajectory2D(interceptRobot->get()->getPos(), interceptRobot->get()->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()).getTotalTime() < + loopTime) { + return {newBallPos, interceptId}; + } std::vector intersections = FieldComputations::getDefenseArea(field, true, 0, 0).intersections({newBallPos, world->getWorld()->getBall()->get()->expectedEndPosition}); if (intersections.size() == 1) newBallPos = intersections.at(0); @@ -275,46 +303,40 @@ HarasserInfo PositionComputations::calculateHarasserId(world::World *world, cons FieldComputations::getDefenseArea(field, false, 0, 0).intersections({newBallPos, world->getWorld()->getBall()->get()->expectedEndPosition}); if (intersections.size() == 1) newBallPos = intersections.at(0); } - + // If the robot with interceptId can get to the new ball pos in time, let it + if (interceptId != -1 && interceptId != keeperId && + Trajectory2D(interceptRobot->get()->getPos(), interceptRobot->get()->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()).getTotalTime() < + loopTime) { + return {newBallPos, interceptId}; + } + // Loop over all robots to determine who can intercept the ball quicker for (const auto &robot : world->getWorld()->getUs()) { if (robot->getId() == keeperId) continue; + // If they are already close to the line, project onto the line + if (LineSegment(newBallPos, ballPosition).distanceToLine(robot->getPos()) < 1.5 * control_constants::ROBOT_RADIUS) { + return {newBallPos, robot->getId()}; + } auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); if (trajectory.getTotalTime() < loopTime) { - return {robot->getId(), loopTime}; + return {newBallPos, robot->getId()}; + } + if (trajectory.getTotalTime() < minTimeToTarget) { + minTimeToTarget = trajectory.getTotalTime(); + interceptLocation = newBallPos; + minTimeRobotId = robot->getId(); } } } - double minTimeToTarget = std::numeric_limits::max(); - int minTimeRobotId; - for (const auto &robot : world->getWorld()->getUs()) { - if (robot->getId() == keeperId) continue; - auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), newBallPos, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - auto timeToTarget = trajectory.getTotalTime(); - if (timeToTarget < minTimeToTarget) { - minTimeToTarget = timeToTarget; - minTimeRobotId = robot->getId(); - } - } - return {minTimeRobotId, maximumTimeToIntercept}; + return {interceptLocation, minTimeRobotId}; } void PositionComputations::calculateInfoForHarasser(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, const Field &field, world::World *world, - double timeToBall) noexcept { - rtt::Vector2 ballPos = FieldComputations::getBallPositionAtTime(*(world->getWorld()->getBall()->get()), timeToBall); - if (field.leftDefenseArea.contains(ballPos)) { - std::vector intersections = - FieldComputations::getDefenseArea(field, true, 0, 0).intersections({ballPos, world->getWorld()->getBall()->get()->expectedEndPosition}); - if (intersections.size() == 1) ballPos = intersections.at(0); - } else if (field.rightDefenseArea.contains(ballPos)) { - std::vector intersections = - FieldComputations::getDefenseArea(field, false, 0, 0).intersections({ballPos, world->getWorld()->getBall()->get()->expectedEndPosition}); - if (intersections.size() == 1) ballPos = intersections.at(0); - } - auto enemyClosestToBall = world->getWorld()->getRobotClosestToPoint(ballPos, world::them); + Vector2 interceptionLocation) noexcept { + auto enemyClosestToBall = world->getWorld()->getRobotClosestToPoint(world->getWorld()->getBall()->get()->position, world::them); // If there is no enemy or we don't have a harasser yet, estimate the position to move to if (!stpInfos["harasser"].getRobot() || !enemyClosestToBall) { - stpInfos["harasser"].setPositionToMoveTo(ballPos); + stpInfos["harasser"].setPositionToMoveTo(interceptionLocation); return; } auto enemyAngle = enemyClosestToBall->get()->getAngle(); @@ -323,7 +345,9 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->hasBall() && enemyAngle.shortestAngleDiff(enemyToGoalAngle) > M_PI / 2) { auto enemyPos = enemyClosestToBall->get()->getPos(); - auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(control_constants::ROBOT_RADIUS * 4); + auto targetPos = + enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(control_constants::ROBOT_RADIUS * 4 + control_constants::GO_TO_POS_ERROR_MARGIN); + stpInfos["harasser"].setNotAvoidTheirRobotId(-1); stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setAngle((world->getWorld()->getBall()->get()->position - targetPos).angle()); } else { @@ -576,5 +600,4 @@ void PositionComputations::recalculateInfoForNonPassers(std::unordered_mapgetWorld().value(), -1, true) / 100; double goalAngle = FieldComputations::getTotalGoalAngle(field, false, point); + double distanceToGoal = point.dist(field.rightDefenseArea.rightLine().center()); + double minDistanceToGoal = field.rightDefenseArea.rightLine().center().dist(field.rightDefenseArea.leftLine().center()); + double maxDistanceToGoal = field.rightDefenseArea.rightLine().center().dist(field.playArea.topLeft()); + double normalizedDistanceToGoal = (distanceToGoal - minDistanceToGoal) / (maxDistanceToGoal - minDistanceToGoal); // The goal angle from right in front of their defense area- i.e. the "best" goal angle double maxGoalAngle = FieldComputations::getTotalGoalAngle(field, false, field.rightDefenseArea.leftLine().center()); - return (scores.scoreGoalShot = stp::evaluation::GoalShotEvaluation::metricCheck(visibility, goalAngle / maxGoalAngle)).value(); + return (scores.scoreGoalShot = stp::evaluation::GoalShotEvaluation::metricCheck(visibility, goalAngle / maxGoalAngle, normalizedDistanceToGoal)).value(); } } // namespace rtt::ai::stp \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp index bd91c9966..dce169564 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp @@ -8,6 +8,9 @@ namespace rtt::ai::stp::evaluation { uint8_t BallPlacementThemGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM || + GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) + ? stp::control_constants::FUZZY_TRUE + : stp::control_constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/position/GoalShotEvaluation.cpp b/roboteam_ai/src/stp/evaluations/position/GoalShotEvaluation.cpp index cb5d1c8a6..d52f945ee 100644 --- a/roboteam_ai/src/stp/evaluations/position/GoalShotEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/position/GoalShotEvaluation.cpp @@ -8,8 +8,8 @@ #include namespace rtt::ai::stp::evaluation { -uint8_t GoalShotEvaluation::metricCheck(double vis, double goalAngle) noexcept { - auto evalScore = std::pow(goalAngle, 1.0 / 2.0) * vis; +uint8_t GoalShotEvaluation::metricCheck(double vis, double goalAngle, double normalizedDistanceToGoal) noexcept { + auto evalScore = 0.5 * std::pow(goalAngle, 1.0 / 2.0) * vis + 0.5 * (1 - normalizedDistanceToGoal); return std::clamp(static_cast(evalScore * 255), 0, 255); } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp index db35da4ff..500902b1b 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp @@ -58,7 +58,7 @@ Dealer::FlagMap DefendPass::decideRoleFlags() const noexcept { Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); - flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.harasserId}}); + flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.interceptId}}); flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); @@ -74,8 +74,7 @@ Dealer::FlagMap DefendPass::decideRoleFlags() const noexcept { void DefendPass::calculateInfoForRoles() noexcept { harasserInfo = PositionComputations::calculateHarasserId(world, field); - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.timeToBall); + PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.interceptLocation); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); } diff --git a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp index 87edb44e1..7836b0f79 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp @@ -55,7 +55,7 @@ Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); - flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.harasserId}}); + flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.interceptId}}); flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"waller_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); @@ -71,8 +71,7 @@ Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { void DefendShot::calculateInfoForRoles() noexcept { harasserInfo = PositionComputations::calculateHarasserId(world, field); - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.timeToBall); + PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.interceptLocation); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); } diff --git a/roboteam_ai/src/stp/plays/offensive/Attack.cpp b/roboteam_ai/src/stp/plays/offensive/Attack.cpp index e20851462..e7a7a0a6e 100644 --- a/roboteam_ai/src/stp/plays/offensive/Attack.cpp +++ b/roboteam_ai/src/stp/plays/offensive/Attack.cpp @@ -73,7 +73,6 @@ Dealer::FlagMap Attack::decideRoleFlags() const noexcept { } void Attack::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp index 83fedf6fa..8eed6494a 100644 --- a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp @@ -76,7 +76,6 @@ Dealer::FlagMap AttackingPass::decideRoleFlags() const noexcept { } void AttackingPass::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp index 52a708960..bb99f8096 100644 --- a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp @@ -78,7 +78,6 @@ Dealer::FlagMap ChippingPass::decideRoleFlags() const noexcept { } void ChippingPass::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); diff --git a/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp b/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp index e0b13b129..75f580580 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/AggressiveStopFormation.cpp @@ -4,6 +4,7 @@ #include "stp/plays/referee_specific/AggressiveStopFormation.h" +#include "stp/roles/Keeper.h" #include "stp/roles/passive/Formation.h" namespace rtt::ai::stp::play { @@ -20,7 +21,7 @@ AggressiveStopFormation::AggressiveStopFormation() : Play() { /// Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("formation_back_0"), std::make_unique("formation_mid_0"), std::make_unique("formation_front_0"), @@ -59,10 +60,7 @@ Dealer::FlagMap AggressiveStopFormation::decideRoleFlags() const noexcept { return flagMap; } -void AggressiveStopFormation::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForFormation(stpInfos, roles, field, world); -} +void AggressiveStopFormation::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForFormation(stpInfos, roles, field, world); } const char* AggressiveStopFormation::getName() const { return "Aggressive Stop Formation"; } } // namespace rtt::ai::stp::play \ No newline at end of file diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp index 9dd5dad26..d6816c08f 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp @@ -22,7 +22,7 @@ BallPlacementThem::BallPlacementThem() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("harasser"), std::make_unique("waller_0"), std::make_unique("waller_1"), @@ -62,7 +62,6 @@ Dealer::FlagMap BallPlacementThem::decideRoleFlags() const noexcept { } void BallPlacementThem::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); calculateInfoForHarasser(); diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index f64f594ac..a140c20df 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -4,6 +4,7 @@ #include "stp/plays/referee_specific/BallPlacementUsForceStart.h" +#include "stp/roles/Keeper.h" #include "stp/roles/active/BallPlacer.h" #include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" @@ -23,7 +24,7 @@ BallPlacementUsForceStart::BallPlacementUsForceStart() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("ball_placer"), std::make_unique("defender_0"), std::make_unique("defender_1"), @@ -65,7 +66,6 @@ Dealer::FlagMap BallPlacementUsForceStart::decideRoleFlags() const noexcept { } void BallPlacementUsForceStart::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index 703f91b31..dd932d7ec 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -4,6 +4,7 @@ #include "stp/plays/referee_specific/BallPlacementUsFreeKick.h" +#include "stp/roles/Keeper.h" #include "stp/roles/active/BallPlacer.h" #include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" @@ -23,7 +24,7 @@ BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("ball_placer"), std::make_unique("attacker_0"), std::make_unique("defender_0"), @@ -65,7 +66,6 @@ Dealer::FlagMap BallPlacementUsFreeKick::decideRoleFlags() const noexcept { } void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); @@ -88,7 +88,6 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { for (auto& stpInfo : stpInfos) { stpInfo.second.setShouldAvoidDefenseArea(false); } - if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < control_constants::BALL_PLACEMENT_MARGIN) { for (auto& role : roles) if (role->getName() == "ball_placer") role->forceLastTactic(); diff --git a/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp b/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp index 4ae9e1c9b..a9bbd1d51 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/DefensiveStopFormation.cpp @@ -4,6 +4,7 @@ #include "stp/plays/referee_specific/DefensiveStopFormation.h" +#include "stp/roles/Keeper.h" #include "stp/roles/passive/Formation.h" namespace rtt::ai::stp::play { @@ -21,7 +22,7 @@ DefensiveStopFormation::DefensiveStopFormation() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("formation_back_0"), std::make_unique("formation_mid_0"), std::make_unique("formation_front_0"), @@ -60,10 +61,7 @@ Dealer::FlagMap DefensiveStopFormation::decideRoleFlags() const noexcept { return flagMap; } -void DefensiveStopFormation::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForFormation(stpInfos, roles, field, world); -} +void DefensiveStopFormation::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForFormation(stpInfos, roles, field, world); } const char* DefensiveStopFormation::getName() const { return "Defensive Stop Formation"; } } // namespace rtt::ai::stp::play \ No newline at end of file diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp index 5a363af7a..054955cf4 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp @@ -65,7 +65,6 @@ Dealer::FlagMap FreeKickThem::decideRoleFlags() const noexcept { } void FreeKickThem::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); calculateInfoForHarasser(); diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp index f411f5eb6..4e7399fc1 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp @@ -68,7 +68,6 @@ Dealer::FlagMap FreeKickUsAtGoal::decideRoleFlags() const noexcept { } void FreeKickUsAtGoal::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index b444795b9..111af1c06 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -69,7 +69,6 @@ Dealer::FlagMap FreeKickUsPass::decideRoleFlags() const noexcept { } void FreeKickUsPass::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.passLocation); diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp index 8df4b1330..a4e636636 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp @@ -61,7 +61,7 @@ Dealer::FlagMap KickOffThem::decideRoleFlags() const noexcept { return flagMap; } -void KickOffThem::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForKeeper(stpInfos, field, world); } +void KickOffThem::calculateInfoForRoles() noexcept {} bool KickOffThem::shouldEndPlay() noexcept { return false; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp index 4ed59fe7b..063461cf9 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp @@ -4,6 +4,7 @@ #include "stp/plays/referee_specific/KickOffThemPrepare.h" +#include "stp/roles/Keeper.h" #include "stp/roles/passive/Formation.h" namespace rtt::ai::stp::play { @@ -20,7 +21,7 @@ KickOffThemPrepare::KickOffThemPrepare() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("formation_back_0"), std::make_unique("formation_mid_0"), std::make_unique("formation_front_0"), @@ -59,10 +60,7 @@ Dealer::FlagMap KickOffThemPrepare::decideRoleFlags() const noexcept { return flagMap; } -void KickOffThemPrepare::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); -} +void KickOffThemPrepare::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); } const char* KickOffThemPrepare::getName() const { return "Kick Off Them Prepare"; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index e0ace4060..dbe41c5c6 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -61,7 +61,6 @@ Dealer::FlagMap KickOffUs::decideRoleFlags() const noexcept { } void KickOffUs::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); // Kicker Vector2 passLocation = Vector2(-field.playArea.width() / 5, 0); stpInfos["kick_off_taker"].setPositionToShootAt(passLocation); diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp index 5166f5378..9f45e4556 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp @@ -21,7 +21,7 @@ KickOffUsPrepare::KickOffUsPrepare() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("kicker"), std::make_unique("formation_mid_0"), std::make_unique("formation_front_0"), @@ -64,7 +64,6 @@ Dealer::FlagMap KickOffUsPrepare::decideRoleFlags() const noexcept { } void KickOffUsPrepare::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); // The "kicker" will go to the ball diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp index 6b72b1b75..be6df6f84 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp @@ -1,5 +1,6 @@ #include "stp/plays/referee_specific/PenaltyThemPrepare.h" +#include "stp/roles/Keeper.h" #include "stp/roles/passive/Formation.h" namespace rtt::ai::stp::play { @@ -19,7 +20,7 @@ PenaltyThemPrepare::PenaltyThemPrepare() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ // Roles is we play 6v6 - std::make_unique("keeper"), + std::make_unique("keeper"), std::make_unique("formation_0"), std::make_unique("formation_1"), std::make_unique("formation_2"), @@ -59,8 +60,6 @@ Dealer::FlagMap PenaltyThemPrepare::decideRoleFlags() const noexcept { } void PenaltyThemPrepare::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - // During their penalty, all our robots should be behind the ball to not interfere. // Create a grid pattern of robots on their side of the field int amountOfPassiveRobots = world->getWorld()->getUs().size() - 1; diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp index 48e3ac93c..a4cc18fe5 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp @@ -60,8 +60,6 @@ Dealer::FlagMap PenaltyUs::decideRoleFlags() const noexcept { } void PenaltyUs::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - auto positionTarget = PositionComputations::getPosition(std::nullopt, field.middleRightGrid, gen::GoalShot, field, world); stpInfos["kicker"].setPositionToMoveTo(positionTarget); auto goalTarget = computations::GoalComputations::calculateGoalTarget(world, field); diff --git a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp index f65983343..f61d83032 100644 --- a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp @@ -5,6 +5,8 @@ #include "control/ControlUtils.h" #include "roboteam_utils/LineSegment.h" +#include "stp/computations/PositionComputations.h" +#include "stp/computations/PositionScoring.h" #include "stp/constants/ControlConstants.h" #include "stp/skills/GoToPos.h" #include "utilities/Constants.h" @@ -27,11 +29,16 @@ KeeperBlockBall::KeeperBlockBall() { skills = rtt::collections::state_machine KeeperBlockBall::calculateInfoForSkill(StpInfo const &info) noexcept { StpInfo skillStpInfo = info; - if (!skillStpInfo.getField() || !skillStpInfo.getBall() || !skillStpInfo.getRobot()) return std::nullopt; + if (!skillStpInfo.getField() || !skillStpInfo.getBall() || !skillStpInfo.getRobot() || !skillStpInfo.getCurrentWorld()) return std::nullopt; skillStpInfo.setShouldAvoidOutOfField(false); - auto targetPosition = calculateTargetPosition(info.getBall().value(), info.getField().value(), info.getEnemyRobot()); + auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); + auto keeper = skillStpInfo.getRobot()->get(); + int keeperId = GameStateManager::getCurrentGameState().keeperId; + Vector2 newBallPos; + InterceptInfo interceptionInfo = PositionComputations::calculateInterceptionInfo(skillStpInfo.getField().value(), skillStpInfo.getCurrentWorld(), keeperId); + if (interceptionInfo.interceptId == keeperId) targetPosition.first = interceptionInfo.interceptLocation; skillStpInfo.setPositionToMoveTo(targetPosition.first); skillStpInfo.setPidType(targetPosition.second); diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index ac75df9f7..20d3d86e0 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -10,6 +10,8 @@ #include #include "control/ControlUtils.h" +#include "stp/computations/PositionComputations.h" +#include "stp/computations/PositionScoring.h" #include "stp/skills/GoToPos.h" #include "stp/skills/Rotate.h" #include "utilities/GameStateManager.hpp" @@ -21,46 +23,26 @@ GetBall::GetBall() { skills = collections::state_machine std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexcept { StpInfo skillStpInfo = info; - if (!skillStpInfo.getRobot() || !skillStpInfo.getBall() || !skillStpInfo.getField()) return std::nullopt; - + if (!skillStpInfo.getRobot() || !skillStpInfo.getBall() || !skillStpInfo.getField() || !skillStpInfo.getCurrentWorld()) return std::nullopt; + skillStpInfo.setShouldAvoidBall(false); Vector2 robotPosition = skillStpInfo.getRobot().value()->getPos(); Vector2 robotVelocity = skillStpInfo.getRobot().value()->getVel(); - Vector2 interceptionPosition; Vector2 ballPosition = skillStpInfo.getBall().value()->position; + Vector2 interceptionPosition = ballPosition; auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); + InterceptInfo interceptionInfo = + PositionComputations::calculateInterceptionInfo(skillStpInfo.getField().value(), skillStpInfo.getCurrentWorld(), skillStpInfo.getRobot().value()->getId()); + if (interceptionInfo.interceptId == skillStpInfo.getRobot().value()->getId()) interceptionPosition = interceptionInfo.interceptLocation; if (skillStpInfo.getRobot()->get()->hasBall()) { maxRobotVelocity = std::clamp(skillStpInfo.getBall().value()->velocity.length() * 0.8, 0.5, maxRobotVelocity); skillStpInfo.setMaxRobotVelocity(maxRobotVelocity); } - for (double loopTime = 0; loopTime < 1; loopTime += 0.1) { - interceptionPosition = FieldComputations::getBallPositionAtTime(*(skillStpInfo.getCurrentWorld()->getWorld()->getBall()->get()), loopTime); - if (skillStpInfo.getObjectsToAvoid().shouldAvoidOutOfField && !skillStpInfo.getField().value().playArea.contains(interceptionPosition, control_constants::BALL_RADIUS)) { - break; - } - if (skillStpInfo.getObjectsToAvoid().shouldAvoidDefenseArea) { - if (skillStpInfo.getField().value().leftDefenseArea.contains(interceptionPosition)) { - std::vector intersections = FieldComputations::getDefenseArea(skillStpInfo.getField().value(), true, 0, 0) - .intersections({interceptionPosition, skillStpInfo.getBall().value()->expectedEndPosition}); - if (intersections.size() == 1) interceptionPosition = intersections.at(0); - } else if (skillStpInfo.getField().value().rightDefenseArea.contains(interceptionPosition)) { - std::vector intersections = FieldComputations::getDefenseArea(skillStpInfo.getField().value(), false, 0, 0) - .intersections({interceptionPosition, skillStpInfo.getBall().value()->expectedEndPosition}); - if (intersections.size() == 1) interceptionPosition = intersections.at(0); - } - } - - auto trajectory = Trajectory2D(robotPosition, robotVelocity, interceptionPosition, maxRobotVelocity, ai::Constants::MAX_ACC_UPPER()); - if (trajectory.getTotalTime() < loopTime) { - break; - } - } // distance to the ball at the time we intercept it double distanceToInterception = (interceptionPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN + 2 * control_constants::BALL_RADIUS; // distance to the ball right now double distanceToBall = (ballPosition - robotPosition).length() - control_constants::BALL_RADIUS - control_constants::ROBOT_RADIUS + control_constants::GO_TO_POS_ERROR_MARGIN + 2 * control_constants::BALL_RADIUS; - if (skillStpInfo.getRobot()->get()->getAngleDiffToBall() > Constants::HAS_BALL_ANGLE() && distanceToBall < control_constants::ROBOT_CLOSE_TO_POINT) { // don't move too close to the ball until the angle to the ball is (roughly) correct skillStpInfo.setPositionToMoveTo( @@ -72,7 +54,6 @@ std::optional GetBall::calculateInfoForSkill(StpInfo const &info) noexc newRobotPosition = FieldComputations::projectPointToValidPosition(info.getField().value(), newRobotPosition, info.getObjectsToAvoid()); skillStpInfo.setPositionToMoveTo(newRobotPosition); } - skillStpInfo.setAngle((ballPosition - robotPosition).angle()); if (distanceToBall < control_constants::TURN_ON_DRIBBLER_DISTANCE) { diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index ead765b47..6c91667aa 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -13,6 +13,7 @@ #include #include +#include "control/positionControl/BBTrajectories/Trajectory2D.h" #include "interface/api/Output.h" #include "utilities/GameStateManager.hpp" #include "world/FieldComputations.h" @@ -244,6 +245,7 @@ double Dealer::getRobotScoreForRole(const std::vector &deale // Get the distance score for a robot to a position when there is a position that role needs to go to double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::RobotView &robot) { double distance; + if (stpInfo.getRoleName().find("halt") == std::string::npos && stpInfo.getRoleName() == "keeper") return 0; std::optional target_position; // Search for position in getEnemyRobot, getPositionToDefend, and getPositionToMoveTo @@ -251,20 +253,14 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro if (stpInfo.getPositionToDefend().has_value()) target_position = stpInfo.getPositionToDefend().value(); if (stpInfo.getPositionToShootAt().has_value()) target_position = world.getBall()->get()->position; if (stpInfo.getPositionToMoveTo().has_value()) target_position = stpInfo.getPositionToMoveTo().value(); - // If robot is keeper, set distance to self. Basically 0 - // TODO: Is this if ever true? This is already handeled before right? - if (stpInfo.getRoleName() == "keeper" && robot->getId() == GameStateManager::getCurrentGameState().keeperId) target_position = robot->getPos(); // No target found to move to if (!target_position) { - // only print warning if halt not in rolename - if (stpInfo.getRoleName().find("halt") == std::string::npos) - RTT_WARNING("No target position found for role " + stpInfo.getRoleName() + " for robot " + std::to_string(robot->getId())) + RTT_WARNING("No target position found for role " + stpInfo.getRoleName() + " for robot " + std::to_string(robot->getId())) return 0; } - distance = robot->getPos().dist(*target_position); - return costForDistance(distance, field->playArea.height()); + return costForDistance(robot, *target_position, stpInfo.getMaxRobotVelocity()); } double Dealer::getDefaultFlagScores(const v::RobotView &robot, const Dealer::DealerFlag &flag) { @@ -298,9 +294,8 @@ void Dealer::setGameStateRoleIds(std::unordered_map o } // Calculate the cost for distance. The further away the target, the higher the cost for that distance. -double Dealer::costForDistance(double distance, double fieldHeight) { - auto fieldDiagonalLength = sqrt(pow(fieldHeight, 2.0) + pow(fieldHeight, 2.0)); - return distance / fieldDiagonalLength; +double Dealer::costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity) { + return Trajectory2D(robot->getPos(), robot->getVel(), target_position, MaxRobotVelocity, ai::Constants::MAX_ACC_UPPER()).getTotalTime(); } double Dealer::costForProperty(bool property) { return property ? 0.0 : 1.0; } diff --git a/roboteam_ai/src/utilities/StrategyManager.cpp b/roboteam_ai/src/utilities/StrategyManager.cpp index 958407ed2..16d31f3f2 100644 --- a/roboteam_ai/src/utilities/StrategyManager.cpp +++ b/roboteam_ai/src/utilities/StrategyManager.cpp @@ -12,7 +12,9 @@ void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCom nextCommand == RefCommand::PREPARE_PENALTY_THEM || nextCommand == RefCommand::PREPARE_PENALTY_US)) { command = nextCommand; } - + if (command != RefCommand::BALL_PLACEMENT_US && nextCommand == RefCommand::FORCED_START) { + command = RefCommand::PREPARE_FORCED_START; + } if (command == RefCommand::FORCED_START) { command = RefCommand::NORMAL_START; } @@ -20,7 +22,7 @@ void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCom if (command == RefCommand::BALL_PLACEMENT_US && nextCommand == RefCommand::DIRECT_FREE_US) { command = RefCommand::BALL_PLACEMENT_US_DIRECT; } - if (command == RefCommand::STOP && (nextCommand == RefCommand::FORCED_START || nextCommand == RefCommand::DIRECT_FREE_THEM)) { + if (command == RefCommand::STOP && nextCommand == RefCommand::DIRECT_FREE_THEM) { command = RefCommand::DIRECT_FREE_THEM_STOP; } diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index f1a392927..fb8ae469d 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -44,16 +44,10 @@ void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { } void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { - auto previousWorld = data->getHistoryWorld(1); - if (!previousWorld || !previousWorld->getBall()) { - return; - } - - auto ball = previousWorld->getBall().value(); - const double ballVelSquared = ball->velocity.length2(); + const double ballVelSquared = velocity.length2(); const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::stp::control_constants::SIMULATION_FRICTION : ai::stp::control_constants::REAL_FRICTION; - expectedEndPosition = ball->position + ball->velocity.stretchToLength(ballVelSquared / frictionCoefficient); + expectedEndPosition = position + velocity.stretchToLength(ballVelSquared / frictionCoefficient); // Uncomment the following lines to calculate the friction coefficient // auto currentTime = std::chrono::steady_clock::now(); @@ -69,7 +63,7 @@ void Ball::updateExpectedBallEndPosition(const world::World* data) noexcept { // std::cout << "Expected friction coefficient: " << maxVelocity.length2() / (posAtMaxVelocity - ball->position).length() << std::endl; // std::cout << "Time till next reset: " << 30.0 - std::chrono::duration(currentTime - timeOfLastReset).count() << std::endl; - std::array ballPoints = {expectedEndPosition, ball->position}; + std::array ballPoints = {expectedEndPosition, position}; rtt::ai::gui::Out::draw( { .label = "expected_end_position_ball", diff --git a/roboteam_ai/src/world/FieldComputations.cpp b/roboteam_ai/src/world/FieldComputations.cpp index d2be13ec2..4d170cc83 100644 --- a/roboteam_ai/src/world/FieldComputations.cpp +++ b/roboteam_ai/src/world/FieldComputations.cpp @@ -10,7 +10,7 @@ namespace rtt::ai { std::tuple FieldComputations::getDefenseAreaMargin() { double theirDefenseAreaMargin = stp::control_constants::ROBOT_RADIUS + stp::control_constants::GO_TO_POS_ERROR_MARGIN; - double ourDefenseAreaMargin = stp::control_constants::ROBOT_RADIUS + stp::control_constants::GO_TO_POS_ERROR_MARGIN; + double ourDefenseAreaMargin = -stp::control_constants::ROBOT_RADIUS + stp::control_constants::GO_TO_POS_ERROR_MARGIN; RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); diff --git a/roboteam_ai/test/WorldTests/FieldComputationTest.cpp b/roboteam_ai/test/WorldTests/FieldComputationTest.cpp index e1bf714ac..c2b2f5dab 100644 --- a/roboteam_ai/test/WorldTests/FieldComputationTest.cpp +++ b/roboteam_ai/test/WorldTests/FieldComputationTest.cpp @@ -191,7 +191,8 @@ TEST(FieldComputationTest, projectionTests) { EXPECT_TRUE(field.leftDefenseArea.contains(pointInDefenseArea)); auto projectedPoint = FieldComputations::projectPointOutOfDefenseArea(field, pointInDefenseArea); - EXPECT_FALSE(field.leftDefenseArea.contains(projectedPoint)); + // Since the projectedPoint can be inside the defense area with ROBOT_RADIUS, we use that as margin + EXPECT_FALSE(field.leftDefenseArea.contains(projectedPoint, -stp::control_constants::ROBOT_RADIUS)); auto pointOutsideField = Vector2(field.playArea.left() - 0.05, field.playArea.top() + 0.05); EXPECT_FALSE(field.playArea.contains(pointOutsideField)); diff --git a/roboteam_ai/test/WorldTests/WhichRobotHasBallTest.cpp b/roboteam_ai/test/WorldTests/WhichRobotHasBallTest.cpp index 2afe2401c..928a28fc8 100644 --- a/roboteam_ai/test/WorldTests/WhichRobotHasBallTest.cpp +++ b/roboteam_ai/test/WorldTests/WhichRobotHasBallTest.cpp @@ -16,67 +16,71 @@ TEST(worldTest, WhichRobotHasBallTest) { rtt::GameSettings::setPrimaryAI(true); rtt::GameSettings::setYellow(true); - // TODO: Extend test to also work in SIM environment making use of dribbler encoder and / or dedicated ballsensor - // Base our test on vision information (instead of ballsensor/dribbler information) - rtt::GameSettings::setRobotHubMode(rtt::net::RobotHubMode::BASESTATION); - - auto const& [_, worldInstance] = rtt::world::World::instance(); - - google::protobuf::RepeatedPtrField robotsYellow; - google::protobuf::RepeatedPtrField robotsBlue; - - // Create robots yellow - proto::WorldRobot roboty1; - roboty1.set_id(1); - roboty1.mutable_pos()->set_x(0.1); - roboty1.set_angle(M_PI); - - proto::WorldRobot roboty2; - roboty2.set_id(2); - roboty2.mutable_pos()->set_x(0.5); - roboty2.set_angle(M_PI); - - // Create robots blue - proto::WorldRobot robotb1; - robotb1.set_id(3); - robotb1.mutable_pos()->set_x(0.05); - robotb1.set_angle(M_PI); - - /* Setup : Ball ..0.05.. robotb1(3) ..0.1.. roboty1(1) ..0.5.. roboty2(2) */ - - // Create ball - proto::WorldBall ball; - ball.mutable_pos()->set_x(0.001); - ball.mutable_pos()->set_y(0); - ball.set_visible(true); - - // Add robots - robotsYellow.Add()->CopyFrom(roboty1); - robotsYellow.Add()->CopyFrom(roboty2); - robotsBlue.Add()->CopyFrom(robotb1); - - /** Test 1 : Two yellow robots (ids 1 & 2), one blue robots (id = 3) and one ball **/ - proto::World msgBall; - msgBall.mutable_yellow()->CopyFrom(robotsYellow); - msgBall.mutable_blue()->CopyFrom(robotsBlue); - msgBall.mutable_ball()->CopyFrom(ball); - worldInstance->updateWorld(msgBall); - auto world = worldInstance->getWorld(); - assert(world->getUs().size() == 2); - assert(world->getThem().size() == 1); - assert(world->getBall().has_value()); - - std::optional robot; - - /** Test 1.1 : us */ - robot = world->whichRobotHasBall(rtt::world::Team::us); - EXPECT_EQ((*robot)->getId(), 1); - /** Test 1.2 : them */ - robot = world->whichRobotHasBall(rtt::world::Team::them); - EXPECT_EQ((*robot)->getId(), 3); - /** Test 1.3 : both */ - robot = world->whichRobotHasBall(rtt::world::Team::both); - EXPECT_EQ((*robot)->getId(), 3); + std::vector modes = {rtt::net::RobotHubMode::BASESTATION, rtt::net::RobotHubMode::SIMULATOR}; + for (const auto& mode : modes) { + rtt::GameSettings::setRobotHubMode(mode); + + auto const& [_, worldInstance] = rtt::world::World::instance(); + + google::protobuf::RepeatedPtrField robotsYellow; + google::protobuf::RepeatedPtrField robotsBlue; + + // Create robots yellow + proto::WorldRobot roboty1; + roboty1.set_id(1); + roboty1.mutable_pos()->set_x(0.1); + roboty1.mutable_feedbackinfo()->set_dribbler_sees_ball(true); + roboty1.mutable_feedbackinfo()->set_ball_sensor_sees_ball(true); + + roboty1.set_angle(M_PI); + + proto::WorldRobot roboty2; + roboty2.set_id(2); + roboty2.mutable_pos()->set_x(0.5); + roboty2.set_angle(M_PI); + + // Create robots blue + proto::WorldRobot robotb1; + robotb1.set_id(3); + robotb1.mutable_pos()->set_x(0.05); + robotb1.set_angle(M_PI); + + /* Setup : Ball ..0.05.. robotb1(3) ..0.1.. roboty1(1) ..0.5.. roboty2(2) */ + + // Create ball + proto::WorldBall ball; + ball.mutable_pos()->set_x(0.001); + ball.mutable_pos()->set_y(0); + ball.set_visible(true); + + // Add robots + robotsYellow.Add()->CopyFrom(roboty1); + robotsYellow.Add()->CopyFrom(roboty2); + robotsBlue.Add()->CopyFrom(robotb1); + + /** Test 1 : Two yellow robots (ids 1 & 2), one blue robots (id = 3) and one ball **/ + proto::World msgBall; + msgBall.mutable_yellow()->CopyFrom(robotsYellow); + msgBall.mutable_blue()->CopyFrom(robotsBlue); + msgBall.mutable_ball()->CopyFrom(ball); + worldInstance->updateWorld(msgBall); + auto world = worldInstance->getWorld(); + assert(world->getUs().size() == 2); + assert(world->getThem().size() == 1); + assert(world->getBall().has_value()); + + std::optional robot; + + /** Test 1.1 : us */ + robot = world->whichRobotHasBall(rtt::world::Team::us); + EXPECT_EQ((*robot)->getId(), 1); + /** Test 1.2 : them */ + robot = world->whichRobotHasBall(rtt::world::Team::them); + EXPECT_EQ((*robot)->getId(), 3); + /** Test 1.3 : both, 1 and 3 return true on has ball. 3 is closer to the ball, however we trust our own dribbler */ + robot = world->whichRobotHasBall(rtt::world::Team::both); + EXPECT_EQ((*robot)->getId(), 1); + } } TEST(worldTest, NoRobotHasBall) { @@ -93,19 +97,22 @@ TEST(worldTest, NoRobotHasBall) { // Create robots yellow proto::WorldRobot roboty1; - roboty1.set_id(1); + roboty1.set_id(4); roboty1.mutable_pos()->set_x(1); + roboty1.mutable_pos()->set_y(0); roboty1.set_angle(M_PI); proto::WorldRobot roboty2; - roboty2.set_id(2); + roboty2.set_id(5); roboty2.mutable_pos()->set_x(2); + roboty2.mutable_pos()->set_y(0); roboty2.set_angle(M_PI); // Create robots blue proto::WorldRobot robotb1; - robotb1.set_id(3); + robotb1.set_id(6); robotb1.mutable_pos()->set_x(3); + robotb1.mutable_pos()->set_y(0); robotb1.set_angle(M_PI); /* Setup : Ball ..1.. robotb1(3) ..2.. roboty1(1) ..3.. roboty2(2) */ @@ -121,7 +128,7 @@ TEST(worldTest, NoRobotHasBall) { robotsYellow.Add()->CopyFrom(roboty2); robotsBlue.Add()->CopyFrom(robotb1); - /** Test 1 : Two yellow robots (ids 1 & 2), one blue robots (id = 3) and one ball **/ + /** Test 1 : Two yellow robots (ids 1 & 2), one blue robots (id 3) and one ball **/ proto::World msgBall; msgBall.mutable_yellow()->CopyFrom(robotsYellow); msgBall.mutable_blue()->CopyFrom(robotsBlue); diff --git a/roboteam_networking/CMakeLists.txt b/roboteam_networking/CMakeLists.txt index fa9121f69..46ce407da 100644 --- a/roboteam_networking/CMakeLists.txt +++ b/roboteam_networking/CMakeLists.txt @@ -55,3 +55,5 @@ add_executable(roboteam_networking_syscheck target_link_libraries(roboteam_networking_syscheck PRIVATE roboteam_networking) +gtest_discover_tests(roboteam_networking_syscheck) + diff --git a/roboteam_networking/system_tests/SendRobotCommands.cpp b/roboteam_networking/system_tests/SendRobotCommands.cpp index a3002ef33..82e64bef8 100644 --- a/roboteam_networking/system_tests/SendRobotCommands.cpp +++ b/roboteam_networking/system_tests/SendRobotCommands.cpp @@ -67,10 +67,14 @@ int main() { std::cout << "Starting to send commands: " << std::endl << std::endl; auto command = getEmptyRobotCommandToAllRobots(); - while (true) { + int count = 0; + // We send 10 commands to all robots + // This number is chosen randomly, just needs to be finite and not extremely large + while (count < 10) { commandsYellowPub->publish(command); commandsBluePub->publish(command); std::cout << "Sent robot command..." << std::endl; - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::milliseconds(16)); + count++; } } \ No newline at end of file