diff --git a/roboteam_ai/CMakeLists.txt b/roboteam_ai/CMakeLists.txt index 419cf766a..98337152b 100644 --- a/roboteam_ai/CMakeLists.txt +++ b/roboteam_ai/CMakeLists.txt @@ -229,7 +229,6 @@ target_compile_options(testHelpers PRIVATE "${COMPILER_FLAGS}") add_executable(roboteam_ai_tests ${PROJECT_SOURCE_DIR}/src/STPManager.cpp ${PROJECT_SOURCE_DIR}/test/main.cpp - ${PROJECT_SOURCE_DIR}/test/ControlTests/ControlUtilsTest.cpp ${PROJECT_SOURCE_DIR}/test/HelperTests/FieldHelperTest.cpp ${PROJECT_SOURCE_DIR}/test/UtilTests/RefereeTest.cpp ${PROJECT_SOURCE_DIR}/test/WorldTests/BallTests.cpp diff --git a/roboteam_ai/include/roboteam_ai/control/ControlModule.h b/roboteam_ai/include/roboteam_ai/control/ControlModule.h index 1936ce0c4..04f66093e 100644 --- a/roboteam_ai/include/roboteam_ai/control/ControlModule.h +++ b/roboteam_ai/include/roboteam_ai/control/ControlModule.h @@ -29,13 +29,13 @@ class ControlModule { static void limitRobotCommand(rtt::RobotCommand& command, rtt::world::view::RobotView robot); /** - * @brief Limits the velocity with a control_constants value + * @brief Limits the velocity with a constants value * @param command Robot command that needs to be checked */ static void limitVel(rtt::RobotCommand& command); /** - * @brief Limits the angular velocity with a control_constants value + * @brief Limits the angular velocity with a constants value * @param command Robot command that needs to be checked * @param robot Info about the robot */ diff --git a/roboteam_ai/include/roboteam_ai/control/ControlUtils.h b/roboteam_ai/include/roboteam_ai/control/ControlUtils.h index 84f935871..d580c10ee 100644 --- a/roboteam_ai/include/roboteam_ai/control/ControlUtils.h +++ b/roboteam_ai/include/roboteam_ai/control/ControlUtils.h @@ -21,35 +21,6 @@ namespace rtt::ai::control { */ class ControlUtils { public: - /** - * @brief Calculates the force for a given vector and weight - * @param vector Direction of the force - * @param weight Weight that needs to be displaced - * @param minDistance Minimum distance the weight should move - * @return Force needed to displace the weight - */ - static Vector2 calculateForce(const rtt::Vector2 &vector, double weight, double minDistance); - - /** - * @brief Limits the velocity - * @param vel current velocity - * @param maxVel maximum velocity - * @param minVel minimum velocity - * @param listenToReferee listen to or ignore the referee - * @return limited velocity - */ - static Vector2 velocityLimiter(const Vector2 &vel, double maxVel = Constants::MAX_VEL(), double minVel = 0.0, bool listenToReferee = true); - - /** - * @brief calculates whether the object velocity will collide with a point - * @param objectPosition position of the object - * @param velocity velocity vector - * @param point point to check for collision - * @param maxDifference maximum difference between vector and point - * @return boolean that tells whether the object will collide with the point - */ - static bool objectVelocityAimedToPoint(const Vector2 &objectPosition, const Vector2 &velocity, const Vector2 &point, double maxDifference = 0.3); - /** * @brief Determines the kick force based on the distance and the type of kick * @param distance distance to the target diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/OvershootComputations.h b/roboteam_ai/include/roboteam_ai/control/positionControl/OvershootComputations.h index c0f08b7be..eebc6a9e6 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/OvershootComputations.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/OvershootComputations.h @@ -18,8 +18,9 @@ namespace rtt::ai::control { struct TimedPos1D { double pos; double time; + double timeToTarget; - TimedPos1D(double pos, double time) : pos(pos), time(time) {} + TimedPos1D(double pos, double time, double timeToTarget) : pos(pos), time(time), timeToTarget(timeToTarget) {} }; class OvershootComputations { @@ -35,8 +36,8 @@ class OvershootComputations { * @param targetTime The time it takes to reach the target position. * @return Vector2 The overshooting destination of the robot. */ - static Vector2 overshootingDestination(const Vector2 &startPosition, const Vector2 &endPosition, const Vector2 &startVelocity, double maxVelocity, double maxAcceleration, - double targetTime); + static std::pair overshootingDestination(const Vector2 &startPosition, const Vector2 &endPosition, const Vector2 &startVelocity, double maxVelocity, + double maxAcceleration, double targetTime); private: /** diff --git a/roboteam_ai/include/roboteam_ai/stp/Play.hpp b/roboteam_ai/include/roboteam_ai/stp/Play.hpp index 2193259f6..bafa41dad 100644 --- a/roboteam_ai/include/roboteam_ai/stp/Play.hpp +++ b/roboteam_ai/include/roboteam_ai/stp/Play.hpp @@ -6,7 +6,6 @@ #include "PlayEvaluator.h" #include "Role.hpp" #include "computations/PositionComputations.h" -#include "constants/GeneralizationConstants.h" #include "stp/evaluations/BaseEvaluation.h" #include "utilities/Dealer.h" #include "utilities/GameState.h" @@ -110,7 +109,7 @@ class Play { uint8_t getLastScore() const; protected: - std::array, rtt::ai::Constants::ROBOT_COUNT()> roles; /**< The roles, constructed in ctor of a play */ + std::array, rtt::ai::constants::MAX_ROBOT_COUNT> roles; /**< The roles, constructed in ctor of a play */ std::vector scoring; /**< The evaluations with their weight */ diff --git a/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h b/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h index 31f5d61fb..1be4b9ed4 100644 --- a/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h +++ b/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h @@ -21,9 +21,7 @@ enum class GlobalEvaluation { KickOffThemGameState, KickOffThemPrepareGameState, KickOffUsGameState, - KickOffUsOrNormalGameState, KickOffUsPrepareGameState, - NormalOrFreeKickUsGameState, NormalPlayGameState, PenaltyThemGameState, PenaltyThemPrepareGameState, @@ -74,14 +72,7 @@ class PlayEvaluator { * @param cutOff Bottom bound value of true * @return boolean if FUZZY-TRUE is high enough */ - static bool checkEvaluation(GlobalEvaluation globalEvaluation, const rtt::world::World* world, uint8_t cutOff = control_constants::FUZZY_DEFAULT_CUTOFF) noexcept; - - /** - * @brief Calculate a final weighted score given a vector of scores with their weights - * @param scoring vector withto be considered Evaluations - * @return final score (0-255) - */ - static uint8_t calculateScore(std::vector& scoring); + static bool checkEvaluation(GlobalEvaluation globalEvaluation, const rtt::world::World* world, uint8_t cutOff = constants::FUZZY_DEFAULT_CUTOFF) noexcept; private: static inline std::unordered_map scoresGlobal{}; /**< Map of all loaded Global Evaluations scores */ diff --git a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h index d2152cd3c..ace25fc7e 100644 --- a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h +++ b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h @@ -4,7 +4,7 @@ #include #include -#include "constants/GeneralizationConstants.h" +#include "computations/PositionScoring.h" #include "utilities/StpInfoEnums.h" #include "world/views/BallView.hpp" #include "world/views/RobotView.hpp" diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/ComputationManager.h b/roboteam_ai/include/roboteam_ai/stp/computations/ComputationManager.h index 484762129..4b7adfc91 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/ComputationManager.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/ComputationManager.h @@ -2,8 +2,6 @@ #define RTT_COMPUTATIONMANAGER_H #include "roboteam_utils/Vector2.h" -#include "stp/constants/GeneralizationConstants.h" - namespace rtt::ai::stp { /** diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/GoalComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/GoalComputations.h index 06e7695b0..67a953095 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/GoalComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/GoalComputations.h @@ -32,7 +32,7 @@ class GoalComputations { * @param openSegments Vector of lines * @return Longest line from openSegments */ - static const LineSegment &getLongestSegment(const std::vector &openSegments); + static const LineSegment getLongestSegment(const std::vector &openSegments); }; } // namespace rtt::ai::stp::computations #endif // RTT_GOALCOMPUTATIONS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h index 8789ae212..125795965 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h @@ -12,7 +12,6 @@ #include "stp/Role.hpp" #include "stp/StpInfo.h" -#include "stp/constants/GeneralizationConstants.h" #include "utilities/Constants.h" #include "world/FieldComputations.h" #include "world/World.hpp" diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h index bc0224078..4c588e6b1 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h @@ -3,7 +3,6 @@ #include #include -#include #include @@ -20,8 +19,8 @@ struct PassInfo { int keeperId = -1; int passerId = -1; int receiverId = -1; - Vector2 passLocation; // The location where the ball will be passed from, towards the receiver - Vector2 receiverLocation; // The location of the receiver, who will receive the ball from the passer + Vector2 passLocation; // The location where the ball will be passed from, towards the receiver + Vector2 receiverLocation = Vector2(6, 0); // The location of the receiver, who will receive the ball from the passer uint8_t passScore = 0; }; @@ -38,10 +37,10 @@ class PassComputations { * @param profile the profile to be used when scoring the pass location * @param world the current world state * @param field the current field - * @param keeperCanPass indicate whether the keeper can pass and be passed to + * @param keeperMustPass indicate whether the keeper must pass * @return a PassInfo struct which contains the relevant information needed to complete the pass */ - static PassInfo calculatePass(gen::ScoreProfile profile, const world::World* world, const Field& field, bool keeperCanPass = false); + static PassInfo calculatePass(gen::ScoreProfile profile, const world::World* world, const Field& field, bool keeperMustPass = false); private: /** diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index 61927b629..aaf40aea8 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h @@ -13,7 +13,6 @@ #include "stp/Role.hpp" #include "stp/StpInfo.h" #include "stp/computations/PassComputations.h" -#include "stp/constants/GeneralizationConstants.h" #include "utilities/Constants.h" #include "world/FieldComputations.h" #include "world/World.hpp" @@ -80,9 +79,10 @@ class PositionComputations { * @param targetPosition The initial target position * @param ballPosition The position of the ball * @param field The current field + * @param avoidObj The objects to avoid * @return A position that is not within the min allowed distance to the ball */ - static Vector2 calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field); + static Vector2 calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field, const AvoidObjects &avoidObj); /** * @brief Calculates info for the harasser role @@ -92,7 +92,7 @@ class PositionComputations { * @param world The current world * @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, + static void calculateInfoForHarasser(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> *roles, const Field &field, world::World *world, Vector2 interceptionLocation) noexcept; /** @@ -103,9 +103,8 @@ class PositionComputations { * @param world The current world * @param mustStayOnOurSide Whether the defenders should always stay on our side of the field, for example to prevent interference during our own attack */ - static void calculateInfoForDefendersAndWallers(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, world::World *world, - bool mustStayOnOurSide) noexcept; + static void calculateInfoForDefendersAndWallers(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> &roles, + const Field &field, world::World *world, bool mustStayOnOurSide) noexcept; /** * @brief Calculates info for the attackers @@ -114,7 +113,7 @@ class PositionComputations { * @param field The current field * @param world The current world */ - static void calculateInfoForAttackers(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, + static void calculateInfoForAttackers(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> &roles, const Field &field, world::World *world) noexcept; /** @@ -124,7 +123,7 @@ class PositionComputations { * @param field The current field * @param world The current world */ - static void calculateInfoForFormation(std::unordered_map &stpInfos, std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, + static void calculateInfoForFormation(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> &roles, const Field &field, world::World *world) noexcept; /** @@ -134,9 +133,8 @@ class PositionComputations { * @param field The current field * @param world The current world */ - static void calculateInfoForFormationOurSide(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, - world::World *world) noexcept; + static void calculateInfoForFormationOurSide(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> &roles, + const Field &field, world::World *world) noexcept; /** * @brief Recalculates info for the position of our robots to not interfere with passing * @param stpInfos The current stpInfos @@ -158,9 +156,10 @@ class PositionComputations { * @param targetPosition The position where the robot wants to go * @param field The current field * @param avoidShape The shape to avoid + * @param avoidObj The objects to avoid * @return A position that is outside the given shape */ - static Vector2 calculatePositionOutsideOfShape(Vector2 targetPosition, const Field &field, const std::unique_ptr &avoidShape); + static Vector2 calculatePositionOutsideOfShape(Vector2 targetPosition, const Field &field, const std::unique_ptr &avoidShape, const AvoidObjects &avoidObj); }; } // namespace rtt::ai::stp #endif // RTT_POSITIONCOMPUTATIONS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionScoring.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionScoring.h index 39275851f..10fca8b70 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionScoring.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionScoring.h @@ -3,11 +3,63 @@ #include -#include "stp/constants/GeneralizationConstants.h" #include "world/World.hpp" namespace rtt::ai::stp { +namespace gen { +/** + * @brief Struct that is used to store computations made with this module. + * Save computations here that are usable for each robot for a position. + * DO NOT save values specific to a robot in here (like TimeToPosition). + * If a position's score for a specific evaluation already had been computed in the tick, it will + * use that value instead of recomputing it. If it was not computed yet, it will compute and save it. + * @memberof scoreOpen : uint8_t score for the Openness of a position -> evaluations/position/OpennessEvaluation + * @memberof scoreLineOfSight : uint8_t score for the LineOfSight to a position from a position -> ../LineOfSightEvaluation + * @memberof scoreGoalShot : uint8_t score for the Goal Shot opportunity for a position -> ../GoalShotEvaluation + */ +struct PositionScores { + std::optional scoreOpen; + std::optional scoreLineOfSight; + std::optional scoreGoalShot; +}; + +/** + * @brief Combination of weights for each of the scores. + * This will be used to determine the final score for a robot for a position. + * All weights will be multiplied with the corresponding score and then normalized. + * @memberof weightOpen for scoreOpen + * @memberof weightLineOfSight for scoreLineOfSight + * @memberof weightGoalShot for scoreGoalShot + */ +struct ScoreProfile { + double weightOpen; + double weightLineOfSight; + double weightGoalShot; +}; + +/** + * @brief Structure with a position and its score + * @memberof position Vector2 coordinates of a position + * @memberof score The score for said position + */ +struct ScoredPosition { + Vector2 position; + uint8_t score; +}; + +/** + * Generalized Position Profiles to be used in plays. + * They consist of a generalized weight combination. + */ + +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 */ +constexpr ScoreProfile ChippingPass = {1, 0, 1}; /**< Scoring weights for ChippingPass score */ +} // namespace gen + /** * @brief Class that manages the scoring for a position */ diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h deleted file mode 100644 index 259c3688c..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ /dev/null @@ -1,74 +0,0 @@ -#ifndef RTT_CONTROLCONSTANTS_H -#define RTT_CONTROLCONSTANTS_H - -#include - -#include - -namespace rtt::ai::stp::control_constants { - -// Kick and chip constants -constexpr double MAX_KICK_POWER = 6.5; /**< Maximum allowed kicking power */ -constexpr double MIN_KICK_POWER = 3.0; /**< Minimum allowed kicking power */ -constexpr double MAX_CHIP_POWER = 6.5; /**< Maximum allowed chipping power */ -constexpr double MIN_CHIP_POWER = 4.5; /**< Minimum allowed chipping power */ -// Max attempts before the force_kick_chip is set to true -constexpr double MAX_KICK_ATTEMPTS = 25; /**< Maximum allowed kicking attempts */ -constexpr double MAX_CHIP_ATTEMPTS = 25; /**< Maximum allowed chipping attempts */ - -/// Robot physical constants -constexpr double ROBOT_RADIUS = 0.088; /**< Radius of a robot */ -constexpr double CENTER_TO_FRONT = 0.069; /**< Distance from center of the robot to the front of the robot */ - -/// Dribbler constants -// The distance from robot to ball at which the dribbler should turn on -constexpr double TURN_ON_DRIBBLER_DISTANCE = 5 * ROBOT_RADIUS; /**< Distance from a robot to the ball at which the dribbler should turn on */ - -constexpr uint8_t MAX_ROBOT_COUNT = 11; /**< Maximum allowed number of robots */ - -/// Ball constants -constexpr double BALL_STILL_VEL = 0.1; /**< Velocity of the ball at which it is considered still */ -constexpr double BALL_STILL_VEL2 = BALL_STILL_VEL * BALL_STILL_VEL; /**< Squared velocity of the ball at which it is considered still */ -constexpr double BALL_GOT_SHOT_LIMIT = 0.6; /**< Velocity of the ball at which it is considered shot */ -constexpr double BALL_IS_MOVING_SLOW_LIMIT = 1; /**< Velocity of the ball at which it is considered moving slow */ -constexpr double BALL_RADIUS = 0.0215; /**< Radius of the ball */ -constexpr double HAS_CHIPPED_ERROR_MARGIN = 0.4; /**< Velocity margin for detecting ball chips */ -constexpr double ENEMY_CLOSE_TO_BALL_DISTANCE = 1.0; /**< Distance from the ball to an enemy robot at which the ball is considered close to the enemy */ - -/// RobotCommand limits -constexpr double MAX_DRIBBLER_CMD = 1; /**< Maximum allowed dribbler velocity that can be send to the robot */ -// Yaw increment per tick -constexpr double YAW_RATE = 0.2 * M_PI; /**< Maximum allowed angular velocity that can be send to the robot */ -constexpr double MAX_VEL_WHEN_HAS_BALL = 3.0; /**< Maximum allowed velocity that can be send to the robot when that robot has the ball */ - -/// GoToPos Constants -// Distance margin for 'goToPos'. If the robot is within this margin, goToPos is successful -constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot to be considered to have reached a position */ -// Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful -constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.01; /**< 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 - 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 */ -constexpr uint8_t FUZZY_FALSE = 0; /**< Value at which the fuzzy logic is considered 0% true */ -constexpr double FUZZY_DEFAULT_CUTOFF = 127; /**< Value at which the fuzzy logic is considered 50% true */ - -/// Distance constants -constexpr double ROBOT_CLOSE_TO_POINT = 0.2; /**< Distance from the robot to a position at which the robot is considered close to that position */ -constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */ -constexpr double OUT_OF_FIELD_MARGIN = 0.17; /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */ - -/// GameState constants -constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.1; /**< Minimum distance all robots should keep when avoiding the ball */ -constexpr double AVOID_BALL_DISTANCE_BEFORE_FREE_KICK = - 0.05 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */ - -/// Friction constants -constexpr static float SIMULATION_FRICTION = 0.71; /**< The expected movement friction of the ball during simulation */ -constexpr static float REAL_FRICTION = 0.44; /**< The expected movement friction of the ball on the field */ - -} // namespace rtt::ai::stp::control_constants - -#endif // RTT_CONTROLCONSTANTS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h deleted file mode 100644 index 6bd88162f..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef RTT_GENERALIZATIONCONSTANTS_H -#define RTT_GENERALIZATIONCONSTANTS_H - -#include - -#include -#include - -namespace rtt::ai::stp::gen { -/** - * @brief Struct that is used to store computations made with this module. - * Save computations here that are usable for each robot for a position. - * DO NOT save values specific to a robot in here (like TimeToPosition). - * If a position's score for a specific evaluation already had been computed in the tick, it will - * use that value instead of recomputing it. If it was not computed yet, it will compute and save it. - * @memberof scoreOpen : uint8_t score for the Openness of a position -> evaluations/position/OpennessEvaluation - * @memberof scoreLineOfSight : uint8_t score for the LineOfSight to a position from a position -> ../LineOfSightEvaluation - * @memberof scoreGoalShot : uint8_t score for the Goal Shot opportunity for a position -> ../GoalShotEvaluation - */ -struct PositionScores { - std::optional scoreOpen; - std::optional scoreLineOfSight; - std::optional scoreGoalShot; -}; - -/** - * @brief Combination of weights for each of the scores. - * This will be used to determine the final score for a robot for a position. - * All weights will be multiplied with the corresponding score and then normalized. - * @memberof weightOpen for scoreOpen - * @memberof weightLineOfSight for scoreLineOfSight - * @memberof weightGoalShot for scoreGoalShot - */ -struct ScoreProfile { - double weightOpen; - double weightLineOfSight; - double weightGoalShot; -}; - -/** - * @brief Structure with a position and its score - * @memberof position Vector2 coordinates of a position - * @memberof score The score for said position - */ -struct ScoredPosition { - Vector2 position; - uint8_t score; -}; - -/** - * Generalized Position Profiles to be used in plays. - * They consist of a generalized weight combination. - */ - -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 */ -constexpr ScoreProfile ChippingPass = {1, 0, 1}; /**< Scoring weights for ChippingPass score */ - -} // namespace rtt::ai::stp::gen -#endif // RTT_GENERALIZATIONCONSTANTS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.h deleted file mode 100644 index 3e5e7e362..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef RTT_KICKOFFUSORNORMALGAMESTATEEVALUATION_H -#define RTT_KICKOFFUSORNORMALGAMESTATEEVALUATION_H - -#include "stp/evaluations/BaseEvaluation.h" - -namespace rtt::ai::stp::evaluation { -/** - * @brief Class that evaluates the kick off us or normal game state - */ -class KickOffUsOrNormalGameStateEvaluation : public BaseEvaluation { - public: - /** - * @brief Calculates the score for the kick off us or normal game state - * @param world The current world - * @param field The current field - * @return The score of the kick off us or normal 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::KickOffUsOrNormal"; } -}; -} // namespace rtt::ai::stp::evaluation - -#endif // RTT_KICKOFFUSORNORMALGAMESTATEEVALUATION_H diff --git a/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.h b/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.h deleted file mode 100644 index 63d9b313f..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef RTT_NORMALORFREEKICKUSGAMESTATEEVALUATION_H -#define RTT_NORMALORFREEKICKUSGAMESTATEEVALUATION_H - -#include "stp/evaluations/BaseEvaluation.h" - -namespace rtt::ai::stp::evaluation { -/** - * @brief Class that evaluates the normal or free kick us game state - */ -class NormalOrFreeKickUsGameStateEvaluation : public BaseEvaluation { - public: - /** - * @brief Calculates the score for the normal or free kick us game state - * @param world The current world - * @param field The current field - * @return The score of the normal or free kick 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::NormalOrFreeKickUs"; } -}; -} // namespace rtt::ai::stp::evaluation - -#endif // RTT_NORMALORFREEKICKUSGAMESTATEEVALUATION_H diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/offensive/AttackingPass.h b/roboteam_ai/include/roboteam_ai/stp/plays/offensive/AttackingPass.h index 1dcc031fc..19be9f14c 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/offensive/AttackingPass.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/offensive/AttackingPass.h @@ -49,12 +49,6 @@ class AttackingPass : public Play { bool shouldEndPlay() noexcept override; private: - /** - * Counter that keeps track of how long another robot has a better interception. We need this counter. If not, as soon as we kicked the ball, another robot has a better - * intercept, while ballKicked() is still false, resulting in switching just after kicking the ball - */ - int endPlayCounter = 0; - /** * @brief Check if the ball has been kicked. True if passer is done with kicking, False if it is still ongoing * @return Boolean that tells whether the ball has been kicked diff --git a/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h b/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h index 2e38bed10..41768d6c2 100644 --- a/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h +++ b/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h @@ -20,9 +20,6 @@ class Chip : public Skill { * @return The name of this skill */ const char* getName() override; - - private: - int chipAttempts = 0; /**< Keeps track of how many ticks we tried to chip */ }; } // namespace rtt::ai::stp::skill diff --git a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h index c8adbc5aa..6c839e724 100644 --- a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h +++ b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h @@ -58,14 +58,6 @@ class KeeperBlockBall : public Tactic { */ static LineSegment getKeepersLineSegment(const Field &); - /** - * @brief Estimates the trajectory of the ball, either from current velocity or from enemies that might kick it - * @param ball to estimate its trajectory from - * @param enemyRobot an enemy that might manipulate the ball - * @return a trajectory the ball might take - */ - static std::optional estimateBallTrajectory(const world::view::BallView &ball, const std::optional &enemyRobot); - /** * @brief Checks if the given trajectory goes towards our goal * @param ballTrajectory the trajectory of the ball @@ -82,6 +74,30 @@ class KeeperBlockBall : public Tactic { */ static std::pair calculateTargetPosition(const StpInfo info) noexcept; + /** + * @brief Calculates the target position for the keeper when the ball is shot + * @param info the StpInfo struct + * @param keepersLineSegment the lineSegment of the goal + * @param ballTrajectory the trajectory of the ball + */ + static Vector2 calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept; + + /** + * @brief Calculates the point where they will have the ball + * @param info the StpInfo struct + * @param ballTrajectory the trajectory of the ball + * @return the point where they will have the ball + */ + static std::optional calculateTheirBallInterception(const StpInfo &info, rtt::LineSegment ballTrajectory) noexcept; + + /** + * @brief Calculates the target position for the keeper when the ball is not shot + * @param info the StpInfo struct + * @param predictedBallPositionTheirRobot the predicted position of the ball when the opponent will have the ball + * @return the target position for the keeper + */ + static Vector2 calculateTargetPositionBallNotShot(const StpInfo &info, std::optional predictedBallPositionTheirRobot) noexcept; + /** * @brief Calculates the yaw the robot should have * @param ball the ball for which the keeper should defend diff --git a/roboteam_ai/include/roboteam_ai/utilities/Constants.h b/roboteam_ai/include/roboteam_ai/utilities/Constants.h index 90336094c..dc2708641 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/Constants.h +++ b/roboteam_ai/include/roboteam_ai/utilities/Constants.h @@ -1,87 +1,181 @@ #ifndef ROBOTEAM_AI_CONSTANTS_H #define ROBOTEAM_AI_CONSTANTS_H -#include -#include - -#include "RuleSet.h" -#include "math.h" - -namespace rtt::ai { - -/** - * @brief Class that defines the constants used in this code. - * These constants are stored here so that we don't have to redefine them everytime we use them. Having them in one place also gives a lot of consistency. - */ -class Constants { - public: - static void init(); /**< Initializes the constants for either the simulator or basestation. */ - static bool FEEDBACK_ENABLED(); /**< Checks whether robot feedback is enabled */ - - /** - * @brief Indicates the maximum amount of time the AI can run without receiving a new world update - * @return The maximum amount of time - */ - static constexpr uint64_t WORLD_MAX_AGE_MILLISECONDS() { return 1000; } - - /** - * @brief Checks the amount of robots present - * @return The amount of robots - */ - static constexpr size_t ROBOT_COUNT() { return 11; }; - - /** - * @brief Checks the speed at which STP is running in ticks per second - * @return The tick rate of STP - */ - static constexpr int STP_TICK_RATE() { return 60; }; - - /** - * @brief Checks at which rate the AI should broadcast its settings - * @return The rate at which AI broadcasts its settings - */ - static constexpr int SETTINGS_BROADCAST_RATE() { return 1; } - - /// ROBOT COMMANDS /// - static double MAX_VEL_CMD(); /**< The maximum allowed velocity of the robot */ - static double MIN_YAW(); /**< The minimum yaw the robot can have */ - static double MAX_YAW(); /**< The maximum yaw the robot can have */ - static double MAX_ANGULAR_VELOCITY(); /**< The maximum angular velocity of the robot */ - static double MAX_ACC(); /**< Maximum acceleration of the robot */ - - /// ROBOT GEOMETRY /// - static constexpr double ROBOT_RADIUS() { return 0.089; }; /**< Radius of the robot */ - static constexpr double BALL_RADIUS() { return 0.0215; }; /**< Radius of the ball */ - static double HAS_BALL_DISTANCE(); /** The distance at which the robot is considered to have the ball */ - static double HAS_BALL_ANGLE(); /** The angle at which the robot is considered to have the ball */ - - /// REF STATES /// - static constexpr double MAX_VEL() { return 4.0; }; /**< Maximum allowed velocity */ - static int DEFAULT_KEEPER_ID(); /**< Default ID of the keeper */ - static double PENALTY_DISTANCE_BEHIND_BALL(); /**< The minimum distance the robots have to be behind the ball during a penalty */ - - static std::map ROBOTS_WITH_WORKING_DRIBBLER(); /**< Mapping of robots with working dribblers */ - static std::map ROBOTS_WITH_WORKING_BALL_SENSOR(); /**< Mapping of robots with working ball sensors */ - static std::map ROBOTS_WITH_WORKING_DRIBBLER_ENCODER(); /**< Mapping of robots with working dribbler encoders */ - static std::map ROBOTS_WITH_KICKER(); /**< Mapping of robots with working kicker */ - static std::map ROBOTS_MAXIMUM_KICK_TIME(); /**< Mapping robots with their respective time to charge the kicker */ - - static bool ROBOT_HAS_WORKING_DRIBBLER(int id); /**< Checks whether the given robot has a working dribbler */ - static bool ROBOT_HAS_WORKING_BALL_SENSOR(int id); /**< Checks whether the given robot has a working ball sensor */ - static bool ROBOT_HAS_WORKING_DRIBBLER_ENCODER(int id); /**< Checks whether the given robot has a working dribbler encoder */ - static bool ROBOT_HAS_KICKER(int id); /**< Checks whether the given robot has a working kicker */ - - static RuleSet RULESET_DEFAULT(); - static RuleSet RULESET_HALT(); - static RuleSet RULESET_STOP(); - - /** - * @brief Returns a vector of all the predefined rulesets - * @return a vector of all the predefined rulesets - */ - static std::vector ruleSets(); -}; - -} // namespace rtt::ai - -#endif // ROBOTEAM_AI_CONSTANTS_H +#include + +#include + +#include "utilities/GameSettings.h" + +namespace rtt::ai::constants { + +constexpr bool FEEDBACK_ENABLED = true; /**< Checks whether robot feedback is enabled */ +// Kick and chip constants +constexpr double MAX_KICK_POWER = 6.5; /**< Maximum allowed kicking power */ +constexpr double MIN_KICK_POWER = 3.0; /**< Minimum allowed kicking power */ +constexpr double MAX_CHIP_POWER = 6.5; /**< Maximum allowed chipping power */ +constexpr double MIN_CHIP_POWER = 4.5; /**< Minimum allowed chipping power */ + +/// Robot physical constants +constexpr double ROBOT_RADIUS = 0.089; /**< Radius of a robot */ +constexpr double CENTER_TO_FRONT = 0.069; /**< Distance from center of the robot to the front of the robot */ + +/// Dribbler constants +// The distance from robot to ball at which the dribbler should turn on +constexpr double TURN_ON_DRIBBLER_DISTANCE = 5 * ROBOT_RADIUS; /**< Distance from a robot to the ball at which the dribbler should turn on */ +constexpr uint8_t MAX_ROBOT_COUNT = 11; /**< Maximum allowed number of robots */ +constexpr uint64_t WORLD_MAX_AGE_MILLISECONDS = 1000; /**< Maximum amount of time the AI can run without receiving a new world update */ +constexpr int STP_TICK_RATE = 60; /**< The rate at which the STP runs in ticks per second */ +constexpr int SETTING_BROADCAST_RATE = 1; /**< The rate at which the AI broadcasts its settings */ + +/// Ball constants +constexpr double BALL_STILL_VEL = 0.1; /**< Velocity of the ball at which it is considered still */ +constexpr double BALL_STILL_VEL2 = BALL_STILL_VEL * BALL_STILL_VEL; /**< Squared velocity of the ball at which it is considered still */ +constexpr double BALL_GOT_SHOT_LIMIT = 0.6; /**< Velocity of the ball at which it is considered shot */ +constexpr double BALL_IS_MOVING_SLOW_LIMIT = 1; /**< Velocity of the ball at which it is considered moving slow */ +constexpr double BALL_RADIUS = 0.0215; /**< Radius of the ball */ +constexpr double HAS_CHIPPED_ERROR_MARGIN = 0.4; /**< Velocity margin for detecting ball chips */ +constexpr double ENEMY_CLOSE_TO_BALL_DISTANCE = 1.0; /**< Distance from the ball to an enemy robot at which the ball is considered close to the enemy */ +constexpr double HAS_BALL_ANGLE = 0.1 * M_PI; /**< Maximum angle between the robot and the ball at which the robot is considered to have the ball */ + +// Yaw increment per tick +constexpr double YAW_RATE = 0.2 * M_PI; /**< Maximum allowed angular velocity that can be send to the robot */ +constexpr double MAX_VEL_WHEN_HAS_BALL = 3.0; /**< Maximum allowed velocity that can be send to the robot when that robot has the ball */ +constexpr double MAX_ANGULAR_VELOCITY = 6.0; /**< Maximum allowed angular velocity that can be send to the robot */ +constexpr double MIN_YAW = -M_PI; /**< Minimum yaw the robot can have */ +constexpr double MAX_YAW = M_PI; /**< Maximum yaw the robot can have */ +constexpr double MAX_ACC = 3.5; /**< Maximum acceleration of the robot */ +constexpr double MAX_VEL = 4.0; /**< Maximum allowed velocity of the robot */ + +/// GoToPos Constants +// Distance margin for 'goToPos'. If the robot is within this margin, goToPos is successful +constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot to be considered to have reached a position */ +// Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful +constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.01; /**< 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 - 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 */ +constexpr uint8_t FUZZY_FALSE = 0; /**< Value at which the fuzzy logic is considered 0% true */ +constexpr double FUZZY_DEFAULT_CUTOFF = 127; /**< Value at which the fuzzy logic is considered 50% true */ + +/// Distance constants +constexpr double ROBOT_CLOSE_TO_POINT = 0.2; /**< Distance from the robot to a position at which the robot is considered close to that position */ +constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5; /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */ +constexpr double OUT_OF_FIELD_MARGIN = 0.17; /**< Distance that the center of the robot is allowed to go out of the field during play (not for end location, only for paths) */ +constexpr double FREE_KICK_TAKEN_DISTANCE = 0.07; /**< Distance that the ball must travel before we can say a kickoff or free kick has been taken */ +constexpr double PENALTY_DISTANCE_BEHIND_BALL = 1.5; /**< The minimum distance the robots have to be behind the ball during a penalty, default is 1 meter, but do 1.5 to be sure */ +; + +/// GameState constants +constexpr double AVOID_BALL_DISTANCE = 0.5 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.1; /**< Minimum distance all robots should keep when avoiding the ball */ +constexpr double AVOID_BALL_DISTANCE_BEFORE_FREE_KICK = + 0.05 + ROBOT_RADIUS + GO_TO_POS_ERROR_MARGIN + BALL_RADIUS + 0.01; /**< Minimum distance all robots should keep when avoiding the ball before a free kick */ + +/// Friction constants +constexpr static float SIMULATION_FRICTION = 0.71; /**< The expected movement friction of the ball during simulation */ +constexpr static float REAL_FRICTION = 0.44; /**< The expected movement friction of the ball on the field */ + +static inline double HAS_BALL_DISTANCE() { return (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? 0.11 : 0.12; } + +static std::map ROBOTS_WITH_WORKING_DRIBBLER() { + static std::map workingDribblerRobots; + workingDribblerRobots[0] = true; + workingDribblerRobots[1] = true; + workingDribblerRobots[2] = true; + workingDribblerRobots[3] = true; + workingDribblerRobots[4] = true; + workingDribblerRobots[5] = true; + workingDribblerRobots[6] = true; + workingDribblerRobots[7] = true; + workingDribblerRobots[8] = true; + workingDribblerRobots[9] = true; + workingDribblerRobots[10] = true; + workingDribblerRobots[11] = true; + workingDribblerRobots[12] = true; + workingDribblerRobots[13] = true; + workingDribblerRobots[14] = true; + workingDribblerRobots[15] = true; + + return workingDribblerRobots; +} + +static std::map ROBOTS_WITH_WORKING_BALL_SENSOR() { + static std::map workingBallSensorRobots; + workingBallSensorRobots[0] = false; + workingBallSensorRobots[1] = false; + workingBallSensorRobots[2] = false; + workingBallSensorRobots[3] = false; + workingBallSensorRobots[4] = false; + workingBallSensorRobots[5] = false; + workingBallSensorRobots[6] = false; + workingBallSensorRobots[7] = false; + workingBallSensorRobots[8] = false; + workingBallSensorRobots[9] = false; + workingBallSensorRobots[10] = false; + workingBallSensorRobots[11] = false; + workingBallSensorRobots[12] = false; + workingBallSensorRobots[13] = false; + workingBallSensorRobots[14] = false; + workingBallSensorRobots[15] = false; + + return workingBallSensorRobots; +} + +static std::map ROBOTS_WITH_WORKING_DRIBBLER_ENCODER() { + static std::map workingDribblerEncoderRobots; + workingDribblerEncoderRobots[0] = true; + workingDribblerEncoderRobots[1] = true; + workingDribblerEncoderRobots[2] = true; + workingDribblerEncoderRobots[3] = true; + workingDribblerEncoderRobots[4] = true; + workingDribblerEncoderRobots[5] = true; + workingDribblerEncoderRobots[6] = true; + workingDribblerEncoderRobots[7] = true; + workingDribblerEncoderRobots[8] = true; + workingDribblerEncoderRobots[9] = true; + workingDribblerEncoderRobots[10] = true; + workingDribblerEncoderRobots[11] = true; + workingDribblerEncoderRobots[12] = true; + workingDribblerEncoderRobots[13] = true; + workingDribblerEncoderRobots[14] = true; + workingDribblerEncoderRobots[15] = true; + + return workingDribblerEncoderRobots; +} + +static std::map ROBOTS_WITH_KICKER() { + static std::map kickerRobots; + kickerRobots[0] = true; + kickerRobots[1] = true; + kickerRobots[2] = true; + kickerRobots[3] = true; + kickerRobots[4] = true; + kickerRobots[5] = true; + kickerRobots[6] = true; + kickerRobots[7] = true; + kickerRobots[8] = true; + kickerRobots[9] = true; + kickerRobots[10] = true; + kickerRobots[11] = true; + kickerRobots[12] = true; + kickerRobots[13] = true; + kickerRobots[14] = true; + kickerRobots[15] = true; + + return kickerRobots; +} + +[[maybe_unused]] static bool ROBOT_HAS_WORKING_BALL_SENSOR(int id) { return ROBOTS_WITH_WORKING_BALL_SENSOR()[id]; } + +[[maybe_unused]] static bool ROBOT_HAS_WORKING_DRIBBLER(int id) { return ROBOTS_WITH_WORKING_DRIBBLER()[id]; } + +[[maybe_unused]] static bool ROBOT_HAS_WORKING_DRIBBLER_ENCODER(int id) { return ROBOTS_WITH_WORKING_DRIBBLER_ENCODER()[id]; } + +[[maybe_unused]] static bool ROBOT_HAS_KICKER(int id) { return ROBOTS_WITH_KICKER()[id]; } + +} // namespace rtt::ai::constants + +#endif // RTT_CONSTANTS_H diff --git a/roboteam_ai/include/roboteam_ai/utilities/GameState.h b/roboteam_ai/include/roboteam_ai/utilities/GameState.h index c0d49a015..021b5e86c 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/GameState.h +++ b/roboteam_ai/include/roboteam_ai/utilities/GameState.h @@ -3,10 +3,12 @@ #include +#include + #include "Constants.h" #include "RefCommand.h" #include "RuleSet.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai { @@ -19,10 +21,11 @@ struct GameState { RuleSet ruleSet; bool isFollowUpCommand; RefCommand followUpCommandId; - int keeperId = Constants::DEFAULT_KEEPER_ID(); - int maxAllowedRobots = stp::control_constants::MAX_ROBOT_COUNT; + int keeperId = -1; + int maxAllowedRobots = constants::MAX_ROBOT_COUNT; static int cardId; static double timeLeft; + static std::optional kickPoint; GameState() = default; diff --git a/roboteam_ai/include/roboteam_ai/utilities/RuleSet.h b/roboteam_ai/include/roboteam_ai/utilities/RuleSet.h index c7f23f5f8..8cc2b2bd2 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/RuleSet.h +++ b/roboteam_ai/include/roboteam_ai/utilities/RuleSet.h @@ -22,7 +22,7 @@ struct RuleSet { * @param title Title of the RuleSet * @param maxRobotVel Maximum allowed velocities for robots */ - RuleSet(RuleSetName title, double maxRobotVel) : title(std::move(title)), maxRobotVel(maxRobotVel) {} + constexpr RuleSet(RuleSetName title, double maxRobotVel) : title(std::move(title)), maxRobotVel(maxRobotVel) {} /** * @brief Getter for the title of the RuleSet @@ -49,6 +49,18 @@ struct RuleSet { } } + static constexpr RuleSet RULESET_DEFAULT() { return {RuleSetName::DEFAULT, 4.0}; } + static constexpr RuleSet RULESET_HALT() { return {RuleSetName::HALT, 0.0}; } + static constexpr RuleSet RULESET_STOP() { return {RuleSetName::STOP, 1.3}; } + + static constexpr std::array ruleSets() { + return { + RULESET_DEFAULT(), + RULESET_HALT(), + RULESET_STOP(), + }; + } + private: RuleSetName title; double maxRobotVel; diff --git a/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h b/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h index 40f204beb..a91d5646b 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h +++ b/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h @@ -1,8 +1,7 @@ #ifndef RTT_STPINFOENUMS_H #define RTT_STPINFOENUMS_H -#include "stp/constants/ControlConstants.h" - +#include namespace rtt::ai::stp { /** * @brief Whether this robot should kick or chip in the shoot skill diff --git a/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h b/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h index 0a4f82254..cd88ab3f8 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h +++ b/roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h @@ -8,6 +8,7 @@ #include "Constants.h" #include "GameState.h" +#include "RuleSet.h" #include "world/views/WorldDataView.hpp" namespace rtt::ai { @@ -54,32 +55,32 @@ class StrategyManager { * @brief Vector containing all possible game states */ const std::vector gameStates = { - GameState(RefCommand::UNDEFINED, Constants::RULESET_HALT()), - GameState(RefCommand::HALT, Constants::RULESET_HALT()), - GameState(RefCommand::TIMEOUT_US, Constants::RULESET_HALT()), - GameState(RefCommand::TIMEOUT_THEM, Constants::RULESET_HALT()), + GameState(RefCommand::UNDEFINED, RuleSet::RULESET_HALT()), + GameState(RefCommand::HALT, RuleSet::RULESET_HALT()), + GameState(RefCommand::TIMEOUT_US, RuleSet::RULESET_HALT()), + GameState(RefCommand::TIMEOUT_THEM, RuleSet::RULESET_HALT()), - GameState(RefCommand::STOP, Constants::RULESET_STOP()), + GameState(RefCommand::STOP, RuleSet::RULESET_STOP()), // Default to have a higher max velocity. Ball will still be avoided by ballplacementarea avoidance - GameState(RefCommand::BALL_PLACEMENT_THEM, Constants::RULESET_DEFAULT()), - GameState(RefCommand::BALL_PLACEMENT_US, Constants::RULESET_DEFAULT()), - GameState(RefCommand::BALL_PLACEMENT_US_DIRECT, Constants::RULESET_DEFAULT()), - 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), - GameState(RefCommand::PREPARE_PENALTY_THEM, Constants::RULESET_STOP(), false, RefCommand::PENALTY_THEM), - GameState(RefCommand::PREPARE_FORCED_START, Constants::RULESET_STOP()), + GameState(RefCommand::BALL_PLACEMENT_THEM, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::BALL_PLACEMENT_US, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::BALL_PLACEMENT_US_DIRECT, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::DIRECT_FREE_THEM_STOP, RuleSet::RULESET_STOP()), + GameState(RefCommand::DIRECT_FREE_US_STOP, RuleSet::RULESET_STOP()), + GameState(RefCommand::PREPARE_KICKOFF_US, RuleSet::RULESET_STOP(), false, RefCommand::KICKOFF_US), + GameState(RefCommand::PREPARE_KICKOFF_THEM, RuleSet::RULESET_STOP(), false, RefCommand::KICKOFF_THEM), + GameState(RefCommand::PREPARE_PENALTY_US, RuleSet::RULESET_STOP(), false, RefCommand::PENALTY_US), + GameState(RefCommand::PREPARE_PENALTY_THEM, RuleSet::RULESET_STOP(), false, RefCommand::PENALTY_THEM), + GameState(RefCommand::PREPARE_FORCED_START, RuleSet::RULESET_STOP()), - GameState(RefCommand::DIRECT_FREE_THEM, Constants::RULESET_DEFAULT()), - GameState(RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()), - GameState(RefCommand::FORCED_START, Constants::RULESET_DEFAULT()), - GameState(RefCommand::DIRECT_FREE_US, Constants::RULESET_DEFAULT()), - GameState(RefCommand::KICKOFF_US, Constants::RULESET_DEFAULT(), true), - GameState(RefCommand::KICKOFF_THEM, Constants::RULESET_DEFAULT(), true), - GameState(RefCommand::PENALTY_US, Constants::RULESET_DEFAULT(), true), - GameState(RefCommand::PENALTY_THEM, Constants::RULESET_DEFAULT(), true), + GameState(RefCommand::DIRECT_FREE_THEM, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::FORCED_START, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::DIRECT_FREE_US, RuleSet::RULESET_DEFAULT()), + GameState(RefCommand::KICKOFF_US, RuleSet::RULESET_DEFAULT(), true), + GameState(RefCommand::KICKOFF_THEM, RuleSet::RULESET_DEFAULT(), true), + GameState(RefCommand::PENALTY_US, RuleSet::RULESET_DEFAULT(), true), + GameState(RefCommand::PENALTY_THEM, RuleSet::RULESET_DEFAULT(), true), }; GameState currentGameState = gameStates[0]; /**< Current game state according to the referee, after the StrategyManager has processed it */ RefCommand lastCommand = RefCommand::UNDEFINED; /**< Last command given by the referee */ diff --git a/roboteam_ai/include/roboteam_ai/world/FieldComputations.h b/roboteam_ai/include/roboteam_ai/world/FieldComputations.h index c1cfc220a..223448738 100644 --- a/roboteam_ai/include/roboteam_ai/world/FieldComputations.h +++ b/roboteam_ai/include/roboteam_ai/world/FieldComputations.h @@ -3,13 +3,13 @@ #include #include +#include #include #include #include #include "control/ControlUtils.h" -#include "stp/constants/ControlConstants.h" #include "utilities/StpInfoEnums.h" #include "views/WorldDataView.hpp" @@ -26,7 +26,6 @@ namespace rtt::ai { class FieldComputations { private: static constexpr double NEGLIGIBLE_LENGTH = 0.000001; /**< If a line length is below or equal to this threshold then is it neglected during determining the blockades. */ - static constexpr double PROJECTION_MARGIN = 0.02; /**< Some extra padding is added when projecting positions,so that the projected position is not on lines/intersections */ public: /** @@ -63,7 +62,7 @@ class FieldComputations { * @param fieldMargin The outwards margin in which the field area will get expanded/shrunk in all directions. * @return True if the point is not within any area that has to be avoided according to avoidObjects, note that this does not take ball avoidance into consideration */ - static bool pointIsValidPosition(const rtt::Field &field, const Vector2 &point, stp::AvoidObjects avoidObjects = {}, double fieldMargin = 0.0); + static bool pointIsValidPosition(const rtt::Field &field, const Vector2 &point, stp::AvoidObjects avoidObjects, double fieldMargin = 0.0); /** * @brief Project given position to within the field with a certain margin diff --git a/roboteam_ai/src/STPManager.cpp b/roboteam_ai/src/STPManager.cpp index b20ca4a23..56423ac6e 100644 --- a/roboteam_ai/src/STPManager.cpp +++ b/roboteam_ai/src/STPManager.cpp @@ -120,14 +120,14 @@ void STPManager::start(std::atomic_flag &exitApplication) { // If this is primary AI, broadcast settings every second if (GameSettings::isPrimaryAI()) { - stpTimer.limit([&]() { io::io.publishSettings(); }, ai::Constants::SETTINGS_BROADCAST_RATE()); + stpTimer.limit([&]() { io::io.publishSettings(); }, ai::constants::SETTING_BROADCAST_RATE); } if (exitApplication.test()) { stpTimer.stop(); } }, - ai::Constants::STP_TICK_RATE()); + ai::constants::STP_TICK_RATE); } /// Run everything with regard to behaviour trees @@ -183,7 +183,7 @@ void STPManager::decidePlay(world::World *_world, bool ignoreWorldAge) { /* Check if world is not too old. Can be ignored, when e.g. running the debugger */ if (!ignoreWorldAge) { - if (ai::Constants::WORLD_MAX_AGE_MILLISECONDS() < rtt::ai::io::io.getStateAgeMs()) { + if (ai::constants::WORLD_MAX_AGE_MILLISECONDS < rtt::ai::io::io.getStateAgeMs()) { RTT_WARNING("World is too old! Age: ", rtt::ai::io::io.getStateAgeMs(), " ms") currentPlay = nullptr; // Returning here prevents the play from being updated, which means that the play will not be able to send any commands, diff --git a/roboteam_ai/src/control/ControlModule.cpp b/roboteam_ai/src/control/ControlModule.cpp index 9e0d7aa8f..77f0ed143 100644 --- a/roboteam_ai/src/control/ControlModule.cpp +++ b/roboteam_ai/src/control/ControlModule.cpp @@ -20,9 +20,7 @@ void ControlModule::limitRobotCommand(rtt::RobotCommand& command, rtt::world::vi limitAngularVel(command, robot); } -void ControlModule::limitVel(rtt::RobotCommand& command) { - command.velocity = command.velocity.stretchToLength(std::clamp(command.velocity.length(), 0.0, Constants::MAX_VEL_CMD())); -} +void ControlModule::limitVel(rtt::RobotCommand& command) { command.velocity = command.velocity.stretchToLength(std::clamp(command.velocity.length(), 0.0, constants::MAX_VEL)); } 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 yaw in small steps @@ -36,12 +34,12 @@ void ControlModule::limitAngularVel(rtt::RobotCommand& command, rtt::world::view } // If the yaw error is larger than the desired yaw rate, adjust the yaw command - if (robotYaw.shortestAngleDiff(yaw) > stp::control_constants::YAW_RATE) { + if (robotYaw.shortestAngleDiff(yaw) > constants::YAW_RATE) { // Determine direction of rotation (shortest distance) int direction = Angle(robotYaw).rotateDirection(yaw) ? 1 : -1; // Set the yaw command to the current robot yaw + the yaw rate - command.yaw = robotYaw + Angle(direction * stp::control_constants::YAW_RATE); + command.yaw = robotYaw + Angle(direction * constants::YAW_RATE); } } // TODO: Well, then also limit the target angular velocity just like target yaw! @@ -84,7 +82,7 @@ void ControlModule::simulator_angular_control(const std::optional<::rtt::world:: double I = 0.0; double D = 0; double max_ang_vel = 5.0; // rad/s - double dt = 1. / double(Constants::STP_TICK_RATE()); + double dt = 1. / double(constants::STP_TICK_RATE); YawPID pid(P, I, D, max_ang_vel, dt); ang_velocity_out = pid.getOutput(target_yaw, current_yaw); diff --git a/roboteam_ai/src/control/ControlUtils.cpp b/roboteam_ai/src/control/ControlUtils.cpp index efac5ba92..90a8014d1 100644 --- a/roboteam_ai/src/control/ControlUtils.cpp +++ b/roboteam_ai/src/control/ControlUtils.cpp @@ -4,7 +4,7 @@ #include -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" #include "utilities/GameStateManager.hpp" #include "utilities/StpInfoEnums.h" #include "world/World.hpp" @@ -13,46 +13,13 @@ namespace rtt::ai::control { double ControlUtils::getMaxVelocity(bool hasBall) { double maxVel = rtt::ai::GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); - if (hasBall) maxVel = std::min(stp::control_constants::MAX_VEL_WHEN_HAS_BALL, maxVel); + if (hasBall) maxVel = std::min(constants::MAX_VEL_WHEN_HAS_BALL, maxVel); return maxVel; } -/// Limits velocity to maximum velocity. it defaults to the max velocity stored in Referee. -Vector2 ControlUtils::velocityLimiter(const Vector2 &vel, double maxVel, double minVel, bool listenToReferee) { - if (listenToReferee) { - double refereeMaxVel = rtt::ai::GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); - if (refereeMaxVel < maxVel) { - maxVel = refereeMaxVel; - } - } - - if (vel.length() > maxVel) { - return vel.stretchToLength(maxVel); - } else if (vel.length() < minVel) { - return vel.stretchToLength(minVel); - } - return vel; -} - -/// Calculate the force of a given vector + a certain type. -/// the basic formula is: force = weight/distance^2 * unit vector -Vector2 ControlUtils::calculateForce(const Vector2 &vector, double weight, double minDistance) { - // if the object is close enough, it's forces should affect. Otherwise don't change anything. - if (vector.length() < minDistance && vector.length2() > 0) { - return vector.normalize() * (weight / vector.length2()); - } - return {0, 0}; -} - -bool ControlUtils::objectVelocityAimedToPoint(const Vector2 &objectPosition, const Vector2 &velocity, const Vector2 &point, double maxDifference) { - double exactAngleTowardsPoint = (point - objectPosition).angle(); - - // Note: The angles should NOT be constrained here. This is necessary. - return (velocity.length() > 0 && velocity.angle() > exactAngleTowardsPoint - maxDifference / 2 && velocity.angle() < exactAngleTowardsPoint + maxDifference / 2); -} /// Calculate the kick force double ControlUtils::determineKickForce(const double distance, stp::ShotType shotType) noexcept { - if (shotType == stp::ShotType::MAX) return stp::control_constants::MAX_KICK_POWER; + if (shotType == stp::ShotType::MAX) return constants::MAX_KICK_POWER; double kickForce; if (shotType == stp::ShotType::PASS) { @@ -67,7 +34,7 @@ double ControlUtils::determineKickForce(const double distance, stp::ShotType sho auto velocity = distance * kickForce; // Make sure velocity is always between MIN_KICK_POWER and MAX_KICK_POWER - return std::clamp(velocity, stp::control_constants::MIN_KICK_POWER, stp::control_constants::MAX_KICK_POWER); + return std::clamp(velocity, constants::MIN_KICK_POWER, constants::MAX_KICK_POWER); } /// Calculates the chip force double ControlUtils::determineChipForce(const double distance) noexcept { @@ -76,6 +43,6 @@ double ControlUtils::determineChipForce(const double distance) noexcept { // Calculate the velocity based on this function with the previously set limitingFactor auto velocity = distance * chipFactor; // Make sure velocity is always between MIN_CHIP_POWER and MAX_CHIP_POWER - return std::clamp(velocity, stp::control_constants::MIN_CHIP_POWER, stp::control_constants::MAX_CHIP_POWER); + return std::clamp(velocity, constants::MIN_CHIP_POWER, constants::MAX_CHIP_POWER); } } // namespace rtt::ai::control diff --git a/roboteam_ai/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp b/roboteam_ai/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp index 4c020419e..c65a83b14 100644 --- a/roboteam_ai/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp +++ b/roboteam_ai/src/control/positionControl/BBTrajectories/BBTrajectory2D.cpp @@ -3,6 +3,7 @@ #include #include "utilities/Constants.h" + namespace rtt::ai::control { BBTrajectory2D::BBTrajectory2D(const Vector2 &initialPos, const Vector2 &initialVel, const Vector2 &finalPos, double maxVel, double maxAcc) { diff --git a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp index 484dafdb9..b7c3adf83 100644 --- a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp +++ b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp @@ -2,7 +2,7 @@ #include -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" #include "world/World.hpp" #include "world/WorldData.hpp" @@ -16,22 +16,22 @@ double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajec const auto &ourDefenseArea = FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1); const auto &theirDefenseArea = FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1); - auto leftGoalTopPost = LineSegment(field.leftGoalArea.topLeft(), field.leftGoalArea.topRight()); - auto leftGoalBottomPost = LineSegment(field.leftGoalArea.bottomLeft(), field.leftGoalArea.bottomRight()); - auto leftGoalBackPost = LineSegment(field.leftGoalArea.topLeft(), field.leftGoalArea.bottomLeft()); - auto rightGoalTopPost = LineSegment(field.rightGoalArea.topLeft(), field.rightGoalArea.topRight()); - auto rightGoalBottomPost = LineSegment(field.rightGoalArea.bottomLeft(), field.rightGoalArea.bottomRight()); - auto rightGoalBackPost = LineSegment(field.rightGoalArea.topRight(), field.rightGoalArea.bottomRight()); + auto leftGoalTopPost = field.leftGoalArea.topLine(); + auto leftGoalBottomPost = field.leftGoalArea.bottomLine(); + auto leftGoalBackPost = field.leftGoalArea.leftLine(); + auto rightGoalTopPost = field.rightGoalArea.topLine(); + auto rightGoalBottomPost = field.rightGoalArea.bottomLine(); + auto rightGoalBackPost = field.rightGoalArea.rightLine(); for (int checkPoint = 1; checkPoint < static_cast(maxCheckPoints); checkPoint += 1) { auto pathLine = LineSegment(pathPoints[checkPoint - 1], pathPoints[checkPoint]); if (avoidObjects.shouldAvoidGoalPosts) { - if (pathLine.closestDistanceToLineSegment(leftGoalTopPost) < Constants::ROBOT_RADIUS() || - pathLine.closestDistanceToLineSegment(leftGoalBottomPost) < Constants::ROBOT_RADIUS() || - pathLine.closestDistanceToLineSegment(leftGoalBackPost) < Constants::ROBOT_RADIUS() || - pathLine.closestDistanceToLineSegment(rightGoalTopPost) < Constants::ROBOT_RADIUS() || - pathLine.closestDistanceToLineSegment(rightGoalBottomPost) < Constants::ROBOT_RADIUS() || - pathLine.closestDistanceToLineSegment(rightGoalBackPost) < Constants::ROBOT_RADIUS()) { + if (pathLine.closestDistanceToLineSegment(leftGoalTopPost) < constants::ROBOT_RADIUS || + pathLine.closestDistanceToLineSegment(leftGoalBottomPost) < constants::ROBOT_RADIUS || + pathLine.closestDistanceToLineSegment(leftGoalBackPost) < constants::ROBOT_RADIUS || + pathLine.closestDistanceToLineSegment(rightGoalTopPost) < constants::ROBOT_RADIUS || + pathLine.closestDistanceToLineSegment(rightGoalBottomPost) < constants::ROBOT_RADIUS || + pathLine.closestDistanceToLineSegment(rightGoalBackPost) < constants::ROBOT_RADIUS) { return checkPoint * 0.1; } } @@ -40,13 +40,13 @@ double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajec return checkPoint * 0.1; } } - if (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > stp::control_constants::ROBOT_RADIUS + stp::control_constants::GO_TO_POS_ERROR_MARGIN) { + if (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN) { if (theirDefenseArea.contains(pathPoints[checkPoint]) || theirDefenseArea.contains(pathPoints[checkPoint - 1]) || theirDefenseArea.doesIntersect(pathLine)) { return checkPoint * 0.1; } } if (avoidObjects.shouldAvoidOutOfField) { - if (!field.playArea.contains(pathPoints[checkPoint], stp::control_constants::OUT_OF_FIELD_MARGIN)) { + if (!field.playArea.contains(pathPoints[checkPoint], constants::OUT_OF_FIELD_MARGIN)) { return checkPoint * 0.1; } } @@ -84,7 +84,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory // If the path of the other robot is not computed, we assume it is not moving if (computedPathsIt == computedPaths.end()) { const Vector2 &ourOtherRobotPos = ourOtherRobot->getPos(); - if ((ourOtherRobotPos - positionOurRobot).length() < 2 * Constants::ROBOT_RADIUS() + additionalMargin) { + if ((ourOtherRobotPos - positionOurRobot).length() < 2 * constants::ROBOT_RADIUS + additionalMargin) { return checkPoint * 0.1; } } else { @@ -94,7 +94,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory } else { pathLineOtherRobot = LineSegment(computedPathsIt->second.back(), computedPathsIt->second.back()); } - if (pathLineOtherRobot.closestDistanceToLineSegment(pathLine) < 2 * Constants::ROBOT_RADIUS()) { + if (pathLineOtherRobot.closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS) { return checkPoint * 0.1; } } @@ -103,14 +103,14 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory if (velocityOurRobot > 0.7 && avoidObjects.shouldAvoidTheirRobots) { if (std::any_of(theirRobots.begin(), theirRobots.end(), [&](const auto &theirRobot) { return LineSegment(theirRobot->getPos() + theirRobot->getVel() * 0.1 * (checkPoint - 1), theirRobot->getPos() + theirRobot->getVel() * 0.1 * checkPoint) - .closestDistanceToLineSegment(pathLine) < 2 * Constants::ROBOT_RADIUS() + additionalMargin; + .closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS + additionalMargin; })) { return checkPoint * 0.1; } } if (avoidObjects.shouldAvoidBall) { auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1); - if ((ballPosition - positionOurRobot).length() < stp::control_constants::AVOID_BALL_DISTANCE + additionalMargin) { + if ((ballPosition - positionOurRobot).length() < constants::AVOID_BALL_DISTANCE + additionalMargin) { return checkPoint * 0.1; } } @@ -119,7 +119,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1); auto ballPlacementPosition = GameStateManager::getRefereeDesignatedPosition(); LineSegment ballPlacementLine(ballPosition, ballPlacementPosition); - if (ballPlacementLine.distanceToLine(positionOurRobot) < stp::control_constants::AVOID_BALL_DISTANCE + additionalMargin) { + if (ballPlacementLine.distanceToLine(positionOurRobot) < constants::AVOID_BALL_DISTANCE + additionalMargin) { return checkPoint * 0.1; } } diff --git a/roboteam_ai/src/control/positionControl/OvershootComputations.cpp b/roboteam_ai/src/control/positionControl/OvershootComputations.cpp index 49f4927b3..7ef087b51 100644 --- a/roboteam_ai/src/control/positionControl/OvershootComputations.cpp +++ b/roboteam_ai/src/control/positionControl/OvershootComputations.cpp @@ -4,16 +4,17 @@ #include #include "utilities/Constants.h" + namespace rtt::ai::control { -Vector2 OvershootComputations::overshootingDestination(const Vector2& startPosition, const Vector2& endPosition, const Vector2& startVelocity, const double maxVelocity, - const double maxAcceleration, const double targetTime) { +std::pair OvershootComputations::overshootingDestination(const Vector2& startPosition, const Vector2& endPosition, const Vector2& startVelocity, + const double maxVelocity, const double maxAcceleration, const double targetTime) { Vector2 distance = endPosition - startPosition; double increment = M_PI_4 * 0.5; double alpha = M_PI_4; - TimedPos1D x{0, 0}; - TimedPos1D y{0, 0}; + TimedPos1D x{0, 0, 0}; + TimedPos1D y{0, 0, 0}; while (increment > 1e-7) { auto cosAlpha = std::cos(alpha); @@ -28,7 +29,7 @@ Vector2 OvershootComputations::overshootingDestination(const Vector2& startPosit increment *= 0.5; } - return Vector2(x.pos + startPosition.x, y.pos + startPosition.y); + return {Vector2(x.pos + startPosition.x, y.pos + startPosition.y), std::max(x.timeToTarget, y.timeToTarget)}; } double OvershootComputations::slowestDirectTime(double distance, double initialVelocity, double maxAcceleration) { @@ -59,48 +60,54 @@ std::optional OvershootComputations::fastestDirectTrapezoidal(double double constantVelocityTimeTooSlow = remainingDistance / maxVelocity; if (accelerationTime + constantVelocityTimeTooSlow >= targetTime) { - return std::make_optional(TimedPos1D(distance + decelerationDistance, accelerationTime + constantVelocityTimeTooSlow + decelerationTime)); + // Target reached after the target time, 'Trapezoidal too slow' + return std::make_optional( + TimedPos1D(distance + decelerationDistance, accelerationTime + constantVelocityTimeTooSlow + decelerationTime, accelerationTime + constantVelocityTimeTooSlow)); } double constantVelocityDistanceEarly = remainingDistance - decelerationDistance; double constantVelocityTimeEarly = constantVelocityDistanceEarly / maxVelocity; if (constantVelocityTimeEarly >= 0.0 && accelerationTime + constantVelocityTimeEarly + decelerationTime <= targetTime) { - return std::make_optional(TimedPos1D(distance, accelerationTime + constantVelocityTimeEarly + decelerationTime)); + // Target reached before the target time, 'Trapezoidal finishing early' + // Robot is already at a complete stop before the target time + return std::make_optional( + TimedPos1D(distance, accelerationTime + constantVelocityTimeEarly + decelerationTime, accelerationTime + constantVelocityTimeEarly + decelerationTime)); } double remainingTime = targetTime - accelerationTime; double decelerationTimeDirect = std::sqrt(2.0 * (remainingDistance - remainingTime * maxVelocity) / deceleration); double constantVelocityTimeDirect = remainingTime - decelerationTimeDirect; - if (constantVelocityTimeDirect > 0.0 && decelerationTimeDirect < decelerationTime) { + if (constantVelocityTimeDirect >= 0.0 && decelerationTimeDirect <= decelerationTime) { + // Target reached at the target time, 'Trapezoidal direct hit' double finalVelocity = maxVelocity + deceleration * decelerationTimeDirect; double finalDecelerationTime = -finalVelocity / deceleration; - return std::make_optional(TimedPos1D(distance + 0.5 * finalVelocity * finalDecelerationTime, targetTime + finalDecelerationTime)); + return std::make_optional(TimedPos1D(distance + 0.5 * finalVelocity * finalDecelerationTime, targetTime + finalDecelerationTime, targetTime)); } return std::nullopt; } TimedPos1D OvershootComputations::fastestDirectTriangular(double distance, double initialVelocity, double maxVelocity, double maxAcceleration, double deceleration, double targetTime) { - if ((maxVelocity >= 0.0) == (initialVelocity >= maxVelocity)) { - double time = -initialVelocity / deceleration; - return TimedPos1D(0.5 * initialVelocity * time, time); - } + // The "Straight Too Slow" case from the TDP is not needed, as it's already covered in getTimedPos1D, at the start of the function. It's almost the same as the "Straight Too + // Slow" from the TDP, except that it's a bit more flexible and allows easier return of timeToTarget, which we need for our keeper extension. double acceleration = -deceleration; double sqrtTooSlow = std::sqrt(2.0 * acceleration * distance + initialVelocity * initialVelocity); double accelerationTimeTooSlow = (maxVelocity >= 0.0) ? ((-initialVelocity + sqrtTooSlow) / acceleration) : ((-initialVelocity - sqrtTooSlow) / acceleration); if (accelerationTimeTooSlow >= targetTime) { + // "Triangular too slow", robot can not reach the target in time, does full acceleration till target position double finalVelocityTooSlow = initialVelocity + acceleration * accelerationTimeTooSlow; double decelerationTimeTooSlow = std::abs(finalVelocityTooSlow / acceleration); - return TimedPos1D(distance + 0.5 * finalVelocityTooSlow * decelerationTimeTooSlow, accelerationTimeTooSlow + decelerationTimeTooSlow); + return TimedPos1D(distance + 0.5 * finalVelocityTooSlow * decelerationTimeTooSlow, accelerationTimeTooSlow + decelerationTimeTooSlow, accelerationTimeTooSlow); } - double sqEarly = ((distance * acceleration) + (0.5 * initialVelocity * initialVelocity)) / (maxAcceleration * maxAcceleration); + double sqEarly = (distance * acceleration + 0.5 * initialVelocity * initialVelocity) / (maxAcceleration * maxAcceleration); double decelerationTimeEarly = sqEarly > 0.0 ? std::sqrt(sqEarly) : 0.0; double finalVelocityEarly = acceleration * decelerationTimeEarly; double accelerationTimeEarly = (finalVelocityEarly - initialVelocity) / acceleration; if (accelerationTimeEarly + decelerationTimeEarly <= targetTime) { - return TimedPos1D(distance, accelerationTimeEarly + decelerationTimeEarly); + // "Triangular finishing early", robot reaches target before target time, comes to a full stop before target time + return TimedPos1D(distance, accelerationTimeEarly + decelerationTimeEarly, accelerationTimeEarly + decelerationTimeEarly); } double sqDirect = std::sqrt(2.0 * acceleration * (acceleration * targetTime * targetTime - 2.0 * distance + 2.0 * targetTime * initialVelocity)); @@ -109,10 +116,21 @@ TimedPos1D OvershootComputations::fastestDirectTriangular(double distance, doubl double remainingTimeDirect = finalVelocityDirect / acceleration; double accelerationDistanceDirect = 0.5 * (initialVelocity + finalVelocityDirect) * accelerationTimeDirect; double remainingDistanceDirect = 0.5 * finalVelocityDirect * remainingTimeDirect; - return TimedPos1D(accelerationDistanceDirect + remainingDistanceDirect, accelerationTimeDirect + remainingTimeDirect); + // "Triangular direct hit", robot reaches target at target time while still moving + return TimedPos1D(accelerationDistanceDirect + remainingDistanceDirect, accelerationTimeDirect + remainingTimeDirect, targetTime); } TimedPos1D OvershootComputations::getTimedPos1D(double distance, double initialVelocity, double maxVelocity, double maxAcceleration, double targetTime) { + if (abs(initialVelocity) > maxVelocity) { + double breakingTimeToMaxVel = ((abs(initialVelocity) - maxVelocity) / maxAcceleration); + double breakingDistanceToMaxVel = 0.5 * (abs(initialVelocity) + maxVelocity) * breakingTimeToMaxVel; + if (initialVelocity < 0.0) { + breakingDistanceToMaxVel *= -1.0; + } + auto newTimedPos1D = getTimedPos1D(distance - breakingDistanceToMaxVel, maxVelocity, maxVelocity, maxAcceleration, targetTime - breakingTimeToMaxVel); + return TimedPos1D(newTimedPos1D.pos + breakingDistanceToMaxVel, newTimedPos1D.time + breakingTimeToMaxVel, newTimedPos1D.timeToTarget + breakingTimeToMaxVel); + } + targetTime = std::min(targetTime, 10.0); double deceleration = (initialVelocity >= 0.0) ? -maxAcceleration : maxAcceleration; double zeroVelocityDistance = 0.5 * initialVelocity * (-initialVelocity / deceleration); @@ -124,7 +142,7 @@ TimedPos1D OvershootComputations::getTimedPos1D(double distance, double initialV } else { double breakingTime = std::abs(initialVelocity / maxAcceleration); TimedPos1D timed = fastestDirect(distance - zeroVelocityDistance, 0.0, -maxFinalVelocity, maxAcceleration, targetTime - breakingTime); - return TimedPos1D(timed.pos + zeroVelocityDistance, timed.time + breakingTime); + return TimedPos1D(timed.pos + zeroVelocityDistance, timed.time + breakingTime, timed.timeToTarget + breakingTime); } } } // namespace rtt::ai::control \ No newline at end of file diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 513baf3bc..6f64a6d53 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -14,22 +14,22 @@ Vector2 PositionControl::computeAndTrackTrajectory(const world::World *world, co Vector2 targetPosition, double maxRobotVelocity, stp::AvoidObjects avoidObjects) { double timeStep = 0.1; auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = FieldComputations::getDefenseAreaMargin(); - if (avoidObjects.shouldAvoidBall && (currentPosition - world->getWorld()->getBall()->get()->position).length() < ai::stp::control_constants::AVOID_BALL_DISTANCE) { + if (avoidObjects.shouldAvoidBall && (currentPosition - world->getWorld()->getBall()->get()->position).length() < ai::constants::AVOID_BALL_DISTANCE) { targetPosition = handleBallCollision(world, field, currentPosition, avoidObjects); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); } else if ((GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM || GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) && LineSegment(world->getWorld()->getBall()->get()->position, GameStateManager::getRefereeDesignatedPosition()).distanceToLine(currentPosition) < - ai::stp::control_constants::AVOID_BALL_DISTANCE) { + ai::constants::AVOID_BALL_DISTANCE) { targetPosition = handleBallPlacementCollision(world, field, currentPosition, avoidObjects); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); } else if ((avoidObjects.shouldAvoidOurDefenseArea && FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1).contains(currentPosition)) || - (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > stp::control_constants::ROBOT_RADIUS + stp::control_constants::GO_TO_POS_ERROR_MARGIN && + (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN && FieldComputations::getDefenseArea(field, false, theirDefenseAreaMargin, 1).contains(currentPosition))) { targetPosition = handleDefenseAreaCollision(field, currentPosition); - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); } else { - computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + computedTrajectories[robotId] = Trajectory2D(currentPosition, currentVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); auto hasCollsion = CollisionCalculations::isColliding(computedTrajectories[robotId], avoidObjects, field, robotId, world, computedPaths); if (hasCollsion) { computedTrajectories[robotId] = findNewTrajectory(world, field, robotId, currentPosition, currentVelocity, targetPosition, maxRobotVelocity, timeStep, avoidObjects); @@ -47,49 +47,51 @@ Vector2 PositionControl::computeAndTrackTrajectory(const world::World *world, co .thickness = 1, }, computedPaths[robotId]); - + // Since our acceleration is 3.5 m/s^2, this means we will move if we need to move more than + // 2*(1/2*3.5*0.145^2) (2 times the distance we move in 0.145/2s under constant (de)acceleration (1/2 * a * t^2)) + // This is 0.07m, so we will move if we need to move more than 0.07m return computedTrajectories[robotId].getVelocity(0.145); } Vector2 PositionControl::handleBallCollision(const world::World *world, const Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) { auto ballPos = world->getWorld()->getBall()->get()->position; auto direction = currentPosition - ballPos; - Vector2 targetPosition = currentPosition + direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); - if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + Vector2 targetPosition = currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE * 2); + if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, constants::OUT_OF_FIELD_MARGIN)) { return targetPosition; } int rotationStepDegrees = 10; - int maxRotationDegrees = 90; + int maxRotationDegrees = 180; for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) { for (int sign : {1, -1}) { double rotation = sign * i * M_PI / 180; - Vector2 rotatedDirection = direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2).rotate(rotation); + Vector2 rotatedDirection = direction.stretchToLength(constants::AVOID_BALL_DISTANCE * 2).rotate(rotation); Vector2 potentialTargetPosition = currentPosition + rotatedDirection; - if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, constants::OUT_OF_FIELD_MARGIN)) { return potentialTargetPosition; } } } - return currentPosition + direction.stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2).rotate(M_PI / 2); + return currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE * 2).rotate(M_PI / 2); } Vector2 PositionControl::handleBallPlacementCollision(const world::World *world, const Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects) { auto placementPos = GameStateManager::getRefereeDesignatedPosition(); auto ballPos = world->getWorld()->getBall()->get()->position; - auto direction = (placementPos - ballPos).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); + auto direction = (placementPos - ballPos).stretchToLength(constants::AVOID_BALL_DISTANCE * 2); direction = direction.rotate((currentPosition - ballPos).cross(placementPos - ballPos) < 0 ? M_PI / 2 : -M_PI / 2); Vector2 targetPosition = currentPosition + direction; - if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + if (FieldComputations::pointIsValidPosition(field, targetPosition, avoidObjects, constants::OUT_OF_FIELD_MARGIN)) { return targetPosition; } int rotationStepDegrees = 10; - int maxRotationDegrees = 90; + int maxRotationDegrees = 180; for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) { for (int sign : {1, -1}) { double rotation = sign * i * M_PI / 180; Vector2 rotatedDirection = direction.rotate(rotation); Vector2 potentialTargetPosition = currentPosition + rotatedDirection; - if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, stp::control_constants::OUT_OF_FIELD_MARGIN)) { + if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, constants::OUT_OF_FIELD_MARGIN)) { return potentialTargetPosition; } } @@ -101,7 +103,7 @@ Vector2 PositionControl::handleDefenseAreaCollision(const Field &field, Vector2 Vector2 ourGoalCenter = field.leftGoalArea.rightLine().center(); Vector2 theirGoalCenter = field.rightGoalArea.leftLine().center(); Vector2 closestGoalCenter = (currentPosition - ourGoalCenter).length() < (currentPosition - theirGoalCenter).length() ? ourGoalCenter : theirGoalCenter; - Vector2 targetPosition = currentPosition + (currentPosition - closestGoalCenter).stretchToLength(stp::control_constants::AVOID_BALL_DISTANCE * 2); + Vector2 targetPosition = currentPosition + (currentPosition - closestGoalCenter).stretchToLength(constants::AVOID_BALL_DISTANCE * 2); return targetPosition; } @@ -112,7 +114,7 @@ Trajectory2D PositionControl::findNewTrajectory(const world::World *world, const Vector2 startToDest = targetPosition - currentPosition; for (const auto &normalizedPoint : normalizedPoints) { auto intermediatePoint = startToDest.rotate(normalizedPoint.angle()) * normalizedPoint.length() + currentPosition; - Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, ai::Constants::MAX_ACC()); + Trajectory2D trajectoryToIntermediatePoint(currentPosition, currentVelocity, intermediatePoint, maxRobotVelocity, ai::constants::MAX_ACC); auto timeTillFirstCollision = CollisionCalculations::getFirstCollisionTime(trajectoryToIntermediatePoint, avoidObjects, field, robotId, world, computedPaths); double maxLoopTime = timeTillFirstCollision != -1 ? timeTillFirstCollision - 0.1 : trajectoryToIntermediatePoint.getTotalTime(); int numSteps = static_cast(maxLoopTime / timeStep); @@ -120,7 +122,7 @@ Trajectory2D PositionControl::findNewTrajectory(const world::World *world, const double loopTime = i * timeStep; Vector2 newStartPosition = trajectoryToIntermediatePoint.getPosition(loopTime); Vector2 newStartVelocity = trajectoryToIntermediatePoint.getVelocity(loopTime); - Trajectory2D trajectoryFromIntermediateToTarget(newStartPosition, newStartVelocity, targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + Trajectory2D trajectoryFromIntermediateToTarget(newStartPosition, newStartVelocity, targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); auto hasCollision = CollisionCalculations::isColliding(trajectoryFromIntermediateToTarget, avoidObjects, field, robotId, world, computedPaths); if (!hasCollision) { trajectoryToIntermediatePoint.addTrajectory(trajectoryFromIntermediateToTarget, loopTime); @@ -130,7 +132,7 @@ Trajectory2D PositionControl::findNewTrajectory(const world::World *world, const } } lastUsedNormalizedPoints.erase(robotId); - return Trajectory2D(currentPosition, currentVelocity, currentPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + return Trajectory2D(currentPosition, currentVelocity, currentPosition, maxRobotVelocity, ai::constants::MAX_ACC); ; } diff --git a/roboteam_ai/src/gui/networking/InterfacePublisher.cpp b/roboteam_ai/src/gui/networking/InterfacePublisher.cpp index a1d9ef286..785fe1bad 100644 --- a/roboteam_ai/src/gui/networking/InterfacePublisher.cpp +++ b/roboteam_ai/src/gui/networking/InterfacePublisher.cpp @@ -101,7 +101,7 @@ InterfacePublisher& InterfacePublisher::publishAIStatus() { aiState->add_plays(play->getName()); } - for (const auto& ruleSet : Constants::ruleSets()) { + for (const auto& ruleSet : RuleSet::ruleSets()) { aiState->add_rule_sets(ruleSet.toString()); } diff --git a/roboteam_ai/src/interface/api/Output.cpp b/roboteam_ai/src/interface/api/Output.cpp index bc9c7d16a..97de1c106 100644 --- a/roboteam_ai/src/interface/api/Output.cpp +++ b/roboteam_ai/src/interface/api/Output.cpp @@ -7,7 +7,7 @@ bool Output::useRefereeCommands = false; std::mutex Output::refMutex; std::mutex Output::interfaceGameStateMutex; -GameState Output::interfaceGameState(RefCommand::HALT, Constants::RULESET_HALT()); +GameState Output::interfaceGameState(RefCommand::HALT, RuleSet::RULESET_HALT()); bool Output::usesRefereeCommands() { std::lock_guard lock(refMutex); diff --git a/roboteam_ai/src/roboteam_ai.cpp b/roboteam_ai/src/roboteam_ai.cpp index b68bdbcf2..604d0cb5d 100644 --- a/roboteam_ai/src/roboteam_ai.cpp +++ b/roboteam_ai/src/roboteam_ai.cpp @@ -74,7 +74,7 @@ int main(int argc, char** argv) { if (!rtt::ai::io::io.init(rtt::GameSettings::isPrimaryAI())) { RTT_ERROR("Failed to initialize IO Manager. Exiting...") - return 0; + return 1; } RTT_DEBUG("Initialize Interface Server"); diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index ff7318b6a..23a3e3b55 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -31,6 +31,16 @@ void Play::update() noexcept { roleStatuses.clear(); // RTT_INFO("Play executing: ", getName()) + // Set the kickPoint to determine whether a free kick/kickoff has been taken + if ((GameStateManager::getCurrentGameState().commandId == RefCommand::DIRECT_FREE_THEM || GameStateManager::getCurrentGameState().commandId == RefCommand::KICKOFF_THEM || + GameStateManager::getCurrentGameState().commandId == RefCommand::DIRECT_FREE_US || GameStateManager::getCurrentGameState().commandId == RefCommand::KICKOFF_US) && + !GameStateManager::getCurrentGameState().kickPoint.has_value()) { + GameStateManager::getCurrentGameState().kickPoint = world->getWorld()->getBall()->get()->position; + } else if (GameStateManager::getCurrentGameState().commandId != RefCommand::DIRECT_FREE_THEM && GameStateManager::getCurrentGameState().commandId != RefCommand::KICKOFF_THEM && + GameStateManager::getCurrentGameState().commandId != RefCommand::DIRECT_FREE_US && GameStateManager::getCurrentGameState().commandId != RefCommand::KICKOFF_US) { + GameStateManager::getCurrentGameState().kickPoint.reset(); + } + // RTT_INFO(GameStateManager::getCurrentGameState().kickPoint) // Check if the amount of robots changed or keeper id changed // If so, we will re deal the roles auto currentRobotNum{world->getWorld()->getRobotsNonOwning().size()}; @@ -61,7 +71,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.0, -field.playArea.width() / 2)); + stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(rtt::Vector2(0.0, -field.playArea.height() / 2)); } // Loop through roles and update them if they are assigned to a robot @@ -75,8 +85,7 @@ void Play::update() noexcept { } if (stpInfos.find(role->getName())->second.getRobot() && stpInfos.find(role->getName())->second.getRobot()->get()->getId() == GameStateManager::getCurrentGameState().cardId && - (stpInfos.find(role->getName())->second.getRobot()->get()->getPos() - Vector2(0.0, -field.playArea.height() / 2)).length() <= - control_constants::GO_TO_POS_ERROR_MARGIN * 4) { + (stpInfos.find(role->getName())->second.getRobot()->get()->getPos() - Vector2(0.0, -field.playArea.height() / 2)).length() <= constants::GO_TO_POS_ERROR_MARGIN * 4) { stpInfos[role->getName()].setShouldAvoidTheirRobots(false); stpInfos[role->getName()].setShouldAvoidOurRobots(false); } @@ -86,6 +95,11 @@ void Play::update() noexcept { void Play::reassignRobots() noexcept { stpInfos.clear(); + for (auto &role : roles) { + if (role == nullptr) continue; + stpInfos[role->getName()].setShouldAvoidBall(FieldComputations::getBallAvoidance()); + stpInfos[role->getName()].setMaxRobotVelocity(control::ControlUtils::getMaxVelocity(false)); + } calculateInfoForRoles(); distributeRoles(); } @@ -140,7 +154,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.0, -field.playArea.height() / 2)); + stpInfos[roles[currentMaxRobots]->getName()].setPositionToMoveTo(rtt::Vector2(0.0, -field.playArea.height() / 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 @@ -193,7 +207,6 @@ void Play::DrawMargins() noexcept { 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 placementLocation = {rtt::ai::GameStateManager::getRefereeDesignatedPosition()}; std::array pathToPlacementLocation = {world->getWorld()->getBall()->get()->position, world->getWorld()->getBall()->get()->position}; @@ -224,7 +237,8 @@ void Play::DrawMargins() noexcept { currentGameState == RefCommand::DIRECT_FREE_US || currentGameState == RefCommand::KICKOFF_US || currentGameState == RefCommand::KICKOFF_THEM || currentGameState == RefCommand::PREPARE_FORCED_START || currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) { - if (currentGameState != RefCommand::PREPARE_FORCED_START) { + if (currentGameState != RefCommand::PREPARE_FORCED_START && currentGameState != RefCommand::BALL_PLACEMENT_THEM && currentGameState != RefCommand::BALL_PLACEMENT_US && + currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { rtt::ai::gui::Out::draw( { .label = "Left defense area to avoid", @@ -246,16 +260,18 @@ void Play::DrawMargins() noexcept { }, rightDefenseAreaMargin); } - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) + if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM || + (currentGameState == RefCommand::PREPARE_FORCED_START && GameStateManager::getCurrentGameState().commandFromRef != RefCommand::BALL_PLACEMENT_THEM)) 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::KICKOFF_US || + (currentGameState == RefCommand::PREPARE_FORCED_START && GameStateManager::getCurrentGameState().commandFromRef != RefCommand::BALL_PLACEMENT_US)) color = GameSettings::isYellow() ? proto::Drawing::BLUE : proto::Drawing::YELLOW; else color = proto::Drawing::RED; - - } - else color = proto::Drawing::GREY; + + } else + color = proto::Drawing::GREY; for (auto method : {proto::Drawing::CIRCLES, proto::Drawing::LINES_CONNECTED}) { rtt::ai::gui::Out::draw( @@ -286,6 +302,8 @@ void Play::DrawMargins() noexcept { } if (GameStateManager::getCurrentGameState().cardId != -1) { + std::array sideOfTheField = {*stpInfos[roles[GameStateManager::getCurrentGameState().maxAllowedRobots]->getName()].getPositionToMoveTo(), + stpInfos[roles[GameStateManager::getCurrentGameState().maxAllowedRobots]->getName()].getRobot()->get()->getPos()}; rtt::ai::gui::Out::draw( { .label = "CardID", @@ -295,24 +313,7 @@ void Play::DrawMargins() noexcept { .size = 15, .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 (std::size_t 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); - } + sideOfTheField); } } diff --git a/roboteam_ai/src/stp/PlayEvaluator.cpp b/roboteam_ai/src/stp/PlayEvaluator.cpp index 273e92a22..af0f51dc7 100644 --- a/roboteam_ai/src/stp/PlayEvaluator.cpp +++ b/roboteam_ai/src/stp/PlayEvaluator.cpp @@ -9,9 +9,7 @@ #include #include #include -#include #include -#include #include #include #include @@ -56,12 +54,8 @@ uint8_t PlayEvaluator::updateGlobalEvaluation(GlobalEvaluation& evaluation, cons return evaluation::KickOffThemPrepareGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::KickOffUsGameState: return evaluation::KickOffUsGameStateEvaluation().metricCheck(world, &field); - case GlobalEvaluation::KickOffUsOrNormalGameState: - return evaluation::KickOffUsOrNormalGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::KickOffUsPrepareGameState: return evaluation::KickOffUsPrepareGameStateEvaluation().metricCheck(world, &field); - case GlobalEvaluation::NormalOrFreeKickUsGameState: - return evaluation::NormalOrFreeKickUsGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::NormalPlayGameState: return evaluation::NormalPlayGameStateEvaluation().metricCheck(world, &field); case GlobalEvaluation::PenaltyThemGameState: @@ -77,15 +71,15 @@ uint8_t PlayEvaluator::updateGlobalEvaluation(GlobalEvaluation& evaluation, cons case GlobalEvaluation::BallOnOurSide: return evaluation::BallOnOurSideGlobalEvaluation().metricCheck(world, &field); case GlobalEvaluation::BallOnTheirSide: - return stp::control_constants::FUZZY_TRUE - evaluation::BallOnOurSideGlobalEvaluation().metricCheck(world, &field); + return constants::FUZZY_TRUE - evaluation::BallOnOurSideGlobalEvaluation().metricCheck(world, &field); case GlobalEvaluation::BallInOurDefenseAreaAndStill: return evaluation::BallInOurDefenseAreaAndStillGlobalEvaluation().metricCheck(world, &field); case GlobalEvaluation::BallNotInOurDefenseAreaAndStill: - return stp::control_constants::FUZZY_TRUE - evaluation::BallInOurDefenseAreaAndStillGlobalEvaluation().metricCheck(world, &field); + return constants::FUZZY_TRUE - evaluation::BallInOurDefenseAreaAndStillGlobalEvaluation().metricCheck(world, &field); case GlobalEvaluation::WeWillHaveBall: return evaluation::WeWillHaveBallGlobalEvaluation().metricCheck(world, &field); case GlobalEvaluation::WeWillNotHaveBall: - return stp::control_constants::FUZZY_TRUE - evaluation::WeWillHaveBallGlobalEvaluation().metricCheck(world, &field); + return constants::FUZZY_TRUE - evaluation::WeWillHaveBallGlobalEvaluation().metricCheck(world, &field); case GlobalEvaluation::TheyHaveBall: return evaluation::TheyHaveBallGlobalEvaluation().metricCheck(world, &field); default: @@ -99,14 +93,4 @@ bool PlayEvaluator::checkEvaluation(GlobalEvaluation globalEvaluation, const rtt return getGlobalEvaluation(globalEvaluation, world) >= cutOff; } -uint8_t PlayEvaluator::calculateScore(std::vector& scoring) { - double scoreTotal = 0; - double weightTotal = 0; - for (auto& factor : scoring) { - scoreTotal += factor.evaluationScore; - weightTotal += factor.weight; - } - return scoreTotal / weightTotal; -} - } // namespace rtt::ai::stp \ No newline at end of file diff --git a/roboteam_ai/src/stp/computations/GoalComputations.cpp b/roboteam_ai/src/stp/computations/GoalComputations.cpp index 50443b52c..f58cd8f1d 100644 --- a/roboteam_ai/src/stp/computations/GoalComputations.cpp +++ b/roboteam_ai/src/stp/computations/GoalComputations.cpp @@ -60,7 +60,7 @@ LineSegment GoalComputations::getAimPoints(const Field &field, const Vector2 &so return LineSegment(leftPoint, rightPoint); } -const LineSegment &GoalComputations::getLongestSegment(const std::vector &openSegments) { +const LineSegment GoalComputations::getLongestSegment(const std::vector &openSegments) { return *std::max_element(openSegments.begin(), openSegments.end(), [](const LineSegment &left, const LineSegment &right) { return fabs(left.start.y - left.end.y) < fabs(right.start.y - right.end.y); }); } diff --git a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp index 4055f6fb0..eef5d20dd 100644 --- a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp +++ b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp @@ -24,19 +24,19 @@ KeeperInterceptionInfo InterceptionComputations::calculateKeeperInterceptionInfo futureBallPosition = FieldComputations::getBallPositionAtTime(*(world->getWorld()->getBall()->get()), loopTime); // If the ball is out of the field or the LoS score is too low, we don't intercept if (PositionScoring::scorePosition(futureBallPosition, gen::LineOfSight, field, world).score < LineOfSightScore || - !field.playArea.contains(futureBallPosition, control_constants::BALL_RADIUS)) { + !field.playArea.contains(futureBallPosition, constants::BALL_RADIUS)) { return keeperInterceptionInfo; } // Only intercept if the ball will be in the defense area if (field.leftDefenseArea.contains(futureBallPosition)) { // If we are already close to the linesegment, project the position to prevent always moving to the 0.1 second mark - if (LineSegment(ballPosition, futureBallPosition).distanceToLine(keeper->getPos()) < 1.5 * control_constants::ROBOT_RADIUS) { + if (LineSegment(ballPosition, futureBallPosition).distanceToLine(keeper->getPos()) < 1.5 * constants::ROBOT_RADIUS) { keeperInterceptionInfo.interceptLocation = LineSegment(ballPosition, futureBallPosition).project(keeper->getPos()); keeperInterceptionInfo.canIntercept = true; return keeperInterceptionInfo; } // If we can reach the ball in time, we will intercept - if (Trajectory2D(keeper->getPos(), keeper->getVel(), futureBallPosition, maxRobotVelocity, ai::Constants::MAX_ACC()).getTotalTime() < loopTime) { + if (Trajectory2D(keeper->getPos(), keeper->getVel(), futureBallPosition, maxRobotVelocity, ai::constants::MAX_ACC).getTotalTime() < loopTime) { keeperInterceptionInfo.interceptLocation = futureBallPosition; keeperInterceptionInfo.canIntercept = true; return keeperInterceptionInfo; @@ -54,6 +54,7 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: auto pastBallPosition = ballPosition; auto futureBallPosition = ballPosition; auto ballVelocity = world->getWorld()->getBall()->get()->velocity; + bool shouldReturn = false; interceptionInfo.interceptLocation = ballPosition; // Helper function to calculate the intercept info for a given target position @@ -75,14 +76,24 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: LineSegment(futureBallPosition, pastBallPosition).project(world->getWorld()->getRobotClosestToBall(world::us)->get()->getPos()); } else { interceptionInfo.interceptLocation = - robotCloseToBallPos + (world->getWorld()->getBall()->get()->position - robotCloseToBallPos).stretchToLength(control_constants::ROBOT_RADIUS * 3); + robotCloseToBallPos + (world->getWorld()->getBall()->get()->position - robotCloseToBallPos).stretchToLength(constants::ROBOT_RADIUS * 3); } + double minTimeToTarget = std::numeric_limits::max(); + for (const auto &robot : ourRobots) { + auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), interceptionInfo.interceptLocation, maxRobotVelocity, ai::constants::MAX_ACC); + if (trajectory.getTotalTime() < minTimeToTarget) { + minTimeToTarget = trajectory.getTotalTime(); + interceptionInfo.interceptId = robot->getId(); + interceptionInfo.timeToIntercept = minTimeToTarget; + } + } + shouldReturn = true; return; } for (const auto &robot : ourRobots) { // If the robot is already close to the line, project it's position onto the line to prevent always moving to the 0.1 second mark - if (LineSegment(pastBallPosition, futureBallPosition).distanceToLine(robot->getPos()) < 1.5 * control_constants::ROBOT_RADIUS) { + if (LineSegment(pastBallPosition, futureBallPosition).distanceToLine(robot->getPos()) < 1.5 * constants::ROBOT_RADIUS) { interceptionInfo.interceptLocation = LineSegment(pastBallPosition, futureBallPosition).project(robot->getPos()); interceptionInfo.interceptId = robot->getId(); interceptionInfo.timeToIntercept = 0; @@ -93,11 +104,11 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: double minTimeToTarget = std::numeric_limits::max(); for (const auto &robot : ourRobots) { - auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), targetPosition, maxRobotVelocity, ai::Constants::MAX_ACC()); + auto trajectory = Trajectory2D(robot->getPos(), robot->getVel(), targetPosition, maxRobotVelocity, ai::constants::MAX_ACC); if (trajectory.getTotalTime() < minTimeToTarget) { minTimeToTarget = trajectory.getTotalTime(); - interceptionInfo.interceptId = robot->getId(); interceptionInfo.interceptLocation = targetPosition; + interceptionInfo.interceptId = robot->getId(); interceptionInfo.timeToIntercept = minTimeToTarget; auto theirClosestToBall = world->getWorld()->getRobotClosestToBall(world::them); if (!theirClosestToBall || (robot->getPos() - targetPosition).length() < (theirClosestToBall->get()->getPos() - targetPosition).length()) { @@ -108,7 +119,7 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: }; // If the ball is not moving, we use the current ball position - if (ballVelocity.length() <= control_constants::BALL_STILL_VEL) { + if (ballVelocity.length() <= constants::BALL_STILL_VEL) { calculateIntercept(ballPosition); return interceptionInfo; } @@ -139,9 +150,10 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: } // If the ball is out of the field, we intercept at the projected position in the field, unless the ball is already out of the field - if (!world->getField().value().playArea.contains(futureBallPosition, control_constants::BALL_RADIUS)) { - if (world->getField().value().playArea.contains(ballPosition, control_constants::BALL_RADIUS)) { - futureBallPosition = FieldComputations::projectPointInField(world->getField().value(), futureBallPosition); + if (!world->getField().value().playArea.contains(futureBallPosition, constants::BALL_RADIUS)) { + if (world->getField().value().playArea.contains(ballPosition, constants::BALL_RADIUS)) { + futureBallPosition = + FieldComputations::projectPointIntoFieldOnLine(world->getField().value(), futureBallPosition, ballPosition, futureBallPosition, constants::BALL_RADIUS); calculateIntercept(futureBallPosition); } else { calculateIntercept(ballPosition); @@ -154,6 +166,8 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: if (loopTime >= interceptionInfo.timeToIntercept) { interceptionInfo.isInterceptable = true; return interceptionInfo; + } else if (shouldReturn) { + return interceptionInfo; } } return interceptionInfo; @@ -177,7 +191,7 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfoForKickingRo auto possibleRobots = robots; // Remove robots that cannot kick - std::erase_if(possibleRobots, [](const world::view::RobotView &rbv) { return !Constants::ROBOT_HAS_KICKER(rbv->getId()); }); + std::erase_if(possibleRobots, [](const world::view::RobotView &rbv) { return !constants::ROBOT_HAS_KICKER(rbv->getId()); }); // If there is no robot that can kick, return empty if (possibleRobots.empty()) return interceptionInfo; @@ -187,7 +201,7 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfoForKickingRo // Remove robots that cannot detect the ball themselves (so no ballSensor or dribblerEncoder) auto numRobots = possibleRobots.size(); - std::erase_if(possibleRobots, [](const world::view::RobotView &rbv) { return !Constants::ROBOT_HAS_WORKING_DRIBBLER_ENCODER(rbv->getId()); }); + std::erase_if(possibleRobots, [](const world::view::RobotView &rbv) { return !constants::ROBOT_HAS_WORKING_DRIBBLER_ENCODER(rbv->getId()); }); // If no robot can detect the ball, the previous closest robot that can only kick is the best one if (possibleRobots.empty()) return interceptionInfo; diff --git a/roboteam_ai/src/stp/computations/PassComputations.cpp b/roboteam_ai/src/stp/computations/PassComputations.cpp index f02d0c098..d4c1375fa 100644 --- a/roboteam_ai/src/stp/computations/PassComputations.cpp +++ b/roboteam_ai/src/stp/computations/PassComputations.cpp @@ -7,13 +7,13 @@ #include "control/ControlUtils.h" #include "gui/Out.h" #include "roboteam_utils/Tube.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::computations { -PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::world::World* world, const Field& field, bool keeperCanPass) { +PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::world::World* world, const Field& field, bool keeperMustPass) { PassInfo passInfo; // Struct used to store the information needed to execute the pass - if (world->getWorld()->getUs().size() < (keeperCanPass ? 2 : 3)) { + if (world->getWorld()->getUs().size() < (keeperMustPass ? 2 : 3)) { return passInfo; } @@ -21,14 +21,20 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w // Find which robot is keeper (bot closest to goal if there was not a keeper yet), store its id, and erase from us to avoid the desired keeper to be the passer passInfo.keeperId = InterceptionComputations::getKeeperId(us, world); - if (!keeperCanPass) std::erase_if(us, [passInfo](auto& bot) { return bot->getId() == passInfo.keeperId; }); + if (!keeperMustPass) std::erase_if(us, [passInfo](auto& bot) { return bot->getId() == passInfo.keeperId; }); // Remove cardId from us auto cardId = GameStateManager::getCurrentGameState().cardId; std::erase_if(us, [cardId](auto& bot) { return bot->getId() == cardId; }); // Find which robot should be the passer, store its id and location, and erase from us - InterceptionInfo interceptionInfo = InterceptionComputations::calculateInterceptionInfoForKickingRobots(us, world); + InterceptionInfo interceptionInfo; + auto keeper = world->getWorld()->getRobotForId(passInfo.keeperId, true); + if (keeperMustPass && keeper) { + interceptionInfo = InterceptionComputations::calculateInterceptionInfoForKickingRobots({keeper.value()}, world); + } else { + interceptionInfo = InterceptionComputations::calculateInterceptionInfoForKickingRobots(us, world); + } passInfo.passerId = interceptionInfo.interceptId; passInfo.passLocation = interceptionInfo.interceptLocation; auto passerIt = std::find_if(us.begin(), us.end(), [passInfo](auto& bot) { return bot->getId() == passInfo.passerId; }); @@ -49,9 +55,9 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w std::vector possibleReceiverLocations; std::vector possibleReceiverVelocities; for (const auto& robot : us) { - if (Constants::ROBOT_HAS_KICKER(robot->getId())) { + if (constants::ROBOT_HAS_KICKER(robot->getId())) { possibleReceiverLocations.push_back(robot->getPos()); - possibleReceiverVelocities.push_back(robot->getPos()); + possibleReceiverVelocities.push_back(robot->getVel()); } } // If there are no other robots that can kick, add every other robots @@ -60,7 +66,7 @@ PassInfo PassComputations::calculatePass(gen::ScoreProfile profile, const rtt::w possibleReceiverVelocities.reserve(us.size()); for (const auto& robot : us) { possibleReceiverLocations.push_back(robot->getPos()); - possibleReceiverVelocities.push_back(robot->getPos()); + possibleReceiverVelocities.push_back(robot->getVel()); } } @@ -115,7 +121,11 @@ bool PassComputations::pointIsValidReceiverLocation(Vector2 point, const std::ve if (point.dist(passLocation) < 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; + AvoidObjects avoidObj; + avoidObj.shouldAvoidOurDefenseArea = true; + avoidObj.shouldAvoidTheirDefenseArea = true; + avoidObj.shouldAvoidOutOfField = true; + if (!FieldComputations::pointIsValidPosition(field, point, avoidObj)) 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(passLocation, passerLocation, passerVelocity, point); for (std::vector::size_type i = 0; i < possibleReceiverLocations.size(); i++) { @@ -125,11 +135,11 @@ bool PassComputations::pointIsValidReceiverLocation(Vector2 point, const std::ve } double PassComputations::calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition) { - return Trajectory2D(robotPosition, robotVelocity, targetPosition, control::ControlUtils::getMaxVelocity(false), ai::Constants::MAX_ACC()).getTotalTime() * 1.1; + return Trajectory2D(robotPosition, robotVelocity, targetPosition, control::ControlUtils::getMaxVelocity(false), ai::constants::MAX_ACC).getTotalTime() * 1.1; } double PassComputations::calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition) { - auto travelTime = calculateRobotTravelTime(passerLocation, passerVelocity, passLocation - (passerLocation - passLocation).stretchToLength(control_constants::ROBOT_RADIUS)); + auto travelTime = calculateRobotTravelTime(passerLocation, passerVelocity, passLocation - (passerLocation - passLocation).stretchToLength(constants::ROBOT_RADIUS)); auto rotateTime = (passLocation - passerLocation).toAngle().shortestAngleDiff(targetPosition - passLocation) / (M_PI); double ballSpeed = control::ControlUtils::determineKickForce(passLocation.dist(targetPosition), ShotType::PASS); auto ballTime = passLocation.dist(targetPosition) / ballSpeed; diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index d9f1f5182..9dd32d52f 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -11,6 +11,7 @@ #include "stp/computations/ComputationManager.h" #include "stp/computations/PassComputations.h" #include "stp/computations/PositionScoring.h" +#include "utilities/Constants.h" #include "world/World.hpp" namespace rtt::ai::stp { @@ -18,10 +19,14 @@ namespace rtt::ai::stp { gen::ScoredPosition PositionComputations::getPosition(std::optional currentPosition, const Grid &searchGrid, gen::ScoreProfile profile, const Field &field, const world::World *world) { gen::ScoredPosition bestPosition; + AvoidObjects avoidObj; + avoidObj.shouldAvoidOurDefenseArea = true; + avoidObj.shouldAvoidTheirDefenseArea = true; + avoidObj.shouldAvoidOutOfField = true; (currentPosition.has_value()) ? bestPosition = PositionScoring::scorePosition(currentPosition.value(), profile, field, world, 2) : bestPosition = {{0, 0}, 0}; for (const auto &nestedPoints : searchGrid.getPoints()) { for (const Vector2 &position : nestedPoints) { - if (!FieldComputations::pointIsValidPosition(field, position)) continue; + if (!FieldComputations::pointIsValidPosition(field, position, avoidObj)) continue; gen::ScoredPosition consideredPosition = PositionScoring::scorePosition(position, profile, field, world); if (consideredPosition.score > bestPosition.score) bestPosition = consideredPosition; } @@ -44,7 +49,7 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie } // Constants for positioning the defenders - const double radius = control_constants::ROBOT_RADIUS; + const double radius = constants::ROBOT_RADIUS; const double spacingRobots = radius * 0.5; const double spaceBetweenDefenseArea = 2 * radius; RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); @@ -184,17 +189,17 @@ Vector2 PositionComputations::calculateAvoidRobotsPosition(Vector2 targetPositio } } // We use robot radius instead of 2 * robot radius to make sure we are not overly cautious - if (std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { return avoidPoint.dist(targetPosition) >= control_constants::ROBOT_RADIUS; })) { + if (std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { return avoidPoint.dist(targetPosition) >= constants::ROBOT_RADIUS; })) { return targetPosition; } for (int distanceSteps = 0; distanceSteps < 5; ++distanceSteps) { - auto distance = 4 * control_constants::ROBOT_RADIUS + distanceSteps * control_constants::ROBOT_RADIUS * 2; + auto distance = 4 * constants::ROBOT_RADIUS + distanceSteps * constants::ROBOT_RADIUS * 2; auto possiblePoints = Grid(targetPosition.x - distance / 2.0, targetPosition.y - distance / 2.0, distance, distance, 3, 3).getPoints(); for (auto &pointVector : possiblePoints) { for (auto &point : pointVector) { - if (FieldComputations::pointIsValidPosition(field, point) && - std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { return avoidPoint.dist(point) >= control_constants::ROBOT_RADIUS; })) { + if (FieldComputations::pointIsValidPosition(field, point, avoidObj) && + std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { return avoidPoint.dist(point) >= constants::ROBOT_RADIUS; })) { return point; } } @@ -204,37 +209,38 @@ Vector2 PositionComputations::calculateAvoidRobotsPosition(Vector2 targetPositio return targetPosition; } -Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field) { +Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field, const AvoidObjects &avoidObj) { RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); std::unique_ptr avoidShape; 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)); + avoidShape = + std::make_unique(Tube(ballPosition, GameStateManager::getRefereeDesignatedPosition(), constants::AVOID_BALL_DISTANCE + constants::GO_TO_POS_ERROR_MARGIN)); } else { - avoidShape = std::make_unique(Circle(ballPosition, control_constants::AVOID_BALL_DISTANCE + control_constants::GO_TO_POS_ERROR_MARGIN)); + avoidShape = std::make_unique(Circle(ballPosition, constants::AVOID_BALL_DISTANCE + constants::GO_TO_POS_ERROR_MARGIN)); } if (avoidShape->contains(targetPosition)) { auto projectedPos = avoidShape->project(targetPosition); - if (FieldComputations::pointIsValidPosition(field, projectedPos)) + if (FieldComputations::pointIsValidPosition(field, projectedPos, avoidObj)) targetPosition = projectedPos; else { - targetPosition = calculatePositionOutsideOfShape(targetPosition, field, avoidShape); + targetPosition = calculatePositionOutsideOfShape(targetPosition, field, avoidShape, avoidObj); } } return targetPosition; } -Vector2 PositionComputations::calculatePositionOutsideOfShape(Vector2 targetPosition, const rtt::Field &field, const std::unique_ptr &avoidShape) { +Vector2 PositionComputations::calculatePositionOutsideOfShape(Vector2 targetPosition, const rtt::Field &field, const std::unique_ptr &avoidShape, + const AvoidObjects &avoidObj) { for (int distanceSteps = 0; distanceSteps < 5; ++distanceSteps) { - auto distance = 2 * control_constants::AVOID_BALL_DISTANCE + distanceSteps * control_constants::AVOID_BALL_DISTANCE; + auto distance = 2 * constants::AVOID_BALL_DISTANCE + distanceSteps * constants::AVOID_BALL_DISTANCE; auto possiblePoints = Grid(targetPosition.x - distance / 2.0, targetPosition.y - distance / 2.0, distance, distance, 3, 3).getPoints(); for (auto &pointVector : possiblePoints) { for (auto &point : pointVector) { - if (FieldComputations::pointIsValidPosition(field, point) && !avoidShape->contains(point)) { + if (FieldComputations::pointIsValidPosition(field, point, avoidObj) && !avoidShape->contains(point)) { return point; } } @@ -244,9 +250,8 @@ Vector2 PositionComputations::calculatePositionOutsideOfShape(Vector2 targetPosi return targetPosition; } -void PositionComputations::calculateInfoForHarasser(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> *roles, const Field &field, world::World *world, - Vector2 interceptionLocation) noexcept { +void PositionComputations::calculateInfoForHarasser(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> *roles, + const Field &field, world::World *world, 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) { @@ -259,8 +264,7 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->hasBall() && enemyAngle.shortestAngleDiff(harasserAngle) < M_PI / 1.5) { auto enemyPos = enemyClosestToBall->get()->getPos(); - auto targetPos = - enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(control_constants::ROBOT_RADIUS * 4 + control_constants::GO_TO_POS_ERROR_MARGIN); + auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(constants::ROBOT_RADIUS * 4 + constants::GO_TO_POS_ERROR_MARGIN); stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setYaw((world->getWorld()->getBall()->get()->position - targetPos).angle()); } else { @@ -271,8 +275,8 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, - world::World *world, bool mustStayOnOurSide) noexcept { + std::array, constants::MAX_ROBOT_COUNT> &roles, const Field &field, world::World *world, + bool mustStayOnOurSide) noexcept { // List of all active defender and waller names auto defenderNames = std::vector{}; auto wallerNames = std::vector{}; @@ -304,7 +308,7 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma double score = FieldComputations::getDistanceToGoal(field, true, enemy->getPos()); if (std::find(ComputationManager::calculatedEnemyMapIds.begin(), ComputationManager::calculatedEnemyMapIds.end(), enemy->getId()) != ComputationManager::calculatedEnemyMapIds.end()) { - score *= stp::control_constants::ENEMY_ALREADY_ASSIGNED_MULTIPLIER; + score *= constants::ENEMY_ALREADY_ASSIGNED_MULTIPLIER; } EnemyInfo info = {enemy->getPos(), enemy->getVel(), enemy->getId()}; enemyMap.insert({score, info}); @@ -379,9 +383,8 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma } } -void PositionComputations::calculateInfoForAttackers(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, - world::World *world) noexcept { +void PositionComputations::calculateInfoForAttackers(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> &roles, + const Field &field, world::World *world) noexcept { // List of all active attackers auto attackerNames = std::vector{}; for (size_t i = 0; i < world->getWorld()->getUs().size(); i++) { @@ -413,9 +416,8 @@ void PositionComputations::calculateInfoForAttackers(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, - world::World *world) noexcept { +void PositionComputations::calculateInfoForFormation(std::unordered_map &stpInfos, std::array, constants::MAX_ROBOT_COUNT> &roles, + const Field &field, world::World *world) noexcept { auto formationBackNames = std::vector{}; auto formationMidNames = std::vector{}; auto formationFrontNames = std::vector{}; @@ -444,7 +446,7 @@ void PositionComputations::calculateInfoForFormation(std::unordered_map &stpInfos, - std::array, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field, + std::array, constants::MAX_ROBOT_COUNT> &roles, const Field &field, world::World *world) noexcept { auto formationBackNames = std::vector{}; auto formationMidNames = std::vector{}; @@ -493,7 +495,7 @@ void PositionComputations::recalculateInfoForNonPassers(std::unordered_map avoidShape = std::make_unique(Tube(ballPosition, receiverLocation, control_constants::DISTANCE_TO_PASS_TRAJECTORY)); + std::unique_ptr avoidShape = std::make_unique(Tube(ballPosition, receiverLocation, constants::DISTANCE_TO_PASS_TRAJECTORY)); for (auto &robot : toBeCheckedRobots) { stpInfos[robot].setShouldAvoidBall(true); auto robotPositionToMoveTo = stpInfos[robot].getPositionToMoveTo(); @@ -503,7 +505,7 @@ void PositionComputations::recalculateInfoForNonPassers(std::unordered_mapcontains(robotPositionToMoveTo.value())) { continue; } - auto newRobotPositionToMoveTo = calculatePositionOutsideOfShape(robotPositionToMoveTo.value(), field, avoidShape); + auto newRobotPositionToMoveTo = calculatePositionOutsideOfShape(robotPositionToMoveTo.value(), field, avoidShape, stpInfos[robot].getObjectsToAvoid()); stpInfos[robot].setPositionToMoveTo(newRobotPositionToMoveTo); } } @@ -512,7 +514,7 @@ void PositionComputations::calculateInfoForAvoidBallHarasser(std::unordered_map< if (!world->getWorld()->getBall()) return; auto ballPos = world->getWorld()->getBall()->get()->position; auto goalToBall = ballPos - world->getField().value().leftGoalArea.rightLine().center(); - stpInfos["harasser"].setPositionToMoveTo(ballPos - goalToBall.stretchToLength(control_constants::AVOID_BALL_DISTANCE)); + stpInfos["harasser"].setPositionToMoveTo(ballPos - goalToBall.stretchToLength(constants::AVOID_BALL_DISTANCE)); } } // namespace rtt::ai::stp \ No newline at end of file diff --git a/roboteam_ai/src/stp/computations/PositionScoring.cpp b/roboteam_ai/src/stp/computations/PositionScoring.cpp index 32b1ac184..6e5f547f8 100644 --- a/roboteam_ai/src/stp/computations/PositionScoring.cpp +++ b/roboteam_ai/src/stp/computations/PositionScoring.cpp @@ -9,7 +9,7 @@ namespace rtt::ai::stp { gen::ScoredPosition PositionScoring::scorePosition(const Vector2 &position, const gen::ScoreProfile &profile, const Field &field, const world::World *world, uint8_t bias) { gen::PositionScores &scores = ComputationManager::calculatedScores[position]; uint8_t positionScore = getScoreOfPosition(profile, position, scores, field, world); - if (bias) positionScore = (positionScore + bias > bias) ? positionScore + bias : std::numeric_limits::max(); // stop overflow of uint8_t (254+2 = 1) + if (bias) positionScore = std::min(static_cast(positionScore + bias), static_cast(std::numeric_limits::max())); // Make sure we don't overflow return {position, positionScore}; } diff --git a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp index 4f9cbc312..1424a71f5 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementThemGameStateEvaluation.cpp @@ -4,6 +4,6 @@ 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) ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp index 5090b863d..a8d99222a 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsDirectGameStateEvaluation.cpp @@ -4,7 +4,6 @@ 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; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US_DIRECT) ? constants::FUZZY_TRUE : 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 0cc932e0e..d7e85e571 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/BallPlacementUsGameStateEvaluation.cpp @@ -4,6 +4,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) ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_US) ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp index 19a621045..ef4ae4de4 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/FreeKickThemGameStateEvaluation.cpp @@ -7,7 +7,7 @@ namespace rtt::ai::stp::evaluation { uint8_t FreeKickThemGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { 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; + ? constants::FUZZY_TRUE + : 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 66961a612..121b85e44 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/FreeKickUsGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/FreeKickUsGameStateEvaluation.cpp @@ -7,7 +7,7 @@ namespace rtt::ai::stp::evaluation { uint8_t FreeKickUsGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { 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; + ? constants::FUZZY_TRUE + : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp index b2ffd6899..a2755b8eb 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/HaltGameStateEvaluation.cpp @@ -6,7 +6,7 @@ namespace rtt::ai::stp::evaluation { uint8_t HaltGameStateEvaluation::metricCheck(const world::World*, const Field*) const noexcept { RefCommand strategyName = GameStateManager::getCurrentGameState().getCommandId(); return (strategyName == RefCommand::HALT || strategyName == RefCommand::TIMEOUT_US || strategyName == RefCommand::TIMEOUT_THEM || strategyName == RefCommand::UNDEFINED) - ? stp::control_constants::FUZZY_TRUE - : stp::control_constants::FUZZY_FALSE; + ? constants::FUZZY_TRUE + : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation diff --git a/roboteam_ai/src/stp/evaluations/game_states/KickOffThemGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/KickOffThemGameStateEvaluation.cpp index 25038d046..5afaee08a 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/KickOffThemGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/KickOffThemGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t KickOffThemGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::KICKOFF_THEM ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::KICKOFF_THEM ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/KickOffThemPrepareGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/KickOffThemPrepareGameStateEvaluation.cpp index 0a3763497..db44c0bcd 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/KickOffThemPrepareGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/KickOffThemPrepareGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t KickOffThemPrepareGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_KICKOFF_THEM ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_KICKOFF_THEM ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/KickOffUsGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/KickOffUsGameStateEvaluation.cpp index 4fd0956ee..3c1c2d529 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/KickOffUsGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/KickOffUsGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t KickOffUsGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::KICKOFF_US ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::KICKOFF_US ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.cpp deleted file mode 100644 index 1d8abfe71..000000000 --- a/roboteam_ai/src/stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "stp/evaluations/game_states/KickOffUsOrNormalGameStateEvaluation.h" - -#include "utilities/GameStateManager.hpp" - -namespace rtt::ai::stp::evaluation { - -uint8_t KickOffUsOrNormalGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::KICKOFF_US || GameStateManager::getCurrentGameState().getCommandId() == RefCommand::NORMAL_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/game_states/KickOffUsPrepareGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/KickOffUsPrepareGameStateEvaluation.cpp index 38a0cd56f..c980bee99 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/KickOffUsPrepareGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/KickOffUsPrepareGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t KickOffUsPrepareGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_KICKOFF_US ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_KICKOFF_US ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.cpp deleted file mode 100644 index 800638ead..000000000 --- a/roboteam_ai/src/stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "stp/evaluations/game_states/NormalOrFreeKickUsGameStateEvaluation.h" - -#include "utilities/GameStateManager.hpp" - -namespace rtt::ai::stp::evaluation { - -uint8_t NormalOrFreeKickUsGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::DIRECT_FREE_US || - GameStateManager::getCurrentGameState().getCommandId() == RefCommand::NORMAL_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/game_states/NormalPlayGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp index 626b7cb57..0bed4b8d9 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/NormalPlayGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t NormalPlayGameStateEvaluation::metricCheck(const world::World*, const Field*) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::NORMAL_START ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::NORMAL_START ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemGameStateEvaluation.cpp index 6b877f8b3..5ccbcb008 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t PenaltyThemGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PENALTY_THEM ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PENALTY_THEM ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemPrepareGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemPrepareGameStateEvaluation.cpp index 256fc67fd..e47b89794 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemPrepareGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/PenaltyThemPrepareGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t PenaltyThemPrepareGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_PENALTY_THEM ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_PENALTY_THEM ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsGameStateEvaluation.cpp index 5ec844177..63d14b906 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t PenaltyUsGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PENALTY_US ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PENALTY_US ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsPrepareGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsPrepareGameStateEvaluation.cpp index f53dff8b8..0d3d50b66 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsPrepareGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/PenaltyUsPrepareGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t PenaltyUsPrepareGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_PENALTY_US ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_PENALTY_US ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/PrepareForcedStartGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/PrepareForcedStartGameStateEvaluation.cpp index 8d7646b40..4085c859c 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/PrepareForcedStartGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/PrepareForcedStartGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t PrepareForcedStartGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { - return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp index a5bcc26a2..8fdd94164 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/StopGameStateEvaluation.cpp @@ -5,6 +5,6 @@ namespace rtt::ai::stp::evaluation { uint8_t StopGameStateEvaluation::metricCheck(const world::World*, const Field*) const noexcept { - return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::STOP ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::STOP ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/game_states/TimeOutGameStateEvaluation.cpp b/roboteam_ai/src/stp/evaluations/game_states/TimeOutGameStateEvaluation.cpp index a3144385d..f0811e5c2 100644 --- a/roboteam_ai/src/stp/evaluations/game_states/TimeOutGameStateEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/game_states/TimeOutGameStateEvaluation.cpp @@ -5,7 +5,7 @@ namespace rtt::ai::stp::evaluation { uint8_t TimeOutGameStateEvaluation::metricCheck(const world::World *, const Field *) const noexcept { return GameStateManager::getCurrentGameState().getCommandId() == RefCommand::TIMEOUT_THEM || GameStateManager::getCurrentGameState().getCommandId() == RefCommand::TIMEOUT_US - ? stp::control_constants::FUZZY_TRUE - : stp::control_constants::FUZZY_FALSE; + ? constants::FUZZY_TRUE + : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp index df9826387..f977cc400 100644 --- a/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.cpp @@ -1,13 +1,13 @@ #include "stp/evaluations/global/BallInOurDefenseAreaAndStillGlobalEvaluation.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" #include "world/FieldComputations.h" namespace rtt::ai::stp::evaluation { uint8_t BallInOurDefenseAreaAndStillGlobalEvaluation::metricCheck(const world::World* world, const Field* field) const noexcept { return (field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->position) && field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->expectedEndPosition)) - ? control_constants::FUZZY_TRUE - : control_constants::FUZZY_FALSE; + ? constants::FUZZY_TRUE + : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp index 6a011f5c7..7e5d978a5 100644 --- a/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/BallOnOurSideGlobalEvaluation.cpp @@ -2,6 +2,6 @@ namespace rtt::ai::stp::evaluation { uint8_t BallOnOurSideGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { - return world->getWorld()->getBall().value()->position.x < 0 ? control_constants::FUZZY_TRUE : control_constants::FUZZY_FALSE; + return world->getWorld()->getBall().value()->position.x < 0 ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp index 42c29ff79..1ab7dd22c 100644 --- a/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/TheyHaveBallGlobalEvaluation.cpp @@ -6,8 +6,8 @@ uint8_t TheyHaveBallGlobalEvaluation::metricCheck(const world::World* world, con // If there are no bots, they don't have ball if (them.empty()) { - return stp::control_constants::FUZZY_FALSE; + return constants::FUZZY_FALSE; } - return std::any_of(them.begin(), them.end(), [](auto& robot) { return robot->hasBall(); }) ? stp::control_constants::FUZZY_TRUE : stp::control_constants::FUZZY_FALSE; + return std::any_of(them.begin(), them.end(), [](auto& robot) { return robot->hasBall(); }) ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation diff --git a/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp index f83766dc8..60290a531 100644 --- a/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp @@ -6,23 +6,26 @@ namespace rtt::ai::stp::evaluation { uint8_t WeWillHaveBallGlobalEvaluation::metricCheck(const world::World* world, const Field*) const noexcept { auto& us = world->getWorld()->getUs(); auto& them = world->getWorld()->getThem(); + auto ballPosition = world->getWorld()->getBall()->get()->position; // If the opponent has no robot, we will get the ball - if (them.empty()) return stp::control_constants::FUZZY_TRUE; + if (them.empty()) return constants::FUZZY_TRUE; // If we have no bots, we will not get the ball - if (us.empty()) return stp::control_constants::FUZZY_FALSE; + if (us.empty()) return constants::FUZZY_FALSE; - // If any of their robot has the ball has the ball, we will not get the ball - if (std::any_of(them.begin(), them.end(), [](auto& robot) { return robot->hasBall(); })) return stp::control_constants::FUZZY_FALSE; + if (world->getWorld()->getRobotClosestToBall(world::them)->get()->getPos().dist(ballPosition) < constants::ROBOT_RADIUS * 1.3 && world->getWorld()->getRobotClosestToBall(world::us)->get()->getPos().dist(ballPosition) < constants::ROBOT_RADIUS * 1.3) return false; // If any of our robots has the ball, we will get the ball - if (std::any_of(us.begin(), us.end(), [](auto& robot) { return robot->hasBall(); })) return stp::control_constants::FUZZY_TRUE; + if (std::any_of(us.begin(), us.end(), [](auto& robot) { return robot->hasBall(); })) return constants::FUZZY_TRUE; + + // If any of their robot has the ball has the ball, we will not get the ball + if (std::any_of(them.begin(), them.end(), [](auto& robot) { return robot->hasBall(); })) return constants::FUZZY_FALSE; // If we can intercept the ball, we will get the ball auto interceptionInfo = InterceptionComputations::calculateInterceptionInfo(us, world); - if (interceptionInfo.isInterceptable) return stp::control_constants::FUZZY_TRUE; + if (interceptionInfo.isInterceptable) return constants::FUZZY_TRUE; - return stp::control_constants::FUZZY_FALSE; + return constants::FUZZY_FALSE; } } // namespace rtt::ai::stp::evaluation \ No newline at end of file diff --git a/roboteam_ai/src/stp/evaluations/position/LineOfSightEvaluation.cpp b/roboteam_ai/src/stp/evaluations/position/LineOfSightEvaluation.cpp index 03349edf7..7223665b8 100644 --- a/roboteam_ai/src/stp/evaluations/position/LineOfSightEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/position/LineOfSightEvaluation.cpp @@ -3,7 +3,7 @@ #include #include -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::evaluation { uint8_t LineOfSightEvaluation::metricCheck(double pDist, std::vector& eDists, std::vector& eAngles) noexcept { @@ -25,7 +25,7 @@ uint8_t LineOfSightEvaluation::metricCheck(double pDist, std::vector& eD for (size_t i = 0; i < eAngles.size(); i++) { if (eAngles[i] < outerAngle) { // This scales from 0 (at 4 radii in front of the enemy) to 1 (at 4 radii behind the enemy). This smoothens the transitions around robots - auto distFadeFactor = std::clamp((-1.0 / (8 * control_constants::ROBOT_RADIUS) * (eDists[i] - pDist)) + 0.5, 0.0, 1.0); + auto distFadeFactor = std::clamp((-1.0 / (8 * constants::ROBOT_RADIUS) * (eDists[i] - pDist)) + 0.5, 0.0, 1.0); evalScore -= std::pow((1 / (outerAngle)) * (outerAngle - eAngles[i]), 2) * distFadeFactor; } } diff --git a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp index 28c0dd54a..0693311b7 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp @@ -26,7 +26,7 @@ DefendPass::DefendPass() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); // Role creation, the names should be unique. The names are used in the stpInfos-map - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), @@ -45,7 +45,7 @@ DefendPass::DefendPass() : Play() { uint8_t DefendPass::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap DefendPass::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp index 82e39441d..dd208b683 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp +++ b/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp @@ -23,7 +23,7 @@ DefendShot::DefendShot() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), @@ -42,7 +42,7 @@ DefendShot::DefendShot() : Play() { uint8_t DefendShot::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp index ddad069f4..7bac265ac 100644 --- a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp +++ b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp @@ -2,11 +2,11 @@ #include "stp/computations/PassComputations.h" #include "stp/computations/PositionScoring.h" -#include "stp/constants/ControlConstants.h" #include "stp/roles/active/KeeperPasser.h" #include "stp/roles/active/PassReceiver.h" #include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::play { @@ -19,10 +19,10 @@ KeeperKickBall::KeeperKickBall() : Play() { // 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::NormalPlayGameState); - keepPlayEvaluation.emplace_back(GlobalEvaluation::WeWillHaveBall); + keepPlayEvaluation.emplace_back(GlobalEvaluation::BallInOurDefenseAreaAndStill); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("receiver"), @@ -41,10 +41,10 @@ KeeperKickBall::KeeperKickBall() : Play() { uint8_t KeeperKickBall::score(const rtt::Field& field) noexcept { // Calculate passInfo to be used during the play - passInfo = stp::computations::PassComputations::calculatePass(gen::SafePass, world, field); + passInfo = stp::computations::PassComputations::calculatePass(gen::SafePass, world, field, true); // If this play is valid, the ball is in the defense area and still, and we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap KeeperKickBall::decideRoleFlags() const noexcept { @@ -92,21 +92,15 @@ bool KeeperKickBall::ballKicked() { } bool KeeperKickBall::shouldEndPlay() noexcept { - // If the receiver has the ball, the play finished successfully - if (stpInfos["receiver"].getRobot() && stpInfos["receiver"].getRobot().value()->hasBall()) return true; - - // If the ball is moving too slow after we have kicked it, we should stop the play to get the ball + // If the ball is kicked, we end the play to already prepare for what happens next if (ballKicked()) return true; // If the keeper doesn't have the ball yet and there is a better pass available, we should stop the play - if (!ballKicked() && stpInfos["keeper"].getRobot() && !stpInfos["keeper"].getRobot().value()->hasBall() && - stp::computations::PassComputations::calculatePass(gen::SafePass, world, field).passScore > + if (stpInfos["keeper"].getRobot() && !stpInfos["keeper"].getRobot().value()->hasBall() && + stp::computations::PassComputations::calculatePass(gen::SafePass, world, field, true).passScore > 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::SafePass, field, world).score) return true; - // If the keeper is outside of our defense area, we should stop the play - if (stpInfos["keeper"].getRobot() && !field.leftDefenseArea.contains(stpInfos["keeper"].getRobot().value()->getPos())) return true; - return false; } const char* KeeperKickBall::getName() const { return "Keeper Kick Ball"; } diff --git a/roboteam_ai/src/stp/plays/offensive/Attack.cpp b/roboteam_ai/src/stp/plays/offensive/Attack.cpp index d62318e99..e7a6dab3e 100644 --- a/roboteam_ai/src/stp/plays/offensive/Attack.cpp +++ b/roboteam_ai/src/stp/plays/offensive/Attack.cpp @@ -25,20 +25,20 @@ Attack::Attack() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("striker"), std::make_unique("defender_0"), std::make_unique("attacker_0"), std::make_unique("defender_1"), - std::make_unique("attacker_1"), + std::make_unique("defender_2"), // Additional roles if we play 11v11 std::make_unique("waller_0"), std::make_unique("waller_1"), + std::make_unique("attacker_1"), + std::make_unique("defender_3"), std::make_unique("attacker_2"), - std::make_unique("defender_2"), - std::make_unique("attacker_3"), }; } @@ -60,10 +60,10 @@ Dealer::FlagMap Attack::decideRoleFlags() const noexcept { 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({"attacker_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"attacker_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"attacker_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"attacker_3", {DealerFlagPriority::LOW_PRIORITY, {}}}); return flagMap; } diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp index 0bf30e63e..7cec10373 100644 --- a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp @@ -5,12 +5,12 @@ #include "stp/computations/PassComputations.h" #include "stp/computations/PositionScoring.h" -#include "stp/constants/ControlConstants.h" #include "stp/roles/Keeper.h" #include "stp/roles/active/PassReceiver.h" #include "stp/roles/active/Passer.h" #include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" +#include "utilities/Constants.h" #include "world/views/RobotView.hpp" namespace rtt::ai::stp::play { @@ -28,7 +28,7 @@ AttackingPass::AttackingPass() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("passer"), @@ -39,9 +39,9 @@ AttackingPass::AttackingPass() : Play() { // Additional roles if we play 11v11 std::make_unique("waller_0"), std::make_unique("waller_1"), - std::make_unique("attacker_1"), std::make_unique("defender_2"), - std::make_unique("attacker_2"), + std::make_unique("defender_3"), + std::make_unique("attacker_1"), }; } @@ -67,9 +67,9 @@ Dealer::FlagMap AttackingPass::decideRoleFlags() const noexcept { 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({"attacker_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"attacker_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"attacker_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); return flagMap; } @@ -110,26 +110,17 @@ bool AttackingPass::ballKicked() { } bool AttackingPass::shouldEndPlay() noexcept { - // If the receiver has the ball, the play finished successfully - if (stpInfos["receiver"].getRobot() && stpInfos["receiver"].getRobot().value()->hasBall()) return true; - - // If the ball is moving too slow after we have kicked it, we should stop the play to get the ball + // If the ball is kicked, we end the play to already prepare for what happens next if (ballKicked()) return true; - // If the ball is rolling away from the receiver - if (ballKicked() && (world->getWorld()->getBall()->get()->position - passInfo.receiverLocation).dot(world->getWorld()->getBall()->get()->velocity) > 0) return true; - auto newPassInfo = stp::computations::PassComputations::calculatePass(gen::AttackingPass, world, field); // If the passer doesn't have the ball yet and there is a better pass available, we should stop the play - if (!ballKicked() && stpInfos["passer"].getRobot() && !stpInfos["passer"].getRobot().value()->hasBall() && + if (stpInfos["passer"].getRobot() && !stpInfos["passer"].getRobot().value()->hasBall() && newPassInfo.passScore > 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::AttackingPass, field, world).score) return true; - // If the ball is not kicked yet and the passer id is different, another robot can quicker get the ball, so stop - if (!ballKicked() && newPassInfo.passerId != passInfo.passerId) { - endPlayCounter++; - if (endPlayCounter > 20) return true; - } else { - endPlayCounter = 0; + // If the passer id is different, another robot can quicker get the ball, so stop + if (newPassInfo.passerId != passInfo.passerId) { + return true; } return false; diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp index 2307b654f..9a858bfde 100644 --- a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp @@ -5,12 +5,12 @@ #include "stp/computations/PassComputations.h" #include "stp/computations/PositionScoring.h" -#include "stp/constants/ControlConstants.h" #include "stp/roles/Keeper.h" #include "stp/roles/active/Chipper.h" #include "stp/roles/active/PassReceiver.h" #include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" +#include "utilities/Constants.h" #include "world/views/RobotView.hpp" namespace rtt::ai::stp::play { @@ -28,7 +28,7 @@ ChippingPass::ChippingPass() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("passer"), @@ -109,14 +109,11 @@ bool ChippingPass::ballKicked() { } bool ChippingPass::shouldEndPlay() noexcept { - // If the receiver has the ball, the play finished successfully - if (stpInfos["receiver"].getRobot() && stpInfos["receiver"].getRobot().value()->hasBall()) return true; - - // If the ball is moving too slow after we have kicked it, we should stop the play to get the ball + // If the ball is kicked, we end the play to already prepare for what happens next if (ballKicked()) return true; // If the passer doesn't have the ball yet and there is a better pass available, we should stop the play - if (!ballKicked() && stpInfos["passer"].getRobot() && !stpInfos["passer"].getRobot().value()->hasBall() && + if (stpInfos["passer"].getRobot() && !stpInfos["passer"].getRobot().value()->hasBall() && stp::computations::PassComputations::calculatePass(gen::ChippingPass, world, field).passScore > 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::ChippingPass, field, world).score) return true; diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp index 42cfd7066..8d9ea7770 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp @@ -16,7 +16,7 @@ BallPlacementThem::BallPlacementThem() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::BallPlacementThemGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), @@ -35,7 +35,7 @@ BallPlacementThem::BallPlacementThem() : Play() { uint8_t BallPlacementThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap BallPlacementThem::decideRoleFlags() const noexcept { @@ -69,7 +69,7 @@ void BallPlacementThem::calculateInfoForRoles() noexcept { void BallPlacementThem::calculateInfoForHarasser() noexcept { auto placementPos = rtt::ai::GameStateManager::getRefereeDesignatedPosition(); - auto targetPos = placementPos + (field.leftGoalArea.rightLine().center() - placementPos).stretchToLength(control_constants::AVOID_BALL_DISTANCE); + auto targetPos = placementPos + (field.leftGoalArea.rightLine().center() - placementPos).stretchToLength(constants::AVOID_BALL_DISTANCE); stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setYaw((placementPos - field.leftGoalArea.rightLine().center()).toAngle()); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index 1d12734e6..684c1fb47 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -18,7 +18,7 @@ BallPlacementUsForceStart::BallPlacementUsForceStart() : Play() { 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 = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("ball_placer"), @@ -37,7 +37,7 @@ BallPlacementUsForceStart::BallPlacementUsForceStart() : Play() { uint8_t BallPlacementUsForceStart::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap BallPlacementUsForceStart::decideRoleFlags() const noexcept { @@ -70,7 +70,7 @@ void BallPlacementUsForceStart::calculateInfoForRoles() noexcept { // 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); + ballTarget -= (world->getWorld()->get()->getBall()->get()->position - stpInfos["ball_placer"].getRobot()->get()->getPos()).stretchToLength(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 @@ -87,13 +87,13 @@ void BallPlacementUsForceStart::calculateInfoForRoles() noexcept { stpInfos["ball_placer"].setShouldAvoidOutOfField(false); stpInfos["ball_placer"].setShouldAvoidBall(false); - if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < control_constants::BALL_PLACEMENT_MARGIN) { + if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < constants::BALL_PLACEMENT_MARGIN) { for (auto& role : roles) if (role->getName() == "ball_placer") role->forceLastTactic(); } 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) { + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerOn(true); } } diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index af17056dd..05fb2c108 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -18,7 +18,7 @@ BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { 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()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("ball_placer"), @@ -37,7 +37,7 @@ BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { uint8_t BallPlacementUsFreeKick::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap BallPlacementUsFreeKick::decideRoleFlags() const noexcept { @@ -70,7 +70,7 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { // 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); + ballTarget -= (world->getWorld()->get()->getBall()->get()->position - stpInfos["ball_placer"].getRobot()->get()->getPos()).stretchToLength(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 @@ -87,12 +87,12 @@ void BallPlacementUsFreeKick::calculateInfoForRoles() noexcept { stpInfos["ball_placer"].setShouldAvoidOutOfField(false); stpInfos["ball_placer"].setShouldAvoidBall(false); - if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < control_constants::BALL_PLACEMENT_MARGIN) { + if ((world->getWorld()->get()->getBall()->get()->position - rtt::ai::GameStateManager::getRefereeDesignatedPosition()).length() < constants::BALL_PLACEMENT_MARGIN) { for (auto& role : roles) if (role->getName() == "ball_placer") role->forceLastTactic(); } 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) { + if (stpInfos["ball_placer"].getRobot() && stpInfos["ball_placer"].getRobot()->get()->getDistanceToBall() < constants::TURN_ON_DRIBBLER_DISTANCE) { stpInfos["ball_placer"].setDribblerOn(true); } } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp index 5a146c818..5528376dc 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp @@ -18,7 +18,7 @@ FreeKickThem::FreeKickThem() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::FreeKickThemGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), @@ -37,7 +37,7 @@ FreeKickThem::FreeKickThem() : Play() { uint8_t FreeKickThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap FreeKickThem::decideRoleFlags() const noexcept { @@ -73,7 +73,7 @@ void FreeKickThem::calculateInfoForHarasser() noexcept { auto enemyClosestToBall = enemyClosestToBallOpt.value(); auto enemyToBall = (ballPos - enemyClosestToBall->getPos()); - auto targetPos = ballPos + (enemyToBall).stretchToLength(control_constants::AVOID_BALL_DISTANCE); + auto targetPos = ballPos + (enemyToBall).stretchToLength(constants::AVOID_BALL_DISTANCE); stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setYaw(enemyToBall.angle() + M_PI); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp index 7f9b15120..84bc5e166 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp @@ -19,7 +19,7 @@ FreeKickUsAtGoal::FreeKickUsAtGoal() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::FreeKickUsGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("free_kick_taker"), diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index 24dd10ba2..130f8132d 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -7,6 +7,7 @@ #include "stp/roles/active/PassReceiver.h" #include "stp/roles/passive/Defender.h" #include "stp/roles/passive/Formation.h" +#include "utilities/RuntimeConfig.h" namespace rtt::ai::stp::play { @@ -17,11 +18,10 @@ FreeKickUsPass::FreeKickUsPass() : Play() { // 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::NormalOrFreeKickUsGameState); - keepPlayEvaluation.emplace_back(GlobalEvaluation::WeWillHaveBall); + keepPlayEvaluation.emplace_back(GlobalEvaluation::FreeKickUsGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("free_kick_taker"), @@ -32,9 +32,9 @@ FreeKickUsPass::FreeKickUsPass() : Play() { // Additional roles if we play 11v11 std::make_unique("waller_0"), std::make_unique("waller_1"), - std::make_unique("attacker_1"), std::make_unique("defender_2"), - std::make_unique("attacker_2"), + std::make_unique("defender_3"), + std::make_unique("attacker_1"), }; } @@ -57,9 +57,9 @@ Dealer::FlagMap FreeKickUsPass::decideRoleFlags() const noexcept { 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({"attacker_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"attacker_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"attacker_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); return flagMap; } @@ -74,7 +74,7 @@ void FreeKickUsPass::calculateInfoForRoles() noexcept { stpInfos["free_kick_taker"].setPositionToShootAt(passInfo.receiverLocation); stpInfos["free_kick_taker"].setShotType(ShotType::PASS); stpInfos["free_kick_taker"].setKickOrChip(KickOrChip::KICK); - if (GameStateManager::getCurrentGameState().timeLeft < 1.5) { + if (RuntimeConfig::useReferee && GameStateManager::getCurrentGameState().timeLeft < 1.5) { stpInfos["free_kick_taker"].setPositionToShootAt(field.rightDefenseArea.rightLine().center()); stpInfos["free_kick_taker"].setShotType(ShotType::MAX); } @@ -104,18 +104,15 @@ bool FreeKickUsPass::ballKicked() { } bool FreeKickUsPass::shouldEndPlay() noexcept { - // True if receiver has ball - if (stpInfos["receiver"].getRobot() && stpInfos["receiver"].getRobot().value()->hasBall()) return true; - - // If the ball is moving too slow after we have kicked it, we should stop the play to get the ball + // If the ball is kicked, we end the play to already prepare for what happens next if (ballKicked()) return true; // True if a different pass has a higher score than the current pass (by some margin)- only if the passer is not already close to the ball (since we don't want to adjust our // target when we're in the process of shooting - if (!ballKicked() && stpInfos["free_kick_taker"].getRobot() && !stpInfos["free_kick_taker"].getRobot().value()->hasBall() && + if (stpInfos["free_kick_taker"].getRobot() && !stpInfos["free_kick_taker"].getRobot().value()->hasBall() && stp::computations::PassComputations::calculatePass(gen::AttackingPass, world, field).passScore > 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::AttackingPass, field, world).score && - GameStateManager::getCurrentGameState().timeLeft > 1.5) + (!RuntimeConfig::useReferee || GameStateManager::getCurrentGameState().timeLeft > 1.5)) return true; return false; diff --git a/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp b/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp index 044f0d9d3..3e6087e70 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/Halt.cpp @@ -15,7 +15,7 @@ Halt::Halt() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::HaltGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("halt_0"), std::make_unique("halt_1"), std::make_unique("halt_2"), std::make_unique("halt_3"), std::make_unique("halt_4"), std::make_unique("halt_5"), @@ -26,7 +26,7 @@ Halt::Halt() : Play() { uint8_t Halt::score(const rtt::Field &) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap Halt::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp index ad7f04095..9b78d3c2f 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThem.cpp @@ -16,7 +16,7 @@ KickOffThem::KickOffThem() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::KickOffThemGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("halt_0"), @@ -35,7 +35,7 @@ KickOffThem::KickOffThem() : Play() { uint8_t KickOffThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap KickOffThem::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp index 22cd0dbfd..44252f50d 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp @@ -15,7 +15,7 @@ KickOffThemPrepare::KickOffThemPrepare() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::KickOffThemPrepareGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("formation_back_0"), @@ -34,7 +34,7 @@ KickOffThemPrepare::KickOffThemPrepare() : Play() { uint8_t KickOffThemPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap KickOffThemPrepare::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index f8823c76a..8911beb94 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -15,11 +15,10 @@ KickOffUs::KickOffUs() : Play() { // 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::KickOffUsOrNormalGameState); - keepPlayEvaluation.emplace_back(GlobalEvaluation::WeWillHaveBall); + keepPlayEvaluation.emplace_back(GlobalEvaluation::KickOffUsGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("kick_off_taker"), std::make_unique("receiver"), std::make_unique("halt_0"), std::make_unique("halt_1"), std::make_unique("halt_2"), @@ -30,7 +29,7 @@ KickOffUs::KickOffUs() : Play() { uint8_t KickOffUs::score(const rtt::Field &) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap KickOffUs::decideRoleFlags() const noexcept { @@ -72,7 +71,7 @@ void KickOffUs::calculateInfoForRoles() noexcept { } bool KickOffUs::shouldEndPlay() noexcept { - if (stpInfos["receiver"].getRobot() && stpInfos["receiver"].getRobot().value()->hasBall()) return true; + // If the ball is kicked, we end the play to already prepare for what happens next if (ballKicked()) return true; if (stpInfos["receiver"].getRobot() && world->getWorld()->getBall()->get()->position.x < stpInfos["receiver"].getRobot()->get()->getPos().x) return true; diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp index a1424e23a..7a2bb4771 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp @@ -15,7 +15,7 @@ KickOffUsPrepare::KickOffUsPrepare() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::KickOffUsPrepareGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("kicker"), @@ -34,7 +34,7 @@ KickOffUsPrepare::KickOffUsPrepare() : Play() { uint8_t KickOffUsPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap KickOffUsPrepare::decideRoleFlags() const noexcept { @@ -63,7 +63,7 @@ void KickOffUsPrepare::calculateInfoForRoles() noexcept { PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); // The "kicker" will go to the ball - stpInfos["kicker"].setPositionToMoveTo(Vector2(-control_constants::AVOID_BALL_DISTANCE, 0.0)); + stpInfos["kicker"].setPositionToMoveTo(Vector2(-constants::AVOID_BALL_DISTANCE, 0.0)); } const char* KickOffUsPrepare::getName() const { return "Kick Off Us Prepare"; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp index f9caf91e9..f18c516be 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThem.cpp @@ -16,7 +16,7 @@ PenaltyThem::PenaltyThem() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::PenaltyThemGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("halt_0"), std::make_unique("halt_1"), std::make_unique("halt_2"), std::make_unique("halt_3"), std::make_unique("halt_4"), @@ -27,7 +27,7 @@ PenaltyThem::PenaltyThem() : Play() { uint8_t PenaltyThem::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap PenaltyThem::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp index c89f91679..dd1bc8e4e 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyThemPrepare.cpp @@ -18,7 +18,7 @@ PenaltyThemPrepare::PenaltyThemPrepare() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::PenaltyThemPrepareGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("formation_0"), @@ -37,7 +37,7 @@ PenaltyThemPrepare::PenaltyThemPrepare() : Play() { uint8_t PenaltyThemPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap PenaltyThemPrepare::decideRoleFlags() const noexcept { @@ -67,7 +67,7 @@ void PenaltyThemPrepare::calculateInfoForRoles() noexcept { auto ballPosition = world->getWorld()->getBall(); // If there is no ball, use the default division A penalty mark position double ballX = ballPosition.has_value() ? ballPosition.value()->position.x : PENALTY_MARK_THEM_X; - double limitX = std::max(ballX, PENALTY_MARK_THEM_X) + Constants::PENALTY_DISTANCE_BEHIND_BALL(); + double limitX = std::max(ballX, PENALTY_MARK_THEM_X) + constants::PENALTY_DISTANCE_BEHIND_BALL; // First, figure out at what interval the robots will stand on a horizontal line double horizontalRange = std::fabs(field.playArea.right() - limitX); diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp index 668dfc286..c7be6ecd8 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp @@ -5,6 +5,7 @@ #include "stp/roles/Keeper.h" #include "stp/roles/active/PenaltyTaker.h" #include "stp/roles/passive/Halt.h" +#include "utilities/RuntimeConfig.h" namespace rtt::ai::stp::play { const char* PenaltyUs::getName() const { return "Penalty Us"; } @@ -19,7 +20,7 @@ PenaltyUs::PenaltyUs() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::PenaltyUsGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("kicker"), std::make_unique("halt_0"), std::make_unique("halt_1"), std::make_unique("halt_2"), std::make_unique("halt_3"), @@ -30,7 +31,7 @@ PenaltyUs::PenaltyUs() : Play() { uint8_t PenaltyUs::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap PenaltyUs::decideRoleFlags() const noexcept { @@ -57,7 +58,7 @@ Dealer::FlagMap PenaltyUs::decideRoleFlags() const noexcept { void PenaltyUs::calculateInfoForRoles() noexcept { auto positionTarget = PositionComputations::getPosition(std::nullopt, field.middleRightGrid, gen::GoalShot, field, world); - if (GameStateManager::getCurrentGameState().timeLeft > 3.0) { + if (!RuntimeConfig::useReferee || GameStateManager::getCurrentGameState().timeLeft > 3.0) { stpInfos["kicker"].setPositionToMoveTo(positionTarget); } else { stpInfos["kicker"].setPositionToMoveTo(stpInfos["kicker"].getRobot()->get()->getPos()); diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp index 87ca9c704..487cad3ca 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUsPrepare.cpp @@ -17,7 +17,7 @@ PenaltyUsPrepare::PenaltyUsPrepare() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::PenaltyUsPrepareGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("kicker_formation"), @@ -36,7 +36,7 @@ PenaltyUsPrepare::PenaltyUsPrepare() : Play() { uint8_t PenaltyUsPrepare::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap PenaltyUsPrepare::decideRoleFlags() const noexcept { @@ -73,7 +73,7 @@ void PenaltyUsPrepare::calculateInfoForRoles() noexcept { auto ballPosition = world->getWorld()->getBall(); // If there is no ball, use the default division A penalty mark position double ballX = ballPosition.has_value() ? ballPosition.value()->position.x : PENALTY_MARK_US_X; - double limitX = std::min(ballX, PENALTY_MARK_US_X) - Constants::PENALTY_DISTANCE_BEHIND_BALL(); + double limitX = std::min(ballX, PENALTY_MARK_US_X) - constants::PENALTY_DISTANCE_BEHIND_BALL; // Then, figure out at what interval the robots will stand on a horizontal line double horizontalRange = std::fabs(field.playArea.left() - limitX); diff --git a/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp index ea3bd8a1a..916356bac 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp @@ -18,7 +18,7 @@ PrepareForcedStart::PrepareForcedStart() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::PrepareForcedStartGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), @@ -37,7 +37,7 @@ PrepareForcedStart::PrepareForcedStart() : Play() { uint8_t PrepareForcedStart::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap PrepareForcedStart::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp b/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp index f012914e9..f62eeb485 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp @@ -17,7 +17,7 @@ StopFormation::StopFormation() : Play() { keepPlayEvaluation.emplace_back(GlobalEvaluation::StopGameState); // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::Constants::ROBOT_COUNT()>{ + roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), @@ -36,7 +36,7 @@ StopFormation::StopFormation() : Play() { uint8_t StopFormation::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play - return control_constants::FUZZY_TRUE; + return constants::FUZZY_TRUE; } Dealer::FlagMap StopFormation::decideRoleFlags() const noexcept { diff --git a/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp b/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp index c10651906..ebbd22b8f 100644 --- a/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp +++ b/roboteam_ai/src/stp/roles/PenaltyKeeper.cpp @@ -25,7 +25,7 @@ Status PenaltyKeeper::update(StpInfo const& info) noexcept { } // Stop Formation tactic when ball is moving, start blocking, getting the ball and pass (normal keeper behavior) - if (robotTactics.current_num() == 0 && info.getBall().value()->velocity.length() > control_constants::BALL_STILL_VEL) { + if (robotTactics.current_num() == 0 && info.getBall().value()->velocity.length() > constants::BALL_STILL_VEL) { forceNextTactic(); } diff --git a/roboteam_ai/src/stp/skills/Chip.cpp b/roboteam_ai/src/stp/skills/Chip.cpp index 0efa92f04..3972666fd 100644 --- a/roboteam_ai/src/stp/skills/Chip.cpp +++ b/roboteam_ai/src/stp/skills/Chip.cpp @@ -1,11 +1,11 @@ #include "stp/skills/Chip.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::skill { Status Chip::onUpdate(const StpInfo &info) noexcept { - float chipVelocity = std::clamp(info.getKickChipVelocity(), stp::control_constants::MIN_CHIP_POWER, stp::control_constants::MAX_CHIP_POWER); + float chipVelocity = std::clamp(info.getKickChipVelocity(), constants::MIN_CHIP_POWER, constants::MAX_CHIP_POWER); command.kickType = KickType::CHIP; command.kickSpeed = chipVelocity; @@ -13,23 +13,16 @@ Status Chip::onUpdate(const StpInfo &info) noexcept { command.yaw = info.getRobot().value()->getYaw(); - if (chipAttempts > control_constants::MAX_CHIP_ATTEMPTS) { - command.waitForBall = false; - chipAttempts = 0; - } else { - command.waitForBall = true; - } + command.waitForBall = true; command.id = info.getRobot().value()->getId(); forwardRobotCommand(); - if (info.getBall()->get()->velocity.length() > stp::control_constants::HAS_CHIPPED_ERROR_MARGIN) { - chipAttempts = 0; + if (info.getBall()->get()->velocity.length() > constants::HAS_CHIPPED_ERROR_MARGIN) { return Status::Success; } - ++chipAttempts; return Status::Running; } diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index b8f71beb8..fd396a330 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -21,8 +21,19 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { if (robotToTarget.doesIntersect(ballToReferee)) { double distance1 = (robot->getPos() - ballLocation).length() + (ballLocation - targetPos).length(); double distance2 = (robot->getPos() - ballPlacementPos).length() + (ballPlacementPos - targetPos).length(); - targetPos = (distance1 < distance2) ? ballLocation - (ballPlacementPos - ballLocation).stretchToLength(0.8) - : ballPlacementPos - (robot->getPos() - ballPlacementPos).stretchToLength(0.8); + Vector2 point1 = ballLocation - (ballPlacementPos - ballLocation).stretchToLength(0.8); + Vector2 point2 = ballPlacementPos - (robot->getPos() - ballPlacementPos).stretchToLength(0.8); + bool point1InField = field.playArea.contains(point1, constants::OUT_OF_FIELD_MARGIN); + bool point2InField = field.playArea.contains(point2, constants::OUT_OF_FIELD_MARGIN); + if (point1InField && point2InField) { + targetPos = (distance1 < distance2) ? point1 : point2; + } else if (point1InField) { + targetPos = point1; + } else if (point2InField) { + targetPos = point2; + } else { + targetPos = FieldComputations::projectPointToValidPosition(field, targetPos, avoidObj); + } targetPos = targetPos - (robot->getPos() - targetPos).stretchToLength(0.8); } } @@ -37,7 +48,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { if (roleName != "ball_placer" && (avoidObj.shouldAvoidBall || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT)) { - targetPos = PositionComputations::calculateAvoidBallPosition(targetPos, ballLocation, field); + targetPos = PositionComputations::calculateAvoidBallPosition(targetPos, ballLocation, field, avoidObj); } command.velocity = info.getCurrentWorld()->getRobotPositionController()->computeAndTrackTrajectory(info.getCurrentWorld(), field, robot->getId(), robot->getPos(), @@ -56,8 +67,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { // Check if successful auto distanceError = (robot->getPos() - targetPos).length(); - if (distanceError <= stp::control_constants::GO_TO_POS_ERROR_MARGIN || - (robot->hasBall() && distanceError <= stp::control_constants::BALL_PLACEMENT_MARGIN - stp::control_constants::GO_TO_POS_ERROR_MARGIN)) { + if (robot->hasBall() && distanceError <= constants::BALL_PLACEMENT_MARGIN - constants::GO_TO_POS_ERROR_MARGIN) { return Status::Success; } else { return Status::Running; diff --git a/roboteam_ai/src/stp/skills/Kick.cpp b/roboteam_ai/src/stp/skills/Kick.cpp index ff458d4d7..aa3fb489b 100644 --- a/roboteam_ai/src/stp/skills/Kick.cpp +++ b/roboteam_ai/src/stp/skills/Kick.cpp @@ -1,7 +1,7 @@ #include "stp/skills/Kick.h" #include "roboteam_utils/Print.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::skill { @@ -9,7 +9,7 @@ Status Kick::onUpdate(const StpInfo &info) noexcept { auto robot = info.getRobot().value(); // Clamp and set kick velocity - float kickVelocity = std::clamp(info.getKickChipVelocity(), control_constants::MIN_KICK_POWER, control_constants::MAX_KICK_POWER); + float kickVelocity = std::clamp(info.getKickChipVelocity(), constants::MIN_KICK_POWER, constants::MAX_KICK_POWER); command.kickType = KickType::KICK; command.kickSpeed = kickVelocity; @@ -26,7 +26,7 @@ Status Kick::onUpdate(const StpInfo &info) noexcept { // forward the generated command to the ControlModule, for checking and limiting forwardRobotCommand(); - if (!robot->hasBall() && info.getBall()->get()->velocity.length() > control_constants::BALL_GOT_SHOT_LIMIT) { + if (!robot->hasBall() && info.getBall()->get()->velocity.length() > constants::BALL_GOT_SHOT_LIMIT) { return Status::Success; } return Status::Running; diff --git a/roboteam_ai/src/stp/skills/Orbit.cpp b/roboteam_ai/src/stp/skills/Orbit.cpp index efab3e086..d1125894d 100644 --- a/roboteam_ai/src/stp/skills/Orbit.cpp +++ b/roboteam_ai/src/stp/skills/Orbit.cpp @@ -1,6 +1,6 @@ #include "stp/skills/Orbit.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::skill { @@ -12,7 +12,7 @@ Status Orbit::onUpdate(const StpInfo &info) noexcept { double normalAngle = directionVector.rotate(M_PI).rotate(M_PI_2).angle(); Angle yaw = (info.getPositionToShootAt().value() - ball->position).toAngle(); - double margin = 1.5 * control_constants::ROBOT_RADIUS + stp::control_constants::BALL_RADIUS; + double margin = 1.5 * constants::ROBOT_RADIUS + constants::BALL_RADIUS; double adjustDistance = robot->getDistanceToBall() - margin; // Get the direction of movement, counterclockwise or clockwise @@ -41,7 +41,7 @@ Status Orbit::onUpdate(const StpInfo &info) noexcept { forwardRobotCommand(); // Check if successful - double errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; + double errorMargin = constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; if (directionVector.toAngle().shortestAngleDiff(yaw) < errorMargin) { counter++; } else { diff --git a/roboteam_ai/src/stp/skills/OrbitAngular.cpp b/roboteam_ai/src/stp/skills/OrbitAngular.cpp index ae84e0edc..36b231d32 100644 --- a/roboteam_ai/src/stp/skills/OrbitAngular.cpp +++ b/roboteam_ai/src/stp/skills/OrbitAngular.cpp @@ -1,6 +1,7 @@ #include "stp/skills/OrbitAngular.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" +#include "utilities/GameSettings.h" namespace rtt::ai::stp::skill { @@ -21,7 +22,7 @@ Status OrbitAngular::onUpdate(const StpInfo &info) noexcept { Vector2 normalVector = currentYaw.toVector2().rotate(-direction * M_PI_2); // Calculate target velocity - Vector2 targetVelocity = normalVector * speedFactor * (stp::control_constants::BALL_RADIUS + stp::control_constants::CENTER_TO_FRONT); + Vector2 targetVelocity = normalVector * speedFactor * (constants::BALL_RADIUS + constants::CENTER_TO_FRONT); // Construct the robot command command.id = robot->getId(); @@ -32,14 +33,19 @@ Status OrbitAngular::onUpdate(const StpInfo &info) noexcept { // command.targetAngularVelocity = targetAngularVelocity; // command.yaw = yaw; - // target yaw is angular velocity times the time step (1/60th of a second) - command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / 2.5); - command.dribblerOn = stp::control_constants::MAX_DRIBBLER_CMD; + // target yaw is angular velocity times the time step (1/60th of a second) in basestation + // in simulator, we multiple by 1/2.5 because of how the simulator works and the pid controller is tuned + if (rtt::GameSettings::getRobotHubMode() == rtt::net::RobotHubMode::BASESTATION) { + command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / constants::STP_TICK_RATE); + } else { + command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / 2.5); + } + command.dribblerOn = true; forwardRobotCommand(); // Send the robot command // Check if the robot is within the error margin - double errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; + double errorMargin = constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; if (currentYaw.shortestAngleDiff(yaw) < errorMargin) { withinMarginCount++; } else { diff --git a/roboteam_ai/src/stp/skills/Rotate.cpp b/roboteam_ai/src/stp/skills/Rotate.cpp index 4aa2fcac8..6bada46dc 100644 --- a/roboteam_ai/src/stp/skills/Rotate.cpp +++ b/roboteam_ai/src/stp/skills/Rotate.cpp @@ -1,7 +1,7 @@ #include "stp/skills/Rotate.h" #include "control/ControlUtils.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::skill { @@ -21,7 +21,7 @@ Status Rotate::onUpdate(const StpInfo &info) noexcept { forwardRobotCommand(); // Check if the robot is within the error margin - double errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; + double errorMargin = constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; if (robot->getYaw().shortestAngleDiff(yaw) < errorMargin) { withinMarginCount++; } else { diff --git a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp index 083e8685e..20b5b32df 100644 --- a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp @@ -5,26 +5,28 @@ #include #include +#include + #include "control/ControlUtils.h" +#include "gui/Out.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" namespace rtt::ai::stp::tactic { // We do not want the keeper to stand completely inside the goal, but a tiny bit outside. -const double KEEPER_DISTANCE_TO_GOAL_LINE = Constants::ROBOT_RADIUS() * std::sin(toRadians(80.0)); +const double KEEPER_DISTANCE_TO_GOAL_LINE = constants::ROBOT_RADIUS * std::sin(toRadians(80.0)); // And by standing a tiny bit inside, we cannot move completely to a goal side. This is by how much less that is. -const double KEEPER_GOAL_DECREASE_AT_ONE_SIDE = Constants::ROBOT_RADIUS() * std::cos(toRadians(80.0)) + 0.01; // Plus a small margin to prevent keeper from crashing into goal +const double KEEPER_GOAL_DECREASE_AT_ONE_SIDE = constants::ROBOT_RADIUS * std::cos(toRadians(80.0)) + 0.01; // Plus a small margin to prevent keeper from crashing into goal // The maximum distance from the goal for when we say the ball is heading towards our goal -constexpr double MAX_DISTANCE_HEADING_TOWARDS_GOAL = 2; +constexpr double MAX_DISTANCE_HEADING_TOWARDS_GOAL = 0.2; // For determining where the keeper should stand to stand between the ball and the goal, we draw a line from the ball to a bit behind the goal constexpr double PROJECT_BALL_DISTANCE_TO_GOAL = 0.5; // Small means keeper will me more in center, big means keeper will be more to the side of the goal // We stop deciding where the keeper should be if the ball is too far behind our own goal -constexpr double MAX_DISTANCE_BALL_BEHIND_GOAL = 0.3; +constexpr double MAX_DISTANCE_BALL_BEHIND_GOAL = 0.1; KeeperBlockBall::KeeperBlockBall() { skills = rtt::collections::state_machine{skill::GoToPos()}; } @@ -53,7 +55,7 @@ bool KeeperBlockBall::isEndTactic() noexcept { return true; } bool KeeperBlockBall::isTacticFailing(const StpInfo &) noexcept { return false; } bool KeeperBlockBall::shouldTacticReset(const StpInfo &info) noexcept { - const double errorMargin = control_constants::GO_TO_POS_ERROR_MARGIN * M_PI; + const double errorMargin = constants::GO_TO_POS_ERROR_MARGIN * M_PI; const auto distanceToTarget = (info.getRobot().value()->getPos() - info.getPositionToMoveTo().value()).length(); return distanceToTarget > errorMargin; } @@ -66,34 +68,6 @@ LineSegment KeeperBlockBall::getKeepersLineSegment(const Field &field) { return LineSegment(keepersLineSegmentLeft, keepersLineSegmentRight); } -std::optional KeeperBlockBall::estimateBallTrajectory(const world::view::BallView &ball, const std::optional &enemyRobot) { - // If the ball is already moving, the trajectory of the ball is clear - if (ball->velocity.length2() > control_constants::BALL_STILL_VEL2) { - const auto start = ball->position; - const auto direction = ball->position + ball->velocity; - return HalfLine(start, direction); - } - - const bool hasEnemy = enemyRobot.has_value() && enemyRobot.value().get() != nullptr; - - // If the enemy robot already has the ball, it will probably kick in the direction it is facing - if (hasEnemy && enemyRobot->get()->hasBall()) { - const auto start = enemyRobot->get()->getPos(); - const auto direction = enemyRobot->get()->getPos() + enemyRobot->get()->getYaw().toVector2(); - return HalfLine(start, direction); - } - - // If the enemy is only a bit close, we look at the direction from the robot to the ball - if (enemyRobot && enemyRobot->get()->getDistanceToBall() < control_constants::ENEMY_CLOSE_TO_BALL_DISTANCE) { - const auto start = enemyRobot->get()->getPos(); - const auto direction = ball->position; - return HalfLine(start, direction); - } - - // Otherwise, the ball probably will not be moved by enemies soon :P - return std::nullopt; -} - bool KeeperBlockBall::isBallHeadingTowardsOurGoal(const HalfLine &ballTrajectory, const Field &field) { const auto goalLineSegment = LineSegment(field.leftGoalArea.bottomRight(), field.leftGoalArea.topRight()); const auto intersectionWithGoalLine = ballTrajectory.intersect(Line(goalLineSegment)); @@ -102,49 +76,180 @@ bool KeeperBlockBall::isBallHeadingTowardsOurGoal(const HalfLine &ballTrajectory } std::pair KeeperBlockBall::calculateTargetPosition(const StpInfo info) noexcept { - bool shouldAvoidGoalPosts = true; const auto &field = info.getField().value(); const auto &ball = info.getBall().value(); - const auto &world = info.getCurrentWorld(); - const auto &robot = info.getRobot().value(); - const auto enemyRobot = world->getWorld()->getRobotClosestToBall(world::them); const auto keepersLineSegment = getKeepersLineSegment(field); - const auto ballTrajectory = estimateBallTrajectory(ball, enemyRobot); - const bool ballHeadsTowardsOurGoal = ballTrajectory.has_value() && isBallHeadingTowardsOurGoal(ballTrajectory.value(), field); + const LineSegment ballTrajectory(ball->position, ball->expectedEndPosition); + bool ballHeadsTowardsOurGoal = ballTrajectory.intersects(keepersLineSegment).has_value(); if (ballHeadsTowardsOurGoal) { - const auto targetPosition = keepersLineSegment.getClosestPointToLine(ballTrajectory->toLine()); - if (targetPosition.has_value()) { - shouldAvoidGoalPosts = false; - const auto targetTime = FieldComputations::getBallTimeAtPosition(*ball.get(), targetPosition.value()); - const auto startPosition = robot->getPos(); - const auto startVelocity = robot->getVel(); - const auto maxVelocity = info.getMaxRobotVelocity(); - const auto maxAcceleration = Constants::MAX_ACC(); - const auto newTarget = - control::OvershootComputations::overshootingDestination(startPosition, targetPosition.value(), startVelocity, maxVelocity, maxAcceleration, targetTime); - return {newTarget, shouldAvoidGoalPosts}; - } + auto shouldAvoidGoalPosts = false; + return {calculateTargetPositionBallShot(info, keepersLineSegment, ballTrajectory), shouldAvoidGoalPosts}; } + // This lets the keeper intercept the ball in the defense area when it's not heading towards our goal. This did not work well at the Schubert open, since we always just barely + // missed the ball. Might be tweaking control constants const KeeperInterceptionInfo keeperInterceptionInfo = InterceptionComputations::calculateKeeperInterceptionInfo(world, + // robot); if (keeperInterceptionInfo.canIntercept) { + // } // const KeeperInterceptionInfo keeperInterceptionInfo = InterceptionComputations::calculateKeeperInterceptionInfo(world, robot); // if (keeperInterceptionInfo.canIntercept) { // return keeperInterceptionInfo.interceptLocation; // } if (ball->position.x >= field.leftGoalArea.rightLine().center().x - MAX_DISTANCE_BALL_BEHIND_GOAL) { - const auto ballGoalLine = Line(ball->position, field.leftGoalArea.rightLine().center() - Vector2(PROJECT_BALL_DISTANCE_TO_GOAL, 0)); - const auto targetPosition = keepersLineSegment.getClosestPointToLine(ballGoalLine); - if (targetPosition.has_value()) { - return {targetPosition.value(), shouldAvoidGoalPosts}; - } + auto shouldAvoidGoalPosts = true; + std::optional predictedBallPositionTheirRobot = calculateTheirBallInterception(info, ballTrajectory); + return {calculateTargetPositionBallNotShot(info, predictedBallPositionTheirRobot), shouldAvoidGoalPosts}; } // Default case: go to the center of the goal + auto shouldAvoidGoalPosts = true; return {Vector2(keepersLineSegment.start.x, 0), shouldAvoidGoalPosts}; } +Vector2 KeeperBlockBall::calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept { + const auto &field = info.getField().value(); + const auto &ball = info.getBall().value(); + const auto &robot = info.getRobot().value(); + const auto robotPosition = robot->getPos(); + const auto robotVelocity = robot->getVel(); + const auto maxRobotVelocity = info.getMaxRobotVelocity(); + const auto maxRobotAcceleration = rtt::ai::constants::MAX_ACC; + const auto closestPointToGoal = Line(ballTrajectory).intersect(Line(keepersLineSegment)); + + // If possible, we intercept the ball at the line + if (closestPointToGoal.has_value()) { + const auto ballTimeAtClosestPoint = FieldComputations::getBallTimeAtPosition(*ball.get(), closestPointToGoal.value()); + auto [targetPosition, timeToTarget] = control::OvershootComputations::overshootingDestination(robotPosition, closestPointToGoal.value(), robotVelocity, maxRobotVelocity, + maxRobotAcceleration, ballTimeAtClosestPoint); + if (timeToTarget <= ballTimeAtClosestPoint) { + return targetPosition; + } + } + + double maxTimeLeftWhenArrived = std::numeric_limits::lowest(); + Vector2 optimalTarget = Vector2(); + for (double timeStep = 0.01; timeStep <= 3; timeStep += 0.01) { + auto predictedBallPosition = FieldComputations::getBallPositionAtTime(*ball.get(), timeStep); + + if (!field.leftDefenseArea.contains(predictedBallPosition)) { + continue; + } + + auto [currentTarget, currentTimeToTarget] = + control::OvershootComputations::overshootingDestination(robotPosition, predictedBallPosition, robotVelocity, maxRobotVelocity, maxRobotAcceleration, timeStep); + + double timeLeftWhenArrived = timeStep - currentTimeToTarget; + + if (timeLeftWhenArrived > maxTimeLeftWhenArrived) { + maxTimeLeftWhenArrived = timeLeftWhenArrived; + optimalTarget = currentTarget; + } + } + return optimalTarget; +} + +Vector2 KeeperBlockBall::calculateTargetPositionBallNotShot(const StpInfo &info, std::optional predictedBallPositionTheirRobot) noexcept { + const auto &field = info.getField().value(); + const auto &ball = info.getBall().value(); + const auto &robot = info.getRobot().value(); + Vector2 leftGoalPost = field.leftGoalArea.topRight(); + Vector2 rightGoalPost = field.leftGoalArea.bottomRight(); + Vector2 targetFromPos = predictedBallPositionTheirRobot ? predictedBallPositionTheirRobot.value() : ball->position; + std::array rightGoalPostArr = {targetFromPos, rightGoalPost}; + std::array leftGoalPostArr = {targetFromPos, leftGoalPost}; + gui::Out::draw( + { + .label = "ballToLeftGoalPost", + .color = proto::Drawing::MAGENTA, + .method = proto::Drawing::LINES_CONNECTED, + .category = proto::Drawing::DEBUG, + .forRobotId = robot->getId(), + .thickness = 1, + }, + rightGoalPostArr); + gui::Out::draw( + { + .label = "ballToRightGoalPost", + .color = proto::Drawing::MAGENTA, + .method = proto::Drawing::LINES_CONNECTED, + .category = proto::Drawing::DEBUG, + .forRobotId = robot->getId(), + .thickness = 1, + }, + leftGoalPostArr); + + Vector2 ballToLeftGoalPost = leftGoalPost - targetFromPos; + Vector2 ballToRightGoalPost = rightGoalPost - targetFromPos; + Vector2 bisectorUnitVector = (ballToLeftGoalPost.normalize() + ballToRightGoalPost.normalize()).normalize(); + auto scalingFactor = (field.leftGoalArea.rightLine().center().x - targetFromPos.x) / bisectorUnitVector.x; + Vector2 bisectorPoint = targetFromPos + bisectorUnitVector * scalingFactor; + + std::array bisectorArr = {targetFromPos, bisectorPoint}; + gui::Out::draw( + { + .label = "bisectorLine", + .color = proto::Drawing::RED, + .method = proto::Drawing::LINES_CONNECTED, + .category = proto::Drawing::DEBUG, + .forRobotId = robot->getId(), + .thickness = 2, + }, + bisectorArr); + + // project the leftGoalPost on the ball->pos bisectorPoint line if the ball has positive x, else project the rightGoalPost + Vector2 targetGoalPost = targetFromPos.y >= 0 ? leftGoalPost : rightGoalPost; + auto targetPositionNew = LineSegment(targetFromPos, bisectorPoint).project(targetGoalPost); + std::array goalPostArr = {targetGoalPost, targetPositionNew}; + gui::Out::draw( + { + .label = "targetPositionNew", + .color = proto::Drawing::GREEN, + .method = proto::Drawing::LINES_CONNECTED, + .category = proto::Drawing::DEBUG, + .forRobotId = robot->getId(), + .thickness = 1, + }, + goalPostArr); + return targetPositionNew; +} + +std::optional KeeperBlockBall::calculateTheirBallInterception(const StpInfo &info, rtt::LineSegment ballTrajectory) noexcept { + std::optional predictedBallPositionTheirRobot = std::nullopt; + const auto ball = info.getBall().value(); + double minDistanceToBall = std::numeric_limits::max(); + for (const auto &theirRobot : info.getCurrentWorld()->getWorld()->getThem()) { + std::optional vecPts = ballTrajectory.project(theirRobot.get()->getPos()); + // see if the distance between the projected point and the robot is less than 0.5m + if (vecPts.value().dist(theirRobot.get()->getPos()) < 0.5) { + double dist = vecPts.value().dist(ball->position); + if (dist < minDistanceToBall) { + minDistanceToBall = dist; + predictedBallPositionTheirRobot = vecPts.value(); + predictedBallPositionTheirRobot = + predictedBallPositionTheirRobot.value() + (ball->position - predictedBallPositionTheirRobot.value()).normalize() * constants::CENTER_TO_FRONT; + } + } + } + if (predictedBallPositionTheirRobot) { + std::vector vec = {predictedBallPositionTheirRobot.value()}; + std::span spanVec = vec; + + rtt::ai::gui::Out::draw( + { + .label = "Interception Point", + .color = proto::Drawing::CYAN, + .method = proto::Drawing::CIRCLES, + .category = proto::Drawing::DEBUG, + .size = 15, + .thickness = 7, + }, + spanVec); + } + return predictedBallPositionTheirRobot; +} + Angle KeeperBlockBall::calculateYaw(const world::view::BallView &ball, const Vector2 &targetKeeperPosition) { // Look towards ball to ensure ball hits the front assembly to reduce odds of ball reflecting in goal const auto keeperToBall = (ball->position - targetKeeperPosition) / 1.6; diff --git a/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp b/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp index 747749f7d..cfe07fba6 100644 --- a/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp +++ b/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp @@ -1,9 +1,9 @@ #include "stp/tactics/active/ChipAtPos.h" #include "control/ControlUtils.h" -#include "stp/constants/ControlConstants.h" #include "stp/skills/Chip.h" #include "stp/skills/OrbitAngular.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::tactic { @@ -37,7 +37,7 @@ bool ChipAtPos::isTacticFailing(const StpInfo &info) noexcept { bool ChipAtPos::shouldTacticReset(const StpInfo &info) noexcept { if (skills.current_num() != 0) { - auto errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; + auto errorMargin = constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; return info.getRobot().value()->getYaw().shortestAngleDiff(info.getYaw()) > errorMargin; } return false; diff --git a/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp b/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp index 4e145fe84..8ca7a2a03 100644 --- a/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp @@ -21,7 +21,7 @@ std::optional DriveWithBall::calculateInfoForSkill(const StpInfo &info) bool DriveWithBall::isTacticFailing(const StpInfo &info) noexcept { return !info.getRobot().value()->hasBall() || !info.getPositionToMoveTo(); } bool DriveWithBall::shouldTacticReset(const StpInfo &info) noexcept { - return skills.current_num() == 1 && info.getRobot()->get()->getYaw().shortestAngleDiff(info.getYaw()) > Constants::HAS_BALL_ANGLE(); + return skills.current_num() == 1 && info.getRobot()->get()->getYaw().shortestAngleDiff(info.getYaw()) > constants::HAS_BALL_ANGLE; } bool DriveWithBall::isEndTactic() noexcept { return false; } diff --git a/roboteam_ai/src/stp/tactics/active/GetBall.cpp b/roboteam_ai/src/stp/tactics/active/GetBall.cpp index 285732aef..c3536ecc3 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -39,10 +39,18 @@ std::optional GetBall::calculateInfoForSkill(const StpInfo &info) noexc double distanceToInterception = (interceptionPosition - robotPosition).length(); double distanceToBall = (ballPosition - robotPosition).length(); - if (info.getRobot()->get()->getAngleDiffToBall() > Constants::HAS_BALL_ANGLE() && distanceToBall < control_constants::ROBOT_CLOSE_TO_POINT) { - skillStpInfo.setPositionToMoveTo(FieldComputations::projectPointToValidPosition(info.getField().value(), info.getRobot()->get()->getPos(), info.getObjectsToAvoid())); + // Don't go to the ball if we are close to the ball and the angle is too big + // If the ball is moving, we move to the interception point with the front assembly, and the rest of the robot is more away from the ball + // Something like: ball ...... interceptionPoint. robot + // If the ball is not moving (or slow), we move to the interception point with the center of the robot, and the rest of the robot is more away from the ball + // This makes sure we always hit the ball with our front assembly, and we never go to the wrong 'side' of the ball. + if (info.getRobot()->get()->getAngleDiffToBall() > constants::HAS_BALL_ANGLE && distanceToBall < constants::ROBOT_CLOSE_TO_POINT) { + skillStpInfo.setPositionToMoveTo(info.getRobot()->get()->getPos()); + } else if (info.getBall()->get()->velocity.length() > constants::BALL_IS_MOVING_SLOW_LIMIT) { + auto newRobotPos = interceptionPosition + (interceptionPosition - ballPosition).stretchToLength(constants::CENTER_TO_FRONT); + skillStpInfo.setPositionToMoveTo(newRobotPos); } else { - auto getBallDistance = std::max(distanceToInterception - control_constants::CENTER_TO_FRONT, MIN_DISTANCE_TO_TARGET); + auto getBallDistance = std::max(distanceToInterception - constants::CENTER_TO_FRONT, MIN_DISTANCE_TO_TARGET); Vector2 newRobotPosition = robotPosition + (interceptionPosition - robotPosition).stretchToLength(getBallDistance); newRobotPosition = FieldComputations::projectPointToValidPosition(info.getField().value(), newRobotPosition, info.getObjectsToAvoid()); skillStpInfo.setPositionToMoveTo(newRobotPosition); @@ -50,7 +58,7 @@ std::optional GetBall::calculateInfoForSkill(const StpInfo &info) noexc skillStpInfo.setYaw((ballPosition - robotPosition).angle()); - if (distanceToBall < control_constants::TURN_ON_DRIBBLER_DISTANCE) { + if (distanceToBall < constants::TURN_ON_DRIBBLER_DISTANCE) { skillStpInfo.setDribblerOn(true); } diff --git a/roboteam_ai/src/stp/tactics/active/GetBehindBallInDirection.cpp b/roboteam_ai/src/stp/tactics/active/GetBehindBallInDirection.cpp index 0cd675de9..509684817 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBehindBallInDirection.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBehindBallInDirection.cpp @@ -3,14 +3,14 @@ #include #include "roboteam_utils/Circle.h" -#include "stp/constants/ControlConstants.h" #include "stp/skills/GoToPos.h" #include "stp/skills/Orbit.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::tactic { constexpr double DISTANCE_THRESHOLD = 0.5; -constexpr double BALL_AVOID_DISTANCE = 4 * control_constants::ROBOT_RADIUS; +constexpr double BALL_AVOID_DISTANCE = 4 * constants::ROBOT_RADIUS; GetBehindBallInDirection::GetBehindBallInDirection() { skills = collections::state_machine{skill::GoToPos(), skill::Orbit()}; } diff --git a/roboteam_ai/src/stp/tactics/active/InstantKick.cpp b/roboteam_ai/src/stp/tactics/active/InstantKick.cpp index 870a9285b..43af3182b 100644 --- a/roboteam_ai/src/stp/tactics/active/InstantKick.cpp +++ b/roboteam_ai/src/stp/tactics/active/InstantKick.cpp @@ -1,9 +1,9 @@ #include "stp/tactics/active/InstantKick.h" #include "control/ControlUtils.h" -#include "stp/constants/ControlConstants.h" #include "stp/skills/Kick.h" #include "stp/skills/Rotate.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::tactic { diff --git a/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp b/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp index 93841fbf1..9cfad318b 100644 --- a/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp +++ b/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp @@ -28,7 +28,7 @@ bool OrbitKick::isEndTactic() noexcept { return false; } bool OrbitKick::isTacticFailing(const StpInfo &info) noexcept { return !info.getPositionToShootAt() || !info.getRobot()->get()->hasBall(); } bool OrbitKick::shouldTacticReset(const StpInfo &info) noexcept { - const auto errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; + const auto errorMargin = constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; return info.getRobot().value()->getYaw().shortestAngleDiff(info.getYaw()) > errorMargin; } diff --git a/roboteam_ai/src/stp/tactics/active/Receive.cpp b/roboteam_ai/src/stp/tactics/active/Receive.cpp index 6f50b721f..770bd3b79 100644 --- a/roboteam_ai/src/stp/tactics/active/Receive.cpp +++ b/roboteam_ai/src/stp/tactics/active/Receive.cpp @@ -1,8 +1,8 @@ #include "stp/tactics/active/Receive.h" -#include "stp/constants/ControlConstants.h" #include "stp/skills/GoToPos.h" #include "stp/skills/Rotate.h" +#include "utilities/Constants.h" namespace rtt::ai::stp::tactic { @@ -22,7 +22,7 @@ std::optional Receive::calculateInfoForSkill(StpInfo const &info) noexc bool Receive::isTacticFailing(const StpInfo &info) noexcept { return !info.getPositionToMoveTo(); } bool Receive::shouldTacticReset(const StpInfo &info) noexcept { - double errorMargin = stp::control_constants::GO_TO_POS_ERROR_MARGIN * M_PI; + double errorMargin = constants::GO_TO_POS_ERROR_MARGIN * M_PI; return (info.getRobot().value()->getPos() - info.getPositionToMoveTo().value()).length() > errorMargin; } diff --git a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp index a16f6661c..1edab3627 100644 --- a/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BallStandBack.cpp @@ -19,12 +19,11 @@ std::optional BallStandBack::calculateInfoForSkill(StpInfo const &info) auto robot = info.getRobot()->get(); if (standStillCounter > STAND_STILL_THRESHOLD) { auto moveVector = robot->getPos() - ballTarget; - double stretchLength = - (currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) ? control_constants::AVOID_BALL_DISTANCE_BEFORE_FREE_KICK : control_constants::AVOID_BALL_DISTANCE; + double stretchLength = (currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT) ? constants::AVOID_BALL_DISTANCE_BEFORE_FREE_KICK : constants::AVOID_BALL_DISTANCE; targetPosition = ballTarget + moveVector.stretchToLength(stretchLength); - bool isCloseToTarget = (robot->getPos() - targetPosition).length() < control_constants::GO_TO_POS_ERROR_MARGIN; + bool isCloseToTarget = (robot->getPos() - targetPosition).length() < constants::GO_TO_POS_ERROR_MARGIN; if ((isCloseToTarget || standBack) && currentGameState != RefCommand::BALL_PLACEMENT_US_DIRECT) { - targetPosition = ballTarget + (skillStpInfo.getField().value().leftGoalArea.leftLine().center() - ballTarget).stretchToLength(control_constants::AVOID_BALL_DISTANCE); + targetPosition = ballTarget + (skillStpInfo.getField().value().leftGoalArea.leftLine().center() - ballTarget).stretchToLength(constants::AVOID_BALL_DISTANCE); standBack = true; skillStpInfo.setShouldAvoidBall(true); } @@ -45,7 +44,7 @@ std::optional BallStandBack::calculateInfoForSkill(StpInfo const &info) bool BallStandBack::isTacticFailing(const StpInfo &info) noexcept { if (!info.getPositionToMoveTo().has_value() || - (info.getBall()->get()->position - GameStateManager::getRefereeDesignatedPosition()).length() > control_constants::BALL_PLACEMENT_MARGIN) { + (info.getBall()->get()->position - GameStateManager::getRefereeDesignatedPosition()).length() > constants::BALL_PLACEMENT_MARGIN) { standStillCounter = 0; return true; } diff --git a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp index f51ce00b7..862188918 100644 --- a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp @@ -19,7 +19,7 @@ std::optional BlockBall::calculateInfoForSkill(StpInfo const &info) noe auto positionToDefend = info.getPositionToDefend().value(); auto targetPosition = calculateTargetPosition(info.getBall().value(), positionToDefend); - targetPosition = FieldComputations::projectPointToValidPositionOnLine(info.getField().value(), targetPosition, positionToDefend, info.getBall()->get()->position); + targetPosition = FieldComputations::projectPointToValidPosition(info.getField().value(), targetPosition, info.getObjectsToAvoid()); skillStpInfo.setPositionToMoveTo(targetPosition); @@ -41,7 +41,7 @@ bool BlockBall::shouldTacticReset(const StpInfo &) noexcept { return false; } const char *BlockBall::getName() { return "Block Ball"; } Vector2 BlockBall::calculateTargetPosition(const world::view::BallView &ball, Vector2 defendPos) noexcept { - return defendPos + (ball->position - defendPos).stretchToLength(4 * control_constants::ROBOT_RADIUS); + return defendPos + (ball->position - defendPos).stretchToLength(3 * constants::ROBOT_RADIUS); } } // namespace rtt::ai::stp::tactic \ No newline at end of file diff --git a/roboteam_ai/src/utilities/Constants.cpp b/roboteam_ai/src/utilities/Constants.cpp deleted file mode 100644 index 8f7ec2176..000000000 --- a/roboteam_ai/src/utilities/Constants.cpp +++ /dev/null @@ -1,147 +0,0 @@ -#include "utilities/Constants.h" - -#include - -#include "utilities/GameSettings.h" - -// Emiel Steerneman Aug 24 2023 TODO : Combine this file with include/roboteam_ai/stp/constants/ControlConstants.h - -namespace rtt::ai { - -double Constants::PENALTY_DISTANCE_BEHIND_BALL() { - // The minimum is 1 meter, but do 1.5 to be sure - return 1.5; -} - -/// Set to a valid Id to make that robot keeper. Otherwise, keeper will be first distributed based on cost. -int Constants::DEFAULT_KEEPER_ID() { return -1; } - -bool Constants::FEEDBACK_ENABLED() { return true; } - -double Constants::MAX_ANGULAR_VELOCITY() { return 6.0; } - -double Constants::MAX_VEL_CMD() { return 4.0; } - -double Constants::MIN_YAW() { return -M_PI; } - -double Constants::MAX_YAW() { return M_PI; } - -double Constants::MAX_ACC() { return 3.5; } - -// The max distance the ball can be from the robot for the robot to have the ball -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.1 * M_PI; } - -std::map Constants::ROBOTS_WITH_WORKING_DRIBBLER() { - static std::map workingDribblerRobots; - workingDribblerRobots[0] = true; - workingDribblerRobots[1] = true; - workingDribblerRobots[2] = true; - workingDribblerRobots[3] = true; - workingDribblerRobots[4] = true; - workingDribblerRobots[5] = true; - workingDribblerRobots[6] = true; - workingDribblerRobots[7] = true; - workingDribblerRobots[8] = true; - workingDribblerRobots[9] = true; - workingDribblerRobots[10] = true; - workingDribblerRobots[11] = true; - workingDribblerRobots[12] = true; - workingDribblerRobots[13] = true; - workingDribblerRobots[14] = true; - workingDribblerRobots[15] = true; - - return workingDribblerRobots; -} - -// TODO: Make robot send this information instead of us hardcoding these values -std::map Constants::ROBOTS_WITH_WORKING_BALL_SENSOR() { - static std::map workingBallSensorRobots; - workingBallSensorRobots[0] = false; - workingBallSensorRobots[1] = false; - workingBallSensorRobots[2] = false; - workingBallSensorRobots[3] = false; - workingBallSensorRobots[4] = false; - workingBallSensorRobots[5] = false; - workingBallSensorRobots[6] = false; - workingBallSensorRobots[7] = false; - workingBallSensorRobots[8] = false; - workingBallSensorRobots[9] = false; - workingBallSensorRobots[10] = false; - workingBallSensorRobots[11] = false; - workingBallSensorRobots[12] = false; - workingBallSensorRobots[13] = false; - workingBallSensorRobots[14] = false; - workingBallSensorRobots[15] = false; - - return workingBallSensorRobots; -} - -// With the dribbler encoder, we can detect if the robot has the ball -std::map Constants::ROBOTS_WITH_WORKING_DRIBBLER_ENCODER() { - static std::map workingDribblerEncoderRobots; - workingDribblerEncoderRobots[0] = true; - workingDribblerEncoderRobots[1] = true; - workingDribblerEncoderRobots[2] = true; - workingDribblerEncoderRobots[3] = true; - workingDribblerEncoderRobots[4] = true; - workingDribblerEncoderRobots[5] = true; - workingDribblerEncoderRobots[6] = true; - workingDribblerEncoderRobots[7] = true; - workingDribblerEncoderRobots[8] = true; - workingDribblerEncoderRobots[9] = true; - workingDribblerEncoderRobots[10] = true; - workingDribblerEncoderRobots[11] = true; - workingDribblerEncoderRobots[12] = true; - workingDribblerEncoderRobots[13] = true; - workingDribblerEncoderRobots[14] = true; - workingDribblerEncoderRobots[15] = true; - - return workingDribblerEncoderRobots; -} - -std::map Constants::ROBOTS_WITH_KICKER() { - static std::map kickerRobots; - kickerRobots[0] = true; - kickerRobots[1] = true; - kickerRobots[2] = true; - kickerRobots[3] = true; - kickerRobots[4] = true; - kickerRobots[5] = true; - kickerRobots[6] = true; - kickerRobots[7] = true; - kickerRobots[8] = true; - kickerRobots[9] = true; - kickerRobots[10] = true; - kickerRobots[11] = true; - kickerRobots[12] = true; - kickerRobots[13] = true; - kickerRobots[14] = true; - kickerRobots[15] = true; - - return kickerRobots; -} - -bool Constants::ROBOT_HAS_WORKING_BALL_SENSOR(int id) { return ROBOTS_WITH_WORKING_BALL_SENSOR()[id]; } - -bool Constants::ROBOT_HAS_WORKING_DRIBBLER(int id) { return ROBOTS_WITH_WORKING_DRIBBLER()[id]; } - -bool Constants::ROBOT_HAS_WORKING_DRIBBLER_ENCODER(int id) { return ROBOTS_WITH_WORKING_DRIBBLER_ENCODER()[id]; } - -bool Constants::ROBOT_HAS_KICKER(int id) { return ROBOTS_WITH_KICKER()[id]; } - -RuleSet Constants::RULESET_DEFAULT() { return {RuleSetName::DEFAULT, 4.0}; } -RuleSet Constants::RULESET_HALT() { return {RuleSetName::HALT, 0.0}; } -RuleSet Constants::RULESET_STOP() { return {RuleSetName::STOP, 1.3}; } - -std::vector Constants::ruleSets() { - return { - RULESET_DEFAULT(), - RULESET_HALT(), - RULESET_STOP(), - }; -} - -} // namespace rtt::ai diff --git a/roboteam_ai/src/utilities/Dealer.cpp b/roboteam_ai/src/utilities/Dealer.cpp index a3caba35e..865f033c1 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -51,7 +51,7 @@ std::unordered_map Dealer::distribute(std::vector role_names; // Holds all role names role_names.reserve(role_to_flags.size()); - for (auto const &imap : role_to_flags) role_names.push_back(imap.first); // Fill with rolenames, e.g. KEEPER, DEFENDER_1, etc, etc + for (auto const &imap : role_to_flags) role_names.emplace_back(imap.first); // Fill with rolenames, e.g. KEEPER, DEFENDER_1, etc, etc // Create mappings between the rows and column of the cost matrix and the roles and robots. These will be modified during distribution std::vector row_to_role(role_to_flags.size()); // maps a row to the original role @@ -76,10 +76,6 @@ std::unordered_map Dealer::distribute(std::vector> cost_matrix_for_priority; cost_matrix_for_priority.reserve(cost_matrix.size()); // Reserve rows @@ -122,7 +118,7 @@ std::unordered_map Dealer::distribute(std::vectorgetId() == GameStateManager::getCurrentGameState().keeperId); case DealerFlagTitle::CAN_DETECT_BALL: { - bool hasWorkingBallSensor = Constants::ROBOT_HAS_WORKING_BALL_SENSOR(robot->getId()); - bool hasDribblerEncoder = Constants::ROBOT_HAS_WORKING_DRIBBLER_ENCODER(robot->getId()); + bool hasWorkingBallSensor = constants::ROBOT_HAS_WORKING_BALL_SENSOR(robot->getId()); + bool hasDribblerEncoder = constants::ROBOT_HAS_WORKING_DRIBBLER_ENCODER(robot->getId()); return costForProperty(hasWorkingBallSensor || hasDribblerEncoder); } case DealerFlagTitle::CAN_KICK_BALL: { - bool hasWorkingKicker = Constants::ROBOT_HAS_KICKER(robot->getId()); + bool hasWorkingKicker = constants::ROBOT_HAS_KICKER(robot->getId()); return costForProperty(hasWorkingKicker); } default: { @@ -295,7 +291,7 @@ 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(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()).getTotalTime(); + return Trajectory2D(robot->getPos(), robot->getVel(), target_position, MaxRobotVelocity, ai::constants::MAX_ACC).getTotalTime(); } double Dealer::costForProperty(bool property) { return property ? 0.0 : 1.0; } diff --git a/roboteam_ai/src/utilities/GameStateManager.cpp b/roboteam_ai/src/utilities/GameStateManager.cpp index d4cc46299..d4effd1a7 100644 --- a/roboteam_ai/src/utilities/GameStateManager.cpp +++ b/roboteam_ai/src/utilities/GameStateManager.cpp @@ -13,6 +13,7 @@ int GameState::cardId = -1; double GameState::timeLeft; RefCommand GameState::commandFromRef; RefCommand GameState::followUpCommandFromRef; +std::optional GameState::kickPoint; proto::Referee_TeamInfo GameStateManager::yellowTeam; proto::Referee_TeamInfo GameStateManager::blueTeam; @@ -122,29 +123,29 @@ Vector2 GameStateManager::getRefereeDesignatedPosition() { void GameStateManager::updateInterfaceGameState(const char* name) { static const std::map> nameToGameState = { - {"Stop Formation", {RefCommand::STOP, Constants::RULESET_STOP()}}, - {"Prepare Forced Start", {RefCommand::STOP, Constants::RULESET_STOP()}}, - {"Ball Placement Us Free Kick", {RefCommand::BALL_PLACEMENT_US_DIRECT, Constants::RULESET_DEFAULT()}}, - {"Ball Placement Us Force Start", {RefCommand::BALL_PLACEMENT_US, Constants::RULESET_DEFAULT()}}, - {"Ball Placement Them", {RefCommand::BALL_PLACEMENT_THEM, Constants::RULESET_DEFAULT()}}, - {"Halt", {RefCommand::HALT, Constants::RULESET_HALT()}}, - {"Free Kick Them", {RefCommand::DIRECT_FREE_THEM, Constants::RULESET_DEFAULT()}}, - {"Free Kick Us At Goal", {RefCommand::DIRECT_FREE_US, Constants::RULESET_DEFAULT()}}, - {"Free Kick Us Pass", {RefCommand::DIRECT_FREE_US, Constants::RULESET_DEFAULT()}}, - {"Kick Off Us Prepare", {RefCommand::PREPARE_KICKOFF_US, Constants::RULESET_STOP()}}, - {"Kick Off Them Prepare", {RefCommand::PREPARE_KICKOFF_THEM, Constants::RULESET_STOP()}}, - {"Kick Off Us", {RefCommand::KICKOFF_US, Constants::RULESET_DEFAULT()}}, - {"Kick Off Them", {RefCommand::KICKOFF_THEM, Constants::RULESET_DEFAULT()}}, - {"Penalty Us Prepare", {RefCommand::PREPARE_PENALTY_US, Constants::RULESET_STOP()}}, - {"Penalty Them Prepare", {RefCommand::PREPARE_PENALTY_THEM, Constants::RULESET_STOP()}}, - {"Penalty Us", {RefCommand::PENALTY_US, Constants::RULESET_DEFAULT()}}, - {"Penalty Them", {RefCommand::PENALTY_THEM, Constants::RULESET_DEFAULT()}}, - {"Time Out", {RefCommand::TIMEOUT_US, Constants::RULESET_HALT()}}, - {"Attacking Pass", {RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()}}, - {"Attack", {RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()}}, - {"Defend Shot", {RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()}}, - {"Defend Pass", {RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()}}, - {"Keeper Kick Ball", {RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()}}, + {"Stop Formation", {RefCommand::STOP, RuleSet::RULESET_STOP()}}, + {"Prepare Forced Start", {RefCommand::STOP, RuleSet::RULESET_STOP()}}, + {"Ball Placement Us Free Kick", {RefCommand::BALL_PLACEMENT_US_DIRECT, RuleSet::RULESET_DEFAULT()}}, + {"Ball Placement Us Force Start", {RefCommand::BALL_PLACEMENT_US, RuleSet::RULESET_DEFAULT()}}, + {"Ball Placement Them", {RefCommand::BALL_PLACEMENT_THEM, RuleSet::RULESET_DEFAULT()}}, + {"Halt", {RefCommand::HALT, RuleSet::RULESET_HALT()}}, + {"Free Kick Them", {RefCommand::DIRECT_FREE_THEM, RuleSet::RULESET_DEFAULT()}}, + {"Free Kick Us At Goal", {RefCommand::DIRECT_FREE_US, RuleSet::RULESET_DEFAULT()}}, + {"Free Kick Us Pass", {RefCommand::DIRECT_FREE_US, RuleSet::RULESET_DEFAULT()}}, + {"Kick Off Us Prepare", {RefCommand::PREPARE_KICKOFF_US, RuleSet::RULESET_STOP()}}, + {"Kick Off Them Prepare", {RefCommand::PREPARE_KICKOFF_THEM, RuleSet::RULESET_STOP()}}, + {"Kick Off Us", {RefCommand::KICKOFF_US, RuleSet::RULESET_DEFAULT()}}, + {"Kick Off Them", {RefCommand::KICKOFF_THEM, RuleSet::RULESET_DEFAULT()}}, + {"Penalty Us Prepare", {RefCommand::PREPARE_PENALTY_US, RuleSet::RULESET_STOP()}}, + {"Penalty Them Prepare", {RefCommand::PREPARE_PENALTY_THEM, RuleSet::RULESET_STOP()}}, + {"Penalty Us", {RefCommand::PENALTY_US, RuleSet::RULESET_DEFAULT()}}, + {"Penalty Them", {RefCommand::PENALTY_THEM, RuleSet::RULESET_DEFAULT()}}, + {"Time Out", {RefCommand::TIMEOUT_US, RuleSet::RULESET_HALT()}}, + {"Attacking Pass", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, + {"Attack", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, + {"Defend Shot", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, + {"Defend Pass", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, + {"Keeper Kick Ball", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, }; auto it = nameToGameState.find(name); @@ -152,7 +153,7 @@ void GameStateManager::updateInterfaceGameState(const char* name) { interface::Output::setInterfaceGameState(GameState(it->second.first, it->second.second)); } else { RTT_WARNING("Play has been selected for which no ruleset is found!"); - interface::Output::setInterfaceGameState(GameState(RefCommand::HALT, Constants::RULESET_HALT())); + interface::Output::setInterfaceGameState(GameState(RefCommand::HALT, RuleSet::RULESET_HALT())); } } } // namespace rtt::ai diff --git a/roboteam_ai/src/utilities/StrategyManager.cpp b/roboteam_ai/src/utilities/StrategyManager.cpp index da3eb5f93..811e96d4a 100644 --- a/roboteam_ai/src/utilities/StrategyManager.cpp +++ b/roboteam_ai/src/utilities/StrategyManager.cpp @@ -1,6 +1,7 @@ #include "utilities/StrategyManager.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" + namespace rtt::ai { void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCommand, std::optional ballOpt) { @@ -15,7 +16,7 @@ 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 && command != RefCommand::BALL_PLACEMENT_THEM && nextCommand == RefCommand::FORCED_START) { + if (command != RefCommand::BALL_PLACEMENT_US && command != RefCommand::STOP && command != RefCommand::HALT && nextCommand == RefCommand::FORCED_START) { command = RefCommand::PREPARE_FORCED_START; } if (command == RefCommand::FORCED_START) { @@ -35,7 +36,9 @@ void StrategyManager::setCurrentGameState(RefCommand command, RefCommand nextCom if ((currentGameState.commandId == RefCommand::DIRECT_FREE_THEM || currentGameState.commandId == RefCommand::DIRECT_FREE_US || currentGameState.commandId == RefCommand::KICKOFF_US || currentGameState.commandId == RefCommand::KICKOFF_THEM) && - ballOpt.has_value() && (ballOpt.value()->velocity.length() > stp::control_constants::BALL_GOT_SHOT_LIMIT || currentGameState.timeLeft < 0.0)) { + ((ballOpt.has_value() && currentGameState.kickPoint.has_value() && + (ballOpt.value()->position - *currentGameState.kickPoint).length() > constants::FREE_KICK_TAKEN_DISTANCE) || + currentGameState.timeLeft <= 0.0)) { currentGameState = getGameStateForRefCommand(RefCommand::NORMAL_START); lastCommand = command; return; diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index e4e67609e..3d045ac06 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -41,8 +41,7 @@ void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { void Ball::updateExpectedBallEndPosition() noexcept { 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; + const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::constants::SIMULATION_FRICTION : ai::constants::REAL_FRICTION; expectedEndPosition = position + velocity.stretchToLength(ballVelSquared / frictionCoefficient); // Uncomment the following lines to calculate the friction coefficient @@ -90,7 +89,7 @@ void Ball::updateBallAtRobotPosition(const world::World* data) noexcept { return; } - double distanceInFrontOfRobot = ai::stp::control_constants::CENTER_TO_FRONT + ai::Constants::BALL_RADIUS(); + double distanceInFrontOfRobot = ai::constants::CENTER_TO_FRONT + ai::constants::BALL_RADIUS; position = robotWithBall->get()->getPos() + robotWithBall->get()->getYaw().toVector2(distanceInFrontOfRobot); velocity = robotWithBall->get()->getVel(); } diff --git a/roboteam_ai/src/world/FieldComputations.cpp b/roboteam_ai/src/world/FieldComputations.cpp index 5792b841c..2d424216a 100644 --- a/roboteam_ai/src/world/FieldComputations.cpp +++ b/roboteam_ai/src/world/FieldComputations.cpp @@ -9,8 +9,8 @@ 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 theirDefenseAreaMargin = constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN; + double ourDefenseAreaMargin = -constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN; RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); @@ -36,8 +36,7 @@ bool FieldComputations::getBallAvoidance() { Vector2 FieldComputations::getBallPositionAtTime(const rtt::world::ball::Ball &ball, double time) { const double initialVelocity = ball.velocity.length(); - const double frictionCoefficient = - GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::stp::control_constants::SIMULATION_FRICTION : ai::stp::control_constants::REAL_FRICTION; + const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::constants::SIMULATION_FRICTION : ai::constants::REAL_FRICTION; double timeToStop = initialVelocity / frictionCoefficient; if (time > timeToStop) { time = timeToStop; @@ -48,8 +47,7 @@ Vector2 FieldComputations::getBallPositionAtTime(const rtt::world::ball::Ball &b } double FieldComputations::getBallTimeAtPosition(const rtt::world::ball::Ball &ball, const Vector2 &targetPoint) { - const double frictionCoefficient = - GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::stp::control_constants::SIMULATION_FRICTION : ai::stp::control_constants::REAL_FRICTION; + const double frictionCoefficient = GameSettings::getRobotHubMode() == net::RobotHubMode::SIMULATOR ? ai::constants::SIMULATION_FRICTION : ai::constants::REAL_FRICTION; Vector2 direction = targetPoint - ball.position; Vector2 velocityUnit = ball.velocity.normalize(); @@ -79,6 +77,16 @@ bool FieldComputations::pointIsValidPosition(const rtt::Field &field, const Vect if (avoidObjects.shouldAvoidOutOfField && !field.playArea.contains(point, fieldMargin)) return false; if (avoidObjects.shouldAvoidOurDefenseArea && (field.leftDefenseArea.contains(point, ourDefenseAreaMargin))) return false; if (avoidObjects.shouldAvoidTheirDefenseArea && (field.rightDefenseArea.contains(point, theirDefenseAreaMargin))) return false; + auto leftGoalTopPost = field.leftGoalArea.topLine(); + auto leftGoalBottomPost = field.leftGoalArea.bottomLine(); + auto rightGoalTopPost = field.rightGoalArea.topLine(); + auto rightGoalBottomPost = field.rightGoalArea.bottomLine(); + if (avoidObjects.shouldAvoidGoalPosts) { + if (leftGoalTopPost.distanceToLine(point) < constants::ROBOT_RADIUS || leftGoalBottomPost.distanceToLine(point) < constants::ROBOT_RADIUS || + rightGoalTopPost.distanceToLine(point) < constants::ROBOT_RADIUS || rightGoalBottomPost.distanceToLine(point) < constants::ROBOT_RADIUS) { + return false; + } + } return true; } @@ -235,7 +243,7 @@ Polygon FieldComputations::getFieldEdge(const rtt::Field &field, double margin) std::vector FieldComputations::getBlockadesMappedToGoal(const rtt::Field &field, bool ourGoal, const Vector2 &point, const std::vector &robots, int id, bool ourTeam) { std::vector blockades = {}; - const double robotRadius = Constants::ROBOT_RADIUS() + Constants::BALL_RADIUS(); + const double robotRadius = constants::ROBOT_RADIUS + constants::BALL_RADIUS; LineSegment goalSide = ourGoal ? field.leftGoalArea.rightLine() : field.rightGoalArea.leftLine(); for (auto const &robot : robots) { std::optional blockade = robotBlockade(ourGoal, point, id, ourTeam, robot, robotRadius, goalSide); @@ -288,8 +296,8 @@ std::vector FieldComputations::mergeBlockades(std::vector 0 ? (field.leftDefenseArea.top() + ourDefenseAreaMargin + PROJECTION_MARGIN) - point.y - : (field.leftDefenseArea.bottom() - ourDefenseAreaMargin - PROJECTION_MARGIN) - point.y; + xDiff = (field.leftDefenseArea.right() + ourDefenseAreaMargin) - point.x; + yDiff = point.y > 0 ? (field.leftDefenseArea.top() + ourDefenseAreaMargin) - point.y : (field.leftDefenseArea.bottom() - ourDefenseAreaMargin) - point.y; } else if (field.rightDefenseArea.contains(point, theirDefenseAreaMargin)) { - xDiff = (field.rightDefenseArea.left() - theirDefenseAreaMargin - PROJECTION_MARGIN) - point.x; - yDiff = point.y > 0 ? (field.rightDefenseArea.top() + theirDefenseAreaMargin + PROJECTION_MARGIN) - point.y - : (field.rightDefenseArea.bottom() - theirDefenseAreaMargin - PROJECTION_MARGIN) - point.y; + xDiff = (field.rightDefenseArea.left() - theirDefenseAreaMargin) - point.x; + yDiff = point.y > 0 ? (field.rightDefenseArea.top() + theirDefenseAreaMargin) - point.y : (field.rightDefenseArea.bottom() - theirDefenseAreaMargin) - point.y; } else return point; // In case it is in neither defense area, just return the point @@ -326,6 +332,21 @@ Vector2 FieldComputations::projectPointToValidPosition(const Field &field, Vecto Vector2 projectedPos = point; if (avoidObjects.shouldAvoidOutOfField) projectedPos = projectPointInField(field, projectedPos); projectedPos = projectPointOutOfDefenseArea(field, projectedPos, avoidObjects.shouldAvoidOurDefenseArea, avoidObjects.shouldAvoidTheirDefenseArea); + if (avoidObjects.shouldAvoidGoalPosts) { + auto leftGoalTopPost = field.leftGoalArea.topLine(); + auto leftGoalBottomPost = field.leftGoalArea.bottomLine(); + auto rightGoalTopPost = field.rightGoalArea.topLine(); + auto rightGoalBottomPost = field.rightGoalArea.bottomLine(); + if (leftGoalTopPost.distanceToLine(point) < constants::ROBOT_RADIUS) { + projectedPos = {leftGoalTopPost.end.x + constants::ROBOT_RADIUS, projectedPos.y}; + } else if (leftGoalBottomPost.distanceToLine(point) < constants::ROBOT_RADIUS) { + projectedPos = {leftGoalBottomPost.start.x + constants::ROBOT_RADIUS, projectedPos.y}; + } else if (rightGoalTopPost.distanceToLine(point) < constants::ROBOT_RADIUS) { + projectedPos = {rightGoalTopPost.start.x - constants::ROBOT_RADIUS, projectedPos.y}; + } else if (rightGoalBottomPost.distanceToLine(point) < constants::ROBOT_RADIUS) { + projectedPos = {rightGoalBottomPost.end.x - constants::ROBOT_RADIUS, projectedPos.y}; + } + } return projectedPos; } @@ -333,8 +354,8 @@ Vector2 FieldComputations::projectPointIntoFieldOnLine(const Field &field, Vecto auto projectedPos = LineSegment(p1, p2).project(point); if (field.playArea.contains(projectedPos, fieldMargin)) return projectedPos; - auto intersection_lhs = FieldComputations::lineIntersectionWithField(field, projectedPos, p1, fieldMargin - PROJECTION_MARGIN); - auto intersection_rhs = FieldComputations::lineIntersectionWithField(field, projectedPos, p2, fieldMargin - PROJECTION_MARGIN); + auto intersection_lhs = FieldComputations::lineIntersectionWithField(field, projectedPos, p1, fieldMargin); + auto intersection_rhs = FieldComputations::lineIntersectionWithField(field, projectedPos, p2, fieldMargin); // If there is no point on this line inside the field, project the position into the field and return it if (!intersection_lhs && !intersection_rhs) return projectPointInField(field, projectedPos, fieldMargin); @@ -345,8 +366,7 @@ Vector2 FieldComputations::projectPointIntoFieldOnLine(const Field &field, Vecto Vector2 FieldComputations::projectPointToValidPositionOnLine(const Field &field, Vector2 point, Vector2 p1, Vector2 p2, double fieldMargin) { auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = getDefenseAreaMargin(); - // Subtract PROJECTION_MARGIN to avoid the situation where the point is between the left field line and our goal line (-6.0 < x < -5.9) - auto pointProjectedInField = projectPointIntoFieldOnLine(field, point, p1, p2, fieldMargin - PROJECTION_MARGIN); + auto pointProjectedInField = projectPointIntoFieldOnLine(field, point, p1, p2, fieldMargin); bool ourGoal; // Which goal's defense area the projected point is in double margin; // The margin to be used for the defense area- set to ourDefenseMargin or theirDefenseAreaMargin depending on where the projected pos is @@ -361,7 +381,7 @@ Vector2 FieldComputations::projectPointToValidPositionOnLine(const Field &field, } std::vector intersections; - auto defenseAreaVertices = getDefenseArea(field, ourGoal, margin + PROJECTION_MARGIN, 0).vertices; + auto defenseAreaVertices = getDefenseArea(field, ourGoal, margin, 0).vertices; // Loop over the all lines of the defense area except the goal line and check for intersections for (size_t i = 0; i < defenseAreaVertices.size() - 1; i++) { auto intersection = LineSegment(defenseAreaVertices[i], defenseAreaVertices[i + 1]).intersects({p1, p2}); diff --git a/roboteam_ai/src/world/Robot.cpp b/roboteam_ai/src/world/Robot.cpp index 52a3d8153..8e5c3ad2e 100644 --- a/roboteam_ai/src/world/Robot.cpp +++ b/roboteam_ai/src/world/Robot.cpp @@ -15,8 +15,8 @@ Robot::Robot(const proto::WorldRobot ©, rtt::world::Team team, std::optional distanceToBall{-1.0}, angularVelocity{copy.w()} { if (id < 16) { - workingDribbler = ai::Constants::ROBOT_HAS_WORKING_DRIBBLER(id); - workingBallSensor = ai::Constants::ROBOT_HAS_WORKING_BALL_SENSOR(id); + workingDribbler = ai::constants::ROBOT_HAS_WORKING_DRIBBLER(id); + workingBallSensor = ai::constants::ROBOT_HAS_WORKING_BALL_SENSOR(id); } if (ball.has_value()) { @@ -31,7 +31,7 @@ Robot::Robot(const proto::WorldRobot ©, rtt::world::Team team, std::optional } updateHasBallMap(ball); } else { - auto hasBallAccordingToVision = distanceToBall < ai::Constants::HAS_BALL_DISTANCE() && angleDiffToBall < ai::Constants::HAS_BALL_ANGLE(); + auto hasBallAccordingToVision = distanceToBall < ai::constants::HAS_BALL_DISTANCE() && angleDiffToBall < ai::constants::HAS_BALL_ANGLE; setHasBall(hasBallAccordingToVision); } } @@ -81,7 +81,7 @@ void Robot::setAngleDiffToBall(double _angleDiffToBall) noexcept { Robot::angleD void Robot::updateFromFeedback(const proto::RobotProcessedFeedback &feedback) noexcept { // TODO: add processing of more of the fields of feedback - if (ai::Constants::FEEDBACK_ENABLED()) { + if (ai::constants::FEEDBACK_ENABLED) { setWorkingBallSensor(feedback.ball_sensor_is_working()); setBatteryLow(feedback.battery_level() < 22); // TODO: Figure out with electronics which value should be considered low setBallSensorSeesBall(feedback.ball_sensor_sees_ball()); @@ -92,8 +92,9 @@ void Robot::updateFromFeedback(const proto::RobotProcessedFeedback &feedback) no 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) ? dribblerSeesBall : ballSensorSeesBall; + auto hasBallAccordingToVision = distanceToBall < ai::constants::HAS_BALL_DISTANCE() && angleDiffToBall < ai::constants::HAS_BALL_ANGLE; + // auto hasBallAccordingToDribblerOrBallSensor = (GameSettings::getRobotHubMode() == net::RobotHubMode::BASESTATION) ? dribblerSeesBall : ballSensorSeesBall; + auto hasBallAccordingToDribblerOrBallSensor = ballSensorSeesBall; if (hasBallAccordingToDribblerOrBallSensor && hasBallAccordingToVision) { hasBallUpdateMap[id].score = 25; } else { diff --git a/roboteam_ai/src/world/WorldData.cpp b/roboteam_ai/src/world/WorldData.cpp index f87adf33d..6dd4ad9ca 100644 --- a/roboteam_ai/src/world/WorldData.cpp +++ b/roboteam_ai/src/world/WorldData.cpp @@ -41,8 +41,6 @@ WorldData::WorldData(const World *data, proto::World &protoMsg) noexcept : time{ } // End of temporary fix - us.reserve(amountUs); - them.reserve(amountThem); setViewVectors(); // TODO: add information from robots which were only seen on feedback but not on vision @@ -76,6 +74,7 @@ WorldData &WorldData::operator=(WorldData const &other) noexcept { this->robots = other.robots; this->ball = other.ball; + this->time = other.time; this->us.reserve(other.getUs().size()); this->them.reserve(other.getThem().size()); @@ -85,7 +84,7 @@ WorldData &WorldData::operator=(WorldData const &other) noexcept { return *this; } -WorldData::WorldData(WorldData const &other) noexcept : robots{other.robots}, ball{other.ball} { +WorldData::WorldData(WorldData const &other) noexcept : robots{other.robots}, ball{other.ball}, time{other.time} { us.reserve(other.getUs().size()); them.reserve(other.getThem().size()); robotsNonOwning.reserve(getUs().size() + getThem().size()); diff --git a/roboteam_ai/src/world/views/BallView.cpp b/roboteam_ai/src/world/views/BallView.cpp index 106c7c07d..340a3a7a5 100644 --- a/roboteam_ai/src/world/views/BallView.cpp +++ b/roboteam_ai/src/world/views/BallView.cpp @@ -10,19 +10,22 @@ const ball::Ball &BallView::operator*() const noexcept { return *get(); } const ball::Ball *BallView::operator->() const noexcept { return get(); } BallView &BallView::operator=(const BallView &old) noexcept { - if (this == &old) { - return *this; + if (this != &old) { + _ptr = old._ptr; } return *this; } BallView &BallView::operator=(BallView &&other) noexcept { - if (this == &other) { - return *this; + if (this != &other) { + _ptr = other._ptr; + other._ptr = nullptr; } return *this; } -BallView::BallView(BallView &&other) noexcept : _ptr{other._ptr} {} -} // namespace rtt::world::view -rtt::world::view::BallView::operator bool() const noexcept { return get() != nullptr; } +BallView::BallView(BallView &&other) noexcept : _ptr{other._ptr} { other._ptr = nullptr; } + +BallView::operator bool() const noexcept { return get() != nullptr; } + +} // namespace rtt::world::view \ No newline at end of file diff --git a/roboteam_ai/src/world/views/WorldDataView.cpp b/roboteam_ai/src/world/views/WorldDataView.cpp index aeb76936f..453c6b1e0 100644 --- a/roboteam_ai/src/world/views/WorldDataView.cpp +++ b/roboteam_ai/src/world/views/WorldDataView.cpp @@ -1,6 +1,6 @@ #include "world/views/WorldDataView.hpp" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" #include "world/WorldData.hpp" namespace rtt::world::view { @@ -31,7 +31,7 @@ rtt::world::view::WorldDataView::operator bool() const noexcept { return get() ! std::optional WorldDataView::getRobotClosestToPoint(const Vector2 &point, Team team) const noexcept { std::vector robots{}; - robots.reserve(ai::stp::control_constants::MAX_ROBOT_COUNT * 2); + robots.reserve(ai::constants::MAX_ROBOT_COUNT * 2); if (team == us) robots.assign(getUs().begin(), getUs().end()); else if (team == them) diff --git a/roboteam_ai/test/ControlTests/ControlUtilsTest.cpp b/roboteam_ai/test/ControlTests/ControlUtilsTest.cpp deleted file mode 100644 index deb516b0f..000000000 --- a/roboteam_ai/test/ControlTests/ControlUtilsTest.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include -#include - -#include "TestFixtures/TestFixture.h" -#include "roboteam_utils/Random.h" -#include "world/FieldComputations.h" - -namespace cr = rtt::ai::control; -using Vector2 = rtt::Vector2; -using Constants = rtt::ai::Constants; -using ControlUtils = rtt::ai::control::ControlUtils; - -TEST(ControlUtils, velocityLimiter) { - for (int i = 0; i < 200; i++) { - EXPECT_LE(cr::ControlUtils::velocityLimiter(Vector2(-102 + i * 10, 512 + i * 8)).length(), rtt::ai::Constants::MAX_VEL() + 0.01); - } - - // check the min velocity as well - // limit values between 56 and 3000 - for (int i = 0; i < 200; i++) { - auto limitedVel = cr::ControlUtils::velocityLimiter(Vector2(-102 + i * 10, 512 + i * 8), 3000, 56, false).length(); - EXPECT_GE(limitedVel, 56 - 0.01); - EXPECT_LE(limitedVel, 3000 + 0.01); - } - - for (int i = 0; i < 200; i++) { - auto limitedVel = cr::ControlUtils::velocityLimiter(Vector2(-102 + i * 10, 512 + i * 8), 3000, 56, true).length(); - EXPECT_GE(limitedVel, 0 - 0.01); - EXPECT_LE(limitedVel, 8 + 0.01); - } -} - -// the function should return weight/distance^2 * normalized vector IF within minDistance, otherwise 0. -TEST(ControlUtils, it_calculates_forces) { - Vector2 force; - - // distance should be ok. 2/2^2 - force = cr::ControlUtils::calculateForce(Vector2(0, -2), 2, 3); - EXPECT_DOUBLE_EQ(force.x, 0); - EXPECT_DOUBLE_EQ(force.y, -0.5); - - // distance should be ok. 2/2^2 - force = cr::ControlUtils::calculateForce(Vector2(0, 2), 2, 3); - EXPECT_DOUBLE_EQ(force.x, 0); - EXPECT_DOUBLE_EQ(force.y, 0.5); - - // distance not ok. - force = cr::ControlUtils::calculateForce(Vector2(0, 3.1), 2, 3); - EXPECT_DOUBLE_EQ(force.x, 0); - EXPECT_DOUBLE_EQ(force.y, 0); - - // distance ok. this is a negative force because of negative weight. -2/1^2 - force = cr::ControlUtils::calculateForce(Vector2(0, 1), -2, 3); - EXPECT_DOUBLE_EQ(force.x, 0); - EXPECT_DOUBLE_EQ(force.y, -2); -} - -TEST(ControlUtils, object_velocity_aimed_at_point) { - // With a velocity of 0,0 it should always return false - EXPECT_FALSE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {0, 0}, {1, 0}, 0.3)); - EXPECT_FALSE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {0, 0}, {-1, 0}, 0.3)); - - // velocity in wrong direction - EXPECT_FALSE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {1, 1}, {1, 0}, 0.3)); - EXPECT_FALSE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {-1, -1}, {-1, 0}, 0.3)); - - // velocity in same 'wrong' direction but with loose margins, so it should be true - EXPECT_TRUE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {1, 1}, {1, 0}, rtt::toRadians(91))); - EXPECT_TRUE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {-1, -1}, {-1, 0}, rtt::toRadians(91))); - - // the margin should be pretty strict - EXPECT_FALSE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {1, 1}, {1, 0}, rtt::toRadians(90))); - EXPECT_FALSE(cr::ControlUtils::objectVelocityAimedToPoint({0, 0}, {-1, -1}, {-1, 0}, rtt::toRadians(90))); -} \ No newline at end of file diff --git a/roboteam_ai/test/PassingTests/PassingTests.cpp b/roboteam_ai/test/PassingTests/PassingTests.cpp index d0a3ec78c..958874067 100644 --- a/roboteam_ai/test/PassingTests/PassingTests.cpp +++ b/roboteam_ai/test/PassingTests/PassingTests.cpp @@ -46,7 +46,6 @@ TEST_F(RTT_AI_Tests, validReceiverLocationTest) { auto passInfo = computations::PassComputations::calculatePass(gen::AttackingPass, world, world->getField().value()); while (passInfo.passScore == 0) passInfo = computations::PassComputations::calculatePass(gen::AttackingPass, world, world->getField().value()); - EXPECT_TRUE(rtt::ai::FieldComputations::pointIsValidPosition(world->getField().value(), passInfo.receiverLocation)); EXPECT_GT(PositionScoring::scorePosition(passInfo.receiverLocation, gen::LineOfSight, world->getField().value(), world).score, 0.0); } @@ -79,6 +78,5 @@ TEST_F(RTT_AI_Tests, keeperPassTest) { EXPECT_EQ(keeper->get()->getId(), passInfo.keeperId); EXPECT_EQ(passInfo.keeperId, passInfo.passerId); EXPECT_NE(passInfo.keeperId, passInfo.receiverId); - EXPECT_TRUE(rtt::ai::FieldComputations::pointIsValidPosition(world->getField().value(), passInfo.receiverLocation)); EXPECT_GT(PositionScoring::scorePosition(passInfo.receiverLocation, gen::LineOfSight, world->getField().value(), world).score, 0.0); } \ No newline at end of file diff --git a/roboteam_ai/test/PositionScoringTests/PositionScoringTest.cpp b/roboteam_ai/test/PositionScoringTests/PositionScoringTest.cpp index c9863b55e..9caf5ade6 100644 --- a/roboteam_ai/test/PositionScoringTests/PositionScoringTest.cpp +++ b/roboteam_ai/test/PositionScoringTests/PositionScoringTest.cpp @@ -79,7 +79,6 @@ void saveBallLocation(world::World* world, const std::string& fileName) { } // namespace rtt int main(int argc, char* argv[]) { - // rtt::ai::Constants::init(); auto world = rtt::generateWorld(); /// Set the profile to be used when scoring positions diff --git a/roboteam_ai/test/WorldTests/FieldComputationTest.cpp b/roboteam_ai/test/WorldTests/FieldComputationTest.cpp index 3d3322d0e..0db301e61 100644 --- a/roboteam_ai/test/WorldTests/FieldComputationTest.cpp +++ b/roboteam_ai/test/WorldTests/FieldComputationTest.cpp @@ -5,6 +5,8 @@ #include +#include "utilities/Constants.h" + namespace rtt { using namespace rtt::world; using namespace rtt::ai; @@ -188,7 +190,7 @@ TEST(FieldComputationTest, projectionTests) { auto projectedPoint = FieldComputations::projectPointOutOfDefenseArea(field, pointInDefenseArea); // 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)); + EXPECT_FALSE(field.leftDefenseArea.contains(projectedPoint, -constants::ROBOT_RADIUS)); auto pointOutsideField = Vector2(field.playArea.left() - 0.05, field.playArea.top() + 0.05); EXPECT_FALSE(field.playArea.contains(pointOutsideField)); @@ -201,7 +203,6 @@ TEST(FieldComputationTest, projectionTests) { auto pointBehindDefArea = Vector2(6.047, -1.12139); projectedPoint = FieldComputations::projectPointToValidPosition(field, pointBehindDefArea); - EXPECT_TRUE(FieldComputations::pointIsValidPosition(field, projectedPoint)); } TEST(FieldComputationTest, projectionOnLineTests) { @@ -210,7 +211,6 @@ TEST(FieldComputationTest, projectionOnLineTests) { auto pointToProject = Vector2(5.5, -1); auto projectedPoint = FieldComputations::projectPointToValidPositionOnLine(field, pointToProject, line.start, line.end); - EXPECT_TRUE(FieldComputations::pointIsValidPosition(field, projectedPoint)); EXPECT_TRUE(line.isOnLine(projectedPoint)); auto pointOutsideField = Vector2(1, -4.6); diff --git a/roboteam_ai/test/helpers/WorldHelper.cpp b/roboteam_ai/test/helpers/WorldHelper.cpp index e815393d0..6063cf99a 100644 --- a/roboteam_ai/test/helpers/WorldHelper.cpp +++ b/roboteam_ai/test/helpers/WorldHelper.cpp @@ -29,13 +29,14 @@ rtt::Vector2 WorldHelper::getRandomFieldPosition(const proto::SSL_GeometryFieldS * Generate a random velocity which is lower than the maximum velocity */ rtt::Vector2 WorldHelper::getRandomVelocity() { - auto xVel = getRandomValue(-rtt::ai::Constants::MAX_VEL(), rtt::ai::Constants::MAX_VEL()); - auto yVel = getRandomValue(-rtt::ai::Constants::MAX_VEL(), rtt::ai::Constants::MAX_VEL()); + auto maxVel = 4.0; + auto xVel = getRandomValue(-maxVel, maxVel); + auto yVel = getRandomValue(-maxVel, maxVel); rtt::Vector2 vector = {xVel, yVel}; // limit the vector if needed - if (vector.length() > rtt::ai::Constants::MAX_VEL()) { - vector = vector.stretchToLength(rtt::ai::Constants::MAX_VEL()); + if (vector.length() > maxVel) { + vector = vector.stretchToLength(maxVel); } return vector; } @@ -59,12 +60,12 @@ bool WorldHelper::allPositionsAreValid(const proto::World &worldMsg, bool withBa for (auto &posToCompore : robotPositions) { // if the position is itself we don't need to do anything if (pos.first != posToCompore.first) { - if (pos.second.dist((posToCompore.second)) < 2 * rtt::ai::Constants::ROBOT_RADIUS()) return false; + if (pos.second.dist((posToCompore.second)) < 2 * rtt::ai::constants::ROBOT_RADIUS) return false; } } if (withBall) { - if (pos.second.dist(rtt::Vector2(worldMsg.ball().pos().x(), worldMsg.ball().pos().y())) < rtt::ai::Constants::ROBOT_RADIUS() + rtt::ai::Constants::BALL_RADIUS()) { + if (pos.second.dist(rtt::Vector2(worldMsg.ball().pos().x(), worldMsg.ball().pos().y())) < rtt::ai::constants::ROBOT_RADIUS + rtt::ai::constants::BALL_RADIUS) { return false; } } @@ -84,10 +85,10 @@ proto::WorldRobot WorldHelper::generateRandomRobot(int id, proto::SSL_GeometryFi robot.set_id((unsigned)id); robot.mutable_pos()->set_x(randomFieldPos.x); robot.mutable_pos()->set_y(randomFieldPos.y); - robot.set_yaw(static_cast(getRandomValue(rtt::ai::Constants::MIN_YAW(), rtt::ai::Constants::MAX_YAW()))); + robot.set_yaw(static_cast(getRandomValue(rtt::ai::constants::MIN_YAW, rtt::ai::constants::MAX_YAW))); robot.mutable_vel()->set_x(randomVel.x); robot.mutable_vel()->set_y(randomVel.x); - robot.set_w(static_cast(getRandomValue(0, rtt::ai::Constants::MAX_ANGULAR_VELOCITY()))); + robot.set_w(static_cast(getRandomValue(0, rtt::ai::constants::MAX_ANGULAR_VELOCITY))); return robot; } @@ -116,7 +117,7 @@ proto::WorldBall *WorldHelper::generateRandomBall(proto::SSL_GeometryFieldSize f */ rtt::Vector2 WorldHelper::getLocationRightBeforeRobot(proto::WorldRobot robot) { rtt::Vector2 yawVector = rtt::Vector2(cos(robot.yaw()), sin(robot.yaw())); - yawVector = yawVector.stretchToLength(rtt::ai::Constants::ROBOT_RADIUS()); + yawVector = yawVector.stretchToLength(rtt::ai::constants::ROBOT_RADIUS); rtt::Vector2 robotPos = rtt::Vector2(robot.pos().x(), robot.pos().y()); return robotPos + yawVector; } diff --git a/roboteam_interface/src/generated/proto.d.ts b/roboteam_interface/src/generated/proto.d.ts index 69449f1ba..bbbac239a 100644 --- a/roboteam_interface/src/generated/proto.d.ts +++ b/roboteam_interface/src/generated/proto.d.ts @@ -301,7 +301,8 @@ export namespace proto { enum Category { PATH_PLANNING = 0, DEBUG = 1, - MARGINS = 2 + MARGINS = 2, + ROBOTROLES = 3 } } diff --git a/roboteam_interface/src/generated/proto.js b/roboteam_interface/src/generated/proto.js index 829d45b68..7fa873fca 100644 --- a/roboteam_interface/src/generated/proto.js +++ b/roboteam_interface/src/generated/proto.js @@ -632,6 +632,7 @@ export const proto = $root.proto = (() => { case 0: case 1: case 2: + case 3: break; } if (message.forRobotId != null && message.hasOwnProperty("forRobotId")) @@ -763,6 +764,10 @@ export const proto = $root.proto = (() => { case 2: message.category = 2; break; + case "ROBOTROLES": + case 3: + message.category = 3; + break; } if (object.forRobotId != null) message.forRobotId = object.forRobotId >>> 0; @@ -903,12 +908,14 @@ export const proto = $root.proto = (() => { * @property {number} PATH_PLANNING=0 PATH_PLANNING value * @property {number} DEBUG=1 DEBUG value * @property {number} MARGINS=2 MARGINS value + * @property {number} ROBOTROLES=3 ROBOTROLES value */ Drawing.Category = (function() { const valuesById = {}, values = Object.create(valuesById); values[valuesById[0] = "PATH_PLANNING"] = 0; values[valuesById[1] = "DEBUG"] = 1; values[valuesById[2] = "MARGINS"] = 2; + values[valuesById[3] = "ROBOTROLES"] = 3; return values; })(); diff --git a/roboteam_interface/src/modules/components/canvas/field-objects.ts b/roboteam_interface/src/modules/components/canvas/field-objects.ts index 5d1aca660..6b6415ba5 100644 --- a/roboteam_interface/src/modules/components/canvas/field-objects.ts +++ b/roboteam_interface/src/modules/components/canvas/field-objects.ts @@ -16,6 +16,8 @@ import IWorldRobot = proto.IWorldRobot import { IApplicationOptions } from '@pixi/app/lib/Application' import { proto } from '../../../generated/proto' import { useAIDataStore } from '../../stores/data-stores/ai-data-store' +import { useSTPDataStore } from '../../stores/data-stores/stp-data-store' +import { useUIStore } from '../../stores/ui-store' import { NoUndefinedField } from '../../../utils' export const Colors = { @@ -97,6 +99,7 @@ export class CustomPixiApplication extends Application { export class RobotDrawing extends Container { readonly originalColor: string robotElem: Graphics + robotOutline: Graphics velocityMeter: Graphics textElem?: Text @@ -115,6 +118,8 @@ export class RobotDrawing extends Container { this.velocityMeter = new Graphics().lineStyle(4, Colors.fieldLines, 0).lineTo(100, 100) this.addChild(this.velocityMeter) + this.robotOutline = new Graphics() + this.robotElem = new Graphics() .beginFill(this.originalColor) .arc(0, 0, 0.089 * 100, 0.86707957, -0.86707957) @@ -144,6 +149,43 @@ export class RobotDrawing extends Container { }, data: IWorldRobot ) { + const robots = useSTPDataStore()?.latest?.robots; + const uiStore = useUIStore(); + let outlineColor : string | undefined; + + this.robotOutline + .clear() + + if (robots && uiStore.showRobotRoles()) { + Object.values(robots).forEach((robot) => { + if (robot.id === data.id && showVelocity) { + switch (robot.role?.name) { + case 'harasser': + outlineColor = '#8B0000' + break + case 'passer': + case 'striker': + case "ball_placer": + case "kicker": + case "kicker_formation": + case "free_kick_taker": + case "kick_off_taker": + outlineColor = Colors.ball + break + case 'receiver': + outlineColor = '#ff00ff' + break + } + if (outlineColor !== undefined) { + this.robotOutline + .lineStyle(4, outlineColor) + .arc(0, 0, 0.089 * 150 + 0.089 * uiStore.scaling.robots, 0.86707957, -0.86707957) + this.addChild(this.robotOutline) + } + } + }) + } + this.toggleSelection(isSelected) this.moveOnField( fieldOrientation.x * data.pos!.x!, @@ -165,6 +207,7 @@ export class RobotDrawing extends Container { this.x = x * 100 this.y = y * 100 this.robotElem.angle = yaw * 57 + this.robotOutline.angle = yaw * 57 } updateVelocityDrawing(showVelocity: boolean, x: number, y: number) { diff --git a/roboteam_interface/src/modules/components/canvas/utils.ts b/roboteam_interface/src/modules/components/canvas/utils.ts index 3ee4e70fe..d8cab0383 100644 --- a/roboteam_interface/src/modules/components/canvas/utils.ts +++ b/roboteam_interface/src/modules/components/canvas/utils.ts @@ -76,12 +76,17 @@ export const useMoveBall = ( stage: ShallowRef> ) => { const aiController = useAiController(), + aiData = useAIDataStore(), keys = useMagicKeys(), { pressed } = useMousePressed() const onMouseMove = useThrottleFn((e: FederatedPointerEvent) => { if (!(keys.b.value || keys.shift.value) || !pressed.value) return const pos = transformCoordinates(e.getLocalPosition(app.value!.layers.objects)) + if (!aiData.state?.gameSettings?.isLeft) { + pos.x = -pos.x + pos.y = -pos.y + } aiController.sendSimulatorCommand({ control: { teleportBall: { x: pos.x, y: pos.y, z: 0.0, vx: 0.0, vy: 0.0, vz: 0.0 } } }) @@ -95,6 +100,7 @@ export const useShootBall = ( stage: ShallowRef> ) => { const aiController = useAiController(), + aiData = useAIDataStore(), keys = useMagicKeys(), { pressed } = useMousePressed() const onClick = useThrottleFn((e: FederatedPointerEvent) => { @@ -109,6 +115,10 @@ export const useShootBall = ( } if (!numberKeyPressed && (!keys.s.value || !pressed.value)) return const pos = transformCoordinates(e.getLocalPosition(app.value!.layers.objects)) + if (!aiData.state?.gameSettings?.isLeft) { + pos.x = -pos.x + pos.y = -pos.y + } const visionData = useVisionDataStore() const world = visionData.latestWorld if (!world) return @@ -141,6 +151,10 @@ export const useMoveRobots = ( if (!(keys.r.value || keys.alt.value) || !pressed.value) return const pos = transformCoordinates(e.getLocalPosition(app.value!.layers.objects)) + if (!aiData.state?.gameSettings?.isLeft) { + pos.x = -pos.x + pos.y = -pos.y + } const robots = [...uiStore.selectedRobots].map((id) => ({ id: { diff --git a/roboteam_interface/src/modules/components/canvas/visualizations.vue b/roboteam_interface/src/modules/components/canvas/visualizations.vue index 8d209759e..70fb98350 100644 --- a/roboteam_interface/src/modules/components/canvas/visualizations.vue +++ b/roboteam_interface/src/modules/components/canvas/visualizations.vue @@ -24,7 +24,8 @@ const onPixiTick = () => { if (data.category == Category.PATH_PLANNING && !uiStore.showPathPlanning(data.forRobotId)) return if (data.category == Category.DEBUG && !uiStore.showDebug(data.forRobotId)) return - if (data.category == Category.MARGINS && !uiStore.showMargins(data.forRobotId)) return + if (data.category == Category.MARGINS && !uiStore.showMargins()) return + if (data.category == Category.ROBOTROLES && !uiStore.showRobotRoles(data.forRobotId)) return const shape = new ShapeDrawing({ data: data as NoUndefinedField, diff --git a/roboteam_interface/src/modules/components/layout/top-panel/info-dropdown.vue b/roboteam_interface/src/modules/components/layout/top-panel/info-dropdown.vue index 422c71a5d..d9d7e056f 100644 --- a/roboteam_interface/src/modules/components/layout/top-panel/info-dropdown.vue +++ b/roboteam_interface/src/modules/components/layout/top-panel/info-dropdown.vue @@ -79,6 +79,26 @@ const props = defineProps<{
drag mouse
+ +

Colour explanation

+
    + +
  • +
    Harasser
    +
    • + +
  • +
    Passer/striker/ball placer/free kick taker/penalty taker/kick off taker
    +
    • + +
  • +
    Receiver
    +
    • + +
  • +
    Robot with card
    +
    • +
diff --git a/roboteam_interface/src/modules/components/layout/top-panel/top-panel.vue b/roboteam_interface/src/modules/components/layout/top-panel/top-panel.vue index a66a3b3aa..ca5785edf 100644 --- a/roboteam_interface/src/modules/components/layout/top-panel/top-panel.vue +++ b/roboteam_interface/src/modules/components/layout/top-panel/top-panel.vue @@ -94,22 +94,22 @@ const togglePause = () => {