diff --git a/roboteam_ai/include/roboteam_ai/control/ControlUtils.h b/roboteam_ai/include/roboteam_ai/control/ControlUtils.h index d580c10ee..b789e05f1 100644 --- a/roboteam_ai/include/roboteam_ai/control/ControlUtils.h +++ b/roboteam_ai/include/roboteam_ai/control/ControlUtils.h @@ -24,10 +24,10 @@ class ControlUtils { /** * @brief Determines the kick force based on the distance and the type of kick * @param distance distance to the target - * @param shotType type of the kick + * @param shotPower type of the kick * @return a kick speed between min and max kick speed */ - static double determineKickForce(const double distance, stp::ShotType shotType) noexcept; + static double determineKickForce(const double distance, stp::ShotPower shotPower) noexcept; /** * @brief Determines the chip force based on the distance and the type of chip * @param distance distance to the target diff --git a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h index aa3d2d3b5..50b7253a1 100644 --- a/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h +++ b/roboteam_ai/include/roboteam_ai/control/positionControl/PositionControl.h @@ -61,10 +61,11 @@ class PositionControl { * @param world a pointer to the current world * @param field the field object, used onwards by the collision detector * @param currentPosition the current position of the robot + * @param targetPosition the desired position that the robot has to reach * @param avoidObjects whether or not to avoid objects * @return A new target position */ - Vector2 handleBallPlacementCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, stp::AvoidObjects avoidObjects); + Vector2 handleBallPlacementCollision(const rtt::world::World *world, const rtt::Field &field, Vector2 currentPosition, Vector2 targetPosition, stp::AvoidObjects avoidObjects); /** * @brief Handles the collision with the defense area at the current position, will move away from the defense area as quickly as possible. diff --git a/roboteam_ai/include/roboteam_ai/stp/Play.hpp b/roboteam_ai/include/roboteam_ai/stp/Play.hpp index bafa41dad..e2018bb42 100644 --- a/roboteam_ai/include/roboteam_ai/stp/Play.hpp +++ b/roboteam_ai/include/roboteam_ai/stp/Play.hpp @@ -25,6 +25,8 @@ class Play { std::vector startPlayEvaluation; /**< Invariant vector that contains invariants that need to be true to start this play */ + static int waller_count; + /** * @brief Initializes stpInfos struct, distributes roles, sets the previousRobotNum variable and calls onInitialize() */ diff --git a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h index ace25fc7e..bada16d34 100644 --- a/roboteam_ai/include/roboteam_ai/stp/StpInfo.h +++ b/roboteam_ai/include/roboteam_ai/stp/StpInfo.h @@ -3,6 +3,7 @@ #include #include +#include #include "computations/PositionScoring.h" #include "utilities/StpInfoEnums.h" @@ -46,10 +47,11 @@ struct StpInfo { bool getDribblerOn() const { return dribblerOn; } void setDribblerOn(bool dribblerOn) { this->dribblerOn = dribblerOn; } - ShotType getShotType() const { return shotType; } - void setShotType(ShotType shotType) { this->shotType = shotType; } + ShotPower getShotPower() const { return shotPower; } + void setShotPower(ShotPower shotPower) { this->shotPower = shotPower; } - void setKickOrChip(const std::optional& kickOrChip) { StpInfo::kickOrChip = kickOrChip; } + rtt::KickType getKickOrChip() const { return kickOrChip; } + void setKickOrChip(rtt::KickType kickOrChip) { this->kickOrChip = kickOrChip; } world::World* getCurrentWorld() const { return currentWorld; } /// This function is used in a lambda, [[maybe_unused]] is to suppress 'unused' warnings @@ -149,7 +151,7 @@ struct StpInfo { /** * Type of the kick/chip */ - ShotType shotType{}; + ShotPower shotPower{}; /** * Reference yaw of the robot @@ -164,7 +166,7 @@ struct StpInfo { /** * Set the shot to be a kick or chip */ - std::optional kickOrChip; + rtt::KickType kickOrChip = rtt::KickType::NO_KICK; /** * The maximum velocity the robot is allowed to have diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h index 125795965..22568a0cf 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/InterceptionComputations.h @@ -78,6 +78,14 @@ class InterceptionComputations { * @return HarasserInfo with the id and the time to the ball */ static InterceptionInfo calculateInterceptionInfoExcludingKeeperAndCarded(const rtt::world::World *world) noexcept; + + /** + * @brief Calculates the point where they will have the ball + * @param info the StpInfo struct + * @param ballTrajectory the trajectory of the ball + * @return the point where they will have the ball + */ + static std::optional calculateTheirBallInterception(const rtt::world::World *world, rtt::LineSegment ballTrajectory) noexcept; }; } // namespace rtt::ai::stp #endif // RTT_INTERCEPTIONCOMPUTATIONS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h index aaf40aea8..ab19a6bff 100644 --- a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h +++ b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h @@ -54,6 +54,14 @@ class PositionComputations { static gen::ScoredPosition getPosition(std::optional currentPosition, const Grid &searchGrid, gen::ScoreProfile profile, const Field &field, const world::World *world); + /** + * @brief Calculates amount of wallers needed based on the position of the ball + * @param field + * @param world + * @return The amount of wallers needed + */ + static void setAmountOfWallers(const rtt::Field &field, rtt::world::World *world) noexcept; + /** * @brief Makes a wall if not ready done, saves it in calculatedWallPositions and deals the index * @param index Index of the wall position (do unique positions) @@ -150,6 +158,8 @@ class PositionComputations { */ static void calculateInfoForAvoidBallHarasser(std::unordered_map &stpInfos, world::World *world) noexcept; + static int amountOfWallers; + private: /** * @brief Calculates a position outside of a given shape diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/Defend.h similarity index 82% rename from roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h rename to roboteam_ai/include/roboteam_ai/stp/plays/defensive/Defend.h index 77e21ba2e..5832f8bbe 100644 --- a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h +++ b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/Defend.h @@ -1,5 +1,5 @@ -#ifndef RTT_DEFENDSHOT_H -#define RTT_DEFENDSHOT_H +#ifndef RTT_DEFEND_H +#define RTT_DEFEND_H #include @@ -8,16 +8,16 @@ namespace rtt::ai::stp::play { /** - * @brief DefendShot Play is executed when the opponent has or is close to the ball and on our side of the field. + * @brief Defend Play is executed when the opponent has or is close to the ball. * In this case they most likely will try to score. Some defenders defend the goal by blocking the path between enemy * robots and the goal. Other defenders block other enemy robots to avoid passes to them. */ -class DefendShot : public Play { +class Defend : public Play { public: /** * @brief Constructor that initializes roles with roles that are necessary for this play */ - DefendShot(); + Defend(); /** * @brief Calculates the score of this play to determine which play is best in this situation @@ -46,4 +46,4 @@ class DefendShot : public Play { }; } // namespace rtt::ai::stp::play -#endif // RTT_DEFENDSHOT_H +#endif // RTT_DEFEND_H diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h b/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h deleted file mode 100644 index 2d1bb5094..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef RTT_DEFENDPASS_H -#define RTT_DEFENDPASS_H - -#include - -#include "stp/Play.hpp" - -namespace rtt::ai::stp::play { - -/** - * @brief DefendPass Play is executed when the opponent has or is close to the ball but not necessarily on our side of the field. - * In this case the opponent most likely will pass to another robot. Our robots will namely block off robots that can - * be passed to. - */ -class DefendPass : public Play { - public: - /** - * @brief Constructor that initializes roles with roles that are necessary for this play - */ - DefendPass(); - - /** - * @brief Calculates the score of this play to determine which play is best in this situation - * @param field The current Field - * @return The score of this play (0-255) - */ - uint8_t score(const rtt::Field& field) noexcept override; - - /** - * @brief Assigns robots to roles of this play - * @return Map with assigned roles - */ - Dealer::FlagMap decideRoleFlags() const noexcept override; - - /** - * @brief Calculates info for the roles - */ - void calculateInfoForRoles() noexcept override; - - /** - * @brief Retrieves the name of the play - * @return The name of the play as string - */ - const char* getName() const override; - - InterceptionInfo harasserInfo; -}; -} // namespace rtt::ai::stp::play - -#endif // RTT_DEFENDPASS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/plays/offensive/ChippingPass.h b/roboteam_ai/include/roboteam_ai/stp/plays/offensive/ChippingPass.h deleted file mode 100644 index b9b484f86..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/plays/offensive/ChippingPass.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef RTT_CHIPPINGPASS_H -#define RTT_CHIPPINGPASS_H - -#include - -#include "stp/Play.hpp" -#include "stp/computations/PassComputations.h" - -namespace rtt::ai::stp::play { -/** - * @brief The chipping pass play is executed when we want to pass the ball to a robot that can not be passed to directly - */ -class ChippingPass : public Play { - public: - /** - * @brief Constructor that initializes roles with roles that are necessary for this play - */ - ChippingPass(); - - /** - * @brief Calculate how beneficial we expect this play to be - * @param field The current field - * @return A score of this play - */ - uint8_t score(const rtt::Field& field) noexcept override; - - /** - * @brief Assigns robots to roles of this play - * @return A mapping of dealer flags per role - */ - Dealer::FlagMap decideRoleFlags() const noexcept override; - - /** - * @brief Calculates info for the roles - */ - void calculateInfoForRoles() noexcept override; - - /** - * @brief Gets the name of the play - * @return The name of the play - */ - const char* getName() const override; - - /** - * @brief Check if play should end. True if pass arrived, if the ball is not moving anymore after pass, or if there is a better pass available - * @return Boolean that indicates whether the play should end - */ - bool shouldEndPlay() noexcept override; - - private: - /** - * @brief Return true if passer is done with ChipAtPos tactic - * @return Boolean that indicates whether the passer is done with the ChipAtPos tactic - */ - bool ballKicked(); - - PassInfo passInfo; /**< Struct containing info about the pass. Calculated once for each time this play is run */ -}; -} // namespace rtt::ai::stp::play - -#endif // RTT_CHIPPINGPASS_H diff --git a/roboteam_ai/include/roboteam_ai/stp/roles/active/Chipper.h b/roboteam_ai/include/roboteam_ai/stp/roles/active/Chipper.h deleted file mode 100644 index 2f29dbeca..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/roles/active/Chipper.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef RTT_CHIPPER_H -#define RTT_CHIPPER_H - -#include "stp/Role.hpp" - -namespace rtt::ai::stp::role { -/** - * @brief Class that defines the Chipper role. The Chipper will chip the ball to another robot. - */ -class Chipper : public Role { - public: - /** - * @brief Constructor that sets the name of the role and creates a state machine of tactics - * @param name name of the role - */ - Chipper(std::string name); -}; -} // namespace rtt::ai::stp::role - -#endif // RTT_CHIPPER_H \ No newline at end of file diff --git a/roboteam_ai/include/roboteam_ai/stp/roles/active/KeeperPasser.h b/roboteam_ai/include/roboteam_ai/stp/roles/active/KeeperPasser.h index fac06e09b..85f79e5a1 100644 --- a/roboteam_ai/include/roboteam_ai/stp/roles/active/KeeperPasser.h +++ b/roboteam_ai/include/roboteam_ai/stp/roles/active/KeeperPasser.h @@ -14,6 +14,13 @@ class KeeperPasser : public Role { * @param name The name of the role */ KeeperPasser(std::string name); + + /** + * @brief Besides the default update from base class Role, it also switches between tactics depending on the ball position + * @param info TacticInfo to be passed to update() + * @return The status that the current tactic returns + */ + [[nodiscard]] Status update(StpInfo const& info) noexcept override; }; } // namespace rtt::ai::stp::role diff --git a/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h b/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h deleted file mode 100644 index 41768d6c2..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/skills/Chip.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef RTT_CHIP_H -#define RTT_CHIP_H - -#include "stp/Skill.h" - -namespace rtt::ai::stp::skill { -/** - * @brief Class that defines the chipping skill. The robot will chip the ball a given distance - */ -class Chip : public Skill { - /** - * @brief On update of this tactic - * @param info StpInfo struct with all relevant info for this robot and this skill - * @return A Status, either Running or Success - */ - Status onUpdate(StpInfo const& info) noexcept override; - - /** - * @brief Gets the skill name - * @return The name of this skill - */ - const char* getName() override; -}; -} // namespace rtt::ai::stp::skill - -#endif // RTT_CHIP_H \ No newline at end of file diff --git a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h index 6c839e724..1a69e8c83 100644 --- a/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h +++ b/roboteam_ai/include/roboteam_ai/stp/tactics/KeeperBlockBall.h @@ -82,14 +82,6 @@ class KeeperBlockBall : public Tactic { */ static Vector2 calculateTargetPositionBallShot(const StpInfo info, rtt::LineSegment keepersLineSegment, rtt::LineSegment ballTrajectory) noexcept; - /** - * @brief Calculates the point where they will have the ball - * @param info the StpInfo struct - * @param ballTrajectory the trajectory of the ball - * @return the point where they will have the ball - */ - static std::optional calculateTheirBallInterception(const StpInfo &info, rtt::LineSegment ballTrajectory) noexcept; - /** * @brief Calculates the target position for the keeper when the ball is not shot * @param info the StpInfo struct diff --git a/roboteam_ai/include/roboteam_ai/stp/tactics/active/ChipAtPos.h b/roboteam_ai/include/roboteam_ai/stp/tactics/active/ChipAtPos.h deleted file mode 100644 index 075da6fce..000000000 --- a/roboteam_ai/include/roboteam_ai/stp/tactics/active/ChipAtPos.h +++ /dev/null @@ -1,55 +0,0 @@ -#ifndef RTT_CHIPATPOS_H -#define RTT_CHIPATPOS_H - -#include "stp/Tactic.h" - -namespace rtt::ai::stp::tactic { -/** - * @brief Class that defines the ChipAtPos tactic. The robot will chip the ball to a given position - */ -class ChipAtPos : public Tactic { - public: - /** - * @brief Constructor for the tactic, it constructs the state machine of skills - */ - ChipAtPos(); - - private: - /** - * @brief Calculate the info for skills from the StpInfo struct parameter - * @param info info is the StpInfo passed by the role - * @return std::optional based on the StpInfo parameter - */ - std::optional calculateInfoForSkill(StpInfo const &info) noexcept override; - - /** - * @brief Is this tactic failing during execution (go back to the previous tactic) - * @param info StpInfo can be used to check some data - * @return true, tactic will fail (go back to prev tactic), false execution will continue as usual - * Returns true when robot doesn't have the ball or if there is no shootTarget - */ - bool isTacticFailing(const StpInfo &info) noexcept override; - - /** - * @brief Should this tactic be reset (go back to the first skill of this tactic) - * @param info StpInfo can be used to check some data - * @return true if tactic should reset, false if execution should continue - * Returns true when the robot yaw is not within its error margin - */ - bool shouldTacticReset(const StpInfo &info) noexcept override; - - /** - * @brief Is this tactic an end tactic? - * @return This will always return false, since it is NOT an endTactic - */ - bool isEndTactic() noexcept override; - - /** - * @brief Gets the tactic name - * @return The name of this tactic - */ - const char *getName() override; -}; -} // namespace rtt::ai::stp::tactic - -#endif // RTT_CHIPATPOS_H diff --git a/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h b/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h index a91d5646b..792e19081 100644 --- a/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h +++ b/roboteam_ai/include/roboteam_ai/utilities/StpInfoEnums.h @@ -3,15 +3,10 @@ #include namespace rtt::ai::stp { -/** - * @brief Whether this robot should kick or chip in the shoot skill - */ -enum class KickOrChip { KICK, CHIP }; - /** * @brief The type of shot this robot should use. Used for determining kick/chip velocity */ -enum class ShotType { PASS, TARGET, MAX }; +enum class ShotPower { PASS, TARGET, MAX }; /** * @brief The status that a skill/tactic can return diff --git a/roboteam_ai/src/STPManager.cpp b/roboteam_ai/src/STPManager.cpp index 56423ac6e..32cfd8037 100644 --- a/roboteam_ai/src/STPManager.cpp +++ b/roboteam_ai/src/STPManager.cpp @@ -18,12 +18,10 @@ * Plays are included here */ #include "gui/Out.h" -#include "stp/plays/defensive/DefendPass.h" -#include "stp/plays/defensive/DefendShot.h" +#include "stp/plays/defensive/Defend.h" #include "stp/plays/defensive/KeeperKickBall.h" #include "stp/plays/offensive/Attack.h" #include "stp/plays/offensive/AttackingPass.h" -#include "stp/plays/offensive/ChippingPass.h" #include "stp/plays/referee_specific/BallPlacementThem.h" #include "stp/plays/referee_specific/BallPlacementUsForceStart.h" #include "stp/plays/referee_specific/BallPlacementUsFreeKick.h" @@ -53,11 +51,9 @@ const STPManager::PlaysVec STPManager::plays = ([] { auto plays = std::vector>(); plays.emplace_back(std::make_unique()); - // plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); diff --git a/roboteam_ai/src/control/ControlUtils.cpp b/roboteam_ai/src/control/ControlUtils.cpp index 90a8014d1..cd12915a2 100644 --- a/roboteam_ai/src/control/ControlUtils.cpp +++ b/roboteam_ai/src/control/ControlUtils.cpp @@ -18,16 +18,16 @@ double ControlUtils::getMaxVelocity(bool hasBall) { } /// Calculate the kick force -double ControlUtils::determineKickForce(const double distance, stp::ShotType shotType) noexcept { - if (shotType == stp::ShotType::MAX) return constants::MAX_KICK_POWER; +double ControlUtils::determineKickForce(const double distance, stp::ShotPower shotPower) noexcept { + if (shotPower == stp::ShotPower::MAX) return constants::MAX_KICK_POWER; double kickForce; - if (shotType == stp::ShotType::PASS) { + if (shotPower == stp::ShotPower::PASS) { kickForce = 1.8; - } else if (shotType == stp::ShotType::TARGET) { + } else if (shotPower == stp::ShotPower::TARGET) { kickForce = 0.5; } else { - RTT_WARNING("No shotType set! Setting kickForce to 0") + RTT_WARNING("No shotPower set! Setting kickForce to 0") kickForce = 0; } // Calculate the velocity based on this function with the previously set limitingFactor diff --git a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp index b7c3adf83..5623c202a 100644 --- a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp +++ b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp @@ -116,10 +116,22 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory } if (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM || GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) { - auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1); auto ballPlacementPosition = GameStateManager::getRefereeDesignatedPosition(); - LineSegment ballPlacementLine(ballPosition, ballPlacementPosition); - if (ballPlacementLine.distanceToLine(positionOurRobot) < constants::AVOID_BALL_DISTANCE + additionalMargin) { + bool isBallPlacementCollision = true; + for (int i = checkPoint; i < checkPoint + 10; i++) { + auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1); + if (i >= static_cast(pathPoints.size())) { + isBallPlacementCollision = false; + break; + } + auto positionOurRobot = Trajectory.getPosition(i * 0.1); + auto ballPlacementLine = LineSegment(ballPlacementPosition, ballPosition); + if (ballPlacementLine.distanceToLine(positionOurRobot) > constants::AVOID_BALL_DISTANCE + additionalMargin) { + isBallPlacementCollision = false; + break; + } + } + if (isBallPlacementCollision) { return checkPoint * 0.1; } } @@ -135,11 +147,13 @@ bool CollisionCalculations::isCollidingWithMovingObject(const Trajectory2D &Traj double CollisionCalculations::getFirstCollisionTime(const Trajectory2D &Trajectory, stp::AvoidObjects avoidObjects, const Field &field, int &robotId, const world::World *world, const std::unordered_map> &computedPaths) { - double collisionTime = getFirstCollisionTimeMotionlessObject(Trajectory, avoidObjects, field); - if (collisionTime != -1.0) { - return collisionTime; - } - return getFirstCollisionTimeMovingObject(Trajectory, avoidObjects, robotId, world, computedPaths); + double collisionTimeMotionless = getFirstCollisionTimeMotionlessObject(Trajectory, avoidObjects, field); + double collisionTimeMoving = getFirstCollisionTimeMovingObject(Trajectory, avoidObjects, robotId, world, computedPaths); + + return (collisionTimeMotionless == -1.0 && collisionTimeMoving == -1.0) ? -1.0 + : (collisionTimeMotionless == -1.0) ? collisionTimeMoving + : (collisionTimeMoving == -1.0) ? collisionTimeMotionless + : std::min(collisionTimeMotionless, collisionTimeMoving); } bool CollisionCalculations::isColliding(const Trajectory2D &Trajectory, stp::AvoidObjects avoidObjects, const Field &field, int &robotId, const world::World *world, diff --git a/roboteam_ai/src/control/positionControl/PositionControl.cpp b/roboteam_ai/src/control/positionControl/PositionControl.cpp index 6f64a6d53..225c8d632 100644 --- a/roboteam_ai/src/control/positionControl/PositionControl.cpp +++ b/roboteam_ai/src/control/positionControl/PositionControl.cpp @@ -21,7 +21,7 @@ Vector2 PositionControl::computeAndTrackTrajectory(const world::World *world, co GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) && LineSegment(world->getWorld()->getBall()->get()->position, GameStateManager::getRefereeDesignatedPosition()).distanceToLine(currentPosition) < ai::constants::AVOID_BALL_DISTANCE) { - targetPosition = handleBallPlacementCollision(world, field, currentPosition, avoidObjects); + targetPosition = handleBallPlacementCollision(world, field, currentPosition, targetPosition, avoidObjects); 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 > constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN && @@ -61,7 +61,7 @@ Vector2 PositionControl::handleBallCollision(const world::World *world, const Fi return targetPosition; } int rotationStepDegrees = 10; - int maxRotationDegrees = 180; + int maxRotationDegrees = 330; for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) { for (int sign : {1, -1}) { double rotation = sign * i * M_PI / 180; @@ -75,28 +75,41 @@ Vector2 PositionControl::handleBallCollision(const world::World *world, const Fi 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) { +Vector2 PositionControl::handleBallPlacementCollision(const world::World *world, const Field &field, Vector2 currentPosition, Vector2 targetPosition, + stp::AvoidObjects avoidObjects) { + if ((targetPosition - currentPosition).length() < constants::AVOID_BALL_DISTANCE * 2) { + return targetPosition; + } auto placementPos = GameStateManager::getRefereeDesignatedPosition(); auto ballPos = world->getWorld()->getBall()->get()->position; 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, constants::OUT_OF_FIELD_MARGIN)) { - return targetPosition; + auto distance = LineSegment(ballPos, placementPos).distanceToLine(currentPosition); + Vector2 targetPositionOne = currentPosition + direction.stretchToLength(constants::AVOID_BALL_DISTANCE - distance); + Vector2 targetPositionTwo = currentPosition - direction.stretchToLength(constants::AVOID_BALL_DISTANCE + distance); + bool targetOneIsValid = FieldComputations::pointIsValidPosition(field, targetPositionOne, avoidObjects, constants::OUT_OF_FIELD_MARGIN); + bool targetTwoIsValid = FieldComputations::pointIsValidPosition(field, targetPositionTwo, avoidObjects, constants::OUT_OF_FIELD_MARGIN); + if (targetOneIsValid && !targetTwoIsValid) { + return currentPosition + direction; } - int rotationStepDegrees = 10; - int maxRotationDegrees = 180; - for (int i = rotationStepDegrees; i <= maxRotationDegrees; i += rotationStepDegrees) { - for (int sign : {1, -1}) { - double rotation = sign * i * M_PI / 180; - Vector2 rotatedDirection = direction.rotate(rotation); - Vector2 potentialTargetPosition = currentPosition + rotatedDirection; - if (FieldComputations::pointIsValidPosition(field, potentialTargetPosition, avoidObjects, constants::OUT_OF_FIELD_MARGIN)) { - return potentialTargetPosition; - } + if (!targetOneIsValid && targetTwoIsValid) { + return currentPosition - direction; + } + if (targetOneIsValid && targetTwoIsValid) { + auto intersectionTargetOne = LineSegment(targetPositionOne, targetPosition).doesIntersect(LineSegment(ballPos, placementPos)); + auto intersectionTargetTwo = LineSegment(targetPositionTwo, targetPosition).doesIntersect(LineSegment(ballPos, placementPos)); + if (!intersectionTargetOne && intersectionTargetTwo) { + return currentPosition + direction; + } else if (intersectionTargetOne && !intersectionTargetTwo) { + return currentPosition - direction; } + return ((targetPosition - targetPositionOne).length() + constants::AVOID_BALL_DISTANCE - distance) < + ((targetPosition - targetPositionTwo).length() + constants::AVOID_BALL_DISTANCE + distance) + ? currentPosition + direction + : currentPosition - direction; + } else { + return currentPosition + direction; } - return targetPosition; } Vector2 PositionControl::handleDefenseAreaCollision(const Field &field, Vector2 currentPosition) { diff --git a/roboteam_ai/src/stp/Play.cpp b/roboteam_ai/src/stp/Play.cpp index 23a3e3b55..14bfd2127 100644 --- a/roboteam_ai/src/stp/Play.cpp +++ b/roboteam_ai/src/stp/Play.cpp @@ -4,6 +4,7 @@ #include "gui/Out.h" namespace rtt::ai::stp { +int Play::waller_count; void Play::initialize() noexcept { stpInfos.clear(); @@ -141,6 +142,7 @@ void Play::distributeRoles() noexcept { auto roleName{role->getName()}; stpInfos[roleName].setRoleName(roleName); } + Play::waller_count = std::count_if(roles.begin(), roles.end(), [](const std::unique_ptr &role) { return role && role->getName().find("waller") != std::string::npos; }); auto flagMap = decideRoleFlags(); @@ -270,21 +272,21 @@ void Play::DrawMargins() noexcept { else color = proto::Drawing::RED; - } else + } else if (!world->getWorld()->getBall()->get()->visible) + color = proto::Drawing::CYAN; + else color = proto::Drawing::GREY; - for (auto method : {proto::Drawing::CIRCLES, proto::Drawing::LINES_CONNECTED}) { - rtt::ai::gui::Out::draw( - { - .label = method == proto::Drawing::CIRCLES ? "Ball area to avoid" : "Path to placement location", - .color = method == proto::Drawing::CIRCLES ? color : proto::Drawing::BLACK, - .method = method, - .category = proto::Drawing::MARGINS, - .size = method == proto::Drawing::CIRCLES ? 52 : 8, - .thickness = method == proto::Drawing::CIRCLES ? 4 : 8, - }, - pathToPlacementLocation); - } + rtt::ai::gui::Out::draw( + { + .label = "Ballplacement area", + .color = color, + .method = proto::Drawing::TUBES, + .category = proto::Drawing::MARGINS, + .size = 52, + .thickness = 4, + }, + pathToPlacementLocation); // Drawing all figures regarding ball placement location and the path towards it if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || diff --git a/roboteam_ai/src/stp/PlayDecider.cpp b/roboteam_ai/src/stp/PlayDecider.cpp index 27a86a7e2..8e82abf59 100644 --- a/roboteam_ai/src/stp/PlayDecider.cpp +++ b/roboteam_ai/src/stp/PlayDecider.cpp @@ -27,10 +27,10 @@ Play* PlayDecider::decideBestPlay(const rtt::world::World* world, const std::vec return getPlayForName(playLock.playName.value(), plays); } - // If there are no valid plays, default to defend shot + // If there are no valid plays, default to defend if (playsWithScores.empty()) { RTT_WARNING("No valid plays found!"); - return getPlayForName("Defend Shot", plays); + return getPlayForName("Defend", plays); } return std::max_element(playsWithScores.begin(), playsWithScores.end(), [](const auto& lhs, const auto& rhs) { return lhs.second < rhs.second; })->first; diff --git a/roboteam_ai/src/stp/Tactic.cpp b/roboteam_ai/src/stp/Tactic.cpp index c1617c0ae..87e19a154 100644 --- a/roboteam_ai/src/stp/Tactic.cpp +++ b/roboteam_ai/src/stp/Tactic.cpp @@ -20,7 +20,7 @@ Status Tactic::update(StpInfo const &info) noexcept { (void)status; // return of update is never used, but it is marked [[no-discard]] in the state machine. This cast suppresses that warning. } else { - RTT_ERROR(getName(), "Not all data was present, bad update!") + RTT_ERROR(getName(), " Not all data was present, bad update!") return Status::Failure; } diff --git a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp index eef5d20dd..d3beacfed 100644 --- a/roboteam_ai/src/stp/computations/InterceptionComputations.cpp +++ b/roboteam_ai/src/stp/computations/InterceptionComputations.cpp @@ -48,7 +48,6 @@ KeeperInterceptionInfo InterceptionComputations::calculateKeeperInterceptionInfo InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std::vector &ourRobots, const world::World *world) { InterceptionInfo interceptionInfo; - int LineOfSightScore = 50; auto maxRobotVelocity = GameStateManager::getCurrentGameState().getRuleSet().getMaxRobotVel(); auto ballPosition = world->getWorld()->getBall()->get()->position; auto pastBallPosition = ballPosition; @@ -56,27 +55,20 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: auto ballVelocity = world->getWorld()->getBall()->get()->velocity; bool shouldReturn = false; interceptionInfo.interceptLocation = ballPosition; + const LineSegment ballTrajectory(world->getWorld()->getBall()->get()->position, world->getWorld()->getBall()->get()->expectedEndPosition); + auto interceptRobot = calculateTheirBallInterception(world, ballTrajectory); // Helper function to calculate the intercept info for a given target position auto calculateIntercept = [&](const Vector2 &targetPosition) { - // If the LoS score is too low - if (PositionScoring::scorePosition(futureBallPosition, gen::LineOfSight, world->getField().value(), world).score < LineOfSightScore) { - auto minDistance = std::numeric_limits::max(); - auto theirRobots = world->getWorld()->getThem(); - Vector2 robotCloseToBallPos; - for (const auto &theirRobot : theirRobots) { - auto distance = LineSegment(futureBallPosition, pastBallPosition).distanceToLine(theirRobot->getPos()); - if (distance < minDistance) { - minDistance = distance; - robotCloseToBallPos = theirRobot->getPos(); - } - } + // Check if they are able to intercept the ball at the part of the trajectory we are checking + if (interceptRobot && (*interceptRobot - pastBallPosition).length() < (futureBallPosition - pastBallPosition).length()) { + auto minDistance = (*interceptRobot - pastBallPosition).length(); + auto robotCloseToBallPos = *interceptRobot; if ((world->getWorld()->getRobotClosestToBall(world::us)->get()->getPos() - pastBallPosition).length() < minDistance) { interceptionInfo.interceptLocation = LineSegment(futureBallPosition, pastBallPosition).project(world->getWorld()->getRobotClosestToBall(world::us)->get()->getPos()); } else { - interceptionInfo.interceptLocation = - robotCloseToBallPos + (world->getWorld()->getBall()->get()->position - robotCloseToBallPos).stretchToLength(constants::ROBOT_RADIUS * 3); + interceptionInfo.interceptLocation = robotCloseToBallPos; } double minTimeToTarget = std::numeric_limits::max(); for (const auto &robot : ourRobots) { @@ -135,14 +127,14 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfo(const std:: continue; } else if (world->getField().value().leftDefenseArea.contains(futureBallPosition) || world->getField().value().rightDefenseArea.contains(futureBallPosition)) { // project futureBallPos to where it enters the defense area - auto intersections = FieldComputations::getDefenseArea(world->getField().value(), world->getField().value().leftDefenseArea.contains(futureBallPosition), 0, 0) + auto intersections = FieldComputations::getDefenseArea(world->getField().value(), world->getField().value().leftDefenseArea.contains(futureBallPosition), 0, 0.5) .intersections({pastBallPosition, futureBallPosition}); if (intersections.size() == 1) { futureBallPosition = intersections.at(0); } } else if (world->getField().value().leftDefenseArea.contains(pastBallPosition) || world->getField().value().rightDefenseArea.contains(pastBallPosition)) { // project pastBallPos to where it exits the defense area - auto intersections = FieldComputations::getDefenseArea(world->getField().value(), world->getField().value().leftDefenseArea.contains(pastBallPosition), 0, 0) + auto intersections = FieldComputations::getDefenseArea(world->getField().value(), world->getField().value().leftDefenseArea.contains(pastBallPosition), 0, 0.5) .intersections({pastBallPosition, futureBallPosition}); if (intersections.size() == 1) { pastBallPosition = intersections.at(0); @@ -222,4 +214,25 @@ InterceptionInfo InterceptionComputations::calculateInterceptionInfoExcludingKee return calculateInterceptionInfoForKickingRobots(ourRobots, world); } + +std::optional InterceptionComputations::calculateTheirBallInterception(const world::World *world, rtt::LineSegment ballTrajectory) noexcept { + auto ballOpt = world->getWorld()->getBall(); + if (!ballOpt) return std::nullopt; + auto ball = ballOpt.value(); + std::optional closestInterceptionPoint = std::nullopt; + double minimumDistanceToBall = std::numeric_limits::max(); + for (const auto &opponentRobot : world->getWorld()->get()->getThem()) { + auto projectedPoint = ballTrajectory.project(opponentRobot->getPos()); + if (projectedPoint.dist(opponentRobot->getPos()) < 0.5) { + double distanceToBallFromProjectedPoint = projectedPoint.dist(ball->position); + if (distanceToBallFromProjectedPoint < minimumDistanceToBall) { + minimumDistanceToBall = distanceToBallFromProjectedPoint; + Vector2 adjustmentVector = (ball->position - projectedPoint).normalize() * constants::CENTER_TO_FRONT; + closestInterceptionPoint = projectedPoint + adjustmentVector; + } + } + } + return closestInterceptionPoint; +} + } // namespace rtt::ai::stp \ No newline at end of file diff --git a/roboteam_ai/src/stp/computations/PassComputations.cpp b/roboteam_ai/src/stp/computations/PassComputations.cpp index d4c1375fa..9fb239d36 100644 --- a/roboteam_ai/src/stp/computations/PassComputations.cpp +++ b/roboteam_ai/src/stp/computations/PassComputations.cpp @@ -141,7 +141,7 @@ double PassComputations::calculateRobotTravelTime(Vector2 robotPosition, Vector2 double PassComputations::calculateBallTravelTime(Vector2 passLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition) { 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); + double ballSpeed = control::ControlUtils::determineKickForce(passLocation.dist(targetPosition), ShotPower::PASS); auto ballTime = passLocation.dist(targetPosition) / ballSpeed; return travelTime + rotateTime + ballTime; } diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index 9dd32d52f..4bc788e3d 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -15,6 +15,7 @@ #include "world/World.hpp" namespace rtt::ai::stp { +int PositionComputations::amountOfWallers = 4; gen::ScoredPosition PositionComputations::getPosition(std::optional currentPosition, const Grid &searchGrid, gen::ScoreProfile profile, const Field &field, const world::World *world) { @@ -34,6 +35,35 @@ gen::ScoredPosition PositionComputations::getPosition(std::optionalgetWorld().value().getBall()->get()->position, constants::BALL_RADIUS); + } + auto lineToBottomPost = ballPos - field.leftGoalArea.bottomRight(); + auto lineToTopPost = ballPos - field.leftGoalArea.topRight(); + auto angleBetweenLines = lineToBottomPost.toAngle().shortestAngleDiff(lineToTopPost.toAngle()); + if (field.rightPlayArea.contains(ballPos, -0.1)) { + PositionComputations::amountOfWallers = 2; + } else if (field.leftPlayArea.contains(ballPos)) { + if (angleBetweenLines > 0.25) + PositionComputations::amountOfWallers = 4; + else if (angleBetweenLines > 0.2) + PositionComputations::amountOfWallers = std::clamp(PositionComputations::amountOfWallers, 4, 3); + else if (angleBetweenLines > 0.13) + PositionComputations::amountOfWallers = 3; + else if (angleBetweenLines > 0.1) + PositionComputations::amountOfWallers = std::clamp(PositionComputations::amountOfWallers, 3, 2); + else + PositionComputations::amountOfWallers = 2; + } +} + Vector2 PositionComputations::getWallPosition(int index, int amountDefenders, const rtt::Field &field, rtt::world::World *world) { if (ComputationManager::calculatedWallPositions.empty()) { ComputationManager::calculatedWallPositions = determineWallPositions(field, world, amountDefenders); @@ -49,67 +79,78 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie } // Constants for positioning the defenders - const double radius = constants::ROBOT_RADIUS; - const double spacingRobots = radius * 0.5; - const double spaceBetweenDefenseArea = 2 * radius; + const double spacingRobots = constants::ROBOT_RADIUS * 0.5; RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); - - Vector2 ballPos; + auto ball = world->getWorld().value().getBall()->get(); + const LineSegment ballTrajectory(ball->position, ball->expectedEndPosition); + Vector2 ballPos = ball->position; // Calculate the position of the ball, projected onto the field if (currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::BALL_PLACEMENT_US || currentGameState == RefCommand::BALL_PLACEMENT_US_DIRECT || currentGameState == RefCommand::PREPARE_FORCED_START) { + // If the ball has to be placed somewhere on the field, assume the ball is already there ballPos = GameStateManager::getRefereeDesignatedPosition(); + } else if (field.leftDefenseArea.contains(ballPos)) { + // If the ball is in our defense area, project it out of it + ballPos = FieldComputations::projectPointOutOfDefenseArea(field, ballPos, true, false); + } else if ((world->getWorld().value().getBall()->get()->velocity).length() > constants::BALL_GOT_SHOT_LIMIT && + InterceptionComputations::calculateTheirBallInterception(world, ballTrajectory).has_value()) { + ballPos = *InterceptionComputations::calculateTheirBallInterception(world, ballTrajectory); } else { - ballPos = FieldComputations::projectPointInField(field, world->getWorld().value().getBall()->get()->position); + // Project the ball into the field and use that location + ballPos = FieldComputations::projectPointInField(field, ballPos, constants::BALL_RADIUS); } + // Dynamic distance, the further the ball is from our goal, the further forwards our wall stands. + double theirDistanceToGoal = std::clamp(((ballPos - field.leftGoalArea.rightLine().center()).length() - 4) / 2, 0.0, field.playArea.width() / 10); + auto extraLength = (ballPos - field.leftGoalArea.rightLine().center()).stretchToLength(theirDistanceToGoal); std::vector positions = {}; Vector2 projectedPosition; // Find the intersection of the ball-to-goal line with the border of the defense area LineSegment ball2GoalLine = LineSegment(ballPos, field.leftGoalArea.rightLine().center()); - std::vector lineBorderIntersects = FieldComputations::getDefenseArea(field, true, spaceBetweenDefenseArea, 0).intersections(ball2GoalLine); - - // If there are intersections, sort them and use the first one. Otherwise, use a default position - if (!lineBorderIntersects.empty()) { - std::sort(lineBorderIntersects.begin(), lineBorderIntersects.end(), [](Vector2 a, Vector2 b) { return a.x > b.x; }); - projectedPosition = lineBorderIntersects.front(); - } else { - projectedPosition = Vector2{field.leftGoalArea.rightLine().center().x, field.leftDefenseArea.bottom()}; - } + std::vector lineBorderIntersects = FieldComputations::getDefenseArea(field, true, extraLength.length(), 0).intersections(ball2GoalLine); + // If the ball is in our defense area, project it outside, otherwise use the intersection with our defense area + std::sort(lineBorderIntersects.begin(), lineBorderIntersects.end(), [](Vector2 a, Vector2 b) { return a.x > b.x; }); + projectedPosition = lineBorderIntersects.front(); // Initialize the wallLine LineSegment wallLine; - // Define the defense areas - auto defenseAreaOur = FieldComputations::getDefenseArea(field, true, 0, 0); - auto defenseAreaTheir = FieldComputations::getDefenseArea(field, false, 0, 0); - - // Determine the wall line based on the ball's position - - if (ballPos.y < field.leftDefenseArea.top() && ballPos.y > field.leftDefenseArea.bottom()) { - // Case when it is to the right of our defense area - double lineX = field.leftDefenseArea.right() + spaceBetweenDefenseArea; - double lineYTop = field.playArea.top(); - double lineYBottom = field.playArea.bottom(); - wallLine = LineSegment({lineX, lineYBottom}, {lineX, lineYTop}); - } else if (ballPos.x < field.leftDefenseArea.right() + spaceBetweenDefenseArea) { - // Case when the projected position is below or above our defense area - if (ballPos.y < 0) { - wallLine = LineSegment(defenseAreaOur[0], defenseAreaTheir[0]); - wallLine.move({0, -spaceBetweenDefenseArea}); + // Define the top and bottom of our defense area and the closest and farthers point from the ball + auto topRightDefense = field.leftDefenseArea.topRight(); + auto bottomRightDefense = field.leftDefenseArea.bottomRight(); + auto closestPoint = ((ballPos - topRightDefense).length() < (ballPos - bottomRightDefense).length()) ? topRightDefense : bottomRightDefense; + auto farthestPoint = ((ballPos - topRightDefense).length() > (ballPos - bottomRightDefense).length()) ? topRightDefense : bottomRightDefense; + + // Determine the wall position based on the ball position + if (extraLength.length() == 0) { + // If we don't want to stand in front of the defense area, we want to tightly hug the defense area + if (projectedPosition.x < field.leftDefenseArea.right()) { + // If the ball is above or below our defense area + auto pointOnOurBackLine = FieldComputations::projectPointToValidPositionOnLine(field, projectedPosition, field.leftPlayArea.topLeft(), field.leftPlayArea.bottomLeft()); + auto pointOnTheirBackLine = + FieldComputations::projectPointToValidPositionOnLine(field, projectedPosition, field.rightPlayArea.topLeft(), field.rightPlayArea.bottomLeft()); + wallLine = LineSegment(pointOnOurBackLine, pointOnTheirBackLine); } else { - wallLine = LineSegment(defenseAreaOur[3], defenseAreaTheir[3]); - wallLine.move({0, spaceBetweenDefenseArea}); + // If the ball is in front of our defense area + auto pointOnTopSideLine = FieldComputations::projectPointToValidPositionOnLine(field, projectedPosition, field.leftPlayArea.topLeft(), field.leftPlayArea.topRight()); + auto pointOnBottomSideLine = + FieldComputations::projectPointToValidPositionOnLine(field, projectedPosition, field.leftPlayArea.bottomLeft(), field.leftPlayArea.bottomRight()); + wallLine = LineSegment(pointOnTopSideLine, pointOnBottomSideLine); } } else { + if ((projectedPosition - closestPoint).length() < amountDefenders / 2 * (spacingRobots + 2 * constants::ROBOT_RADIUS)) { + // If the position for the wall is too close to the defense area, we have to push it a bit forward to prevent robots from standing in the defense area + projectedPosition -= (field.leftGoalArea.rightLine().center() - ballPos).stretchToLength((projectedPosition - closestPoint).length() / 1.8); + } + // We put the wall line perpendicular to the ball-goal line wallLine = LineSegment(ballPos, field.leftGoalArea.rightLine().center()); wallLine.rotate(M_PI / 2, projectedPosition); // And resize it to make sure enough robots can fit on it - double newLength = 2 * std::max(field.playArea.width(), field.playArea.height()); + double newLength = 2 * field.playArea.width(); wallLine.resize(newLength); // Limit this resizing to the edges of the field (we don't want to place robots outside of the field) @@ -127,9 +168,10 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie wallLine.end = intersectionOther.value(); } } + if (wallLine.length() == 0) { RTT_WARNING("Wall line length is 0"); - double lineX = field.leftDefenseArea.right() + spaceBetweenDefenseArea; + double lineX = field.leftDefenseArea.right(); double lineYTop = field.playArea.top(); double lineYBottom = field.playArea.bottom(); wallLine = LineSegment({lineX, lineYBottom}, {lineX, lineYTop}); @@ -138,27 +180,60 @@ std::vector PositionComputations::determineWallPositions(const rtt::Fie size_t defendersCount = static_cast(amountDefenders); int i = 1; - // If the number of defenders is even, start just outside of the projected position to make sure the projected position is the middle of the wall - if (amountDefenders % 2 == 0) { - int j = 1; + if ((projectedPosition - wallLine.start).length() < constants::ROBOT_RADIUS * 2 + spacingRobots) { + // If the the middle of the wall is close to our goalline, assemble the wall from the goalline to ensure proper placement of robots + double distance = constants::ROBOT_RADIUS; while (positions.size() < defendersCount) { - double circleRadius = (i - 0.5) * spacingRobots + j * radius; - Circle circle = Circle(projectedPosition, circleRadius); - std::vector intersects = circle.intersects(wallLine); - positions.insert(positions.end(), intersects.begin(), intersects.end()); - j += 2; - i += 1; + auto positionWaller = wallLine.start - (wallLine.start - wallLine.end).stretchToLength(distance); + distance += (spacingRobots + 2 * constants::ROBOT_RADIUS); + positions.insert(positions.end(), positionWaller); + } + } else { + // If the number of defenders is even, start just outside of the projected position to make sure the projected position is the middle of the wall + if (amountDefenders % 2 == 0) { + int j = 1; + while (positions.size() < defendersCount) { + double circleRadius = (i - 0.5) * spacingRobots + j * constants::ROBOT_RADIUS; + Circle circle = Circle(projectedPosition, circleRadius); + std::vector intersects = circle.intersects(wallLine); + positions.insert(positions.end(), intersects.begin(), intersects.end()); + j += 2; + i += 1; + } + } + // If the number of defenders is odd, start positioning from the projected position + else { + positions.push_back(projectedPosition); + while (positions.size() < defendersCount) { + double circleRadius = i * 2 * constants::ROBOT_RADIUS + spacingRobots * i; + Circle circle = Circle(projectedPosition, circleRadius); + std::vector intersects = circle.intersects(wallLine); + positions.insert(positions.end(), intersects.begin(), intersects.end()); + i += 1; + } } } - // If the number of defenders is odd, start positioning from the projected position - else { - positions.push_back(projectedPosition); - while (positions.size() < defendersCount) { - double circleRadius = i * 2 * radius + spacingRobots * i; - Circle circle = Circle(projectedPosition, circleRadius); - std::vector intersects = circle.intersects(wallLine); - positions.insert(positions.end(), intersects.begin(), intersects.end()); - i += 1; + + if (extraLength.length() == 0) { + // If we do not want to stand in front of the defense area, we want to check whether all robots are as close to the defense area as they could + int k = 1; + int m = 0; + for (auto &position : positions) { + auto distance = (k - 0.5) * spacingRobots + 2 * k * constants::ROBOT_RADIUS; + if (position.x - constants::ROBOT_RADIUS > field.leftDefenseArea.right()) { + // If the robot is standing too far to the right + auto positionWaller = closestPoint - (closestPoint - farthestPoint).stretchToLength(distance); + positions[m] = positionWaller; + k++; + } else if (position.y + constants::ROBOT_RADIUS < field.leftDefenseArea.bottom() || position.y - constants::ROBOT_RADIUS > field.leftDefenseArea.top()) { + // If the robot is standing above or below our defense area + auto pointOnOurBackLine = + FieldComputations::projectPointToValidPositionOnLine(field, projectedPosition, field.leftPlayArea.topLeft(), field.leftPlayArea.bottomLeft()); + auto positionWaller = closestPoint - (closestPoint - pointOnOurBackLine).stretchToLength(distance); + positions[m] = positionWaller; + k++; + } + m++; } } @@ -269,8 +344,17 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapgetWorld()->getBall()->get()->position - targetPos).angle()); } else { auto harasser = std::find_if(roles->begin(), roles->end(), [](const std::unique_ptr &role) { return role != nullptr && role->getName() == "harasser"; }); - if (harasser != roles->end() && !harasser->get()->finished() && strcmp(harasser->get()->getCurrentTactic()->getName(), "Formation") == 0) - harasser->get()->forceNextTactic(); + if (harasser != roles->end() && !harasser->get()->finished() && strcmp(harasser->get()->getCurrentTactic()->getName(), "Formation") == 0) { + auto enemyPos = enemyClosestToBall->get()->getPos(); + auto targetPos = enemyPos + (world->getWorld()->getBall()->get()->position - enemyPos).stretchToLength(constants::ROBOT_RADIUS * 3); + // prevent the harasser from being stuck on the side of the enemy + if (enemyClosestToBall->get()->hasBall() && ((stpInfos["harasser"].getRobot()->get()->getPos() - targetPos).length() > constants::ROBOT_RADIUS)) { + stpInfos["harasser"].setPositionToMoveTo(targetPos); + stpInfos["harasser"].setYaw((world->getWorld()->getBall()->get()->position - targetPos).angle()); + } else { + harasser->get()->forceNextTactic(); + } + } } } @@ -281,9 +365,12 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma auto defenderNames = std::vector{}; auto wallerNames = std::vector{}; auto additionalWallerNames = std::vector{}; + PositionComputations::setAmountOfWallers(field, world); for (size_t i = 0; i < world->getWorld()->getUs().size(); i++) { - if (roles[i]->getName().find("waller") != std::string::npos) { + if (roles[i]->getName().find("waller") != std::string::npos && static_cast(wallerNames.size()) < PositionComputations::amountOfWallers) { wallerNames.emplace_back(roles[i]->getName()); + } else if (roles[i]->getName().find("waller") != std::string::npos) { + defenderNames.emplace_back(roles[i]->getName()); } else if (roles[i]->getName().find("defender") != std::string::npos) { defenderNames.emplace_back(roles[i]->getName()); } @@ -322,12 +409,12 @@ void PositionComputations::calculateInfoForDefendersAndWallers(std::unordered_ma if (mustStayOnOurSide && defendPostion.x > 0) { defendPostion.x = 0.0; } - stpInfos["defender_" + std::to_string(i)].setPositionToDefend(defendPostion); + stpInfos[defenderNames[i]].setPositionToDefend(defendPostion); ComputationManager::calculatedEnemyMapIds.emplace_back(enemyMap.begin()->second.id); enemyMap.erase(enemyMap.begin()); } for (size_t i = loopSize; i < defenderNames.size(); i++) { - additionalWallerNames.emplace_back("defender_" + std::to_string(i)); + additionalWallerNames.emplace_back(defenderNames[i]); } } else { // Calculate the distance between the defenders and their enemies @@ -472,11 +559,7 @@ void PositionComputations::calculateInfoForFormationOurSide(std::unordered_mapleftDefenseArea.contains(world->getWorld()->getBall()->get()->position) && - field->leftDefenseArea.contains(world->getWorld()->getBall()->get()->expectedEndPosition)) + double ballRadius = constants::BALL_RADIUS / 2; + auto ourDefenseArea = FieldComputations::getDefenseArea(*field, true, ballRadius, 0); + return (ourDefenseArea.contains(world->getWorld()->getBall()->get()->position) && ourDefenseArea.contains(world->getWorld()->getBall()->get()->expectedEndPosition)) ? constants::FUZZY_TRUE : constants::FUZZY_FALSE; } diff --git a/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp b/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp index 60290a531..a787dbc61 100644 --- a/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp +++ b/roboteam_ai/src/stp/evaluations/global/WeWillHaveBallGlobalEvaluation.cpp @@ -14,7 +14,9 @@ uint8_t WeWillHaveBallGlobalEvaluation::metricCheck(const world::World* world, c // If we have no bots, we will not get the ball if (us.empty()) return constants::FUZZY_FALSE; - if (world->getWorld()->getRobotClosestToBall(world::them)->get()->getPos().dist(ballPosition) < constants::ROBOT_RADIUS * 1.3 && world->getWorld()->getRobotClosestToBall(world::us)->get()->getPos().dist(ballPosition) < constants::ROBOT_RADIUS * 1.3) return false; + if (world->getWorld()->getRobotClosestToBall(world::them)->get()->getPos().dist(ballPosition) < constants::ROBOT_RADIUS * 1.3 && + world->getWorld()->getRobotClosestToBall(world::us)->get()->getPos().dist(ballPosition) < constants::ROBOT_RADIUS * 1.3) + return false; // If any of our robots has the ball, we will get the ball if (std::any_of(us.begin(), us.end(), [](auto& robot) { return robot->hasBall(); })) return constants::FUZZY_TRUE; diff --git a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp b/roboteam_ai/src/stp/plays/defensive/Defend.cpp similarity index 75% rename from roboteam_ai/src/stp/plays/defensive/DefendShot.cpp rename to roboteam_ai/src/stp/plays/defensive/Defend.cpp index dd208b683..b85f98be3 100644 --- a/roboteam_ai/src/stp/plays/defensive/DefendShot.cpp +++ b/roboteam_ai/src/stp/plays/defensive/Defend.cpp @@ -1,4 +1,4 @@ -#include "stp/plays/defensive/DefendShot.h" +#include "stp/plays/defensive/Defend.h" #include @@ -8,12 +8,11 @@ namespace rtt::ai::stp::play { -DefendShot::DefendShot() : Play() { +Defend::Defend() : Play() { // Evaluations that have to be true in order for this play to be considered valid. startPlayEvaluation.clear(); startPlayEvaluation.emplace_back(GlobalEvaluation::NormalPlayGameState); startPlayEvaluation.emplace_back(GlobalEvaluation::WeWillNotHaveBall); - startPlayEvaluation.emplace_back(GlobalEvaluation::BallOnOurSide); startPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); // Evaluations that have to be true to allow the play to continue, otherwise the play will change. Plays can also end using the shouldEndPlay(). @@ -27,35 +26,37 @@ DefendShot::DefendShot() : Play() { // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("defender_0"), std::make_unique("defender_1"), // Additional roles if we play 11v11 std::make_unique("defender_2"), - std::make_unique("waller_2"), + std::make_unique("waller_2"), std::make_unique("attacker_0"), - std::make_unique("waller_3"), + std::make_unique("waller_3"), std::make_unique("defender_3"), }; } -uint8_t DefendShot::score(const rtt::Field&) noexcept { +uint8_t Defend::score(const rtt::Field&) noexcept { // If this play is valid we always want to execute this play return constants::FUZZY_TRUE; } -Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { +Dealer::FlagMap Defend::decideRoleFlags() const noexcept { Dealer::FlagMap flagMap; Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.interceptId}}); - flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + for (int i = 0; i < Play::waller_count; i++) { + if (i <= PositionComputations::amountOfWallers) { + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::HIGH_PRIORITY, {}}}); + } else + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + } flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); @@ -65,13 +66,13 @@ Dealer::FlagMap DefendShot::decideRoleFlags() const noexcept { return flagMap; } -void DefendShot::calculateInfoForRoles() noexcept { +void Defend::calculateInfoForRoles() noexcept { harasserInfo = InterceptionComputations::calculateInterceptionInfoExcludingKeeperAndCarded(world); PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.interceptLocation); PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); } -const char* DefendShot::getName() const { return "Defend Shot"; } +const char* Defend::getName() const { return "Defend"; } } // namespace rtt::ai::stp::play diff --git a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp b/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp deleted file mode 100644 index 0693311b7..000000000 --- a/roboteam_ai/src/stp/plays/defensive/DefendPass.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "stp/plays/defensive/DefendPass.h" - -#include - -#include "roboteam_utils/Hungarian.h" -#include "stp/roles/Keeper.h" -#include "stp/roles/active/Harasser.h" -#include "stp/roles/passive/Defender.h" -#include "stp/roles/passive/Formation.h" - -namespace rtt::ai::stp::play { - -DefendPass::DefendPass() : Play() { - // Evaluations that have to be true in order for this play to be considered valid. - startPlayEvaluation.clear(); - startPlayEvaluation.emplace_back(GlobalEvaluation::NormalPlayGameState); - startPlayEvaluation.emplace_back(GlobalEvaluation::WeWillNotHaveBall); - startPlayEvaluation.emplace_back(GlobalEvaluation::BallOnTheirSide); - startPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); - - // Evaluations that have to be true to allow the play to continue, otherwise the play will change. Plays can also end using the shouldEndPlay(). - keepPlayEvaluation.clear(); - keepPlayEvaluation.emplace_back(GlobalEvaluation::NormalPlayGameState); - keepPlayEvaluation.emplace_back(GlobalEvaluation::TheyHaveBall); - keepPlayEvaluation.emplace_back(GlobalEvaluation::BallOnTheirSide); - 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::MAX_ROBOT_COUNT>{ - // Roles is we play 6v6 - std::make_unique("keeper"), - std::make_unique("harasser"), - std::make_unique("defender_0"), - std::make_unique("defender_1"), - std::make_unique("waller_0"), - std::make_unique("defender_2"), - // Additional roles if we play 11v11 - std::make_unique("defender_3"), - std::make_unique("attacker_0"), - std::make_unique("waller_1"), - std::make_unique("defender_4"), - std::make_unique("defender_5"), - }; -} - -uint8_t DefendPass::score(const rtt::Field&) noexcept { - // If this play is valid we always want to execute this play - return constants::FUZZY_TRUE; -} - -Dealer::FlagMap DefendPass::decideRoleFlags() const noexcept { - Dealer::FlagMap flagMap; - - Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); - - flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); - flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}, harasserInfo.interceptId}}); - flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_3", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_4", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_5", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"attacker_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - - return flagMap; -} - -void DefendPass::calculateInfoForRoles() noexcept { - harasserInfo = InterceptionComputations::calculateInterceptionInfoExcludingKeeperAndCarded(world); - PositionComputations::calculateInfoForHarasser(stpInfos, &roles, field, world, harasserInfo.interceptLocation); - PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, false); - PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); -} - -const char* DefendPass::getName() const { return "Defend Pass"; } - -} // namespace rtt::ai::stp::play \ No newline at end of file diff --git a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp index 7bac265ac..eea6744ed 100644 --- a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp +++ b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp @@ -41,7 +41,7 @@ KeeperKickBall::KeeperKickBall() : Play() { uint8_t KeeperKickBall::score(const rtt::Field& field) noexcept { // Calculate passInfo to be used during the play - passInfo = stp::computations::PassComputations::calculatePass(gen::SafePass, world, field, true); + passInfo = stp::computations::PassComputations::calculatePass(gen::ChippingPass, 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 constants::FUZZY_TRUE; @@ -75,7 +75,11 @@ void KeeperKickBall::calculateInfoForRoles() noexcept { if (!ballKicked()) { stpInfos["receiver"].setPositionToMoveTo(passInfo.receiverLocation); stpInfos["keeper"].setPositionToShootAt(passInfo.receiverLocation); - stpInfos["keeper"].setShotType(ShotType::PASS); + stpInfos["keeper"].setShotPower(ShotPower::PASS); + stpInfos["keeper"].setKickOrChip(KickType::CHIP); + if (stpInfos["keeper"].getRobot() && stpInfos["keeper"].getRobot()->get()->hasBall()) { + stpInfos["keeper"].setMaxRobotVelocity(0.2); + } } else { auto ball = world->getWorld()->getBall().value(); // Receiver goes to the receiverLocation projected on the trajectory of the ball @@ -97,8 +101,8 @@ bool KeeperKickBall::shouldEndPlay() noexcept { // If the keeper doesn't have the ball yet and there is a better pass available, we should stop the play if (stpInfos["keeper"].getRobot() && !stpInfos["keeper"].getRobot().value()->hasBall() && - stp::computations::PassComputations::calculatePass(gen::SafePass, world, field, true).passScore > - 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::SafePass, field, world).score) + stp::computations::PassComputations::calculatePass(gen::ChippingPass, world, field, true).passScore > + 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::ChippingPass, field, world).score) return true; return false; diff --git a/roboteam_ai/src/stp/plays/offensive/Attack.cpp b/roboteam_ai/src/stp/plays/offensive/Attack.cpp index e7a6dab3e..667578555 100644 --- a/roboteam_ai/src/stp/plays/offensive/Attack.cpp +++ b/roboteam_ai/src/stp/plays/offensive/Attack.cpp @@ -34,8 +34,8 @@ Attack::Attack() : Play() { std::make_unique("defender_1"), std::make_unique("defender_2"), // Additional roles if we play 11v11 - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("attacker_1"), std::make_unique("defender_3"), std::make_unique("attacker_2"), @@ -76,8 +76,8 @@ void Attack::calculateInfoForRoles() noexcept { auto goalTarget = computations::GoalComputations::calculateGoalTarget(world, field); goalTarget.y = std::clamp(goalTarget.y, field.rightGoalArea.bottom() + 0.2, field.rightGoalArea.top() - 0.2); stpInfos["striker"].setPositionToShootAt(goalTarget); - stpInfos["striker"].setKickOrChip(KickOrChip::KICK); - stpInfos["striker"].setShotType(ShotType::MAX); + stpInfos["striker"].setKickOrChip(KickType::KICK); + stpInfos["striker"].setShotPower(ShotPower::MAX); PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, goalTarget); } diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp index 7cec10373..417cf2cb7 100644 --- a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp +++ b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp @@ -33,14 +33,14 @@ AttackingPass::AttackingPass() : Play() { std::make_unique("keeper"), std::make_unique("passer"), std::make_unique("receiver"), - std::make_unique("defender_0"), - std::make_unique("defender_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("attacker_0"), // Additional roles if we play 11v11 - std::make_unique("waller_0"), - std::make_unique("waller_1"), - std::make_unique("defender_2"), - std::make_unique("defender_3"), + std::make_unique("waller_2"), + std::make_unique("waller_3"), + std::make_unique("defender_0"), + std::make_unique("defender_1"), std::make_unique("attacker_1"), }; } @@ -62,12 +62,14 @@ Dealer::FlagMap AttackingPass::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {}, passInfo.keeperId}}); flagMap.insert({"passer", {DealerFlagPriority::REQUIRED, {}, passInfo.passerId}}); flagMap.insert({"receiver", {DealerFlagPriority::REQUIRED, {}, passInfo.receiverId}}); - flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + for (int i = 0; i < Play::waller_count; i++) { + if (i <= PositionComputations::amountOfWallers) { + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::HIGH_PRIORITY, {}}}); + } else + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + } flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_3", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"attacker_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"attacker_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); @@ -81,8 +83,8 @@ void AttackingPass::calculateInfoForRoles() noexcept { if (!ballKicked()) { stpInfos["passer"].setPositionToShootAt(passInfo.receiverLocation); - stpInfos["passer"].setShotType(ShotType::PASS); - stpInfos["passer"].setKickOrChip(KickOrChip::KICK); + stpInfos["passer"].setShotPower(ShotPower::PASS); + stpInfos["passer"].setKickOrChip(KickType::KICK); stpInfos["receiver"].setPositionToMoveTo(passInfo.receiverLocation); } else { // Receiver goes to the receiverLocation projected on the trajectory of the ball diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp deleted file mode 100644 index 9a858bfde..000000000 --- a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "stp/plays/offensive/ChippingPass.h" - -#include -#include - -#include "stp/computations/PassComputations.h" -#include "stp/computations/PositionScoring.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 { -ChippingPass::ChippingPass() : Play() { - // Evaluations that have to be true in order for this play to be considered valid. - startPlayEvaluation.clear(); - startPlayEvaluation.emplace_back(GlobalEvaluation::NormalPlayGameState); - startPlayEvaluation.emplace_back(GlobalEvaluation::WeWillHaveBall); - startPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); - - // Evaluations that have to be true to allow the play to continue, otherwise the play will change. Plays can also end using the shouldEndPlay(). - keepPlayEvaluation.clear(); - keepPlayEvaluation.emplace_back(GlobalEvaluation::NormalPlayGameState); - keepPlayEvaluation.emplace_back(GlobalEvaluation::WeWillHaveBall); - keepPlayEvaluation.emplace_back(GlobalEvaluation::BallNotInOurDefenseAreaAndStill); - - // Role creation, the names should be unique. The names are used in the stpInfos-map. - roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ - // Roles is we play 6v6 - std::make_unique("keeper"), - std::make_unique("passer"), - std::make_unique("receiver"), - std::make_unique("defender_0"), - std::make_unique("defender_1"), - std::make_unique("attacker_0"), - // Additional roles if we play 11v11 - std::make_unique("waller_0"), - std::make_unique("waller_1"), - std::make_unique("attacker_1"), - std::make_unique("defender_2"), - std::make_unique("attacker_2"), - }; -} - -uint8_t ChippingPass::score(const rtt::Field&) noexcept { - // Robots can't chip yet - return 0; - // passInfo = stp::computations::PassComputations::calculatePass(gen::ChippingPass, world, field); - - // if (passInfo.receiverLocation == Vector2()) return 0; // In case no pass is found - - // return passInfo.passScore; -} - -Dealer::FlagMap ChippingPass::decideRoleFlags() const noexcept { - Dealer::FlagMap flagMap; - - flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {}, passInfo.keeperId}}); - flagMap.insert({"passer", {DealerFlagPriority::REQUIRED, {}, passInfo.passerId}}); - flagMap.insert({"receiver", {DealerFlagPriority::REQUIRED, {}, passInfo.receiverId}}); - flagMap.insert({"waller_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"attacker_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"attacker_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"attacker_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - - return flagMap; -} - -void ChippingPass::calculateInfoForRoles() noexcept { - PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); - PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world); - PositionComputations::recalculateInfoForNonPassers(stpInfos, field, world, passInfo.receiverLocation); - - if (!ballKicked()) { - stpInfos["receiver"].setPositionToMoveTo(passInfo.receiverLocation); - stpInfos["passer"].setPositionToShootAt(passInfo.receiverLocation); - stpInfos["passer"].setShotType(ShotType::PASS); - stpInfos["passer"].setKickOrChip(KickOrChip::KICK); - } else { - // Receiver goes to the receiverLocation projected on the trajectory of the ball - auto ball = world->getWorld()->getBall()->get(); - auto ballTrajectory = LineSegment(ball->position, ball->position + ball->velocity.stretchToLength(field.playArea.width())); - Vector2 receiverLocation = FieldComputations::projectPointToValidPositionOnLine(field, passInfo.receiverLocation, ballTrajectory.start, ballTrajectory.end); - stpInfos["receiver"].setPositionToMoveTo(receiverLocation); - - // Passer now goes to a front grid, where the receiver is not - if (receiverLocation.y > field.topRightGrid.getOffSetY()) { // Receiver is going to left of the field - stpInfos["passer"].setPositionToMoveTo(PositionComputations::getPosition(std::nullopt, field.bottomMidGrid, gen::SafePass, field, world)); - } else if (receiverLocation.y < field.middleMidGrid.getOffSetY()) { // Receiver is going to right of the field - stpInfos["passer"].setPositionToMoveTo(PositionComputations::getPosition(std::nullopt, field.topMidGrid, gen::SafePass, field, world)); - } else { // Receiver is going to middle of the field- passer will go to the closest grid on the side of the field - auto targetGrid = stpInfos["passer"].getRobot()->get()->getPos().y < 0 ? field.bottomMidGrid : field.topMidGrid; - stpInfos["passer"].setPositionToMoveTo(PositionComputations::getPosition(std::nullopt, targetGrid, gen::SafePass, field, world)); - } - } -} - -bool ChippingPass::ballKicked() { - return std::any_of(roles.begin(), roles.end(), [](const std::unique_ptr& role) { - return role != nullptr && role->getName() == "passer" && strcmp(role->getCurrentTactic()->getName(), "Formation") == 0; - }); -} - -bool ChippingPass::shouldEndPlay() noexcept { - // If the ball is kicked, we end the play to already prepare for what happens next - if (ballKicked()) return true; - - // If the passer doesn't have the ball yet and there is a better pass available, we should stop the play - if (stpInfos["passer"].getRobot() && !stpInfos["passer"].getRobot().value()->hasBall() && - stp::computations::PassComputations::calculatePass(gen::ChippingPass, world, field).passScore > - 1.05 * stp::PositionScoring::scorePosition(passInfo.receiverLocation, gen::ChippingPass, field, world).score) - return true; - - return false; -} - -const char* ChippingPass::getName() const { return "ChippingPass"; } -} // namespace rtt::ai::stp::play \ No newline at end of file diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp index 8d9ea7770..e481dd223 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementThem.cpp @@ -20,15 +20,15 @@ BallPlacementThem::BallPlacementThem() : Play() { // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("defender_0"), std::make_unique("defender_1"), // Additional roles if we play 11v11 std::make_unique("defender_2"), - std::make_unique("waller_2"), + std::make_unique("waller_2"), std::make_unique("attacker_0"), - std::make_unique("waller_3"), + std::make_unique("waller_3"), std::make_unique("defender_3"), }; } @@ -44,14 +44,16 @@ Dealer::FlagMap BallPlacementThem::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}}}); - flagMap.insert({"waller_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"waller_3", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"defender_3", {DealerFlagPriority::LOW_PRIORITY, {}}}); + for (int i = 0; i < Play::waller_count; i++) { + if (i <= PositionComputations::amountOfWallers) { + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::HIGH_PRIORITY, {}}}); + } else + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + } + flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"defender_3", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"attacker_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); return flagMap; diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp index 684c1fb47..873e4665e 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsForceStart.cpp @@ -24,12 +24,12 @@ BallPlacementUsForceStart::BallPlacementUsForceStart() : Play() { std::make_unique("ball_placer"), std::make_unique("defender_0"), std::make_unique("defender_1"), - std::make_unique("waller_0"), + std::make_unique("waller_0"), std::make_unique("defender_2"), // Additional roles if we play 11v11 std::make_unique("defender_3"), std::make_unique("attacker_0"), - std::make_unique("waller_1"), + std::make_unique("waller_1"), std::make_unique("defender_4"), std::make_unique("defender_5"), }; diff --git a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp index 05fb2c108..5355680ec 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/BallPlacementUsFreeKick.cpp @@ -27,8 +27,8 @@ BallPlacementUsFreeKick::BallPlacementUsFreeKick() : Play() { std::make_unique("defender_1"), std::make_unique("attacker_1"), // Additional roles if we play 11v11 - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("attacker_2"), std::make_unique("defender_2"), std::make_unique("attacker_3"), diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp index 5528376dc..e65dc0445 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickThem.cpp @@ -22,15 +22,15 @@ FreeKickThem::FreeKickThem() : Play() { // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("defender_0"), std::make_unique("defender_1"), // Additional roles if we play 11v11 std::make_unique("defender_2"), - std::make_unique("waller_2"), + std::make_unique("waller_2"), std::make_unique("attacker_0"), - std::make_unique("waller_3"), + std::make_unique("waller_3"), std::make_unique("defender_3"), }; } @@ -47,10 +47,12 @@ Dealer::FlagMap FreeKickThem::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}}}); - flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + for (int i = 0; i < Play::waller_count; i++) { + if (i <= PositionComputations::amountOfWallers) { + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::HIGH_PRIORITY, {}}}); + } else + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + } flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp index 84bc5e166..3f19db672 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsAtGoal.cpp @@ -25,12 +25,12 @@ FreeKickUsAtGoal::FreeKickUsAtGoal() : Play() { std::make_unique("free_kick_taker"), std::make_unique("defender_0"), std::make_unique("defender_1"), - std::make_unique("waller_0"), + std::make_unique("waller_0"), std::make_unique("defender_2"), // Additional roles if we play 11v11 std::make_unique("defender_3"), std::make_unique("attacker_0"), - std::make_unique("waller_1"), + std::make_unique("waller_1"), std::make_unique("defender_4"), std::make_unique("defender_5"), }; @@ -70,8 +70,8 @@ void FreeKickUsAtGoal::calculateInfoForRoles() noexcept { // FreeKickTaker auto goalTarget = computations::GoalComputations::calculateGoalTarget(world, field); stpInfos["free_kick_taker"].setPositionToShootAt(goalTarget); - stpInfos["free_kick_taker"].setKickOrChip(KickOrChip::KICK); - stpInfos["free_kick_taker"].setShotType(ShotType::MAX); + stpInfos["free_kick_taker"].setKickOrChip(KickType::KICK); + stpInfos["free_kick_taker"].setShotPower(ShotPower::MAX); } bool FreeKickUsAtGoal::shouldEndPlay() noexcept { diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp index 130f8132d..6223e5a57 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp @@ -30,8 +30,8 @@ FreeKickUsPass::FreeKickUsPass() : Play() { std::make_unique("defender_1"), std::make_unique("attacker_0"), // Additional roles if we play 11v11 - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("defender_2"), std::make_unique("defender_3"), std::make_unique("attacker_1"), @@ -72,11 +72,12 @@ void FreeKickUsPass::calculateInfoForRoles() noexcept { if (!ballKicked()) { stpInfos["receiver"].setPositionToMoveTo(passInfo.receiverLocation); stpInfos["free_kick_taker"].setPositionToShootAt(passInfo.receiverLocation); - stpInfos["free_kick_taker"].setShotType(ShotType::PASS); - stpInfos["free_kick_taker"].setKickOrChip(KickOrChip::KICK); + stpInfos["free_kick_taker"].setShotPower(ShotPower::PASS); + stpInfos["free_kick_taker"].setKickOrChip(KickType::KICK); if (RuntimeConfig::useReferee && GameStateManager::getCurrentGameState().timeLeft < 1.5) { stpInfos["free_kick_taker"].setPositionToShootAt(field.rightDefenseArea.rightLine().center()); - stpInfos["free_kick_taker"].setShotType(ShotType::MAX); + stpInfos["free_kick_taker"].setShotPower(ShotPower::MAX); + stpInfos["free_kick_taker"].setKickOrChip(KickType::CHIP); } } else { // Receiver goes to the receiverLocation projected on the trajectory of the ball diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp index 44252f50d..da2fc1904 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffThemPrepare.cpp @@ -18,17 +18,17 @@ KickOffThemPrepare::KickOffThemPrepare() : Play() { roles = std::array, rtt::ai::constants::MAX_ROBOT_COUNT>{ // Roles is we play 6v6 std::make_unique("keeper"), - std::make_unique("formation_back_0"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("formation_mid_0"), std::make_unique("formation_front_0"), - std::make_unique("formation_back_1"), - std::make_unique("formation_mid_1"), - // Additional roles if we play 11v11 std::make_unique("formation_front_1"), - std::make_unique("formation_back_2"), - std::make_unique("formation_mid_2"), std::make_unique("formation_front_2"), - std::make_unique("formation_mid_3"), + // Additional roles if we play 11v11 + std::make_unique("formation_front_3"), + std::make_unique("formation_front_4"), + std::make_unique("formation_front_5"), + std::make_unique("formation_front_6"), }; } @@ -42,23 +42,23 @@ Dealer::FlagMap KickOffThemPrepare::decideRoleFlags() const noexcept { Dealer::DealerFlag keeperFlag(DealerFlagTitle::KEEPER); flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); - flagMap.insert({"formation_back_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"formation_back_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"formation_back_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"waller_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"waller_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"formation_mid_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_mid_3", {DealerFlagPriority::LOW_PRIORITY, {}}}); - flagMap.insert({"formation_front_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"formation_front_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"formation_front_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"formation_front_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_4", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_5", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_6", {DealerFlagPriority::HIGH_PRIORITY, {}}}); return flagMap; } void KickOffThemPrepare::calculateInfoForRoles() noexcept { + PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); - stpInfos["formation_front_0"].setPositionToMoveTo(Vector2(-0.73, 0)); } const char* KickOffThemPrepare::getName() const { return "Kick Off Them Prepare"; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp index 8911beb94..e381fc80c 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUs.cpp @@ -2,7 +2,6 @@ #include "stp/roles/Keeper.h" #include "stp/roles/active/FreeKickTaker.h" -#include "stp/roles/active/PassReceiver.h" #include "stp/roles/passive/Halt.h" #include "utilities/GameStateManager.hpp" @@ -20,11 +19,19 @@ KickOffUs::KickOffUs() : Play() { // Role creation, the names should be unique. The names are used in the stpInfos-map. 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"), + std::make_unique("keeper"), + std::make_unique("kick_off_taker"), + std::make_unique("halt_0"), + std::make_unique("halt_1"), + std::make_unique("halt_2"), + std::make_unique("halt_3"), // Additional roles if we play 11v11 - std::make_unique("halt_3"), std::make_unique("halt_4"), std::make_unique("halt_5"), std::make_unique("halt_6"), - std::make_unique("halt_7")}; + std::make_unique("halt_4"), + std::make_unique("halt_5"), + std::make_unique("halt_6"), + std::make_unique("halt_7"), + std::make_unique("halt_8"), + }; } uint8_t KickOffUs::score(const rtt::Field &) noexcept { @@ -42,7 +49,6 @@ Dealer::FlagMap KickOffUs::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"kick_off_taker", {DealerFlagPriority::REQUIRED, {kickerFlag, detectionFlag}}}); - flagMap.insert({"receiver", {DealerFlagPriority::HIGH_PRIORITY, {detectionFlag}}}); flagMap.insert({"halt_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"halt_1", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"halt_2", {DealerFlagPriority::LOW_PRIORITY, {}}}); @@ -51,30 +57,21 @@ Dealer::FlagMap KickOffUs::decideRoleFlags() const noexcept { flagMap.insert({"halt_5", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"halt_6", {DealerFlagPriority::LOW_PRIORITY, {}}}); flagMap.insert({"halt_7", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"halt_8", {DealerFlagPriority::LOW_PRIORITY, {}}}); return flagMap; } void KickOffUs::calculateInfoForRoles() noexcept { - Vector2 receiverLocation = Vector2(-field.playArea.width() / 5, 0); - stpInfos["kick_off_taker"].setPositionToShootAt(receiverLocation); - stpInfos["kick_off_taker"].setShotType(ShotType::PASS); - stpInfos["kick_off_taker"].setKickOrChip(KickOrChip::KICK); - if (!ballKicked()) { - stpInfos["receiver"].setPositionToMoveTo(receiverLocation); - } else { - stpInfos["kick_off_taker"].setPositionToMoveTo(PositionComputations::getPosition(std::nullopt, field.middleRightGrid, gen::AttackingPass, field, world)); - auto ball = world->getWorld()->getBall()->get(); - auto ballTrajectory = LineSegment(ball->position, ball->position + ball->velocity.stretchToLength(field.playArea.width())); - stpInfos["receiver"].setPositionToMoveTo(FieldComputations::projectPointToValidPositionOnLine(field, receiverLocation, ballTrajectory.start, ballTrajectory.end)); - } + Vector2 theirGoal = Vector2(6, 0); + stpInfos["kick_off_taker"].setPositionToShootAt(theirGoal); + stpInfos["kick_off_taker"].setShotPower(ShotPower::PASS); + stpInfos["kick_off_taker"].setKickOrChip(KickType::CHIP); } bool KickOffUs::shouldEndPlay() noexcept { // If the ball is kicked, we end the play to already prepare for what happens next if (ballKicked()) return true; - if (stpInfos["receiver"].getRobot() && world->getWorld()->getBall()->get()->position.x < stpInfos["receiver"].getRobot()->get()->getPos().x) return true; - return false; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp index 7a2bb4771..472880c6c 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/KickOffUsPrepare.cpp @@ -19,16 +19,16 @@ KickOffUsPrepare::KickOffUsPrepare() : Play() { // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("kicker"), - std::make_unique("formation_mid_0"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("formation_front_0"), std::make_unique("formation_front_1"), std::make_unique("formation_front_2"), // Additional roles if we play 11v11 - std::make_unique("formation_back_0"), - std::make_unique("formation_back_1"), std::make_unique("formation_front_3"), std::make_unique("formation_front_4"), std::make_unique("formation_front_5"), + std::make_unique("formation_front_6"), }; } @@ -46,24 +46,25 @@ Dealer::FlagMap KickOffUsPrepare::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"kicker", {DealerFlagPriority::REQUIRED, {kickerFlag, detectionFlag}}}); - flagMap.insert({"formation_back_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"formation_back_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); - flagMap.insert({"formation_mid_0", {DealerFlagPriority::LOW_PRIORITY, {}}}); + flagMap.insert({"waller_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + flagMap.insert({"waller_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"formation_front_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_4", {DealerFlagPriority::HIGH_PRIORITY, {}}}); flagMap.insert({"formation_front_5", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + flagMap.insert({"formation_front_6", {DealerFlagPriority::LOW_PRIORITY, {}}}); return flagMap; } void KickOffUsPrepare::calculateInfoForRoles() noexcept { + PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world, true); PositionComputations::calculateInfoForFormationOurSide(stpInfos, roles, field, world); // The "kicker" will go to the ball - stpInfos["kicker"].setPositionToMoveTo(Vector2(-constants::AVOID_BALL_DISTANCE, 0.0)); + stpInfos["kicker"].setPositionToMoveTo(Vector2(-constants::AVOID_BALL_DISTANCE, -constants::AVOID_BALL_DISTANCE)); } const char* KickOffUsPrepare::getName() const { return "Kick Off Us Prepare"; } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp index c7be6ecd8..13a4c6848 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PenaltyUs.cpp @@ -62,10 +62,12 @@ void PenaltyUs::calculateInfoForRoles() noexcept { stpInfos["kicker"].setPositionToMoveTo(positionTarget); } else { stpInfos["kicker"].setPositionToMoveTo(stpInfos["kicker"].getRobot()->get()->getPos()); + // set kick + stpInfos["kicker"].setKickOrChip(KickType::KICK); } auto goalTarget = computations::GoalComputations::calculateGoalTarget(world, field); stpInfos["kicker"].setPositionToShootAt(goalTarget); - stpInfos["kicker"].setShotType(ShotType::MAX); + stpInfos["kicker"].setShotPower(ShotPower::MAX); if (stpInfos["kicker"].getRobot().has_value() && stpInfos["kicker"].getRobot()->get()->hasBall()) { stpInfos["kicker"].setMaxRobotVelocity((stpInfos["kicker"].getRobot()->get()->getPos() - positionTarget.position).length() * 4.8); } diff --git a/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp b/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp index 916356bac..9bdc5af4d 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/PrepareForcedStart.cpp @@ -22,15 +22,15 @@ PrepareForcedStart::PrepareForcedStart() : Play() { // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("defender_0"), std::make_unique("defender_1"), // Additional roles if we play 11v11 std::make_unique("defender_2"), - std::make_unique("waller_2"), + std::make_unique("waller_2"), std::make_unique("attacker_0"), - std::make_unique("waller_3"), + std::make_unique("waller_3"), std::make_unique("defender_3"), }; } @@ -47,10 +47,12 @@ Dealer::FlagMap PrepareForcedStart::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}}}); - flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + for (int i = 0; i < Play::waller_count; i++) { + if (i <= PositionComputations::amountOfWallers) { + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::HIGH_PRIORITY, {}}}); + } else + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + } flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); diff --git a/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp b/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp index f62eeb485..4ab7e5132 100644 --- a/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp +++ b/roboteam_ai/src/stp/plays/referee_specific/StopFormation.cpp @@ -21,15 +21,15 @@ StopFormation::StopFormation() : Play() { // Roles is we play 6v6 std::make_unique("keeper"), std::make_unique("harasser"), - std::make_unique("waller_0"), - std::make_unique("waller_1"), + std::make_unique("waller_0"), + std::make_unique("waller_1"), std::make_unique("defender_0"), std::make_unique("defender_1"), // Additional roles if we play 11v11 std::make_unique("defender_2"), - std::make_unique("waller_2"), + std::make_unique("waller_2"), std::make_unique("attacker_0"), - std::make_unique("waller_3"), + std::make_unique("waller_3"), std::make_unique("defender_3"), }; } @@ -45,10 +45,12 @@ Dealer::FlagMap StopFormation::decideRoleFlags() const noexcept { flagMap.insert({"keeper", {DealerFlagPriority::KEEPER, {keeperFlag}}}); flagMap.insert({"harasser", {DealerFlagPriority::REQUIRED, {}}}); - flagMap.insert({"waller_0", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_1", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_2", {DealerFlagPriority::HIGH_PRIORITY, {}}}); - flagMap.insert({"waller_3", {DealerFlagPriority::HIGH_PRIORITY, {}}}); + for (int i = 0; i < Play::waller_count; i++) { + if (i <= PositionComputations::amountOfWallers) { + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::HIGH_PRIORITY, {}}}); + } else + flagMap.insert({"waller_" + std::to_string(i), {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); + } flagMap.insert({"defender_0", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_1", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); flagMap.insert({"defender_2", {DealerFlagPriority::MEDIUM_PRIORITY, {}}}); diff --git a/roboteam_ai/src/stp/roles/active/Chipper.cpp b/roboteam_ai/src/stp/roles/active/Chipper.cpp deleted file mode 100644 index 7a49d9ffc..000000000 --- a/roboteam_ai/src/stp/roles/active/Chipper.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "stp/roles/active/Chipper.h" - -#include "stp/tactics/active/ChipAtPos.h" -#include "stp/tactics/active/GetBall.h" -#include "stp/tactics/passive/Formation.h" -namespace rtt::ai::stp::role { - -Chipper::Chipper(std::string name) : Role(std::move(name)) { - robotTactics = collections::state_machine{tactic::GetBall(), tactic::ChipAtPos(), tactic::Formation()}; -} -} // namespace rtt::ai::stp::role \ No newline at end of file diff --git a/roboteam_ai/src/stp/roles/active/KeeperPasser.cpp b/roboteam_ai/src/stp/roles/active/KeeperPasser.cpp index e4831b745..790281f70 100644 --- a/roboteam_ai/src/stp/roles/active/KeeperPasser.cpp +++ b/roboteam_ai/src/stp/roles/active/KeeperPasser.cpp @@ -1,6 +1,7 @@ #include "stp/roles/active/KeeperPasser.h" #include "stp/tactics/KeeperBlockBall.h" +#include "stp/tactics/active/DriveWithBall.h" #include "stp/tactics/active/GetBall.h" #include "stp/tactics/active/GetBehindBallInDirection.h" #include "stp/tactics/active/InstantKick.h" @@ -8,6 +9,31 @@ namespace rtt::ai::stp::role { KeeperPasser::KeeperPasser(std::string name) : Role(std::move(name)) { - robotTactics = collections::state_machine{tactic::GetBehindBallInDirection(), tactic::GetBall(), tactic::InstantKick(), tactic::KeeperBlockBall()}; + robotTactics = collections::state_machine{tactic::GetBehindBallInDirection(), tactic::GetBall(), tactic::DriveWithBall(), tactic::InstantKick(), + tactic::KeeperBlockBall()}; +} + +Status KeeperPasser::update(StpInfo const& info) noexcept { + if (!info.getBall() || !info.getRobot() || !info.getField()) { + RTT_WARNING("Required information missing in the tactic info for ", roleName) + return Status::Failure; + } + + StpInfo skillStpInfo = info; + auto shootArea = FieldComputations::getDefenseArea(*info.getField(), true, -0.4, 0.0); + // Stop Formation tactic when ball is moving, start blocking, getting the ball and pass (normal keeper behavior) + if (robotTactics.current_num() == 2 && shootArea.contains(info.getBall().value()->position)) { + forceNextTactic(); + } else if (robotTactics.current_num() == 2) { + auto intersections = shootArea.intersections(LineSegment(info.getField().value().leftGoalArea.leftLine().center(), info.getRobot().value()->getPos())); + + if (intersections.size() == 2) { + Vector2 intersection_one = intersections[0]; + + skillStpInfo.setPositionToMoveTo(intersection_one); + } + } + + return Role::update(skillStpInfo); } } // namespace rtt::ai::stp::role diff --git a/roboteam_ai/src/stp/skills/Chip.cpp b/roboteam_ai/src/stp/skills/Chip.cpp deleted file mode 100644 index 3972666fd..000000000 --- a/roboteam_ai/src/stp/skills/Chip.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include "stp/skills/Chip.h" - -#include "utilities/Constants.h" - -namespace rtt::ai::stp::skill { - -Status Chip::onUpdate(const StpInfo &info) noexcept { - float chipVelocity = std::clamp(info.getKickChipVelocity(), constants::MIN_CHIP_POWER, constants::MAX_CHIP_POWER); - command.kickType = KickType::CHIP; - command.kickSpeed = chipVelocity; - - command.dribblerOn = info.getDribblerOn(); - - command.yaw = info.getRobot().value()->getYaw(); - - command.waitForBall = true; - - command.id = info.getRobot().value()->getId(); - - forwardRobotCommand(); - - if (info.getBall()->get()->velocity.length() > constants::HAS_CHIPPED_ERROR_MARGIN) { - return Status::Success; - } - - return Status::Running; -} - -const char *Chip::getName() { return "Chip"; } - -} // namespace rtt::ai::stp::skill \ No newline at end of file diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp index fd396a330..d9a1411e8 100644 --- a/roboteam_ai/src/stp/skills/GoToPos.cpp +++ b/roboteam_ai/src/stp/skills/GoToPos.cpp @@ -14,30 +14,6 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept { auto ballLocation = info.getBall()->get()->position; RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); - if (currentGameState == RefCommand::BALL_PLACEMENT_THEM) { - auto ballPlacementPos = GameStateManager::getRefereeDesignatedPosition(); - auto robotToTarget = LineSegment(robot->getPos(), targetPos); - auto ballToReferee = LineSegment(ballLocation, ballPlacementPos); - if (robotToTarget.doesIntersect(ballToReferee)) { - double distance1 = (robot->getPos() - ballLocation).length() + (ballLocation - targetPos).length(); - 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, constants::OUT_OF_FIELD_MARGIN); - bool point2InField = field.playArea.contains(point2, constants::OUT_OF_FIELD_MARGIN); - if (point1InField && point2InField) { - targetPos = (distance1 < distance2) ? point1 : point2; - } else if (point1InField) { - targetPos = point1; - } else if (point2InField) { - targetPos = point2; - } else { - targetPos = FieldComputations::projectPointToValidPosition(field, targetPos, avoidObj); - } - targetPos = targetPos - (robot->getPos() - targetPos).stretchToLength(0.8); - } - } - if (!FieldComputations::pointIsValidPosition(field, targetPos, avoidObj) && roleName != "ball_placer") { targetPos = FieldComputations::projectPointToValidPosition(field, targetPos, avoidObj); } diff --git a/roboteam_ai/src/stp/skills/Kick.cpp b/roboteam_ai/src/stp/skills/Kick.cpp index aa3fb489b..42b42f84d 100644 --- a/roboteam_ai/src/stp/skills/Kick.cpp +++ b/roboteam_ai/src/stp/skills/Kick.cpp @@ -10,7 +10,7 @@ Status Kick::onUpdate(const StpInfo &info) noexcept { // Clamp and set kick velocity float kickVelocity = std::clamp(info.getKickChipVelocity(), constants::MIN_KICK_POWER, constants::MAX_KICK_POWER); - command.kickType = KickType::KICK; + command.kickType = info.getKickOrChip(); command.kickSpeed = kickVelocity; command.dribblerOn = info.getDribblerOn(); diff --git a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp index 20b5b32df..7e683b9dc 100644 --- a/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp +++ b/roboteam_ai/src/stp/tactics/KeeperBlockBall.cpp @@ -99,7 +99,7 @@ std::pair KeeperBlockBall::calculateTargetPosition(const StpInfo if (ball->position.x >= field.leftGoalArea.rightLine().center().x - MAX_DISTANCE_BALL_BEHIND_GOAL) { auto shouldAvoidGoalPosts = true; - std::optional predictedBallPositionTheirRobot = calculateTheirBallInterception(info, ballTrajectory); + std::optional predictedBallPositionTheirRobot = InterceptionComputations::calculateTheirBallInterception(info.getCurrentWorld(), ballTrajectory); return {calculateTargetPositionBallNotShot(info, predictedBallPositionTheirRobot), shouldAvoidGoalPosts}; } @@ -215,41 +215,6 @@ Vector2 KeeperBlockBall::calculateTargetPositionBallNotShot(const StpInfo &info, return targetPositionNew; } -std::optional KeeperBlockBall::calculateTheirBallInterception(const StpInfo &info, rtt::LineSegment ballTrajectory) noexcept { - std::optional predictedBallPositionTheirRobot = std::nullopt; - const auto ball = info.getBall().value(); - double minDistanceToBall = std::numeric_limits::max(); - for (const auto &theirRobot : info.getCurrentWorld()->getWorld()->getThem()) { - std::optional vecPts = ballTrajectory.project(theirRobot.get()->getPos()); - // see if the distance between the projected point and the robot is less than 0.5m - if (vecPts.value().dist(theirRobot.get()->getPos()) < 0.5) { - double dist = vecPts.value().dist(ball->position); - if (dist < minDistanceToBall) { - minDistanceToBall = dist; - predictedBallPositionTheirRobot = vecPts.value(); - predictedBallPositionTheirRobot = - predictedBallPositionTheirRobot.value() + (ball->position - predictedBallPositionTheirRobot.value()).normalize() * constants::CENTER_TO_FRONT; - } - } - } - if (predictedBallPositionTheirRobot) { - std::vector vec = {predictedBallPositionTheirRobot.value()}; - std::span spanVec = vec; - - rtt::ai::gui::Out::draw( - { - .label = "Interception Point", - .color = proto::Drawing::CYAN, - .method = proto::Drawing::CIRCLES, - .category = proto::Drawing::DEBUG, - .size = 15, - .thickness = 7, - }, - spanVec); - } - return predictedBallPositionTheirRobot; -} - Angle KeeperBlockBall::calculateYaw(const world::view::BallView &ball, const Vector2 &targetKeeperPosition) { // Look towards ball to ensure ball hits the front assembly to reduce odds of ball reflecting in goal const auto keeperToBall = (ball->position - targetKeeperPosition) / 1.6; diff --git a/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp b/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp deleted file mode 100644 index cfe07fba6..000000000 --- a/roboteam_ai/src/stp/tactics/active/ChipAtPos.cpp +++ /dev/null @@ -1,48 +0,0 @@ -#include "stp/tactics/active/ChipAtPos.h" - -#include "control/ControlUtils.h" -#include "stp/skills/Chip.h" -#include "stp/skills/OrbitAngular.h" -#include "utilities/Constants.h" - -namespace rtt::ai::stp::tactic { - -ChipAtPos::ChipAtPos() { skills = rtt::collections::state_machine{skill::OrbitAngular(), skill::Chip()}; } - -std::optional ChipAtPos::calculateInfoForSkill(const StpInfo &info) noexcept { - if (!info.getPositionToShootAt() || !info.getRobot() || !info.getBall()) return std::nullopt; - - StpInfo skillStpInfo = info; - auto angleToTarget = (info.getPositionToShootAt().value() - info.getRobot().value()->getPos()).angle(); - skillStpInfo.setYaw(angleToTarget); - - auto distanceBallToTarget = (info.getBall()->get()->position - info.getPositionToShootAt().value()).length(); - skillStpInfo.setKickChipVelocity(control::ControlUtils::determineChipForce(distanceBallToTarget)); - - if (skills.current_num() == 0) { - skillStpInfo.setDribblerOn(true); - } - - return skillStpInfo; -} - -bool ChipAtPos::isEndTactic() noexcept { return false; } - -bool ChipAtPos::isTacticFailing(const StpInfo &info) noexcept { - if (skills.current_num() != 1) { - return info.getRobot().value()->hasBall() && !info.getPositionToShootAt(); - } - return false; -} - -bool ChipAtPos::shouldTacticReset(const StpInfo &info) noexcept { - if (skills.current_num() != 0) { - auto errorMargin = constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI; - return info.getRobot().value()->getYaw().shortestAngleDiff(info.getYaw()) > errorMargin; - } - return false; -} - -const char *ChipAtPos::getName() { return "Chip At Pos"; } - -} // namespace rtt::ai::stp::tactic diff --git a/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp b/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp index 8ca7a2a03..d51ab3620 100644 --- a/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp +++ b/roboteam_ai/src/stp/tactics/active/DriveWithBall.cpp @@ -12,7 +12,9 @@ std::optional DriveWithBall::calculateInfoForSkill(const StpInfo &info) StpInfo skillStpInfo = info; auto angleToTarget = (info.getPositionToMoveTo().value() - info.getRobot()->get()->getPos()).angle(); - skillStpInfo.setYaw(skills.current_num() == 0 ? angleToTarget : (info.getPositionToMoveTo().value() - info.getBall()->get()->position).angle()); + if (info.getRoleName() != "keeper") { + skillStpInfo.setYaw(skills.current_num() == 0 ? angleToTarget : (info.getPositionToMoveTo().value() - info.getBall()->get()->position).angle()); + } skillStpInfo.setDribblerOn(true); return skillStpInfo; diff --git a/roboteam_ai/src/stp/tactics/active/InstantKick.cpp b/roboteam_ai/src/stp/tactics/active/InstantKick.cpp index 43af3182b..73fd3a68a 100644 --- a/roboteam_ai/src/stp/tactics/active/InstantKick.cpp +++ b/roboteam_ai/src/stp/tactics/active/InstantKick.cpp @@ -14,7 +14,7 @@ std::optional InstantKick::calculateInfoForSkill(const StpInfo &info) n StpInfo skillStpInfo = info; double distanceBallToTarget = (info.getBall()->get()->position - info.getPositionToShootAt().value()).length(); - skillStpInfo.setKickChipVelocity(control::ControlUtils::determineKickForce(distanceBallToTarget, skillStpInfo.getShotType())); + skillStpInfo.setKickChipVelocity(control::ControlUtils::determineKickForce(distanceBallToTarget, skillStpInfo.getShotPower())); skillStpInfo.setDribblerOn(false); return skillStpInfo; diff --git a/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp b/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp index 9cfad318b..68bc887f2 100644 --- a/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp +++ b/roboteam_ai/src/stp/tactics/active/OrbitKick.cpp @@ -16,7 +16,7 @@ std::optional OrbitKick::calculateInfoForSkill(const StpInfo &info) noe skillStpInfo.setYaw(angleToTarget); double distanceBallToTarget = (info.getBall()->get()->position - info.getPositionToShootAt().value()).length(); - skillStpInfo.setKickChipVelocity(control::ControlUtils::determineKickForce(distanceBallToTarget, skillStpInfo.getShotType())); + skillStpInfo.setKickChipVelocity(control::ControlUtils::determineKickForce(distanceBallToTarget, skillStpInfo.getShotPower())); skillStpInfo.setDribblerOn(true); diff --git a/roboteam_ai/src/utilities/GameStateManager.cpp b/roboteam_ai/src/utilities/GameStateManager.cpp index d4effd1a7..704f2358f 100644 --- a/roboteam_ai/src/utilities/GameStateManager.cpp +++ b/roboteam_ai/src/utilities/GameStateManager.cpp @@ -143,8 +143,7 @@ void GameStateManager::updateInterfaceGameState(const char* name) { {"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()}}, + {"Defend", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, {"Keeper Kick Ball", {RefCommand::NORMAL_START, RuleSet::RULESET_DEFAULT()}}, }; diff --git a/roboteam_ai/src/world/Ball.cpp b/roboteam_ai/src/world/Ball.cpp index 3d045ac06..672ce50fe 100644 --- a/roboteam_ai/src/world/Ball.cpp +++ b/roboteam_ai/src/world/Ball.cpp @@ -17,18 +17,6 @@ Ball::Ball(const proto::WorldBall& copy, const World* data) : position{copy.pos( } updateBallAtRobotPosition(data); updateExpectedBallEndPosition(); - - if (position != Vector2()) { - std::array point = {position}; - rtt::ai::gui::Out::draw( - { - .label = "position_ball_AI", - .color = proto::Drawing::CYAN, - .method = proto::Drawing::CIRCLES, - .size = 4, - }, - point); - } } void Ball::initBallAtExpectedPosition(const world::World* data) noexcept { diff --git a/roboteam_ai/src/world/FieldComputations.cpp b/roboteam_ai/src/world/FieldComputations.cpp index 2d424216a..199fc210e 100644 --- a/roboteam_ai/src/world/FieldComputations.cpp +++ b/roboteam_ai/src/world/FieldComputations.cpp @@ -27,7 +27,8 @@ bool FieldComputations::getBallAvoidance() { RuleSetName ruleSetTitle = GameStateManager::getCurrentGameState().getRuleSet().getTitle(); RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId(); - if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM) { + if (ruleSetTitle == RuleSetName::STOP || currentGameState == RefCommand::DIRECT_FREE_THEM || currentGameState == RefCommand::KICKOFF_THEM || + currentGameState == RefCommand::BALL_PLACEMENT_THEM || currentGameState == RefCommand::PREPARE_FORCED_START) { return true; } diff --git a/roboteam_ai/test/TestFixtures/TestFixture.h b/roboteam_ai/test/TestFixtures/TestFixture.h index 878efbd25..a287bd517 100644 --- a/roboteam_ai/test/TestFixtures/TestFixture.h +++ b/roboteam_ai/test/TestFixtures/TestFixture.h @@ -19,8 +19,7 @@ /** * Plays are included here */ -#include "stp/plays/defensive/DefendPass.h" -#include "stp/plays/defensive/DefendShot.h" +#include "stp/plays/defensive/Defend.h" #include "stp/plays/defensive/KeeperKickBall.h" #include "stp/plays/offensive/Attack.h" #include "stp/plays/offensive/AttackingPass.h" @@ -97,8 +96,7 @@ class RTT_AI_Tests : public ::testing::Test { plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); - plays.emplace_back(std::make_unique()); + plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); plays.emplace_back(std::make_unique()); diff --git a/roboteam_interface/src/app.vue b/roboteam_interface/src/app.vue index 60f1d4247..ccf26754c 100644 --- a/roboteam_interface/src/app.vue +++ b/roboteam_interface/src/app.vue @@ -7,7 +7,6 @@ import { useVisionDataStore } from './modules/stores/data-stores/vision-data-sto import SidebarResizer from './modules/components/layout/sidebar-resizer.vue' import ConnectModal from './modules/components/connect-modal.vue' import { useAiController } from './modules/composables/ai-controller' - import PixiApp from './modules/components/canvas/pixi-app.vue' import FieldLines from './modules/components/canvas/field-lines.vue' import Ball from './modules/components/canvas/ball.vue' @@ -68,6 +67,17 @@ const bottomPanelSize = computed(() => `${uiStore.panelSize('bottomPanel')}px`) + +
+
+ Battery Levels +
+
+
+ {{ key }} : {{ voltage ?? 'N/A' }} +
+
+
@@ -127,6 +137,14 @@ const bottomPanelSize = computed(() => `${uiStore.panelSize('bottomPanel')}px`) grid-area: bsb; } +.right-side { + position: absolute; + top: 0; + right: 0; + padding: 20px; + background-color: #303030; +} + .bottom-sidebar-resize { grid-area: bbr; } diff --git a/roboteam_interface/src/generated/proto.d.ts b/roboteam_interface/src/generated/proto.d.ts index bbbac239a..3af6b8675 100644 --- a/roboteam_interface/src/generated/proto.d.ts +++ b/roboteam_interface/src/generated/proto.d.ts @@ -281,7 +281,8 @@ export namespace proto { DOTS = 1, CROSSES = 2, PLUSES = 3, - CIRCLES = 4 + CIRCLES = 4, + TUBES = 5 } /** Color enum. */ diff --git a/roboteam_interface/src/generated/proto.js b/roboteam_interface/src/generated/proto.js index 7fa873fca..de687dc84 100644 --- a/roboteam_interface/src/generated/proto.js +++ b/roboteam_interface/src/generated/proto.js @@ -614,6 +614,7 @@ export const proto = $root.proto = (() => { case 2: case 3: case 4: + case 5: break; } if (message.points != null && message.hasOwnProperty("points")) { @@ -734,6 +735,10 @@ export const proto = $root.proto = (() => { case 4: message.method = 4; break; + case "TUBES": + case 5: + message.method = 5; + break; } if (object.points) { if (!Array.isArray(object.points)) @@ -862,6 +867,7 @@ export const proto = $root.proto = (() => { * @property {number} CROSSES=2 CROSSES value * @property {number} PLUSES=3 PLUSES value * @property {number} CIRCLES=4 CIRCLES value + * @property {number} TUBES=5 TUBES value */ Drawing.Method = (function() { const valuesById = {}, values = Object.create(valuesById); @@ -870,6 +876,7 @@ export const proto = $root.proto = (() => { values[valuesById[2] = "CROSSES"] = 2; values[valuesById[3] = "PLUSES"] = 3; values[valuesById[4] = "CIRCLES"] = 4; + values[valuesById[5] = "TUBES"] = 5; return values; })(); diff --git a/roboteam_interface/src/modules/components/canvas/field-objects.ts b/roboteam_interface/src/modules/components/canvas/field-objects.ts index 6b6415ba5..2771b9422 100644 --- a/roboteam_interface/src/modules/components/canvas/field-objects.ts +++ b/roboteam_interface/src/modules/components/canvas/field-objects.ts @@ -175,6 +175,12 @@ export class RobotDrawing extends Container { case 'receiver': outlineColor = '#ff00ff' break + case 'waller_0': + case 'waller_1': + case 'waller_2': + case 'waller_3': + outlineColor = '#FFC0CB' + break } if (outlineColor !== undefined) { this.robotOutline @@ -369,13 +375,30 @@ export class ShapeDrawing extends Container { graphicsPrototype.lineStyle(data.thickness, protoColorToHex(data.color!)) graphicsPrototype.drawCircle(0, 0, data.size) break + case proto.Drawing.Method.TUBES: + if (points.length >= 2) { + const [startPoint, endPoint] = points; + const angle = Math.atan2(endPoint.y - startPoint.y, endPoint.x - startPoint.x); + const radius = data.size; // 'size' is the radius of the tube ends + graphicsPrototype.lineStyle(data.thickness, protoColorToHex(data.color!), 1); + // Draw outward-oriented semi-circles at the ends of the tube + graphicsPrototype.arc(startPoint.x, startPoint.y, radius, angle - Math.PI / 2, angle + Math.PI / 2, true); + graphicsPrototype.arc(endPoint.x, endPoint.y, radius, angle + Math.PI / 2, angle - Math.PI / 2, true); + graphicsPrototype.closePath(); + } + break; + } + if (data.method !== proto.Drawing.Method.TUBES) { + points.forEach((point) => { + const graphics = graphicsPrototype.clone(); + graphics.x = point.x; + graphics.y = point.y; + this.addChild(graphics); + }); + } else { + const graphics = graphicsPrototype.clone(); + this.addChild(graphics); } - points.forEach((point) => { - const graphics = graphicsPrototype.clone() - graphics.x = point.x - graphics.y = point.y - this.addChild(graphics) - }) } } diff --git a/roboteam_interface/src/modules/components/canvas/utils.ts b/roboteam_interface/src/modules/components/canvas/utils.ts index d8cab0383..54cf89c10 100644 --- a/roboteam_interface/src/modules/components/canvas/utils.ts +++ b/roboteam_interface/src/modules/components/canvas/utils.ts @@ -18,28 +18,24 @@ const transformCoordinates = (point: IPoint) => { } export const zoom = (factor: number, x: number, y: number, stage: Container) => { - factor = factor > 0 ? 1.1 : 0.9 + factor = factor > 0 ? 1.1 : 0.9; const worldPos = { x: (x - stage.x) / stage.scale.x, - y: (y - stage.y) / stage.scale.y - } + y: (y - stage.y) / stage.scale.y, + }; - const newScale = { - x: stage.scale.x * factor, - y: stage.scale.y * factor - } + stage.scale.x *= factor; + stage.scale.y *= factor; const newScreenPos = { - x: worldPos.x * newScale.x + stage.x, - y: worldPos.y * newScale.y + stage.y - } - - stage.x -= newScreenPos.x - x - stage.y -= newScreenPos.y - y - stage.scale.x = newScale.x - stage.scale.y = newScale.y -} + x: worldPos.x * stage.scale.x + stage.x, + y: worldPos.y * stage.scale.y + stage.y, + }; + + stage.x -= newScreenPos.x - x; + stage.y -= newScreenPos.y - y; +}; export const useMoveCamera = ( canvasRef: Ref, @@ -49,7 +45,6 @@ export const useMoveCamera = ( let lastPos: null | { x: number; y: number } = null useEventListener(canvasRef, 'wheel', (e: WheelEvent) => { - if (!ctrlKey.value) return if (appRef.value?.stage === null) return zoom(e.deltaY, e.offsetX, e.offsetY, appRef.value!.stage!) }) 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 d9d7e056f..2de1575d4 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 @@ -65,9 +65,7 @@ const props = defineProps<{
  • Zoom in/out: -
    ctrl
    - + -
    wheel
    +
    scroll wheel
  • diff --git a/roboteam_interface/src/modules/stores/data-stores/vision-data-store.ts b/roboteam_interface/src/modules/stores/data-stores/vision-data-store.ts index 02fd2f7fc..659f6e142 100644 --- a/roboteam_interface/src/modules/stores/data-stores/vision-data-store.ts +++ b/roboteam_interface/src/modules/stores/data-stores/vision-data-store.ts @@ -10,10 +10,12 @@ export const useVisionDataStore = defineStore('visionDataStore', () => { const latestWorld = shallowRef(null) const latestField = shallowRef(null) + const knownBatteryLevels = shallowRef<{ [robotId: number]: number }>({}); const $reset = () => { latestWorld.value = null latestField.value = null + knownBatteryLevels.value = {} } const processVisionMsg = (msg: proto.IState) => { @@ -26,6 +28,15 @@ export const useVisionDataStore = defineStore('visionDataStore', () => { if (JSON.stringify(msg.field?.field) !== JSON.stringify(latestField.value)) { latestField.value = msg.field?.field ?? null } + + if (ourRobots && ourRobots.value) { + ourRobots.value + .forEach((robot) => { + if (robot.feedbackInfo?.batteryLevel && robot.id !== undefined && robot.id !== null) { + knownBatteryLevels.value[robot.id] = robot.feedbackInfo?.batteryLevel; + } + }); + } } const ourRobots = computed(() => { @@ -39,6 +50,7 @@ export const useVisionDataStore = defineStore('visionDataStore', () => { latestField: readonly(latestField), $reset, processVisionMsg, - ourRobots + ourRobots, + knownBatteryLevels: readonly(knownBatteryLevels), } }) diff --git a/roboteam_interface/src/utils.ts b/roboteam_interface/src/utils.ts index d0cb1ebfa..779bc7874 100644 --- a/roboteam_interface/src/utils.ts +++ b/roboteam_interface/src/utils.ts @@ -21,21 +21,22 @@ export const OUT_OF_CANVAS_COORDINATES = { export const robotNameMap = (team: 'BLACK' | 'PURPLE', id: number) => { if (team === 'PURPLE') { return { + 0: 'Vissues', 1: 'Wall-E', - 2: 'R2D2', - 3: 'Tron', - 4: 'Marvin', - 5: 'Jarvis', - 6: 'Baymax', - 7: 'Noo-Noo', - 8: 'T-800', - 9: 'K-9', - 10: 'Bender', - 11: 'Holt', - 12: 'Chappie', - 13: 'TARS', - 14: '', - 15: 'Herman' + 2: 'Bob', + 3: 'Pumba', + 4: 'Ted', + 5: 'Eve', + 6: 'Susie', + 7: 'James', + 8: 'Lizzy', + 9: 'McQueen', + 10: 'Kevin', + 11: 'Brum', + 12: 'Van Robogh', + 13: 'Wout', + 14: 'Jenny', + 15: 'Hermann' }[id] } diff --git a/roboteam_networking/proto/GUI.proto b/roboteam_networking/proto/GUI.proto index 0c940f56a..63f39bfcd 100644 --- a/roboteam_networking/proto/GUI.proto +++ b/roboteam_networking/proto/GUI.proto @@ -13,6 +13,7 @@ message Drawing { CROSSES = 2; PLUSES = 3; CIRCLES = 4; + TUBES = 5; } enum Color { diff --git a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp index 585a78e5f..8bf3a9fa7 100644 --- a/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp +++ b/roboteam_observer/observer/src/filters/vision/ball/GroundBallExtendedKalmanFilter.cpp @@ -6,8 +6,8 @@ void GroundBallExtendedKalmanFilter::update(const Eigen::Vector2d &observation) y = observation - (H * X); // Variance of innovation Eigen::Matrix2d S = H * P * H.transpose() + R; - // compute kalman gain. For small matrices, Eigen's inverse function is efficient - Eigen::Matrix K = P * H.transpose() * S.inverse(); + // compute kalman gain. For small matrices, Eigen's inverse function is efficient, but this is more numerically stable + Eigen::Matrix K = P * H.transpose() * S.llt().solve(Eigen::Matrix2d::Identity()); // update state with prediction X = X + K * y; // update covariance @@ -127,37 +127,36 @@ void GroundBallExtendedKalmanFilter::predict(Time timeStamp) { dt = -vel / acceleration; } - // update the transition matrix - double velCubed = vel * vel * vel; - double vysq = currentVel.y() * currentVel.y() / velCubed; - double vxvy = -currentVel.x() * currentVel.y() / velCubed; - double vxsq = currentVel.x() * currentVel.x() / velCubed; - - // To prevent division by zero, we simply put the velocity of the ball to zero if the ball is almost lying still + // Prevent division by zero by checking if velocity is very low if (vel <= BALL_STILL_VELOCITY) { - vxsq = 0.0; - vxvy = 0.0; - vysq = 0.0; + F.setIdentity(); // Use identity matrix when ball is nearly or completely still + } else { + double velCubed = vel * vel * vel; + double vysq = currentVel.y() * currentVel.y() / velCubed; + double vxvy = -currentVel.x() * currentVel.y() / velCubed; + double vxsq = currentVel.x() * currentVel.x() / velCubed; + + F(0, 2) = dt + 0.5 * vysq * acceleration * dt * dt; + F(0, 3) = 0.5 * vxvy * acceleration * dt * dt; + F(1, 2) = 0.5 * vxvy * acceleration * dt * dt; + F(1, 3) = dt + 0.5 * vxsq * acceleration * dt * dt; + F(2, 2) = 1 + vysq * acceleration * dt; + F(2, 3) = vxvy * acceleration * dt; + F(3, 2) = vxvy * acceleration * dt; + F(3, 3) = 1 + vxsq * acceleration * dt; } - F(0, 2) = dt + 0.5 * vysq * acceleration * dt * dt; - F(0, 3) = 0.5 * vxvy * acceleration * dt * dt; - F(1, 2) = 0.5 * vxvy * acceleration * dt * dt; - F(1, 3) = dt + 0.5 * vxsq * acceleration * dt * dt; - F(2, 2) = 1 + vysq * acceleration * dt; - F(2, 3) = vxvy * acceleration * dt; - F(3, 2) = vxvy * acceleration * dt; - F(3, 3) = 1 + vxsq * acceleration * dt; - - // setting the process noise matrix (Q) + // Update the process noise matrix Q setProccessNoise(frame_dt); - // now update the state given the transition function + // Update the state X based on the transition matrix F if (vel > BALL_STILL_VELOCITY) { X.head<2>() = currentPos + currentVel * dt + 0.5 * currentVel / vel * acceleration * dt * dt; X.tail<2>() = currentVel + currentVel / vel * acceleration * dt; - } else { // Set the velocity of the ball to zero - X.tail<2>() = Eigen::Vector2d::Zero(); + } else { + X.tail<2>() = Eigen::Vector2d::Zero(); // Set velocity to zero if ball is nearly still } + + // Update the covariance matrix P P = F * P * F.transpose() + Q; } \ No newline at end of file diff --git a/roboteam_robothub/roboteam_embedded_messages b/roboteam_robothub/roboteam_embedded_messages index 169668f96..9cb3c212a 160000 --- a/roboteam_robothub/roboteam_embedded_messages +++ b/roboteam_robothub/roboteam_embedded_messages @@ -1 +1 @@ -Subproject commit 169668f96aa6b3eef4c4409bc947110956d7ae0a +Subproject commit 9cb3c212ae7a06dc7e8744eee3d2b0ad7ea986ae