Merge pull request #121 from RoboTeamTwente/Update/pass-computations

Update/pass computations

roboteam_ai/include/roboteam_ai/stp/PlayDecider.hpp

@@ -40,6 +40,11 @@ class PlayDecider {
      */
     static void lockPlay(const std::optional<std::string> playName);
 
+    /**
+     * @brief Unlocks the play
+     */
+    static void unlockPlay();
+
     /**
      * @brief This function checks if there is a locked play. If there is, pick that play.
      * If there isn't, pick the play with the highest score (either a locked play through the interface or just the highest scored play)

roboteam_ai/include/roboteam_ai/stp/computations/PassComputations.h

@@ -58,12 +58,15 @@ class PassComputations {
      * @param point the point to check for validity
      * @param ballLocation the current ball location
      * @param possibleReceiverLocations the locations of all robots that could receive the ball
+     * @param possibleReceiverVelocities the velocities of all robots that could receive the ball
      * @param passerLocation the location of the robot that will pass the ball
+     * @param passerVelocity the velocity of the robot that will pass the ball
      * @param field the current field
      * @param world the current world
      * @return bool indicating whether this point is (likely) possible to pass to
      */
-    static bool pointIsValidPassLocation(Vector2 point, Vector2 ballLocation, const std::vector<Vector2>& possibleReceiverLocations, Vector2 passerLocation, const Field& field,
+    static bool pointIsValidPassLocation(Vector2 point, Vector2 ballLocation, const std::vector<Vector2>& possibleReceiverLocations,
+                                         const std::vector<Vector2>& possibleReceiverVelocities, Vector2 passerLocation, Vector2 passerVelocity, const Field& field,
                                          const world::World* world);
 
     /**
@@ -87,19 +90,21 @@ class PassComputations {
     /**
      * @brief Approximate the time it takes a robot to reach a point
      * @param robotPosition current position of robot
+     * @param robotVelocity current velocity of robot
      * @param targetPosition position to calculate travel time to
      * @return approximated time to reach target from position
      */
-    static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 targetPosition);
+    static double calculateRobotTravelTime(Vector2 robotPosition, Vector2 robotVelocity, Vector2 targetPosition);
 
     /**
      * Approximate the time it takes the ball to reach a point
      * @param ballLocation current location of the ball
      * @param passerLocation current location of the passer
+     * @param robotVelocity current velocity of robot
      * @param targetPosition position to calculate travel time to
      * @return approximated time for ball to reach target position
      */
-    static double calculateBallTravelTime(Vector2 ballLocation, Vector2 passerLocation, Vector2 targetPosition);
+    static double calculateBallTravelTime(Vector2 ballLocation, Vector2 passerLocation, Vector2 passerVelocity, Vector2 targetPosition);
 };
 }  // namespace rtt::ai::stp::computations
 #endif  // RTT_PASSCOMPUTATIONS_H

roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h

@@ -31,9 +31,9 @@ struct EnemyInfo {
     int id;
 };
 
