From 42172bf57568c457aa61708e7330455f061ce514 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 00:33:23 -0700 Subject: [PATCH 01/20] init merge --- .../message_translation/tbots_protobuf.cpp | 16 +- .../message_translation/tbots_protobuf.h | 11 +- .../tbots_protobuf_test.cpp | 4 +- src/shared/constants.h | 3 + src/shared/robot_constants.h | 86 +++-- src/software/ai/evaluation/intercept.cpp | 2 +- .../ai/hl/stp/tactic/move_primitive.cpp | 2 +- src/software/ai/navigator/trajectory/BUILD | 1 + .../trajectory/bang_bang_trajectory_1d.h | 14 + .../bang_bang_trajectory_1d_angular.h | 13 + .../ai/navigator/trajectory/trajectory.hpp | 10 + .../ai/navigator/trajectory/trajectory_2d.h | 14 + src/software/constants.h | 2 +- src/software/embedded/BUILD | 23 ++ src/software/embedded/motion_control/BUILD | 3 - .../embedded/motion_control/controller.h | 2 + .../motion_control/orientation_controller.h | 4 +- .../motion_control/pid_controller.cpp | 27 -- .../embedded/motion_control/pid_controller.h | 62 +++- .../motion_control/pid_controller_test.cpp | 27 +- .../motion_control/position_controller.h | 8 +- .../stspin_motor_controller.cpp | 7 +- src/software/embedded/primitive_executor.cpp | 196 ++++++---- src/software/embedded/primitive_executor.h | 88 ++--- src/software/embedded/robot_localizer.cpp | 306 ++++++++++++++++ src/software/embedded/robot_localizer.h | 178 +++++++++ src/software/embedded/services/BUILD | 19 + src/software/embedded/spi_utils.h | 7 +- src/software/embedded/thunderloop.cpp | 345 +++++++++--------- src/software/embedded/thunderloop.h | 41 ++- .../field_tests/field_test_fixture.py | 120 ++++-- .../field_tests/movement_robot_field_test.py | 191 +++++----- src/software/logger/network_logger.cpp | 13 +- .../simulation/er_force_simulator.cpp | 75 ++-- src/software/simulation/er_force_simulator.h | 13 +- src/software/util/pid/BUILD | 8 + src/software/util/pid/pid_controller.hpp | 82 +++++ src/software/world/robot.cpp | 3 +- 38 files changed, 1423 insertions(+), 603 deletions(-) delete mode 100644 src/software/embedded/motion_control/pid_controller.cpp create mode 100644 src/software/embedded/robot_localizer.cpp create mode 100644 src/software/embedded/robot_localizer.h create mode 100644 src/software/util/pid/BUILD create mode 100644 src/software/util/pid/pid_controller.hpp diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..23e7d0c51c 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -425,7 +425,8 @@ std::unique_ptr createCostVisualization( } std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants) { double max_speed = convertMaxAllowedSpeedModeToMaxAllowedSpeed( @@ -449,8 +450,7 @@ std::optional createTrajectoryPathFromParams( } auto trajectory = std::make_shared( - createPoint(params.start_position()), initial_destination, initial_velocity, - constraints); + start_position, initial_destination, initial_velocity, constraints); TrajectoryPath trajectory_path(trajectory, BangBangTrajectory2D::generator); @@ -475,14 +475,14 @@ std::optional createTrajectoryPathFromParams( } BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants) { return BangBangTrajectory1DAngular( - createAngle(params.start_angle()), createAngle(params.final_angle()), - initial_velocity, - AngularVelocity::fromRadians(robot_constants.robot_max_ang_speed_rad_per_s), + start_angle, createAngle(params.final_angle()), initial_velocity, + AngularVelocity::fromRadians( + robot_constants.robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot_constants.robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_m_per_s; + return robot_constants.robot_max_speed_trajectory_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/proto/message_translation/tbots_protobuf.h b/src/proto/message_translation/tbots_protobuf.h index 5976da5fcc..2831857e02 100644 --- a/src/proto/message_translation/tbots_protobuf.h +++ b/src/proto/message_translation/tbots_protobuf.h @@ -238,25 +238,28 @@ std::unique_ptr createCostVisualization( * Generate a 2D Trajectory Path given 2D trajectory parameters * * @param params 2D Trajectory Path + * @param start_position Initial position to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return TrajectoryPath, or std::nullopt if the trajectory path could not be created * from the given parameters */ std::optional createTrajectoryPathFromParams( - const TbotsProto::TrajectoryPathParams2D& params, const Vector& initial_velocity, + const TbotsProto::TrajectoryPathParams2D& params, const Point& start_position, + const Vector& initial_velocity, const robot_constants::RobotConstants& robot_constants); /** - * Generate an angular trajectory Path given angular trajectory proto parameters + * Generate an angular trajectory path given angular trajectory proto parameters * - * @param params angular Trajectory Path + * @param params Angular trajectory path + * @param start_angle Initial angle to use for the trajectory * @param initial_velocity Initial velocity to use for the trajectory * @param robot_constants Constants to use for the trajectory * @return Generate angular trajectory */ BangBangTrajectory1DAngular createAngularTrajectoryFromParams( - const TbotsProto::TrajectoryParamsAngular1D& params, + const TbotsProto::TrajectoryParamsAngular1D& params, const Angle& start_angle, const AngularVelocity& initial_velocity, const robot_constants::RobotConstants& robot_constants); diff --git a/src/proto/message_translation/tbots_protobuf_test.cpp b/src/proto/message_translation/tbots_protobuf_test.cpp index 0f82de69d6..2735d57a3b 100644 --- a/src/proto/message_translation/tbots_protobuf_test.cpp +++ b/src/proto/message_translation/tbots_protobuf_test.cpp @@ -209,8 +209,8 @@ TEST_P(TrajectoryParamConversionTest, trajectory_params_msg_test) *(params.add_sub_destinations()) = sub_destination_proto; } - auto converted_trajectory_path_opt = - createTrajectoryPathFromParams(params, initial_velocity, robot_constants); + auto converted_trajectory_path_opt = createTrajectoryPathFromParams( + params, createPoint(params.start_position()), initial_velocity, robot_constants); ASSERT_TRUE(converted_trajectory_path_opt.has_value()); TrajectoryPath converted_trajectory_path = converted_trajectory_path_opt.value(); diff --git a/src/shared/constants.h b/src/shared/constants.h index ed94c0cde9..e02cfb1b58 100644 --- a/src/shared/constants.h +++ b/src/shared/constants.h @@ -224,6 +224,9 @@ static const unsigned THUNDERLOOP_HZ = 300u; static const unsigned NUM_GENEVA_ANGLES = 5; + +static constexpr double RTT_S = 0.03; + // Robot diagnostics constants constexpr double AUTO_CHIP_DISTANCE_DEFAULT_M = 1.5; constexpr double AUTO_KICK_SPEED_DEFAULT_M_PER_S = 1.5; diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 00a77c94da..620861d8aa 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -21,21 +21,21 @@ struct RobotConstants // The angles are assumed to be symmetric for the left and right sides of the robot. // // FRONT OF ROBOT - // | ▲ X-Axis - // | / -------+--------- \ eric - // |A / .' │ '. \ ◄─── Front wheel - // | /.' │ .'.\ grayson - // |// │ . \\ samuel - // ; │ . Lever ; - // | │ . Arm | - // ◄─────┼──────────────┼ | - // Y-Axis| | - // ; ; - // |\\ // - // | \'. .'/ - // |B \ '. .' / ◄─── Back wheel - // | \ '-. _.-' / - // | ''------'' + // | ▲ X-Axis // + // | / -------+--------- \ // + // |A / .' │ '. \ ◄─── Front wheel // + // | /.' │ .'.\ // + // |// │ . |\ // + // ; │ . Lever ; // + // | │ . Arm | // + // ◄─────┼──────────────┼ | // + // Y-Axis| | // + // ; ; // + // |\\ // // + // | \'. .'/ // + // |B \ '. .' / ◄─── Back wheel // + // | \ '-. _.-' / // + // | ''------'' // // clang-format on @@ -88,6 +88,10 @@ struct RobotConstants // The maximum speed achievable by our robots, in metres per second [m/s] float robot_max_speed_m_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] + float robot_max_speed_trajectory_m_per_s; + // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -97,6 +101,11 @@ struct RobotConstants // The maximum angular speed achievable by our robots [rad/s] float robot_max_ang_speed_rad_per_s; + // The maximum speed that the trajectory planner is allowed to command the robot to + // move at, while still leaving headroom for the PID to apply correction on lag. + // [rad/s] + float robot_max_ang_speed_trajectory_rad_per_s; + // The maximum angular acceleration achievable by our robots [rad/s^2] float robot_max_ang_acceleration_rad_per_s_2; @@ -107,6 +116,13 @@ struct RobotConstants // Found by sqrt(x^2 + y^2) of a wheel. // Front wheel arm = Rear wheel arm. See ASCII image above. float expected_lever_arm; + + // Various variances for the robot localizer + float kalman_process_noise_variance_rad_per_s_4; + + float kalman_vision_noise_variance_rad_2; + + float kalman_motor_sensor_noise_variance_rad_per_s_2; }; /** @@ -120,7 +136,7 @@ constexpr RobotConstants createRobotConstants() return { .robot_radius_m = static_cast(ROBOT_MAX_RADIUS_METERS), .front_wheel_angle_deg = 32.0f, - .back_wheel_angle_deg = 44.0f, + .back_wheel_angle_deg = 46.0f, .fr_x_pos_meters = 0.03485f, .fr_y_pos_meters = -0.06632f, @@ -142,17 +158,24 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 2.0f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.0f, + .robot_max_speed_trajectory_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 2.0f, + .robot_max_deceleration_m_per_s_2 = 1.5f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 6.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, .wheel_radius_meters = 0.03f, - .expected_lever_arm = 0.0749f}; + .expected_lever_arm = 0.0749f, + + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 1.0f, + .kalman_vision_noise_variance_rad_2 = 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5}; } #elif CHECK_VERSION(2021) constexpr RobotConstants createRobotConstants() @@ -173,15 +196,22 @@ constexpr RobotConstants createRobotConstants() .motor_max_acceleration_m_per_s_2 = 4.5f, // Robot's linear movement constants - .robot_max_speed_m_per_s = 3.000f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_m_per_s = 3.000f, + .robot_max_speed_trajectory_m_per_s = 3.000f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + + .wheel_radius_meters = 0.03f, - .wheel_radius_meters = 0.03f}; + // Kalman filter variances for robot localizer + .kalman_process_noise_variance_rad_per_s_4 = 0.5f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, + .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f * 0.5f}; } #endif diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 457ffb0639..2c729dd516 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p Point best_position = intercept_position; double final_speed = step_speed; bool finished = false; - double max_speed = robot.robotConstants().robot_max_speed_m_per_s; + double max_speed = robot.robotConstants().robot_max_speed_trajectory_m_per_s; double max_acc = robot.robotConstants().robot_max_acceleration_m_per_s_2; while (!finished) diff --git a/src/software/ai/hl/stp/tactic/move_primitive.cpp b/src/software/ai/hl/stp/tactic/move_primitive.cpp index 7e748c2068..325da493a3 100644 --- a/src/software/ai/hl/stp/tactic/move_primitive.cpp +++ b/src/software/ai/hl/stp/tactic/move_primitive.cpp @@ -40,7 +40,7 @@ MovePrimitive::MovePrimitive( angular_trajectory.generate( robot.orientation(), final_angle, robot.angularVelocity(), AngularVelocity::fromRadians( - robot.robotConstants().robot_max_ang_speed_rad_per_s), + robot.robotConstants().robot_max_ang_speed_trajectory_rad_per_s), AngularVelocity::fromRadians( robot.robotConstants().robot_max_ang_acceleration_rad_per_s_2), AngularVelocity::fromRadians( diff --git a/src/software/ai/navigator/trajectory/BUILD b/src/software/ai/navigator/trajectory/BUILD index abc4f5c82f..5b2ef42fb2 100644 --- a/src/software/ai/navigator/trajectory/BUILD +++ b/src/software/ai/navigator/trajectory/BUILD @@ -21,6 +21,7 @@ cc_library( deps = [ ":trajectory", "//software/geom:point", + "//software/geom/algorithms", ], ) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index d5a54bf7af..4095faea25 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "software/ai/navigator/trajectory/trajectory.hpp" @@ -103,6 +104,19 @@ class BangBangTrajectory1D : public Trajectory */ size_t getNumTrajectoryParts() const; + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different + * @return True if the trajectories are different, false otherwise + */ + bool isNew(const Trajectory& other, + double threshold) const override + { + return std::abs(getDestination() - other.getDestination()) > threshold; + } + private: /** * Generate a trapezoidal trajectory and fill in `trajectory_parts`. diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 2ffbff5ec2..859034685e 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -83,6 +83,19 @@ class BangBangTrajectory1DAngular */ double getTotalTime() const override; + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different in degrees. + * @return True if the trajectories are different, false otherwise + */ + bool isNew(const Trajectory& other, + double threshold) const override + { + return getDestination().minDiff(other.getDestination()).toDegrees() > threshold; + } + private: BangBangTrajectory1D trajectory; }; diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 4e7a7ee77e..57789684a3 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -59,4 +59,14 @@ class Trajectory { return getPosition(getTotalTime()); } + + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different + * @return True if the trajectories are different, false otherwise + */ + virtual bool isNew(const Trajectory& other, + double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index 9699f07c17..4f21a9a25b 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -1,6 +1,7 @@ #pragma once #include "software/ai/navigator/trajectory/trajectory.hpp" +#include "software/geom/algorithms/distance.h" #include "software/geom/point.h" #include "software/geom/rectangle.h" @@ -14,4 +15,17 @@ class Trajectory2D : virtual public Trajectory * @return bounding boxes which this trajectory passes through */ virtual std::vector getBoundingBoxes() const = 0; + + /** + * Check if this trajectory is meaningfully different from another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold above which the trajectories are considered + * different + * @return True if the trajectories are different, false otherwise + */ + bool isNew(const Trajectory& other, + double threshold) const override + { + return distance(getDestination(), other.getDestination()) > threshold; + } }; diff --git a/src/software/constants.h b/src/software/constants.h index d1592e54b1..4e549fa4cf 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; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index e5bba4bd99..2ae80efcd7 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -37,6 +37,9 @@ cc_library( "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", + "//software/embedded/motion_control:orientation_controller", + "//software/embedded/motion_control:position_controller", + "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", ], @@ -58,12 +61,14 @@ cc_library( deps = [ ":primitive_executor", "//proto:tbots_cc_proto", + "//software/embedded:robot_localizer", "//software/embedded/services:imu", "//software/embedded/services:motor", "//software/embedded/services:power", "//software/embedded/services/network", "//software/embedded/toml_config", "//software/logger:network_logger", + "//software/physics:velocity_conversion_util", "//software/tracy:tracy_constants", "//software/util/scoped_timespec_timer", "@tracy", @@ -101,3 +106,21 @@ cc_test( "//shared/test_util:tbots_gtest_main", ], ) + +cc_library( + name = "robot_localizer", + srcs = ["robot_localizer.cpp"], + hdrs = ["robot_localizer.h"], + deps = [ + "//proto:tbots_cc_proto", + "//software:constants", + "//software/embedded/services:imu", + "//software/geom:angle", + "//software/geom:angular_velocity", + "//software/geom:point", + "//software/geom:vector", + "//software/sensor_fusion/filter:kalman_filter", + "//software/util/scoped_timespec_timer", + "@eigen", + ], +) diff --git a/src/software/embedded/motion_control/BUILD b/src/software/embedded/motion_control/BUILD index 7fae1e1cd9..715cb54839 100644 --- a/src/software/embedded/motion_control/BUILD +++ b/src/software/embedded/motion_control/BUILD @@ -44,9 +44,6 @@ cc_library( cc_library( name = "pid_controller", - srcs = [ - "pid_controller.cpp", - ], hdrs = [ "pid_controller.h", ], diff --git a/src/software/embedded/motion_control/controller.h b/src/software/embedded/motion_control/controller.h index 6de0d30237..6882618082 100644 --- a/src/software/embedded/motion_control/controller.h +++ b/src/software/embedded/motion_control/controller.h @@ -1,3 +1,5 @@ +#pragma once + #include "software/time/duration.h" /** diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 700df6347f..63b90ed6d1 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -37,8 +37,8 @@ class OrientationController private: // TODO(#3737): tune constants - PidController w_pid_{0.7, 0.0, 2.0, 0.0}; - PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; + PidController w_pid_{0.7, 0.0, 2.0, 0.0}; + PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/motion_control/pid_controller.cpp b/src/software/embedded/motion_control/pid_controller.cpp deleted file mode 100644 index fe19d403dd..0000000000 --- a/src/software/embedded/motion_control/pid_controller.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "software/embedded/motion_control/pid_controller.h" - -#include -#include - -PidController::PidController(double k_p, double k_i, double k_d, double max_integral) - : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral) -{ - assert(max_integral >= 0.0); -}; - -double PidController::step(double error, double delta_time) -{ - integral_ = std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_); - - const double derivative = (error - last_error_.value_or(error)) / delta_time; - - last_error_ = error; - - return error * k_p_ + integral_ * k_i_ + derivative * k_d_; -} - -void PidController::reset() -{ - integral_ = 0.0; - last_error_ = std::nullopt; -} diff --git a/src/software/embedded/motion_control/pid_controller.h b/src/software/embedded/motion_control/pid_controller.h index 6282e41317..16c10b9393 100644 --- a/src/software/embedded/motion_control/pid_controller.h +++ b/src/software/embedded/motion_control/pid_controller.h @@ -1,21 +1,25 @@ #pragma once +#include +#include #include +/** + * A PID controller is used to calculate corrections based on error values over + * time as the difference between a desired value and the actual measured value. + * This PID controller also limits integral windup using a max_integral value. + * + * Resources: + * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ + * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ + */ +template class PidController { public: /** * Constructs a new PID controller. * - * A PID controller is used to calculate corrections based on error values over - * time as the difference between a desired value and the actual measured value. - * This PID controller also limits integral windup using a max_integral value. - * - * Resources: - * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ - * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ - * * @pre max_integral must be >= 0.0 * * @param k_p The proportional gain. @@ -24,7 +28,11 @@ class PidController * @param max_integral The maximum absolute value that the integrator term can * accumulate to. **/ - PidController(double k_p, double k_i, double k_d, double max_integral); + PidController(T k_p, T k_i, T k_d, T max_integral) + : k_p_(k_p), k_i_(k_i), k_d_(k_d), max_integral_(max_integral) + { + assert(max_integral >= T(0.0)); + } /** * Given an error, returns the control effort to minimize it. @@ -33,19 +41,39 @@ class PidController * @param delta_time The time passed since last step, for calculating the integrator * and derivative. **/ - double step(double error, double delta_time); + T step(T error, T delta_time = T(1.0)) + { + // If sign of error swaps, reset integrator + if (last_error_.has_value() && (last_error_.value() * error < T(0.0))) + { + integral_ = T(0.0); + } + + integral_ = + std::clamp(integral_ + error * delta_time, -max_integral_, max_integral_); + + const T derivative = (error - last_error_.value_or(error)) / delta_time; + + last_error_ = error; + + return error * k_p_ + integral_ * k_i_ + derivative * k_d_; + } /** * Resets the integrator and clears the last error used for derivative calculation. **/ - void reset(); + void reset() + { + integral_ = T(0.0); + last_error_ = std::nullopt; + } private: - double k_p_; - double k_i_; - double k_d_; - double max_integral_; + T k_p_; + T k_i_; + T k_d_; + T max_integral_; - double integral_ = 0.0; - std::optional last_error_ = std::nullopt; + T integral_ = T(0.0); + std::optional last_error_ = std::nullopt; }; diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp index c7c0db12b3..307100d84a 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -4,7 +4,7 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) { - PidController pid{1.0, 0.0, 0.0, 0.0}; + PidController pid{1.0, 0.0, 0.0, 0.0}; EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); @@ -13,7 +13,7 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) EXPECT_DOUBLE_EQ(pid.step(-5.0, 1.0), -5.0); EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), -1.0); - pid = PidController{5.0, 0.0, 0.0, 0.0}; + pid = PidController{5.0, 0.0, 0.0, 0.0}; EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), 0.0); @@ -26,27 +26,31 @@ TEST(PidControllerTest, OnlyProportionTermNonZero) TEST(PidControllerTest, OnlyIntegralTermNonZero) { constexpr double k_i = 2.0; - PidController pid{0.0, k_i, 0.0, 10.0}; + PidController pid{0.0, k_i, 0.0, 10.0}; EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 1.0); EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 2.0); EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 3.0); EXPECT_DOUBLE_EQ(pid.step(0.5, 1.0), k_i * 3.5); - EXPECT_DOUBLE_EQ(pid.step(-0.2, 1.0), k_i * 3.3); - EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), k_i * 2.3); - EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * 2.3); + // Sign swap should reset integrator to 0, then add -0.2 + EXPECT_DOUBLE_EQ(pid.step(-0.2, 1.0), k_i * -0.2); + EXPECT_DOUBLE_EQ(pid.step(-1.0, 1.0), k_i * -1.2); + // Sign swap back to 0.0? No, 0.0 doesn't swap sign usually (0*X is 0, not < 0) + // My implementation: last_error_.value() * error < T(0.0) + // If error is 0, it's not < 0. + EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_i * -1.2); // should not accumulate integral term above max_integral - EXPECT_DOUBLE_EQ(pid.step(6.7, 1.0), k_i * 9.0); - EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); + EXPECT_DOUBLE_EQ(pid.step(6.7, 1.0), k_i * 5.5); // Sign not swapped from 0.0 to 6.7 + EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 10.0) EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 10.0); } TEST(PidControllerTest, OnlyDerivativeTermNonZero) { constexpr double k_d = 3.0; - PidController pid{0.0, 0.0, k_d, 0.0}; + PidController pid{0.0, 0.0, k_d, 0.0}; EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 0.0); EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_d * 0.0); @@ -64,12 +68,13 @@ TEST(PidControllerTest, GeneralApplication) constexpr double k_i = 3.0; constexpr double k_d = -1.0; constexpr double max_integral = 10.0; - PidController pid{k_p, k_i, k_d, max_integral}; + PidController pid{k_p, k_i, k_d, max_integral}; EXPECT_DOUBLE_EQ(pid.step(12.0, 0.75), k_p * 12.0 + k_i * 9.0 + k_d * 0.0); EXPECT_DOUBLE_EQ(pid.step(24.0, 0.75), k_p * 24.0 + k_i * 10.0 + k_d * 12.0 / 0.75); EXPECT_DOUBLE_EQ(pid.step(4.0, 1.0), k_p * 4.0 + k_i * 10.0 + k_d * -20.0); EXPECT_DOUBLE_EQ(pid.step(0.0, 1.0), k_p * 0.0 + k_i * 10.0 + k_d * -4.0); EXPECT_DOUBLE_EQ(pid.step(2.0, 1.0), k_p * 2.0 + k_i * 10.0 + k_d * 2.0); - EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * 8.0 + k_d * -4.0); + // Sign swap reset: integral becomes 0 + -2.0*1.0 = -2.0 + EXPECT_DOUBLE_EQ(pid.step(-2.0, 1.0), k_p * -2.0 + k_i * -2.0 + k_d * -4.0); } diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index ca2f41a8d5..12725e09d7 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -35,11 +35,11 @@ class PositionController : public MotionController x_pid_{0.8, 0.0, 0.0, 0.0}; + PidController y_pid_{0.8, 0.0, 0.0, 0.0}; - PidController x_pid_close_{2.0, 0.0, 0.0, 0.0}; - PidController y_pid_close_{2.0, 0.0, 0.0, 0.0}; + PidController x_pid_close_{2.0, 0.0, 0.0, 0.0}; + PidController y_pid_close_{2.0, 0.0, 0.0, 0.0}; static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 1c29915b4d..7324b0a6a4 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -165,12 +165,7 @@ void StSpinMotorController::updateFaults(const MotorIndex motor, motor_faults.drive_enabled = false; } - LOG(WARNING) - << oss.str(); // Consider moving this to a helper and also making the stream - // object persistent so we do not need to continuously recreate it - // (it is notoriously expensive). We could also consider another - // approach to formatting this string as using streams in general - // is less than ideal. Or We can probably just use string::append + LOG(WARNING) << oss.str(); } int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..9c1783ef07 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,13 +11,9 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, const RobotId robot_id) - : current_primitive_(), - friendly_team_colour_(friendly_team_colour), - robot_constants_(robot_constants), - time_step_(time_step), - robot_id_(robot_id) + const robot_constants::RobotConstants& robot_constants) + : last_step_time_(std::chrono::steady_clock::now()), + robot_constants_(robot_constants) { } @@ -27,82 +23,118 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (current_primitive_.has_move()) { - trajectory_path_ = createTrajectoryPathFromParams( - current_primitive_.move().xy_traj_params(), velocity_, robot_constants_); + const std::optional new_trajectory_path = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); - angular_trajectory_ = + const bool is_linear_traj_new = + (new_trajectory_path.has_value() != trajectory_path_.has_value()) || + (new_trajectory_path.has_value() && + trajectory_path_->isNew(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); + + if (is_linear_traj_new) + { + trajectory_path_ = new_trajectory_path; + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); + position_controller_.reset(); + } + + const BangBangTrajectory1DAngular new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - angular_velocity_, robot_constants_); + orientation_, angular_velocity_, + robot_constants_); + + const bool is_angular_traj_new = + !angular_trajectory_.has_value() || + angular_trajectory_->isNew(new_angular_trajectory, + ANGULAR_DESTINATION_THRESHOLD_DEGREES); - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); + if (is_angular_traj_new) + { + angular_trajectory_ = new_angular_trajectory; + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); + orientation_controller_.reset(); + } } } -void PrimitiveExecutor::setStopPrimitive() +void PrimitiveExecutor::updateState(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity) { - current_primitive_ = *createStopPrimitiveProto(); -} + position_ = position; + velocity_ = velocity; + orientation_ = orientation; + angular_velocity_ = angular_velocity; + + // If we are lagging behind trajectory too much, we have stalled! We need to + // regenerate trajectory. + if (trajectory_path_.has_value()) + { + const double linear_following_error = + (position_ - trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.count())) + .length(); -void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity) -{ - Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_); - velocity_ = actual_global_velocity; - angular_velocity_ = angular_velocity; -} + if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) + { + // regenerate linear trajectory + trajectory_path_ = + createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), + position_, velocity_, robot_constants_); -Vector PrimitiveExecutor::getTargetLinearVelocity() -{ - Vector local_velocity = globalToLocalVelocity( - trajectory_path_->getVelocity(time_since_trajectory_creation_.toSeconds()), - orientation_); - Point position = - trajectory_path_->getPosition(time_since_trajectory_creation_.toSeconds()); - double distance_to_destination = - distance(position, trajectory_path_->getDestination()); - - // Dampen velocity as we get closer to the destination to reduce jittering - if (distance_to_destination < MAX_DAMPENING_VELOCITY_DISTANCE_M) - { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; + time_since_linear_trajectory_creation_ = + std::chrono::duration::zero(); + } } - return local_velocity; -} -AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() -{ - orientation_ = - angular_trajectory_->getPosition(time_since_trajectory_creation_.toSeconds()); - - AngularVelocity angular_velocity = - angular_trajectory_->getVelocity(time_since_trajectory_creation_.toSeconds()); - Angle orientation_to_destination = - orientation_.minDiff(angular_trajectory_->getDestination()); - if (orientation_to_destination.toDegrees() < 5) + if (angular_trajectory_.has_value()) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; - } + const double angular_following_error = + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.count()) + .minDiff(orientation_) + .toDegrees(); - return angular_velocity; -} + if (angular_following_error > ANGULAR_STALL_ERROR_MAX_DEGREES) + { + // regenerate angular trajectory + angular_trajectory_ = createAngularTrajectoryFromParams( + current_primitive_.move().w_traj_params(), orientation_, + angular_velocity_, robot_constants_); + time_since_angular_trajectory_creation_ = + std::chrono::duration::zero(); + } + } +} std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - time_since_trajectory_creation_ += time_step_; + const auto current_time = std::chrono::steady_clock::now(); + const auto delta_time_chrono = current_time - last_step_time_; + const Duration delta_time = Duration::fromSeconds( + std::chrono::duration(delta_time_chrono).count()); + + time_since_linear_trajectory_creation_ += delta_time_chrono; + time_since_angular_trajectory_creation_ += delta_time_chrono; + last_step_time_ = current_time; + status.set_running_primitive(true); switch (current_primitive_.primitive_case()) { case TbotsProto::Primitive::kStop: { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( - prim->direct_control()); + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); status.set_running_primitive(false); - return output; + return std::make_unique( + prim->direct_control()); } case TbotsProto::Primitive::kDirectControl: { @@ -113,40 +145,46 @@ std::unique_ptr PrimitiveExecutor::stepPrimi { if (!trajectory_path_.has_value() || !angular_trajectory_.has_value()) { - auto prim = createDirectControlPrimitive(Vector(), AngularVelocity(), 0.0, - TbotsProto::AutoChipOrKick()); - auto output = std::make_unique( + const auto prim = createDirectControlPrimitive( + Vector(), AngularVelocity(), 0.0, TbotsProto::AutoChipOrKick()); + LOG(INFO) << "Not moving because trajectory_path_ has value " + << trajectory_path_.has_value() << " angular has " + << angular_trajectory_.has_value(); + return std::make_unique( prim->direct_control()); - LOG(INFO) - << "Not moving because trajectory_path_ or angular_trajectory_ is not set"; - return output; } - Vector local_velocity = getTargetLinearVelocity(); - AngularVelocity angular_velocity = getTargetAngularVelocity(); + const Duration elapsed_linear = Duration::fromSeconds( + time_since_linear_trajectory_creation_.count()); + const Duration elapsed_angular = Duration::fromSeconds( + time_since_angular_trajectory_creation_.count()); + + const Vector target_linear_velocity_global = position_controller_.step( + position_, *trajectory_path_, elapsed_linear, delta_time); + + const AngularVelocity target_angular_velocity = orientation_controller_.step( + orientation_, *angular_trajectory_, elapsed_angular, delta_time); + + Vector target_linear_velocity_local = + globalToLocalVelocity(target_linear_velocity_global, orientation_); + + // Make sure target linear velocity is clamped + target_linear_velocity_local = target_linear_velocity_local.normalize( + std::min(target_linear_velocity_local.length(), + static_cast(robot_constants_.robot_max_speed_m_per_s))); - auto output = createDirectControlPrimitive( - local_velocity, angular_velocity, + const auto prim = createDirectControlPrimitive( + target_linear_velocity_local, target_angular_velocity, convertDribblerModeToDribblerSpeed( current_primitive_.move().dribbler_mode(), robot_constants_), current_primitive_.move().auto_chip_or_kick()); return std::make_unique( - output->direct_control()); + prim->direct_control()); } case TbotsProto::Primitive::PRIMITIVE_NOT_SET: { - // TODO (#2283) Once we can add/remove robots, this log should - // be re-enabled. Right now it just gets spammed because we command - // 6 robots for Div B when there are 11 on the field. - // - // LOG(DEBUG) << "No primitive set!"; } } return std::make_unique(); } - -void PrimitiveExecutor::setRobotId(const RobotId robot_id) -{ - robot_id_ = robot_id; -} diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 145874f3c2..952816282a 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -5,96 +5,80 @@ #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" #include "software/geom/vector.h" -#include "software/time/duration.h" #include "software/world/robot_state.h" -#include "software/world/team_types.h" +#include "software/embedded/motion_control/orientation_controller.h" +#include "software/embedded/motion_control/position_controller.h" class PrimitiveExecutor { public: /** * Constructor - * @param time_step Time step which this primitive executor operates in - * @param robot_constants The robot constants for the robot which uses this primitive - * executor - * @param friendly_team_colour The colour of the friendly team - * @param robot_id The id of the robot which uses this primitive executor + * + * @param robot_constants The robot constants for the robot */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants, - const TeamColour friendly_team_colour, - const RobotId robot_id); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive + * * @param primitive_msg The primitive to start */ void updatePrimitive(const TbotsProto::Primitive& primitive_msg); /** - * Set the current primitive to the stop primitive - */ - void setStopPrimitive(); - - /** - * Update primitive executor with the current velocity of the robot + * Update primitive executor with the current velocity and orientation of the robot * - * @param local_velocity The current _local_ velocity + * @param position The current position + * @param velocity The current velocity + * @param orientation The current orientation of the robot * @param angular_velocity The current angular velocity */ - void updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity); - - /** - * Set the robot id - * @param robot_id The id of the robot which uses this primitive executor - */ - void setRobotId(RobotId robot_id); + void updateState(const Point& position, const Vector& velocity, + const Angle& orientation, const AngularVelocity& angular_velocity); /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive - * * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status); private: - /* - * Compute the next target linear _local_ velocity the robot should be at. - * @returns Vector The target linear _local_ velocity - */ - Vector getTargetLinearVelocity(); + TbotsProto::Primitive current_primitive_; - /* - * Returns the next target angular velocity the robot - * - * @returns AngularVelocity The target angular velocity - */ - AngularVelocity getTargetAngularVelocity(); + std::optional trajectory_path_; + std::optional angular_trajectory_; - TbotsProto::Primitive current_primitive_; - Duration time_since_trajectory_creation_; + std::chrono::time_point last_step_time_; + std::chrono::duration time_since_linear_trajectory_creation_; + std::chrono::duration time_since_angular_trajectory_creation_; + + Point position_; Vector velocity_; AngularVelocity angular_velocity_; Angle orientation_; - TeamColour friendly_team_colour_; + + Point last_position_; + Angle last_orientation_; + robot_constants::RobotConstants robot_constants_; - std::optional trajectory_path_; - std::optional angular_trajectory_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - RobotId robot_id_; + PositionController position_controller_; + OrientationController orientation_controller_; - // Estimated delay between a vision frame to AI processing to robot executing - static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; + // If distance between current linear trajectory destination and new one is larger + // than this, we change trajectories. + static constexpr double LINEAR_DESTINATION_THRESHOLD_METERS = 0.03; + static constexpr double ANGULAR_DESTINATION_THRESHOLD_DEGREES = 4; - // The distance away from the destination at which we start dampening the velocity - // to avoid jittering around the destination. - static constexpr double MAX_DAMPENING_VELOCITY_DISTANCE_M = 0.05; + // These constants were lost during a refactor/revert and are currently set to + // estimated defaults. + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp new file mode 100644 index 0000000000..41744aa047 --- /dev/null +++ b/src/software/embedded/robot_localizer.cpp @@ -0,0 +1,306 @@ +#include "robot_localizer.h" + +#include + +RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config) + : process_linear_acceleration_noise_variance_(config.process_noise_variance), + process_angular_acceleration_noise_variance_(config.process_noise_variance) +{ + filter_.state_covariance = + Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal(); + + filter_.measurement_covariance = + Eigen::Vector( + config.vision_noise_variance, config.vision_noise_variance, + config.vision_noise_variance, config.motor_sensor_noise_variance, + config.motor_sensor_noise_variance, config.motor_sensor_noise_variance, + ImuService::IMU_VARIANCE) + .asDiagonal(); + + last_step_time_ = std::chrono::steady_clock::now(); +} + +void RobotLocalizer::step(const Vector& linear_acceleration) +{ + FilterStep step{ + .prediction = FilterStep::Predict{}, + .update = std::nullopt, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + const std::chrono::duration delta_time = step.time - last_step_time_; + const double delta_time_seconds = delta_time.count(); + last_step_time_ = step.time; + + // clang-format off + step.prediction->process_model << + 1, 0, 0, delta_time_seconds, 0, 0, + 0, 1, 0, 0, delta_time_seconds, 0, + 0, 0, 1, 0, 0, delta_time_seconds, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1; + // clang-format on + + const double delta_time_squared = delta_time_seconds * delta_time_seconds; + const double delta_time_cubed = delta_time_squared * delta_time_seconds; + const double delta_time_fourth = delta_time_cubed * delta_time_seconds; + + auto& process_covariance = step.prediction->process_covariance; + process_covariance.setZero(); + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_POSITION), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = + delta_time_fourth / 4 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_POSITION), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_POSITION)) = + delta_time_cubed / 2 * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = + delta_time_squared * process_linear_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = + delta_time_fourth / 4 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ORIENTATION), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ORIENTATION)) = + delta_time_cubed / 2 * process_angular_acceleration_noise_variance_; + + process_covariance(static_cast(StateIndex::ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = + delta_time_squared * process_angular_acceleration_noise_variance_; + + auto& control_model = step.prediction->control_model; + control_model.setZero(); + + control_model(static_cast(StateIndex::X_POSITION), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::Y_POSITION), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_squared / 2; + + control_model(static_cast(StateIndex::X_VELOCITY), + static_cast(ControlIndex::X_ACCELERATION)) = + delta_time_seconds; + + control_model(static_cast(StateIndex::Y_VELOCITY), + static_cast(ControlIndex::Y_ACCELERATION)) = + delta_time_seconds; + + step.prediction->control_input << linear_acceleration.x(), linear_acceleration.y(); + + filter_.process_model = step.prediction->process_model; + filter_.process_covariance = step.prediction->process_covariance; + filter_.control_model = step.prediction->control_model; + + history.push_front(step); + filter_.predict(step.prediction->control_input); +} + +void RobotLocalizer::update(const VisionData& data) +{ + if (history.empty()) + { + updateFilterWithVision(data.position, data.orientation); + return; + } + + const auto current_time = std::chrono::steady_clock::now(); + const auto sample_age = std::chrono::duration(data.age_seconds); + + const auto rollback_point = std::find_if( + history.begin(), history.end(), + [&](const FilterStep& step) { return step.time - current_time >= sample_age; }); + + if (rollback_point == history.begin()) + { + // All history predates the sample, no need to rollback + updateFilterWithVision(data.position, data.orientation); + return; + } + + auto replay_iter = std::make_reverse_iterator(rollback_point); + + // Truncate history after the rollback point + history.erase(rollback_point, history.end()); + + filter_.state_estimate = replay_iter->state_estimate; + filter_.state_covariance = replay_iter->state_covariance; + + updateFilterWithVision(data.position, data.orientation); + + // Replay from the rollback point back to the current estimate + for (; replay_iter != history.rbegin(); --replay_iter) + { + if (replay_iter->prediction.has_value()) + { + const auto& prediction = replay_iter->prediction.value(); + filter_.process_model = prediction.process_model; + filter_.process_covariance = prediction.process_covariance; + filter_.control_model = prediction.control_model; + filter_.predict(prediction.control_input); + } + + if (replay_iter->update.has_value()) + { + const auto& update = replay_iter->update.value(); + filter_.measurement_model = update.measurement_model; + filter_.update(update.measurement); + } + } +} + +void RobotLocalizer::updateFilterWithVision(const Point& position, + const Angle& orientation) +{ + const double orientation_estimate = + filter_.state_estimate(static_cast(StateIndex::ORIENTATION)); + + Eigen::Vector measurement; + measurement.setZero(); + + measurement(static_cast(MeasurementIndex::VISION_X_POSITION)) = + position.x(); + measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) = + position.y(); + + // Coterminal angle that is closest to current estimate + measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) = + orientation_estimate + + (orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians(); + + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_X_POSITION), + static_cast(StateIndex::X_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_Y_POSITION), + static_cast(StateIndex::Y_POSITION)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::VISION_ORIENTATION), + static_cast(StateIndex::ORIENTATION)) = 1; + + filter_.update(measurement); +} + +void RobotLocalizer::update(const MotorData& data) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_X_VELOCITY), + static_cast(StateIndex::X_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_Y_VELOCITY), + static_cast(StateIndex::Y_VELOCITY)) = 1; + filter_.measurement_model( + static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast(MeasurementIndex::MOTOR_X_VELOCITY)) = + data.velocity.x(); + update.measurement(static_cast(MeasurementIndex::MOTOR_Y_VELOCITY)) = + data.velocity.y(); + update.measurement(static_cast( + MeasurementIndex::MOTOR_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +void RobotLocalizer::update(const ImuData& data) +{ + filter_.measurement_model.setZero(); + filter_.measurement_model( + static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY), + static_cast(StateIndex::ANGULAR_VELOCITY)) = 1; + + FilterStep::Update update{ + .measurement_model = filter_.measurement_model, + .measurement = Eigen::Vector::Zero(), + }; + + update.measurement(static_cast( + MeasurementIndex::IMU_ANGULAR_VELOCITY)) = data.angular_velocity.toRadians(); + + const FilterStep step{ + .prediction = std::nullopt, + .update = update, + .state_estimate = filter_.state_estimate, + .state_covariance = filter_.state_covariance, + .time = std::chrono::steady_clock::now(), + }; + + history.push_front(step); + filter_.update(step.update->measurement); +} + +Point RobotLocalizer::getPosition() const +{ + return Point( + filter_.state_estimate(static_cast(StateIndex::X_POSITION)), + filter_.state_estimate(static_cast(StateIndex::Y_POSITION))); +} + +Vector RobotLocalizer::getVelocity() const +{ + return Vector( + filter_.state_estimate(static_cast(StateIndex::X_VELOCITY)), + filter_.state_estimate(static_cast(StateIndex::Y_VELOCITY))); +} + +Angle RobotLocalizer::getOrientation() const +{ + return Angle::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ORIENTATION))) + .clamp(); +} + +AngularVelocity RobotLocalizer::getAngularVelocity() const +{ + return AngularVelocity::fromRadians( + filter_.state_estimate(static_cast(StateIndex::ANGULAR_VELOCITY))); +} diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer.h new file mode 100644 index 0000000000..3a46a2ff5e --- /dev/null +++ b/src/software/embedded/robot_localizer.h @@ -0,0 +1,178 @@ +#pragma once + +#include +#include +#include + +#include "proto/world.pb.h" +#include "software/embedded/services/imu.h" +#include "software/geom/angle.h" +#include "software/geom/point.h" +#include "software/geom/vector.h" +#include "software/sensor_fusion/filter/kalman_filter.hpp" +#include "software/util/make_enum/make_enum.hpp" + +MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY, + ANGULAR_VELOCITY); + +MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION, + MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY, + IMU_ANGULAR_VELOCITY); + +MAKE_ENUM(ControlIndex, X_ACCELERATION, Y_ACCELERATION); + +/** + * Estimates robot orientation, angular velocity, and angular acceleration + * using a Kalman filter. + * + * The filter keeps a history of recent predict/update operations. When delayed + * vision data arrives, the localizer rewinds to the matching historical state, + * applies the delayed measurement, then replays newer steps to recover the + * current estimate. + */ +class RobotLocalizer +{ + public: + struct VisionData + { + Point position; + Angle orientation; + double age_seconds; + }; + + struct MotorData + { + Vector velocity; + AngularVelocity angular_velocity; + }; + + struct ImuData + { + AngularVelocity angular_velocity; + }; + + struct RobotLocalizerConfig + { + double process_noise_variance; + double vision_noise_variance; + double motor_sensor_noise_variance; + }; + + /** + * Creates a new robot localizer. + * + * The variances determine how strongly each source influences the estimate. + * + * @param config Configuration for the localizer variances. + */ + explicit RobotLocalizer(const RobotLocalizerConfig& config); + + /** + * Runs one prediction step using elapsed time since the previous call. + * + * @param linear_acceleration The current linear acceleration of the robot + */ + void step(const Vector& linear_acceleration); + + /** + * Update the robot's position and orientation from data reported by vision. + * + * @param data Vision reading of the robot's position, orientation and age + */ + void update(const VisionData& data); + + /** + * Update the robot's velocity from data reported by motor sensors + * (i.e. encoders or Hall sensors). + * + * @param data Motor sensor reading of the robot's velocity and angular velocity + */ + void update(const MotorData& data); + + /** + * Update the angular velocity from IMU. + * + * @param data IMU reading of the robot's angular velocity + */ + void update(const ImuData& data); + + /** + * Gets the estimated position of the robot in world space. + * + * @return the estimated position of the robot in world space + */ + Point getPosition() const; + + /** + * Gets the estimated velocity of the robot in world space. + * + * @return the estimated velocity of the robot in world space + */ + Vector getVelocity() const; + + /** + * Gets the estimated orientation of the robot in world space. + * + * @return estimated orientation of the robot in world space + */ + Angle getOrientation() const; + + /** + * Gets the estimated angular velocity of the robot. + * + * @return estimated angular velocity of the robot + */ + AngularVelocity getAngularVelocity() const; + + private: + /** + * Update the Kalman filter with the robot's position and orientation from vision. + * + * @param position Vision reading of the robot's position in world space + * @param orientation Vision reading of the robot's orientation in world space + */ + void updateFilterWithVision(const Point& position, const Angle& orientation); + + static constexpr size_t STATE_SIZE = reflective_enum::size(); + static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size(); + static constexpr size_t CONTROL_SIZE = reflective_enum::size(); + + /** + * Snapshot of a Kalman filter predict/update step needed for rollback/replay. + */ + struct FilterStep + { + struct Predict + { + Eigen::Matrix process_model; + Eigen::Matrix process_covariance; + Eigen::Matrix control_model; + Eigen::Vector control_input; + }; + + struct Update + { + Eigen::Matrix measurement_model; + Eigen::Vector measurement; + }; + + std::optional prediction; + std::optional update; + + Eigen::Vector state_estimate; + Eigen::Matrix state_covariance; + + std::chrono::time_point time; + }; + + KalmanFilter filter_; + + // Process noise variance used in prediction + double process_linear_acceleration_noise_variance_; + double process_angular_acceleration_noise_variance_; + + std::chrono::time_point last_step_time_; + + // History is ordered newest-first (front is the most recent step) + std::deque history; +}; diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index fa9571a16c..c0d1eff518 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -51,3 +51,22 @@ cc_library( "@eigen", ], ) + +cc_binary( + name = "robot_auto_test", + srcs = ["robot_auto_test.cpp"], + linkopts = [ + "-lpthread", + "-lrt", + ], + deps = [ + ":motor", + ":power", + "//proto/message_translation:tbots_geometry", + "//proto/primitive:primitive_msg_factory", + "//shared:robot_constants", + "//software/embedded:primitive_executor", + "//software/logger", + "@trinamic", + ], +) diff --git a/src/software/embedded/spi_utils.h b/src/software/embedded/spi_utils.h index 1b38e14f22..ee5933d172 100644 --- a/src/software/embedded/spi_utils.h +++ b/src/software/embedded/spi_utils.h @@ -32,7 +32,6 @@ void spiTransfer(int fd, uint8_t const* tx, uint8_t const* rx, unsigned len, * @param read_rx the buffer our read response will be placed in * @param spi_speed the speed to run spi at in Hz */ -void readThenWriteSpiTransfer( - int fd, const uint8_t* read_tx, const uint8_t* write_tx, const uint8_t* read_rx, - const uint32_t read_len, const uint32_t write_len, - uint32_t spi_speed); // refactor to take std::array, not raw pointers +void readThenWriteSpiTransfer(int fd, const uint8_t* read_tx, const uint8_t* write_tx, + const uint8_t* read_rx, const uint32_t read_len, + const uint32_t write_len, uint32_t spi_speed); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index afc30bdd03..fca9b69cd1 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -4,6 +4,7 @@ #include #include "proto/message_translation/tbots_protobuf.h" +#include "proto/primitive/primitive_msg_factory.h" #include "proto/robot_crash_msg.pb.h" #include "proto/robot_status_msg.pb.h" #include "proto/tbots_software_msgs.pb.h" @@ -15,6 +16,7 @@ #include "software/logger/logger.h" #include "software/logger/network_logger.h" #include "software/networking/tbots_network_exception.h" +#include "software/physics/velocity_conversion_util.h" #include "software/tracy/tracy_constants.h" #include "software/util/scoped_timespec_timer/scoped_timespec_timer.h" @@ -71,7 +73,6 @@ extern "C" Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, bool enable_log_merging, const int loop_hz) - // TODO (#2495): Set the friendly team colour : toml_config_client_(std::make_unique(TOML_CONFIG_FILE_PATH)), motor_status_(std::nullopt), robot_constants_(robot_constants), @@ -84,8 +85,11 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants, - TeamColour::YELLOW, robot_id_) + primitive_executor_(robot_constants), + robot_localizer_(RobotLocalizer::RobotLocalizerConfig{ + robot_constants.kalman_process_noise_variance_rad_per_s_4, + robot_constants.kalman_vision_noise_variance_rad_2, + robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2}) { waitForNetworkUp(); @@ -117,7 +121,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, LOG(INFO) << "THUNDERLOOP: Network Service initialized! Next initializing Power Service"; - power_service_ = std::make_unique(); + // power_service_ = std::make_unique(); LOG(INFO) << "THUNDERLOOP: Power Service initialized! Next initializing Motor Service"; @@ -154,45 +158,27 @@ void Thunderloop::runLoop() struct timespec last_kicker_fired; struct timespec prev_iter_start_time; - // Input buffer - TbotsProto::Primitive new_primitive; - // Loop interval int interval = static_cast(1.0f / static_cast(loop_hz_) * NANOSECONDS_PER_SECOND); - // Get current time - // Note: CLOCK_MONOTONIC is used over CLOCK_REALTIME since - // CLOCK_REALTIME can jump backwards clock_gettime(CLOCK_MONOTONIC, &next_shot); clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); clock_gettime(CLOCK_MONOTONIC, &prev_iter_start_time); + // Initial Version Setup std::string thunderloop_hash, thunderloop_date_flashed; - std::ifstream hashFile("~/thunderbots_hashes/thunderloop.hash"); std::ifstream dateFile("~/thunderbots_hashes/thunderloop.date"); - std::getline(hashFile, thunderloop_hash); std::getline(dateFile, thunderloop_date_flashed); - hashFile.close(); dateFile.close(); - robot_status_.set_thunderloop_version(thunderloop_hash); robot_status_.set_thunderloop_date_flashed(thunderloop_date_flashed); - - std::optional imu_poll = imu_service_->poll(); - - // TODO (3725): Replace with actual IMU data usage - if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) - { - LOG(INFO) << "IMU Angular Velocity: " << imu_poll->angular_velocity->toRadians(); - } - for (;;) { struct timespec time_since_prev_iter; @@ -200,139 +186,19 @@ void Thunderloop::runLoop() ScopedTimespecTimer::timespecDiff(¤t_time, &prev_iter_start_time, &time_since_prev_iter); prev_iter_start_time = current_time; + { - // Wait until next shot - // - // Note: CLOCK_MONOTONIC is used over CLOCK_REALTIME since - // CLOCK_REALTIME can jump backwards clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_shot, NULL); - FrameMarkStart(TracyConstants::THUNDERLOOP_FRAME_MARKER); - ScopedTimespecTimer iteration_timer(&iteration_time); - // Network Service: receive newest primitives and send out the last - // robot status - { - ScopedTimespecTimer timer(&poll_time); + processNetworkPolling(poll_time, last_primitive_received_time); - ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + processLocalizationUpdates(); - new_primitive = network_service_->poll(robot_status_); - } + processPrimitiveExecution(poll_time, last_primitive_received_time); - thunderloop_status_.set_network_service_poll_time_ms( - getMilliseconds(poll_time)); - - uint64_t last_handled_primitive_set = primitive_.sequence_number(); - - // Updating primitives with newly received data - // and setting the correct time elasped since last primitive - - struct timespec time_since_last_primitive_received; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, - &last_primitive_received_time, - &time_since_last_primitive_received); - network_status_.set_ms_since_last_primitive_received( - getMilliseconds(time_since_last_primitive_received)); - - // If the primitive msg is new, update the internal buffer - // and start the new primitive. - if (new_primitive.time_sent().epoch_timestamp_seconds() > - primitive_.time_sent().epoch_timestamp_seconds()) - { - // Save new primitive - primitive_ = new_primitive; - - // Update primitive executor's primitive set - { - clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); - - // Start new primitive - { - ScopedTimespecTimer timer(&poll_time); - primitive_executor_.updatePrimitive(primitive_); - } - - thunderloop_status_.set_primitive_executor_start_time_ms( - getMilliseconds(poll_time)); - } - } - - if (motor_status_.has_value()) - { - auto status = motor_status_.value(); - primitive_executor_.updateVelocity( - createVector(status.local_velocity()), - createAngularVelocity(status.angular_velocity())); - } - - // Timeout Overrides for Primitives - // These should be after the new primitive update section above - - // If primitive not received in a while, stop robot - // Primitive Executor: run the last primitive if we have not timed out - { - ScopedTimespecTimer timer(&poll_time); - - ZoneNamedN(_tracy_step_primitive, "Thunderloop: Step Primitive", true); - - // Handle emergency stop override - auto nanoseconds_elapsed_since_last_primitive = - getNanoseconds(time_since_last_primitive_received); - - if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) - { - primitive_executor_.setStopPrimitive(); - } - - direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_); - } - - thunderloop_status_.set_primitive_executor_step_time_ms( - getMilliseconds(poll_time)); - - // Power Service: execute the power control command - power_status_ = pollPowerService(poll_time); - thunderloop_status_.set_power_service_poll_time_ms( - getMilliseconds(poll_time)); - - struct timespec time_since_kicker_fired; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, &last_kicker_fired, - &time_since_kicker_fired); - chipper_kicker_status_.set_ms_since_kicker_fired( - getMilliseconds(time_since_kicker_fired)); - - struct timespec time_since_chipper_fired; - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - ScopedTimespecTimer::timespecDiff(¤t_time, &last_chipper_fired, - &time_since_chipper_fired); - chipper_kicker_status_.set_ms_since_chipper_fired( - getMilliseconds(time_since_chipper_fired)); - - // if a kick proto is sent or if autokick is on - if (direct_control_.power_control().chicker().has_kick_speed_m_per_s() || - direct_control_.power_control() - .chicker() - .auto_chip_or_kick() - .has_autokick_speed_m_per_s()) - { - clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); - } - // if a chip proto is sent or if autochip is on - else if (direct_control_.power_control() - .chicker() - .has_chip_distance_meters() || - direct_control_.power_control() - .chicker() - .auto_chip_or_kick() - .has_autochip_distance_meters()) - { - clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); - } + updateChickerStatus(last_chipper_fired, last_kicker_fired); // Motor Service: execute the motor control command motor_status_ = pollMotorService(poll_time, direct_control_.motor_control(), @@ -340,37 +206,182 @@ void Thunderloop::runLoop() thunderloop_status_.set_motor_service_poll_time_ms( getMilliseconds(poll_time)); - clock_gettime(CLOCK_MONOTONIC, ¤t_time); - time_sent_.set_epoch_timestamp_seconds( - static_cast(current_time.tv_sec)); - - // Update Robot Status with poll responses - robot_status_.set_robot_id(robot_id_); - robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); - *(robot_status_.mutable_time_sent()) = time_sent_; - *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; - *(robot_status_.mutable_motor_status()) = motor_status_.value(); - *(robot_status_.mutable_power_status()) = power_status_; - *(robot_status_.mutable_network_status()) = network_status_; - *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; - *(robot_status_.mutable_primitive_executor_status()) = - primitive_executor_status_; - - updateErrorCodes(); + updateAndSendRobotStatus(primitive_.sequence_number()); } auto loop_duration_ns = getNanoseconds(iteration_time); thunderloop_status_.set_iteration_time_ms(loop_duration_ns / NANOSECONDS_PER_MILLISECOND); - // Calculate next shot (which is an absolute time) next_shot.tv_nsec += interval; timespecNorm(next_shot); - FrameMarkEnd(TracyConstants::THUNDERLOOP_FRAME_MARKER); } } +inline void Thunderloop::processNetworkPolling(struct timespec& poll_time, + struct timespec& last_primitive_received_time) +{ + struct timespec current_time; + TbotsProto::Primitive new_primitive; + + { + ScopedTimespecTimer timer(&poll_time); + ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + new_primitive = network_service_->poll(robot_status_); + } + + thunderloop_status_.set_network_service_poll_time_ms(getMilliseconds(poll_time)); + + struct timespec time_since_last_primitive_received; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + ScopedTimespecTimer::timespecDiff(¤t_time, &last_primitive_received_time, + &time_since_last_primitive_received); + network_status_.set_ms_since_last_primitive_received( + getMilliseconds(time_since_last_primitive_received)); + + if (new_primitive.time_sent().epoch_timestamp_seconds() > + primitive_.time_sent().epoch_timestamp_seconds()) + { + primitive_ = new_primitive; + + if (primitive_.has_move()) + { + const Point position = + createPoint(primitive_.move().xy_traj_params().start_position()); + const Angle orientation = + createAngle(primitive_.move().w_traj_params().start_angle()); + + robot_localizer_.update( + RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); + } + + clock_gettime(CLOCK_MONOTONIC, &last_primitive_received_time); + + { + ScopedTimespecTimer timer(&poll_time); + primitive_executor_.updatePrimitive(primitive_); + } + + thunderloop_status_.set_primitive_executor_start_time_ms( + getMilliseconds(poll_time)); + } +} + +inline void Thunderloop::processLocalizationUpdates() +{ + const std::optional imu_poll = imu_service_->poll(); + + if (imu_poll.has_value() && imu_poll->angular_velocity.has_value()) + { + robot_localizer_.update( + RobotLocalizer::ImuData{imu_poll->angular_velocity.value()}); + } + + if (motor_status_.has_value()) + { + auto status = motor_status_.value(); + + robot_localizer_.update(RobotLocalizer::MotorData{ + localToGlobalVelocity(createVector(status.local_velocity()), + robot_localizer_.getOrientation()), + createAngularVelocity(status.angular_velocity())}); + + Vector linear_acceleration; + if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) + { + const auto accel = imu_poll->linear_acceleration.value(); + linear_acceleration = Vector(accel[1], accel[0]); + } + + robot_localizer_.step(linear_acceleration); + + primitive_executor_.updateState(robot_localizer_.getPosition(), + robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), + robot_localizer_.getAngularVelocity()); + } +} + +inline void Thunderloop::processPrimitiveExecution( + struct timespec& poll_time, struct timespec& last_primitive_received_time) +{ + struct timespec current_time; + struct timespec time_since_last_primitive_received; + + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + ScopedTimespecTimer::timespecDiff(¤t_time, &last_primitive_received_time, + &time_since_last_primitive_received); + + { + ScopedTimespecTimer timer(&poll_time); + ZoneNamedN(_tracy_step_primitive, "Thunderloop: Step Primitive", true); + + auto nanoseconds_elapsed_since_last_primitive = + getNanoseconds(time_since_last_primitive_received); + + if (nanoseconds_elapsed_since_last_primitive > PACKET_TIMEOUT_NS) + { + primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); + } + + direct_control_ = *primitive_executor_.stepPrimitive(primitive_executor_status_); + } + + thunderloop_status_.set_primitive_executor_step_time_ms(getMilliseconds(poll_time)); +} + +inline void Thunderloop::updateChickerStatus(struct timespec& last_chipper_fired, + struct timespec& last_kicker_fired) +{ + struct timespec current_time; + struct timespec time_diff; + + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + + ScopedTimespecTimer::timespecDiff(¤t_time, &last_kicker_fired, &time_diff); + chipper_kicker_status_.set_ms_since_kicker_fired(getMilliseconds(time_diff)); + + ScopedTimespecTimer::timespecDiff(¤t_time, &last_chipper_fired, &time_diff); + chipper_kicker_status_.set_ms_since_chipper_fired(getMilliseconds(time_diff)); + + if (direct_control_.power_control().chicker().has_kick_speed_m_per_s() || + direct_control_.power_control() + .chicker() + .auto_chip_or_kick() + .has_autokick_speed_m_per_s()) + { + clock_gettime(CLOCK_MONOTONIC, &last_kicker_fired); + } + else if (direct_control_.power_control().chicker().has_chip_distance_meters() || + direct_control_.power_control() + .chicker() + .auto_chip_or_kick() + .has_autochip_distance_meters()) + { + clock_gettime(CLOCK_MONOTONIC, &last_chipper_fired); + } +} + +inline void Thunderloop::updateAndSendRobotStatus(uint64_t last_handled_primitive_set) +{ + struct timespec current_time; + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + time_sent_.set_epoch_timestamp_seconds(static_cast(current_time.tv_sec)); + + robot_status_.set_robot_id(robot_id_); + robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); + *(robot_status_.mutable_time_sent()) = time_sent_; + *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; + *(robot_status_.mutable_motor_status()) = motor_status_.value_or(TbotsProto::MotorStatus()); + *(robot_status_.mutable_power_status()) = power_status_; + *(robot_status_.mutable_network_status()) = network_status_; + *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; + *(robot_status_.mutable_primitive_executor_status()) = primitive_executor_status_; + + updateErrorCodes(); +} + double Thunderloop::getMilliseconds(timespec time) { return (static_cast(time.tv_sec) * MILLISECONDS_PER_SECOND) + diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 94d43da359..347c655d83 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -3,13 +3,12 @@ #include #include #include -#include -#include #include "proto/tbots_software_msgs.pb.h" #include "shared/constants.h" #include "shared/robot_constants.h" #include "software/embedded/primitive_executor.h" +#include "software/embedded/robot_localizer.h" #include "software/embedded/services/imu.h" #include "software/embedded/services/motor.h" #include "software/embedded/services/network/network.h" @@ -125,6 +124,41 @@ class Thunderloop */ void waitForNetworkUp(); + /** + * Polls the network service for new primitives and updates timing status. + * + * @param poll_time Output for polling duration + * @param last_primitive_received_time Input/Output for tracking last received packet + */ + inline void processNetworkPolling(struct timespec& poll_time, + struct timespec& last_primitive_received_time); + + /** + * Processes robot localization updates from sensors (IMU, Motors). + */ + inline void processLocalizationUpdates(); + + /** + * Steps the primitive executor and handles timeouts. + * + * @param poll_time Output for execution duration + * @param last_primitive_received_time Input for checking timeouts + */ + inline void processPrimitiveExecution(struct timespec& poll_time, + struct timespec& last_primitive_received_time); + + /** + * Updates internal tracking for chipper/kicker firing events. + */ + inline void updateChickerStatus(struct timespec& last_chipper_fired, + struct timespec& last_kicker_fired); + + /** + * Aggregates and sends the final robot status message. + * @param last_handled_primitive_set + */ + inline void updateAndSendRobotStatus(uint64_t last_handled_primitive_set); + // Input Msg Buffers TbotsProto::World world_; @@ -157,6 +191,9 @@ class Thunderloop // Primitive Executor PrimitiveExecutor primitive_executor_; + // Robot localization model + RobotLocalizer robot_localizer_; + // 500 millisecond timeout on receiving primitives before we stop the robots const double PACKET_TIMEOUT_NS = 500.0 * NANOSECONDS_PER_MILLISECOND; diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index ebf0480ac3..9535e727d1 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -43,6 +43,7 @@ def __init__( blue_full_system_proto_unix_io, yellow_full_system_proto_unix_io, gamecontroller, + robot_communication, publish_validation_protos=True, is_yellow_friendly=False, ): @@ -53,6 +54,7 @@ def __init__( :param blue_full_system_proto_unix_io: The blue full system proto unix io to use :param yellow_full_system_proto_unix_io: The yellow full system proto unix io to use :param gamecontroller: The gamecontroller context managed instance + :param robot_communication: The robot communication instance :param publish_validation_protos: whether to publish validation protos :param: is_yellow_friendly: if yellow is the friendly team """ @@ -66,25 +68,45 @@ def __init__( ) self.publish_validation_protos = publish_validation_protos self.is_yellow_friendly = is_yellow_friendly + self.robot_communication = robot_communication logger.info("determining robots on field") # survey field for available robot ids - try: - world = self.world_buffer.get(block=True, timeout=WORLD_BUFFER_TIMEOUT) - self.initial_world = world - self.friendly_robot_ids_field = [ - robot.id for robot in world.friendly_team.team_robots - ] - - logger.info(f"friendly team ids {self.friendly_robot_ids_field}") - - if len(self.friendly_robot_ids_field) == 0: - raise Exception("no friendly robots found on field") + survey_start_time = time.time() + self.friendly_robot_ids_field = [] + while time.time() - survey_start_time < WORLD_BUFFER_TIMEOUT: + try: + world = self.world_buffer.get(block=True, timeout=0.1) + self.initial_world = world + self.friendly_robot_ids_field = [ + robot.id for robot in world.friendly_team.team_robots + ] + + if len(self.friendly_robot_ids_field) > 0: + logger.info(f"friendly team ids {self.friendly_robot_ids_field}") + break + except queue.Empty: + continue + + if len(self.friendly_robot_ids_field) == 0: + raise Exception("no friendly robots found on field within timeout") + + def wait_for_estop_play(self): + """Blocks until the estop is in the PLAY state""" + if self.robot_communication.estop_is_playing: + return + + logger.info("\x1b[33m" + "Waiting for Estop to be in PLAY state..." + "\x1b[0m") + while not self.robot_communication.estop_is_playing: + # We must process events if Thunderscope is running to keep it responsive + if self.thunderscope: + from pyqtgraph.Qt import QtWidgets - except queue.Empty: - raise Exception( - f"No Worlds were received with in {WORLD_BUFFER_TIMEOUT} seconds. Please make sure atleast 1 robot and 1 ball is present on the field." - ) + QtWidgets.QApplication.processEvents() + time.sleep(0.1) + logger.info( + "\x1b[32m" + "Estop is in PLAY state. Proceeding with test." + "\x1b[0m" + ) @override def send_gamecontroller_command( @@ -99,7 +121,7 @@ def send_gamecontroller_command( :param team: The team which the command as attributed to :param final_ball_placement_point: The ball placement point """ - self.gamecontroller.send_ci_input( + self.gamecontroller.send_gc_command( gc_command=gc_command, team=team, final_ball_placement_point=final_ball_placement_point, @@ -124,14 +146,19 @@ def run_test( def stop_test(delay): time.sleep(delay) - if self.thunderscope: - self.thunderscope.close() + # We no longer close thunderscope here, because a test might call run_test multiple times. + # Thunderscope will be closed when the fixture is torn down. def __runner(): time.sleep(LAUNCH_DELAY_S) test_end_time = time.time() + test_timeout_s + # Keep track if we started with any eventually validations + has_eventually_validations = any( + len(seq) > 0 for seq in eventually_validation_sequence_set + ) + while time.time() < test_end_time: while True: try: @@ -172,9 +199,15 @@ def __runner(): # Check that all always validations are always valid validation.check_validation(always_validation_proto_set) - # Check that all eventually validations are eventually valid + # Break if eventually validation passes + if has_eventually_validations and all( + len(seq) == 0 for seq in eventually_validation_sequence_set + ): + break + validation.check_validation(eventually_validation_proto_set) - stop_test(TEST_END_DELAY) + + stop_test(delay=PROCESS_BUFFER_DELAY_S) def excepthook(args): """This function is _critical_ for show_thunderscope to work. @@ -189,14 +222,22 @@ def excepthook(args): threading.excepthook = excepthook + # If visualization is enabled, we need to be careful. + # Thunderscope.show() is blocking. if self.thunderscope: run_test_thread = threading.Thread(target=__runner, daemon=True) run_test_thread.start() - self.thunderscope.show() + + # Only call show if the window is not already open. + # If it IS open, it means we are ALREADY in the Qt event loop, + # which can only happen if we are running this run_test in a background thread. + if not self.thunderscope.is_open(): + self.thunderscope.show() + run_test_thread.join() if self.last_exception: - pytest.fail(str(ex.last_exception)) + pytest.fail(str(self.last_exception)) else: __runner() @@ -322,6 +363,13 @@ def load_command_line_arguments(): help="Disables checking for estop plugged in (ONLY USE FOR LOCAL TESTING)", ) + parser.add_argument( + "--no_visualization", + action="store_true", + default=False, + help="Disables the Thunderscope GUI", + ) + return parser.parse_args() @@ -388,15 +436,17 @@ def field_test_runner(): simulator_proto_unix_io=simulator_proto_unix_io, ) # Inject the proto unix ios into thunderscope and start the test - tscope = Thunderscope( - configure_field_test_view( - simulator_proto_unix_io=simulator_proto_unix_io, - blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, - yellow_is_friendly=args.run_yellow, - ), - layout_path=None, - ) + tscope = None + if not args.no_visualization: + tscope = Thunderscope( + configure_field_test_view( + simulator_proto_unix_io=simulator_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + yellow_is_friendly=args.run_yellow, + ), + layout_path=None, + ) # Set control mode for all robots to AI so that packets are sent to the robots for robot_id in range(MAX_ROBOT_IDS_PER_SIDE): @@ -407,9 +457,10 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: - tscope.keyboard_estop_shortcut.activated.connect( - rc_friendly.toggle_keyboard_estop - ) + if tscope: + tscope.keyboard_estop_shortcut.activated.connect( + rc_friendly.toggle_keyboard_estop + ) # we call this method to enable estop automatically when a field test starts rc_friendly.toggle_keyboard_estop() logger.warning( @@ -425,6 +476,7 @@ def field_test_runner(): yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, gamecontroller=gamecontroller, thunderscope=tscope, + robot_communication=rc_friendly, is_yellow_friendly=args.run_yellow, ) diff --git a/src/software/field_tests/movement_robot_field_test.py b/src/software/field_tests/movement_robot_field_test.py index 56c31df60b..2aaa6fa7e7 100644 --- a/src/software/field_tests/movement_robot_field_test.py +++ b/src/software/field_tests/movement_robot_field_test.py @@ -3,68 +3,10 @@ from software.simulated_tests.simulated_test_fixture import * from software.logger.logger import create_logger -import math logger = create_logger(__name__) - - -# TODO 2908: Support running this test in both simulator or field mode -# this test can be run either in simulation or on the field -# @pytest.mark.parametrize( -# "robot_x_destination, robot_y_destination", -# [(-2.0, -1), (-2.0, 1.0), (0.0, 1.0), (0.0, -1.0)], -# ) -# def test_basic_movement(simulated_test_runner): -# -# robot_starting_x = 0 -# robot_starting_y = 0 -# ROBOT_ID = 0 -# angle = 0 -# robot_x_destination = -2 -# robot_y_destination = -1 -# rob_pos_p = Point(x_meters=robot_x_destination, y_meters=robot_y_destination) -# -# move_tactic = MoveTactic() -# move_tactic.destination.CopyFrom(rob_pos_p) -# move_tactic.dribbler_mode = DribblerMode.OFF -# move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) -# move_tactic.ball_collision_type = BallCollisionType.AVOID -# move_tactic.auto_chip_or_kick.CopyFrom(AutoChipOrKick(autokick_speed_m_per_s=0.0)) -# move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT -# move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE -# -# # setup world state -# initial_worldstate = create_world_state( -# yellow_robot_locations=[], -# blue_robot_locations=[tbots_cpp.Point(robot_starting_x, robot_starting_y)], -# ball_location=tbots_cpp.Point(1, 1), -# ball_velocity=tbots_cpp.Point(0, 0), -# ) -# simulated_test_runner.set_world_state(initial_worldstate) -# -# # Setup Tactic -# params = AssignedTacticPlayControlParams() -# -# params.assigned_tactics[ROBOT_ID].move.CopyFrom(move_tactic) -# -# # Eventually Validation -# eventually_validation_sequence_set = [ -# [ -# RobotEventuallyEntersRegion( -# regions=[ -# tbots_cpp.Circle( -# tbots_cpp.Point(robot_x_destination, robot_y_destination), 0.2 -# ) -# ] -# ), -# ] -# ] -# simulated_test_runner.set_tactics(params, True) -# -# simulated_test_runner.run_test( -# eventually_validation_sequence_set=eventually_validation_sequence_set, -# test_timeout_s=5, -# ) +import math +import threading # this test can only be run on the field @@ -88,38 +30,60 @@ def test_basic_rotation(field_test_runner): robot = world.friendly_team.team_robots[0] rob_pos_p = robot.current_state.global_position - logger.info("staying in pos {rob_pos_p}") - - for angle in test_angles: - move_tactic = MoveTactic() - move_tactic.destination.CopyFrom(rob_pos_p) - move_tactic.dribbler_mode = DribblerMode.OFF - move_tactic.final_orientation.CopyFrom(Angle(radians=angle)) - move_tactic.ball_collision_type = BallCollisionType.AVOID - move_tactic.auto_chip_or_kick.CopyFrom( - AutoChipOrKick(autokick_speed_m_per_s=0.0) - ) - move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT - move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE - # Setup Tactic - field_test_runner.set_tactics( - blue_tactics={id: move_tactic}, yellow_tactics=None - ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=5, + def execute_test(): + # Wait for the user to flip the estop to PLAY + field_test_runner.wait_for_estop_play() + + # Force start the game automatically + field_test_runner.gamecontroller.send_gc_command( + proto.ssl_gc_state_pb2.Command.FORCE_START, + proto.ssl_gc_common_pb2.Team.UNKNOWN, + final_ball_placement_point=None, ) + + for angle in test_angles: + print(f"Rotating to {angle} degrees") + move_tactic = MoveTactic() + move_tactic.destination.CopyFrom(rob_pos_p) + move_tactic.dribbler_mode = DribblerMode.OFF + move_tactic.final_orientation.CopyFrom( + Angle(radians=angle * math.pi / 180.0) + ) + move_tactic.ball_collision_type = BallCollisionType.AVOID + move_tactic.auto_chip_or_kick.CopyFrom( + AutoChipOrKick(autokick_speed_m_per_s=0.0) + ) + move_tactic.max_allowed_speed_mode = MaxAllowedSpeedMode.PHYSICAL_LIMIT + move_tactic.obstacle_avoidance_mode = ObstacleAvoidanceMode.SAFE + + # Setup Tactic + field_test_runner.set_tactics( + blue_tactics={id: move_tactic}, yellow_tactics=None + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=5, + ) + + # validate by eye + logger.info(f"robot set to {angle} orientation") + time.sleep(2) + # Send a halt tactic after the test finishes field_test_runner.set_tactics( blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - # validate by eye - logger.info(f"robot set to {angle} orientation") + test_thread = threading.Thread(target=execute_test, daemon=True) + test_thread.start() - time.sleep(2) + # If thunderscope is enabled, show it in the main thread (blocking) + if field_test_runner.thunderscope: + field_test_runner.thunderscope.show() + + test_thread.join() def test_one_robots_square(field_test_runner): @@ -138,10 +102,10 @@ def test_one_robots_square(field_test_runner): id = world.friendly_team.team_robots[0].id print(f"Running test on robot {id}") - point1 = Point(x_meters=-0.3, y_meters=0.6) - point2 = Point(x_meters=-0.3, y_meters=-0.6) - point3 = Point(x_meters=-1.5, y_meters=-0.6) - point4 = Point(x_meters=-1.5, y_meters=0.6) + point1 = Point(x_meters=0.4, y_meters=0.4) + point2 = Point(x_meters=0.4, y_meters=-0.4) + point3 = Point(x_meters=-0.4, y_meters=-0.4) + point4 = Point(x_meters=-0.4, y_meters=0.4) tactic_0 = MoveTactic( destination=point1, @@ -179,25 +143,48 @@ def test_one_robots_square(field_test_runner): max_allowed_speed_mode=MaxAllowedSpeedMode.PHYSICAL_LIMIT, obstacle_avoidance_mode=ObstacleAvoidanceMode.SAFE, ) - tactics = [tactic_0, tactic_1, tactic_2, tactic_3] - for tactic in tactics: - print(f"Going to {tactic.destination}") + tactics = [tactic_0, tactic_1, tactic_2, tactic_3, tactic_0] - field_test_runner.set_tactics( - blue_tactics={ - id: tactic, - }, - yellow_tactics=None, + def execute_test(): + # Wait for the user to flip the estop to PLAY + field_test_runner.wait_for_estop_play() + + # Force start the game automatically + field_test_runner.gamecontroller.send_gc_command( + proto.ssl_gc_state_pb2.Command.FORCE_START, + proto.ssl_gc_common_pb2.Team.UNKNOWN, + final_ball_placement_point=None, ) - field_test_runner.run_test( - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], - test_timeout_s=4, + + for tactic in tactics: + print(f"Going to {tactic.destination}") + + field_test_runner.set_tactics( + blue_tactics={ + id: tactic, + }, + yellow_tactics=None, + ) + field_test_runner.run_test( + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + test_timeout_s=8, + ) + + # Send a halt tactic after the test finishes + field_test_runner.set_tactics( + blue_tactics={id: HaltTactic()}, yellow_tactics=None ) - # Send a halt tactic after the test finishes - field_test_runner.set_tactics(blue_tactics={id: HaltTactic()}, yellow_tactics=None) + test_thread = threading.Thread(target=execute_test, daemon=True) + test_thread.start() + + # If thunderscope is enabled, show it in the main thread (blocking) + if field_test_runner.thunderscope: + field_test_runner.thunderscope.show() + + test_thread.join() if __name__ == "__main__": diff --git a/src/software/logger/network_logger.cpp b/src/software/logger/network_logger.cpp index f14ed77157..833d103ef3 100644 --- a/src/software/logger/network_logger.cpp +++ b/src/software/logger/network_logger.cpp @@ -14,18 +14,19 @@ NetworkLoggerSingleton::NetworkLoggerSingleton(RobotId robot_id, bool enable_log logWorker->addSink(std::make_unique(robot_id, enable_log_merging), &NetworkSink::sendToNetwork); - // Sink for outputting logs to the terminal - auto colour_cout_sink_handle = logWorker->addSink( - std::make_unique(true), &ColouredCoutSink::displayColouredLog); + logWorker->addSink(std::make_unique(true), + &ColouredCoutSink::displayColouredLog); - auto csv_sink_handle = - logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); + logWorker->addSink(std::make_unique(CSV_PATH), &CSVSink::appendToFile); // Sink for PlotJuggler plotting auto plotjuggler_handle = logWorker->addSink(std::make_unique(network_interface), &PlotJugglerSink::sendToPlotJuggler); + g3::only_change_at_initialization::addLogLevel(CSV); + g3::only_change_at_initialization::addLogLevel(PLOTJUGGLER); + g3::initializeLogging(logWorker.get()); } @@ -34,7 +35,7 @@ void NetworkLoggerSingleton::initializeLogger(RobotId robot_id, bool enable_log_ { if (!instance) { - NetworkLoggerSingleton::instance = std::shared_ptr( + instance = std::shared_ptr( new NetworkLoggerSingleton(robot_id, enable_log_merging, network_interface)); } } diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..7593b87ce1 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -280,16 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::BLUE, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants, - TeamColour::YELLOW, id); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -299,19 +297,22 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.yellow_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.yellow_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); yellow_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *yellow_team_world_msg; + for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -320,20 +321,22 @@ void ErForceSimulator::setBlueRobotPrimitiveSet( const TbotsProto::PrimitiveSet& primitive_set_msg, std::unique_ptr world_msg) { - auto sim_state = getSimulatorState(); - const auto& sim_robots = sim_state.blue_robots(); - const auto robot_to_vel_pair_map = getRobotIdToLocalVelocityMap(sim_robots); + auto sim_state = getSimulatorState(); + const auto& sim_robots = sim_state.blue_robots(); + const auto robot_to_state = getRobotIdToStateMap(sim_robots); blue_team_world_msg = std::move(world_msg); const TbotsProto::World world_proto = *blue_team_world_msg; for (auto& [robot_id, primitive] : primitive_set_msg.robot_primitives()) { - if (robot_to_vel_pair_map.contains(robot_id)) + if (robot_to_state.contains(robot_id)) { - auto& [local_vel, angular_vel] = robot_to_vel_pair_map.at(robot_id); + const auto& [position, orientation, velocity, angular_velocity] = + robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, blue_primitive_executor_map, - world_proto, local_vel, angular_vel); + world_proto, position, orientation, velocity, + angular_velocity); } } } @@ -342,8 +345,8 @@ void ErForceSimulator::setRobotPrimitive( RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity) + const TbotsProto::World& world_msg, const Point& position, const Angle& orientation, + const Vector& velocity, const AngularVelocity& angular_velocity) { // Set to NEG_X because the world msg in this simulator is normalized // correctly @@ -352,8 +355,9 @@ void ErForceSimulator::setRobotPrimitive( if (robot_primitive_executor_iter != robot_primitive_executor_map.end()) { auto primitive_executor = robot_primitive_executor_iter->second; + primitive_executor->updateState(position, velocity, orientation, + angular_velocity); primitive_executor->updatePrimitive(primitive_set_msg.robot_primitives().at(id)); - primitive_executor->updateVelocity(local_velocity, angular_velocity); } else { @@ -370,16 +374,16 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( SSLSimulationProto::RobotControl robot_control; auto sim_state = getSimulatorState(); - std::map> current_velocity_map; + std::map> current_state; if (side == gameController::Team::BLUE) { const auto& sim_robots = sim_state.blue_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } else { const auto& sim_robots = sim_state.yellow_robots(); - current_velocity_map = getRobotIdToLocalVelocityMap(sim_robots); + current_state = getRobotIdToStateMap(sim_robots); } for (auto& primitive_executor_with_id : robot_primitive_executor_map) @@ -393,8 +397,9 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), + std::get<1>(current_state.at(robot_id))), + std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, primitive_executor_time_step_s); } else @@ -564,18 +569,18 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } -std::map> -ErForceSimulator::getRobotIdToLocalVelocityMap( +std::map> +ErForceSimulator::getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots) { - std::map> robot_to_local_velocity; + std::map> map; for (const auto& sim_robot : sim_robots) { - const Vector local_vel = - globalToLocalVelocity(Vector(sim_robot.v_x(), sim_robot.v_y()), - Angle::fromRadians(sim_robot.angle())); - const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); - robot_to_local_velocity[sim_robot.id()] = {local_vel, angular_vel}; + const Point position = {sim_robot.p_x(), sim_robot.p_y()}; + const Angle orientation = Angle::fromRadians(sim_robot.angle()); + const Vector velocity = {sim_robot.v_x(), sim_robot.v_y()}; + const AngularVelocity angular_vel = Angle::fromRadians(sim_robot.r_z()); + map[sim_robot.id()] = {position, orientation, velocity, angular_vel}; } - return robot_to_local_velocity; + return map; } diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..9b65c81449 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -158,18 +158,19 @@ class ErForceSimulator RobotId id, const TbotsProto::PrimitiveSet& primitive_set_msg, std::unordered_map>& robot_primitive_executor_map, - const TbotsProto::World& world_msg, const Vector& local_velocity, - const AngularVelocity angular_velocity); + const TbotsProto::World& world_msg, const Point& position, + const Angle& orientation, const Vector& velocity, + const AngularVelocity& angular_velocity); /** - * Gets a map from robot id to local and angular velocity from repeated sim robots + * Gets a map from robot id to position and velocity from repeated sim robots * * @param sim_robots Repeated er force sim robot protos * - * @return a map from robot id to local velocity and angular velocity + * @return a map from robot id to position and velocity */ - static std::map> - getRobotIdToLocalVelocityMap( + static std::map> + getRobotIdToStateMap( const google::protobuf::RepeatedPtrField& sim_robots); /** diff --git a/src/software/util/pid/BUILD b/src/software/util/pid/BUILD new file mode 100644 index 0000000000..156e74085e --- /dev/null +++ b/src/software/util/pid/BUILD @@ -0,0 +1,8 @@ +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "pid", + hdrs = [ + "pid_controller.hpp", + ], +) diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp new file mode 100644 index 0000000000..d3e673efc2 --- /dev/null +++ b/src/software/util/pid/pid_controller.hpp @@ -0,0 +1,82 @@ +#pragma once + +#include + +namespace controls +{ +template +class PIDController +{ + public: + /** + * Constructs a new PID controller. + * + * A PID controller is used to calculate corrections based on error values over + * time as the difference between a desired value and the actual measured value. + * This PID controller also limits integral windup using a max_integral value. + * + * Resources: + * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ + * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ + * + * @pre max_integral must be >= 0.0 + * + * @param k_p The proportional gain. + * @param k_i The integral gain. + * @param k_d The derivative gain. + * @param max_integral The maximum absolute value that the integrator term can + * accumulate to. + **/ + PIDController(T k_p, T k_i, T k_d, T max_integral); + + /** + * Given an error, returns the control effort to minimize it. + * + * @param deltaTime The time passed since last step, for calculating integrator and + *derivative. If this function is calling in invariant intervals, deltaTime can be set + *to 1 and any effects it would have can be handled by tuning kD. + **/ + T step(T error, T delta_time = 1.0f); + + /** + * Resets the integrator and clears the last error used for derivative calculation. + **/ + void reset(); + + T k_p; + T k_i; + T k_d; + T max_integral; + + protected: + T integral; + std::optional last_error = std::nullopt; +}; +} // namespace controls + +template +controls::PIDController::PIDController(T k_p, T k_i, T k_d, T max_integral) + : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral){}; + +template +T controls::PIDController::step(T error, T delta_time) +{ + // If sign of error swaps, reset integrator + if (last_error.value_or(0) * error < 0) + { + integral = 0; + } + + integral += std::max(std::min(error * delta_time, max_integral), -max_integral); + // set derivative, if no last_error, just set to 0 + const T derivative = (last_error.value_or(error) - error) / delta_time; + last_error = error; + return error * k_p + integral * k_i + derivative * k_d; +} + +template +void controls::PIDController::reset() +{ + integral = 0; + last_error = std::nullopt; +} diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 66e2244d76..5a13d519a6 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -218,7 +218,8 @@ Duration Robot::getTimeToPosition(const Point& destination, double initial_velocity_1d = velocity().dot(dist_vector.normalize()); double final_velocity_1d = final_velocity.dot(dist_vector.normalize()); - return getTimeToTravelDistance(dist, robot_constants_.robot_max_speed_m_per_s, + return getTimeToTravelDistance(dist, + robot_constants_.robot_max_speed_trajectory_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); } From fa58f79194cbb9f7be0822a5895145803632fe36 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 00:34:01 -0700 Subject: [PATCH 02/20] linting --- .../ai/navigator/trajectory/trajectory.hpp | 3 +-- src/software/embedded/BUILD | 4 ++-- .../motion_control/pid_controller_test.cpp | 3 ++- src/software/embedded/primitive_executor.cpp | 19 +++++++-------- src/software/embedded/primitive_executor.h | 12 +++++----- src/software/embedded/thunderloop.cpp | 24 +++++++++---------- src/software/embedded/thunderloop.h | 2 +- 7 files changed, 33 insertions(+), 34 deletions(-) diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 57789684a3..4f970c2e7a 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -67,6 +67,5 @@ class Trajectory * different * @return True if the trajectories are different, false otherwise */ - virtual bool isNew(const Trajectory& other, - double threshold) const = 0; + virtual bool isNew(const Trajectory& other, double threshold) const = 0; }; diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 2ae80efcd7..6bc28d668e 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -34,11 +34,11 @@ cc_library( "//proto/primitive:primitive_msg_factory", "//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular", "//software/ai/navigator/trajectory:trajectory_path", + "//software/embedded/motion_control:orientation_controller", + "//software/embedded/motion_control:position_controller", "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", - "//software/embedded/motion_control:orientation_controller", - "//software/embedded/motion_control:position_controller", "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", diff --git a/src/software/embedded/motion_control/pid_controller_test.cpp b/src/software/embedded/motion_control/pid_controller_test.cpp index 307100d84a..90248c5459 100644 --- a/src/software/embedded/motion_control/pid_controller_test.cpp +++ b/src/software/embedded/motion_control/pid_controller_test.cpp @@ -43,7 +43,8 @@ TEST(PidControllerTest, OnlyIntegralTermNonZero) // should not accumulate integral term above max_integral EXPECT_DOUBLE_EQ(pid.step(6.7, 1.0), k_i * 5.5); // Sign not swapped from 0.0 to 6.7 - EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 10.0) + EXPECT_DOUBLE_EQ(pid.step(5.0, 1.0), + k_i * 10.0); // Clamped (5.5 + 5.0 = 10.5 -> 10.0) EXPECT_DOUBLE_EQ(pid.step(1.0, 1.0), k_i * 10.0); } diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 9c1783ef07..fea41e7295 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -12,8 +12,7 @@ PrimitiveExecutor::PrimitiveExecutor( const robot_constants::RobotConstants& robot_constants) - : last_step_time_(std::chrono::steady_clock::now()), - robot_constants_(robot_constants) + : last_step_time_(std::chrono::steady_clock::now()), robot_constants_(robot_constants) { } @@ -49,7 +48,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const bool is_angular_traj_new = !angular_trajectory_.has_value() || angular_trajectory_->isNew(new_angular_trajectory, - ANGULAR_DESTINATION_THRESHOLD_DEGREES); + ANGULAR_DESTINATION_THRESHOLD_DEGREES); if (is_angular_traj_new) { @@ -115,10 +114,10 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status) { - const auto current_time = std::chrono::steady_clock::now(); + const auto current_time = std::chrono::steady_clock::now(); const auto delta_time_chrono = current_time - last_step_time_; - const Duration delta_time = Duration::fromSeconds( - std::chrono::duration(delta_time_chrono).count()); + const Duration delta_time = + Duration::fromSeconds(std::chrono::duration(delta_time_chrono).count()); time_since_linear_trajectory_creation_ += delta_time_chrono; time_since_angular_trajectory_creation_ += delta_time_chrono; @@ -154,10 +153,10 @@ std::unique_ptr PrimitiveExecutor::stepPrimi prim->direct_control()); } - const Duration elapsed_linear = Duration::fromSeconds( - time_since_linear_trajectory_creation_.count()); - const Duration elapsed_angular = Duration::fromSeconds( - time_since_angular_trajectory_creation_.count()); + const Duration elapsed_linear = + Duration::fromSeconds(time_since_linear_trajectory_creation_.count()); + const Duration elapsed_angular = + Duration::fromSeconds(time_since_angular_trajectory_creation_.count()); const Vector target_linear_velocity_global = position_controller_.step( position_, *trajectory_path_, elapsed_linear, delta_time); diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 952816282a..1be81b2483 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -4,10 +4,10 @@ #include "proto/tbots_software_msgs.pb.h" #include "software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h" #include "software/ai/navigator/trajectory/trajectory_path.h" -#include "software/geom/vector.h" -#include "software/world/robot_state.h" #include "software/embedded/motion_control/orientation_controller.h" #include "software/embedded/motion_control/position_controller.h" +#include "software/geom/vector.h" +#include "software/world/robot_state.h" class PrimitiveExecutor { @@ -77,8 +77,8 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index fca9b69cd1..433b7b0129 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -219,8 +219,8 @@ void Thunderloop::runLoop() } } -inline void Thunderloop::processNetworkPolling(struct timespec& poll_time, - struct timespec& last_primitive_received_time) +inline void Thunderloop::processNetworkPolling( + struct timespec& poll_time, struct timespec& last_primitive_received_time) { struct timespec current_time; TbotsProto::Primitive new_primitive; @@ -296,10 +296,9 @@ inline void Thunderloop::processLocalizationUpdates() robot_localizer_.step(linear_acceleration); - primitive_executor_.updateState(robot_localizer_.getPosition(), - robot_localizer_.getVelocity(), - robot_localizer_.getOrientation(), - robot_localizer_.getAngularVelocity()); + primitive_executor_.updateState( + robot_localizer_.getPosition(), robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity()); } } @@ -371,12 +370,13 @@ inline void Thunderloop::updateAndSendRobotStatus(uint64_t last_handled_primitiv robot_status_.set_robot_id(robot_id_); robot_status_.set_last_handled_primitive_set(last_handled_primitive_set); - *(robot_status_.mutable_time_sent()) = time_sent_; - *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; - *(robot_status_.mutable_motor_status()) = motor_status_.value_or(TbotsProto::MotorStatus()); - *(robot_status_.mutable_power_status()) = power_status_; - *(robot_status_.mutable_network_status()) = network_status_; - *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; + *(robot_status_.mutable_time_sent()) = time_sent_; + *(robot_status_.mutable_thunderloop_status()) = thunderloop_status_; + *(robot_status_.mutable_motor_status()) = + motor_status_.value_or(TbotsProto::MotorStatus()); + *(robot_status_.mutable_power_status()) = power_status_; + *(robot_status_.mutable_network_status()) = network_status_; + *(robot_status_.mutable_chipper_kicker_status()) = chipper_kicker_status_; *(robot_status_.mutable_primitive_executor_status()) = primitive_executor_status_; updateErrorCodes(); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 347c655d83..8aa4423991 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -145,7 +145,7 @@ class Thunderloop * @param last_primitive_received_time Input for checking timeouts */ inline void processPrimitiveExecution(struct timespec& poll_time, - struct timespec& last_primitive_received_time); + struct timespec& last_primitive_received_time); /** * Updates internal tracking for chipper/kicker firing events. From 1cd0268a7c39655eaf8c6dda3a4eede25213ee39 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 11:00:32 -0700 Subject: [PATCH 03/20] remove auto test --- src/software/embedded/services/BUILD | 19 +------------------ 1 file changed, 1 insertion(+), 18 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index c0d1eff518..1aee8c7016 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -52,21 +52,4 @@ cc_library( ], ) -cc_binary( - name = "robot_auto_test", - srcs = ["robot_auto_test.cpp"], - linkopts = [ - "-lpthread", - "-lrt", - ], - deps = [ - ":motor", - ":power", - "//proto/message_translation:tbots_geometry", - "//proto/primitive:primitive_msg_factory", - "//shared:robot_constants", - "//software/embedded:primitive_executor", - "//software/logger", - "@trinamic", - ], -) + From aff8056f0aa43e6b62f17f1d1a4e5fb9f0024da1 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 11:01:24 -0700 Subject: [PATCH 04/20] remove auto test --- src/software/embedded/services/BUILD | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/software/embedded/services/BUILD b/src/software/embedded/services/BUILD index 1aee8c7016..fa9571a16c 100644 --- a/src/software/embedded/services/BUILD +++ b/src/software/embedded/services/BUILD @@ -51,5 +51,3 @@ cc_library( "@eigen", ], ) - - From 41df92c60cfa3895b55652cdcaa9d874081aa92a Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 14:52:35 -0700 Subject: [PATCH 05/20] test fixture fixes --- src/shared/robot_constants.h | 12 +- .../ball_placement_play_test.py | 4 +- .../kickoff_enemy/kickoff_enemy_play_test.py | 11 +- .../kickoff_friendly_play_fsm.cpp | 2 +- .../kickoff_friendly_play_test.py | 11 +- .../motion_control/orientation_controller.h | 4 +- .../motion_control/position_controller.h | 6 +- src/software/embedded/primitive_executor.cpp | 46 ++- src/software/embedded/primitive_executor.h | 14 +- src/software/embedded/thunderloop.cpp | 12 +- src/software/embedded/thunderloop.h | 6 +- .../simulated_tests/simulated_test_fixture.py | 270 +++++++----------- .../simulation/er_force_simulator.cpp | 16 +- 13 files changed, 168 insertions(+), 246 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 620861d8aa..e4eb334f96 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_max_speed_trajectory_m_per_s = 2.5f, - .robot_max_acceleration_m_per_s_2 = 2.0f, - .robot_max_deceleration_m_per_s_2 = 1.5f, + .robot_max_speed_trajectory_m_per_s = 3.0f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 6.0f, - .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f, diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py index 77e4e196ab..ef3132c563 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py @@ -200,7 +200,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, ag_always_validation_sequence_set=[[]], ag_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, - test_timeout_s=[15], + test_timeout_s=15, ) simulated_test_runner.run_test( @@ -209,7 +209,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, ag_always_validation_sequence_set=drop_ball_always_validation_sequence_set, ag_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, - test_timeout_s=[5], + test_timeout_s=5, ) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py index ce8968de8f..a76fa9b3a7 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py @@ -57,14 +57,6 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.YELLOW ) - # Let robots get ready before starting kickoff - threading.Timer( - 4.0, - lambda: simulated_test_runner.send_gamecontroller_command( - gc_command=Command.Type.NORMAL_START, team=Team.YELLOW - ), - ).start() - simulated_test_runner.set_plays( blue_play=PlayName.KickoffEnemyPlay, yellow_play=PlayName.KickoffFriendlyPlay, @@ -145,6 +137,9 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, + ci_cmd_with_delay=[ + (4.0, Command.Type.NORMAL_START, Team.YELLOW), + ], ) diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp index 7c1afd31f9..a2cb6c42af 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp @@ -42,7 +42,7 @@ void KickoffFriendlyPlayFSM::createKickoffSetupPositions(const WorldPtr& world_p kickoff_setup_positions = { // Robot 1 Point(world_ptr->field().centerPoint() + - Vector(-world_ptr->field().centerCircleRadius(), 0)), + Vector(-0.95 * world_ptr->field().centerCircleRadius(), 0)), // Robot 2 // Goalie positions will be handled by the goalie tactic // Robot 3 diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py index a7e33d5d34..6e85349b02 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py @@ -62,14 +62,6 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.BLUE ) - # Let robots get ready before starting kickoff - threading.Timer( - 4.0, - lambda: simulated_test_runner.send_gamecontroller_command( - gc_command=Command.Type.NORMAL_START, team=Team.BLUE - ), - ).start() - simulated_test_runner.set_plays( blue_play=PlayName.KickoffFriendlyPlay, yellow_play=PlayName.KickoffEnemyPlay, @@ -145,6 +137,9 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, + ci_cmd_with_delay=[ + (4.0, Command.Type.NORMAL_START, Team.BLUE), + ], ) diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 63b90ed6d1..8799380e9b 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -38,7 +38,7 @@ class OrientationController private: // TODO(#3737): tune constants PidController w_pid_{0.7, 0.0, 2.0, 0.0}; - PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; + PidController w_pid_close_{10.0, 0.0, 4.0, 0.0}; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; }; diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index 12725e09d7..d3fc239d10 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -38,8 +38,8 @@ class PositionController : public MotionController x_pid_{0.8, 0.0, 0.0, 0.0}; PidController y_pid_{0.8, 0.0, 0.0, 0.0}; - PidController x_pid_close_{2.0, 0.0, 0.0, 0.0}; - PidController y_pid_close_{2.0, 0.0, 0.0, 0.0}; + PidController x_pid_close_{5.0, 0.0, 0.0, 0.0}; + PidController y_pid_close_{5.0, 0.0, 0.0, 0.0}; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index fea41e7295..66c818dc79 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -12,7 +12,9 @@ PrimitiveExecutor::PrimitiveExecutor( const robot_constants::RobotConstants& robot_constants) - : last_step_time_(std::chrono::steady_clock::now()), robot_constants_(robot_constants) + : time_since_linear_trajectory_creation_(Duration::fromSeconds(0)), + time_since_angular_trajectory_creation_(Duration::fromSeconds(0)), + robot_constants_(robot_constants) { } @@ -35,8 +37,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_linear_traj_new) { trajectory_path_ = new_trajectory_path; - time_since_linear_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } @@ -53,8 +54,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_angular_traj_new) { angular_trajectory_ = new_angular_trajectory; - time_since_angular_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); orientation_controller_.reset(); } } @@ -75,7 +75,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit { const double linear_following_error = (position_ - trajectory_path_->getPosition( - time_since_linear_trajectory_creation_.count())) + time_since_linear_trajectory_creation_.toSeconds())) .length(); if (linear_following_error > LINEAR_STALL_ERROR_MAX_METERS) @@ -85,8 +85,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit createTrajectoryPathFromParams(current_primitive_.move().xy_traj_params(), position_, velocity_, robot_constants_); - time_since_linear_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); } } @@ -94,7 +93,7 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit { const double angular_following_error = angular_trajectory_ - ->getPosition(time_since_angular_trajectory_creation_.count()) + ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) .minDiff(orientation_) .toDegrees(); @@ -105,23 +104,16 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit current_primitive_.move().w_traj_params(), orientation_, angular_velocity_, robot_constants_); - time_since_angular_trajectory_creation_ = - std::chrono::duration::zero(); + time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); } } } std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status) + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time) { - const auto current_time = std::chrono::steady_clock::now(); - const auto delta_time_chrono = current_time - last_step_time_; - const Duration delta_time = - Duration::fromSeconds(std::chrono::duration(delta_time_chrono).count()); - - time_since_linear_trajectory_creation_ += delta_time_chrono; - time_since_angular_trajectory_creation_ += delta_time_chrono; - last_step_time_ = current_time; + time_since_linear_trajectory_creation_ = time_since_linear_trajectory_creation_ + delta_time; + time_since_angular_trajectory_creation_ = time_since_angular_trajectory_creation_ + delta_time; status.set_running_primitive(true); @@ -153,16 +145,14 @@ std::unique_ptr PrimitiveExecutor::stepPrimi prim->direct_control()); } - const Duration elapsed_linear = - Duration::fromSeconds(time_since_linear_trajectory_creation_.count()); - const Duration elapsed_angular = - Duration::fromSeconds(time_since_angular_trajectory_creation_.count()); - const Vector target_linear_velocity_global = position_controller_.step( - position_, *trajectory_path_, elapsed_linear, delta_time); + position_, *trajectory_path_, time_since_linear_trajectory_creation_, + delta_time); - const AngularVelocity target_angular_velocity = orientation_controller_.step( - orientation_, *angular_trajectory_, elapsed_angular, delta_time); + const AngularVelocity target_angular_velocity = + orientation_controller_.step(orientation_, *angular_trajectory_, + time_since_angular_trajectory_creation_, + delta_time); Vector target_linear_velocity_local = globalToLocalVelocity(target_linear_velocity_global, orientation_); diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 1be81b2483..bba3069456 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -42,10 +42,11 @@ class PrimitiveExecutor * target wheel velocities * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive + * @param delta_time The time passed since the last step * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status); + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time); private: TbotsProto::Primitive current_primitive_; @@ -53,9 +54,8 @@ class PrimitiveExecutor std::optional trajectory_path_; std::optional angular_trajectory_; - std::chrono::time_point last_step_time_; - std::chrono::duration time_since_linear_trajectory_creation_; - std::chrono::duration time_since_angular_trajectory_creation_; + Duration time_since_linear_trajectory_creation_; + Duration time_since_angular_trajectory_creation_; Point position_; Vector velocity_; @@ -77,8 +77,8 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.2; static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; }; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 433b7b0129..aab30afeba 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -192,11 +192,15 @@ void Thunderloop::runLoop() FrameMarkStart(TracyConstants::THUNDERLOOP_FRAME_MARKER); ScopedTimespecTimer iteration_timer(&iteration_time); + const Duration delta_time = Duration::fromSeconds( + getMilliseconds(time_since_prev_iter) * SECONDS_PER_MILLISECOND); + processNetworkPolling(poll_time, last_primitive_received_time); processLocalizationUpdates(); - processPrimitiveExecution(poll_time, last_primitive_received_time); + processPrimitiveExecution(poll_time, last_primitive_received_time, + delta_time); updateChickerStatus(last_chipper_fired, last_kicker_fired); @@ -303,7 +307,8 @@ inline void Thunderloop::processLocalizationUpdates() } inline void Thunderloop::processPrimitiveExecution( - struct timespec& poll_time, struct timespec& last_primitive_received_time) + struct timespec& poll_time, struct timespec& last_primitive_received_time, + const Duration& delta_time) { struct timespec current_time; struct timespec time_since_last_primitive_received; @@ -324,7 +329,8 @@ inline void Thunderloop::processPrimitiveExecution( primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } - direct_control_ = *primitive_executor_.stepPrimitive(primitive_executor_status_); + direct_control_ = + *primitive_executor_.stepPrimitive(primitive_executor_status_, delta_time); } thunderloop_status_.set_primitive_executor_step_time_ms(getMilliseconds(poll_time)); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 8aa4423991..85464b06de 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -143,9 +143,11 @@ class Thunderloop * * @param poll_time Output for execution duration * @param last_primitive_received_time Input for checking timeouts + * @param delta_time The time passed since the last step */ - inline void processPrimitiveExecution(struct timespec& poll_time, - struct timespec& last_primitive_received_time); + inline void processPrimitiveExecution( + struct timespec& poll_time, struct timespec& last_primitive_received_time, + const Duration& delta_time); /** * Updates internal tracking for chipper/kicker firing events. diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 25e92dce64..ec0659d87f 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -24,10 +24,11 @@ logger = create_logger(__name__) -LAUNCH_DELAY_S = 0.1 -WORLD_BUFFER_TIMEOUT = 0.5 -PROCESS_BUFFER_DELAY_S = 0.01 -PAUSE_AFTER_FAIL_DELAY_S = 3 +LAUNCH_DELAY_S = 2 +WORLD_BUFFER_TIMEOUT = 10 +PROCESS_BUFFER_DELAY_S = 0.1 +PAUSE_AFTER_FAIL_DELAY_S = 5 +SECONDS_PER_MILLISECOND = 0.001 class SimulatedTestRunner(TbotsTestRunner): @@ -101,57 +102,50 @@ def sync_setup(self, setup, param): WorldStateReceivedTrigger, world_state_received_buffer ) - while True: + retries = 0 + MAX_RETRIES = 100 # 10 seconds total at 0.1s sleep + while retries < MAX_RETRIES: setup(param) try: world_state_received_buffer.get( - block=True, timeout=WORLD_BUFFER_TIMEOUT + block=True, timeout=0.1, return_cached=False ) + return except queue.Empty: - # Did not receive a response within timeout period - continue - else: - # Received a response from the simulator - break + retries += 1 + + raise TimeoutError("Timed out waiting for Simulator to receive world state") def runner( self, always_validation_sequence_set, eventually_validation_sequence_set, - test_timeout_s, - tick_duration_s, - ci_cmd_with_delay, - run_till_end, + test_timeout_s=3, + tick_duration_s=1.0 / 60.0, + ci_cmd_with_delay=[], + run_till_end=False, + **kwargs, ): - """Run a test - - :param always_validation_sequence_set: Validation functions that should - hold on every tick - :param eventually_validation_sequence_set: Validation that should - eventually be true, before the test ends - :param test_timeout_s: The timeout for the test, if any eventually_validations - remain after the timeout, the test fails. - :param tick_duration_s: The simulation step duration + """Ticks the simulation forward while running the validations + + :param eventually_validation_sequence_set: validation set that must eventually be true + :param always_validation_sequence_set: validation set that must always be true + :param test_timeout_s: how long the test will run + :param tick_duration_s: how long each simulation step will be + :param run_till_end: if the test should run till the test timeout even if a pass condition is reached :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. - [ - (time, command, team), - (time, command, team), - ... - ] - :param run_till_end: If true, test runs till the end even if eventually validation passes - If false, test stops once eventually validation passes and fails if time out + [(1.0, Command.Type.NORMAL_START, Team.BLUE)] """ time_elapsed_s = 0 - eventually_validation_failure_msg = "Test Timed Out" + eventually_validation_proto_set = None while time_elapsed_s < test_timeout_s: - # get time before we execute the loop - processing_start_time = time.time() + start_time = time.time() # Check for new CI commands at this time step - for delay, cmd, team in ci_cmd_with_delay: + for delay, cmd, team in ci_cmd_with_delay[:]: # If delay matches time if delay <= time_elapsed_s: # send command @@ -163,7 +157,9 @@ def runner( self.simulator_proto_unix_io.send_proto(SimulatorTick, tick) time_elapsed_s += tick_duration_s - while True: + retry_count = 0 + MAX_RETRIES = 5 + while retry_count < MAX_RETRIES: try: world = self.world_buffer.get( block=True, timeout=WORLD_BUFFER_TIMEOUT, return_cached=False @@ -179,24 +175,23 @@ def runner( break except queue.Empty: - # If we timeout, that means full_system missed the last - # wrapper and robot status, lets resend it. - logger.warning("Fullsystem missed last wrapper, resending ...") - - ssl_wrapper = self.ssl_wrapper_buffer.get(block=False) - robot_status = self.robot_status_buffer.get(block=False) - - self.blue_full_system_proto_unix_io.send_proto( - SSL_WrapperPacket, ssl_wrapper + retry_count += 1 + logger.warning( + f"Timeout waiting for world/primitives (retry {retry_count}/{MAX_RETRIES}). Resending SSL Wrapper." ) - self.blue_full_system_proto_unix_io.send_proto( - RobotStatus, robot_status - ) - - # get the time difference after we get the primitive (after any blocking that happened) - processing_time = time.time() - processing_start_time - - # if the time we have blocked is less than a tick, sleep for the remaining time (for Thunderscope only) + # No world or primitives was found within the given timeout. Re-send the SSL wrapper packet and try again. + for packet in self.ssl_wrapper_buffer.get_all(): + self.blue_full_system_proto_unix_io.send_proto( + SSL_WrapperPacket, packet + ) + self.yellow_full_system_proto_unix_io.send_proto( + SSL_WrapperPacket, packet + ) + + if retry_count == MAX_RETRIES: + raise TimeoutError("Timed out waiting for world/primitive updates from AI/Simulator") + + processing_time = time.time() - start_time if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) @@ -248,121 +243,51 @@ def runner( # Check that all eventually validations are eventually valid validation.check_validation(eventually_validation_proto_set) - self.__stopper() @override def run_test( self, - always_validation_sequence_set, - eventually_validation_sequence_set, + always_validation_sequence_set=[[]], + eventually_validation_sequence_set=[[]], + setup=None, test_timeout_s=3, - tick_duration_s=0.0166, # Default to 60hz - index=0, + tick_duration_s=1.0 / 60.0, ci_cmd_with_delay=[], - run_till_end=True, - **kwargs, - ): - """Helper function to run a test, with thunderscope if enabled - - :param always_validation_sequence_set: validation that should always be true - :param eventually_validation_sequence_set: validation that should eventually be true - :param test_timeout_s: how long the test should run before timing out - :param tick_duration_s: length of a tick - :param index: index of the current test. default is 0 (invariant test) - values can be passed in during aggregate testing for different timeout durations - :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. - [ - (time, command, team), - (time, command, team), - ... - ] - :param run_till_end: If true, test runs till the end even if eventually validation passes - If false, test stops once eventually validation passes and fails if time out - """ - test_timeout_duration = ( - test_timeout_s[index] if type(test_timeout_s) == list else test_timeout_s - ) - - # If thunderscope is enabled, run the test in a thread and show - # thunderscope on this thread. The excepthook is setup to catch - # any test failures and propagate them to the main thread - if self.thunderscope: - run_sim_thread = threading.Thread( - target=self.runner, - daemon=True, - args=[ - always_validation_sequence_set, - eventually_validation_sequence_set, - test_timeout_duration, - tick_duration_s, - ci_cmd_with_delay, - run_till_end, - ], - ) - run_sim_thread.start() - self.thunderscope.show() - run_sim_thread.join() - - if self.last_exception: - pytest.fail(str(self.last_exception)) - - # If thunderscope is disabled, just run the test - else: - self.runner( - always_validation_sequence_set, - eventually_validation_sequence_set, - test_timeout_duration, - tick_duration_s, - ci_cmd_with_delay=ci_cmd_with_delay, - run_till_end=run_till_end, - ) - - -class InvariantTestRunner(SimulatedTestRunner): - """Runs a simulated test only once with a given parameter - - Test passes or fails based on the outcome of this test - """ - - def __init__(self, *args, **kwargs): - super().__init__(*args, **kwargs) - - @override - def run_test( - self, - setup=(lambda x: None), - params=[0], - inv_always_validation_sequence_set=[[]], - inv_eventually_validation_sequence_set=[[]], + run_till_end=False, **kwargs, ): - """Run an invariant test + """Begins validating a test based on incoming world protos - :param setup: Function that sets up the World state and the gamecontroller before running the test - :param params: List of parameters for each iteration of the test - (this method only uses the first element) - :param inv_always_validation_sequence_set: Validation functions for invariant testing - that should hold on every tick - :param inv_eventually_validation_sequence_set: Validation functions for invariant testing - that should eventually be true, before the test ends + :param eventually_validation_sequence_set: validation set that must eventually be true + :param always_validation_sequence_set: validation set that must always be true + :param setup: initialization function for this test + :param test_timeout_s: how long the test will run """ - threading.excepthook = self.excepthook - - super().sync_setup(setup, params[0]) - - super().run_test( - inv_always_validation_sequence_set, - inv_eventually_validation_sequence_set, + # Set the hook for exception handling so that we can close the thunderscope + # instance should one exist + sys.excepthook = self.excepthook + + # Only run setup if provided and if we are not being called from AggregateTestRunner + # (which handles its own setup loop) + if setup and "params" not in kwargs: + self.sync_setup(setup, self) + + self.runner( + always_validation_sequence_set=always_validation_sequence_set, + eventually_validation_sequence_set=eventually_validation_sequence_set, + test_timeout_s=test_timeout_s, + tick_duration_s=tick_duration_s, + ci_cmd_with_delay=ci_cmd_with_delay, + run_till_end=run_till_end, **kwargs, ) class AggregateTestRunner(SimulatedTestRunner): - """Runs a simulated test multiple times with different given parameters - - Result of the test is determined by comparing the number of - passing iterations to a predetermined acceptable threshold + """A test runner for aggregate tests. + These tests are a collection of invariant tests. If any of the invariant tests fail, + the aggregate test fails. """ def __init__(self, *args, **kwargs): @@ -377,30 +302,29 @@ def run_test( ag_eventually_validation_sequence_set=[[]], **kwargs, ): - """Run an aggregate test + """Begins validating a test based on incoming world protos. Runs the + invariant test first, then the aggregate test. - :param setup: Function that sets up the World state and the gamecontroller before running the test + :param setup: initialization function for this test :param params: List of parameters for each iteration of the test - :param ag_always_validation_sequence_set: Validation functions for aggregate testing - that should hold on every tick - :param ag_eventually_validation_sequence_set: Validation functions for aggregate testing - that should eventually be true, before the test end + :param ag_eventually_validation_sequence_set: validation set for aggregate test that must eventually be true + :param ag_always_validation_sequence_set: validation set for aggregate test that must always be true """ - threading.excepthook = self.excepthook + sys.excepthook = self.excepthook failed_tests = 0 - # Runs the test once for each given parameter - # Catches Assertion Error thrown by failing test and increments counter - # Calculates overall results and prints them + # Create a copy of kwargs without 'params' to avoid double-setup in SimulatedTestRunner + clean_kwargs = {k: v for k, v in kwargs.items() if k != "params"} + for x in range(len(params)): super().sync_setup(setup, params[x]) try: super().run_test( - ag_always_validation_sequence_set, - ag_eventually_validation_sequence_set, - **kwargs, + always_validation_sequence_set=ag_always_validation_sequence_set, + eventually_validation_sequence_set=ag_eventually_validation_sequence_set, + **clean_kwargs, ) except AssertionError: failed_tests += 1 @@ -494,6 +418,12 @@ def load_command_line_arguments(allow_unrecognized: bool = False): default=False, help="Use realism in the simulator", ) + parser.add_argument( + "--ci_mode", + action="store_true", + default=False, + help="Run in CI mode (faster execution)", + ) return parser.parse_known_args()[0] if allow_unrecognized else parser.parse_args() @@ -542,14 +472,14 @@ def simulated_test_runner(): args.debug_blue_full_system, False, should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, + running_in_realtime=not args.ci_mode, ) 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, + running_in_realtime=not args.ci_mode, ) as yellow_fs: with Gamecontroller( suppress_logs=(not args.show_gamecontroller_logs) @@ -580,7 +510,9 @@ def simulated_test_runner(): layout_path=args.layout, ) - time.sleep(LAUNCH_DELAY_S) + # Even in CI mode, give a small delay for processes to start up + actual_launch_delay = 0.5 if args.ci_mode else LAUNCH_DELAY_S + time.sleep(actual_launch_delay) runner = None @@ -595,7 +527,7 @@ def simulated_test_runner(): gamecontroller, ) else: - runner = InvariantTestRunner( + runner = SimulatedTestRunner( current_test, tscope, simulator_proto_unix_io, diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7593b87ce1..b0fa2cad84 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -395,16 +395,18 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( TbotsProto::PrimitiveExecutorStatus status; // Added for compilation if (ramping) { - auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); - direct_control = getRampedVelocityPrimitive( - globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), - std::get<1>(current_state.at(robot_id))), - std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, - primitive_executor_time_step_s); + auto direct_control_no_ramp = primitive_executor->stepPrimitive( + status, Duration::fromSeconds(primitive_executor_time_step_s)); + direct_control = getRampedVelocityPrimitive( + globalToLocalVelocity(std::get<2>(current_state.at(robot_id)), + std::get<1>(current_state.at(robot_id))), + std::get<3>(current_state.at(robot_id)), *direct_control_no_ramp, + primitive_executor_time_step_s); } else { - direct_control = primitive_executor->stepPrimitive(status); + direct_control = primitive_executor->stepPrimitive( + status, Duration::fromSeconds(primitive_executor_time_step_s)); } auto command = *getRobotCommandFromDirectControl( From a67712551148112f5c4f8756ac5095e3c058790a Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 14:58:28 -0700 Subject: [PATCH 06/20] test fixture fixes linting --- .../kickoff_enemy/kickoff_enemy_play_test.py | 2 -- .../kickoff_friendly_play_test.py | 2 -- src/software/embedded/primitive_executor.cpp | 17 +++++++++-------- src/software/embedded/thunderloop.cpp | 2 +- src/software/embedded/thunderloop.h | 6 +++--- .../simulated_tests/simulated_test_fixture.py | 5 +++-- 6 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py index a76fa9b3a7..7b31b26718 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py @@ -1,5 +1,3 @@ -import threading - import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py index 6e85349b02..9510feae33 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py @@ -1,5 +1,3 @@ -import threading - import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 66c818dc79..07587da036 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -36,7 +36,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_linear_traj_new) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } @@ -53,7 +53,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_angular_traj_new) { - angular_trajectory_ = new_angular_trajectory; + angular_trajectory_ = new_angular_trajectory; time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); orientation_controller_.reset(); } @@ -112,8 +112,10 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit std::unique_ptr PrimitiveExecutor::stepPrimitive( TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time) { - time_since_linear_trajectory_creation_ = time_since_linear_trajectory_creation_ + delta_time; - time_since_angular_trajectory_creation_ = time_since_angular_trajectory_creation_ + delta_time; + time_since_linear_trajectory_creation_ = + time_since_linear_trajectory_creation_ + delta_time; + time_since_angular_trajectory_creation_ = + time_since_angular_trajectory_creation_ + delta_time; status.set_running_primitive(true); @@ -149,10 +151,9 @@ std::unique_ptr PrimitiveExecutor::stepPrimi position_, *trajectory_path_, time_since_linear_trajectory_creation_, delta_time); - const AngularVelocity target_angular_velocity = - orientation_controller_.step(orientation_, *angular_trajectory_, - time_since_angular_trajectory_creation_, - delta_time); + const AngularVelocity target_angular_velocity = orientation_controller_.step( + orientation_, *angular_trajectory_, + time_since_angular_trajectory_creation_, delta_time); Vector target_linear_velocity_local = globalToLocalVelocity(target_linear_velocity_global, orientation_); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index aab30afeba..a3d89e1fe0 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -200,7 +200,7 @@ void Thunderloop::runLoop() processLocalizationUpdates(); processPrimitiveExecution(poll_time, last_primitive_received_time, - delta_time); + delta_time); updateChickerStatus(last_chipper_fired, last_kicker_fired); diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h index 85464b06de..434035c933 100644 --- a/src/software/embedded/thunderloop.h +++ b/src/software/embedded/thunderloop.h @@ -145,9 +145,9 @@ class Thunderloop * @param last_primitive_received_time Input for checking timeouts * @param delta_time The time passed since the last step */ - inline void processPrimitiveExecution( - struct timespec& poll_time, struct timespec& last_primitive_received_time, - const Duration& delta_time); + inline void processPrimitiveExecution(struct timespec& poll_time, + struct timespec& last_primitive_received_time, + const Duration& delta_time); /** * Updates internal tracking for chipper/kicker firing events. diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index ec0659d87f..87557a278a 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -1,4 +1,3 @@ -import threading import queue import argparse import time @@ -189,7 +188,9 @@ def runner( ) if retry_count == MAX_RETRIES: - raise TimeoutError("Timed out waiting for world/primitive updates from AI/Simulator") + raise TimeoutError( + "Timed out waiting for world/primitive updates from AI/Simulator" + ) processing_time = time.time() - start_time if self.thunderscope and tick_duration_s > processing_time: From 4e4a2c4be48cf3ef74df925f8447a504a19cc322 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 15:04:45 -0700 Subject: [PATCH 07/20] tuning constants pid --- .../embedded/motion_control/orientation_controller.h | 4 ++-- src/software/embedded/motion_control/position_controller.h | 6 +++--- src/software/embedded/primitive_executor.h | 6 +++--- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 8799380e9b..63b90ed6d1 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -38,7 +38,7 @@ class OrientationController private: // TODO(#3737): tune constants PidController w_pid_{0.7, 0.0, 2.0, 0.0}; - PidController w_pid_close_{10.0, 0.0, 4.0, 0.0}; + PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/motion_control/position_controller.h b/src/software/embedded/motion_control/position_controller.h index d3fc239d10..12725e09d7 100644 --- a/src/software/embedded/motion_control/position_controller.h +++ b/src/software/embedded/motion_control/position_controller.h @@ -38,8 +38,8 @@ class PositionController : public MotionController x_pid_{0.8, 0.0, 0.0, 0.0}; PidController y_pid_{0.8, 0.0, 0.0, 0.0}; - PidController x_pid_close_{5.0, 0.0, 0.0, 0.0}; - PidController y_pid_close_{5.0, 0.0, 0.0, 0.0}; + PidController x_pid_close_{2.0, 0.0, 0.0, 0.0}; + PidController y_pid_close_{2.0, 0.0, 0.0, 0.0}; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; }; diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index bba3069456..a8db26d346 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -77,8 +77,8 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.2; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.05; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 5.0; + static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; + static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; From cc994bef3b27b1520d8778e887cd2c17254595f3 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 15:21:13 -0700 Subject: [PATCH 08/20] tuning constants pid --- src/software/embedded/primitive_executor.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index a8db26d346..d432af01c9 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -77,8 +77,6 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.1; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 20.0; - static constexpr double LINEAR_PURE_PID_THRESHOLD_METERS = 0.5; - static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.4; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; }; From 8639b2c258b67cebfdd9ab7d1ea5fb725afd80dc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 29 May 2026 22:34:31 +0000 Subject: [PATCH 09/20] [pre-commit.ci lite] apply automatic fixes --- src/software/embedded/primitive_executor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index d432af01c9..26b400c637 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -77,6 +77,6 @@ class PrimitiveExecutor // These constants were lost during a refactor/revert and are currently set to // estimated defaults. - static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.4; - static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; + static constexpr double LINEAR_STALL_ERROR_MAX_METERS = 0.4; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; }; From 573dc741e6255a21e41631d65d871b675c05f130 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Fri, 29 May 2026 15:54:26 -0700 Subject: [PATCH 10/20] removed old pid --- src/software/util/pid/BUILD | 8 --- src/software/util/pid/pid_controller.hpp | 82 ------------------------ 2 files changed, 90 deletions(-) delete mode 100644 src/software/util/pid/BUILD delete mode 100644 src/software/util/pid/pid_controller.hpp diff --git a/src/software/util/pid/BUILD b/src/software/util/pid/BUILD deleted file mode 100644 index 156e74085e..0000000000 --- a/src/software/util/pid/BUILD +++ /dev/null @@ -1,8 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "pid", - hdrs = [ - "pid_controller.hpp", - ], -) diff --git a/src/software/util/pid/pid_controller.hpp b/src/software/util/pid/pid_controller.hpp deleted file mode 100644 index d3e673efc2..0000000000 --- a/src/software/util/pid/pid_controller.hpp +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -#include - -namespace controls -{ -template -class PIDController -{ - public: - /** - * Constructs a new PID controller. - * - * A PID controller is used to calculate corrections based on error values over - * time as the difference between a desired value and the actual measured value. - * This PID controller also limits integral windup using a max_integral value. - * - * Resources: - * - https://raw.org/book/control-theory/introduction-to-pid-controllers/ - * - http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-reset-windup/ - * - * @pre max_integral must be >= 0.0 - * - * @param k_p The proportional gain. - * @param k_i The integral gain. - * @param k_d The derivative gain. - * @param max_integral The maximum absolute value that the integrator term can - * accumulate to. - **/ - PIDController(T k_p, T k_i, T k_d, T max_integral); - - /** - * Given an error, returns the control effort to minimize it. - * - * @param deltaTime The time passed since last step, for calculating integrator and - *derivative. If this function is calling in invariant intervals, deltaTime can be set - *to 1 and any effects it would have can be handled by tuning kD. - **/ - T step(T error, T delta_time = 1.0f); - - /** - * Resets the integrator and clears the last error used for derivative calculation. - **/ - void reset(); - - T k_p; - T k_i; - T k_d; - T max_integral; - - protected: - T integral; - std::optional last_error = std::nullopt; -}; -} // namespace controls - -template -controls::PIDController::PIDController(T k_p, T k_i, T k_d, T max_integral) - : k_p(k_p), k_i(k_i), k_d(k_d), max_integral(max_integral){}; - -template -T controls::PIDController::step(T error, T delta_time) -{ - // If sign of error swaps, reset integrator - if (last_error.value_or(0) * error < 0) - { - integral = 0; - } - - integral += std::max(std::min(error * delta_time, max_integral), -max_integral); - // set derivative, if no last_error, just set to 0 - const T derivative = (last_error.value_or(error) - error) / delta_time; - last_error = error; - return error * k_p + integral * k_i + derivative * k_d; -} - -template -void controls::PIDController::reset() -{ - integral = 0; - last_error = std::nullopt; -} From 39d7d7b0e33a5aa4ef01bfe41e61b307bb8f446b Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Fri, 29 May 2026 16:52:30 -0700 Subject: [PATCH 11/20] tuning fixes --- src/software/embedded/BUILD | 1 - .../embedded/motion_control/orientation_controller.h | 4 ++-- .../motor_controller/stspin_motor_controller.cpp | 4 ++-- src/software/embedded/services/motor.cpp | 10 +++++----- src/software/embedded/thunderloop.cpp | 5 +++-- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD index 6bc28d668e..e56929c45f 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -39,7 +39,6 @@ cc_library( "//software/math:math_functions", "//software/physics:velocity_conversion_util", "//software/time:duration", - "//software/util/pid", "//software/world:robot_state", "//software/world:team_colour", ], diff --git a/src/software/embedded/motion_control/orientation_controller.h b/src/software/embedded/motion_control/orientation_controller.h index 63b90ed6d1..ddd74f03d3 100644 --- a/src/software/embedded/motion_control/orientation_controller.h +++ b/src/software/embedded/motion_control/orientation_controller.h @@ -37,8 +37,8 @@ class OrientationController private: // TODO(#3737): tune constants - PidController w_pid_{0.7, 0.0, 2.0, 0.0}; - PidController w_pid_close_{2.0, 0.0, 4.0, 0.0}; + PidController w_pid_{0.7, 0.0, 0.02, 0.0}; + PidController w_pid_close_{4.0, 0.0, 0.2, 0.0}; static constexpr double ANGULAR_PURE_PID_THRESHOLD_DEGREES = 25.0; }; diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index 7324b0a6a4..d28a38f390 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -189,8 +189,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, void StSpinMotorController::updateEuclideanVelocity( EuclideanSpace_t target_euclidean_velocity) { - const Vector local_velocity(target_euclidean_velocity[1], - -target_euclidean_velocity[0]); + const Vector local_velocity(target_euclidean_velocity[0], + target_euclidean_velocity[1]); if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { diff --git a/src/software/embedded/services/motor.cpp b/src/software/embedded/services/motor.cpp index 6c838b0c5a..f2fefbba19 100644 --- a/src/software/embedded/services/motor.cpp +++ b/src/software/embedded/services/motor.cpp @@ -171,9 +171,9 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor euclidean_to_four_wheel_.getEuclideanVelocity(current_wheel_velocities); motor_status.mutable_local_velocity()->set_x_component_meters( - current_euclidean_velocity[1]); + current_euclidean_velocity[0]); motor_status.mutable_local_velocity()->set_y_component_meters( - -current_euclidean_velocity[0]); + current_euclidean_velocity[1]); motor_status.mutable_angular_velocity()->set_radians_per_second( current_euclidean_velocity[2]); @@ -194,8 +194,8 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor const auto& direct_velocity = motor_control.direct_velocity_control(); const EuclideanSpace_t target_euclidean_velocity = { - -direct_velocity.velocity().y_component_meters(), direct_velocity.velocity().x_component_meters(), + direct_velocity.velocity().y_component_meters(), direct_velocity.angular_velocity().radians_per_second()}; motor_controller_->updateEuclideanVelocity(target_euclidean_velocity); @@ -213,9 +213,9 @@ TbotsProto::MotorStatus MotorService::poll(const TbotsProto::MotorControl& motor euclidean_to_four_wheel_.getEuclideanVelocity(target_wheel_velocities_); motor_status.mutable_target_local_velocity()->set_x_component_meters( - target_euclidean_velocity[1]); + target_euclidean_velocity[0]); motor_status.mutable_target_local_velocity()->set_y_component_meters( - -target_euclidean_velocity[0]); + target_euclidean_velocity[1]); motor_status.mutable_target_angular_velocity()->set_radians_per_second( target_euclidean_velocity[2]); diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index a3d89e1fe0..7433ed2306 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -254,7 +254,8 @@ inline void Thunderloop::processNetworkPolling( const Point position = createPoint(primitive_.move().xy_traj_params().start_position()); const Angle orientation = - createAngle(primitive_.move().w_traj_params().start_angle()); + createAngle(primitive_.move().w_traj_params().start_angle()) + + Angle::half(); robot_localizer_.update( RobotLocalizer::VisionData{position, orientation, RTT_S / 2}); @@ -295,7 +296,7 @@ inline void Thunderloop::processLocalizationUpdates() if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) { const auto accel = imu_poll->linear_acceleration.value(); - linear_acceleration = Vector(accel[1], accel[0]); + linear_acceleration = Vector(accel[0], accel[1]); } robot_localizer_.step(linear_acceleration); From 24a50acf02543247a3f86f189b84e399b67571b4 Mon Sep 17 00:00:00 2001 From: Thunderbots Date: Fri, 29 May 2026 16:52:47 -0700 Subject: [PATCH 12/20] tuning fixes --- src/shared/robot_constants.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index e4eb334f96..4e60d0ee90 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_max_speed_trajectory_m_per_s = 3.0f, - .robot_max_acceleration_m_per_s_2 = 3.0f, - .robot_max_deceleration_m_per_s_2 = 3.0f, + .robot_max_speed_trajectory_m_per_s = 2.5f, + .robot_max_acceleration_m_per_s_2 = 2.0f, + .robot_max_deceleration_m_per_s_2 = 1.5f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 10.0f, - .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, + .robot_max_ang_speed_rad_per_s = 6.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, .wheel_radius_meters = 0.03f, @@ -174,7 +174,7 @@ constexpr RobotConstants createRobotConstants() // Kalman filter variances for robot localizer .kalman_process_noise_variance_rad_per_s_4 = 1.0f, - .kalman_vision_noise_variance_rad_2 = 0.01f, + .kalman_vision_noise_variance_rad_2 = 0.01f * 0.01f, .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5}; } #elif CHECK_VERSION(2021) From acedc5a2ea6b7e4a06fb7418e45b3aff08fbf168 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Sun, 31 May 2026 13:05:50 -0700 Subject: [PATCH 13/20] refactor and localizer fixes --- .../trajectory/bang_bang_trajectory_1d.h | 14 +++---- .../bang_bang_trajectory_1d_angular.h | 14 +++---- .../ai/navigator/trajectory/trajectory.hpp | 11 ++--- .../ai/navigator/trajectory/trajectory_2d.h | 14 +++---- src/software/embedded/primitive_executor.cpp | 10 ++--- src/software/embedded/robot_localizer.cpp | 41 ++++++++++++------- .../field_tests/field_test_fixture.py | 30 +++++--------- 7 files changed, 69 insertions(+), 65 deletions(-) diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h index 4095faea25..4bce8c76df 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d.h @@ -105,16 +105,16 @@ class BangBangTrajectory1D : public Trajectory size_t getNumTrajectoryParts() const; /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise */ - bool isNew(const Trajectory& other, - double threshold) const override + bool equals(const Trajectory& other, + double threshold) const override { - return std::abs(getDestination() - other.getDestination()) > threshold; + return std::abs(getDestination() - other.getDestination()) <= threshold; } private: diff --git a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h index 859034685e..3c87b039c2 100644 --- a/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h +++ b/src/software/ai/navigator/trajectory/bang_bang_trajectory_1d_angular.h @@ -84,16 +84,16 @@ class BangBangTrajectory1DAngular double getTotalTime() const override; /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different in degrees. - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal in degrees. + * @return True if the trajectories are equal, false otherwise */ - bool isNew(const Trajectory& other, - double threshold) const override + bool equals(const Trajectory& other, + double threshold) const override { - return getDestination().minDiff(other.getDestination()).toDegrees() > threshold; + return getDestination().minDiff(other.getDestination()).toDegrees() <= threshold; } private: diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index 4f970c2e7a..b147f24684 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -61,11 +61,12 @@ class Trajectory } /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise */ - virtual bool isNew(const Trajectory& other, double threshold) const = 0; + virtual bool equals(const Trajectory& other, + double threshold) const = 0; }; diff --git a/src/software/ai/navigator/trajectory/trajectory_2d.h b/src/software/ai/navigator/trajectory/trajectory_2d.h index 4f21a9a25b..e40b367f58 100644 --- a/src/software/ai/navigator/trajectory/trajectory_2d.h +++ b/src/software/ai/navigator/trajectory/trajectory_2d.h @@ -17,15 +17,15 @@ class Trajectory2D : virtual public Trajectory virtual std::vector getBoundingBoxes() const = 0; /** - * Check if this trajectory is meaningfully different from another trajectory. + * Check if this trajectory is meaningfully equal to another trajectory. * @param other The other trajectory to compare to - * @param threshold The threshold above which the trajectories are considered - * different - * @return True if the trajectories are different, false otherwise + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise */ - bool isNew(const Trajectory& other, - double threshold) const override + bool equals(const Trajectory& other, + double threshold) const override { - return distance(getDestination(), other.getDestination()) > threshold; + return distance(getDestination(), other.getDestination()) <= threshold; } }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 07587da036..831a5e6af6 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -31,12 +31,12 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const bool is_linear_traj_new = (new_trajectory_path.has_value() != trajectory_path_.has_value()) || (new_trajectory_path.has_value() && - trajectory_path_->isNew(*new_trajectory_path, - LINEAR_DESTINATION_THRESHOLD_METERS)); + !trajectory_path_->equals(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); if (is_linear_traj_new) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } @@ -48,8 +48,8 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m const bool is_angular_traj_new = !angular_trajectory_.has_value() || - angular_trajectory_->isNew(new_angular_trajectory, - ANGULAR_DESTINATION_THRESHOLD_DEGREES); + !angular_trajectory_->equals(new_angular_trajectory, + ANGULAR_DESTINATION_THRESHOLD_DEGREES); if (is_angular_traj_new) { diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 41744aa047..73f24164eb 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -139,45 +139,58 @@ void RobotLocalizer::update(const VisionData& data) const auto current_time = std::chrono::steady_clock::now(); const auto sample_age = std::chrono::duration(data.age_seconds); - const auto rollback_point = std::find_if( + auto rollback_point = std::find_if( history.begin(), history.end(), - [&](const FilterStep& step) { return step.time - current_time >= sample_age; }); + [&](const FilterStep& step) { return (current_time - step.time) >= sample_age; }); if (rollback_point == history.begin()) { - // All history predates the sample, no need to rollback + // All history predates the sample, or is exactly at the same time. + // No need to rollback, just apply to the current state. updateFilterWithVision(data.position, data.orientation); + history.clear(); // Safe to clear, since all history is older than current state return; } - auto replay_iter = std::make_reverse_iterator(rollback_point); + if (rollback_point == history.end()) + { + // The vision sample is older than our entire history. + // Roll back as far as we can (to the oldest step). + rollback_point = std::prev(history.end()); + } - // Truncate history after the rollback point - history.erase(rollback_point, history.end()); + // 1. Save the state from the rollback point + filter_.state_estimate = rollback_point->state_estimate; + filter_.state_covariance = rollback_point->state_covariance; - filter_.state_estimate = replay_iter->state_estimate; - filter_.state_covariance = replay_iter->state_covariance; + // 2. Truncate history (erase rollback_point and everything older) + history.erase(rollback_point, history.end()); + // 3. Apply the delayed vision measurement updateFilterWithVision(data.position, data.orientation); - // Replay from the rollback point back to the current estimate - for (; replay_iter != history.rbegin(); --replay_iter) + // 4. Replay the remaining history (from oldest to newest) + for (auto it = history.rbegin(); it != history.rend(); ++it) { - if (replay_iter->prediction.has_value()) + if (it->prediction.has_value()) { - const auto& prediction = replay_iter->prediction.value(); + const auto& prediction = it->prediction.value(); filter_.process_model = prediction.process_model; filter_.process_covariance = prediction.process_covariance; filter_.control_model = prediction.control_model; filter_.predict(prediction.control_input); } - if (replay_iter->update.has_value()) + if (it->update.has_value()) { - const auto& update = replay_iter->update.value(); + const auto& update = it->update.value(); filter_.measurement_model = update.measurement_model; filter_.update(update.measurement); } + + // IMPORTANT: Update the history with the recomputed state so future rollbacks are correct + it->state_estimate = filter_.state_estimate; + it->state_covariance = filter_.state_covariance; } } diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index 9535e727d1..e4c857d15a 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -363,13 +363,6 @@ def load_command_line_arguments(): help="Disables checking for estop plugged in (ONLY USE FOR LOCAL TESTING)", ) - parser.add_argument( - "--no_visualization", - action="store_true", - default=False, - help="Disables the Thunderscope GUI", - ) - return parser.parse_args() @@ -436,17 +429,15 @@ def field_test_runner(): simulator_proto_unix_io=simulator_proto_unix_io, ) # Inject the proto unix ios into thunderscope and start the test - tscope = None - if not args.no_visualization: - tscope = Thunderscope( - configure_field_test_view( - simulator_proto_unix_io=simulator_proto_unix_io, - blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, - yellow_is_friendly=args.run_yellow, - ), - layout_path=None, - ) + tscope = Thunderscope( + configure_field_test_view( + simulator_proto_unix_io=simulator_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + yellow_is_friendly=args.run_yellow, + ), + layout_path=None, + ) # Set control mode for all robots to AI so that packets are sent to the robots for robot_id in range(MAX_ROBOT_IDS_PER_SIDE): @@ -457,8 +448,7 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: - if tscope: - tscope.keyboard_estop_shortcut.activated.connect( + tscope.keyboard_estop_shortcut.activated.connect( rc_friendly.toggle_keyboard_estop ) # we call this method to enable estop automatically when a field test starts From 244f17c28cda434ae0cf348ab30947dc229ee679 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 14:56:18 -0700 Subject: [PATCH 14/20] sim tests --- .../simulated_tests/simulated_test_fixture.py | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 87557a278a..edd312b7e8 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -23,10 +23,10 @@ logger = create_logger(__name__) -LAUNCH_DELAY_S = 2 -WORLD_BUFFER_TIMEOUT = 10 +LAUNCH_DELAY_S = 0.1 +WORLD_BUFFER_TIMEOUT = 0.5 PROCESS_BUFFER_DELAY_S = 0.1 -PAUSE_AFTER_FAIL_DELAY_S = 5 +PAUSE_AFTER_FAIL_DELAY_S = 3 SECONDS_PER_MILLISECOND = 0.001 @@ -122,7 +122,7 @@ def runner( eventually_validation_sequence_set, test_timeout_s=3, tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=[], + ci_cmd_with_delay=None, run_till_end=False, **kwargs, ): @@ -136,6 +136,7 @@ def runner( :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. [(1.0, Command.Type.NORMAL_START, Team.BLUE)] """ + ci_cmd_with_delay = list(ci_cmd_with_delay) if ci_cmd_with_delay else [] time_elapsed_s = 0 eventually_validation_failure_msg = "Test Timed Out" eventually_validation_proto_set = None @@ -249,12 +250,12 @@ def runner( @override def run_test( self, - always_validation_sequence_set=[[]], - eventually_validation_sequence_set=[[]], + always_validation_sequence_set=None, + eventually_validation_sequence_set=None, setup=None, test_timeout_s=3, tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=[], + ci_cmd_with_delay=None, run_till_end=False, **kwargs, ): @@ -265,6 +266,8 @@ def run_test( :param setup: initialization function for this test :param test_timeout_s: how long the test will run """ + always_validation_sequence_set = always_validation_sequence_set or [[]] + eventually_validation_sequence_set = eventually_validation_sequence_set or [[]] # Set the hook for exception handling so that we can close the thunderscope # instance should one exist sys.excepthook = self.excepthook @@ -298,9 +301,9 @@ def __init__(self, *args, **kwargs): def run_test( self, setup=(lambda arg: None), - params=[], - ag_always_validation_sequence_set=[[]], - ag_eventually_validation_sequence_set=[[]], + params=None, + ag_always_validation_sequence_set=None, + ag_eventually_validation_sequence_set=None, **kwargs, ): """Begins validating a test based on incoming world protos. Runs the @@ -311,6 +314,9 @@ def run_test( :param ag_eventually_validation_sequence_set: validation set for aggregate test that must eventually be true :param ag_always_validation_sequence_set: validation set for aggregate test that must always be true """ + params = params or [] + ag_always_validation_sequence_set = ag_always_validation_sequence_set or [[]] + ag_eventually_validation_sequence_set = ag_eventually_validation_sequence_set or [[]] sys.excepthook = self.excepthook failed_tests = 0 From 96c20c1663388686b971fa820a7f4aade1e1df3b Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 14:57:25 -0700 Subject: [PATCH 15/20] linting --- src/software/ai/navigator/trajectory/trajectory.hpp | 3 +-- src/software/embedded/primitive_executor.cpp | 2 +- src/software/embedded/robot_localizer.cpp | 5 +++-- src/software/field_tests/field_test_fixture.py | 4 ++-- src/software/simulated_tests/simulated_test_fixture.py | 4 +++- 5 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/software/ai/navigator/trajectory/trajectory.hpp b/src/software/ai/navigator/trajectory/trajectory.hpp index b147f24684..caffd6b86c 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -67,6 +67,5 @@ class Trajectory * equal * @return True if the trajectories are equal, false otherwise */ - virtual bool equals(const Trajectory& other, - double threshold) const = 0; + virtual bool equals(const Trajectory& other, double threshold) const = 0; }; diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 831a5e6af6..89f329753e 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -36,7 +36,7 @@ void PrimitiveExecutor::updatePrimitive(const TbotsProto::Primitive& primitive_m if (is_linear_traj_new) { - trajectory_path_ = new_trajectory_path; + trajectory_path_ = new_trajectory_path; time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); position_controller_.reset(); } diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp index 73f24164eb..a095983d90 100644 --- a/src/software/embedded/robot_localizer.cpp +++ b/src/software/embedded/robot_localizer.cpp @@ -148,7 +148,7 @@ void RobotLocalizer::update(const VisionData& data) // All history predates the sample, or is exactly at the same time. // No need to rollback, just apply to the current state. updateFilterWithVision(data.position, data.orientation); - history.clear(); // Safe to clear, since all history is older than current state + history.clear(); // Safe to clear, since all history is older than current state return; } @@ -188,7 +188,8 @@ void RobotLocalizer::update(const VisionData& data) filter_.update(update.measurement); } - // IMPORTANT: Update the history with the recomputed state so future rollbacks are correct + // IMPORTANT: Update the history with the recomputed state so future rollbacks are + // correct it->state_estimate = filter_.state_estimate; it->state_covariance = filter_.state_covariance; } diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index e4c857d15a..a67bc341a4 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -449,8 +449,8 @@ def field_test_runner(): # connect the keyboard estop toggle to the key event if needed if estop_mode == EstopMode.KEYBOARD_ESTOP: tscope.keyboard_estop_shortcut.activated.connect( - rc_friendly.toggle_keyboard_estop - ) + rc_friendly.toggle_keyboard_estop + ) # we call this method to enable estop automatically when a field test starts rc_friendly.toggle_keyboard_estop() logger.warning( diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index edd312b7e8..4d048d995a 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -316,7 +316,9 @@ def run_test( """ params = params or [] ag_always_validation_sequence_set = ag_always_validation_sequence_set or [[]] - ag_eventually_validation_sequence_set = ag_eventually_validation_sequence_set or [[]] + ag_eventually_validation_sequence_set = ( + ag_eventually_validation_sequence_set or [[]] + ) sys.excepthook = self.excepthook failed_tests = 0 From 1bd2cd9002e7bdefb14065305c3e60bbff1f52f6 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 15:47:10 -0700 Subject: [PATCH 16/20] test fixes --- .../message_translation/tbots_protobuf.cpp | 2 +- src/shared/robot_constants.h | 14 ++++++------ src/software/ai/evaluation/intercept.cpp | 2 +- src/software/embedded/primitive_executor.cpp | 5 +++++ src/software/embedded/thunderloop.cpp | 22 +++++++++---------- src/software/world/robot.cpp | 2 +- 6 files changed, 26 insertions(+), 21 deletions(-) diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 23e7d0c51c..2ba0c2c6b9 100644 --- a/src/proto/message_translation/tbots_protobuf.cpp +++ b/src/proto/message_translation/tbots_protobuf.cpp @@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed( switch (max_allowed_speed_mode) { case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT: - return robot_constants.robot_max_speed_trajectory_m_per_s; + return robot_constants.robot_trajectory_max_speed_m_per_s; case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND: return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND - STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND; diff --git a/src/shared/robot_constants.h b/src/shared/robot_constants.h index 4e60d0ee90..0ab068eded 100644 --- a/src/shared/robot_constants.h +++ b/src/shared/robot_constants.h @@ -90,7 +90,7 @@ struct RobotConstants // The maximum speed that the trajectory planner is allowed to command the robot to // move at, while still leaving headroom for the PID to apply correction on lag. [m/s] - float robot_max_speed_trajectory_m_per_s; + float robot_trajectory_max_speed_m_per_s; // The maximum acceleration achievable by our robots [m/s^2] float robot_max_acceleration_m_per_s_2; @@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants() // Robot's linear movement constants .robot_max_speed_m_per_s = 3.0f, - .robot_max_speed_trajectory_m_per_s = 2.5f, - .robot_max_acceleration_m_per_s_2 = 2.0f, - .robot_max_deceleration_m_per_s_2 = 1.5f, + .robot_trajectory_max_speed_m_per_s = 3.0f, + .robot_max_acceleration_m_per_s_2 = 3.0f, + .robot_max_deceleration_m_per_s_2 = 3.0f, // Robot's angular movement constants - .robot_max_ang_speed_rad_per_s = 6.0f, - .robot_max_ang_speed_trajectory_rad_per_s = 5.0f, - .robot_max_ang_acceleration_rad_per_s_2 = 10.0f, + .robot_max_ang_speed_rad_per_s = 10.0f, + .robot_max_ang_speed_trajectory_rad_per_s = 7.0f, + .robot_max_ang_acceleration_rad_per_s_2 = 30.0f, .wheel_radius_meters = 0.03f, diff --git a/src/software/ai/evaluation/intercept.cpp b/src/software/ai/evaluation/intercept.cpp index 2c729dd516..ea4fa9099c 100644 --- a/src/software/ai/evaluation/intercept.cpp +++ b/src/software/ai/evaluation/intercept.cpp @@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p Point best_position = intercept_position; double final_speed = step_speed; bool finished = false; - double max_speed = robot.robotConstants().robot_max_speed_trajectory_m_per_s; + double max_speed = robot.robotConstants().robot_trajectory_max_speed_m_per_s; double max_acc = robot.robotConstants().robot_max_acceleration_m_per_s_2; while (!finished) diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 89f329753e..691889c4a7 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -69,6 +69,11 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit orientation_ = orientation; angular_velocity_ = angular_velocity; + if (!current_primitive_.has_move()) + { + return; + } + // If we are lagging behind trajectory too much, we have stalled! We need to // regenerate trajectory. if (trajectory_path_.has_value()) diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index 7433ed2306..b23926e9fe 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -291,20 +291,20 @@ inline void Thunderloop::processLocalizationUpdates() localToGlobalVelocity(createVector(status.local_velocity()), robot_localizer_.getOrientation()), createAngularVelocity(status.angular_velocity())}); + } - Vector linear_acceleration; - if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) - { - const auto accel = imu_poll->linear_acceleration.value(); - linear_acceleration = Vector(accel[0], accel[1]); - } + Vector linear_acceleration; + if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value()) + { + const auto accel = imu_poll->linear_acceleration.value(); + linear_acceleration = Vector(accel[0], accel[1]); + } - robot_localizer_.step(linear_acceleration); + robot_localizer_.step(linear_acceleration); - primitive_executor_.updateState( - robot_localizer_.getPosition(), robot_localizer_.getVelocity(), - robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity()); - } + primitive_executor_.updateState( + robot_localizer_.getPosition(), robot_localizer_.getVelocity(), + robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity()); } inline void Thunderloop::processPrimitiveExecution( diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index 5a13d519a6..2b987e7b60 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -219,7 +219,7 @@ Duration Robot::getTimeToPosition(const Point& destination, double final_velocity_1d = final_velocity.dot(dist_vector.normalize()); return getTimeToTravelDistance(dist, - robot_constants_.robot_max_speed_trajectory_m_per_s, + robot_constants_.robot_trajectory_max_speed_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); } From 4b62eaebc9afe5ee8c8d8bde64d08934e169e1b1 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 16:43:05 -0700 Subject: [PATCH 17/20] more euclid change --- .../embedded/motor_controller/stspin_motor_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/embedded/motor_controller/stspin_motor_controller.cpp b/src/software/embedded/motor_controller/stspin_motor_controller.cpp index d28a38f390..e28ccfb402 100644 --- a/src/software/embedded/motor_controller/stspin_motor_controller.cpp +++ b/src/software/embedded/motor_controller/stspin_motor_controller.cpp @@ -189,8 +189,8 @@ int StSpinMotorController::readThenWriteVelocity(const MotorIndex motor, void StSpinMotorController::updateEuclideanVelocity( EuclideanSpace_t target_euclidean_velocity) { - const Vector local_velocity(target_euclidean_velocity[0], - target_euclidean_velocity[1]); + const Vector local_velocity(target_euclidean_velocity[1], + target_euclidean_velocity[0]); if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { From 9335cb7d1edd22716365dbc6cc894293525882f0 Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 2 Jun 2026 16:48:46 -0700 Subject: [PATCH 18/20] more euclid change --- src/software/physics/euclidean_to_wheel.cpp | 8 +-- .../physics/euclidean_to_wheel_test.cpp | 58 +++++++++---------- 2 files changed, 33 insertions(+), 33 deletions(-) diff --git a/src/software/physics/euclidean_to_wheel.cpp b/src/software/physics/euclidean_to_wheel.cpp index 63c24d9666..815077f402 100644 --- a/src/software/physics/euclidean_to_wheel.cpp +++ b/src/software/physics/euclidean_to_wheel.cpp @@ -39,10 +39,10 @@ EuclideanToWheel::EuclideanToWheel(const robot_constants::RobotConstants& robot_ // clang-format off euclidean_to_wheel_velocity_D_ << - std::cos(b_fr), std::sin(b_fr), (fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)), - std::cos(b_fl), std::sin(b_fl), (fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)), - std::cos(b_bl), std::sin(b_bl), (bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)), - std::cos(b_br), std::sin(b_br), (br_x * std::sin(b_br) - br_y * std::cos(b_br)); + -std::cos(b_fr), -std::sin(b_fr), -(fr_x * std::sin(b_fr) - fr_y * std::cos(b_fr)), + -std::cos(b_fl), -std::sin(b_fl), -(fl_x * std::sin(b_fl) - fl_y * std::cos(b_fl)), + -std::cos(b_bl), -std::sin(b_bl), -(bl_x * std::sin(b_bl) - bl_y * std::cos(b_bl)), + -std::cos(b_br), -std::sin(b_br), -(br_x * std::sin(b_br) - br_y * std::cos(b_br)); // clang-format on // Inverse of euclidean to wheel matrix (D) for converting wheel velocity to euclidean diff --git a/src/software/physics/euclidean_to_wheel_test.cpp b/src/software/physics/euclidean_to_wheel_test.cpp index 0890f6cde9..ff4fb52d1d 100644 --- a/src/software/physics/euclidean_to_wheel_test.cpp +++ b/src/software/physics/euclidean_to_wheel_test.cpp @@ -48,11 +48,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_x) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be - velocity, left wheels must be + velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be + velocity, left wheels must be - velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); // Right wheels must have same velocity magnitude as left wheels, but opposite sign. @@ -69,11 +69,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_x) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, left wheels must be - velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be - velocity, left wheels must be + velocity. + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); // Right wheels must have same velocity magnitude as left wheels, but opposite sign. EXPECT_DOUBLE_EQ(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], @@ -89,11 +89,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_y) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Back wheels must be + velocity, front wheels must be - velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Back wheels must be - velocity, front wheels must be + velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) @@ -103,11 +103,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_y) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Back wheels must be - velocity, front wheels must be + velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Back wheels must be + velocity, front wheels must be - velocity. + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign) @@ -117,11 +117,11 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_sign) calculated_wheel_speeds = euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); - // Right wheels must be + velocity, Left wheels must be - velocity. - EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + // Right wheels must be - velocity, Left wheels must be + velocity. + EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign) @@ -132,10 +132,10 @@ TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_negative_w_sign) euclidean_to_four_wheel.getWheelVelocity(target_euclidean_velocity); // Right wheels must be + velocity, Left wheels must be - velocity. - EXPECT_GT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); - EXPECT_GT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_RIGHT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[FRONT_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_LEFT_WHEEL_SPACE_INDEX], 0); + EXPECT_LT(calculated_wheel_speeds[BACK_RIGHT_WHEEL_SPACE_INDEX], 0); } TEST_F(EuclideanToWheelTest, test_target_wheel_speeds_positive_w_magnitude) From d1f3efb6c74a517b5e700bc71bf6f5948216be79 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Wed, 3 Jun 2026 13:58:21 -0700 Subject: [PATCH 19/20] temporary fix for ai vs ai --- src/software/simulation/er_force_simulator.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b0fa2cad84..a0b3c9ee51 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -311,8 +311,8 @@ void ErForceSimulator::setYellowRobotPrimitiveSet( const auto& [position, orientation, velocity, angular_velocity] = robot_to_state.at(robot_id); setRobotPrimitive(robot_id, primitive_set_msg, yellow_primitive_executor_map, - world_proto, position, orientation, velocity, - angular_velocity); + world_proto, -position, orientation + Angle::half(), + velocity, angular_velocity); } } } @@ -384,6 +384,12 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( { const auto& sim_robots = sim_state.yellow_robots(); current_state = getRobotIdToStateMap(sim_robots); + for (auto& [robot_id, state] : current_state) + { + auto& [position, orientation, velocity, angular_velocity] = state; + position = -position; + orientation = orientation + Angle::half(); + } } for (auto& primitive_executor_with_id : robot_primitive_executor_map) From 6e0bc08f08c9eb2e2f9a7edcdd4af19cc43f2bef Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Thu, 4 Jun 2026 11:45:36 -0700 Subject: [PATCH 20/20] reseting --- .../ball_placement_play_test.py | 4 +- .../kickoff_enemy/kickoff_enemy_play_test.py | 13 +- .../kickoff_friendly_play_fsm.cpp | 2 +- .../kickoff_friendly_play_test.py | 13 +- .../simulated_tests/simulated_test_fixture.py | 281 +++++++++++------- 5 files changed, 193 insertions(+), 120 deletions(-) diff --git a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py index ef3132c563..77e4e196ab 100644 --- a/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py +++ b/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_test.py @@ -200,7 +200,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, ag_always_validation_sequence_set=[[]], ag_eventually_validation_sequence_set=placement_eventually_validation_sequence_set, - test_timeout_s=15, + test_timeout_s=[15], ) simulated_test_runner.run_test( @@ -209,7 +209,7 @@ def run_ball_placement_scenario( inv_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, ag_always_validation_sequence_set=drop_ball_always_validation_sequence_set, ag_eventually_validation_sequence_set=drop_ball_eventually_validation_sequence_set, - test_timeout_s=5, + test_timeout_s=[5], ) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py index 7b31b26718..ce8968de8f 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.py @@ -1,3 +1,5 @@ +import threading + import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName @@ -55,6 +57,14 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.YELLOW ) + # Let robots get ready before starting kickoff + threading.Timer( + 4.0, + lambda: simulated_test_runner.send_gamecontroller_command( + gc_command=Command.Type.NORMAL_START, team=Team.YELLOW + ), + ).start() + simulated_test_runner.set_plays( blue_play=PlayName.KickoffEnemyPlay, yellow_play=PlayName.KickoffFriendlyPlay, @@ -135,9 +145,6 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, - ci_cmd_with_delay=[ - (4.0, Command.Type.NORMAL_START, Team.YELLOW), - ], ) diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp index a2cb6c42af..7c1afd31f9 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp @@ -42,7 +42,7 @@ void KickoffFriendlyPlayFSM::createKickoffSetupPositions(const WorldPtr& world_p kickoff_setup_positions = { // Robot 1 Point(world_ptr->field().centerPoint() + - Vector(-0.95 * world_ptr->field().centerCircleRadius(), 0)), + Vector(-world_ptr->field().centerCircleRadius(), 0)), // Robot 2 // Goalie positions will be handled by the goalie tactic // Robot 3 diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py index 9510feae33..a7e33d5d34 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.py @@ -1,3 +1,5 @@ +import threading + import software.python_bindings as tbots_cpp from proto.play_pb2 import PlayName @@ -60,6 +62,14 @@ def setup(*args): gc_command=Command.Type.KICKOFF, team=Team.BLUE ) + # Let robots get ready before starting kickoff + threading.Timer( + 4.0, + lambda: simulated_test_runner.send_gamecontroller_command( + gc_command=Command.Type.NORMAL_START, team=Team.BLUE + ), + ).start() + simulated_test_runner.set_plays( blue_play=PlayName.KickoffFriendlyPlay, yellow_play=PlayName.KickoffEnemyPlay, @@ -135,9 +145,6 @@ def setup(*args): ag_eventually_validation_sequence_set=eventually_validation_sequence_set, ag_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, - ci_cmd_with_delay=[ - (4.0, Command.Type.NORMAL_START, Team.BLUE), - ], ) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 4d048d995a..25e92dce64 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -1,3 +1,4 @@ +import threading import queue import argparse import time @@ -25,9 +26,8 @@ LAUNCH_DELAY_S = 0.1 WORLD_BUFFER_TIMEOUT = 0.5 -PROCESS_BUFFER_DELAY_S = 0.1 +PROCESS_BUFFER_DELAY_S = 0.01 PAUSE_AFTER_FAIL_DELAY_S = 3 -SECONDS_PER_MILLISECOND = 0.001 class SimulatedTestRunner(TbotsTestRunner): @@ -101,51 +101,57 @@ def sync_setup(self, setup, param): WorldStateReceivedTrigger, world_state_received_buffer ) - retries = 0 - MAX_RETRIES = 100 # 10 seconds total at 0.1s sleep - while retries < MAX_RETRIES: + while True: setup(param) try: world_state_received_buffer.get( - block=True, timeout=0.1, return_cached=False + block=True, timeout=WORLD_BUFFER_TIMEOUT ) - return except queue.Empty: - retries += 1 - - raise TimeoutError("Timed out waiting for Simulator to receive world state") + # Did not receive a response within timeout period + continue + else: + # Received a response from the simulator + break def runner( self, always_validation_sequence_set, eventually_validation_sequence_set, - test_timeout_s=3, - tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=None, - run_till_end=False, - **kwargs, + test_timeout_s, + tick_duration_s, + ci_cmd_with_delay, + run_till_end, ): - """Ticks the simulation forward while running the validations - - :param eventually_validation_sequence_set: validation set that must eventually be true - :param always_validation_sequence_set: validation set that must always be true - :param test_timeout_s: how long the test will run - :param tick_duration_s: how long each simulation step will be - :param run_till_end: if the test should run till the test timeout even if a pass condition is reached + """Run a test + + :param always_validation_sequence_set: Validation functions that should + hold on every tick + :param eventually_validation_sequence_set: Validation that should + eventually be true, before the test ends + :param test_timeout_s: The timeout for the test, if any eventually_validations + remain after the timeout, the test fails. + :param tick_duration_s: The simulation step duration :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. - [(1.0, Command.Type.NORMAL_START, Team.BLUE)] + [ + (time, command, team), + (time, command, team), + ... + ] + :param run_till_end: If true, test runs till the end even if eventually validation passes + If false, test stops once eventually validation passes and fails if time out """ - ci_cmd_with_delay = list(ci_cmd_with_delay) if ci_cmd_with_delay else [] time_elapsed_s = 0 + eventually_validation_failure_msg = "Test Timed Out" - eventually_validation_proto_set = None while time_elapsed_s < test_timeout_s: - start_time = time.time() + # get time before we execute the loop + processing_start_time = time.time() # Check for new CI commands at this time step - for delay, cmd, team in ci_cmd_with_delay[:]: + for delay, cmd, team in ci_cmd_with_delay: # If delay matches time if delay <= time_elapsed_s: # send command @@ -157,9 +163,7 @@ def runner( self.simulator_proto_unix_io.send_proto(SimulatorTick, tick) time_elapsed_s += tick_duration_s - retry_count = 0 - MAX_RETRIES = 5 - while retry_count < MAX_RETRIES: + while True: try: world = self.world_buffer.get( block=True, timeout=WORLD_BUFFER_TIMEOUT, return_cached=False @@ -175,25 +179,24 @@ def runner( break except queue.Empty: - retry_count += 1 - logger.warning( - f"Timeout waiting for world/primitives (retry {retry_count}/{MAX_RETRIES}). Resending SSL Wrapper." + # If we timeout, that means full_system missed the last + # wrapper and robot status, lets resend it. + logger.warning("Fullsystem missed last wrapper, resending ...") + + ssl_wrapper = self.ssl_wrapper_buffer.get(block=False) + robot_status = self.robot_status_buffer.get(block=False) + + self.blue_full_system_proto_unix_io.send_proto( + SSL_WrapperPacket, ssl_wrapper ) - # No world or primitives was found within the given timeout. Re-send the SSL wrapper packet and try again. - for packet in self.ssl_wrapper_buffer.get_all(): - self.blue_full_system_proto_unix_io.send_proto( - SSL_WrapperPacket, packet - ) - self.yellow_full_system_proto_unix_io.send_proto( - SSL_WrapperPacket, packet - ) - - if retry_count == MAX_RETRIES: - raise TimeoutError( - "Timed out waiting for world/primitive updates from AI/Simulator" - ) + self.blue_full_system_proto_unix_io.send_proto( + RobotStatus, robot_status + ) + + # get the time difference after we get the primitive (after any blocking that happened) + processing_time = time.time() - processing_start_time - processing_time = time.time() - start_time + # if the time we have blocked is less than a tick, sleep for the remaining time (for Thunderscope only) if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) @@ -245,53 +248,121 @@ def runner( # Check that all eventually validations are eventually valid validation.check_validation(eventually_validation_proto_set) + self.__stopper() @override def run_test( self, - always_validation_sequence_set=None, - eventually_validation_sequence_set=None, - setup=None, + always_validation_sequence_set, + eventually_validation_sequence_set, test_timeout_s=3, - tick_duration_s=1.0 / 60.0, - ci_cmd_with_delay=None, - run_till_end=False, + tick_duration_s=0.0166, # Default to 60hz + index=0, + ci_cmd_with_delay=[], + run_till_end=True, + **kwargs, + ): + """Helper function to run a test, with thunderscope if enabled + + :param always_validation_sequence_set: validation that should always be true + :param eventually_validation_sequence_set: validation that should eventually be true + :param test_timeout_s: how long the test should run before timing out + :param tick_duration_s: length of a tick + :param index: index of the current test. default is 0 (invariant test) + values can be passed in during aggregate testing for different timeout durations + :param ci_cmd_with_delay: A list consisting of tuples with a duration and CI command, e.g. + [ + (time, command, team), + (time, command, team), + ... + ] + :param run_till_end: If true, test runs till the end even if eventually validation passes + If false, test stops once eventually validation passes and fails if time out + """ + test_timeout_duration = ( + test_timeout_s[index] if type(test_timeout_s) == list else test_timeout_s + ) + + # If thunderscope is enabled, run the test in a thread and show + # thunderscope on this thread. The excepthook is setup to catch + # any test failures and propagate them to the main thread + if self.thunderscope: + run_sim_thread = threading.Thread( + target=self.runner, + daemon=True, + args=[ + always_validation_sequence_set, + eventually_validation_sequence_set, + test_timeout_duration, + tick_duration_s, + ci_cmd_with_delay, + run_till_end, + ], + ) + run_sim_thread.start() + self.thunderscope.show() + run_sim_thread.join() + + if self.last_exception: + pytest.fail(str(self.last_exception)) + + # If thunderscope is disabled, just run the test + else: + self.runner( + always_validation_sequence_set, + eventually_validation_sequence_set, + test_timeout_duration, + tick_duration_s, + ci_cmd_with_delay=ci_cmd_with_delay, + run_till_end=run_till_end, + ) + + +class InvariantTestRunner(SimulatedTestRunner): + """Runs a simulated test only once with a given parameter + + Test passes or fails based on the outcome of this test + """ + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + @override + def run_test( + self, + setup=(lambda x: None), + params=[0], + inv_always_validation_sequence_set=[[]], + inv_eventually_validation_sequence_set=[[]], **kwargs, ): - """Begins validating a test based on incoming world protos + """Run an invariant test - :param eventually_validation_sequence_set: validation set that must eventually be true - :param always_validation_sequence_set: validation set that must always be true - :param setup: initialization function for this test - :param test_timeout_s: how long the test will run + :param setup: Function that sets up the World state and the gamecontroller before running the test + :param params: List of parameters for each iteration of the test + (this method only uses the first element) + :param inv_always_validation_sequence_set: Validation functions for invariant testing + that should hold on every tick + :param inv_eventually_validation_sequence_set: Validation functions for invariant testing + that should eventually be true, before the test ends """ - always_validation_sequence_set = always_validation_sequence_set or [[]] - eventually_validation_sequence_set = eventually_validation_sequence_set or [[]] - # Set the hook for exception handling so that we can close the thunderscope - # instance should one exist - sys.excepthook = self.excepthook - - # Only run setup if provided and if we are not being called from AggregateTestRunner - # (which handles its own setup loop) - if setup and "params" not in kwargs: - self.sync_setup(setup, self) - - self.runner( - always_validation_sequence_set=always_validation_sequence_set, - eventually_validation_sequence_set=eventually_validation_sequence_set, - test_timeout_s=test_timeout_s, - tick_duration_s=tick_duration_s, - ci_cmd_with_delay=ci_cmd_with_delay, - run_till_end=run_till_end, + threading.excepthook = self.excepthook + + super().sync_setup(setup, params[0]) + + super().run_test( + inv_always_validation_sequence_set, + inv_eventually_validation_sequence_set, **kwargs, ) class AggregateTestRunner(SimulatedTestRunner): - """A test runner for aggregate tests. - These tests are a collection of invariant tests. If any of the invariant tests fail, - the aggregate test fails. + """Runs a simulated test multiple times with different given parameters + + Result of the test is determined by comparing the number of + passing iterations to a predetermined acceptable threshold """ def __init__(self, *args, **kwargs): @@ -301,39 +372,35 @@ def __init__(self, *args, **kwargs): def run_test( self, setup=(lambda arg: None), - params=None, - ag_always_validation_sequence_set=None, - ag_eventually_validation_sequence_set=None, + params=[], + ag_always_validation_sequence_set=[[]], + ag_eventually_validation_sequence_set=[[]], **kwargs, ): - """Begins validating a test based on incoming world protos. Runs the - invariant test first, then the aggregate test. + """Run an aggregate test - :param setup: initialization function for this test + :param setup: Function that sets up the World state and the gamecontroller before running the test :param params: List of parameters for each iteration of the test - :param ag_eventually_validation_sequence_set: validation set for aggregate test that must eventually be true - :param ag_always_validation_sequence_set: validation set for aggregate test that must always be true + :param ag_always_validation_sequence_set: Validation functions for aggregate testing + that should hold on every tick + :param ag_eventually_validation_sequence_set: Validation functions for aggregate testing + that should eventually be true, before the test end """ - params = params or [] - ag_always_validation_sequence_set = ag_always_validation_sequence_set or [[]] - ag_eventually_validation_sequence_set = ( - ag_eventually_validation_sequence_set or [[]] - ) - sys.excepthook = self.excepthook + threading.excepthook = self.excepthook failed_tests = 0 - # Create a copy of kwargs without 'params' to avoid double-setup in SimulatedTestRunner - clean_kwargs = {k: v for k, v in kwargs.items() if k != "params"} - + # Runs the test once for each given parameter + # Catches Assertion Error thrown by failing test and increments counter + # Calculates overall results and prints them for x in range(len(params)): super().sync_setup(setup, params[x]) try: super().run_test( - always_validation_sequence_set=ag_always_validation_sequence_set, - eventually_validation_sequence_set=ag_eventually_validation_sequence_set, - **clean_kwargs, + ag_always_validation_sequence_set, + ag_eventually_validation_sequence_set, + **kwargs, ) except AssertionError: failed_tests += 1 @@ -427,12 +494,6 @@ def load_command_line_arguments(allow_unrecognized: bool = False): default=False, help="Use realism in the simulator", ) - parser.add_argument( - "--ci_mode", - action="store_true", - default=False, - help="Run in CI mode (faster execution)", - ) return parser.parse_known_args()[0] if allow_unrecognized else parser.parse_args() @@ -481,14 +542,14 @@ def simulated_test_runner(): args.debug_blue_full_system, False, should_restart_on_crash=False, - running_in_realtime=not args.ci_mode, + 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=not args.ci_mode, + running_in_realtime=args.enable_thunderscope, ) as yellow_fs: with Gamecontroller( suppress_logs=(not args.show_gamecontroller_logs) @@ -519,9 +580,7 @@ def simulated_test_runner(): layout_path=args.layout, ) - # Even in CI mode, give a small delay for processes to start up - actual_launch_delay = 0.5 if args.ci_mode else LAUNCH_DELAY_S - time.sleep(actual_launch_delay) + time.sleep(LAUNCH_DELAY_S) runner = None @@ -536,7 +595,7 @@ def simulated_test_runner(): gamecontroller, ) else: - runner = SimulatedTestRunner( + runner = InvariantTestRunner( current_test, tscope, simulator_proto_unix_io,