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
@@ -102,6 +106,8 @@ static const double COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND = 0.5;
 static const double STOP_COMMAND_BALL_AVOIDANCE_DISTANCE_M = 0.5;
 // The maximum speed attainable by enemy robots
 static const double ENEMY_ROBOT_MAX_SPEED_METERS_PER_SECOND = 3.0;
+// The speed at which enemy robots can intercept a moving ball
+static const double ENEMY_ROBOT_INTERCEPTION_SPEED_METERS_PER_SECOND = 0.5;
 // The maximum acceleration achievable by enemy robots, in metres per seconds squared.
 static const double ENEMY_ROBOT_MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 4.0;
 
@@ -228,3 +234,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,24 @@ double calculateInterceptRisk(const Team& enemy_team, const Pass& pass,
     return *std::max_element(enemy_intercept_risks.begin(), enemy_intercept_risks.end());
 }
 
+Duration getEnemyTimeToInterceptPoint(const Robot& enemy_robot,
+                                      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);
+
+    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);
+}
+
 double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
                               const TbotsProto::PassingConfig& passing_config)
 {
@@ -157,26 +175,13 @@ 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();
+    double time_to_intercept_s =
+        getEnemyTimeToInterceptPoint(enemy_robot, closest_interception_point).toSeconds();
+
     // 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 *
-                              passing_config.enemy_interception_time_multiplier());
+    Duration scaled_time_to_intercept = Duration::fromSeconds(
+        time_to_intercept_s * passing_config.enemy_interception_time_multiplier());
 
     // TODO (#2988): We should generate a more realistic ball trajectory
     Duration ball_time_to_interception_point =
@@ -185,7 +190,7 @@ double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
         Duration::fromSeconds(passing_config.pass_delay_sec());
 
     Duration interception_delta_time =
-        ball_time_to_interception_point - enemy_robot_time_to_interception_point;
+        ball_time_to_interception_point - scaled_time_to_intercept;
 
     // Whether or not the enemy will be able to intercept the pass can be determined
     // by whether or not they will be able to reach the pass receive position before
@@ -195,6 +200,54 @@ double calculateInterceptRisk(const Robot& enemy_robot, const Pass& pass,
                       0.0, 1.0);
 }
 
+std::optional<const Robot> getClosestReceiverToPass(const Team& friendly_team,
+                                                    const Pass& pass)
+{
+    if (friendly_team.getAllRobots().empty())
+        return std::nullopt;
+
+    auto best_receiver        = friendly_team.getAllRobots().at(0);
+    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 +264,27 @@ 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())
+    auto best_receiver_opt = getClosestReceiverToPass(friendly_team, pass);
+
+    if (!best_receiver_opt.has_value())
     {
-        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;
-        }
+        return 0;
     }
 
+    const Robot& best_receiver = best_receiver_opt.value();
+
+    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 +302,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))));