diff --git a/src/proto/message_translation/tbots_protobuf.cpp b/src/proto/message_translation/tbots_protobuf.cpp index 7c146b4c45..2ba0c2c6b9 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_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/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..0ab068eded 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_trajectory_max_speed_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_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 = 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, - .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 * 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..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_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/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..4bce8c76df 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 equal to another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise + */ + bool equals(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..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 @@ -83,6 +83,19 @@ class BangBangTrajectory1DAngular */ double getTotalTime() const override; + /** + * Check if this trajectory is meaningfully equal to another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold below which the trajectories are considered + * equal in degrees. + * @return True if the trajectories are equal, false otherwise + */ + bool equals(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..caffd6b86c 100644 --- a/src/software/ai/navigator/trajectory/trajectory.hpp +++ b/src/software/ai/navigator/trajectory/trajectory.hpp @@ -59,4 +59,13 @@ class Trajectory { return getPosition(getTotalTime()); } + + /** + * Check if this trajectory is meaningfully equal to another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise + */ + 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 9699f07c17..e40b367f58 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 equal to another trajectory. + * @param other The other trajectory to compare to + * @param threshold The threshold below which the trajectories are considered + * equal + * @return True if the trajectories are equal, false otherwise + */ + bool equals(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..e56929c45f 100644 --- a/src/software/embedded/BUILD +++ b/src/software/embedded/BUILD @@ -34,6 +34,8 @@ 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", @@ -58,12 +60,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 +105,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..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/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..90248c5459 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,32 @@ 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 +69,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..e28ccfb402 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, @@ -195,7 +190,7 @@ void StSpinMotorController::updateEuclideanVelocity( EuclideanSpace_t target_euclidean_velocity) { const Vector local_velocity(target_euclidean_velocity[1], - -target_euclidean_velocity[0]); + target_euclidean_velocity[0]); if (local_velocity.length() <= MINIMUM_SPEED_FOR_FEED_FORWARD) { diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index d1f56161ee..691889c4a7 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,13 +11,10 @@ #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) + : time_since_linear_trajectory_creation_(Duration::fromSeconds(0)), + time_since_angular_trajectory_creation_(Duration::fromSeconds(0)), + robot_constants_(robot_constants) { } @@ -27,82 +24,115 @@ 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_->equals(*new_trajectory_path, + LINEAR_DESTINATION_THRESHOLD_METERS)); + + if (is_linear_traj_new) + { + trajectory_path_ = new_trajectory_path; + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); + position_controller_.reset(); + } + + const BangBangTrajectory1DAngular new_angular_trajectory = createAngularTrajectoryFromParams(current_primitive_.move().w_traj_params(), - angular_velocity_, robot_constants_); + orientation_, angular_velocity_, + robot_constants_); - time_since_trajectory_creation_ = Duration::fromSeconds(VISION_TO_ROBOT_DELAY_S); - } -} + const bool is_angular_traj_new = + !angular_trajectory_.has_value() || + !angular_trajectory_->equals(new_angular_trajectory, + ANGULAR_DESTINATION_THRESHOLD_DEGREES); -void PrimitiveExecutor::setStopPrimitive() -{ - current_primitive_ = *createStopPrimitiveProto(); + if (is_angular_traj_new) + { + angular_trajectory_ = new_angular_trajectory; + time_since_angular_trajectory_creation_ = Duration::fromSeconds(0); + orientation_controller_.reset(); + } + } } -void PrimitiveExecutor::updateVelocity(const Vector& local_velocity, - const AngularVelocity& angular_velocity) +void PrimitiveExecutor::updateState(const Point& position, const Vector& velocity, + const Angle& orientation, + const AngularVelocity& angular_velocity) { - Vector actual_global_velocity = localToGlobalVelocity(local_velocity, orientation_); - velocity_ = actual_global_velocity; - angular_velocity_ = angular_velocity; -} + position_ = position; + velocity_ = velocity; + orientation_ = orientation; + angular_velocity_ = angular_velocity; -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) + if (!current_primitive_.has_move()) { - local_velocity *= distance_to_destination / MAX_DAMPENING_VELOCITY_DISTANCE_M; + return; } - 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 we are lagging behind trajectory too much, we have stalled! We need to + // regenerate trajectory. + if (trajectory_path_.has_value()) { - angular_velocity *= orientation_to_destination.toDegrees() / 5; + const double linear_following_error = + (position_ - trajectory_path_->getPosition( + time_since_linear_trajectory_creation_.toSeconds())) + .length(); + + 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_); + + time_since_linear_trajectory_creation_ = Duration::fromSeconds(0); + } } - return angular_velocity; -} + if (angular_trajectory_.has_value()) + { + const double angular_following_error = + angular_trajectory_ + ->getPosition(time_since_angular_trajectory_creation_.toSeconds()) + .minDiff(orientation_) + .toDegrees(); + 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_ = Duration::fromSeconds(0); + } + } +} std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status) + TbotsProto::PrimitiveExecutorStatus& status, const Duration& delta_time) { - time_since_trajectory_creation_ += time_step_; + 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); 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 +143,43 @@ 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 Vector target_linear_velocity_global = position_controller_.step( + 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); + + 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..26b400c637 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -4,97 +4,79 @@ #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/embedded/motion_control/orientation_controller.h" +#include "software/embedded/motion_control/position_controller.h" #include "software/geom/vector.h" -#include "software/time/duration.h" #include "software/world/robot_state.h" -#include "software/world/team_types.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 - * + * @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: - /* - * 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_; + Duration time_since_linear_trajectory_creation_; + 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.4; + static constexpr double ANGULAR_STALL_ERROR_MAX_DEGREES = 13.0; }; diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer.cpp new file mode 100644 index 0000000000..a095983d90 --- /dev/null +++ b/src/software/embedded/robot_localizer.cpp @@ -0,0 +1,320 @@ +#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); + + auto rollback_point = std::find_if( + history.begin(), history.end(), + [&](const FilterStep& step) { return (current_time - step.time) >= sample_age; }); + + if (rollback_point == history.begin()) + { + // 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; + } + + 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()); + } + + // 1. Save the state from the rollback point + filter_.state_estimate = rollback_point->state_estimate; + filter_.state_covariance = rollback_point->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); + + // 4. Replay the remaining history (from oldest to newest) + for (auto it = history.rbegin(); it != history.rend(); ++it) + { + if (it->prediction.has_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 (it->update.has_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; + } +} + +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/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/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..b23926e9fe 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,23 @@ 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); + const Duration delta_time = Duration::fromSeconds( + getMilliseconds(time_since_prev_iter) * SECONDS_PER_MILLISECOND); - ZoneNamedN(_tracy_network_poll, "Thunderloop: Poll NetworkService", true); + processNetworkPolling(poll_time, last_primitive_received_time); - new_primitive = network_service_->poll(robot_status_); - } + processLocalizationUpdates(); - thunderloop_status_.set_network_service_poll_time_ms( - getMilliseconds(poll_time)); + processPrimitiveExecution(poll_time, last_primitive_received_time, + delta_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 +210,185 @@ 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()) + + Angle::half(); + + 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[0], accel[1]); + } + + 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, + const Duration& delta_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_, delta_time); + } + + 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..434035c933 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,43 @@ 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 + * @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); + + /** + * 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 +193,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..a67bc341a4 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() @@ -425,6 +466,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/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) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index 7a645b85e1..a0b3c9ee51 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 + Angle::half(), + 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,22 @@ 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& [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) @@ -391,15 +401,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( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *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( @@ -564,18 +577,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/world/robot.cpp b/src/software/world/robot.cpp index 66e2244d76..2b987e7b60 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_trajectory_max_speed_m_per_s, robot_constants_.robot_max_acceleration_m_per_s_2, initial_velocity_1d, final_velocity_1d); }