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/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 5860f20aa..1be4b9ed4 100644 --- a/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h +++ b/roboteam_ai/include/roboteam_ai/stp/PlayEvaluator.h @@ -72,7 +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; + 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/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 5bcf86d64..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; }; diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index ab9156206..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" @@ -93,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; /** @@ -104,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 @@ -115,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; /** @@ -125,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; /** @@ -135,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 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 baf771da0..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h +++ /dev/null @@ -1,75 +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) */ -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 */ - -/// 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/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/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 b4c524bc7..021b5e86c 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/GameState.h +++ b/roboteam_ai/include/roboteam_ai/utilities/GameState.h @@ -8,7 +8,7 @@ #include "Constants.h" #include "RefCommand.h" #include "RuleSet.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" namespace rtt::ai { @@ -21,8 +21,8 @@ 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; 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 d1586aa0f..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" 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 aa7941621..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,13 +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; } /// 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) { @@ -34,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 { @@ -43,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 c0e15b942..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" @@ -26,12 +26,12 @@ double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajec 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 148faf091..7ef087b51 100644 --- a/roboteam_ai/src/control/positionControl/OvershootComputations.cpp +++ b/roboteam_ai/src/control/positionControl/OvershootComputations.cpp @@ -4,6 +4,7 @@ #include #include "utilities/Constants.h" + namespace rtt::ai::control { std::pair OvershootComputations::overshootingDestination(const Vector2& startPosition, const Vector2& endPosition, const Vector2& startVelocity, diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 1eb09e47b..19987eead 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -14,22 +14,22 @@ std::pair PositionControl::computeAndTrackTrajectory(const worl 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); @@ -58,8 +58,8 @@ std::pair PositionControl::computeAndTrackTrajectory(const worl 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; @@ -67,23 +67,23 @@ Vector2 PositionControl::handleBallCollision(const world::World *world, const Fi 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; @@ -93,7 +93,7 @@ Vector2 PositionControl::handleBallPlacementCollision(const world::World *world, 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; } } @@ -105,7 +105,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; } @@ -116,7 +116,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); @@ -124,7 +124,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); @@ -134,7 +134,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/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index 5b443be1e..23a3e3b55 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -85,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); } @@ -316,23 +315,6 @@ void Play::DrawMargins() noexcept { }, sideOfTheField); } - 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::ROBOTROLES, - .size = 15, - .thickness = 7, - }, - position); - } - } } } // namespace rtt::ai::stp diff --git a/roboteam_ai/src/stp/PlayEvaluator.cpp b/roboteam_ai/src/stp/PlayEvaluator.cpp index 67a85c7f7..af0f51dc7 100644 --- a/roboteam_ai/src/stp/PlayEvaluator.cpp +++ b/roboteam_ai/src/stp/PlayEvaluator.cpp @@ -71,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: diff --git a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp index f25d01702..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; @@ -76,11 +76,11 @@ 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()); + 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(); @@ -93,7 +93,7 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: 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; @@ -104,7 +104,7 @@ 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.interceptLocation = targetPosition; @@ -119,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; } @@ -150,10 +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)) { + 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, control_constants::BALL_RADIUS); + FieldComputations::projectPointIntoFieldOnLine(world->getField().value(), futureBallPosition, ballPosition, futureBallPosition, constants::BALL_RADIUS); calculateIntercept(futureBallPosition); } else { calculateIntercept(ballPosition); @@ -191,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; @@ -201,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 e0aff12f8..d4c1375fa 100644 --- a/roboteam_ai/src/stp/computations/PassComputations.cpp +++ b/roboteam_ai/src/stp/computations/PassComputations.cpp @@ -7,7 +7,7 @@ #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 { @@ -55,7 +55,7 @@ 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->getVel()); } @@ -135,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 68b0cf95a..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 { @@ -48,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(); @@ -188,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, avoidObj) && - std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { return avoidPoint.dist(point) >= control_constants::ROBOT_RADIUS; })) { + std::all_of(pointsToAvoid.begin(), pointsToAvoid.end(), [&](const Vector2 &avoidPoint) { return avoidPoint.dist(point) >= constants::ROBOT_RADIUS; })) { return point; } } @@ -215,10 +216,10 @@ Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, 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)) { @@ -235,7 +236,7 @@ Vector2 PositionComputations::calculateAvoidBallPosition(Vector2 targetPosition, 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) { @@ -249,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) { @@ -264,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 { @@ -276,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{}; @@ -309,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}); @@ -384,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++) { @@ -418,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{}; @@ -449,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{}; @@ -498,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(); @@ -517,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/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/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/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 46487740a..8e6ba2333 100644 --- a/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp @@ -8,21 +8,21 @@ uint8_t WeWillHaveBallGlobalEvaluation::metricCheck(const world::World* world, c auto& them = world->getWorld()->getThem(); // 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 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 stp::control_constants::FUZZY_FALSE; + 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 30e8b6636..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 { @@ -22,7 +22,7 @@ KeeperKickBall::KeeperKickBall() : Play() { 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"), @@ -44,7 +44,7 @@ uint8_t KeeperKickBall::score(const rtt::Field& field) noexcept { 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 { diff --git a/roboteam_ai/src/stp/plays/offensive/Attack.cpp b/roboteam_ai/src/stp/plays/offensive/Attack.cpp index d62318e99..e56905369 100644 --- a/roboteam_ai/src/stp/plays/offensive/Attack.cpp +++ b/roboteam_ai/src/stp/plays/offensive/Attack.cpp @@ -25,7 +25,7 @@ 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"), diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp index 3695f31c5..81a547498 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"), diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp index 1fc3b1204..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"), 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 decb1efd0..90b7bfe37 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -21,7 +21,7 @@ FreeKickUsPass::FreeKickUsPass() : 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/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 756f5bd08..8911beb94 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -18,7 +18,7 @@ KickOffUs::KickOffUs() : Play() { 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"), @@ -29,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 { 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 f6f876aba..c7be6ecd8 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp @@ -20,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"), @@ -31,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 { 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 a5c4c9245..a4fcf615b 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -23,8 +23,8 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { double distance2 = (robot->getPos() - ballPlacementPos).length() + (ballPlacementPos - targetPos).length(); Vector2 point1 = ballLocation - (ballPlacementPos - ballLocation).stretchToLength(0.8); Vector2 point2 = ballPlacementPos - (robot->getPos() - ballPlacementPos).stretchToLength(0.8); - bool point1InField = field.playArea.contains(point1, control_constants::OUT_OF_FIELD_MARGIN); - bool point2InField = field.playArea.contains(point2, control_constants::OUT_OF_FIELD_MARGIN); + 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) { @@ -71,7 +71,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { // Check if successful auto distanceError = (robot->getPos() - targetPos).length(); - if (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 3056caed7..36b231d32 100644 --- a/roboteam_ai/src/stp/skills/OrbitAngular.cpp +++ b/roboteam_ai/src/stp/skills/OrbitAngular.cpp @@ -1,6 +1,6 @@ #include "stp/skills/OrbitAngular.h" -#include "stp/constants/ControlConstants.h" +#include "utilities/Constants.h" #include "utilities/GameSettings.h" namespace rtt::ai::stp::skill { @@ -22,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(); @@ -36,16 +36,16 @@ Status OrbitAngular::onUpdate(const StpInfo &info) noexcept { // 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()); + command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / constants::STP_TICK_RATE); } else { command.yaw = currentYaw + Angle(targetAngularVelocity * 1 / 2.5); } - command.dribblerOn = stp::control_constants::MAX_DRIBBLER_CMD; + 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 6f3eada35..20b5b32df 100644 --- a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp @@ -12,16 +12,15 @@ #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 = 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 @@ -56,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; } @@ -116,7 +115,7 @@ Vector2 KeeperBlockBall::calculateTargetPositionBallShot(const StpInfo info, rtt const auto robotPosition = robot->getPos(); const auto robotVelocity = robot->getVel(); const auto maxRobotVelocity = info.getMaxRobotVelocity(); - const auto maxRobotAcceleration = Constants::MAX_ACC(); + 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 @@ -229,7 +228,7 @@ std::optional KeeperBlockBall::calculateTheirBallInterception(const Stp minDistanceToBall = dist; predictedBallPositionTheirRobot = vecPts.value(); predictedBallPositionTheirRobot = - predictedBallPositionTheirRobot.value() + (ball->position - predictedBallPositionTheirRobot.value()).normalize() * control_constants::CENTER_TO_FRONT; + predictedBallPositionTheirRobot.value() + (ball->position - predictedBallPositionTheirRobot.value()).normalize() * constants::CENTER_TO_FRONT; } } } 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 8ef507e46..c3536ecc3 100644 --- a/roboteam_ai/src/stp/tactics/active/GetBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/GetBall.cpp @@ -44,13 +44,13 @@ std::optional GetBall::calculateInfoForSkill(const StpInfo &info) noexc // 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 < control_constants::ROBOT_CLOSE_TO_POINT) { + 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() > control_constants::BALL_IS_MOVING_SLOW_LIMIT) { - auto newRobotPos = interceptionPosition + (interceptionPosition - ballPosition).stretchToLength(control_constants::CENTER_TO_FRONT); + } 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); @@ -58,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 f7d576895..862188918 100644 --- a/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/passive/BlockBall.cpp @@ -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(3 * 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 cc2287a26..865f033c1 100644 --- a/roboteam_ai/src/utilities/Dealer.cpp +++ b/roboteam_ai/src/utilities/Dealer.cpp @@ -268,12 +268,12 @@ double Dealer::getDefaultFlagScores(const v::RobotView &robot, const Dealer::Dea case DealerFlagTitle::KEEPER: return costForProperty(robot->getId() == 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: { @@ -291,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 41435027a..d4effd1a7 100644 --- a/roboteam_ai/src/utilities/GameStateManager.cpp +++ b/roboteam_ai/src/utilities/GameStateManager.cpp @@ -123,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); @@ -153,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 e8eafbd3a..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) { @@ -36,7 +37,7 @@ 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() && currentGameState.kickPoint.has_value() && - (ballOpt.value()->position - *currentGameState.kickPoint).length() > stp::control_constants::FREE_KICK_TAKEN_DISTANCE) || + (ballOpt.value()->position - *currentGameState.kickPoint).length() > constants::FREE_KICK_TAKEN_DISTANCE) || currentGameState.timeLeft <= 0.0)) { currentGameState = getGameStateForRefCommand(RefCommand::NORMAL_START); lastCommand = command; 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 a8bc02874..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(); @@ -84,8 +82,8 @@ bool FieldComputations::pointIsValidPosition(const rtt::Field &field, const Vect 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()) { + 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; } } @@ -245,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); @@ -339,14 +337,14 @@ Vector2 FieldComputations::projectPointToValidPosition(const Field &field, Vecto 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}; + 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; diff --git a/roboteam_ai/src/world/Robot.cpp b/roboteam_ai/src/world/Robot.cpp index 7475f8281..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,7 +92,7 @@ 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 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) { 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/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 c3088d8ea..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)); 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/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/visualizations.vue b/roboteam_interface/src/modules/components/canvas/visualizations.vue index 9d48f5e1e..70fb98350 100644 --- a/roboteam_interface/src/modules/components/canvas/visualizations.vue +++ b/roboteam_interface/src/modules/components/canvas/visualizations.vue @@ -24,7 +24,7 @@ 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({ 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 7c1ce898e..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 @@ -84,22 +84,18 @@ const props = defineProps<{
  • - Red colour: -
    Harasser
    +
    Harasser
  • - White colour: -
    Passer/striker
    +
    Passer/striker/ball placer/free kick taker/penalty taker/kick off taker
  • - Magenta colour:
    Receiver
  • - Black colour:
    Robot with card