-struct HarasserInfo {
-    int harasserId;
-    double timeToBall;
+struct InterceptInfo {
+    Vector2 interceptLocation;
+    int interceptId = -1;
 };
 
 /**
@@ -94,31 +94,31 @@ class PositionComputations {
     static Vector2 calculateAvoidBallPosition(Vector2 targetPosition, Vector2 ballPosition, const Field &field);
 
     /**
-     * @brief Calculates info for the keeper
-     * @param stpInfos The current stpInfos
-     * @param field The current field
+     * @brief Calculates the id of the harasser
      * @param world The current world
+     * @param field The current field
+     * @return HarasserInfo with the id and the time to the ball
      */
-    static void calculateInfoForKeeper(std::unordered_map<std::string, StpInfo> &stpInfos, const Field &field, world::World *world) noexcept;
+    static InterceptInfo calculateHarasserId(rtt::world::World *world, const Field &field) noexcept;
 
     /**
-     * @brief Calculates the id of the harasser
-     * @param world The current world
+     * @brief Calculates the position for an intercept
      * @param field The current field
-     * @return HarasserInfo with the id and the time to the ball
+     * @param world The current world
+     * @param interceptId The Id of the robot which will intercept
      */
-    static HarasserInfo calculateHarasserId(rtt::world::World *world, const Field &field) noexcept;
+    static InterceptInfo calculateInterceptionInfo(const Field &field, rtt::world::World *world, int interceptId) noexcept;
 
     /**
      * @brief Calculates info for the harasser role
      * @param stpInfos The current stpInfos
      * @param roles The current roles
      * @param field The current field
      * @param world The current world
-     * @param timeToBall The time to the ball
+     * @param interceptionLocation The location where the harasser should go to
      */
     static void calculateInfoForHarasser(std::unordered_map<std::string, StpInfo> &stpInfos, std::array<std::unique_ptr<Role>, stp::control_constants::MAX_ROBOT_COUNT> *roles,
-                                         const Field &field, world::World *world, double timeToBall) noexcept;
+                                         const Field &field, world::World *world, Vector2 interceptionLocation) noexcept;
 
     /**
      * @brief Calculates info for the defenders

roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h

@@ -52,8 +52,8 @@ constexpr double GO_TO_POS_ERROR_MARGIN = 0.01; /**< Distance error for a robot
 // Angle margin for 'goToPos'. If the robot is within this margin, goToPos is successful
 constexpr double GO_TO_POS_ANGLE_ERROR_MARGIN = 0.04; /**< Angle error for a robot to be considered to have reached a position */
 // Maximum inaccuracy during ballplacement
-constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS; /**< Distance error for the ball to be considered to have reached the ball placement position*/
-constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9;    /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */
+constexpr double BALL_PLACEMENT_MARGIN = 0.15 - BALL_RADIUS - 0.02; /**< Distance error for the ball to be considered to have reached the ball placement position*/
+constexpr double ENEMY_ALREADY_ASSIGNED_MULTIPLIER = 0.9;           /**< Multiplication factor for the distance to goal used by the dealer when the enemy is already assigned */
 
 /// Invariant constants
 constexpr uint8_t FUZZY_TRUE = 255;          /**< Value at which the fuzzy logic is considered 100% true */

roboteam_ai/include/roboteam_ai/stp/constants/GeneralizationConstants.h

@@ -56,7 +56,7 @@ struct ScoredPosition {
  * They consist of a generalized weight combination.
  */
 
-constexpr ScoreProfile AttackingPass = {0.5, 1, 1}; /**< Scoring weights for Attacking Pass */
+constexpr ScoreProfile AttackingPass = {0.5, 1, 2}; /**< Scoring weights for Attacking Pass */
 constexpr ScoreProfile SafePass = {1, 1, 0};        /**< Scoring weights for Safe Pass, used by the keeper */
 constexpr ScoreProfile LineOfSight = {0, 1, 0};     /**< Scoring weights for Line of Sight score, only used for testing minimum line of sight */
 constexpr ScoreProfile GoalShot = {0, 0, 1};        /**< Scoring weights for Goal Shot Score, a position where we can shoot at goal */

roboteam_ai/include/roboteam_ai/stp/evaluations/position/GoalShotEvaluation.h

@@ -16,10 +16,11 @@ class GoalShotEvaluation {
     /**
      * @brief Score ability to make a goal from position based on how much of the goal is visible and the angular size of the goal
      * @param goalVisibility % visibility of the goal (taking in account other robots)
-     * @param goalAngle angular size of the goal (relative to the goal angle from the penalty point)
+     * @param goalAngle angular size of the goal (relative to the goal angle from the best position)
+     * @param normalizedDistanceToGoal normalized distance to the goal (0 = closest, 1 = furthest)
      * @return uint8_t score
      */
-    [[nodiscard]] static uint8_t metricCheck(double goalVisibility, double goalAngle) noexcept;
+    [[nodiscard]] static uint8_t metricCheck(double goalVisibility, double goalAngle, double normalizedDistanceToGoal) noexcept;
 };
 }  // namespace rtt::ai::stp::evaluation
 #endif  // RTT_GOALSHOTEVALUATION_H

roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendPass.h

@@ -45,7 +45,7 @@ class DefendPass : public Play {
      */
     const char* getName() const override;
 
-    HarasserInfo harasserInfo;
+    InterceptInfo harasserInfo;
 };
 }  // namespace rtt::ai::stp::play
 

