diff --git a/environment_setup/ubuntu24_requirements.txt b/environment_setup/ubuntu24_requirements.txt index e3cad5dd7d..68c667fa79 100644 --- a/environment_setup/ubuntu24_requirements.txt +++ b/environment_setup/ubuntu24_requirements.txt @@ -1,7 +1,7 @@ ansible-lint==26.4.0 pyqtgraph==0.13.7 thefuzz==0.19.0 -iterfzf==0.5.0.20.0 +iterfzf==1.9.0.67.0 python-Levenshtein==0.25.1 psutil==5.9.0 PyOpenGL==3.1.6 diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/BUILD b/src/software/ai/hl/stp/play/kickoff_friendly/BUILD index 35325ce76a..833c3b77dd 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/BUILD +++ b/src/software/ai/hl/stp/play/kickoff_friendly/BUILD @@ -24,7 +24,6 @@ cc_library( ], alwayslink = True, ) - py_test( name = "kickoff_friendly_play_test", srcs = ["kickoff_friendly_play_test.py"], diff --git a/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD b/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD index b83140a6e5..7594f9f9b9 100644 --- a/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD +++ b/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD @@ -33,7 +33,6 @@ cc_test( "//software/test_util", ], ) - py_test( name = "penalty_kick_enemy_play_test", srcs = ["penalty_kick_enemy_play_test.py"], @@ -44,3 +43,4 @@ py_test( requirement("pytest"), ], ) + diff --git a/src/software/ai/hl/stp/tactic/attacker/BUILD b/src/software/ai/hl/stp/tactic/attacker/BUILD index 86634c632e..3b775b6236 100644 --- a/src/software/ai/hl/stp/tactic/attacker/BUILD +++ b/src/software/ai/hl/stp/tactic/attacker/BUILD @@ -34,7 +34,6 @@ cc_test( "//software/test_util", ], ) - py_test( name = "attacker_tactic_test", srcs = ["attacker_tactic_test.py"], diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp index b779a3d2fd..9ead30b9e0 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp @@ -4,6 +4,7 @@ #include "software/ai/evaluation/intercept.h" #include "software/ai/hl/stp/tactic/move_primitive.h" #include "software/math/math_functions.h" +#include "software/logger/logger.h" GoalieFSM::GoalieFSM(std::shared_ptr ai_config_ptr, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode) @@ -167,9 +168,13 @@ bool GoalieFSM::shouldPanic(const Update& event) bool GoalieFSM::shouldPivotChip(const Update& event) { double ball_speed_panic = ai_config_ptr->goalie_tactic_config().ball_speed_panic(); - return event.common.world_ptr->ball().velocity().length() <= ball_speed_panic && + bool chip = event.common.world_ptr->ball().velocity().length() <= ball_speed_panic && event.common.world_ptr->field().pointInFriendlyDefenseArea( event.common.world_ptr->ball().position()); + if (chip){LOG(INFO)<<"CHIPPED" < ValidationStatus: + def get_validation_status( + self, world: tbots.World, simulator_state=None + ) -> ValidationStatus: """Checks if a specific robot id is in the provided region Then checks if that robot is stationary within a threshold for the provided number of ticks diff --git a/src/software/constants.h b/src/software/constants.h index d1592e54b1..1d32b7d5d9 100644 --- a/src/software/constants.h +++ b/src/software/constants.h @@ -53,7 +53,7 @@ const std::string ROBOT_KICK_EXP_COEFF_CONFIG_KEY = "kick_coeff"; const std::string ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY = "chip_pulse_width"; const std::string SSL_VISION_ADDRESS = "224.5.23.2"; -static constexpr unsigned int SSL_VISION_PORT = 10020; +static constexpr unsigned int SSL_VISION_PORT = 10006; -const std::string SSL_REFEREE_ADDRESS = "224.5.23.1"; -static constexpr unsigned int SSL_REFEREE_PORT = 10003; +const std::string SSL_REFEREE_ADDRESS = "224.5.23.2"; +static constexpr unsigned int SSL_REFEREE_PORT = 10005; diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 51c5bb2ea9..fe371634ec 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,6 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); + LOG(CSV, "realism_kalman_filter_v12.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -134,6 +135,7 @@ int main(int argc, char** argv) runtime_dir + WORLD_STATE_RECEIVED_TRIGGER_PATH); bool has_sent_world_state_trigger = false; + double start_timestamp_s = 0.0; // Inputs // World State Input: Configures the ERForceSimulator @@ -222,7 +224,26 @@ int main(int argc, char** argv) yellow_robot_status_output.sendProto(packet); } - simulator_state_output.sendProto(er_force_sim->getSimulatorState()); + auto sim_state = er_force_sim->getSimulatorState(); + double current_ts = yellow_vision.time_sent().epoch_timestamp_seconds(); + if (start_timestamp_s == 0.0) + { + start_timestamp_s = current_ts; + } + LOG(CSV, "realism_kalman_filter_v12.csv") + << (current_ts - start_timestamp_s) << "," + << yellow_vision.ball().current_state().global_position().x_meters() + << "," + << yellow_vision.ball().current_state().global_position().y_meters() + << "," + << yellow_vision.ball().current_state().global_velocity().x_component_meters() + << "," + << yellow_vision.ball().current_state().global_velocity().y_component_meters() + << "," << sim_state.ball().p_x() << "," << sim_state.ball().p_y() + << "," << sim_state.ball().v_x() << "," << sim_state.ball().v_y() + << "," << !er_force_sim->isBallVisible() + << "\n"; + simulator_state_output.sendProto(sim_state); }); // This blocks forever without using the CPU diff --git a/src/software/networking/unix/proto_unix_listener.hpp b/src/software/networking/unix/proto_unix_listener.hpp index c984eff065..57f68329e3 100644 --- a/src/software/networking/unix/proto_unix_listener.hpp +++ b/src/software/networking/unix/proto_unix_listener.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -73,6 +75,7 @@ ProtoUnixListener::ProtoUnixListener( socket_.open(); socket_.bind(listen_endpoint_); + ::chmod(unix_path.c_str(), 0777); boost::asio::local::datagram_protocol::socket::receive_buffer_size option( UNIX_BUFFER_SIZE); diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index fccb82975e..ef8220817d 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -16,29 +16,31 @@ cc_library( ) cc_library( - name = "ball_filter", - srcs = ["ball_filter.cpp"], - hdrs = ["ball_filter.h"], + name = "kalman_filter", + srcs = ["kalman_filter.cpp"], + hdrs = ["kalman_filter.h"], + deps = [ + "@eigen", + ], +) +cc_library( + name = "ball_tracker", + srcs = ["ball_tracker.cpp"], + hdrs = ["ball_tracker.h"], deps = [ ":vision_detection", + ":kalman_filter", "//software/geom/algorithms", "//software/math:math_functions", "//software/world:ball", "//software/world:field", + "//software/world:robot", + "//software:constants", "@boost//:circular_buffer", "@eigen", ], ) -cc_test( - name = "ball_filter_test", - srcs = ["ball_filter_test.cpp"], - deps = [ - ":ball_filter", - "//shared/test_util:tbots_gtest_main", - "//software/world:field", - ], -) cc_library( name = "robot_filter", @@ -83,18 +85,7 @@ cc_test( cc_library( name = "sensor_fusion_filters", deps = [ - ":ball_filter", + ":ball_tracker", ":robot_team_filter", ], ) - -py_test( - name = "ball_occlusion_test", - srcs = ["ball_occlusion_test.py"], - tags = ["exclusive"], - deps = [ - "//software:conftest", - "//software/simulated_tests/validation:validations", - requirement("pytest"), - ], -) diff --git a/src/software/sensor_fusion/filter/ball_filter.h b/src/software/sensor_fusion/filter/ball_filter.h deleted file mode 100644 index cf108b2ac6..0000000000 --- a/src/software/sensor_fusion/filter/ball_filter.h +++ /dev/null @@ -1,196 +0,0 @@ -#pragma once - -#include -#include - -#include "software/geom/line.h" -#include "software/geom/point.h" -#include "software/geom/rectangle.h" -#include "software/sensor_fusion/filter/vision_detection.h" -#include "software/time/timestamp.h" -#include "software/world/ball.h" - -/** - * Given ball data from SSL Vision, filters and returns the position/velocity of the - * "real" ball. - * - * This ball filter stores a buffer of previous SSL Vision detections, and uses linear - * regression to find the path the ball is travelling on and estimate its position - * and velocity. This buffer/regression system was chosen because it results in a - * very stable output, particularly for the ball velocity. The data we receive isn't - * perfect (which is why we have a filter). If we receive a noisy position that is off - * the ball's current trajectory, it will have minimal impact. This means that as - * the ball is travelling, this filter will return a very steady velocity vector. - * This is important because small deviations in velocity orientation can have large - * effects when the AI tries to predict the future position of the ball. For example, - * consistently receiving a pass relies on the ball's velocity being very stable, - * otherwise the robot would "jiggle" back and forth as the estimated receiver position - * would keep changing. - */ -class BallFilter -{ - public: - // The min and max sizes of the ball detection buffer. - // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE. - // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE. - static constexpr unsigned int MIN_BUFFER_SIZE = 4; - static constexpr unsigned int MAX_BUFFER_SIZE = 10; - // If the estimated ball speed is less than this value, the largest possible buffer - // will be used by the filter - static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5; - // If the estimated ball speed is greater than this value, the smallest possible - // buffer will be used by the filter - static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0; - // The extra amount beyond the ball's max speed that we treat ball detections as valid - static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0; - // The maximum root mean squared error threshold to considering using the generated - // linear regression. - // TODO (#2752): Investigate different values of error threshold - static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0; - - /** - * Creates a new Ball Filter - */ - explicit BallFilter(); - - /** - * Update the filter with the new ball detection data, and returns the new - * estimated state of the ball given the new data - * - * @param new_ball_detections A list of new Ball detections - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - * - * @return The new ball based on the estimated state of the ball given the new data. - * If a filtered result cannot be calculated, returns std::nullopt - */ - std::optional estimateBallState( - const std::vector& new_ball_detections, - const Rectangle& filter_area); - - private: - /** - * A simple struct we use to pass around velocity estimate data - */ - struct BallVelocityEstimate - { - Vector average_velocity; - double average_velocity_magnitude; - // The average of the max velocity magnitude and min velocity magnitude - double min_max_magnitude_average; - }; - - /** - * A simple struct to pass around linear regression data - */ - struct LinearRegressionResults - { - Line regression_line; - // Regression error is root mean squared error - double regression_error; - }; - - /** - * Adds ball detections to the buffer stored by this filter. This function will ignore - * data if: - * - the data is outside of the filter_area, or - * - the data is too far away from the current known ball position - * (since it is likely to be random noise). - * - * @param new_ball_detections The ball detections to try add to the buffer - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - */ - void addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle& filter_area); - - /** - * Uses linear regression to filter the given list of ball detections to find the - * current "real" state of the ball. - * - * @param ball_detections The detections to filter - * - * @return The new ball based on the filtered state. If a filtered result cannot be - * calculated, returns std::nullopt - */ - static std::optional estimateBallStateFromBuffer( - boost::circular_buffer ball_detections); - - /** - * Returns how large the buffer of ball detections should be based on the ball's - * estimated velocity. A slower moving ball will result in a larger buffer size, and a - * faster ball will result in a smaller buffer size. This is because with a slow - * moving ball, we need more data in order to fit a line with reasonable accuracy, - * since the datapoints will be very close to one another. - * - * @param ball_detections The full list of ball detections - * - * @return The size the buffer should be to perform filtering operations. If an error - * occurs that prevents the size from being calculated correctly, returns std::nullopt - */ - static std::optional getAdjustedBufferSize( - boost::circular_buffer ball_detections); - - /** - * Given a buffer of ball detections, returns the line of best fit through - * the detection positions, and calculate the root mean squared error of this - * regression. - * Note: also considers vertical lines. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to fit - * - * @return The line of best fit through the given ball detection positions - */ - static LinearRegressionResults calculateLineOfBestFit( - boost::circular_buffer ball_detections); - - /** - * Given a list of ball detections, use linear regression to find a line of best fit - * through the ball positions, and calculate the root mean squared error of this - * regression. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to use in the regression - * - * @return A struct containing the regression line and error of the linear regression - */ - static LinearRegressionResults calculateLinearRegression( - boost::circular_buffer ball_detections); - - /** - * Estimates the current position of the ball given a buffer of ball detections - * and the line of best fit through them. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections - * @param regression_line The line of best fit through the ball positions - * - * @return The estimated position of the ball - */ - static Point estimateBallPosition( - boost::circular_buffer ball_detections, - const Line& regression_line); - - /** - * Estimates the ball's velocity based on the current detections in the given buffer. - * If the ball_regression_line is provided, the detection positions are projected onto - * the line before the velocities are calculated. If no velocity can be estimated, - * std::nullopt is returned. - * - * @param ball_detections The ball detections to use to calculate - * @param ball_regression_line The ball_regression_line to snap detections to before - * calculating velocities. - * - * @return A struct containing various estimates of the ball's velocity based on the - * given detections. If no velocity can be estimated, std::nullopt is returned - */ - static std::optional estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional& ball_regression_line = std::nullopt); - - boost::circular_buffer ball_detection_buffer; -}; diff --git a/src/software/sensor_fusion/filter/ball_filter_test.cpp b/src/software/sensor_fusion/filter/ball_filter_test.cpp deleted file mode 100644 index 1de4515861..0000000000 --- a/src/software/sensor_fusion/filter/ball_filter_test.cpp +++ /dev/null @@ -1,548 +0,0 @@ -#include "software/sensor_fusion/filter/ball_filter.h" - -#include - -#include -#include -#include - -#include "shared/constants.h" -#include "software/geom/algorithms/distance.h" -#include "software/geom/ray.h" -#include "software/geom/segment.h" -#include "software/world/field.h" - -class BallFilterTest : public ::testing::Test -{ - protected: - BallFilterTest() - : field(Field::createSSLDivisionBField()), - ball_filter(), - current_timestamp(Timestamp::fromSeconds(123)), - time_step(Duration::fromSeconds(1.0 / 60.0)) - { - } - - void SetUp() override - { - // Use a constant seed to results are deterministic - random_generator.seed(1); - } - - /** - * Simulates the ball moving in a certain direction with a specified amount of noise - * in the measurements, and verifies that the filtered ball data is within the given - * tolerances of the real ball. - * - * @param start_time The time at which the ball starts moving - * @param ball_trajectory A ray indicating the ball's starting position and direction - * of travel - * @param ball_velocity_magnitude How fast the ball is moving - * @param ball_position_variance The variance in the noise the ball's position will be - * sampled with - * @param time_step_variance The variance in the timestep the ball will be sampled - * with - * @param expected_position_tolerance How close to the real position the filtered ball - * position should be - * @param expected_velocity_angle_tolerance How close to the real velocity angle the - * filtered ball velocity angle should be - * @param expected_velocity_magnitude_tolerance How close to the real velocity - * magnitude the filtered ball velocity magnitude should be - * @param num_iterations For how many iterations the simulation should run - * @param num_steps_to_ignore The number of iterations at the beginning of the - * simulation to not check tolerances - */ - void testFilterAlongBallTrajectory( - const Timestamp& start_time, const Ray& ball_trajectory, - double ball_velocity_magnitude, double ball_position_variance, - double time_step_variance, double expected_position_tolerance, - Angle expected_velocity_angle_tolerance, - double expected_velocity_magnitude_tolerance, unsigned int num_iterations, - unsigned int num_steps_to_ignore) - { - Point ball_starting_position = ball_trajectory.getStart(); - Vector ball_velocity = - ball_trajectory.toUnitVector().normalize(ball_velocity_magnitude); - Duration max_ball_travel_duration = - Duration::fromSeconds(std::numeric_limits::max()); - - testFilterHelper(start_time, ball_starting_position, ball_velocity, - ball_position_variance, time_step_variance, - expected_position_tolerance, expected_velocity_angle_tolerance, - expected_velocity_magnitude_tolerance, num_iterations, - max_ball_travel_duration, num_steps_to_ignore); - } - - /** - * Simulates the ball moving in a line, starting at one point and ending at another - * with a specified amount of noise in the measurements, and verifies the filtered - * ball data is within the given tolerances of the real ball. - * - * @param start_time The time at which the ball starts moving - * @param ball_path The segment along which the ball will travel - * @param ball_velocity_magnitude How fast the ball is moving - * @param ball_position_variance The variance in the noise the ball's position will be - * sampled with - * @param time_step_variance The variance in the timestep the ball will be sampled - * with - * @param expected_position_tolerance How close to the real position the filtered ball - * position should be - * @param expected_velocity_angle_tolerance How close to the real velocity angle the - * filtered ball velocity angle should be - * @param expected_velocity_magnitude_tolerance How close to the real velocity - * magnitude the filtered ball velocity magnitude should be - * @param num_steps_to_ignore The number of iterations at the beginning of the - * simulation to not check tolerances - */ - void testFilterAlongLineSegment(const Timestamp& start_time, const Segment& ball_path, - double ball_velocity_magnitude, - double ball_position_variance, - double time_step_variance, - double expected_position_tolerance, - Angle expected_velocity_angle_tolerance, - double expected_velocity_magnitude_tolerance, - unsigned int num_steps_to_ignore) - { - Point ball_starting_position = ball_path.getStart(); - Vector ball_velocity = ball_path.toVector().normalize(ball_velocity_magnitude); - // Check for division by 0 - if (ball_velocity_magnitude == 0) - { - throw std::invalid_argument( - "ball_velocity_magnitude with value of 0 given, this will result in division by 0"); - } - // Calculate how many simulation steps to take, given the ball's velocity and the - // time step in order for the ball to reach the end of the given segment. - Duration max_ball_travel_duration = - Duration::fromSeconds(ball_path.length() / ball_velocity_magnitude); - int num_iterations = static_cast( - std::round(max_ball_travel_duration.toSeconds() / time_step.toSeconds())); - - testFilterHelper(start_time, ball_starting_position, ball_velocity, - ball_position_variance, time_step_variance, - expected_position_tolerance, expected_velocity_angle_tolerance, - expected_velocity_magnitude_tolerance, num_iterations, - max_ball_travel_duration, num_steps_to_ignore); - } - - /** - * A helper function that simulates the ball moving in a straight line. The ball's - * position will be sampled with a given amount of noise / variance, and the filtered - * data will be checked to verify it is within the specified tolerances compared to - * the real ball - * @param start_time The time at which the ball starts moving - * @param ball_starting_position The position the ball starts from - * @param ball_velocity The velocity of the ball - * @param ball_position_variance The variance in the noise the ball's position will be - * sampled with - * @param time_step_variance The variance in the timestep the ball will be sampled - * with - * @param expected_position_tolerance How close to the real position the filtered ball - * position should be - * @param expected_velocity_angle_tolerance How close to the real velocity angle the - * filtered ball velocity angle should be - * @param expected_velocity_magnitude_tolerance How close to the real velocity - * magnitude the filtered ball velocity magnitude should be - * @param num_iterations For how many iterations the simulation should run - * @param max_ball_travel_duration The maximum duration for which the ball may travel - * / be simulated - * @param num_steps_to_ignore The number of iterations at the beginning of the - * simulation to not check tolerances - */ - void testFilterHelper(const Timestamp& start_time, - const Point& ball_starting_position, - const Vector& ball_velocity, double ball_position_variance, - double time_step_variance, double expected_position_tolerance, - const Angle& expected_velocity_angle_tolerance, - double expected_velocity_magnitude_tolerance, - unsigned int num_iterations, - const Duration& max_ball_travel_duration, - unsigned int num_steps_to_ignore = 0) - { - // Create the distributions we use to generate noise when sampling the ball - std::normal_distribution position_noise_distribution( - 0, ball_position_variance); - std::normal_distribution time_step_noise_distribution(0, - time_step_variance); - - for (unsigned i = 0; i < num_iterations; i++) - { - // Generate the noise that will be added to the position and time step to - // simulate imperfect data - Vector position_noise(position_noise_distribution(random_generator), - position_noise_distribution(random_generator)); - Duration time_step_noise = - Duration::fromSeconds(time_step_noise_distribution(random_generator)); - - // Calculate the current time and add noise - // We make sure the applied noise doesn't cause the timestamp to be larger - // than expected on the last iteration so the ball's final position is close - // to what's expected by the caller of this function - Timestamp current_timestamp_with_noise = - start_time + Duration::fromSeconds(i * time_step.toSeconds() + - time_step_noise.toSeconds()); - this->current_timestamp = std::min(current_timestamp_with_noise, - start_time + max_ball_travel_duration); - - // Take the time difference from the start time and calculate the ball's - // current position based on it's velocity and the elapsed time - Duration time_diff = current_timestamp - start_time; - Point current_ball_position = - ball_starting_position + - ball_velocity.normalize(ball_velocity.length() * time_diff.toSeconds()); - - // Apply noise to the ball's position to simulate measurement noise - Point ball_position_with_noise = current_ball_position + position_noise; - - // Create the detection that would have been seen by the vision system - std::vector ball_detections = { - BallDetection{ball_position_with_noise, BALL_DISTANCE_FROM_GROUND, - current_timestamp, 0.9}}; - - // Get the filtered result given the new detection information - auto filtered_ball = - ball_filter.estimateBallState(ball_detections, field.fieldBoundary()); - if (i < num_steps_to_ignore) - { - continue; - } - - ASSERT_TRUE(filtered_ball); - double ball_position_difference = - (filtered_ball->position() - current_ball_position).length(); - EXPECT_LT(ball_position_difference, expected_position_tolerance); - // Only check the velocity once we have more than 1 data entry in the filter - // since the filter can't return a realistic velocity with only a single - // detection - if (i > 0) - { - // Check the direction of the velocity - double velocity_orientation_difference = - std::fabs(filtered_ball->velocity() - .orientation() - .minDiff(ball_velocity.orientation()) - .toDegrees()); - EXPECT_LE(velocity_orientation_difference, - expected_velocity_angle_tolerance.toDegrees()); - // Check the magnitude of the velocity - double velocity_magnitude_difference = std::fabs( - filtered_ball->velocity().length() - ball_velocity.length()); - EXPECT_LE(velocity_magnitude_difference, - expected_velocity_magnitude_tolerance); - } - - // Make sure the timestamps are always increasing - EXPECT_GE(filtered_ball->timestamp(), current_timestamp); - } - } - - Field field; - BallFilter ball_filter; - Timestamp current_timestamp; - Duration time_step; - std::mt19937 random_generator; - // For these tests, the ball is always on the ground. The filters - // are not designed for filtering balls in the air - static constexpr double BALL_DISTANCE_FROM_GROUND = 0.0; -}; - -TEST_F(BallFilterTest, ball_sitting_still_with_low_noise) -{ - Ray ball_trajectory = Ray(Point(0, 0), Vector(0, 0)); - double ball_velocity_magnitude = 0; - double ball_position_variance = 0.001; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.005; - // When the ball is sitting still, the velocity could be any direction so we do - // not use a strict tolerance for this test. We only really care about the - // magnitude of the velocity to make sure it's small enough - Angle expected_velocity_angle_tolernace = Angle::threeQuarter(); - double expected_velocity_magnitude_tolerance = 0.075; - int num_simulation_steps = 200; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongBallTrajectory( - start_time, ball_trajectory, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_simulation_steps, num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_sitting_still_with_moderate_noise) -{ - Ray ball_trajectory = Ray(Point(0, 0), Vector(0, 0)); - double ball_velocity_magnitude = 0; - double ball_position_variance = 0.003; - double time_step_variance = 0.003; - double expected_position_tolerance = 0.016; - // When the ball is sitting still, the velocity could be any direction so we do - // not use a strict tolerance for this test. We only really care about the - // magnitude of the velocity to make sure it's small enough - Angle expected_velocity_angle_tolernace = Angle::threeQuarter(); - double expected_velocity_magnitude_tolerance = 1.0; - int num_simulation_steps = 10; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongBallTrajectory( - start_time, ball_trajectory, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_simulation_steps, num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_no_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 0.31; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_small_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 0.3; - double ball_position_variance = 0.001; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.004; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(5.5); - double expected_velocity_magnitude_tolerance = 0.04; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_medium_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 0.3; - double ball_position_variance = 0.003; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.011; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(16); - double expected_velocity_magnitude_tolerance = 0.11; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_no_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 6.2; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_small_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 5.72; - double ball_position_variance = 0.001; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.003; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.9); - double expected_velocity_magnitude_tolerance = 0.07; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_medium_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0.003; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.008; - Angle expected_velocity_angle_tolerance = Angle::fromDegrees(3.0); - double expected_velocity_magnitude_tolerance = 0.21; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolerance, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, - ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.0001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); - - ball_path = Segment(field.enemyCornerPos(), field.enemyCornerNeg()); - ball_velocity_magnitude = 4.8; - expected_position_tolerance = 0.0001; - expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - expected_velocity_magnitude_tolerance = 0.01; - num_steps_to_ignore = 5; - start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, - ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data_2) -{ - Segment ball_path = - Segment(field.friendlyCornerNeg(), field.friendlyHalf().posXPosYCorner()); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.0001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - double expected_velocity_magnitude_tolerance = 0.1; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); - - ball_path = Segment(field.friendlyHalf().posXPosYCorner(), field.enemyCornerNeg()); - ball_velocity_magnitude = 4.8; - expected_position_tolerance = 0.0001; - expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - expected_velocity_magnitude_tolerance = 0.1; - num_steps_to_ignore = 5; - start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, - ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data_3) -{ - Segment ball_path = Segment(Point(-3, -1), Point(0, 0)); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.0001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - double expected_velocity_magnitude_tolerance = 0.1; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); - - ball_path = Segment(Point(0, 0), Point(3, 0)); - ball_velocity_magnitude = 4.8; - expected_position_tolerance = 0.0001; - expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - expected_velocity_magnitude_tolerance = 0.1; - num_steps_to_ignore = 5; - start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_along_x_axis) -{ - Segment ball_path = Segment(field.friendlyGoalCenter(), field.enemyGoalCenter()); - double ball_velocity_magnitude = 5; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallFilterTest, ball_moving_along_y_axis) -{ - Segment ball_path = field.halfwayLine(); - double ball_velocity_magnitude = 5; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp new file mode 100644 index 0000000000..32c3378888 --- /dev/null +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -0,0 +1,266 @@ +#include "software/sensor_fusion/filter/ball_tracker.h" + +#include +#include "software/logger/logger.h" +#include +#include + +#include "shared/constants.h" +#include "software/constants.h" +#include "software/geom/algorithms/closest_point.h" +#include "software/geom/algorithms/contains.h" +#include "software/math/math_functions.h" +#include "software/sensor_fusion/filter/kalman_filter.h" + +namespace { + const Eigen::Vector INITIAL_STATE = Eigen::Vector::Zero(); + + const Eigen::Matrix INITIAL_COV = (Eigen::Matrix() << + 1000, 0, 0, 0, + 0, 1000, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1).finished(); + + const Eigen::Matrix Q = (Eigen::Matrix() << + 2.222e-8, 0, 2.000e-6, 0, + 0, 2.222e-8, 0, 2.000e-6, + 2.000e-6, 0, 2.400e-4, 0, + 0, 2.000e-6, 0, 2.400e-4).finished(); + + const Eigen::Matrix R = (Eigen::Matrix() << + 0.0226, 0, + 0, 0.00445).finished(); + + const Eigen::Matrix R_DRIBBLING = (Eigen::Matrix() << + 0.0001, 0, + 0, 0.0001).finished(); + + const Eigen::Matrix C = (Eigen::Matrix() << + 1, 0, 0, 0, + 0, 1, 0, 0).finished(); + + // Empirically measured per-second velocity retention during normal tracking + const double DAMPING = 0.994; + // Per-second retention when fully occluded long enough (velocity decays much faster) + const double DAMPING_OCCLUDED = 0.3; + // Occlusion time (seconds) before damping starts to increase + const double OCCLUSION_GRACE_SECONDS = 1.0; + // Occlusion time (seconds) past the grace period at which damping fully transitions to DAMPING_OCCLUDED + const double MAX_OCCLUSION_SECONDS = 1.0; + + const double MAHANALOGIS_THRESHOLD = 1; + const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; + + const double DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS = 0.08; + + // Speed gate for the init buffer: reject detections that imply a ball jump + // beyond this threshold from the last known position. + // BALL_MAX_SPEED_METERS_PER_SECOND (6.5) + 2.5 buffer for measurement error. + const double INIT_BUFFER_MAX_SPEED_M_PER_S = BALL_MAX_SPEED_METERS_PER_SECOND + 2.5; +} + +BallTracker::BallTracker() : + consecutive_outliers(0), + velocity_initialized(false), + kalman_filter(INITIAL_STATE, INITIAL_COV, + Eigen::Matrix::Identity(), + Q, + Eigen::Matrix::Zero(), + C, R) +{ +} + +std::optional BallTracker::estimateBallState( + const std::vector &new_ball_detections, const Rectangle &filter_area, + const Timestamp& current_time, std::optional dribbling_robot) +{ + Point dribbler_pos; + if (dribbling_robot.has_value()) + { + dribbler_pos = + dribbling_robot->position() + + Vector::createFromAngle(dribbling_robot->orientation()) + .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + + BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING); + } + + std::optional best_ball_detection = getBestBallDetection(new_ball_detections); + + if (!velocity_initialized) + { + if (best_ball_detection) + velocity_initialized = tryInitVelocityFromBuffer( + best_ball_detection->position, current_time); + + const Point pos = best_ball_detection + ? best_ball_detection->position + : Point(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); + const double z = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; + return Ball(BallState(pos, Vector(0, 0), z), current_time); + } + + double dt = 0.0; + if (prev_detection_timestamp){ + dt = (current_time - *prev_detection_timestamp).toSeconds(); + prev_detection_timestamp = current_time; + + const double occlusion_seconds = last_measurement_timestamp.has_value() + ? (current_time - *last_measurement_timestamp).toSeconds() + : 0.0; + const double alpha = std::min(std::max(occlusion_seconds - OCCLUSION_GRACE_SECONDS, 0.0) / MAX_OCCLUSION_SECONDS, 1.0); + const double effective_damping = DAMPING * (1.0 - alpha) + DAMPING_OCCLUDED * alpha; + + Eigen::Matrix A; + A << 1, 0, dt, 0, + 0, 1, 0, dt, + 0, 0, effective_damping, 0, + 0, 0, 0, effective_damping; + kalman_filter.process_model = A; + kalman_filter.predict(Eigen::Vector::Zero()); + } + + if (dribbling_robot.has_value()) + { + bool near_dribbler = + best_ball_detection.has_value() && + (best_ball_detection->position - dribbler_pos).length() <= + DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; + + Eigen::Vector measurement; + if (near_dribbler) + { + measurement << best_ball_detection->position.x(), + best_ball_detection->position.y(); + } + else + { + measurement << dribbler_pos.x(), dribbler_pos.y(); + } + LOG(INFO) << "[BallTracker] dribbling: tight update at (" + << measurement(0) << "," << measurement(1) << ")"; + kalman_filter.measurement_covariance = R_DRIBBLING; + kalman_filter.update(measurement); + kalman_filter.measurement_covariance = R; + prev_detection_timestamp = current_time; + last_measurement_timestamp = current_time; + consecutive_outliers = 0; + } + else if (best_ball_detection){ + Eigen::Vector measurement; + measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); + + Point predicted_pos(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); + if (!contains(filter_area, predicted_pos)){ + LOG(INFO) << "[BallTracker] predicted pos out of bounds, reset to measurement"; + kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; + kalman_filter.state_covariance = INITIAL_COV; + consecutive_outliers = 0; + velocity_initialized = false; + velocity_init_buffer.clear(); + } + + const Eigen::Vector innovation = + measurement - kalman_filter.measurement_model * kalman_filter.state_estimate; + const Eigen::Matrix S = + kalman_filter.measurement_model * kalman_filter.state_covariance * + kalman_filter.measurement_model.transpose() + kalman_filter.measurement_covariance; + double mahalanobis = (innovation.transpose() * S.inverse() * innovation)(0, 0); + + if (mahalanobis < MAHANALOGIS_THRESHOLD){ + kalman_filter.update(measurement); + last_measurement_timestamp = current_time; + } + else{ + LOG(INFO) << "[BallTracker] outlier rejected (mahalanobis=" << mahalanobis << ", consecutive=" << consecutive_outliers + 1 << ")"; + consecutive_outliers++; + } + + if (consecutive_outliers > CONSECUTIVE_OUTLIERS_THRESHOLD){ + LOG(INFO) << "[BallTracker] too many consecutive outliers, hard reset to measurement"; + kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; + kalman_filter.state_covariance = INITIAL_COV; + last_measurement_timestamp = current_time; + consecutive_outliers = 0; + velocity_initialized = false; + velocity_init_buffer.clear(); + } + prev_detection_timestamp = current_time; + } + + Point ball_position = Point(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); + Vector ball_velocity = Vector(kalman_filter.state_estimate(2), kalman_filter.state_estimate(3)); + double z_height = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; + + BallState ball_state(ball_position, ball_velocity, z_height); + return Ball(ball_state, current_time); +} + +bool BallTracker::tryInitVelocityFromBuffer(const Point& position, + const Timestamp& current_time) +{ + // Gate: reject detections that imply the ball teleported from the last + // known position. Clear the buffer and reanchor rather than poisoning the + // regression with a noise spike. + if (!velocity_init_buffer.empty()) + { + const Point last_pos(kalman_filter.state_estimate(0), + kalman_filter.state_estimate(1)); + const double dt_s = current_time.toSeconds() - velocity_init_buffer.back().second; + if (dt_s > 0 && + (position - last_pos).length() / dt_s > INIT_BUFFER_MAX_SPEED_M_PER_S) + { + LOG(INFO) << "[BallTracker] init buffer: noisy detection rejected, reanchor"; + velocity_init_buffer.clear(); + kalman_filter.state_estimate(0) = position.x(); + kalman_filter.state_estimate(1) = position.y(); + prev_detection_timestamp = current_time; + return false; + } + } + + velocity_init_buffer.emplace_back(position, current_time.toSeconds()); + kalman_filter.state_estimate(0) = position.x(); + kalman_filter.state_estimate(1) = position.y(); + prev_detection_timestamp = current_time; + + if (static_cast(velocity_init_buffer.size()) < VELOCITY_INIT_BUFFER_SIZE) + return false; + + const int N = static_cast(velocity_init_buffer.size()); + double t_mean = 0.0, x_mean = 0.0, y_mean = 0.0; + for (const auto& [pos, t] : velocity_init_buffer) { + t_mean += t; x_mean += pos.x(); y_mean += pos.y(); + } + t_mean /= N; x_mean /= N; y_mean /= N; + + double Sxx = 0.0, Stx = 0.0, Sty = 0.0; + for (const auto& [pos, t] : velocity_init_buffer) { + const double dt_i = t - t_mean; + Sxx += dt_i * dt_i; + Stx += dt_i * (pos.x() - x_mean); + Sty += dt_i * (pos.y() - y_mean); + } + if (Sxx > 1e-10) + { + kalman_filter.state_estimate(2) = Stx / Sxx; + kalman_filter.state_estimate(3) = Sty / Sxx; + kalman_filter.state_covariance(2, 2) = R(0, 0) / Sxx; + kalman_filter.state_covariance(3, 3) = R(1, 1) / Sxx; + } + LOG(INFO) << "[BallTracker] velocity initialized from buffer: vx=" << kalman_filter.state_estimate(2) << " vy=" << kalman_filter.state_estimate(3); + last_measurement_timestamp = current_time; + consecutive_outliers = 0; + velocity_init_buffer.clear(); + return true; +} + +std::optional BallTracker::getBestBallDetection(const std::vector &new_ball_detections){ + if (new_ball_detections.empty()){ + return std::nullopt; + } + else{ + return *std::max_element(new_ball_detections.begin(), new_ball_detections.end(),[](const BallDetection& a, const BallDetection& b){ + return a.confidence +#include +#include + +#include "software/geom/line.h" +#include "software/geom/point.h" +#include "software/geom/rectangle.h" +#include "software/sensor_fusion/filter/vision_detection.h" +#include "software/time/timestamp.h" +#include "software/world/ball.h" +#include "software/world/robot.h" +#include "software/sensor_fusion/filter/kalman_filter.h" + +/** + * Tracks a ball across frames using a Kalman filter with a constant-velocity + * motion model. Outlier detections are rejected via Mahalanobis-distance gating; + * if too many consecutive detections are rejected the filter is hard-reset to the + * latest measurement. + */ +class BallTracker +{ + public: + /** + * Creates a new BallTracker with default noise parameters and initial state. + */ + BallTracker(); + + /** + * Updates the tracker with the latest ball detections and returns an estimated + * ball state. + * + * The best detection (highest confidence) within the filter area is selected + * and used to update the Kalman filter. If the selected detection is an + * outlier (Mahalanobis distance exceeds threshold), it is rejected and the + * outlier count is incremented. Once the outlier count exceeds the threshold, + * the filter is reset to the outlier detection. + * + * @param new_ball_detections A list of new ball detections from vision + * @param filter_area The area within which ball detections are considered + * valid; detections outside this area are ignored + * @param current_time The timestamp of the current frame + * + * @return The estimated Ball state. If no valid detection has ever been + * received, returns a Ball at the origin with zero velocity. + */ + std::optional estimateBallState( + const std::vector& new_ball_detections, + const Rectangle& filter_area, + const Timestamp& current_time, + std::optional dribbling_robot = std::nullopt); + + private: + /** + * Selects the highest-confidence detection from the given list. + * + * @param new_ball_detections A list of ball detections to choose from + * + * @return The detection with the highest confidence, or std::nullopt if the + * list is empty + */ + std::optional getBestBallDetection(const std::vector &new_ball_detections); + + /** + * Appends the detection to the velocity init buffer and, once + * VELOCITY_INIT_BUFFER_SIZE samples have accumulated, fits a least-squares + * line through them to initialize the filter's velocity state and covariance. + * + * @param position The ball position for this sample + * @param current_time The timestamp of this sample + * @return true if velocity was initialized this call (buffer just became full) + */ + bool tryInitVelocityFromBuffer(const Point& position, const Timestamp& current_time); + + int consecutive_outliers; + bool velocity_initialized; + KalmanFilter<4, 2, 1> kalman_filter; + std::optional prev_detection_timestamp; + std::optional last_measurement_timestamp; + static constexpr int VELOCITY_INIT_BUFFER_SIZE = 4; + std::vector> velocity_init_buffer; +}; diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp new file mode 100644 index 0000000000..33d8b4f4e1 --- /dev/null +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -0,0 +1,71 @@ +#include "software/sensor_fusion/filter/kalman_filter.h" + +template +KalmanFilter::KalmanFilter() + : state_estimate(Eigen::Vector::Zero()), + state_covariance(Eigen::Matrix::Zero()), + process_model(Eigen::Matrix::Zero()), + process_covariance(Eigen::Matrix::Zero()), + control_model(Eigen::Matrix::Zero()), + measurement_model(Eigen::Matrix::Zero()), + measurement_covariance(Eigen::Matrix::Zero()) +{ +} + +template +KalmanFilter::KalmanFilter( + Eigen::Vector initial_state, + Eigen::Matrix initial_state_covariance, + Eigen::Matrix initial_process_model, + Eigen::Matrix initial_process_covariance, + Eigen::Matrix initial_control_model, + Eigen::Matrix initial_measurement_model, + Eigen::Matrix initial_measurement_covariance) + : state_estimate(initial_state), + state_covariance(initial_state_covariance), + process_model(initial_process_model), + process_covariance(initial_process_covariance), + control_model(initial_control_model), + measurement_model(initial_measurement_model), + measurement_covariance(initial_measurement_covariance) +{ +} + +template +void KalmanFilter::predict(Eigen::Vector control_input) +{ + state_estimate = process_model * state_estimate + control_model * control_input; + state_covariance = + process_model * state_covariance * process_model.transpose() + process_covariance; +} + +template +void KalmanFilter::update(Eigen::Vector measurement) +{ + const Eigen::Vector innovation = + measurement - measurement_model * state_estimate; + + const Eigen::Matrix innovation_covariance = + measurement_model * state_covariance * measurement_model.transpose() + + measurement_covariance; + const Eigen::Matrix regularized_innovation_covariance = + innovation_covariance.unaryExpr( + [](double value) { return (std::abs(value) < 1.0e-20) ? 0.0 : value; }); + + const Eigen::Matrix kalman_gain = + state_covariance * + (measurement_model.transpose() * + regularized_innovation_covariance.completeOrthogonalDecomposition() + .pseudoInverse()); + + state_estimate = state_estimate + kalman_gain * innovation; + + // Joseph form — more numerically stable than P = (I - K*H) * P + const Eigen::Matrix factor = + Eigen::Matrix::Identity() - kalman_gain * measurement_model; + state_covariance = factor * state_covariance * factor.transpose() + + kalman_gain * measurement_covariance * kalman_gain.transpose(); +} + +// Explicit instantiation for the ball tracker +template class KalmanFilter<4, 2, 1>; diff --git a/src/software/sensor_fusion/filter/kalman_filter.h b/src/software/sensor_fusion/filter/kalman_filter.h new file mode 100644 index 0000000000..566faf042b --- /dev/null +++ b/src/software/sensor_fusion/filter/kalman_filter.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include + +/** + * Linear Kalman filter for discrete-time state estimation. + * + * A Kalman filter combines a process model (how the state evolves over time) + * with noisy measurements (what sensors report) to produce an optimal estimate + * of the system state over time (assuming system model and noise are Gaussian). + * + * It alternates between two steps: + * 1) predict: propagate state/covariance forward through the process model + * 2) update: correct that prediction with a new measurement + * + * Resources: + * - https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ + * - https://kalmanfilter.net/ + * - https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python + * - https://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf + * + * @tparam DimX The dimension of the state + * @tparam DimY The dimension of measurement space + * @tparam DimU The dimension of control space + */ +template +class KalmanFilter +{ + public: + /** + * Creates a Kalman filter with all internal matrices and vectors set to zero. + */ + KalmanFilter(); + + /** + * Creates a Kalman filter with the given initial state and model parameters. + * + * @param initial_state Initial state estimate (x) + * @param initial_state_covariance Initial state covariance (P) + * @param initial_process_model Initial process/state transition model (F) + * @param initial_process_covariance Initial process noise covariance (Q) + * @param initial_control_model Initial control-to-state transformation (B) + * @param initial_measurement_model Initial state-to-measurement transformation (H) + * @param initial_measurement_covariance Initial measurement noise covariance (R) + */ + KalmanFilter(Eigen::Vector initial_state, + Eigen::Matrix initial_state_covariance, + Eigen::Matrix initial_process_model, + Eigen::Matrix initial_process_covariance, + Eigen::Matrix initial_control_model, + Eigen::Matrix initial_measurement_model, + Eigen::Matrix initial_measurement_covariance); + + /** + * Predict the next state estimate. + * + * @param control_input Control input vector + */ + void predict(Eigen::Vector control_input); + + /** + * Correct the current state estimate with the given measurement. + * + * @param measurement Measurement vector + */ + void update(Eigen::Vector measurement); + + Eigen::Vector state_estimate; + Eigen::Matrix state_covariance; + Eigen::Matrix process_model; + Eigen::Matrix process_covariance; + Eigen::Matrix control_model; + Eigen::Matrix measurement_model; + Eigen::Matrix measurement_covariance; +}; diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 42296afa2d..f810b7886b 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -1,8 +1,11 @@ #include "software/sensor_fusion/sensor_fusion.h" +#include "software/constants.h" #include "software/geom/algorithms/distance.h" #include "software/logger/logger.h" +static constexpr double PROXIMITY_DRIBBLING_THRESHOLD_METERS = 0.12; + SensorFusion::SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config) : sensor_fusion_config(sensor_fusion_config), field(std::nullopt), @@ -12,7 +15,7 @@ SensorFusion::SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config) game_state(), referee_stage(std::nullopt), dribble_displacement(std::nullopt), - ball_filter(), + ball_tracker(), friendly_team_filter(), enemy_team_filter(), possession(TeamPossession::FRIENDLY_TEAM), @@ -287,18 +290,7 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame& ssl_detection // checked by the member function shouldTrustRobotStatus std::optional robot_with_ball_in_dribbler = friendly_team.getRobotById(friendly_robot_id_with_ball_in_dribbler.value()); - - std::vector dribbler_in_ball_detection = {BallDetection{ - .position = - robot_with_ball_in_dribbler->position() + - Vector::createFromAngle(robot_with_ball_in_dribbler->orientation()) - .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + - BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING), - .distance_from_ground = 0, - .timestamp = Timestamp::fromSeconds(ssl_detection_frame.t_capture()), - .confidence = 1}}; - - std::optional new_ball = createBall(dribbler_in_ball_detection); + std::optional new_ball = createBall({}, Timestamp::fromSeconds(ssl_detection_frame.t_capture()), robot_with_ball_in_dribbler); if (new_ball) { @@ -307,7 +299,26 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame& ssl_detection } else { - std::optional new_ball = createBall(ball_detections); + std::optional proximity_dribbling_robot; + if (ball) + { + std::optional nearest_friendly = friendly_team.getNearestRobot(ball->position()); + if (nearest_friendly.has_value()) + { + Point dribbler_pos = + nearest_friendly->position() + + Vector::createFromAngle(nearest_friendly->orientation()) + .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + + BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING); + if (distance(dribbler_pos, ball->position()) <= + PROXIMITY_DRIBBLING_THRESHOLD_METERS) + { + proximity_dribbling_robot = nearest_friendly; + } + } + } + + std::optional new_ball = createBall(ball_detections, Timestamp::fromSeconds(ssl_detection_frame.t_capture()), proximity_dribbling_robot); if (new_ball) { // If vision detected a new ball, then use that one @@ -349,12 +360,14 @@ void SensorFusion::updateBall(Ball new_ball) } std::optional SensorFusion::createBall( - const std::vector& ball_detections) + const std::vector &ball_detections, const Timestamp& current_time, + std::optional dribbling_robot) { if (field) { std::optional new_ball = - ball_filter.estimateBallState(ball_detections, field.value().fieldBoundary()); + ball_tracker.estimateBallState(ball_detections, field.value().fieldBoundary(), + current_time, dribbling_robot); return new_ball; } return std::nullopt; @@ -512,7 +525,7 @@ void SensorFusion::resetWorldComponents() enemy_team = Team(); game_state = GameState(); referee_stage = std::nullopt; - ball_filter = BallFilter(); + ball_tracker = BallTracker(); friendly_team_filter = RobotTeamFilter(); enemy_team_filter = RobotTeamFilter(); possession = TeamPossession::FRIENDLY_TEAM; diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index 63f47cbada..d6c90169ef 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -7,7 +7,7 @@ #include "proto/message_translation/ssl_referee.h" #include "proto/parameters.pb.h" #include "proto/sensor_msg.pb.h" -#include "software/sensor_fusion/filter/ball_filter.h" +#include "software/sensor_fusion/filter/ball_tracker.h" #include "software/sensor_fusion/filter/robot_team_filter.h" #include "software/sensor_fusion/filter/vision_detection.h" #include "software/sensor_fusion/possession/possession_tracker.h" @@ -95,7 +95,9 @@ class SensorFusion * * @return Ball if filtered from ball detections */ - std::optional createBall(const std::vector& ball_detections); + std::optional createBall(const std::vector &ball_detections, + const Timestamp& current_time, + std::optional dribbling_robot = std::nullopt); /** * Create team from a list of robot detections @@ -191,7 +193,7 @@ class SensorFusion std::optional dribble_displacement; - BallFilter ball_filter; + BallTracker ball_tracker; RobotTeamFilter friendly_team_filter; RobotTeamFilter enemy_team_filter; diff --git a/src/software/simulated_tests/BUILD b/src/software/simulated_tests/BUILD index d1acb78f3e..478e123940 100644 --- a/src/software/simulated_tests/BUILD +++ b/src/software/simulated_tests/BUILD @@ -18,6 +18,7 @@ py_library( "//software:py_constants.so", ], deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", "//proto:import_all_protos", "//software/logger:py_logger", "//software/networking/unix:threaded_unix_listener_py", diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 25e92dce64..6a16809c06 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -1,26 +1,25 @@ -import threading -import queue import argparse -import time -import sys import os +import queue +import sys +import threading +import time +from typing import override import pytest -from proto.import_all_protos import * +from software.py_constants import MILLISECONDS_PER_SECOND -from software.simulated_tests.validation import validation +from proto.import_all_protos import * +from software.logger.logger import create_logger from software.simulated_tests.tbots_test_runner import TbotsTestRunner -from software.thunderscope.thunderscope import Thunderscope -from software.thunderscope.proto_unix_io import ProtoUnixIO -from software.py_constants import MILLISECONDS_PER_SECOND +from software.simulated_tests.validation import validation from software.thunderscope.binary_context_managers.full_system import FullSystem -from software.thunderscope.binary_context_managers.simulator import Simulator from software.thunderscope.binary_context_managers.game_controller import Gamecontroller -from software.thunderscope.thunderscope_config import configure_simulated_test_view +from software.thunderscope.binary_context_managers.simulator import Simulator +from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer - -from software.logger.logger import create_logger -from typing import override +from software.thunderscope.thunderscope import Thunderscope +from software.thunderscope.thunderscope_config import configure_simulated_test_view logger = create_logger(__name__) @@ -200,6 +199,28 @@ def runner( if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) + simulator_state = self.simulator_state_buffer.get( + block=False, return_cached=True + ) + + sf_pos = world.ball.current_state.global_position + sf_vel = world.ball.current_state.global_velocity + if simulator_state is not None: + sim_b = simulator_state.ball + print( + f"[BallState] t={time_elapsed_s:.3f} " + f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " + f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f}) " + f"sim_pos=({sim_b.p_x:.3f},{sim_b.p_y:.3f}) " + f"sim_vel=({sim_b.v_x:.3f},{sim_b.v_y:.3f})" + ) + else: + print( + f"[BallState] t={time_elapsed_s:.3f} " + f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " + f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f})" + ) + # Validate ( eventually_validation_proto_set, @@ -208,6 +229,7 @@ def runner( world, eventually_validation_sequence_set, always_validation_sequence_set, + simulator_state=simulator_state, ) # Set the test name @@ -532,25 +554,29 @@ def simulated_test_runner(): test_name = current_test.split("-")[0] # Launch all binaries - with Simulator( - f"{args.simulator_runtime_dir}/test/{test_name}", - args.debug_simulator, - args.enable_realism, - ) as simulator, FullSystem( - "software/unix_full_system", - f"{args.blue_full_system_runtime_dir}/test/{test_name}", - args.debug_blue_full_system, - False, - should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, - ) as blue_fs, FullSystem( - "software/unix_full_system", - f"{args.yellow_full_system_runtime_dir}/test/{test_name}", - args.debug_yellow_full_system, - True, - should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, - ) as yellow_fs: + with ( + Simulator( + f"{args.simulator_runtime_dir}/test/{test_name}", + args.debug_simulator, + args.enable_realism, + ) as simulator, + FullSystem( + "software/unix_full_system", + f"{args.blue_full_system_runtime_dir}/test/{test_name}", + args.debug_blue_full_system, + False, + should_restart_on_crash=False, + running_in_realtime=args.enable_thunderscope, + ) as blue_fs, + FullSystem( + "software/unix_full_system", + f"{args.yellow_full_system_runtime_dir}/test/{test_name}", + args.debug_yellow_full_system, + True, + should_restart_on_crash=False, + running_in_realtime=args.enable_thunderscope, + ) as yellow_fs, + ): with Gamecontroller( suppress_logs=(not args.show_gamecontroller_logs) ) as gamecontroller: diff --git a/src/software/simulated_tests/tbots_test_runner.py b/src/software/simulated_tests/tbots_test_runner.py index 03e7d4ae85..bc983c0b0e 100644 --- a/src/software/simulated_tests/tbots_test_runner.py +++ b/src/software/simulated_tests/tbots_test_runner.py @@ -1,4 +1,5 @@ from proto.import_all_protos import * +from extlibs.er_force_sim.src.protobuf.world_pb2 import SimulatorState from software.logger.logger import create_logger from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from abc import abstractmethod @@ -46,6 +47,9 @@ def __init__( self.robot_status_buffer = ThreadSafeBuffer( buffer_size=1, protobuf_type=RobotStatus ) + self.simulator_state_buffer = ThreadSafeBuffer( + buffer_size=20, protobuf_type=SimulatorState + ) self.blue_full_system_proto_unix_io.register_observer( SSL_WrapperPacket, self.ssl_wrapper_buffer @@ -53,6 +57,9 @@ def __init__( self.blue_full_system_proto_unix_io.register_observer( RobotStatus, self.robot_status_buffer ) + self.blue_full_system_proto_unix_io.register_observer( + SimulatorState, self.simulator_state_buffer + ) if self.is_yellow_friendly: self.yellow_full_system_proto_unix_io.register_observer( World, self.world_buffer diff --git a/src/software/simulated_tests/validation/BUILD b/src/software/simulated_tests/validation/BUILD index 3bf65f83e6..28f9f9790c 100644 --- a/src/software/simulated_tests/validation/BUILD +++ b/src/software/simulated_tests/validation/BUILD @@ -33,6 +33,7 @@ py_library( "//software:python_bindings.so", ], deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", "//proto:software_py_proto", "//proto:tbots_py_proto", "//software/logger:py_logger", diff --git a/src/software/simulated_tests/validation/avoid_collisions.py b/src/software/simulated_tests/validation/avoid_collisions.py index 41953cd994..44d71805d1 100644 --- a/src/software/simulated_tests/validation/avoid_collisions.py +++ b/src/software/simulated_tests/validation/avoid_collisions.py @@ -21,7 +21,9 @@ def __init__(self): self.fouled_robots = [] @override - def get_validation_status(self, world: World) -> ValidationStatus: + def get_validation_status( + self, world: World, simulator_state=None + ) -> ValidationStatus: """Checks if any 2 robots in the world have collided :param world: the World message to validate diff --git a/src/software/simulated_tests/validation/ball_enters_region.py b/src/software/simulated_tests/validation/ball_enters_region.py index ff6e840f59..7363002664 100644 --- a/src/software/simulated_tests/validation/ball_enters_region.py +++ b/src/software/simulated_tests/validation/ball_enters_region.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -17,18 +18,16 @@ def __init__(self, regions=None): self.ball_position = None @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball enters the provided regions :param world: The world msg to validate :return: FAILING until a ball enters any of the regions PASSING when a ball enters """ - self.ball_position = world.ball.current_state.global_position + self.ball_position = get_ball_pos(world, simulator_state) for region in self.regions: - if tbots_cpp.contains( - region, tbots_cpp.createPoint(world.ball.current_state.global_position) - ): + if tbots_cpp.contains(region, self.ball_position): return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/ball_is_off_ground.py b/src/software/simulated_tests/validation/ball_is_off_ground.py index 880150c838..3b6c84442b 100644 --- a/src/software/simulated_tests/validation/ball_is_off_ground.py +++ b/src/software/simulated_tests/validation/ball_is_off_ground.py @@ -21,7 +21,7 @@ def __init__(self, threshold=0.2): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball has is threshold meters off the ground :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_kicked_in_direction.py b/src/software/simulated_tests/validation/ball_kicked_in_direction.py index 3bd99978ce..b3dab03848 100644 --- a/src/software/simulated_tests/validation/ball_kicked_in_direction.py +++ b/src/software/simulated_tests/validation/ball_kicked_in_direction.py @@ -5,6 +5,8 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, + get_ball_vel, ) from typing import override @@ -33,20 +35,15 @@ def __init__( ) @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball has been kicked in the expected direction :param world: The world msg to validate :return: FAILING if the ball has not been kicked in the expected direction PASSING if the ball has been kicked in the expected direction """ - ball_pos = world.ball.current_state.global_position - ball_vel = world.ball.current_state.global_velocity - - point = tbots_cpp.Point(ball_pos.x_meters, ball_pos.y_meters) - vector = tbots_cpp.Vector( - ball_vel.x_component_meters, ball_vel.y_component_meters - ) + point = get_ball_pos(world, simulator_state) + vector = get_ball_vel(world, simulator_state) ball = tbots_cpp.Ball(point, vector, tbots_cpp.Timestamp()) diff --git a/src/software/simulated_tests/validation/ball_moves_in_direction.py b/src/software/simulated_tests/validation/ball_moves_in_direction.py index 533d818fc7..97d9f74f4e 100644 --- a/src/software/simulated_tests/validation/ball_moves_in_direction.py +++ b/src/software/simulated_tests/validation/ball_moves_in_direction.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -24,14 +25,14 @@ def __init__(self, initial_ball_position: tbots.Point, tolerance: float = 0.1): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if ball is moving forward :param world: The world msg to validate :return: FAILING if ball doesn't move in the direction PASSING if ball moves in the direction """ - current_ball_position = world.ball.current_state.global_position.x_meters + current_ball_position = get_ball_pos(world, simulator_state).x() # if max displacement is not set or current ball is moving in the right direction # set it and return PASSING @@ -84,12 +85,10 @@ def __init__(self, initial_ball_position, regions=[], tolerance: float = 0.1): self.regions = regions @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: for region in self.regions: - if tbots.contains( - region, tbots.createPoint(world.ball.current_state.global_position) - ): - return super().get_validation_status(world) + if tbots.contains(region, get_ball_pos(world, simulator_state)): + return super().get_validation_status(world, simulator_state=simulator_state) return ValidationStatus.PASSING diff --git a/src/software/simulated_tests/validation/ball_speed_threshold.py b/src/software/simulated_tests/validation/ball_speed_threshold.py index f1f8dcec46..76e41b4ea0 100644 --- a/src/software/simulated_tests/validation/ball_speed_threshold.py +++ b/src/software/simulated_tests/validation/ball_speed_threshold.py @@ -7,6 +7,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_vel, ) from typing import override @@ -22,17 +23,14 @@ def __init__(self, speed_threshold): self.speed_threshold = speed_threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball speed is at or above some threshold :param world: The world msg to validate :return: FAILING if the ball speed is below some threshold PASSING if the ball speed is at or above some threshold """ - if ( - tbots_cpp.createVector(world.ball.current_state.global_velocity).length() - >= self.speed_threshold - ): + if get_ball_vel(world, simulator_state).length() >= self.speed_threshold: return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/ball_stops_in_region.py b/src/software/simulated_tests/validation/ball_stops_in_region.py index 38b0312481..e8d6273e09 100644 --- a/src/software/simulated_tests/validation/ball_stops_in_region.py +++ b/src/software/simulated_tests/validation/ball_stops_in_region.py @@ -5,6 +5,8 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, + get_ball_vel, ) from typing import override @@ -16,7 +18,7 @@ def __init__(self, regions=None): self.regions = regions if regions else [] @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball stops in the provided regions :param world: The world msg to validate @@ -25,13 +27,8 @@ def get_validation_status(self, world) -> ValidationStatus: """ for region in self.regions: if tbots_cpp.contains( - region, tbots_cpp.createPoint(world.ball.current_state.global_position) - ) and ( - tbots_cpp.createVector( - world.ball.current_state.global_velocity - ).length() - <= 0.01 - ): + region, get_ball_pos(world, simulator_state) + ) and get_ball_vel(world, simulator_state).length() <= 0.01: return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/delay_validation.py b/src/software/simulated_tests/validation/delay_validation.py index b51725f7d4..1c23c16ea7 100644 --- a/src/software/simulated_tests/validation/delay_validation.py +++ b/src/software/simulated_tests/validation/delay_validation.py @@ -16,7 +16,7 @@ def __init__(self, delay_s, validation): self.validation = validation @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks validation after delay finished. If during delay, returns default validation status. :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/duration_validation.py b/src/software/simulated_tests/validation/duration_validation.py index 1020e024d2..0cc4d4bdb3 100644 --- a/src/software/simulated_tests/validation/duration_validation.py +++ b/src/software/simulated_tests/validation/duration_validation.py @@ -23,7 +23,7 @@ def __init__(self, duration_s, validation): self.validation = validation @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if validation has been consecutively PASSING for a duration. :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/excessive_dribbling.py b/src/software/simulated_tests/validation/excessive_dribbling.py index 98a386b055..d3f1ea70ca 100644 --- a/src/software/simulated_tests/validation/excessive_dribbling.py +++ b/src/software/simulated_tests/validation/excessive_dribbling.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -19,7 +20,7 @@ def __init__(self): self.dribbling_error_margin = 0.05 @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if any friendly robot is excessively dribbling the ball past the max dribble displacement minus the dribbling error margin @@ -27,7 +28,7 @@ def get_validation_status(self, world) -> ValidationStatus: :return: FAILING when the robot is excessively dribbling PASSING when the robot is not excessively dribbling """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_position = get_ball_pos(world, simulator_state) for robot in world.friendly_team.team_robots: if tbots_cpp.Robot(robot).isNearDribbler( ball_position, self.dribbler_tolerance diff --git a/src/software/simulated_tests/validation/friendly_has_ball_possession.py b/src/software/simulated_tests/validation/friendly_has_ball_possession.py index bf1013f7ed..292c281bc6 100644 --- a/src/software/simulated_tests/validation/friendly_has_ball_possession.py +++ b/src/software/simulated_tests/validation/friendly_has_ball_possession.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -22,14 +23,14 @@ def __init__(self, robot_id=None, tolerance=0.01): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if friendly robot has possession of the ball :param world: The world msg to validate :return: FAILING when friendly robot does not have possession of the ball PASSING when friendly robot has possession of the ball """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_position = get_ball_pos(world, simulator_state) for robot in world.friendly_team.team_robots: if self.robot_id is not None and robot.id != self.robot_id: continue diff --git a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py index 219271e001..14d8420c0d 100644 --- a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py +++ b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py @@ -3,6 +3,7 @@ from software.simulated_tests.validation.validation import ( create_validation_types, + get_ball_vel, ) from software.simulated_tests.validation.friendly_has_ball_possession import ( FriendlyHasBallPossession, @@ -24,7 +25,7 @@ def __init__(self, robot_id, max_receive_speed): self.max_receive_speed = max_receive_speed @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the specified robot receives the ball too fast :param world: The world msg to validate @@ -33,10 +34,8 @@ def get_validation_status(self, world) -> ValidationStatus: PASSING if the ball is not near the dribbler, or if it is near the dribbler at a speed slower than the max """ - if super().get_validation_status(world) == ValidationStatus.PASSING: - ball_velocity = tbots_cpp.createVector( - world.ball.current_state.global_velocity - ) + if super().get_validation_status(world, simulator_state=simulator_state) == ValidationStatus.PASSING: + ball_velocity = get_ball_vel(world, simulator_state) if ball_velocity.length() - self.max_receive_speed > 0.2: return ValidationStatus.FAILING return ValidationStatus.PASSING diff --git a/src/software/simulated_tests/validation/friendly_team_scored.py b/src/software/simulated_tests/validation/friendly_team_scored.py index 458502f152..f0f4912ec6 100644 --- a/src/software/simulated_tests/validation/friendly_team_scored.py +++ b/src/software/simulated_tests/validation/friendly_team_scored.py @@ -16,7 +16,7 @@ def __init__(self): self.region = tbots_cpp.Field.createSSLDivisionBField().enemyGoal() @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball enters the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/or_validation.py b/src/software/simulated_tests/validation/or_validation.py index b14affe8c3..110b2b66de 100644 --- a/src/software/simulated_tests/validation/or_validation.py +++ b/src/software/simulated_tests/validation/or_validation.py @@ -17,9 +17,12 @@ def __init__(self, validations): self.validations = validations @override - def get_validation_status(self, world): + def get_validation_status(self, world, simulator_state=None): for validation in self.validations: - if validation.get_validation_status(world) == ValidationStatus.PASSING: + if ( + validation.get_validation_status(world, simulator_state=simulator_state) + == ValidationStatus.PASSING + ): return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/robot_at_angular_velocity.py b/src/software/simulated_tests/validation/robot_at_angular_velocity.py index f9fec65db8..7650762bde 100644 --- a/src/software/simulated_tests/validation/robot_at_angular_velocity.py +++ b/src/software/simulated_tests/validation/robot_at_angular_velocity.py @@ -24,7 +24,7 @@ def __init__(self, robot_id, angular_velocity, threshold=0.1): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the robot is at the expected angular velocity :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_at_orientation.py b/src/software/simulated_tests/validation/robot_at_orientation.py index df3c113257..a30458203b 100644 --- a/src/software/simulated_tests/validation/robot_at_orientation.py +++ b/src/software/simulated_tests/validation/robot_at_orientation.py @@ -26,7 +26,7 @@ def __init__(self, robot_id, orientation, threshold=0.1): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the robot is at the expected orientation :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_at_position.py b/src/software/simulated_tests/validation/robot_at_position.py index 0637585254..9dc8a9644c 100644 --- a/src/software/simulated_tests/validation/robot_at_position.py +++ b/src/software/simulated_tests/validation/robot_at_position.py @@ -27,7 +27,7 @@ def __init__( self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the robot is at the target position :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_placement_region.py b/src/software/simulated_tests/validation/robot_enters_placement_region.py index db6275e1f2..296d1da958 100644 --- a/src/software/simulated_tests/validation/robot_enters_placement_region.py +++ b/src/software/simulated_tests/validation/robot_enters_placement_region.py @@ -26,7 +26,7 @@ def __init__(self, placement_point): ) @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if robots enter the ball placement region :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_region.py b/src/software/simulated_tests/validation/robot_enters_region.py index d5d6584487..6b1b3a553a 100644 --- a/src/software/simulated_tests/validation/robot_enters_region.py +++ b/src/software/simulated_tests/validation/robot_enters_region.py @@ -24,7 +24,7 @@ def __init__(self, regions, req_robot_cnt): self.robot_in_zone = {} @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if a specific number of robots enter the provided set of regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_region_and_stops.py b/src/software/simulated_tests/validation/robot_enters_region_and_stops.py index 5cc4a5b052..ebdf6296fd 100644 --- a/src/software/simulated_tests/validation/robot_enters_region_and_stops.py +++ b/src/software/simulated_tests/validation/robot_enters_region_and_stops.py @@ -32,7 +32,7 @@ def __init__(self, regions=None, num_ticks=1): self.passing_robot_id = None @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if a robot is in the provided region Then checks if that robot is stationary within a threshold for the provided number of ticks diff --git a/src/software/simulated_tests/validation/robot_received_ball.py b/src/software/simulated_tests/validation/robot_received_ball.py index 674f75c800..a5599660f5 100644 --- a/src/software/simulated_tests/validation/robot_received_ball.py +++ b/src/software/simulated_tests/validation/robot_received_ball.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -22,14 +23,14 @@ def __init__(self, robot_id, tolerance=0.02): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the specific robot has received the ball :param world: The world msg to validate :return: FAILING when the robot does not have the ball PASSING when the robot has the ball """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_position = get_ball_pos(world, simulator_state) for robot in world.friendly_team.team_robots: if robot.id == self.robot_id: if tbots_cpp.Robot(robot).isNearDribbler(ball_position, self.tolerance): diff --git a/src/software/simulated_tests/validation/robot_speed_threshold.py b/src/software/simulated_tests/validation/robot_speed_threshold.py index 19e20db786..c7a679849c 100644 --- a/src/software/simulated_tests/validation/robot_speed_threshold.py +++ b/src/software/simulated_tests/validation/robot_speed_threshold.py @@ -23,7 +23,7 @@ def __init__(self, speed_threshold): self.speed_threshold = speed_threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the friendly robots' speed is at or above some threshold :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/validation.py b/src/software/simulated_tests/validation/validation.py index 3b5e70d538..cea43656b7 100644 --- a/src/software/simulated_tests/validation/validation.py +++ b/src/software/simulated_tests/validation/validation.py @@ -4,11 +4,25 @@ from abc import abstractmethod +def get_ball_pos(world, simulator_state=None): + """Return true ball position from simulator if available, else from world.""" + if simulator_state is not None and simulator_state.HasField("ball"): + return tbots_cpp.Point(simulator_state.ball.p_x, simulator_state.ball.p_y) + return tbots_cpp.createPoint(world.ball.current_state.global_position) + + +def get_ball_vel(world, simulator_state=None): + """Return true ball velocity from simulator if available, else from world.""" + if simulator_state is not None and simulator_state.HasField("ball"): + return tbots_cpp.Vector(simulator_state.ball.v_x, simulator_state.ball.v_y) + return tbots_cpp.createVector(world.ball.current_state.global_velocity) + + class Validation: """A validation function""" @abstractmethod - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: raise NotImplementedError("get_validation_status is not implemented") @abstractmethod @@ -66,15 +80,16 @@ def constructor(self, *args, **kwargs): """ self.validation = validation_class(*args, **kwargs) - def flip_validation(self, world): + def flip_validation(self, world, simulator_state=None): """Flip the validation status :param world: The world msg to validate on + :param simulator_state: The simulator state with true positions, or None """ return { ValidationStatus.FAILING: ValidationStatus.PASSING, ValidationStatus.PASSING: ValidationStatus.FAILING, - }[self.validation.get_validation_status(world)] + }[self.validation.get_validation_status(world, simulator_state=simulator_state)] # Generate the types: specifically, all Eventually validations will return # EVENTUALLY when get_validation_type is called, and all Always validations @@ -95,7 +110,10 @@ def flip_validation(self, world): + repr(self.validation), "get_validation_type": lambda self: ValidationType.EVENTUALLY, "get_validation_status": lambda self, - world: self.validation.get_validation_status(world), + world, + simulator_state=None: self.validation.get_validation_status( + world, simulator_state=simulator_state + ), }, ) @@ -107,7 +125,11 @@ def flip_validation(self, world): "__repr__": lambda self: "EventuallyFalseValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.EVENTUALLY, - "get_validation_status": lambda self, world: flip_validation(self, world), + "get_validation_status": lambda self, + world, + simulator_state=None: flip_validation( + self, world, simulator_state=simulator_state + ), }, ) @@ -119,7 +141,10 @@ def flip_validation(self, world): "__repr__": lambda self: "AlwaysTrueValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.ALWAYS, "get_validation_status": lambda self, - world: self.validation.get_validation_status(world), + world, + simulator_state=None: self.validation.get_validation_status( + world, simulator_state=simulator_state + ), }, ) @@ -130,7 +155,11 @@ def flip_validation(self, world): **common, "__repr__": lambda self: "AlwaysFalseValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.ALWAYS, - "get_validation_status": lambda self, world: flip_validation(self, world), + "get_validation_status": lambda self, + world, + simulator_state=None: flip_validation( + self, world, simulator_state=simulator_state + ), }, ) @@ -138,7 +167,10 @@ def flip_validation(self, world): def run_validation_sequence_sets( - world, eventually_validation_sequence_set, always_validation_sequence_set + world, + eventually_validation_sequence_set, + always_validation_sequence_set, + simulator_state=None, ): """Given both eventually and always validation sequence sets, (and world) run validation and aggregate the results in a validation proto set. @@ -170,7 +202,9 @@ def create_validation_proto_helper(validation_proto_set, validation): validation_proto = ValidationProto() # Get status - status = validation.get_validation_status(world) + status = validation.get_validation_status( + world, simulator_state=simulator_state + ) # Create validation proto validation_proto.status = status diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..9d143a71a4 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -122,7 +122,7 @@ std::unique_ptr ErForceSimulator::createRealisticRealismCo realism_config->set_vision_delay(35000000); realism_config->set_vision_processing_time(10000000); realism_config->set_missing_ball_detections(0.02f); - realism_config->set_simulate_dribbling(false); + realism_config->set_simulate_dribbling(true); return realism_config; } @@ -472,11 +472,13 @@ void ErForceSimulator::stepSimulation(const Duration& time_step) blue_robot_with_ball.reset(); yellow_robot_with_ball.reset(); + ball_is_visible_ = true; for (const auto& response : yellow_radio_responses) { if (response.has_ball_detected() && response.ball_detected()) { yellow_robot_with_ball = response.id(); + ball_is_visible_ = false; } } @@ -485,6 +487,7 @@ void ErForceSimulator::stepSimulation(const Duration& time_step) if (response.has_ball_detected() && response.ball_detected()) { blue_robot_with_ball = response.id(); + ball_is_visible_ = false; } } @@ -546,7 +549,8 @@ std::vector ErForceSimulator::getSSLWrapperPackets( world::SimulatorState ErForceSimulator::getSimulatorState() const { - return er_force_sim->getSimulatorState(); + auto state = er_force_sim->getSimulatorState(); + return state; } Field ErForceSimulator::getField() const @@ -564,6 +568,11 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } +bool ErForceSimulator::isBallVisible() const +{ + return ball_is_visible_; +} + std::map> ErForceSimulator::getRobotIdToLocalVelocityMap( const google::protobuf::RepeatedPtrField& sim_robots) diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..b98bfc7f3b 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -130,6 +130,11 @@ class ErForceSimulator */ void resetCurrentTime(); + /** + * Returns whether the ball is currently visible (not occluded by a robot) + */ + bool isBallVisible() const; + /** * Creates the default realism config using erforce simulator's default config * @return a pointer to default realism config @@ -229,7 +234,9 @@ class ErForceSimulator std::optional yellow_robot_with_ball; bool ramping; + bool ball_is_visible_ = true; const std::string CONFIG_FILE = "simulator/2020"; const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; + bool is_ball_visible; }; diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index a57f298ba8..ddfc700c03 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -29,7 +29,8 @@ class Gamecontroller: """Gamecontroller Context Manager""" - CI_MODE_LAUNCH_DELAY_S = 0.3 + CI_MODE_LAUNCH_DELAY_S = 0.1 + CI_MODE_CONNECT_TIMEOUT_S = 5.0 CI_MODE_OUTPUT_RECEIVE_BUFFER_SIZE = 9000 REFEREE_IP = "224.5.23.1" RESET_MATCH_DELAY_S = 1 @@ -99,17 +100,45 @@ def __enter__(self) -> Gamecontroller: command += ["-publishAddress", f"{self.REFEREE_IP}:{self.referee_port}"] command += ["-ciAddress", f"localhost:{self.ci_port}"] - if self.suppress_logs: - with open(os.devnull, "w") as fp: - self.gamecontroller_proc = Popen(command, stdout=fp, stderr=fp) + # Run in a temp dir so the gamecontroller always starts with a clean + # state store — the state-store.json.stream can get corrupted when the + # process is killed mid-write, causing a panic on the next startup. + import tempfile + self._gc_workdir = tempfile.mkdtemp(prefix="gamecontroller_") + if self.suppress_logs: + self._gc_log = open( + os.path.join(self._gc_workdir, "gamecontroller.log"), "w" + ) + self.gamecontroller_proc = Popen( + command, stdout=self._gc_log, stderr=self._gc_log, + cwd=self._gc_workdir, + ) else: - self.gamecontroller_proc = Popen(command) + self._gc_log = None + self.gamecontroller_proc = Popen(command, cwd=self._gc_workdir) - # We can't connect to the ci port right away, it takes - # CI_MODE_LAUNCH_DELAY_S to start up the gamecontroller - time.sleep(Gamecontroller.CI_MODE_LAUNCH_DELAY_S) - self.ci_socket = SslSocket(self.ci_port) + # Poll until the gamecontroller CI port is ready, up to CI_MODE_CONNECT_TIMEOUT_S + deadline = time.time() + Gamecontroller.CI_MODE_CONNECT_TIMEOUT_S + while True: + if self.gamecontroller_proc.poll() is not None: + log_contents = "" + log_path = os.path.join(self._gc_workdir, "gamecontroller.log") + if os.path.exists(log_path): + with open(log_path) as f: + log_contents = f.read() + raise RuntimeError( + f"Gamecontroller process exited with code {self.gamecontroller_proc.returncode} " + f"before CI port {self.ci_port} became available" + + (f"\nGamecontroller output:\n{log_contents}" if log_contents else "") + ) + try: + self.ci_socket = SslSocket(self.ci_port) + break + except ConnectionRefusedError: + if time.time() >= deadline: + raise + time.sleep(Gamecontroller.CI_MODE_LAUNCH_DELAY_S) return self @@ -123,6 +152,10 @@ def __exit__(self, type, value, traceback) -> None: self.gamecontroller_proc.terminate() self.gamecontroller_proc.wait() self.ci_socket.close() + if self._gc_log is not None: + self._gc_log.close() + import shutil + shutil.rmtree(self._gc_workdir, ignore_errors=True) def refresh(self): """Gets any manual gamecontroller commands from the buffer and executes them""" @@ -301,6 +334,8 @@ def send_ci_input(self, ci_input: proto.ssl_gc_ci_pb2.CiInput) -> list[CiOutput] "error receiving CiOutput proto from the gamecontroller: " + parse_err.args ) + except OSError: + break return ci_output_list diff --git a/src/software/thunderscope/binary_context_managers/simulator.py b/src/software/thunderscope/binary_context_managers/simulator.py index 20cf4b6853..106b394611 100644 --- a/src/software/thunderscope/binary_context_managers/simulator.py +++ b/src/software/thunderscope/binary_context_managers/simulator.py @@ -134,6 +134,10 @@ def setup_proto_unix_io( WorldStateReceivedTrigger, ) + simulator_proto_unix_io.attach_unix_receiver( + self.simulator_runtime_dir, "/log", RobotLog + ) + # setup blue full system unix io for arg in [ (BLUE_WORLD_PATH, World), diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 1b8a81ffff..d399b2667d 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -7,33 +7,31 @@ import google.protobuf from google.protobuf.internal import api_implementation +from software.py_constants import * +import software.thunderscope.thunderscope_config as config +from proto.import_all_protos import * +from software.thunderscope.binary_context_managers import * +from software.thunderscope.binary_context_managers.full_system import FullSystem +from software.thunderscope.binary_context_managers.game_controller import Gamecontroller from software.thunderscope.binary_context_managers.runtime_manager import ( runtime_manager_instance, ) -from software.thunderscope.log.stats.stats import Stats - -from software.thunderscope.thunderscope import Thunderscope -from software.thunderscope.constants import LogLevels -from software.thunderscope.binary_context_managers import * -from proto.import_all_protos import * -from software.py_constants import * -from software.thunderscope.robot_communication import RobotCommunication -from software.thunderscope.wifi_communication_manager import WifiCommunicationManager +from software.thunderscope.binary_context_managers.simulator import Simulator +from software.thunderscope.binary_context_managers.tigers_autoref import TigersAutoref from software.thunderscope.constants import ( + CI_DURATION_S, EstopMode, + LogLevels, ProtoUnixIOTypes, ) from software.thunderscope.estop_helpers import get_estop_config +from software.thunderscope.log.stats.stats import Stats from software.thunderscope.proto_unix_io import ProtoUnixIO -import software.thunderscope.thunderscope_config as config -from software.thunderscope.constants import CI_DURATION_S +from software.thunderscope.robot_communication import RobotCommunication +from software.thunderscope.thunderscope import Thunderscope from software.thunderscope.util import * - -from software.thunderscope.binary_context_managers.full_system import FullSystem -from software.thunderscope.binary_context_managers.simulator import Simulator -from software.thunderscope.binary_context_managers.game_controller import Gamecontroller -from software.thunderscope.binary_context_managers.tigers_autoref import TigersAutoref +from software.thunderscope.wifi_communication_manager import WifiCommunicationManager protobuf_impl_type = api_implementation.Type() assert protobuf_impl_type == "upb", ( @@ -302,26 +300,30 @@ ) with ( - Gamecontroller( - suppress_logs=(not args.verbose), - use_conventional_port=False, - ) - if args.launch_gc - else contextlib.nullcontext() - ) as gamecontroller, WifiCommunicationManager( - current_proto_unix_io=current_proto_unix_io, - multicast_channel=getRobotMulticastChannel(args.channel), - should_setup_full_system=(args.run_blue or args.run_yellow), - interface=args.interface, - referee_port=gamecontroller.get_referee_port() - if gamecontroller - else SSL_REFEREE_PORT, - ) as wifi_communication_manager, RobotCommunication( - current_proto_unix_io=current_proto_unix_io, - communication_manager=wifi_communication_manager, - estop_mode=estop_mode, - estop_path=estop_path, - ) as robot_communication: + ( + Gamecontroller( + suppress_logs=(not args.verbose), + use_conventional_port=False, + ) + if args.launch_gc + else contextlib.nullcontext() + ) as gamecontroller, + WifiCommunicationManager( + current_proto_unix_io=current_proto_unix_io, + multicast_channel=getRobotMulticastChannel(args.channel), + should_setup_full_system=(args.run_blue or args.run_yellow), + interface=args.interface, + referee_port=gamecontroller.get_referee_port() + if gamecontroller + else SSL_REFEREE_PORT, + ) as wifi_communication_manager, + RobotCommunication( + current_proto_unix_io=current_proto_unix_io, + communication_manager=wifi_communication_manager, + estop_mode=estop_mode, + estop_path=estop_path, + ) as robot_communication, + ): if estop_mode == EstopMode.KEYBOARD_ESTOP: tscope.keyboard_estop_shortcut.activated.connect( robot_communication.toggle_keyboard_estop @@ -333,12 +335,12 @@ if args.run_blue or args.run_yellow: with FullSystem( - path_to_binary=runtime_config.get_blue_runtime_path(), + path_to_binary="software/unix_full_system", full_system_runtime_dir=runtime_dir, debug_full_system=debug, friendly_colour_yellow=friendly_colour_yellow, should_restart_on_crash=True, - run_sudo=args.sudo, + run_sudo=True, log_level=args.log_level, ) as full_system: full_system.setup_proto_unix_io(current_proto_unix_io) @@ -412,54 +414,62 @@ def __ticker(tick_rate_ms: int) -> None: runtime_config = runtime_manager_instance.fetch_runtime_config() # Launch all binaries - with Simulator( - args.simulator_runtime_dir, args.debug_simulator, args.enable_realism - ) as simulator, FullSystem( - path_to_binary=runtime_config.get_blue_runtime_path(), - full_system_runtime_dir=args.blue_full_system_runtime_dir, - debug_full_system=args.debug_blue_full_system, - friendly_colour_yellow=False, - should_restart_on_crash=False, - run_sudo=args.sudo, - running_in_realtime=(not args.ci_mode), - log_level=args.log_level, - ) as blue_fs, FullSystem( - path_to_binary=runtime_config.get_yellow_runtime_path(), - full_system_runtime_dir=args.yellow_full_system_runtime_dir, - debug_full_system=args.debug_yellow_full_system, - friendly_colour_yellow=True, - should_restart_on_crash=False, - run_sudo=args.sudo, - running_in_realtime=(not args.ci_mode), - log_level=args.log_level, - ) as yellow_fs, Gamecontroller( - suppress_logs=(not args.verbose), - automate_referee=args.enable_autogc, - ) as gamecontroller, ( - # Here we only initialize autoref if the --enable_autoref flag is requested. - # To avoid nested Python withs, the autoref is initialized as None when this flag doesn't exist. - # All calls to autoref should be guarded with args.enable_autoref - TigersAutoref( - ci_mode=True, - gc=gamecontroller, + with ( + Simulator( + args.simulator_runtime_dir, args.debug_simulator, args.enable_realism + ) as simulator, + FullSystem( + path_to_binary=runtime_config.get_blue_runtime_path(), + full_system_runtime_dir=args.blue_full_system_runtime_dir, + debug_full_system=args.debug_blue_full_system, + friendly_colour_yellow=False, + should_restart_on_crash=False, + run_sudo=args.sudo, + running_in_realtime=(not args.ci_mode), + log_level=args.log_level, + ) as blue_fs, + FullSystem( + path_to_binary=runtime_config.get_yellow_runtime_path(), + full_system_runtime_dir=args.yellow_full_system_runtime_dir, + debug_full_system=args.debug_yellow_full_system, + friendly_colour_yellow=True, + should_restart_on_crash=False, + run_sudo=args.sudo, + running_in_realtime=(not args.ci_mode), + log_level=args.log_level, + ) as yellow_fs, + Gamecontroller( suppress_logs=(not args.verbose), - tick_rate_ms=DEFAULT_SIMULATOR_TICK_RATE_MILLISECONDS_PER_TICK, - show_gui=args.show_autoref_gui, - ) - if args.enable_autoref - else contextlib.nullcontext() - ) as autoref, ( - Stats( - proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.BLUE], - record_enemy_stats=True, - ) - if args.record_stats - else contextlib.nullcontext() - ) as blue_stats, ( - Stats(proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.YELLOW]) - if args.record_stats - else contextlib.nullcontext() - ) as yellow_stats: + automate_referee=args.enable_autogc, + ) as gamecontroller, + ( + # Here we only initialize autoref if the --enable_autoref flag is requested. + # To avoid nested Python withs, the autoref is initialized as None when this flag doesn't exist. + # All calls to autoref should be guarded with args.enable_autoref + TigersAutoref( + ci_mode=True, + gc=gamecontroller, + suppress_logs=(not args.verbose), + tick_rate_ms=DEFAULT_SIMULATOR_TICK_RATE_MILLISECONDS_PER_TICK, + show_gui=args.show_autoref_gui, + ) + if args.enable_autoref + else contextlib.nullcontext() + ) as autoref, + ( + Stats( + proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.BLUE], + record_enemy_stats=True, + ) + if args.record_stats + else contextlib.nullcontext() + ) as blue_stats, + ( + Stats(proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.YELLOW]) + if args.record_stats + else contextlib.nullcontext() + ) as yellow_stats, + ): tscope.register_refresh_function(gamecontroller.refresh) autoref_proto_unix_io = ProtoUnixIO() diff --git a/src/software/thunderscope/wifi_communication_manager.py b/src/software/thunderscope/wifi_communication_manager.py index 7cb80f25b3..09f511f815 100644 --- a/src/software/thunderscope/wifi_communication_manager.py +++ b/src/software/thunderscope/wifi_communication_manager.py @@ -78,6 +78,10 @@ def __init__( referee_interface=DISCONNECTED, vision_interface=DISCONNECTED, ) + ## Thread Management ## + self.running = True + self.broadcast_ip: Thread | None = None + if interface: self.accept_next_network_config = False self.__setup_robot_communication(interface) @@ -89,10 +93,6 @@ def __init__( referee_interface=interface, vision_interface=interface ) - ## Thread Management ## - self.running = True - self.broadcast_ip: Thread | None = None - logger.debug("[WifiCommunicationManager] Initialized") self.__print_current_network_config()