From 1cb6f0d315d395ee5886c2284fd201523aaec0a9 Mon Sep 17 00:00:00 2001 From: Luuk Date: Wed, 31 Jan 2024 12:08:02 +0100 Subject: [PATCH 01/10] Replace BallPlacementUs with BallPlacementUsForceStart and BallPlacementUsFreeKick depending on the next ref command --- roboteam_ai/CMakeLists.txt | 3 +- .../BallPlacementUsForceStart.h | 45 +++++++++ ...lacementUs.h => BallPlacementUsFreeKick.h} | 12 +-- roboteam_ai/src/STPManager.cpp | 6 +- ...ntUs.cpp => BallPlacementUsForceStart.cpp} | 54 +++++----- .../BallPlacementUsFreeKick.cpp | 99 +++++++++++++++++++ .../src/utilities/GameStateManager.cpp | 3 +- roboteam_ai/test/TestFixtures/TestFixture.h | 7 +- 8 files changed, 195 insertions(+), 34 deletions(-) create mode 100644 roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsForceStart.h rename roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/{BallPlacementUs.h => BallPlacementUsFreeKick.h} (72%) rename roboteam_ai/src/stp/plays/referee_specific/{BallPlacementUs.cpp => BallPlacementUsForceStart.cpp} (58%) create mode 100644 roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp diff --git a/roboteam_ai/CMakeLists.txt b/roboteam_ai/CMakeLists.txt index 925303a99..6e0731f90 100644 --- a/roboteam_ai/CMakeLists.txt +++ b/roboteam_ai/CMakeLists.txt @@ -119,7 +119,8 @@ add_library(roboteam_ai_plays # referee_specific/ ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/AggressiveStopFormation.cpp ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementThem.cpp - ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementUs.cpp + ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp + ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/DefensiveStopFormation.cpp ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/FreeKickThem.cpp ${PROJECT_SOURCE_DIR}/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsForceStart.h b/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsForceStart.h new file mode 100644 index 000000000..2281d9714 --- /dev/null +++ b/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsForceStart.h @@ -0,0 +1,45 @@ +#ifndef RTT_BallPlacementUsForceStart_H +#define RTT_BallPlacementUsForceStart_H + +#include "stp/Play.hpp" + +namespace rtt::ai::stp::play { + +/** + * @brief The ball placement us Force Start play is executed when the ball placement us game state is selected and the next ref command is not Free kick us, meaning it will be a + * force start + */ +class BallPlacementUsForceStart : public Play { + public: + /** + * @brief Constructor that initializes roles with roles that are necessary for this play + */ + BallPlacementUsForceStart(); + + /** + * @brief Calculates the score of this play to determine which play is best in this situation + * @param field The current Field + * @return The score of this play (0-255) + */ + uint8_t score(const rtt::Field& field) noexcept override; + + /** + * @brief Assigns robots to roles of this play + * @return A map with assigned roles + */ + Dealer::FlagMap decideRoleFlags() const noexcept override; + + /** + * @brief Calculates info for the roles + */ + void calculateInfoForRoles() noexcept override; + + /** + * @brief Retrieves the name of the play + * @return The name of the play as a string + */ + const char* getName() const override; +}; +} // namespace rtt::ai::stp::play + +#endif // RTT_BallPlacementUsForceStart_H \ No newline at end of file diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUs.h b/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsFreeKick.h similarity index 72% rename from roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUs.h rename to roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsFreeKick.h index 88c6f03f5..0420140e7 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUs.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/referee_specific/BallPlacementUsFreeKick.h @@ -1,19 +1,19 @@ -#ifndef RTT_BALLPLACEMENTUS_H -#define RTT_BALLPLACEMENTUS_H +#ifndef RTT_BallPlacementUsFreeKick_H +#define RTT_BallPlacementUsFreeKick_H #include "stp/Play.hpp" namespace rtt::ai::stp::play { /** - * @brief The ball placement us play is executed when the ball placement us game state is selected + * @brief The ball placement us Free kick play is executed when the ball placement us game state is selected and the next ref command is Free Kick Us */ -class BallPlacementUs : public Play { +class BallPlacementUsFreeKick : public Play { public: /** * @brief Constructor that initializes roles with roles that are necessary for this play */ - BallPlacementUs(); + BallPlacementUsFreeKick(); /** * @brief Calculates the score of this play to determine which play is best in this situation @@ -41,4 +41,4 @@ class BallPlacementUs : public Play { }; } // namespace rtt::ai::stp::play -#endif // RTT_BALLPLACEMENTUS_H \ No newline at end of file +#endif // RTT_BallPlacementUsFreeKick_H \ No newline at end of file diff --git a/roboteam_ai/src/STPManager.cpp b/roboteam_ai/src/STPManager.cpp index d3a7024db..a7ca2f6c5 100644 --- a/roboteam_ai/src/STPManager.cpp +++ b/roboteam_ai/src/STPManager.cpp @@ -26,7 +26,8 @@ #include "stp/plays/offensive/ChippingPass.h" #include "stp/plays/referee_specific/AggressiveStopFormation.h" #include "stp/plays/referee_specific/BallPlacementThem.h" -#include "stp/plays/referee_specific/BallPlacementUs.h" +#include "stp/plays/referee_specific/BallPlacementUsForceStart.h" +#include "stp/plays/referee_specific/BallPlacementUsFreeKick.h" #include "stp/plays/referee_specific/DefensiveStopFormation.h" #include "stp/plays/referee_specific/FreeKickThem.h" #include "stp/plays/referee_specific/FreeKickUsAtGoal.h" @@ -60,7 +61,8 @@ const STPManager::PlaysVec STPManager::plays = ([] { plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp similarity index 58% rename from roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp rename to roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index ce1999dd6..dcb68fd98 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -1,16 +1,17 @@ // -// Created by jessevw on 24.03.20. +// Created by Luuk on 31/1/2024. // -#include "stp/plays/referee_specific/BallPlacementUs.h" +#include "stp/plays/referee_specific/BallPlacementUsForceStart.h" #include "stp/roles/active/BallPlacer.h" +#include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" #include "utilities/GameStateManager.hpp" namespace rtt::ai::stp::play { -BallPlacementUs::BallPlacementUs() : Play() { +BallPlacementUsForceStart::BallPlacementUsForceStart() : Play() { // Evaluations that have to be true in order for this play to be considered valid. startPlayEvaluation.clear(); startPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsGameState); @@ -22,19 +23,27 @@ BallPlacementUs::BallPlacementUs() : 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("ball_placer"), std::make_unique("formation_back_0"), - std::make_unique("formation_mid_0"), std::make_unique("formation_front_0"), std::make_unique("formation_mid_1"), + std::make_unique("keeper"), + std::make_unique("ball_placer"), + std::make_unique("defender_0"), + std::make_unique("defender_1"), + std::make_unique("waller_0"), + std::make_unique("defender_2"), // Additional roles if we play 11v11 - std::make_unique("formation_back_1"), std::make_unique("formation_front_1"), std::make_unique("formation_back_2"), - std::make_unique("formation_mid_2"), std::make_unique("formation_front_2")}; + std::make_unique("defender_3"), + std::make_unique("attacker_0"), + std::make_unique("waller_1"), + std::make_unique("defender_4"), + std::make_unique("defender_5"), + }; } -uint8_t BallPlacementUs::score(const rtt::Field&) noexcept { +uint8_t BallPlacementUsForceStart::score(const rtt::Field& field) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() != RefCommand::BALL_PLACEMENT_US_DIRECT); } -Dealer::FlagMap BallPlacementUs::decideRoleFlags() const noexcept { +Dealer::FlagMap BallPlacementUsForceStart::decideRoleFlags() const noexcept { Dealer::FlagMap flagMap; Dealer::DealerFlag detectionFlag(DealerFlagTitle::CAN_DETECT_BALL); Dealer::DealerFlag dribblerFlag(DealerFlagTitle::WITH_WORKING_DRIBBLER); @@ -42,22 +51,23 @@ Dealer::FlagMap BallPlacementUs::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"ball_placer", {DealerFlagPriority::REQUIRED, {dribblerFlag, detectionFlag}}}); - flagMap.insert({"formation_back_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_back_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_back_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_front_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_front_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_front_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"waller_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"waller_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_3", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_4", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_5", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"attacker_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); return flagMap; } -void BallPlacementUs::calculateInfoForRoles() noexcept { +void BallPlacementUsForceStart::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForKeeper(stpInfos, field, world); - PositionComputations::calculateInfoForFormation(stpInfos, roles, field, world); + PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); + PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); Vector2 ballTarget = Vector2(); @@ -85,5 +95,5 @@ void BallPlacementUs::calculateInfoForRoles() noexcept { } } -const char* BallPlacementUs::getName() const { return "Ball Placement Us"; } +const char* BallPlacementUsForceStart::getName() const { return "Ball Placement Us Force Start"; } } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp new file mode 100644 index 000000000..31ab330de --- /dev/null +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -0,0 +1,99 @@ +// +// Created by Luuk on 31/1/2024. +// + +#include "stp/plays/referee_specific/BallPlacementUsFreeKick.h" + +#include "stp/roles/active/BallPlacer.h" +#include "stp/roles/passive/Defender.h" +#include "stp/roles/passive/Formation.h" +#include "utilities/GameStateManager.hpp" + +namespace rtt::ai::stp::play { + +BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { + // Evaluations that have to be true in order for this play to be considered valid. + startPlayEvaluation.clear(); + startPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsGameState); + + // Evaluations that have to be true to allow the play to continue, otherwise the play will change. Plays can also end using the shouldEndPlay(). + keepPlayEvaluation.clear(); + keepPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsGameState); + + // 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("ball_placer"), + std::make_unique("attacker_0"), + std::make_unique("defender_0"), + std::make_unique("defender_1"), + std::make_unique("attacker_1"), + // Additional roles if we play 11v11 + std::make_unique("waller_0"), + std::make_unique("waller_1"), + std::make_unique("attacker_2"), + std::make_unique("defender_2"), + std::make_unique("attacker_3"), + }; +} + +uint8_t BallPlacementUsFreeKick::score(const rtt::Field& field) noexcept { + // If this play is valid we always want to execute this play + return (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US_DIRECT); +} + +Dealer::FlagMap BallPlacementUsFreeKick::decideRoleFlags() const noexcept { + Dealer::FlagMap flagMap; + Dealer::DealerFlag detectionFlag(DealerFlagTitle::CAN_DETECT_BALL); + Dealer::DealerFlag dribblerFlag(DealerFlagTitle::WITH_WORKING_DRIBBLER); + Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); + + flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); + flagMap.insert({"ball_placer", {DealerFlagPriority::REQUIRED, {dribblerFlag, detectionFlag}}}); + flagMap.insert({"waller_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"waller_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"attacker_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"attacker_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"attacker_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"attacker_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + + return flagMap; +} + +void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { + PositionComputations::calculateInfoForKeeper(stpInfos, field, world); + PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); + PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); + + Vector2 ballTarget = Vector2(); + + // Adjust placement position to be one robot radius away in the distance of movement + if (stpInfos["ball_placer"].getRobot()) { + ballTarget = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); + ballTarget -= (world->getWorld()->get()->getBall()->get()->position - stpInfos["ball_placer"].getRobot()->get()->getPos()).stretchToLength(control_constants::ROBOT_RADIUS); + } else { + // If we don't have a ball placer, set the target location to the ball, such that the dealer will + // assign the robot closest to the ball to the ball placer role + ballTarget = world->getWorld()->get()->getBall()->get()->position; + } + + // stpInfos["ball_placer"].setPositionToShootAt(ballTarget); + stpInfos["ball_placer"].setPositionToMoveTo(ballTarget); + stpInfos["ball_placer"].setShouldAvoidOutOfField(false); + stpInfos["ball_placer"].setShouldAvoidBall(false); + for (auto& stpInfo : stpInfos) { + stpInfo.second.setShouldAvoidDefenseArea(false); + } + + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < 1.0) stpInfos["ball_placer"].setMaxRobotVelocity(0.75); + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < control_constants::TURN_ON_DRIBBLER_DISTANCE) { + stpInfos["ball_placer"].setDribblerSpeed(100); + } +} + +const char* BallPlacementUsFreeKick::getName() const { return "Ball Placement Us Free Kick"; } +} // namespace rtt::ai::stp::play diff --git a/roboteam_ai/src/utilities/GameStateManager.cpp b/roboteam_ai/src/utilities/GameStateManager.cpp index a49e8c733..dd3523a1e 100644 --- a/roboteam_ai/src/utilities/GameStateManager.cpp +++ b/roboteam_ai/src/utilities/GameStateManager.cpp @@ -105,7 +105,8 @@ void GameStateManager::updateInterfaceGameState(const char* name) { static const std::map> nameToGameState = { {"Aggressive Stop Formation", {RefCommand::STOP, Constants::RULESET_STOP()}}, {"Defensive Stop Formation", {RefCommand::STOP, Constants::RULESET_STOP()}}, - {"Ball Placement Us", {RefCommand::BALL_PLACEMENT_US, Constants::RULESET_STOP()}}, + {"Ball Placement Us Free Kick", {RefCommand::BALL_PLACEMENT_US_DIRECT, Constants::RULESET_STOP()}}, + {"Ball Placement Us Force Start", {RefCommand::BALL_PLACEMENT_US, Constants::RULESET_STOP()}}, {"Ball Placement Them", {RefCommand::BALL_PLACEMENT_THEM, Constants::RULESET_STOP()}}, {"Halt", {RefCommand::HALT, Constants::RULESET_HALT()}}, {"Free Kick Them", {RefCommand::DIRECT_FREE_THEM, Constants::RULESET_DEFAULT()}}, diff --git a/roboteam_ai/test/TestFixtures/TestFixture.h b/roboteam_ai/test/TestFixtures/TestFixture.h index 3ec9bf46d..3e1e7d0e1 100644 --- a/roboteam_ai/test/TestFixtures/TestFixture.h +++ b/roboteam_ai/test/TestFixtures/TestFixture.h @@ -30,7 +30,8 @@ #include "stp/plays/offensive/AttackingPass.h" #include "stp/plays/referee_specific/AggressiveStopFormation.h" #include "stp/plays/referee_specific/BallPlacementThem.h" -#include "stp/plays/referee_specific/BallPlacementUs.h" +#include "stp/plays/referee_specific/BallPlacementUsForceStart.h" +#include "stp/plays/referee_specific/BallPlacementUsFreeKick.h" #include "stp/plays/referee_specific/DefensiveStopFormation.h" #include "stp/plays/referee_specific/FreeKickThem.h" #include "stp/plays/referee_specific/Halt.h" @@ -103,7 +104,9 @@ class RTT_AI_Tests : public ::testing::Test { plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); + // plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); From efa69464f3a986501419f7c7165105b8491c44be Mon Sep 17 00:00:00 2001 From: Jorn Date: Sat, 3 Feb 2024 15:26:23 +0100 Subject: [PATCH 02/10] Fix release build, split for loop that should be splitted --- roboteam_ai/src/stp/Play.cpp | 4 ++-- .../src/stp/computations/PositionComputations.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index b02649fd2..7a500d258 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -63,7 +63,7 @@ void Play::update() noexcept { // If we have more robots than allowed, one drives to the edge of the field if (currentMaxRobots < sizeUs) { stpInfos[roles[currentMaxRobots]->getName()].setShouldAvoidBall(true); - stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0, -field.playArea.width() / 2)); + stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0.0, -field.playArea.width() / 2)); } // Loop through roles and update them if they are assigned to a robot @@ -135,7 +135,7 @@ void Play::distributeRoles() noexcept { flagMap[roles[currentMaxRobots]->getName()].priority = DealerFlagPriority::CARD; flagMap[roles[currentMaxRobots]->getName()].forcedID = cardId; stpInfos[roles[currentMaxRobots]->getName()].setShouldAvoidBall(true); - stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0, -field.playArea.width() / 2)); + stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(Vector2(0.0, -field.playArea.width() / 2)); } // Only keep the first n roles, where n is the amount of robots we have // This order is based on the order of the roles array diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index c70456713..2c99bae20 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -312,17 +312,17 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma std::vector> cost_matrix; cost_matrix.resize(activeDefenderNames.size()); int row_length = std::min(activeDefenderNames.size(), enemyMap.size()); + for (size_t i = 0; i < row_length; i++) { + enemies.emplace_back((mustStayOnOurSide && (enemyMap.begin()->second.position.x > 0.0)) ? Vector2{0.0, enemyMap.begin()->second.position.y} + : enemyMap.begin()->second.position); + ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); + enemyMap.erase(enemyMap.begin()); + } for (size_t i = 0; i < activeDefenderNames.size(); i++) { cost_matrix[i].resize(row_length); - // Check if there are still enemies left - if (enemyMap.empty()) continue; - enemies.emplace_back((mustStayOnOurSide && (enemyMap.begin()->second.position.x > 0)) ? Vector2{0, enemyMap.begin()->second.position.y} - : enemyMap.begin()->second.position); - ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); for (int j = 0; j < row_length; j++) { cost_matrix[i][j] = stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos().dist(enemies[j]); } - enemyMap.erase(enemyMap.begin()); } // Calculate the optimal assignment of enemies to pass_defenders using the hungarian algorithm and set the position to defend for each // active pass defender From 8588b00a3d7ec1387d061a7869b3b57e8e69a0df Mon Sep 17 00:00:00 2001 From: Jorn Date: Sat, 3 Feb 2024 16:17:15 +0100 Subject: [PATCH 03/10] Move BallPlacement evals to seperate files --- roboteam_ai/CMakeLists.txt | 1 + .../include/roboteam_ai/stp/PlayEvaluator.h | 1 + ...BallPlacementUsDirectGameStateEvaluation.h | 32 +++++++++++++++++++ roboteam_ai/src/stp/PlayEvaluator.cpp | 3 ++ .../stp/computations/PositionComputations.cpp | 4 +-- ...llPlacementUsDirectGameStateEvaluation.cpp | 10 ++++++ .../BallPlacementUsGameStateEvaluation.cpp | 5 +-- .../BallPlacementUsForceStart.cpp | 4 +-- .../BallPlacementUsFreeKick.cpp | 11 ++++--- roboteam_ai/test/TestFixtures/TestFixture.h | 19 +++++++---- 10 files changed, 71 insertions(+), 19 deletions(-) create mode 100644 roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h create mode 100644 roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp diff --git a/roboteam_ai/CMakeLists.txt b/roboteam_ai/CMakeLists.txt index 6e0731f90..6172cb409 100644 --- a/roboteam_ai/CMakeLists.txt +++ b/roboteam_ai/CMakeLists.txt @@ -158,6 +158,7 @@ add_library(roboteam_ai_evaluation ${PROJECT_SOURCE_DIR}/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp ${PROJECT_SOURCE_DIR}/src/stp/evaluations/global/BallNotInOurDefenseAreaAndStillGlobalEvaluation.cpp ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp + ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp ${PROJECT_SOURCE_DIR}/src/stp/evaluations/game_states/KickOffUsGameStateEvaluation.cpp diff --git a/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h b/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h index 3e1d41496..a89c5738c 100644 --- a/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h +++ b/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h @@ -17,6 +17,7 @@ enum class GlobalEvaluation { /// Game States BallPlacementThemGameState = 0, BallPlacementUsGameState, + BallPlacementUsDirectGameState, FreeKickThemGameState, FreeKickUsGameState, HaltGameState, diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h new file mode 100644 index 000000000..5d1f56a0a --- /dev/null +++ b/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h @@ -0,0 +1,32 @@ +// +// Created by timovdk on 4/14/20. +// + +#ifndef RTT_BALLPLACEMENTUSDIRECTGAMESTATEEVALUATION_H +#define RTT_BALLPLACEMENTUSDIRECTGAMESTATEEVALUATION_H + +#include "stp/evaluations/BaseEvaluation.h" + +namespace rtt::ai::stp::evaluation { +/** + * @brief Class that evaluates the ball placement us game state when the next command is direct free kick + */ +class BallPlacementUsDirectGameStateEvaluation : public BaseEvaluation { + public: + /** + * @brief Calculates the score for the ball placement us game state when the next command is direct free kick + * @param world The current world + * @param field The current field + * @return The score of the ball placement direct us game state + */ + [[nodiscard]] uint8_t metricCheck(const world::World* world, const Field* field) const noexcept override; + + /** + * @brief Retrieves the name of the evaluation + * @return A string containing the name of the evaluation + */ + const char* getName() override { return "gs::BallPlacementUsDirect"; } +}; +} // namespace rtt::ai::stp::evaluation + +#endif // RTT_BALLPLACEMENTUSDIRECTGAMESTATEEVALUATION_H diff --git a/roboteam_ai/src/stp/PlayEvaluator.cpp b/roboteam_ai/src/stp/PlayEvaluator.cpp index 5b7a03458..712b83ebf 100644 --- a/roboteam_ai/src/stp/PlayEvaluator.cpp +++ b/roboteam_ai/src/stp/PlayEvaluator.cpp @@ -5,6 +5,7 @@ #include "stp/PlayEvaluator.h" #include +#include #include #include #include @@ -44,6 +45,8 @@ uint8_t PlayEvaluator::updateGlobalEvaluation(GlobalEvaluation& evaluation, cons return evaluation::BallPlacementThemGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::BallPlacementUsGameState: return evaluation::BallPlacementUsGameStateEvaluation().metricCheck(world, &field); + case GlobalEvaluation::BallPlacementUsDirectGameState: + return evaluation::BallPlacementUsDirectGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::FreeKickThemGameState: return evaluation::FreeKickThemGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::FreeKickUsGameState: diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 2c99bae20..06bd6b76e 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -311,7 +311,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma // Calculate the distance between the defenders and their enemies std::vector> cost_matrix; cost_matrix.resize(activeDefenderNames.size()); - int row_length = std::min(activeDefenderNames.size(), enemyMap.size()); + size_t row_length = std::min(activeDefenderNames.size(), enemyMap.size()); for (size_t i = 0; i < row_length; i++) { enemies.emplace_back((mustStayOnOurSide && (enemyMap.begin()->second.position.x > 0.0)) ? Vector2{0.0, enemyMap.begin()->second.position.y} : enemyMap.begin()->second.position); @@ -320,7 +320,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } for (size_t i = 0; i < activeDefenderNames.size(); i++) { cost_matrix[i].resize(row_length); - for (int j = 0; j < row_length; j++) { + for (size_t j = 0; j < row_length; j++) { cost_matrix[i][j] = stpInfos[activeDefenderNames[i]].getRobot()->get()->getPos().dist(enemies[j]); } } diff --git a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp new file mode 100644 index 000000000..5090b863d --- /dev/null +++ b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp @@ -0,0 +1,10 @@ +#include "stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.h" + +#include "utilities/GameStateManager.hpp" + +namespace rtt::ai::stp::evaluation { +uint8_t BallPlacementUsDirectGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US_DIRECT) ? 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/game_states/BallPlacementUsGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp index a64d7146e..1d9097392 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp @@ -8,9 +8,6 @@ namespace rtt::ai::stp::evaluation { uint8_t BallPlacementUsGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US || - GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US_DIRECT) - ? stp::control_constants::FUZZY_TRUE - : stp::control_constants::FUZZY_FALSE; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US) ? 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/plays/referee_specific/BallPlacementUsForceStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index dcb68fd98..cf11e624e 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -38,9 +38,9 @@ BallPlacementUsForceStart::BallPlacementUsForceStart() : Play() { }; } -uint8_t BallPlacementUsForceStart::score(const rtt::Field& field) noexcept { +uint8_t BallPlacementUsForceStart::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() != RefCommand::BALL_PLACEMENT_US_DIRECT); + return control_constants::FUZZY_TRUE; } Dealer::FlagMap BallPlacementUsForceStart::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index 31ab330de..d445f0470 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -14,11 +14,11 @@ namespace rtt::ai::stp::play { BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { // Evaluations that have to be true in order for this play to be considered valid. startPlayEvaluation.clear(); - startPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsGameState); + startPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsDirectGameState); // Evaluations that have to be true to allow the play to continue, otherwise the play will change. Plays can also end using the shouldEndPlay(). keepPlayEvaluation.clear(); - keepPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsGameState); + keepPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementUsDirectGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ @@ -38,9 +38,9 @@ BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { }; } -uint8_t BallPlacementUsFreeKick::score(const rtt::Field& field) noexcept { +uint8_t BallPlacementUsFreeKick::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US_DIRECT); + return control_constants::FUZZY_TRUE; } Dealer::FlagMap BallPlacementUsFreeKick::decideRoleFlags() const noexcept { @@ -93,6 +93,9 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < control_constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerSpeed(100); } + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->hasBall()) { + stpInfos["ball_placer"].second.setShouldAvoidTheirRobots(false); + } } const char* BallPlacementUsFreeKick::getName() const { return "Ball Placement Us Free Kick"; } diff --git a/roboteam_ai/test/TestFixtures/TestFixture.h b/roboteam_ai/test/TestFixtures/TestFixture.h index 3e1e7d0e1..146a798c6 100644 --- a/roboteam_ai/test/TestFixtures/TestFixture.h +++ b/roboteam_ai/test/TestFixtures/TestFixture.h @@ -34,6 +34,8 @@ #include "stp/plays/referee_specific/BallPlacementUsFreeKick.h" #include "stp/plays/referee_specific/DefensiveStopFormation.h" #include "stp/plays/referee_specific/FreeKickThem.h" +#include "stp/plays/referee_specific/FreeKickUsAtGoal.h" +#include "stp/plays/referee_specific/FreeKickUsPass.h" #include "stp/plays/referee_specific/Halt.h" #include "stp/plays/referee_specific/KickOffThem.h" #include "stp/plays/referee_specific/KickOffThemPrepare.h" @@ -104,20 +106,23 @@ class RTT_AI_Tests : public ::testing::Test { plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); // plays.emplace_back(std::make_unique()); From 4f1c8f90cca30555271269183cfc5a99be9325e9 Mon Sep 17 00:00:00 2001 From: Jorn Date: Sat, 3 Feb 2024 16:18:43 +0100 Subject: [PATCH 04/10] remove unitentional commit lines --- .../src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index d445f0470..0a727597f 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -93,9 +93,6 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < control_constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerSpeed(100); } - if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->hasBall()) { - stpInfos["ball_placer"].second.setShouldAvoidTheirRobots(false); - } } const char* BallPlacementUsFreeKick::getName() const { return "Ball Placement Us Free Kick"; } From 349f24b8159770c0653b636015ba76bbf04f340f Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 5 Feb 2024 15:02:09 +0100 Subject: [PATCH 05/10] Change compiler settings to prevent world from crashing when playing vs tigers ai --- CMakeLists.txt | 2 +- roboteam_ai/src/stp/computations/PositionComputations.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 47fe0f2c6..ede6c406e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ endif () set(DEBUG_COMPILE_FLAGS "${CPP_STANDARD_FLAG}" "-fPIC" "-g" "-O0" "-Wall" "-Wextra" "-Wnon-virtual-dtor" "-pedantic" "-fdiagnostics-color=always") # march mtune https://stackoverflow.com/questions/54039176/mtune-and-march-when-compiling-in-a-docker-image -set(RELEASE_COMPILE_FLAGS "${CPP_STANDARD_FLAG}" "-fPIC" "-Ofast" "-march=x86-64" "-mtune=generic" "-fdiagnostics-color=always") +set(RELEASE_COMPILE_FLAGS "${CPP_STANDARD_FLAG}" "-fPIC" "-O3" "-march=native" "-fdiagnostics-color=always") # Specify manually which compiler arguments we want to use, either DEBUG (default) or RELEASE ones # If you want to build in release mode, pass '-DCMAKE_BUILD_TYPE=RELEASE' as argument to cmake diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 06bd6b76e..d3b99dc76 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -298,7 +298,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma for (size_t i = 0; i < loopSize; i++) { Vector2 defendPostion = enemyMap.begin()->second.position; if (mustStayOnOurSide && defendPostion.x > 0) { - defendPostion.x = 0; + defendPostion.x = 0.0; } stpInfos["defender_" + std::to_string(i)].setPositionToDefend(defendPostion); ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); From bd0d6adda38318af6c1fb78e4010f7c2ce08e7e1 Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 5 Feb 2024 16:36:00 +0100 Subject: [PATCH 06/10] Small changes wrt kick off location and ref state using next ref command --- roboteam_ai/src/stp/computations/PositionComputations.cpp | 7 ++++++- roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp | 3 +-- roboteam_ai/src/utilities/StrategyManager.cpp | 7 ++++++- 3 files changed, 13 insertions(+), 4 deletions(-) diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index d3b99dc76..b02fafdd5 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -452,7 +452,12 @@ void PositionComputations::calculateInfoForFormationOurSide(std::unordered_maphasBall()) return true; - - // If the ball is moving too slow after we have kicked it, we should stop the play to get the ball if (ballKicked() && world->getWorld()->getBall()->get()->velocity.length() < control_constants::BALL_IS_MOVING_SLOW_LIMIT) return true; + if (stpInfos["receiver"].getRobot() && world->getWorld()->getBall()->get()->position.x < stpInfos["receiver"].getRobot()->get()->getPos().x) return true; return false; } diff --git a/roboteam_ai/src/utilities/StrategyManager.cpp b/roboteam_ai/src/utilities/StrategyManager.cpp index f28a8e3fb..a9082a1fb 100644 --- a/roboteam_ai/src/utilities/StrategyManager.cpp +++ b/roboteam_ai/src/utilities/StrategyManager.cpp @@ -8,13 +8,18 @@ namespace rtt::ai { void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCommand, std::optional ballOpt) { - if (command == RefCommand::STOP && (nextCommand == RefCommand::PREPARE_KICKOFF_THEM || nextCommand == RefCommand::PREPARE_KICKOFF_US)) { + if (command == RefCommand::STOP && (nextCommand == RefCommand::PREPARE_KICKOFF_THEM || nextCommand == RefCommand::PREPARE_KICKOFF_US || + nextCommand == RefCommand::PREPARE_PENALTY_THEM || nextCommand == RefCommand::PREPARE_PENALTY_US ||)) { command = nextCommand; } if (command == RefCommand::BALL_PLACEMENT_US && nextCommand == RefCommand::DIRECT_FREE_US) { command = RefCommand::BALL_PLACEMENT_US_DIRECT; } + // Go to ball placement them, such that we are standing between the ball and our goal while keeping the required distance + if (command == RefCommand::STOP && nextCommand == RefCommand::FORCED_START) { + command = RefCommand::BALL_PLACEMENT_THEM; + } if ((currentGameState.commandId == RefCommand::DIRECT_FREE_THEM || currentGameState.commandId == RefCommand::DIRECT_FREE_US || currentGameState.commandId == RefCommand::KICKOFF_US || currentGameState.commandId == RefCommand::KICKOFF_THEM) && From 8d17c4daefac6579cc73ff16541f40e6ec3741dd Mon Sep 17 00:00:00 2001 From: Jorn Date: Mon, 5 Feb 2024 16:39:16 +0100 Subject: [PATCH 07/10] Fix small typo --- roboteam_ai/src/utilities/StrategyManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/roboteam_ai/src/utilities/StrategyManager.cpp b/roboteam_ai/src/utilities/StrategyManager.cpp index a9082a1fb..556ccec73 100644 --- a/roboteam_ai/src/utilities/StrategyManager.cpp +++ b/roboteam_ai/src/utilities/StrategyManager.cpp @@ -9,7 +9,7 @@ namespace rtt::ai { void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCommand, std::optional ballOpt) { if (command == RefCommand::STOP && (nextCommand == RefCommand::PREPARE_KICKOFF_THEM || nextCommand == RefCommand::PREPARE_KICKOFF_US || - nextCommand == RefCommand::PREPARE_PENALTY_THEM || nextCommand == RefCommand::PREPARE_PENALTY_US ||)) { + nextCommand == RefCommand::PREPARE_PENALTY_THEM || nextCommand == RefCommand::PREPARE_PENALTY_US)) { command = nextCommand; } From 363c085f09b4c1cdbdeaf9725fc5afff7452a77c Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 7 Feb 2024 10:21:25 +0100 Subject: [PATCH 08/10] Use nextref command during free kicks --- roboteam_ai/include/roboteam_ai/utilities/RefCommand.h | 8 ++++++++ .../include/roboteam_ai/utilities/StrategyManager.h | 2 ++ .../game_states/FreeKickThemGameStateEvaluation.cpp | 5 ++++- .../game_states/FreeKickUsGameStateEvaluation.cpp | 5 ++++- roboteam_ai/src/utilities/StrategyManager.cpp | 9 ++++++--- 5 files changed, 24 insertions(+), 5 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h b/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h index 7be4b418e..92eeccf46 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h +++ b/roboteam_ai/include/roboteam_ai/utilities/RefCommand.h @@ -37,6 +37,8 @@ enum class RefCommand { PENALTY_US = 20, PENALTY_THEM = 21, 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 UNDEFINED = -1 }; @@ -80,6 +82,12 @@ inline std::ostream &operator<<(std::ostream &os, const RefCommand &command) { case RefCommand::DIRECT_FREE_THEM: os << "DIRECT_FREE_THEM"; break; + case RefCommand::DIRECT_FREE_US_STOP: + os << "DIRECT_FREE_US_STOP"; + break; + case RefCommand::DIRECT_FREE_THEM_STOP: + os << "DIRECT_FREE_THEM_STOP"; + break; case RefCommand::TIMEOUT_US: os << "TIMEOUT_US"; break; diff --git a/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h b/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h index 920545064..cbfbeb737 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h +++ b/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h @@ -67,6 +67,8 @@ class StrategyManager { GameState(RefCommand::BALL_PLACEMENT_THEM, Constants::RULESET_STOP()), GameState(RefCommand::BALL_PLACEMENT_US, Constants::RULESET_STOP()), GameState(RefCommand::BALL_PLACEMENT_US_DIRECT, Constants::RULESET_STOP()), + GameState(RefCommand::DIRECT_FREE_THEM_STOP, Constants::RULESET_STOP()), + GameState(RefCommand::DIRECT_FREE_US_STOP, Constants::RULESET_STOP()), GameState(RefCommand::PREPARE_KICKOFF_US, Constants::RULESET_STOP(), false, RefCommand::KICKOFF_US), GameState(RefCommand::PREPARE_KICKOFF_THEM, Constants::RULESET_STOP(), false, RefCommand::KICKOFF_THEM), GameState(RefCommand::PREPARE_PENALTY_US, Constants::RULESET_STOP(), false, RefCommand::PENALTY_US), diff --git a/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp index ce629d4d5..7f81b168f 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp @@ -9,6 +9,9 @@ namespace rtt::ai::stp::evaluation { uint8_t FreeKickThemGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_THEM ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_THEM || + GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_THEM_STOP) + ? 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/game_states/FreeKickUsGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/FreeKickUsGameStateEvaluation.cpp index 6aa73ef8b..1814063dc 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/FreeKickUsGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/FreeKickUsGameStateEvaluation.cpp @@ -9,6 +9,9 @@ namespace rtt::ai::stp::evaluation { uint8_t FreeKickUsGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_US ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_US || + GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_US_STOP) + ? 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/utilities/StrategyManager.cpp b/roboteam_ai/src/utilities/StrategyManager.cpp index 556ccec73..d5cd03de2 100644 --- a/roboteam_ai/src/utilities/StrategyManager.cpp +++ b/roboteam_ai/src/utilities/StrategyManager.cpp @@ -16,9 +16,12 @@ void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCom if (command == RefCommand::BALL_PLACEMENT_US && nextCommand == RefCommand::DIRECT_FREE_US) { command = RefCommand::BALL_PLACEMENT_US_DIRECT; } - // Go to ball placement them, such that we are standing between the ball and our goal while keeping the required distance - if (command == RefCommand::STOP && nextCommand == RefCommand::FORCED_START) { - command = RefCommand::BALL_PLACEMENT_THEM; + if (command == RefCommand::STOP && (nextCommand == RefCommand::FORCED_START || nextCommand == RefCommand::DIRECT_FREE_THEM)) { + command = RefCommand::DIRECT_FREE_THEM_STOP; + } + + if (command == RefCommand::STOP && nextCommand == RefCommand::DIRECT_FREE_US) { + command = RefCommand::DIRECT_FREE_US_STOP; } if ((currentGameState.commandId == RefCommand::DIRECT_FREE_THEM || currentGameState.commandId == RefCommand::DIRECT_FREE_US || From 31ea9665310ccd6f5a85589f6d6048d220c642e7 Mon Sep 17 00:00:00 2001 From: Jorn Date: Wed, 7 Feb 2024 18:27:59 +0100 Subject: [PATCH 09/10] Fix ball sensor vs dribbler bar ball detection usage Change angle where we have the ball to match the design of the robot --- roboteam_ai/src/utilities/Constants.cpp | 2 +- roboteam_ai/src/world/Robot.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/roboteam_ai/src/utilities/Constants.cpp b/roboteam_ai/src/utilities/Constants.cpp index ca15cac4c..bf71cf561 100644 --- a/roboteam_ai/src/utilities/Constants.cpp +++ b/roboteam_ai/src/utilities/Constants.cpp @@ -38,7 +38,7 @@ double Constants::MAX_DEC_LOWER() { return MAX_ACC_LOWER() * 1.2; } // magic nu double Constants::HAS_BALL_DISTANCE() { return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.11 : 0.12; } // The max angle the ball can have to the robot for the robot to have the ball -double Constants::HAS_BALL_ANGLE() { return 0.10 * M_PI; } +double Constants::HAS_BALL_ANGLE() { return 0.15 * M_PI; } std::map Constants::ROBOTS_WITH_WORKING_DRIBBLER() { static std::map workingDribblerRobots; diff --git a/roboteam_ai/src/world/Robot.cpp b/roboteam_ai/src/world/Robot.cpp index 5a5759831..d7580c290 100644 --- a/roboteam_ai/src/world/Robot.cpp +++ b/roboteam_ai/src/world/Robot.cpp @@ -100,7 +100,7 @@ void Robot::updateHasBallMap(std::optional &ball) { if (!ball) return; auto hasBallAccordingToVision = distanceToBall < ai::Constants::HAS_BALL_DISTANCE() && angleDiffToBall < ai::Constants::HAS_BALL_ANGLE(); - auto hasBallAccordingToDribblerOrBallSensor = (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? ballSensorSeesBall : dribblerSeesBall; + auto hasBallAccordingToDribblerOrBallSensor = (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? dribblerSeesBall : ballSensorSeesBall; if (hasBallAccordingToDribblerOrBallSensor && hasBallAccordingToVision) { hasBallUpdateMap[id].score = 25; } else { From 194c01c69cefb92776d600709ebe539bc246d878 Mon Sep 17 00:00:00 2001 From: Jorn Date: Thu, 8 Feb 2024 12:08:07 +0100 Subject: [PATCH 10/10] Resolve and remove outdated TODO's --- .../roboteam_ai/control/ControlModule.h | 6 +-- roboteam_ai/src/control/ControlModule.cpp | 42 +++++++++---------- roboteam_ai/src/stp/Play.cpp | 1 - .../stp/plays/defensive/KeeperKickBall.cpp | 1 - .../src/stp/plays/offensive/AttackingPass.cpp | 1 - .../src/stp/plays/offensive/ChippingPass.cpp | 3 +- .../plays/referee_specific/FreeKickUsPass.cpp | 1 - .../stp/plays/referee_specific/KickOffUs.cpp | 1 - roboteam_ai/src/stp/skills/GoToPos.cpp | 15 +------ .../src/stp/tactics/passive/Formation.cpp | 2 +- roboteam_ai/src/utilities/Dealer.cpp | 1 - roboteam_ai/src/utilities/IOManager.cpp | 1 - roboteam_ai/src/world/Robot.cpp | 2 +- .../BBTrajectory/BBTrajectory1DTest.cpp | 1 - .../test/HelperTests/FieldHelperTest.cpp | 3 +- roboteam_utils/test/utils/Vector2Test.cpp | 2 +- 16 files changed, 31 insertions(+), 52 deletions(-) diff --git a/roboteam_ai/include/roboteam_ai/control/ControlModule.h b/roboteam_ai/include/roboteam_ai/control/ControlModule.h index 1b56a9052..fb016723a 100644 --- a/roboteam_ai/include/roboteam_ai/control/ControlModule.h +++ b/roboteam_ai/include/roboteam_ai/control/ControlModule.h @@ -30,7 +30,7 @@ class ControlModule { * @param command Robot command that needs to be checked * @param robot Info about the robot */ - static void limitRobotCommand(rtt::RobotCommand& command, std::optional robot); + static void limitRobotCommand(rtt::RobotCommand& command, rtt::world::view::RobotView robot); /** * @brief Limits the velocity with a control_constants value @@ -43,7 +43,7 @@ class ControlModule { * @param command Robot command that needs to be checked * @param robot Info about the robot */ - static void limitAngularVel(rtt::RobotCommand& command, std::optional robot); + static void limitAngularVel(rtt::RobotCommand& command, rtt::world::view::RobotView robot); /** * @brief Rotates the robot command to the other side of the field @@ -57,7 +57,7 @@ class ControlModule { * @param command Robot command that needs to be added * @param robot Info about the robot */ - static void addRobotCommand(std::optional robot, const rtt::RobotCommand& command) noexcept; + static void addRobotCommand(std::optional robot, rtt::RobotCommand command) noexcept; /** * @brief Sends all commands to robothub */ diff --git a/roboteam_ai/src/control/ControlModule.cpp b/roboteam_ai/src/control/ControlModule.cpp index de30fcad9..2da4f3c36 100644 --- a/roboteam_ai/src/control/ControlModule.cpp +++ b/roboteam_ai/src/control/ControlModule.cpp @@ -19,34 +19,31 @@ void ControlModule::rotateRobotCommand(rtt::RobotCommand& command) { command.targetAngle += M_PI; } -void ControlModule::limitRobotCommand(rtt::RobotCommand& command, std::optional robot) { +void ControlModule::limitRobotCommand(rtt::RobotCommand& command, rtt::world::view::RobotView robot) { limitVel(command); limitAngularVel(command, robot); } void ControlModule::limitVel(rtt::RobotCommand& command) { - // The robot can currently not reach very low speeds- if we want it to move a non-trivial amount, we need to send a higher velocity than the path-planning outputs - // TODO: Look at the correct value for this with control - if (command.velocity.length() > 0.03 && command.velocity.length() < 0.25) command.velocity = command.velocity.stretchToLength(0.25); command.velocity = command.velocity.stretchToLength(std::clamp(command.velocity.length(), 0.0, Constants::MAX_VEL_CMD())); } -void ControlModule::limitAngularVel(rtt::RobotCommand& command, std::optional robot) { +void ControlModule::limitAngularVel(rtt::RobotCommand& command, rtt::world::view::RobotView robot) { // Limit the angular velocity when the robot has the ball by setting the target angle in small steps - // Might want to limit on the robot itself - if (robot.value()->hasBall() && !command.useAngularVelocity) { + if (robot->hasBall() && !command.useAngularVelocity) { auto targetAngle = command.targetAngle; - // TODO: Why use optional robotView if we never check for the case where it does not contain one? - auto robotAngle = robot.value()->getAngle(); + auto robotAngle = robot->getAngle(); + // Adjust robot angle if game is not on the left if (!GameSettings::isLeft()) { robotAngle += M_PI; } - // If the angle error is larger than the desired angle rate, the angle command is adjusted + // If the angle error is larger than the desired angle rate, adjust the angle command if (robotAngle.shortestAngleDiff(targetAngle) > stp::control_constants::ANGLE_RATE) { - // Direction of rotation is the shortest distance + // Determine direction of rotation (shortest distance) int direction = Angle(robotAngle).rotateDirection(targetAngle) ? 1 : -1; + // Set the angle command to the current robot angle + the angle rate command.targetAngle = robotAngle + Angle(direction * stp::control_constants::ANGLE_RATE); } @@ -54,23 +51,22 @@ void ControlModule::limitAngularVel(rtt::RobotCommand& command, std::optional robot, const rtt::RobotCommand& command) noexcept { - rtt::RobotCommand robot_command = command; // TODO: Why make a copy of the command? It will be copied anyway when we put it in the vector +void ControlModule::addRobotCommand(std::optional<::rtt::world::view::RobotView> robot, rtt::RobotCommand command) noexcept { // If we are not left, commands should be rotated (because we play as right) if (!GameSettings::isLeft()) { - rotateRobotCommand(robot_command); + rotateRobotCommand(command); } - if (robot) limitRobotCommand(robot_command, robot); + if (robot) limitRobotCommand(command, *robot); // if we are in simulation; adjust w() to be angular velocity) - if (GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR && !robot_command.useAngularVelocity) { - simulator_angular_control(robot, robot_command); + if (GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR && !command.useAngularVelocity) { + simulator_angular_control(robot, command); } // Only add commands with a robotID that is not in the vector yet - if (robot_command.id >= 0 && robot_command.id < 16) { - robotCommands.emplace_back(robot_command); + if (command.id >= 0 && command.id < 16) { + robotCommands.emplace_back(command); } } @@ -105,9 +101,13 @@ void ControlModule::simulator_angular_control(const std::optional<::rtt::world:: } void ControlModule::sendAllCommands() { - // TODO: check for double commands (But do we really have to do that?) + // Remove duplicate commands + std::sort(robotCommands.begin(), robotCommands.end(), [](const rtt::RobotCommand& a, const rtt::RobotCommand& b) { return a.id < b.id; }); + auto last = std::unique(robotCommands.begin(), robotCommands.end(), [](const rtt::RobotCommand& a, const rtt::RobotCommand& b) { return a.id == b.id; }); + robotCommands.erase(last, robotCommands.end()); - io::io.publishAllRobotCommands(robotCommands); // When vector has all commands, send in one go + // When vector has all commands, send in one go + io::io.publishAllRobotCommands(robotCommands); robotCommands.clear(); } } // namespace rtt::ai::control \ No newline at end of file diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index 7a500d258..71ea5d044 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -144,7 +144,6 @@ void Play::distributeRoles() noexcept { } auto distribution = dealer.distribute(world->getWorld()->getUs(), flagMap, stpInfos); - // TODO-Max if role exists in oldStpInfos then copy those. // Clear the stpInfos for the new role assignment bool cardIdAssigned = false; for (auto &role : roles) { diff --git a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp index fb33e7d1e..954862968 100644 --- a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp +++ b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp @@ -91,7 +91,6 @@ void KeeperKickBall::calculateInfoForRoles() noexcept { } bool KeeperKickBall::ballKicked() { - // TODO: create better way of checking when ball has been kicked return std::any_of(roles.begin(), roles.end(), [](const std::unique_ptr& role) { return role != nullptr && role->getName() == "keeper" && strcmp(role->getCurrentTactic()->getName(), "Keeper Block Ball") == 0; }); diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp index 01b6a9559..83fedf6fa 100644 --- a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp @@ -107,7 +107,6 @@ void AttackingPass::calculateInfoForRoles() noexcept { } bool AttackingPass::ballKicked() { - // TODO: create better way of checking when ball has been kicked return std::any_of(roles.begin(), roles.end(), [](const std::unique_ptr& role) { return role != nullptr && role->getName() == "passer" && strcmp(role->getCurrentTactic()->getName(), "Formation") == 0; }); diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp index 1b0eaec7d..52a708960 100644 --- a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp @@ -107,8 +107,7 @@ void ChippingPass::calculateInfoForRoles() noexcept { } } -bool ChippingPass::ballKicked() { // Also for ball chipped - // TODO: create better way of checking when ball has been kicked +bool ChippingPass::ballKicked() { return std::any_of(roles.begin(), roles.end(), [](const std::unique_ptr& role) { return role != nullptr && role->getName() == "passer" && strcmp(role->getCurrentTactic()->getName(), "Formation") == 0; }); diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index 536142d55..b444795b9 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -100,7 +100,6 @@ void FreeKickUsPass::calculateInfoForRoles() noexcept { } bool FreeKickUsPass::ballKicked() { - // TODO: create better way of checking when ball has been kicked return std::any_of(roles.begin(), roles.end(), [](const std::unique_ptr& role) { return role != nullptr && role->getName() == "free_kick_taker" && strcmp(role->getCurrentTactic()->getName(), "Formation") == 0; }); diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index 7f0de9d5b..e0ace4060 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -88,7 +88,6 @@ bool KickOffUs::shouldEndPlay() noexcept { } bool KickOffUs::ballKicked() { - // TODO: create better way of checking when ball has been kicked return std::any_of(roles.begin(), roles.end(), [](const std::unique_ptr &role) { return role != nullptr && role->getName() == "kick_off_taker" && strcmp(role->getCurrentTactic()->getName(), "Formation") == 0; }); diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index 98ccbe188..adfb1f595 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -28,19 +28,8 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { info.getCurrentWorld(), info.getField().value(), info.getRobot().value()->getId(), info.getRobot().value()->getPos(), info.getRobot().value()->getVel(), targetPos, info.getMaxRobotVelocity(), info.getPidType().value(), avoidObj); - double targetVelocityLength; - if (info.getPidType() == stp::PIDType::KEEPER && (info.getRobot()->get()->getPos() - targetPos).length() > 2.0 * control_constants::ROBOT_RADIUS) { - targetVelocityLength = std::max(std::clamp(commandCollision.robotCommand.velocity.length(), 0.0, info.getMaxRobotVelocity()), 1.5); // TODO: Tune this value better - } else if (info.getPidType() == stp::PIDType::INTERCEPT && (info.getRobot()->get()->getPos() - targetPos).length() > 2.0 * control_constants::ROBOT_RADIUS) { - targetVelocityLength = std::max(std::clamp(commandCollision.robotCommand.velocity.length(), 0.0, info.getMaxRobotVelocity()), 1.5); // TODO: Tune this value better - } else { - targetVelocityLength = std::clamp(commandCollision.robotCommand.velocity.length(), 0.0, info.getMaxRobotVelocity()); - } - // Clamp and set velocity - Vector2 targetVelocity = commandCollision.robotCommand.velocity.stretchToLength(targetVelocityLength); - - // Set velocity and angle commands - command.velocity = targetVelocity; + auto &velocity = commandCollision.robotCommand.velocity; + command.velocity = velocity.stretchToLength(std::clamp(velocity.length(), 0.0, info.getMaxRobotVelocity())); // TODO: Test with control peeps to see what angle works best when driving. // Driving and turning do not work well together, so we only turn when we are close to the target position. diff --git a/roboteam_ai/src/stp/tactics/passive/Formation.cpp b/roboteam_ai/src/stp/tactics/passive/Formation.cpp index a97849781..3307390a0 100644 --- a/roboteam_ai/src/stp/tactics/passive/Formation.cpp +++ b/roboteam_ai/src/stp/tactics/passive/Formation.cpp @@ -1,7 +1,6 @@ // // Created by timovdk on 3/27/20. /// Goes to a give POSITION and ROTATEs to face forwards -/// TODO-Max Make angle an optional info /// PASSIVE // @@ -21,6 +20,7 @@ std::optional Formation::calculateInfoForSkill(StpInfo const &info) noe StpInfo skillStpInfo = info; if (!info.getPositionToMoveTo()) return std::nullopt; + if (!info.getAngle()) skillStpInfo.setAngle(0); // Be 100% sure the dribbler is off during the formation skillStpInfo.setDribblerSpeed(0); diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index 2fab7a3ca..49b623c7d 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -273,7 +273,6 @@ double Dealer::getRobotScoreForDistance(const stp::StpInfo &stpInfo, const v::Ro return costForDistance(distance, field->playArea.height()); } -// TODO these values need to be tuned. double Dealer::getDefaultFlagScores(const v::RobotView &robot, const Dealer::DealerFlag &flag) { switch (flag.title) { // case DealerFlagTitle::WITH_WORKING_BALL_SENSOR: diff --git a/roboteam_ai/src/utilities/IOManager.cpp b/roboteam_ai/src/utilities/IOManager.cpp index 01ba18255..e26b6032f 100644 --- a/roboteam_ai/src/utilities/IOManager.cpp +++ b/roboteam_ai/src/utilities/IOManager.cpp @@ -124,7 +124,6 @@ bool IOManager::publishRobotCommands(const rtt::RobotCommands& aiCommand, bool f } if (!sentCommands) { - // TODO: Uncomment RTT_ERROR("Failed to send command: Publisher is not initialized (yet)"); } diff --git a/roboteam_ai/src/world/Robot.cpp b/roboteam_ai/src/world/Robot.cpp index d7580c290..3bdafd6a8 100644 --- a/roboteam_ai/src/world/Robot.cpp +++ b/roboteam_ai/src/world/Robot.cpp @@ -89,7 +89,7 @@ void Robot::updateFromFeedback(const proto::RobotProcessedFeedback &feedback) no // TODO: add processing of more of the fields of feedback if (ai::Constants::FEEDBACK_ENABLED()) { setWorkingBallSensor(feedback.ball_sensor_is_working()); - setBatteryLow(feedback.battery_level() < 22); // TODO: Define what is considered a 'low' voltage + setBatteryLow(feedback.battery_level() < 22); // TODO: Figure out with electronics which value should be considered low setBallSensorSeesBall(feedback.ball_sensor_sees_ball()); setDribblerSeesBall(feedback.dribbler_sees_ball()); setBallPosBallSensor(feedback.ball_position()); diff --git a/roboteam_ai/test/ControlTests/BBTrajectory/BBTrajectory1DTest.cpp b/roboteam_ai/test/ControlTests/BBTrajectory/BBTrajectory1DTest.cpp index 3565ca159..45764f962 100644 --- a/roboteam_ai/test/ControlTests/BBTrajectory/BBTrajectory1DTest.cpp +++ b/roboteam_ai/test/ControlTests/BBTrajectory/BBTrajectory1DTest.cpp @@ -48,7 +48,6 @@ TEST(BBTrajectories, onlyDecelerateOtherWay) { EXPECT_DOUBLE_EQ(test.getPosition(1), -0.75); EXPECT_DOUBLE_EQ(test.getVelocity(2), 0); EXPECT_DOUBLE_EQ(test.getVelocity(3), 0); - // TODO: fix buggy parts EXPECT_FALSE(test.inLastPart(0.000)); EXPECT_TRUE(test.inLastPart(2.5)); } diff --git a/roboteam_ai/test/HelperTests/FieldHelperTest.cpp b/roboteam_ai/test/HelperTests/FieldHelperTest.cpp index 00cc75195..9a9a39a68 100644 --- a/roboteam_ai/test/HelperTests/FieldHelperTest.cpp +++ b/roboteam_ai/test/HelperTests/FieldHelperTest.cpp @@ -51,8 +51,7 @@ TEST(FieldHelperTests, FieldTest) { auto fieldWidth = field.playArea.width(); auto fieldHeight = field.playArea.height(); - // TODO: What does this line do? Should it be tested? Or can it be removed? - field.playArea.contains(rtt::Vector2(0.99 * fieldWidth, 0.99 * fieldHeight)); + EXPECT_FALSE(field.playArea.contains(rtt::Vector2(0.99 * fieldWidth, 0.99 * fieldHeight))); EXPECT_TRUE(field.playArea.contains(rtt::Vector2(0.49 * fieldWidth, 0.49 * fieldHeight))); EXPECT_TRUE(field.playArea.contains(rtt::Vector2(-0.49 * fieldWidth, -0.49 * fieldHeight))); diff --git a/roboteam_utils/test/utils/Vector2Test.cpp b/roboteam_utils/test/utils/Vector2Test.cpp index 6638c52ef..3bd8ed129 100644 --- a/roboteam_utils/test/utils/Vector2Test.cpp +++ b/roboteam_utils/test/utils/Vector2Test.cpp @@ -74,7 +74,7 @@ TEST(VectorTests, math) { ASSERT_DOUBLE_EQ(def.dot(tenten), 0); ASSERT_DOUBLE_EQ(tenten.dot(fivezero), 50); - // ASSERT_DOUBLE_EQ(M_PI_4, tenten.angle()); //TODO: somehow is not compiling on macOS + ASSERT_DOUBLE_EQ(M_PI_4, tenten.angle()); ASSERT_DOUBLE_EQ(0, fivezero.angle()); Vector2 proj = fivezero.project(def, tenten);