roboteam_ai/include/roboteam_ai/stp/plays/defensive/DefendShot.h

@@ -44,7 +44,7 @@ class DefendShot : public Play {
      */
     const char* getName() const override;
 
-    HarasserInfo harasserInfo;
+    InterceptInfo harasserInfo;
 };
 }  // namespace rtt::ai::stp::play
 

roboteam_ai/include/roboteam_ai/utilities/Dealer.h

@@ -142,11 +142,12 @@ class Dealer {
 
     /**
      * @brief Calculates the cost of travelling a certain distance
-     * @param distance  The distance to travel
-     * @param fieldHeight The height of the field
+     * @param robot the robot for which the cost will be determined
+     * @param target_position the position the robot will be travelling to
+     * @param MaxRobotVelocity the maximum velocity a robot can have
      * @return Cost of travelling that distance
      */
-    static double costForDistance(double distance, double fieldHeight);
+    static double costForDistance(const v::RobotView &robot, const rtt::Vector2 target_position, const double MaxRobotVelocity);
 
     /**
      * @brief Calculates the cost of a property of a robot

roboteam_ai/include/roboteam_ai/utilities/RefCommand.h

@@ -39,6 +39,7 @@ enum class RefCommand {
     BALL_PLACEMENT_US_DIRECT = 22,  // Ball placement before a direct free kick
     DIRECT_FREE_US_STOP = 23,       // Direct free kick us directly after a stop
     DIRECT_FREE_THEM_STOP = 24,     // Direct free kick them directly after a stop
+    PREPARE_FORCED_START = 25,      // The state before a forced start, after ball placement them
 
     UNDEFINED = -1
 };
@@ -115,6 +116,9 @@ inline std::ostream &operator<<(std::ostream &os, const RefCommand &command) {
         case RefCommand::PENALTY_THEM:
             os << "PENALTY_THEM";
             break;
+        case RefCommand::PREPARE_FORCED_START:
+            os << "PREPARE_FORCED_START";
+            break;
         case RefCommand::UNDEFINED:
             os << "UNDEFINED";
             break;

roboteam_ai/include/roboteam_ai/utilities/StrategyManager.h

@@ -73,6 +73,7 @@ class StrategyManager {
         GameState(RefCommand::PREPARE_KICKOFF_THEM, Constants::RULESET_STOP(), false, RefCommand::KICKOFF_THEM),
         GameState(RefCommand::PREPARE_PENALTY_US, Constants::RULESET_STOP(), false, RefCommand::PENALTY_US),
         GameState(RefCommand::PREPARE_PENALTY_THEM, Constants::RULESET_STOP(), false, RefCommand::PENALTY_THEM),
+        GameState(RefCommand::PREPARE_FORCED_START, Constants::RULESET_STOP()),
 
         GameState(RefCommand::DIRECT_FREE_THEM, Constants::RULESET_DEFAULT()),
         GameState(RefCommand::NORMAL_START, Constants::RULESET_DEFAULT()),

roboteam_ai/src/control/positionControl/BBTrajectories/WorldObjects.cpp

@@ -27,7 +27,8 @@ std::optional<CollisionData> WorldObjects::getFirstCollision(const rtt::world::W
     if (avoidObjects.shouldAvoidDefenseArea) {
         calculateDefenseAreaCollisions(field, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]);
     }
-    if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM) {
+    if (rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM ||
+        rtt::ai::GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) {
         calculateBallPlacementCollision(world, collisionDatas, pathPoints, timeStep, completedTimeSteps[robotId]);
     }
     if (avoidObjects.shouldAvoidBall) {

roboteam_ai/src/gui/networking/InterfaceSubscriber.cpp

@@ -30,6 +30,9 @@ void InterfaceSubscriber::onMessage(const proto::MsgFromInterface&& message) {
         case proto::MsgFromInterface::kSetRuntimeConfig: {
             RuntimeConfig::useReferee = message.set_runtime_config().use_referee();
             RuntimeConfig::ignoreInvariants = message.set_runtime_config().ignore_invariants();
+            if (RuntimeConfig::useReferee) {
+                stp::PlayDecider::unlockPlay();
+            }
         } break;
 
         case proto::MsgFromInterface::kSetGameSettings: {