Add Pass Feature Collection class

src/proto/message_translation/tbots_protobuf.cpp

@@ -544,3 +544,19 @@ std::unique_ptr<TbotsProto::Shape> createShapeProto(const Stadium& stadium)
     (*shape_msg->mutable_stadium()) = *createStadiumProto(stadium);
     return shape_msg;
 }
+
+std::unique_ptr<TbotsProto::PassFeatures> createPassFeaturesProto(const Pass& pass,
+                                                                  const World& world,
+                                                                  double score)
+{
+    auto pass_features_msg = std::make_unique<TbotsProto::PassFeatures>();
+    (*pass_features_msg->mutable_passer_point()) = *createPointProto(pass.passerPoint());
+    (*pass_features_msg->mutable_receiver_point()) =
+        *createPointProto(pass.receiverPoint());
+
+    (*pass_features_msg->mutable_world_state()) = *createWorld(world);
+
+    pass_features_msg->set_score(score);
+
+    return pass_features_msg;
+}

src/proto/message_translation/tbots_protobuf.h

@@ -213,6 +213,19 @@ BallState createBallState(const TbotsProto::BallState ball_state);
 std::unique_ptr<TbotsProto::PassVisualization> createPassVisualization(
     const std::vector<PassWithRating>& passes_with_rating);
 