diff --git a/roboteam_robothub/include/simulation/RobotProperties.hpp b/roboteam_robothub/include/simulation/RobotProperties.hpp index 4d259488e..f87262cd7 100644 --- a/roboteam_robothub/include/simulation/RobotProperties.hpp +++ b/roboteam_robothub/include/simulation/RobotProperties.hpp @@ -4,19 +4,19 @@ namespace rtt::robothub::simulation { // Contains all the properties that can be changed in the simulator typedef struct RobotProperties { // Units in meters, kilograms, degrees, m/s or m/s^2. - float radius = 0.09f; + float radius = 0.089f; float height = 0.15f; - float mass = 2.0f; - float maxKickSpeed = 6.5f; - float maxChipSpeed = 6.5f; - float centerToDribblerDistance = 0.09; + float mass = 2.4f; + float maxKickSpeed = 6.3f; + float maxChipSpeed = 6.3f; + float centerToDribblerDistance = 0.069f; // Robot limits - float maxAcceleration = 3.0f; - float maxAngularAcceleration = 3.0f; - float maxDeceleration = 3.0f; - float maxAngularDeceleration = 3.0f; - float maxVelocity = 3.0f; - float maxAngularVelocity = 3.0f; + float maxAcceleration = 3.5f; + float maxAngularAcceleration = 5.0f; + float maxDeceleration = 3.5f; + float maxAngularDeceleration = 5.0f; + float maxVelocity = 4.0f; + float maxAngularVelocity = 6.0f; // Wheel angles. Counter-clockwise starting from dribbler float frontRightWheelAngle = 300.0f; float backRightWheelAngle = 210.0f; diff --git a/roboteam_robothub/src/RobotHub.cpp b/roboteam_robothub/src/RobotHub.cpp index 635ed984f..9cff22c76 100644 --- a/roboteam_robothub/src/RobotHub.cpp +++ b/roboteam_robothub/src/RobotHub.cpp @@ -166,11 +166,6 @@ void RobotHub::sendCommandsToBasestation(const rtt::RobotCommands &commands, rtt command.wheelsOff = robotCommand.wheelsOff; - // command.rho = 0; - // command.theta = 0; - // command.angularVelocity = 1; - // command.useYaw = 0; - int bytesSent = 0; { std::lock_guard lock(basestationManagerMutex);