+/**
+ * Returns a pass features message with the given pass, game state, and score
+ *
+ * @param pass the pass to get features from
+ * @param world the current world state
+ * @param score the score for the pass in the current world
+ *
+ * @return The unique_ptr to a PassFeatures proto
+ */
+std::unique_ptr<TbotsProto::PassFeatures> createPassFeaturesProto(const Pass& pass,
+                                                                  const World& world,
+                                                                  double score);
+
 /**
  * Returns the WorldStateReceivedTrigger given the world state received trigger
  *

src/proto/visualization.proto

@@ -97,3 +97,14 @@ message DebugShapes
     // Unique ID to a named shape
     repeated DebugShape debug_shapes = 1;
 }
+
+message PassFeatures
+{
+    Point passer_point        = 1;
+    Point receiver_point      = 2;
+    double pass_speed_m_per_s = 3;
+
+    World world_state = 4;
+
+    double score = 5;
+}

src/shared/constants.h

@@ -2,7 +2,7 @@
 #include <math.h>
 
 // Some platformio targets don't support STL, so we can't
-// use unordered_map, string, .... We guard all networking stuff with
+// use unordered_map, string, .... We guard all constants that need these types with
 #ifndef PLATFORMIO_BUILD
 #include <string>
 #include <unordered_map>
@@ -32,6 +32,10 @@ static const std::string REPLAY_METADATA_DELIMITER  = ",";
 static const std::string REPLAY_FILE_VERSION_PREFIX = "version:";
 static const unsigned int REPLAY_FILE_VERSION       = 2;
 
+static const std::string PASS_FEATURE_DELIMITER = ",";
+static const std::string PASS_FEATURE_DIR       = "/tmp/tbots/ml";
+static const std::string PASS_FEATURE_FILE      = "pass_features.csv";
+
 #endif  // PLATFORMIO_BUILD
 
 // TOML config file path for robot configuration
@@ -228,3 +232,9 @@ static const unsigned NUM_GENEVA_ANGLES = 5;
 constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M     = 1.5;
 constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S  = 1.5;
 constexpr double WHEEL_ROTATION_MAX_SPEED_M_PER_S = 15.2;
+
+// how often to sample pass feature data, so once for every n passes considered
+static constexpr unsigned int PASS_SAMPLING_FREQUENCY = 10;
+
+// how often to commit to a random pass instead of the best pass, so once every n passes
+static constexpr unsigned int BEST_PASS_OVERRIDE_FREQUENCY = 10;

src/software/ai/passing/BUILD

@@ -131,6 +131,7 @@ cc_library(
     hdrs = ["pass_generator.h"],
     deps = [
         ":cost_functions",
+        ":pass_feature_collector",
         ":pass_with_rating",
         "//software/optimization:gradient_descent",
         "//software/world",
@@ -147,3 +148,15 @@ cc_test(
         "//software/world",
     ],
 )
+
+cc_library(
+    name = "pass_feature_collector",
+    srcs = ["pass_feature_collector.cpp"],
+    hdrs = ["pass_feature_collector.h"],
+    deps = [
+        ":cost_functions",
+        ":pass",
+        "//software/ai/evaluation:calc_best_shot",
+        "//software/world",
+    ],
+)

src/software/ai/passing/cost_function.cpp

@@ -144,6 +144,26 @@ double calculateInterceptRisk(const Team& enemy_team, const Pass& pass,
     return *std::max_element(enemy_intercept_risks.begin(), enemy_intercept_risks.end());
 }
 
+double getEnemyTimeToInterceptPoint(const Robot& enemy_robot, const Pass& pass,
+                                    const Point& interception_point)
+{
+    Vector enemy_interception_vector = interception_point - enemy_robot.position();
+    // Take into account the enemy robot's radius for minimum min_interception_distance
+    // required to travel to intercept the pass.
+    double min_interception_distance =
+        std::max(0.0, enemy_interception_vector.length() - ROBOT_MAX_RADIUS_METERS);
+
+    const double ENEMY_ROBOT_INTERCEPTION_SPEED_METERS_PER_SECOND = 0.5;
+    double signed_1d_enemy_vel =
+        enemy_robot.velocity().dot(enemy_interception_vector.normalize());
+
+    return getTimeToTravelDistance(
+               min_interception_distance, ENEMY_ROBOT_MAX_SPEED_METERS_PER_SECOND,
+               ENEMY_ROBOT_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED,
+               signed_1d_enemy_vel, ENEMY_ROBOT_INTERCEPTION_SPEED_METERS_PER_SECOND)
+        .toSeconds();
+}
+
 double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
                               const TbotsProto::PassingConfig& passing_config)
 {
@@ -157,22 +177,10 @@ double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
     // point on the pass before the ball
     Point closest_interception_point = closestPoint(
         enemy_robot.position(), Segment(pass.passerPoint(), pass.receiverPoint()));
-    Vector enemy_interception_vector =
-        closest_interception_point - enemy_robot.position();
-    // Take into account the enemy robot's radius for minimum min_interception_distance
-    // required to travel to intercept the pass.
-    double min_interception_distance =
-        std::max(0.0, enemy_interception_vector.length() - ROBOT_MAX_RADIUS_METERS);
 
-    const double ENEMY_ROBOT_INTERCEPTION_SPEED_METERS_PER_SECOND = 0.5;
-    double signed_1d_enemy_vel =
-        enemy_robot.velocity().dot(enemy_interception_vector.normalize());
     double enemy_robot_time_to_interception_point_sec =
-        getTimeToTravelDistance(
-            min_interception_distance, ENEMY_ROBOT_MAX_SPEED_METERS_PER_SECOND,
-            ENEMY_ROBOT_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED, signed_1d_enemy_vel,
-            ENEMY_ROBOT_INTERCEPTION_SPEED_METERS_PER_SECOND)
-            .toSeconds();
+        getEnemyTimeToInterceptPoint(enemy_robot, pass, closest_interception_point);
+
     // Scale the time to interception point by the enemy robot's interception capability
     Duration enemy_robot_time_to_interception_point =
         Duration::fromSeconds(enemy_robot_time_to_interception_point_sec *
@@ -195,6 +203,53 @@ double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
                       0.0, 1.0);
 }
 
+const Robot* getClosestReceiverToPass(const Team& friendly_team, const Pass& pass)
+{
+    if (friendly_team.getAllRobots().empty())
+        return nullptr;
+
+    const Robot* best_receiver = nullptr;
+    double curr_best_distance  = std::numeric_limits<double>::max();
+
+    for (const Robot& robot : friendly_team.getAllRobots())
+    {
+        double distance = (robot.position() - pass.receiverPoint()).length();
+        if (distance < curr_best_distance)
+        {
+            best_receiver      = &robot;
+            curr_best_distance = distance;
+        }
+    }
+
+    return best_receiver;
+}
+
+Duration getBallTravelTime(const Pass& pass,
+                           const TbotsProto::PassingConfig& passing_config)
+{
+    return Duration::fromSeconds((pass.receiverPoint() - pass.passerPoint()).length() /
+                                 pass.speed()) +
+           Duration::fromSeconds(passing_config.pass_delay_sec());
+}
+
+Timestamp getEarliestReceiveTime(const Robot* best_receiver, const Pass& pass,
+                                 const TbotsProto::PassingConfig& passing_config)
+{
+    Duration min_robot_travel_time =
+        best_receiver->getTimeToPosition(pass.receiverPoint());
+    Timestamp earliest_time_to_receive_point =
+        best_receiver->timestamp() + min_robot_travel_time;
+
+    return earliest_time_to_receive_point;
+}
+
+Timestamp getEarliestTimeToAngle(const Robot* best_receiver, const Pass& pass)
+{
+    Angle receive_angle = (pass.passerPoint() - best_receiver->position()).orientation();
+    Duration time_to_receive_angle = best_receiver->getTimeToOrientation(receive_angle);
+    return best_receiver->timestamp() + time_to_receive_angle;
+}
+
 double ratePassFriendlyCapability(const Team& friendly_team, const Pass& pass,
                                   const TbotsProto::PassingConfig& passing_config)
 {
@@ -211,37 +266,20 @@ double ratePassFriendlyCapability(const Team& friendly_team, const Pass& pass,
     }
 
     // Get the robot that is closest to where the pass would be received
-    Robot best_receiver = friendly_team.getAllRobots()[0];
-    for (const Robot& robot : friendly_team.getAllRobots())
-    {
-        double distance = (robot.position() - pass.receiverPoint()).length();
-        double curr_best_distance =
-            (best_receiver.position() - pass.receiverPoint()).length();
-        if (distance < curr_best_distance)
-        {
-            best_receiver = robot;
-        }
-    }
+    const Robot* best_receiver = getClosestReceiverToPass(friendly_team, pass);
+
+    Duration ball_travel_time = getBallTravelTime(pass, passing_config);
+    Timestamp receive_time    = best_receiver->timestamp() + ball_travel_time;
 
     // Figure out what time the robot would have to receive the ball at
+    // and how long it would take our robot to get there
     // TODO (#2988): We should generate a more realistic ball trajectory
-    Duration ball_travel_time =
-        Duration::fromSeconds((pass.receiverPoint() - pass.passerPoint()).length() /
-                              pass.speed()) +
-        Duration::fromSeconds(passing_config.pass_delay_sec());
-    Timestamp receive_time = best_receiver.timestamp() + ball_travel_time;
-
-    // Figure out how long it would take our robot to get there
-    Duration min_robot_travel_time =
-        best_receiver.getTimeToPosition(pass.receiverPoint());
-    Timestamp earliest_time_to_receive_point =
-        best_receiver.timestamp() + min_robot_travel_time;
+    const Timestamp earliest_time_to_receive_point =
+        getEarliestReceiveTime(best_receiver, pass, passing_config);
 
     // Figure out what angle the robot would have to be at to receive the ball
-    Angle receive_angle = (pass.passerPoint() - best_receiver.position()).orientation();
-    Duration time_to_receive_angle = best_receiver.getTimeToOrientation(receive_angle);
-    Timestamp earliest_time_to_receive_angle =
-        best_receiver.timestamp() + time_to_receive_angle;
+    const Timestamp earliest_time_to_receive_angle =
+        getEarliestTimeToAngle(best_receiver, pass);
 
     // Figure out if rotation or moving will take us longer
     Timestamp latest_time_to_receiver_state =
@@ -259,30 +297,36 @@ double ratePassFriendlyCapability(const Team& friendly_team, const Pass& pass,
         sigmoid_width);
 }
 
-double getStaticPositionQuality(const Field& field, const Point& position,
-                                const TbotsProto::PassingConfig& passing_config)
+Rectangle getReducedField(const Field& field, TbotsProto::PassingConfig passing_config)
 {
-    // This constant is used to determine how steep the sigmoid slopes below are
-    static const double sig_width = 0.1;
-
     // The offset from the sides of the field for the center of the sigmoid functions
     double x_offset = passing_config.static_field_position_quality_x_offset();
     double y_offset = passing_config.static_field_position_quality_y_offset();
-    double friendly_goal_weight =
-        passing_config.static_field_position_quality_friendly_goal_distance_weight();
 
-    // Make a slightly smaller field, and positive weight values in this reduced field
     double half_field_length = field.xLength() / 2;
     double half_field_width  = field.yLength() / 2;
     Rectangle reduced_size_field(
         Point(-half_field_length + x_offset, -half_field_width + y_offset),
         Point(half_field_length - x_offset, half_field_width - y_offset));
+    return reduced_size_field;
+}
+
+double getStaticPositionQuality(const Field& field, const Point& position,
+                                const TbotsProto::PassingConfig& passing_config)
+{
+    // This constant is used to determine how steep the sigmoid slopes below are
+    static const double sig_width = 0.1;
+
+    // Make a slightly smaller field, and positive weight values in this reduced field
+    const auto reduced_size_field = getReducedField(field, passing_config);
     double on_field_quality = rectangleSigmoid(reduced_size_field, position, sig_width);
 
     // Add a negative weight for positions closer to our goal
     Vector vec_to_friendly_goal = Vector(field.friendlyGoalCenter().x() - position.x(),
                                          field.friendlyGoalCenter().y() - position.y());
     double distance_to_friendly_goal = vec_to_friendly_goal.length();
+    double friendly_goal_weight =
+        passing_config.static_field_position_quality_friendly_goal_distance_weight();
     double near_friendly_goal_quality =
         (1 -
          std::exp(-friendly_goal_weight * (std::pow(5, -2 + distance_to_friendly_goal))));

src/software/ai/passing/cost_function.h

@@ -117,6 +117,19 @@ double ratePassNotTooClose(const Pass& pass,
 double calculateInterceptRisk(const Team& enemy_team, const Pass& pass,
                               const TbotsProto::PassingConfig& passing_config);
 
+/**
+ * Calculates the time taken by an enemy to get to a specific intercept point
+ *
+ * @param enemy_robot The robot that might intercept our pass
+ * @param pass The pass we want to get the intercept probability for
+ * @param interception_point The point on the pass's path that the enemy is trying to
+ *                           intercept the ball at
+ * @return A value in seconds, which is the time taken by the enemy to get to the
+ * interception point
+ */
+double getEnemyTimeToInterceptPoint(const Robot& enemy_robot, const Pass& pass,
+                                    const Point& interception_point);
+
 /**
  * Calculates the likelihood that the given pass will be intercepted by a given robot
  *
@@ -131,6 +144,15 @@ double calculateInterceptRisk(const Team& enemy_team, const Pass& pass,
 double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
                               const TbotsProto::PassingConfig& passing_config);
 
+Duration getBallTravelTime(const Pass& pass,
+                           const TbotsProto::PassingConfig& passing_config);
+
+const Robot* getClosestReceiverToPass(const Team& friendly_team, const Pass& pass);
+
+Timestamp getEarliestReceiveTime(const Robot* best_receiver, const Pass& pass,
+                                 const TbotsProto::PassingConfig& passing_config);
+
+Timestamp getEarliestTimeToAngle(const Robot* best_receiver, const Pass& pass);
 
 /**
  * Calculate the probability of a friendly robot receiving the given pass
@@ -149,6 +171,8 @@ double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
 double ratePassFriendlyCapability(const Team& friendly_team, const Pass& pass,
                                   const TbotsProto::PassingConfig& passing_config);
 
+Rectangle getReducedField(const Field& field, TbotsProto::PassingConfig passing_config);
+
 /**
  * Calculates the static position quality for a given position on a given field
  *