diff --git a/.clang-format b/.clang-format index a6ac2dd8be..e19490c8cd 100644 --- a/.clang-format +++ b/.clang-format @@ -19,7 +19,7 @@ IncludeBlocks: Regroup IndentCaseLabels: 'true' IndentWidth: '4' MaxEmptyLinesToKeep: '3' -NamespaceIndentation: All +NamespaceIndentation: None ReflowComments: 'true' SortIncludes: 'true' Standard: Cpp11 @@ -50,7 +50,7 @@ IncludeBlocks: Regroup IndentCaseLabels: 'true' IndentWidth: '4' MaxEmptyLinesToKeep: '3' -NamespaceIndentation: All +NamespaceIndentation: None ReflowComments: 'true' SortIncludes: 'true' Standard: Cpp11 diff --git a/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h index 3670e3f81e..3c3cf20176 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/robotmesh.h @@ -26,11 +26,11 @@ namespace camun { - namespace simulator - { - std::vector>> createRobotMesh( - float radius, float height, float angle, float holeDepth, float holeHeight); - } // namespace simulator +namespace simulator +{ +std::vector>> createRobotMesh( + float radius, float height, float angle, float holeDepth, float holeHeight); +} // namespace simulator } // namespace camun #endif // ROBOTMESH_H diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simball.h b/src/extlibs/er_force_sim/src/amun/simulator/simball.h index 1d9022c837..9e70d359a3 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simball.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simball.h @@ -31,10 +31,10 @@ namespace camun { - namespace simulator - { - class SimBall; - } // namespace simulator +namespace simulator +{ +class SimBall; +} // namespace simulator } // namespace camun class camun::simulator::SimBall diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h index b7743d2f43..f4798fadcc 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simfield.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simfield.h @@ -27,10 +27,10 @@ namespace camun { - namespace simulator - { - class SimField; - } // namespace simulator +namespace simulator +{ +class SimField; +} // namespace simulator } // namespace camun class camun::simulator::SimField diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h index c156f80ee5..5467013d08 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simrobot.h @@ -33,10 +33,10 @@ namespace camun { - namespace simulator - { - class SimRobot; - } // namespace simulator +namespace simulator +{ +class SimRobot; +} // namespace simulator } // namespace camun class camun::simulator::SimRobot diff --git a/src/extlibs/er_force_sim/src/amun/simulator/simulator.h b/src/extlibs/er_force_sim/src/amun/simulator/simulator.h index 48d3560c15..3f6fa3d124 100644 --- a/src/extlibs/er_force_sim/src/amun/simulator/simulator.h +++ b/src/extlibs/er_force_sim/src/amun/simulator/simulator.h @@ -40,11 +40,11 @@ const float FOCAL_LENGTH = 390.f; namespace camun { - namespace simulator - { - class Simulator; - struct SimulatorData; - } // namespace simulator +namespace simulator +{ +class Simulator; +struct SimulatorData; +} // namespace simulator } // namespace camun class camun::simulator::Simulator diff --git a/src/extlibs/er_force_sim/src/core/coordinates.h b/src/extlibs/er_force_sim/src/core/coordinates.h index 20137e3587..6f1a272cd1 100644 --- a/src/extlibs/er_force_sim/src/core/coordinates.h +++ b/src/extlibs/er_force_sim/src/core/coordinates.h @@ -27,201 +27,201 @@ namespace core { - namespace internal - { - // for pairs - template - auto set(T& t, float x, float y) -> decltype(t.first = x, void()) - { - t.first = x; - t.second = y; - } - - // for core vectors - template - auto set(T& t, float x, float y) -> decltype(t.x = x, void()) - { - t.x = x; - t.y = y; - } - - // for bullet vectors - template - auto set(T& t, float x, float y) -> decltype(t.setX(x), void()) - { - t.setX(x); - t.setY(y); - } - - // for protobuf messages - template - auto setPos(T& t, float x, float y) -> decltype(t.set_x(x), void()) - { - t.set_x(x); - t.set_y(y); - } - - template - auto setPos(T& t, float x, float y) -> decltype(set(t, x, y), void()) - { - set(t, x, y); - } - - template - auto setVel(T& t, float x, float y) -> decltype(set(t, x, y), void()) - { - set(t, x, y); - } - - template - auto setVel(T& t, float x, float y) -> decltype(t.set_v_x(x), void()) - { - t.set_v_x(x); - t.set_v_y(y); - } - - template - auto setVel(T& t, float x, float y) -> decltype(t.set_vx(x), void()) - { - t.set_vx(x); - t.set_vy(y); - } - - template - auto get(const T& t, float& x, float& y) -> decltype(x = t.first, void()) - { - x = t.first; - y = t.second; - } - - - template - auto get(const T& t, float& x, float& y) -> decltype(x = t.x, void()) - { - x = t.x; - y = t.y; - } - - - // for our bullet vectors and some messages - template - auto getPos(const T& t, float& x, float& y) -> decltype(x = t.x(), void()) - { - x = t.x(); - y = t.y(); - } - - template - auto getPos(const T& t, float& x, float& y) -> decltype(get(t, x, y)) - { - get(t, x, y); - } - - template - auto getVel(const T& t, float& x, float& y) -> decltype(get(t, x, y)) - { - get(t, x, y); - } - - template - auto getVel(const T& t, float& x, float& y) -> decltype(x = t.v_x(), void()) - { - x = t.v_x(); - y = t.v_y(); - } - - template - auto getVel(const T& t, float& x, float& y) -> decltype(x = t.vx(), void()) - { - x = t.vx(); - y = t.vy(); - } - } // namespace internal +namespace internal +{ +// for pairs +template +auto set(T& t, float x, float y) -> decltype(t.first = x, void()) +{ + t.first = x; + t.second = y; +} + +// for core vectors +template +auto set(T& t, float x, float y) -> decltype(t.x = x, void()) +{ + t.x = x; + t.y = y; +} + +// for bullet vectors +template +auto set(T& t, float x, float y) -> decltype(t.setX(x), void()) +{ + t.setX(x); + t.setY(y); +} + +// for protobuf messages +template +auto setPos(T& t, float x, float y) -> decltype(t.set_x(x), void()) +{ + t.set_x(x); + t.set_y(y); +} + +template +auto setPos(T& t, float x, float y) -> decltype(set(t, x, y), void()) +{ + set(t, x, y); +} + +template +auto setVel(T& t, float x, float y) -> decltype(set(t, x, y), void()) +{ + set(t, x, y); +} + +template +auto setVel(T& t, float x, float y) -> decltype(t.set_v_x(x), void()) +{ + t.set_v_x(x); + t.set_v_y(y); +} + +template +auto setVel(T& t, float x, float y) -> decltype(t.set_vx(x), void()) +{ + t.set_vx(x); + t.set_vy(y); +} + +template +auto get(const T& t, float& x, float& y) -> decltype(x = t.first, void()) +{ + x = t.first; + y = t.second; +} + + +template +auto get(const T& t, float& x, float& y) -> decltype(x = t.x, void()) +{ + x = t.x; + y = t.y; +} + + +// for our bullet vectors and some messages +template +auto getPos(const T& t, float& x, float& y) -> decltype(x = t.x(), void()) +{ + x = t.x(); + y = t.y(); +} + +template +auto getPos(const T& t, float& x, float& y) -> decltype(get(t, x, y)) +{ + get(t, x, y); +} + +template +auto getVel(const T& t, float& x, float& y) -> decltype(get(t, x, y)) +{ + get(t, x, y); +} + +template +auto getVel(const T& t, float& x, float& y) -> decltype(x = t.v_x(), void()) +{ + x = t.v_x(); + y = t.v_y(); +} + +template +auto getVel(const T& t, float& x, float& y) -> decltype(x = t.vx(), void()) +{ + x = t.vx(); + y = t.vy(); +} +} // namespace internal } // namespace core namespace coordinates { - template - void fromVision(const F& from, T& to) - { - float detectionX, detectionY; - core::internal::getPos(from, detectionX, detectionY); - float x = -detectionY / 1000.0f; - float y = detectionX / 1000.0f; - core::internal::setPos(to, x, y); - } - - template - void toVision(const F& from, T& to) - { - float x, y; - core::internal::getPos(from, x, y); - float visionX, visionY; - visionX = y * 1000.f; - visionY = -x * 1000.f; - core::internal::setPos(to, visionX, visionY); - } - - template - void fromVisionVelocity(const F& from, T& to) - { - std::pair vision; - std::pair result; - core::internal::getVel(from, vision.first, vision.second); - fromVision(vision, result); - core::internal::setVel(to, result.first, result.second); - } - - template - void toVisionVelocity(const F& from, T& to) - { - std::pair vision; - std::pair result; - core::internal::getVel(from, vision.first, vision.second); - toVision(vision, result); - core::internal::setVel(to, result.first, result.second); - } - - inline float fromVisionRotation(float vision) - { - return vision + M_PI_2; - } - - inline float toVisionRotation(float internRotation) - { - return internRotation - M_PI_2; - } - - inline float chipVelFromChipDistance(float distance) - { - const float angle = 45.f / 180 * M_PI; - const float dirFloor = std::cos(angle); - const float dirUp = std::sin(angle); - const float gravity = 9.81; - // airtime = 2 * (shootSpeed * dirUp) / g - // targetDist = shootSpeed * dirFloor * airtime - // => targetDist = shootSpeed * dirFloor * (2 * shootSpeed * dirUp) / g = 2 * - // shootSpeed**2 * dirFloor * dirUp / g - const float shootSpeed = - std::sqrt(distance * gravity / (2 * std::abs(dirUp * dirFloor))); - return shootSpeed; - } - - inline float chipDistanceFromChipVel(float velocity) - { - const float angle = 45.f / 180 * M_PI; - const float dirFloor = std::cos(angle); - const float dirUp = std::sin(angle); - const float gravity = 9.81; - // airtime = 2 * (shootSpeed * dirUp) / g - // targetDist = shootSpeed * dirFloor * airtime - // => targetDist = shootSpeed * dirFloor * (2 * shootSpeed * dirUp) / g = 2 * - // shootSpeed**2 * dirFloor * dirUp / g - const float targetDist = 2 * velocity * velocity * dirFloor * dirUp / gravity; - - return targetDist; - } +template +void fromVision(const F& from, T& to) +{ + float detectionX, detectionY; + core::internal::getPos(from, detectionX, detectionY); + float x = -detectionY / 1000.0f; + float y = detectionX / 1000.0f; + core::internal::setPos(to, x, y); +} + +template +void toVision(const F& from, T& to) +{ + float x, y; + core::internal::getPos(from, x, y); + float visionX, visionY; + visionX = y * 1000.f; + visionY = -x * 1000.f; + core::internal::setPos(to, visionX, visionY); +} + +template +void fromVisionVelocity(const F& from, T& to) +{ + std::pair vision; + std::pair result; + core::internal::getVel(from, vision.first, vision.second); + fromVision(vision, result); + core::internal::setVel(to, result.first, result.second); +} + +template +void toVisionVelocity(const F& from, T& to) +{ + std::pair vision; + std::pair result; + core::internal::getVel(from, vision.first, vision.second); + toVision(vision, result); + core::internal::setVel(to, result.first, result.second); +} + +inline float fromVisionRotation(float vision) +{ + return vision + M_PI_2; +} + +inline float toVisionRotation(float internRotation) +{ + return internRotation - M_PI_2; +} + +inline float chipVelFromChipDistance(float distance) +{ + const float angle = 45.f / 180 * M_PI; + const float dirFloor = std::cos(angle); + const float dirUp = std::sin(angle); + const float gravity = 9.81; + // airtime = 2 * (shootSpeed * dirUp) / g + // targetDist = shootSpeed * dirFloor * airtime + // => targetDist = shootSpeed * dirFloor * (2 * shootSpeed * dirUp) / g = 2 * + // shootSpeed**2 * dirFloor * dirUp / g + const float shootSpeed = + std::sqrt(distance * gravity / (2 * std::abs(dirUp * dirFloor))); + return shootSpeed; +} + +inline float chipDistanceFromChipVel(float velocity) +{ + const float angle = 45.f / 180 * M_PI; + const float dirFloor = std::cos(angle); + const float dirUp = std::sin(angle); + const float gravity = 9.81; + // airtime = 2 * (shootSpeed * dirUp) / g + // targetDist = shootSpeed * dirFloor * airtime + // => targetDist = shootSpeed * dirFloor * (2 * shootSpeed * dirUp) / g = 2 * + // shootSpeed**2 * dirFloor * dirUp / g + const float targetDist = 2 * velocity * velocity * dirFloor * dirUp / gravity; + + return targetDist; +} } // namespace coordinates diff --git a/src/extlibs/er_force_sim/src/protobuf/geometry.cpp b/src/extlibs/er_force_sim/src/protobuf/geometry.cpp index 63bb8be0e8..6af751499e 100644 --- a/src/extlibs/er_force_sim/src/protobuf/geometry.cpp +++ b/src/extlibs/er_force_sim/src/protobuf/geometry.cpp @@ -118,35 +118,35 @@ void convertFromSSlGeometry(const SSLProto::SSL_GeometryFieldSize &g, namespace proto_geom_internal { - static void fieldAddLine(SSLProto::SSL_GeometryFieldSize *field, std::string name, - float x1, float y1, float x2, float y2, - const world::Geometry &geometry) - { - SSLProto::SSL_FieldLineSegment *line = field->add_field_lines(); - line->set_name(std::move(name)); - SSLProto::Vector2f *p1 = line->mutable_p1(); - p1->set_x(x1); - p1->set_y(y1); - SSLProto::Vector2f *p2 = line->mutable_p2(); - p2->set_x(x2); - p2->set_y(y2); - line->set_thickness(geometry.line_width() * 1000.0f); - } +static void fieldAddLine(SSLProto::SSL_GeometryFieldSize *field, std::string name, + float x1, float y1, float x2, float y2, + const world::Geometry &geometry) +{ + SSLProto::SSL_FieldLineSegment *line = field->add_field_lines(); + line->set_name(std::move(name)); + SSLProto::Vector2f *p1 = line->mutable_p1(); + p1->set_x(x1); + p1->set_y(y1); + SSLProto::Vector2f *p2 = line->mutable_p2(); + p2->set_x(x2); + p2->set_y(y2); + line->set_thickness(geometry.line_width() * 1000.0f); +} - static void fieldAddCircularArc(SSLProto::SSL_GeometryFieldSize *field, - std::string name, float x, float y, float radius, - float a1, float a2, const world::Geometry &geometry) - { - SSLProto::SSL_FieldCircularArc *arc = field->add_field_arcs(); - arc->set_name(name); - SSLProto::Vector2f *center = arc->mutable_center(); - center->set_x(x); - center->set_y(y); - arc->set_radius(radius); - arc->set_a1(a1); - arc->set_a2(a2); - arc->set_thickness(geometry.line_width() * 1000.0f); - } +static void fieldAddCircularArc(SSLProto::SSL_GeometryFieldSize *field, std::string name, + float x, float y, float radius, float a1, float a2, + const world::Geometry &geometry) +{ + SSLProto::SSL_FieldCircularArc *arc = field->add_field_arcs(); + arc->set_name(name); + SSLProto::Vector2f *center = arc->mutable_center(); + center->set_x(x); + center->set_y(y); + arc->set_radius(radius); + arc->set_a1(a1); + arc->set_a2(a2); + arc->set_thickness(geometry.line_width() * 1000.0f); +} } // namespace proto_geom_internal using namespace proto_geom_internal; diff --git a/src/proto/message_translation/ssl_referee.cpp b/src/proto/message_translation/ssl_referee.cpp index a6bdf36f35..f96ae84a19 100644 --- a/src/proto/message_translation/ssl_referee.cpp +++ b/src/proto/message_translation/ssl_referee.cpp @@ -2,62 +2,8 @@ #include "shared/constants.h" -// this maps a protobuf SSLProto::Referee_Command enum to its equivalent internal type -// this map is used when we are on the blue team -const static std::unordered_map - blue_team_command_map = { - {SSLProto::Referee_Command_HALT, RefereeCommand::HALT}, - {SSLProto::Referee_Command_STOP, RefereeCommand::STOP}, - {SSLProto::Referee_Command_NORMAL_START, RefereeCommand::NORMAL_START}, - {SSLProto::Referee_Command_FORCE_START, RefereeCommand::FORCE_START}, - {SSLProto::Referee_Command_PREPARE_KICKOFF_BLUE, - RefereeCommand::PREPARE_KICKOFF_US}, - {SSLProto::Referee_Command_PREPARE_KICKOFF_YELLOW, - RefereeCommand::PREPARE_KICKOFF_THEM}, - {SSLProto::Referee_Command_PREPARE_PENALTY_BLUE, - RefereeCommand::PREPARE_PENALTY_US}, - {SSLProto::Referee_Command_PREPARE_PENALTY_YELLOW, - RefereeCommand::PREPARE_PENALTY_THEM}, - {SSLProto::Referee_Command_DIRECT_FREE_BLUE, RefereeCommand::DIRECT_FREE_US}, - {SSLProto::Referee_Command_DIRECT_FREE_YELLOW, RefereeCommand::DIRECT_FREE_THEM}, - {SSLProto::Referee_Command_INDIRECT_FREE_BLUE, RefereeCommand::INDIRECT_FREE_US}, - {SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, - RefereeCommand::INDIRECT_FREE_THEM}, - {SSLProto::Referee_Command_TIMEOUT_BLUE, RefereeCommand::TIMEOUT_US}, - {SSLProto::Referee_Command_TIMEOUT_YELLOW, RefereeCommand::TIMEOUT_THEM}, - {SSLProto::Referee_Command_BALL_PLACEMENT_BLUE, - RefereeCommand::BALL_PLACEMENT_US}, - {SSLProto::Referee_Command_BALL_PLACEMENT_YELLOW, - RefereeCommand::BALL_PLACEMENT_THEM}}; - -// this maps a protobuf SSLProto::Referee_Command enum to its equivalent internal type -// this map is used when we are on the yellow team -const static std::unordered_map - yellow_team_command_map = { - {SSLProto::Referee_Command_HALT, RefereeCommand::HALT}, - {SSLProto::Referee_Command_STOP, RefereeCommand::STOP}, - {SSLProto::Referee_Command_NORMAL_START, RefereeCommand::NORMAL_START}, - {SSLProto::Referee_Command_FORCE_START, RefereeCommand::FORCE_START}, - {SSLProto::Referee_Command_PREPARE_KICKOFF_BLUE, - RefereeCommand::PREPARE_KICKOFF_THEM}, - {SSLProto::Referee_Command_PREPARE_KICKOFF_YELLOW, - RefereeCommand::PREPARE_KICKOFF_US}, - {SSLProto::Referee_Command_PREPARE_PENALTY_BLUE, - RefereeCommand::PREPARE_PENALTY_THEM}, - {SSLProto::Referee_Command_PREPARE_PENALTY_YELLOW, - RefereeCommand::PREPARE_PENALTY_US}, - {SSLProto::Referee_Command_DIRECT_FREE_BLUE, RefereeCommand::DIRECT_FREE_THEM}, - {SSLProto::Referee_Command_DIRECT_FREE_YELLOW, RefereeCommand::DIRECT_FREE_US}, - {SSLProto::Referee_Command_INDIRECT_FREE_BLUE, - RefereeCommand::INDIRECT_FREE_THEM}, - {SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, - RefereeCommand::INDIRECT_FREE_US}, - {SSLProto::Referee_Command_TIMEOUT_BLUE, RefereeCommand::TIMEOUT_THEM}, - {SSLProto::Referee_Command_TIMEOUT_YELLOW, RefereeCommand::TIMEOUT_US}, - {SSLProto::Referee_Command_BALL_PLACEMENT_BLUE, - RefereeCommand::BALL_PLACEMENT_THEM}, - {SSLProto::Referee_Command_BALL_PLACEMENT_YELLOW, - RefereeCommand::BALL_PLACEMENT_US}}; +namespace ssl_referee +{ RefereeCommand createRefereeCommand(const SSLProto::Referee &packet, TeamColour team_colour) @@ -72,29 +18,6 @@ RefereeCommand createRefereeCommand(const SSLProto::Referee &packet, } } -// this maps a protobuf SSLProto::Referee_Stage enum to its RefereeStage equivalent -const static std::unordered_map - referee_stage_map = { - {SSLProto::Referee_Stage_NORMAL_FIRST_HALF_PRE, - RefereeStage::NORMAL_FIRST_HALF_PRE}, - {SSLProto::Referee_Stage_NORMAL_FIRST_HALF, RefereeStage::NORMAL_FIRST_HALF}, - {SSLProto::Referee_Stage_NORMAL_HALF_TIME, RefereeStage::NORMAL_HALF_TIME}, - {SSLProto::Referee_Stage_NORMAL_SECOND_HALF_PRE, - RefereeStage::NORMAL_SECOND_HALF_PRE}, - {SSLProto::Referee_Stage_NORMAL_SECOND_HALF, RefereeStage::NORMAL_SECOND_HALF}, - {SSLProto::Referee_Stage_EXTRA_TIME_BREAK, RefereeStage::EXTRA_TIME_BREAK}, - {SSLProto::Referee_Stage_EXTRA_FIRST_HALF_PRE, - RefereeStage::EXTRA_FIRST_HALF_PRE}, - {SSLProto::Referee_Stage_EXTRA_FIRST_HALF, RefereeStage::EXTRA_FIRST_HALF}, - {SSLProto::Referee_Stage_EXTRA_HALF_TIME, RefereeStage::EXTRA_HALF_TIME}, - {SSLProto::Referee_Stage_EXTRA_SECOND_HALF_PRE, - RefereeStage::EXTRA_SECOND_HALF_PRE}, - {SSLProto::Referee_Stage_EXTRA_SECOND_HALF, RefereeStage::EXTRA_SECOND_HALF}, - {SSLProto::Referee_Stage_PENALTY_SHOOTOUT_BREAK, - RefereeStage::PENALTY_SHOOTOUT_BREAK}, - {SSLProto::Referee_Stage_PENALTY_SHOOTOUT, RefereeStage::PENALTY_SHOOTOUT}, - {SSLProto::Referee_Stage_POST_GAME, RefereeStage::POST_GAME}}; - RefereeStage createRefereeStage(const SSLProto::Referee &packet) { return referee_stage_map.at(packet.stage()); @@ -114,3 +37,4 @@ std::optional getBallPlacementPoint(const SSLProto::Referee &packet) return std::nullopt; } } +} // namespace ssl_referee diff --git a/src/proto/message_translation/ssl_referee.h b/src/proto/message_translation/ssl_referee.h index e24d7c998f..bb37893b7c 100644 --- a/src/proto/message_translation/ssl_referee.h +++ b/src/proto/message_translation/ssl_referee.h @@ -5,6 +5,110 @@ #include "software/world/game_state.h" #include "software/world/team_types.h" + +namespace ssl_referee +{ + +/** + * @brief this maps a protobuf SSLProto::Referee_Command enum to its equivalent + * internal type + * @note this map is used when we are on the blue team + */ +inline const static std::unordered_map + blue_team_command_map = { + {SSLProto::Referee_Command_HALT, RefereeCommand::HALT}, + {SSLProto::Referee_Command_STOP, RefereeCommand::STOP}, + {SSLProto::Referee_Command_NORMAL_START, RefereeCommand::NORMAL_START}, + {SSLProto::Referee_Command_FORCE_START, RefereeCommand::FORCE_START}, + {SSLProto::Referee_Command_PREPARE_KICKOFF_BLUE, + RefereeCommand::PREPARE_KICKOFF_US}, + {SSLProto::Referee_Command_PREPARE_KICKOFF_YELLOW, + RefereeCommand::PREPARE_KICKOFF_THEM}, + {SSLProto::Referee_Command_PREPARE_PENALTY_BLUE, + RefereeCommand::PREPARE_PENALTY_US}, + {SSLProto::Referee_Command_PREPARE_PENALTY_YELLOW, + RefereeCommand::PREPARE_PENALTY_THEM}, + {SSLProto::Referee_Command_DIRECT_FREE_BLUE, RefereeCommand::DIRECT_FREE_US}, + {SSLProto::Referee_Command_DIRECT_FREE_YELLOW, RefereeCommand::DIRECT_FREE_THEM}, + {SSLProto::Referee_Command_INDIRECT_FREE_BLUE, RefereeCommand::INDIRECT_FREE_US}, + {SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, + RefereeCommand::INDIRECT_FREE_THEM}, + {SSLProto::Referee_Command_TIMEOUT_BLUE, RefereeCommand::TIMEOUT_US}, + {SSLProto::Referee_Command_TIMEOUT_YELLOW, RefereeCommand::TIMEOUT_THEM}, + {SSLProto::Referee_Command_BALL_PLACEMENT_BLUE, + RefereeCommand::BALL_PLACEMENT_US}, + {SSLProto::Referee_Command_BALL_PLACEMENT_YELLOW, + RefereeCommand::BALL_PLACEMENT_THEM}}; + +/** + * @brief this set contains an ongoing list of deprecated SSLProto::Referee_Commands + */ +inline const static std::unordered_set deprecated_commands = { + SSLProto::Referee_Command_GOAL_YELLOW, + SSLProto::Referee_Command_GOAL_BLUE, + SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, + SSLProto::Referee_Command_INDIRECT_FREE_BLUE, +}; + +/** + * @brief this maps a protobuf SSLProto::Referee_Command enum to its equivalent + * internal type + * @note this map is used when we are on the yellow team + */ +inline const static std::unordered_map + yellow_team_command_map = { + {SSLProto::Referee_Command_HALT, RefereeCommand::HALT}, + {SSLProto::Referee_Command_STOP, RefereeCommand::STOP}, + {SSLProto::Referee_Command_NORMAL_START, RefereeCommand::NORMAL_START}, + {SSLProto::Referee_Command_FORCE_START, RefereeCommand::FORCE_START}, + {SSLProto::Referee_Command_PREPARE_KICKOFF_BLUE, + RefereeCommand::PREPARE_KICKOFF_THEM}, + {SSLProto::Referee_Command_PREPARE_KICKOFF_YELLOW, + RefereeCommand::PREPARE_KICKOFF_US}, + {SSLProto::Referee_Command_PREPARE_PENALTY_BLUE, + RefereeCommand::PREPARE_PENALTY_THEM}, + {SSLProto::Referee_Command_PREPARE_PENALTY_YELLOW, + RefereeCommand::PREPARE_PENALTY_US}, + {SSLProto::Referee_Command_DIRECT_FREE_BLUE, RefereeCommand::DIRECT_FREE_THEM}, + {SSLProto::Referee_Command_DIRECT_FREE_YELLOW, RefereeCommand::DIRECT_FREE_US}, + {SSLProto::Referee_Command_INDIRECT_FREE_BLUE, + RefereeCommand::INDIRECT_FREE_THEM}, + {SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, + RefereeCommand::INDIRECT_FREE_US}, + {SSLProto::Referee_Command_TIMEOUT_BLUE, RefereeCommand::TIMEOUT_THEM}, + {SSLProto::Referee_Command_TIMEOUT_YELLOW, RefereeCommand::TIMEOUT_US}, + {SSLProto::Referee_Command_BALL_PLACEMENT_BLUE, + RefereeCommand::BALL_PLACEMENT_THEM}, + {SSLProto::Referee_Command_BALL_PLACEMENT_YELLOW, + RefereeCommand::BALL_PLACEMENT_US}}; + + +/** + * @brief this maps a protobuf SSLProto::Referee_Stage enum to its RefereeStage + * equivalent + */ +inline const static std::unordered_map + referee_stage_map = { + {SSLProto::Referee_Stage_NORMAL_FIRST_HALF_PRE, + RefereeStage::NORMAL_FIRST_HALF_PRE}, + {SSLProto::Referee_Stage_NORMAL_FIRST_HALF, RefereeStage::NORMAL_FIRST_HALF}, + {SSLProto::Referee_Stage_NORMAL_HALF_TIME, RefereeStage::NORMAL_HALF_TIME}, + {SSLProto::Referee_Stage_NORMAL_SECOND_HALF_PRE, + RefereeStage::NORMAL_SECOND_HALF_PRE}, + {SSLProto::Referee_Stage_NORMAL_SECOND_HALF, RefereeStage::NORMAL_SECOND_HALF}, + {SSLProto::Referee_Stage_EXTRA_TIME_BREAK, RefereeStage::EXTRA_TIME_BREAK}, + {SSLProto::Referee_Stage_EXTRA_FIRST_HALF_PRE, + RefereeStage::EXTRA_FIRST_HALF_PRE}, + {SSLProto::Referee_Stage_EXTRA_FIRST_HALF, RefereeStage::EXTRA_FIRST_HALF}, + {SSLProto::Referee_Stage_EXTRA_HALF_TIME, RefereeStage::EXTRA_HALF_TIME}, + {SSLProto::Referee_Stage_EXTRA_SECOND_HALF_PRE, + RefereeStage::EXTRA_SECOND_HALF_PRE}, + {SSLProto::Referee_Stage_EXTRA_SECOND_HALF, RefereeStage::EXTRA_SECOND_HALF}, + {SSLProto::Referee_Stage_PENALTY_SHOOTOUT_BREAK, + RefereeStage::PENALTY_SHOOTOUT_BREAK}, + {SSLProto::Referee_Stage_PENALTY_SHOOTOUT, RefereeStage::PENALTY_SHOOTOUT}, + {SSLProto::Referee_Stage_POST_GAME, RefereeStage::POST_GAME}}; + /** * Converts a SSLProto::Referee packet into a RefereeCommand for the GameController * command contained in the packet, based on which team we are (blue or yellow). @@ -18,8 +122,8 @@ RefereeCommand createRefereeCommand(const SSLProto::Referee &packet, TeamColour team_colour); /** - * Converts a SSLProto::Referee protobuf Stage contained in the SSLProto::Referee packet - * into a RefereeStage + * Converts a SSLProto::Referee protobuf Stage contained in the SSLProto::Referee + * packet into a RefereeStage * * @param packet SSLProto::Referee protobuf * @@ -35,3 +139,4 @@ RefereeStage createRefereeStage(const SSLProto::Referee &packet); * @return ball placement point if found */ std::optional getBallPlacementPoint(const SSLProto::Referee &packet); +} // namespace ssl_referee diff --git a/src/proto/message_translation/ssl_referee_test.cpp b/src/proto/message_translation/ssl_referee_test.cpp index 7f64a06061..b47b9dbe4c 100644 --- a/src/proto/message_translation/ssl_referee_test.cpp +++ b/src/proto/message_translation/ssl_referee_test.cpp @@ -15,7 +15,7 @@ TEST_P(RefereeStageTest, test_referee_stage) SSLProto::Referee ref; ref.set_stage(input); - ASSERT_EQ(expected, createRefereeStage(ref)); + ASSERT_EQ(expected, ssl_referee::createRefereeStage(ref)); } INSTANTIATE_TEST_CASE_P( @@ -64,7 +64,7 @@ TEST_P(RefereeCommandTest, test_referee_command) SSLProto::Referee ref; ref.set_command(input); - ASSERT_EQ(expected, createRefereeCommand(ref, team_colour)); + ASSERT_EQ(expected, ssl_referee::createRefereeCommand(ref, team_colour)); } INSTANTIATE_TEST_CASE_P( @@ -124,17 +124,6 @@ INSTANTIATE_TEST_CASE_P( std::make_tuple(RefereeCommand::DIRECT_FREE_THEM, SSLProto::Referee_Command_DIRECT_FREE_YELLOW, TeamColour::BLUE), - // indirect free - std::make_tuple(RefereeCommand::INDIRECT_FREE_US, - SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, - TeamColour::YELLOW), - std::make_tuple(RefereeCommand::INDIRECT_FREE_US, - SSLProto::Referee_Command_INDIRECT_FREE_BLUE, TeamColour::BLUE), - std::make_tuple(RefereeCommand::INDIRECT_FREE_THEM, - SSLProto::Referee_Command_INDIRECT_FREE_BLUE, TeamColour::YELLOW), - std::make_tuple(RefereeCommand::INDIRECT_FREE_THEM, - SSLProto::Referee_Command_INDIRECT_FREE_YELLOW, TeamColour::BLUE), - // timeout std::make_tuple(RefereeCommand::TIMEOUT_US, SSLProto::Referee_Command_TIMEOUT_YELLOW, TeamColour::YELLOW), @@ -169,5 +158,5 @@ TEST(BallPlacementPointTest, test_ball_placement_point) *(ref.mutable_designated_position()) = *ref_point; - ASSERT_EQ(Point(0.1, 0.2), getBallPlacementPoint(ref)); + ASSERT_EQ(Point(0.1, 0.2), ssl_referee::getBallPlacementPoint(ref)); } diff --git a/src/shared/test_util/test_util.h b/src/shared/test_util/test_util.h index dee8aa31d7..0b20956748 100644 --- a/src/shared/test_util/test_util.h +++ b/src/shared/test_util/test_util.h @@ -9,17 +9,17 @@ namespace TestUtil { - /** - * The struct represents the relevant aggregate functions used in testing - */ - struct AggregateFunctions +/** + * The struct represents the relevant aggregate functions used in testing + */ +struct AggregateFunctions +{ + double average, maximum, minimum; + AggregateFunctions() { - double average, maximum, minimum; - AggregateFunctions() - { - average = 0; - maximum = 0; - minimum = DBL_MAX; - } - }; + average = 0; + maximum = 0; + minimum = DBL_MAX; + } +}; }; // namespace TestUtil diff --git a/src/shared/uart_framing/uart_framing.hpp b/src/shared/uart_framing/uart_framing.hpp index 9a7cea8b68..259bfe1a87 100644 --- a/src/shared/uart_framing/uart_framing.hpp +++ b/src/shared/uart_framing/uart_framing.hpp @@ -19,147 +19,146 @@ const uint8_t START_END_FLAG_BYTE = 0x00; namespace { - /** - * Generates a CRC-16 adhering to the CRC-16-CCITT standard - * - * @param data the data to calculate the crc of - * @param length the length of the data - * @return CRC-16 for given data and length - */ - uint16_t crc16(const std::vector& data, uint16_t length) - { - uint8_t x; - uint16_t crc = 0xFFFF; - int i = 0; - - while (length--) - { - x = static_cast(crc >> 8u) ^ data[i++]; - x ^= static_cast(x >> 4u); - crc = static_cast((crc << 8u) ^ (x << 12u) ^ (x << 5u) ^ x); - } +/** + * Generates a CRC-16 adhering to the CRC-16-CCITT standard + * + * @param data the data to calculate the crc of + * @param length the length of the data + * @return CRC-16 for given data and length + */ +uint16_t crc16(const std::vector& data, uint16_t length) +{ + uint8_t x; + uint16_t crc = 0xFFFF; + int i = 0; - return crc; + while (length--) + { + x = static_cast(crc >> 8u) ^ data[i++]; + x ^= static_cast(x >> 4u); + crc = static_cast((crc << 8u) ^ (x << 12u) ^ (x << 5u) ^ x); } - /** - * Checks if the length and crc of the TbotsProto_PowerFrame match - * the length and crc of the message - * - * @param frame the frame to verify - * @return true if the length and crc match false otherwise - */ - bool verifyLengthAndCrc(const TbotsProto_PowerFrame& frame) + return crc; +} + +/** + * Checks if the length and crc of the TbotsProto_PowerFrame match + * the length and crc of the message + * + * @param frame the frame to verify + * @return true if the length and crc match false otherwise + */ +bool verifyLengthAndCrc(const TbotsProto_PowerFrame& frame) +{ + uint16_t expected_length; + std::vector bytes; + switch (frame.which_power_msg) { - uint16_t expected_length; - std::vector bytes; - switch (frame.which_power_msg) - { - case TbotsProto_PowerFrame_power_control_tag: - expected_length = TbotsProto_PowerPulseControl_size; - bytes = serializeToVector(frame.power_msg.power_control); - break; - case TbotsProto_PowerFrame_power_status_tag: - expected_length = TbotsProto_PowerStatus_size; - bytes = serializeToVector(frame.power_msg.power_status); - break; - default: - return false; - } - uint16_t expected_crc = crc16(bytes, static_cast(frame.length)); - return frame.crc == expected_crc && frame.length == expected_length; + case TbotsProto_PowerFrame_power_control_tag: + expected_length = TbotsProto_PowerPulseControl_size; + bytes = serializeToVector(frame.power_msg.power_control); + break; + case TbotsProto_PowerFrame_power_status_tag: + expected_length = TbotsProto_PowerStatus_size; + bytes = serializeToVector(frame.power_msg.power_status); + break; + default: + return false; } + uint16_t expected_crc = crc16(bytes, static_cast(frame.length)); + return frame.crc == expected_crc && frame.length == expected_length; +} - /** - * Implementation of modified Consistent Overhead Byte Stuffing(COBS) encoding - * https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing - * - * Uses a Delimiter byte at both the beginning(different) and end(normal) - * - * @param to_encode the vector of bytes to encode - * @return a vector of bytes encoded with COBS - */ - std::vector cobsEncoding(const std::vector& to_encode) - { - auto encoded = std::vector(); - encoded.emplace_back(START_END_FLAG_BYTE); +/** + * Implementation of modified Consistent Overhead Byte Stuffing(COBS) encoding + * https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing + * + * Uses a Delimiter byte at both the beginning(different) and end(normal) + * + * @param to_encode the vector of bytes to encode + * @return a vector of bytes encoded with COBS + */ +std::vector cobsEncoding(const std::vector& to_encode) +{ + auto encoded = std::vector(); + encoded.emplace_back(START_END_FLAG_BYTE); - size_t overhead_location = 1; - uint8_t overhead = 0x01; + size_t overhead_location = 1; + uint8_t overhead = 0x01; - for (auto byte : to_encode) + for (auto byte : to_encode) + { + if (byte == START_END_FLAG_BYTE) { - if (byte == START_END_FLAG_BYTE) + encoded.insert(encoded.begin() + overhead_location, overhead); + overhead_location = encoded.size(); + overhead = 0x01; + } + else + { + encoded.emplace_back(byte); + if (++overhead == 0xFF) { encoded.insert(encoded.begin() + overhead_location, overhead); overhead_location = encoded.size(); overhead = 0x01; } - else - { - encoded.emplace_back(byte); - if (++overhead == 0xFF) - { - encoded.insert(encoded.begin() + overhead_location, overhead); - overhead_location = encoded.size(); - overhead = 0x01; - } - } } - encoded.insert(encoded.begin() + overhead_location, overhead); - encoded.emplace_back(START_END_FLAG_BYTE); + } + encoded.insert(encoded.begin() + overhead_location, overhead); + encoded.emplace_back(START_END_FLAG_BYTE); + + return encoded; +} - return encoded; +/** + * Implementation of modified Consistent Overhead Byte Stuffing(COBS) decoding + * https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing + * + * Uses a Delimiter byte at both the beginning and end instead of a single + * delimiter byte at the end as in the wikipedia example. + * + * @param to_decode the vector of bytes to decode + * @param decoded the vector that bytes are decoded to + * @return whether the decode was successful + */ +bool cobsDecoding(const std::vector& to_decode, std::vector& decoded) +{ + if (to_decode.front() != START_END_FLAG_BYTE || + to_decode.back() != START_END_FLAG_BYTE) + { + return false; } - /** - * Implementation of modified Consistent Overhead Byte Stuffing(COBS) decoding - * https://en.wikipedia.org/wiki/Consistent_Overhead_Byte_Stuffing - * - * Uses a Delimiter byte at both the beginning and end instead of a single - * delimiter byte at the end as in the wikipedia example. - * - * @param to_decode the vector of bytes to decode - * @param decoded the vector that bytes are decoded to - * @return whether the decode was successful - */ - bool cobsDecoding(const std::vector& to_decode, - std::vector& decoded) + uint16_t i = 1; + while (i < to_decode.size()) { - if (to_decode.front() != START_END_FLAG_BYTE || - to_decode.back() != START_END_FLAG_BYTE) + uint8_t overhead = to_decode[i++]; + // Check that overhead does not point to past the end of the vector + if (overhead + i > to_decode.size()) { return false; } - - uint16_t i = 1; - while (i < to_decode.size()) + // Check that instances of the START_END_FLAG_BYTE are not in the middle of + // the vector + if (overhead == START_END_FLAG_BYTE && i != to_decode.size()) { - uint8_t overhead = to_decode[i++]; - // Check that overhead does not point to past the end of the vector - if (overhead + i > to_decode.size()) - { - return false; - } - // Check that instances of the START_END_FLAG_BYTE are not in the middle of - // the vector - if (overhead == START_END_FLAG_BYTE && i != to_decode.size()) - { - return false; - } - for (uint16_t j = 1; i < to_decode.size() && j < overhead; j++) - { - decoded.emplace_back(to_decode[i++]); - } - if (overhead < 0xFF && i != to_decode.size() - 1 && - overhead != START_END_FLAG_BYTE) - { - decoded.emplace_back(START_END_FLAG_BYTE); - } + return false; + } + for (uint16_t j = 1; i < to_decode.size() && j < overhead; j++) + { + decoded.emplace_back(to_decode[i++]); + } + if (overhead < 0xFF && i != to_decode.size() - 1 && + overhead != START_END_FLAG_BYTE) + { + decoded.emplace_back(START_END_FLAG_BYTE); } - - return true; } + + return true; +} } // anonymous namespace /** diff --git a/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp b/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp index 2b4b2444f5..ad111c3c2a 100644 --- a/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp +++ b/src/software/ai/motion_constraint/motion_constraint_set_builder_test.cpp @@ -11,119 +11,117 @@ // This namespace contains all the test parameters namespace { - std::shared_ptr world = ::TestUtil::createBlankTestingWorld(); - Pass pass({1, 1}, {0.5, 0}, 2.29); - TbotsProto::AiConfig ai_config; - - // vector of tuples of Tactic, MotionConstraints that should be removed, - // MotionConstraints that should be added - std::vector< - std::tuple, std::set, - std::set>> - test_vector = { - std::make_tuple(std::make_shared(), - std::set(), - std::set()), - std::make_tuple(std::make_shared(ai_config), - std::set( - {TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL}), - std::set()), - std::make_tuple(std::make_shared(ai_config), - std::set( - {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::FRIENDLY_HALF}), - std::set()), - std::make_tuple(std::make_shared(), - std::set(), - std::set()), - std::make_tuple( - std::make_shared(), - std::set( - {TbotsProto::MotionConstraint::CENTER_CIRCLE, - TbotsProto::MotionConstraint::ENEMY_HALF, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL}), - std::set( - {TbotsProto::MotionConstraint::ENEMY_HALF_WITHOUT_CENTRE_CIRCLE})), - std::make_tuple(std::make_shared(ai_config), - std::set( - {TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA, - TbotsProto::MotionConstraint::ENEMY_HALF}), - std::set()), - std::make_tuple(std::make_shared(), - std::set( - {TbotsProto::MotionConstraint::ENEMY_HALF, - TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA, - TbotsProto::MotionConstraint::FRIENDLY_HALF, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL}), - std::set()), - std::make_tuple( - std::make_shared(ai_config.receiver_tactic_config()), - std::set(), - std::set()), - std::make_tuple(std::make_shared(), - std::set(), - std::set()), - std::make_tuple(std::make_shared(ai_config), - std::set(), - std::set()), - std::make_tuple(std::make_shared(), - std::set(), - std::set()), - std::make_tuple( - std::make_shared(), - std::set( - {TbotsProto::MotionConstraint::CENTER_CIRCLE, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::ENEMY_HALF}), - std::set( - {TbotsProto::MotionConstraint::ENEMY_HALF_WITHOUT_CENTRE_CIRCLE})), - std::make_tuple(std::make_shared(ai_config), - std::set(), - std::set())}; - - // sets of motion constraints for each type of game state - auto stoppage_or_them_motion_constraints = std::set( - {TbotsProto::MotionConstraint::INFLATED_ENEMY_DEFENSE_AREA, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::FRIENDLY_GOAL, - TbotsProto::MotionConstraint::ENEMY_GOAL, - TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA}); - - auto gamestart_or_us_motion_constraints = std::set( - {TbotsProto::MotionConstraint::INFLATED_ENEMY_DEFENSE_AREA, - TbotsProto::MotionConstraint::FRIENDLY_GOAL, - TbotsProto::MotionConstraint::ENEMY_GOAL, - TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA}); - - auto kickoff_motion_constraints = std::set( - {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, - TbotsProto::MotionConstraint::FRIENDLY_GOAL, - TbotsProto::MotionConstraint::ENEMY_GOAL, - TbotsProto::MotionConstraint::CENTER_CIRCLE, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::ENEMY_HALF}); - - auto our_penalty_motion_constraints = std::set( - {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, - TbotsProto::MotionConstraint::FRIENDLY_GOAL, - TbotsProto::MotionConstraint::ENEMY_GOAL, - TbotsProto::MotionConstraint::ENEMY_HALF}); - - auto them_penalty_motion_constraints = std::set( - {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, - TbotsProto::MotionConstraint::FRIENDLY_GOAL, - TbotsProto::MotionConstraint::ENEMY_GOAL, - TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::FRIENDLY_HALF}); - - auto them_ball_placement = std::set( - {TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, - TbotsProto::MotionConstraint::FRIENDLY_GOAL, - TbotsProto::MotionConstraint::ENEMY_GOAL, - TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, - TbotsProto::MotionConstraint::AVOID_BALL_PLACEMENT_INTERFERENCE}); +std::shared_ptr world = ::TestUtil::createBlankTestingWorld(); +Pass pass({1, 1}, {0.5, 0}, 2.29); +TbotsProto::AiConfig ai_config; + +// vector of tuples of Tactic, MotionConstraints that should be removed, +// MotionConstraints that should be added +std::vector, std::set, + std::set>> + test_vector = { + std::make_tuple(std::make_shared(), + std::set(), + std::set()), + std::make_tuple(std::make_shared(ai_config), + std::set( + {TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL}), + std::set()), + std::make_tuple(std::make_shared(ai_config), + std::set( + {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::FRIENDLY_HALF}), + std::set()), + std::make_tuple(std::make_shared(), + std::set(), + std::set()), + std::make_tuple( + std::make_shared(), + std::set( + {TbotsProto::MotionConstraint::CENTER_CIRCLE, + TbotsProto::MotionConstraint::ENEMY_HALF, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL}), + std::set( + {TbotsProto::MotionConstraint::ENEMY_HALF_WITHOUT_CENTRE_CIRCLE})), + std::make_tuple(std::make_shared(ai_config), + std::set( + {TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA, + TbotsProto::MotionConstraint::ENEMY_HALF}), + std::set()), + std::make_tuple(std::make_shared(), + std::set( + {TbotsProto::MotionConstraint::ENEMY_HALF, + TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA, + TbotsProto::MotionConstraint::FRIENDLY_HALF, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL}), + std::set()), + std::make_tuple( + std::make_shared(ai_config.receiver_tactic_config()), + std::set(), + std::set()), + std::make_tuple(std::make_shared(), + std::set(), + std::set()), + std::make_tuple(std::make_shared(ai_config), + std::set(), + std::set()), + std::make_tuple(std::make_shared(), + std::set(), + std::set()), + std::make_tuple( + std::make_shared(), + std::set( + {TbotsProto::MotionConstraint::CENTER_CIRCLE, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::ENEMY_HALF}), + std::set( + {TbotsProto::MotionConstraint::ENEMY_HALF_WITHOUT_CENTRE_CIRCLE})), + std::make_tuple(std::make_shared(ai_config), + std::set(), + std::set())}; + +// sets of motion constraints for each type of game state +auto stoppage_or_them_motion_constraints = std::set( + {TbotsProto::MotionConstraint::INFLATED_ENEMY_DEFENSE_AREA, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::FRIENDLY_GOAL, + TbotsProto::MotionConstraint::ENEMY_GOAL, + TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA}); + +auto gamestart_or_us_motion_constraints = std::set( + {TbotsProto::MotionConstraint::INFLATED_ENEMY_DEFENSE_AREA, + TbotsProto::MotionConstraint::FRIENDLY_GOAL, + TbotsProto::MotionConstraint::ENEMY_GOAL, + TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA}); + +auto kickoff_motion_constraints = std::set( + {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, + TbotsProto::MotionConstraint::FRIENDLY_GOAL, + TbotsProto::MotionConstraint::ENEMY_GOAL, + TbotsProto::MotionConstraint::CENTER_CIRCLE, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::ENEMY_HALF}); + +auto our_penalty_motion_constraints = std::set( + {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, + TbotsProto::MotionConstraint::FRIENDLY_GOAL, + TbotsProto::MotionConstraint::ENEMY_GOAL, TbotsProto::MotionConstraint::ENEMY_HALF}); + +auto them_penalty_motion_constraints = std::set( + {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, + TbotsProto::MotionConstraint::FRIENDLY_GOAL, + TbotsProto::MotionConstraint::ENEMY_GOAL, + TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::FRIENDLY_HALF}); + +auto them_ball_placement = std::set( + {TbotsProto::MotionConstraint::HALF_METER_AROUND_BALL, + TbotsProto::MotionConstraint::FRIENDLY_GOAL, + TbotsProto::MotionConstraint::ENEMY_GOAL, + TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA, + TbotsProto::MotionConstraint::AVOID_BALL_PLACEMENT_INTERFERENCE}); } // namespace class CheckMotionConstraints diff --git a/src/software/field_tests/field_test_fixture.py b/src/software/field_tests/field_test_fixture.py index 82445f8938..8d9c3a732f 100644 --- a/src/software/field_tests/field_test_fixture.py +++ b/src/software/field_tests/field_test_fixture.py @@ -383,8 +383,9 @@ def field_test_runner(): rc_friendly.setup_for_fullsystem() gamecontroller.setup_proto_unix_io( - blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + simulator_proto_unix_io=simulator_proto_unix_io, ) # Inject the proto unix ios into thunderscope and start the test tscope = Thunderscope( diff --git a/src/software/geom/algorithms/rasterize_test.cpp b/src/software/geom/algorithms/rasterize_test.cpp index a81d41a69d..a319685a41 100644 --- a/src/software/geom/algorithms/rasterize_test.cpp +++ b/src/software/geom/algorithms/rasterize_test.cpp @@ -9,75 +9,72 @@ namespace TestUtil { - void checkPointsCloseToEachOther(std::vector all_points, double max_dist) +void checkPointsCloseToEachOther(std::vector all_points, double max_dist) +{ + for (Point& p : all_points) { - for (Point& p : all_points) - { - auto compare_based_on_distance_to_p = [p](const Point& a, - const Point& b) -> bool - { return distance(a, p) < distance(b, p); }; - - std::vector all_points_copy = all_points; - all_points_copy.erase( - std::remove(all_points_copy.begin(), all_points_copy.end(), p), - all_points_copy.end()); - Point closest_p = - *std::min_element(all_points_copy.begin(), all_points_copy.end(), - compare_based_on_distance_to_p); - EXPECT_LE(distance(p, closest_p), max_dist); - } + auto compare_based_on_distance_to_p = [p](const Point& a, const Point& b) -> bool + { return distance(a, p) < distance(b, p); }; + + std::vector all_points_copy = all_points; + all_points_copy.erase( + std::remove(all_points_copy.begin(), all_points_copy.end(), p), + all_points_copy.end()); + Point closest_p = + *std::min_element(all_points_copy.begin(), all_points_copy.end(), + compare_based_on_distance_to_p); + EXPECT_LE(distance(p, closest_p), max_dist); } +} - void checkPolygonPointsInsideBoundingBox(Polygon polygon, - std::vector all_points) +void checkPolygonPointsInsideBoundingBox(Polygon polygon, std::vector all_points) +{ + const std::vector& polygon_vertices = polygon.getPoints(); + auto max_point_y = [](const Point& a, const Point& b) { return a.y() < b.y(); }; + + auto max_point_x = [](const Point& a, const Point& b) { return a.x() < b.x(); }; + + // Calculate the highest and lowest x and y points + double min_y = + std::min_element(polygon_vertices.begin(), polygon_vertices.end(), max_point_y) + ->y(); + double min_x = + std::min_element(polygon_vertices.begin(), polygon_vertices.end(), max_point_x) + ->x(); + double max_y = + std::max_element(polygon_vertices.begin(), polygon_vertices.end(), max_point_y) + ->y(); + double max_x = + std::max_element(polygon_vertices.begin(), polygon_vertices.end(), max_point_x) + ->x(); + + Rectangle bounding_box = Rectangle(Point(min_x, min_y), Point(max_x, max_y)); + for (Point p : all_points) { - const std::vector& polygon_vertices = polygon.getPoints(); - auto max_point_y = [](const Point& a, const Point& b) { return a.y() < b.y(); }; - - auto max_point_x = [](const Point& a, const Point& b) { return a.x() < b.x(); }; - - // Calculate the highest and lowest x and y points - double min_y = std::min_element(polygon_vertices.begin(), polygon_vertices.end(), - max_point_y) - ->y(); - double min_x = std::min_element(polygon_vertices.begin(), polygon_vertices.end(), - max_point_x) - ->x(); - double max_y = std::max_element(polygon_vertices.begin(), polygon_vertices.end(), - max_point_y) - ->y(); - double max_x = std::max_element(polygon_vertices.begin(), polygon_vertices.end(), - max_point_x) - ->x(); - - Rectangle bounding_box = Rectangle(Point(min_x, min_y), Point(max_x, max_y)); - for (Point p : all_points) - { - EXPECT_TRUE(contains(bounding_box, p)); - } + EXPECT_TRUE(contains(bounding_box, p)); } +} - void checkStadiumPointsInsideBoundingBox(Stadium stadium, - std::vector all_points) +void checkStadiumPointsInsideBoundingBox(Stadium stadium, std::vector all_points) +{ + auto min_x = + std::min(stadium.segment().getStart().x(), stadium.segment().getEnd().x()); + auto max_x = + std::max(stadium.segment().getStart().x(), stadium.segment().getEnd().x()); + auto min_y = + std::min(stadium.segment().getStart().y(), stadium.segment().getEnd().y()); + auto max_y = + std::max(stadium.segment().getStart().y(), stadium.segment().getEnd().y()); + + Rectangle bounding_box = + Rectangle(Point(min_x - stadium.radius(), min_y - stadium.radius()), + Point(max_x + stadium.radius(), max_y + stadium.radius())); + + for (Point p : all_points) { - auto min_x = - std::min(stadium.segment().getStart().x(), stadium.segment().getEnd().x()); - auto max_x = - std::max(stadium.segment().getStart().x(), stadium.segment().getEnd().x()); - auto min_y = - std::min(stadium.segment().getStart().y(), stadium.segment().getEnd().y()); - auto max_y = - std::max(stadium.segment().getStart().y(), stadium.segment().getEnd().y()); - - Rectangle bounding_box = - Rectangle(Point(min_x - stadium.radius(), min_y - stadium.radius()), - Point(max_x + stadium.radius(), max_y + stadium.radius())); - - for (Point p : all_points) - { - EXPECT_TRUE(contains(bounding_box, p)); - } + EXPECT_TRUE(contains(bounding_box, p)); } +} }; // namespace TestUtil ////////////////////////////////////////////////////// diff --git a/src/software/geom/algorithms/voronoi_diagram.h b/src/software/geom/algorithms/voronoi_diagram.h index 3ed6e49853..00409f7915 100644 --- a/src/software/geom/algorithms/voronoi_diagram.h +++ b/src/software/geom/algorithms/voronoi_diagram.h @@ -12,10 +12,10 @@ // compilation if included in too many places namespace boost::polygon { - template - class voronoi_diagram_traits; - template - class voronoi_diagram; +template +class voronoi_diagram_traits; +template +class voronoi_diagram; } // namespace boost::polygon class VoronoiDiagram diff --git a/src/software/geom/point.h b/src/software/geom/point.h index 05fdc6e6cf..c20a5786d5 100644 --- a/src/software/geom/point.h +++ b/src/software/geom/point.h @@ -228,13 +228,13 @@ bool operator!=(const Point &p, const Point &q); // https://prateekvjoshi.com/2014/06/05/using-hash-function-in-c-for-user-defined-classes/ namespace std { - template <> - struct hash final +template <> +struct hash final +{ + size_t operator()(const Point &p) const { - size_t operator()(const Point &p) const - { - hash h; - return h(p.x()) * 17 + h(p.y()); - } - }; + hash h; + return h(p.x()) * 17 + h(p.y()); + } +}; } // namespace std diff --git a/src/software/geom/vector.h b/src/software/geom/vector.h index 24eecbabda..c07eedbbcc 100644 --- a/src/software/geom/vector.h +++ b/src/software/geom/vector.h @@ -357,13 +357,13 @@ bool operator!=(const Vector &u, const Vector &v); // https://prateekvjoshi.com/2014/06/05/using-hash-function-in-c-for-user-defined-classes/ namespace std { - template <> - struct hash final +template <> +struct hash final +{ + size_t operator()(const Vector &v) const { - size_t operator()(const Vector &v) const - { - hash h; - return h(v.x()) * 17 + h(v.y()); - } - }; + hash h; + return h(v.x()) * 17 + h(v.y()); + } +}; } // namespace std diff --git a/src/software/multithreading/observer_test.cpp b/src/software/multithreading/observer_test.cpp index 0e3a16fcfd..52001d05d9 100644 --- a/src/software/multithreading/observer_test.cpp +++ b/src/software/multithreading/observer_test.cpp @@ -17,49 +17,49 @@ class TestObserver : public Observer namespace TestUtil { - /** - * Tests getDataReceivedPerSecond by filling the buffer - * - * @param test_observer The observer to test - * @param data_received_period_ms The period between receiving data in milliseconds - * @param number_of_messages number of messages to send to the buffer - * - * @return AssertionSuccess the observer returns the correct data received per second - */ - ::testing::AssertionResult testGetDataReceivedPerSecondByFillingBuffer( - TestObserver test_observer, unsigned int data_received_period_ms, - unsigned int number_of_messages) +/** + * Tests getDataReceivedPerSecond by filling the buffer + * + * @param test_observer The observer to test + * @param data_received_period_ms The period between receiving data in milliseconds + * @param number_of_messages number of messages to send to the buffer + * + * @return AssertionSuccess the observer returns the correct data received per second + */ +::testing::AssertionResult testGetDataReceivedPerSecondByFillingBuffer( + TestObserver test_observer, unsigned int data_received_period_ms, + unsigned int number_of_messages) +{ + auto wall_time_start = FakeClock::now(); + for (unsigned int i = 0; i < number_of_messages; i++) { - auto wall_time_start = FakeClock::now(); - for (unsigned int i = 0; i < number_of_messages; i++) - { - test_observer.receiveValue(i); - FakeClock::advance(std::chrono::milliseconds(data_received_period_ms)); - } - - auto wall_time_now = FakeClock::now(); - double test_duration_s = - static_cast(std::chrono::duration_cast( - wall_time_now - wall_time_start) - .count()) * - SECONDS_PER_MILLISECOND; - double scaling_factor = - test_duration_s / - (data_received_period_ms * SECONDS_PER_MILLISECOND * number_of_messages); - double expected_actual_difference = std::abs( - test_observer.getDataReceivedPerSecond() - - 1 / (data_received_period_ms * SECONDS_PER_MILLISECOND) * scaling_factor); - if (expected_actual_difference < 50) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "The difference between expected and actual data received per seconds was " - << expected_actual_difference; - } + test_observer.receiveValue(i); + FakeClock::advance(std::chrono::milliseconds(data_received_period_ms)); + } + + auto wall_time_now = FakeClock::now(); + double test_duration_s = + static_cast(std::chrono::duration_cast( + wall_time_now - wall_time_start) + .count()) * + SECONDS_PER_MILLISECOND; + double scaling_factor = + test_duration_s / + (data_received_period_ms * SECONDS_PER_MILLISECOND * number_of_messages); + double expected_actual_difference = std::abs( + test_observer.getDataReceivedPerSecond() - + 1 / (data_received_period_ms * SECONDS_PER_MILLISECOND) * scaling_factor); + if (expected_actual_difference < 50) + { + return ::testing::AssertionSuccess(); } + else + { + return ::testing::AssertionFailure() + << "The difference between expected and actual data received per seconds was " + << expected_actual_difference; + } +} }; // namespace TestUtil TEST(Observer, receiveValue_value_already_available) diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index dced5047ee..a31342c382 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -124,7 +124,9 @@ void SensorFusion::updateWorld(const SSLProto::Referee &packet) { if (sensor_fusion_config.friendly_color_yellow()) { - game_state.updateRefereeCommand(createRefereeCommand(packet, TeamColour::YELLOW)); + if (!ssl_referee::deprecated_commands.contains(packet.command())) + game_state.updateRefereeCommand( + ssl_referee::createRefereeCommand(packet, TeamColour::YELLOW)); friendly_goalie_id = packet.yellow().goalkeeper(); enemy_goalie_id = packet.blue().goalkeeper(); if (packet.has_blue_team_on_positive_half()) @@ -134,7 +136,9 @@ void SensorFusion::updateWorld(const SSLProto::Referee &packet) } else { - game_state.updateRefereeCommand(createRefereeCommand(packet, TeamColour::BLUE)); + if (!ssl_referee::deprecated_commands.contains(packet.command())) + game_state.updateRefereeCommand( + ssl_referee::createRefereeCommand(packet, TeamColour::BLUE)); friendly_goalie_id = packet.blue().goalkeeper(); enemy_goalie_id = packet.yellow().goalkeeper(); if (packet.has_blue_team_on_positive_half()) @@ -159,7 +163,7 @@ void SensorFusion::updateWorld(const SSLProto::Referee &packet) } } - referee_stage = createRefereeStage(packet); + referee_stage = ssl_referee::createRefereeStage(packet); } void SensorFusion::updateWorld( @@ -433,7 +437,7 @@ Team SensorFusion::createEnemyTeam(const std::vector &robot_dete std::optional SensorFusion::getBallPlacementPoint(const SSLProto::Referee &packet) { - std::optional point_opt = ::getBallPlacementPoint(packet); + std::optional point_opt = ssl_referee::getBallPlacementPoint(packet); if (!point_opt) { diff --git a/src/software/sensor_fusion/sensor_fusion_test.cpp b/src/software/sensor_fusion/sensor_fusion_test.cpp index 3cb5dffdfb..aa70021016 100644 --- a/src/software/sensor_fusion/sensor_fusion_test.cpp +++ b/src/software/sensor_fusion/sensor_fusion_test.cpp @@ -25,8 +25,8 @@ class SensorFusionTest : public ::testing::Test robot_status_msg_dribble_motor_hot(initDribbleMotorHotErrorCode()), robot_status_msg_multiple_error_codes(initMultipleErrorCode()), robot_status_msg_no_error_code(initNoErrorCode()), - referee_indirect_yellow(initRefereeIndirectYellow()), - referee_indirect_blue(initRefereeIndirectBlue()), + referee_direct_yellow(initRefereeDirectYellow()), + referee_direct_blue(initRefereeDirectBlue()), referee_normal_start(initRefereeNormalStart()), referee_ball_placement_yellow(initRefereeBallPlacementYellow()), referee_ball_placement_blue(initRefereeBallPlacementBlue()), @@ -49,8 +49,8 @@ class SensorFusionTest : public ::testing::Test std::unique_ptr robot_status_msg_dribble_motor_hot; std::unique_ptr robot_status_msg_multiple_error_codes; std::unique_ptr robot_status_msg_no_error_code; - std::unique_ptr referee_indirect_yellow; - std::unique_ptr referee_indirect_blue; + std::unique_ptr referee_direct_yellow; + std::unique_ptr referee_direct_blue; std::unique_ptr referee_normal_start; std::unique_ptr referee_ball_placement_yellow; std::unique_ptr referee_ball_placement_blue; @@ -282,17 +282,17 @@ class SensorFusionTest : public ::testing::Test return robot_msg; } - std::unique_ptr initRefereeIndirectYellow() + std::unique_ptr initRefereeDirectYellow() { auto ref_msg = std::make_unique(); - ref_msg->set_command(SSLProto::Referee_Command_INDIRECT_FREE_YELLOW); + ref_msg->set_command(SSLProto::Referee_Command_DIRECT_FREE_YELLOW); return ref_msg; } - std::unique_ptr initRefereeIndirectBlue() + std::unique_ptr initRefereeDirectBlue() { auto ref_msg = std::make_unique(); - ref_msg->set_command(SSLProto::Referee_Command_INDIRECT_FREE_BLUE); + ref_msg->set_command(SSLProto::Referee_Command_DIRECT_FREE_BLUE); return ref_msg; } @@ -545,7 +545,7 @@ TEST_F(SensorFusionTest, test_complete_wrapper_with_robot_status_msg_2_at_a_time TEST_F(SensorFusionTest, test_referee_yellow_then_normal) { GameState expected_1; - expected_1.updateRefereeCommand(RefereeCommand::INDIRECT_FREE_US); + expected_1.updateRefereeCommand(RefereeCommand::DIRECT_FREE_US); GameState expected_2 = expected_1; expected_2.updateRefereeCommand(RefereeCommand::NORMAL_START); @@ -555,7 +555,7 @@ TEST_F(SensorFusionTest, test_referee_yellow_then_normal) createSSLWrapperPacket(std::move(geom_data), initDetectionFrame()); // set vision msg so that world is valid *(sensor_msg_1.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; - *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_indirect_yellow; + *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_direct_yellow; sensor_fusion.processSensorProto(sensor_msg_1); World result_1 = *sensor_fusion.getWorld(); EXPECT_EQ(expected_1, result_1.gameState()); @@ -570,7 +570,7 @@ TEST_F(SensorFusionTest, test_referee_yellow_then_normal) TEST_F(SensorFusionTest, test_referee_blue_then_normal) { GameState expected_1; - expected_1.updateRefereeCommand(RefereeCommand::INDIRECT_FREE_THEM); + expected_1.updateRefereeCommand(RefereeCommand::DIRECT_FREE_THEM); GameState expected_2 = expected_1; expected_2.updateRefereeCommand(RefereeCommand::NORMAL_START); @@ -580,7 +580,7 @@ TEST_F(SensorFusionTest, test_referee_blue_then_normal) createSSLWrapperPacket(std::move(geom_data), initDetectionFrame()); // set vision msg so that world is valid *(sensor_msg_1.mutable_ssl_vision_msg()) = *ssl_wrapper_packet; - *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_indirect_blue; + *(sensor_msg_1.mutable_ssl_referee_msg()) = *referee_direct_blue; sensor_fusion.processSensorProto(sensor_msg_1); World result_1 = *sensor_fusion.getWorld(); EXPECT_EQ(expected_1, result_1.gameState()); diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 2ff467bc13..9f37b33f32 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -530,8 +530,9 @@ def simulated_test_runner(): ProtoUnixIO(), ) gamecontroller.setup_proto_unix_io( - blue_full_system_proto_unix_io, - yellow_full_system_proto_unix_io, + blue_full_system_proto_unix_io=blue_full_system_proto_unix_io, + yellow_full_system_proto_unix_io=yellow_full_system_proto_unix_io, + simulator_proto_unix_io=simulator_proto_unix_io, ) # If we want to run thunderscope, inject the proto unix ios diff --git a/src/software/test_util/equal_within_tolerance.cpp b/src/software/test_util/equal_within_tolerance.cpp index 368c854fbf..b514b84963 100644 --- a/src/software/test_util/equal_within_tolerance.cpp +++ b/src/software/test_util/equal_within_tolerance.cpp @@ -4,271 +4,265 @@ namespace TestUtil { - ::testing::AssertionResult equalWithinTolerance(const Duration &duration1, - const Duration &duration2, - const Duration &tolerance) +::testing::AssertionResult equalWithinTolerance(const Duration &duration1, + const Duration &duration2, + const Duration &tolerance) +{ + if (std::abs((duration1 - duration2).toMilliseconds()) < + std::abs(tolerance.toMilliseconds())) { - if (std::abs((duration1 - duration2).toMilliseconds()) < - std::abs(tolerance.toMilliseconds())) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Duration 1 was " << duration1 << ", duration 2 was " << duration2; - } + return ::testing::AssertionSuccess(); } + else + { + return ::testing::AssertionFailure() + << "Duration 1 was " << duration1 << ", duration 2 was " << duration2; + } +} - ::testing::AssertionResult equalWithinTolerance(const Polygon &poly1, - const Polygon &poly2, - double tolerance) +::testing::AssertionResult equalWithinTolerance(const Polygon &poly1, + const Polygon &poly2, double tolerance) +{ + auto ppts1 = poly1.getPoints(); + auto ppts2 = poly2.getPoints(); + if (std::equal(ppts1.begin(), ppts1.end(), ppts2.begin(), + [tolerance](const Point &p1, const Point &p2) + { return equalWithinTolerance(p1, p2, tolerance); })) { - auto ppts1 = poly1.getPoints(); - auto ppts2 = poly2.getPoints(); - if (std::equal(ppts1.begin(), ppts1.end(), ppts2.begin(), - [tolerance](const Point &p1, const Point &p2) - { return equalWithinTolerance(p1, p2, tolerance); })) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Polygon 1 was " << poly1 << ", polygon 2 was " << poly2; - } + return ::testing::AssertionSuccess(); + } + else + { + return ::testing::AssertionFailure() + << "Polygon 1 was " << poly1 << ", polygon 2 was " << poly2; } +} - ::testing::AssertionResult equalWithinTolerance(const Stadium &stadium1, - const Stadium &stadium2, - double tolerance) +::testing::AssertionResult equalWithinTolerance(const Stadium &stadium1, + const Stadium &stadium2, double tolerance) +{ + if (((equalWithinTolerance(stadium1.segment().getStart(), + stadium2.segment().getStart(), tolerance) && + equalWithinTolerance(stadium1.segment().getEnd(), stadium2.segment().getEnd(), + tolerance)) || + (equalWithinTolerance(stadium1.segment().getStart(), stadium2.segment().getEnd(), + tolerance) && + equalWithinTolerance(stadium1.segment().getEnd(), stadium2.segment().getStart(), + tolerance))) && + equalWithinTolerance(stadium1.radius(), stadium2.radius(), tolerance)) { - if (((equalWithinTolerance(stadium1.segment().getStart(), - stadium2.segment().getStart(), tolerance) && - equalWithinTolerance(stadium1.segment().getEnd(), - stadium2.segment().getEnd(), tolerance)) || - (equalWithinTolerance(stadium1.segment().getStart(), - stadium2.segment().getEnd(), tolerance) && - equalWithinTolerance(stadium1.segment().getEnd(), - stadium2.segment().getStart(), tolerance))) && - equalWithinTolerance(stadium1.radius(), stadium2.radius(), tolerance)) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Stadium 1 was " << stadium1 << ", stadium 2 was " << stadium2; - } + return ::testing::AssertionSuccess(); + } + else + { + return ::testing::AssertionFailure() + << "Stadium 1 was " << stadium1 << ", stadium 2 was " << stadium2; } +} - ::testing::AssertionResult equalWithinTolerance(const Circle &c1, const Circle &c2, - double tolerance) +::testing::AssertionResult equalWithinTolerance(const Circle &c1, const Circle &c2, + double tolerance) +{ + if (equalWithinTolerance(c1.origin(), c2.origin(), tolerance) && + equalWithinTolerance(c1.radius(), c2.radius(), tolerance)) { - if (equalWithinTolerance(c1.origin(), c2.origin(), tolerance) && - equalWithinTolerance(c1.radius(), c2.radius(), tolerance)) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Circle 1 was " << c1 << ", circle 2 was " << c2; - } + return ::testing::AssertionSuccess(); } + else + { + return ::testing::AssertionFailure() + << "Circle 1 was " << c1 << ", circle 2 was " << c2; + } +} - ::testing::AssertionResult equalWithinTolerance(const Angle &a1, const Angle &a2, - const Angle &tolerance) +::testing::AssertionResult equalWithinTolerance(const Angle &a1, const Angle &a2, + const Angle &tolerance) +{ + // subtract a fixed epsilon for error in: + // - angle subtraction (internal to minDiff) + // - angle clamping (internal to minDiff) + // - angle absolute value (internal to minDiff) + // - the tolerance abs() + auto difference = a1.minDiff(a2) - Angle::fromRadians(FIXED_EPSILON * 4); + if (difference < tolerance.abs()) { - // subtract a fixed epsilon for error in: - // - angle subtraction (internal to minDiff) - // - angle clamping (internal to minDiff) - // - angle absolute value (internal to minDiff) - // - the tolerance abs() - auto difference = a1.minDiff(a2) - Angle::fromRadians(FIXED_EPSILON * 4); - if (difference < tolerance.abs()) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Angle 1 was " << a1 << ", angle 2 was " << a2; - } + return ::testing::AssertionSuccess(); + } + else + { + return ::testing::AssertionFailure() + << "Angle 1 was " << a1 << ", angle 2 was " << a2; } +} - ::testing::AssertionResult equalWithinTolerance(const Vector &v1, const Vector &v2, - double tolerance) +::testing::AssertionResult equalWithinTolerance(const Vector &v1, const Vector &v2, + double tolerance) +{ + double distance = (v1 - v2).length(); + if (equalWithinTolerance(distance, 0, tolerance)) { - double distance = (v1 - v2).length(); - if (equalWithinTolerance(distance, 0, tolerance)) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Vector 1 was " << v1 << ", vector 2 was " << v2; - } + return ::testing::AssertionSuccess(); } - - ::testing::AssertionResult equalWithinTolerance(const Point &pt1, const Point &pt2, - double tolerance) + else { - double dist = distance(pt1, pt2); - if (equalWithinTolerance(dist, 0, tolerance)) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Point 1 was " << pt1 << ", point 2 was " << pt2; - } + return ::testing::AssertionFailure() + << "Vector 1 was " << v1 << ", vector 2 was " << v2; } +} - ::testing::AssertionResult equalWithinTolerance(double val1, double val2, - double tolerance) +::testing::AssertionResult equalWithinTolerance(const Point &pt1, const Point &pt2, + double tolerance) +{ + double dist = distance(pt1, pt2); + if (equalWithinTolerance(dist, 0, tolerance)) { - // subtracting one fixed epsilon to account for the error in fabs and one fixed - // epsilon to account for the error in subtracting the two vals - double difference = fabs(val1 - val2) - FIXED_EPSILON * 2; - if (difference < tolerance) - { - return ::testing::AssertionSuccess(); - } - else - { - return ::testing::AssertionFailure() - << "Value 1 was " << val1 << ", value 2 was " << val2; - } + return ::testing::AssertionSuccess(); } - - ::testing::AssertionResult equalWithinTolerance(const RobotState &state1, - const RobotState &state2, - const double linear_tolerance, - const Angle &angular_tolerance) + else { - auto position_equality_result = - equalWithinTolerance(state1.position(), state2.position(), linear_tolerance); - auto velocity_equality_result = - equalWithinTolerance(state1.velocity(), state2.velocity(), linear_tolerance); - auto orientation_equality_result = equalWithinTolerance( - state1.orientation(), state2.orientation(), angular_tolerance); - auto angular_velocity_equality_result = equalWithinTolerance( - state1.angularVelocity(), state2.angularVelocity(), angular_tolerance); - - auto assertion_result = ::testing::AssertionSuccess(); + return ::testing::AssertionFailure() + << "Point 1 was " << pt1 << ", point 2 was " << pt2; + } +} - if (!position_equality_result || !velocity_equality_result || - !orientation_equality_result || !angular_velocity_equality_result) - { - assertion_result = ::testing::AssertionFailure(); +::testing::AssertionResult equalWithinTolerance(double val1, double val2, + double tolerance) +{ + // subtracting one fixed epsilon to account for the error in fabs and one fixed + // epsilon to account for the error in subtracting the two vals + double difference = fabs(val1 - val2) - FIXED_EPSILON * 2; + if (difference < tolerance) + { + return ::testing::AssertionSuccess(); + } + else + { + return ::testing::AssertionFailure() + << "Value 1 was " << val1 << ", value 2 was " << val2; + } +} - if (!position_equality_result) - { - assertion_result << "The first state's position was " << state1.position() - << ", the second state's position was " - << state2.position(); - } - if (!velocity_equality_result) - { - assertion_result << std::endl - << "The first state's velocity was " << state1.velocity() - << ", the second state's velocity was " - << state2.velocity(); - } - if (!orientation_equality_result) - { - assertion_result - << std::endl - << "The first state's orientation was " << state1.orientation() - << ", the second state's orientation was " << state2.orientation(); - } - if (!angular_velocity_equality_result) - { - assertion_result << std::endl - << "The first state's angular velocity was " - << state1.angularVelocity() - << ", the second state's angular velocity was " - << state2.angularVelocity(); - } - } +::testing::AssertionResult equalWithinTolerance(const RobotState &state1, + const RobotState &state2, + const double linear_tolerance, + const Angle &angular_tolerance) +{ + auto position_equality_result = + equalWithinTolerance(state1.position(), state2.position(), linear_tolerance); + auto velocity_equality_result = + equalWithinTolerance(state1.velocity(), state2.velocity(), linear_tolerance); + auto orientation_equality_result = equalWithinTolerance( + state1.orientation(), state2.orientation(), angular_tolerance); + auto angular_velocity_equality_result = equalWithinTolerance( + state1.angularVelocity(), state2.angularVelocity(), angular_tolerance); - return assertion_result; - } + auto assertion_result = ::testing::AssertionSuccess(); - ::testing::AssertionResult equalWithinTolerance(const RobotStateWithId &state1, - const RobotStateWithId &state2, - const double linear_tolerance, - const Angle &angular_tolerance) + if (!position_equality_result || !velocity_equality_result || + !orientation_equality_result || !angular_velocity_equality_result) { - if (state1.id != state2.id) + assertion_result = ::testing::AssertionFailure(); + + if (!position_equality_result) { - return ::testing::AssertionFailure() - << "The first state's id was " << state1.id - << ", the second state's id was " << state2.id; + assertion_result << "The first state's position was " << state1.position() + << ", the second state's position was " << state2.position(); } - auto state_equality_result = equalWithinTolerance( - state1.robot_state, state2.robot_state, linear_tolerance, angular_tolerance); - if (!state_equality_result) + if (!velocity_equality_result) { - return state_equality_result; + assertion_result << std::endl + << "The first state's velocity was " << state1.velocity() + << ", the second state's velocity was " << state2.velocity(); + } + if (!orientation_equality_result) + { + assertion_result << std::endl + << "The first state's orientation was " + << state1.orientation() + << ", the second state's orientation was " + << state2.orientation(); + } + if (!angular_velocity_equality_result) + { + assertion_result << std::endl + << "The first state's angular velocity was " + << state1.angularVelocity() + << ", the second state's angular velocity was " + << state2.angularVelocity(); } - - return ::testing::AssertionSuccess(); } - ::testing::AssertionResult equalWithinTolerance(const BallState &state1, - const BallState &state2, - double tolerance) - { - auto position_equality_result = - equalWithinTolerance(state1.position(), state2.position(), tolerance); - auto velocity_equality_result = - equalWithinTolerance(state1.velocity(), state2.velocity(), tolerance); + return assertion_result; +} - auto assertion_result = ::testing::AssertionSuccess(); +::testing::AssertionResult equalWithinTolerance(const RobotStateWithId &state1, + const RobotStateWithId &state2, + const double linear_tolerance, + const Angle &angular_tolerance) +{ + if (state1.id != state2.id) + { + return ::testing::AssertionFailure() + << "The first state's id was " << state1.id + << ", the second state's id was " << state2.id; + } + auto state_equality_result = equalWithinTolerance( + state1.robot_state, state2.robot_state, linear_tolerance, angular_tolerance); + if (!state_equality_result) + { + return state_equality_result; + } - if (!position_equality_result || !velocity_equality_result) - { - assertion_result = ::testing::AssertionFailure(); + return ::testing::AssertionSuccess(); +} - if (!position_equality_result) - { - assertion_result << "The first state's position was " << state1.position() - << ", the second state's position was " - << state2.position(); - } - if (!velocity_equality_result) - { - assertion_result << std::endl - << "The first state's velocity was " << state1.velocity() - << ", the second state's velocity was " - << state2.velocity(); - } - } +::testing::AssertionResult equalWithinTolerance(const BallState &state1, + const BallState &state2, double tolerance) +{ + auto position_equality_result = + equalWithinTolerance(state1.position(), state2.position(), tolerance); + auto velocity_equality_result = + equalWithinTolerance(state1.velocity(), state2.velocity(), tolerance); - return assertion_result; - } + auto assertion_result = ::testing::AssertionSuccess(); - ::testing::AssertionResult equalWithinTolerance(const Eigen::MatrixXd &matrix1, - const Eigen::MatrixXd &matrix2, - double tolerance) + if (!position_equality_result || !velocity_equality_result) { - auto distance = matrix1 - matrix2; - auto norm = distance.norm(); + assertion_result = ::testing::AssertionFailure(); - if (equalWithinTolerance(norm, 0, tolerance)) + if (!position_equality_result) { - return ::testing::AssertionSuccess(); + assertion_result << "The first state's position was " << state1.position() + << ", the second state's position was " << state2.position(); } - else + if (!velocity_equality_result) { - return ::testing::AssertionFailure() << "Matrix 1 was \n" - << matrix1 << "\n, matrix 2 was \n" - << matrix2; + assertion_result << std::endl + << "The first state's velocity was " << state1.velocity() + << ", the second state's velocity was " << state2.velocity(); } } + + return assertion_result; +} + +::testing::AssertionResult equalWithinTolerance(const Eigen::MatrixXd &matrix1, + const Eigen::MatrixXd &matrix2, + double tolerance) +{ + auto distance = matrix1 - matrix2; + auto norm = distance.norm(); + + if (equalWithinTolerance(norm, 0, tolerance)) + { + return ::testing::AssertionSuccess(); + } + else + { + return ::testing::AssertionFailure() << "Matrix 1 was \n" + << matrix1 << "\n, matrix 2 was \n" + << matrix2; + } +} }; // namespace TestUtil diff --git a/src/software/test_util/equal_within_tolerance.h b/src/software/test_util/equal_within_tolerance.h index 58811b8b51..f5914a5a91 100644 --- a/src/software/test_util/equal_within_tolerance.h +++ b/src/software/test_util/equal_within_tolerance.h @@ -19,174 +19,174 @@ namespace TestUtil { - /** - * Checks if two durations are within tolerance of each other - * Two durations are within tolerance of each other if the absolute duration - * difference is less than the tolerance duration - * - * @param duration1, duration2 Durations to compare - * @param tolerance tolerance to check equality with, default is 1 microsecond (1/1000 - * millisecond) - * - * @return AssertionSuccess if the two durations are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance( - const Duration &duration1, const Duration &duration2, - const Duration &tolerance = Duration::fromMilliseconds(0.001)); +/** + * Checks if two durations are within tolerance of each other + * Two durations are within tolerance of each other if the absolute duration + * difference is less than the tolerance duration + * + * @param duration1, duration2 Durations to compare + * @param tolerance tolerance to check equality with, default is 1 microsecond (1/1000 + * millisecond) + * + * @return AssertionSuccess if the two durations are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance( + const Duration &duration1, const Duration &duration2, + const Duration &tolerance = Duration::fromMilliseconds(0.001)); - /** - * Checks if two polygons are within tolerance of each other - * Two polygons are within tolerance of each other if the corresponding points are - * within tolerance of each other - * - * @param poly1, poly2 Polygons to compare - * @param tolerance tolerance to check equality with, default is - * METERS_PER_MILLIMETER - * - * @return AssertionSuccess if the two polygons are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance( - const Polygon &poly1, const Polygon &poly2, - double tolerance = METERS_PER_MILLIMETER); +/** + * Checks if two polygons are within tolerance of each other + * Two polygons are within tolerance of each other if the corresponding points are + * within tolerance of each other + * + * @param poly1, poly2 Polygons to compare + * @param tolerance tolerance to check equality with, default is + * METERS_PER_MILLIMETER + * + * @return AssertionSuccess if the two polygons are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const Polygon &poly1, + const Polygon &poly2, + double tolerance = METERS_PER_MILLIMETER); - /** - * Checks if two stadiums are within tolerance of each other - * Two stadiums are within tolerance of each other if the corresponding points are - * within tolerance of each other and the radii are within tolerance - * - * @param stadium1, stadium2 Stadiums to compare - * @param tolerance tolerance to check equality with, default is - * METERS_PER_MILLIMETER - * - * @return AssertionSuccess if the two polygons are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance( - const Stadium &stadium1, const Stadium &stadium2, - double tolerance = METERS_PER_MILLIMETER); +/** + * Checks if two stadiums are within tolerance of each other + * Two stadiums are within tolerance of each other if the corresponding points are + * within tolerance of each other and the radii are within tolerance + * + * @param stadium1, stadium2 Stadiums to compare + * @param tolerance tolerance to check equality with, default is + * METERS_PER_MILLIMETER + * + * @return AssertionSuccess if the two polygons are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const Stadium &stadium1, + const Stadium &stadium2, + double tolerance = METERS_PER_MILLIMETER); - /** - * Checks if two circles are within tolerance of each other - * Two circles are within tolerance of each other if the origins are within - * tolerance of each other and radius is within tolerance of each other - * - * @param c1, c2 Circles to compare - * @param tolerance tolerance to check equality with, default is - * METERS_PER_MILLIMETER - * - * @return AssertionSuccess if the two circles are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance( - const Circle &c1, const Circle &c2, double tolerance = METERS_PER_MILLIMETER); +/** + * Checks if two circles are within tolerance of each other + * Two circles are within tolerance of each other if the origins are within + * tolerance of each other and radius is within tolerance of each other + * + * @param c1, c2 Circles to compare + * @param tolerance tolerance to check equality with, default is + * METERS_PER_MILLIMETER + * + * @return AssertionSuccess if the two circles are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const Circle &c1, const Circle &c2, + double tolerance = METERS_PER_MILLIMETER); - /** - * Checks if two Angles are within tolerance of each other - * - * @param a1, a2 Angles to compare - * @param tolerance tolerance to check equality with - * - * @return AssertionSuccess if the two Angles are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(const Angle &a1, const Angle &a2, - const Angle &tolerance); +/** + * Checks if two Angles are within tolerance of each other + * + * @param a1, a2 Angles to compare + * @param tolerance tolerance to check equality with + * + * @return AssertionSuccess if the two Angles are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const Angle &a1, const Angle &a2, + const Angle &tolerance); - /** - * Checks if two vectors are within tolerance of each other - * Two vectors are within tolerance of each other if the respective x and y values - * are within tolerance of each other - * - * @param v1, v2 Vectors to compare - * @param tolerance tolerance to check equality with, default is - * METERS_PER_MILLIMETER - * - * @return AssertionSuccess if the two points are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(const Vector &v1, const Vector &v2, - double tolerance); +/** + * Checks if two vectors are within tolerance of each other + * Two vectors are within tolerance of each other if the respective x and y values + * are within tolerance of each other + * + * @param v1, v2 Vectors to compare + * @param tolerance tolerance to check equality with, default is + * METERS_PER_MILLIMETER + * + * @return AssertionSuccess if the two points are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const Vector &v1, const Vector &v2, + double tolerance); - /** - * Checks if two points are within tolerance of each other - * Two points are within tolerance of each other if the respective x and y values - * are within tolerance of each other - * - * @param pt1, pt2 Points to compare - * @param tolerance tolerance to check equality with, default is - * METERS_PER_MILLIMETER - * - * @return AssertionSuccess if the two points are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(const Point &pt1, const Point &pt2, - double tolerance); +/** + * Checks if two points are within tolerance of each other + * Two points are within tolerance of each other if the respective x and y values + * are within tolerance of each other + * + * @param pt1, pt2 Points to compare + * @param tolerance tolerance to check equality with, default is + * METERS_PER_MILLIMETER + * + * @return AssertionSuccess if the two points are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const Point &pt1, const Point &pt2, + double tolerance); - /** - * Checks if two values are within tolerance of each other - * - * @param val1, val2 values to compare - * @param tolerance tolerance to check equality with, default is - * METERS_PER_MILLIMETER - * - * @return AssertionSuccess if the two values are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(double val1, double val2, - double tolerance); +/** + * Checks if two values are within tolerance of each other + * + * @param val1, val2 values to compare + * @param tolerance tolerance to check equality with, default is + * METERS_PER_MILLIMETER + * + * @return AssertionSuccess if the two values are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(double val1, double val2, + double tolerance); - /** - * Checks if two RobotStates are within tolerance of each other. - * Two states are within tolerance if each other if their positions and velocities - * are within tolerance of each other - * - * @param state1, state2 states to compare - * @param tolerance The tolerance to check equality with - * - * @return AssertionSuccess if the two values are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(const RobotState &state1, - const RobotState &state2, - double linear_tolerance, - const Angle &angular_tolerance); +/** + * Checks if two RobotStates are within tolerance of each other. + * Two states are within tolerance if each other if their positions and velocities + * are within tolerance of each other + * + * @param state1, state2 states to compare + * @param tolerance The tolerance to check equality with + * + * @return AssertionSuccess if the two values are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const RobotState &state1, + const RobotState &state2, + double linear_tolerance, + const Angle &angular_tolerance); - /** - * Checks if two RobotStateWithIds are within tolerance of each other. - * Two states are within tolerance if their states are within tolerance - * of each other - * - * @param state1, state2 states to compare - * @param linear_tolerance The tolerance to check positions and velocities with - * @param angular_tolerance The tolerance to check orientations and angular - * velocities with - * - * @return AssertionSuccess if the two values are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(const RobotStateWithId &state1, - const RobotStateWithId &state2, - double linear_tolerance, - const Angle &angular_tolerance); +/** + * Checks if two RobotStateWithIds are within tolerance of each other. + * Two states are within tolerance if their states are within tolerance + * of each other + * + * @param state1, state2 states to compare + * @param linear_tolerance The tolerance to check positions and velocities with + * @param angular_tolerance The tolerance to check orientations and angular + * velocities with + * + * @return AssertionSuccess if the two values are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const RobotStateWithId &state1, + const RobotStateWithId &state2, + double linear_tolerance, + const Angle &angular_tolerance); - /** - * Checks if two BallStates are within tolerance of each other. - * Two states are within tolerance if their states are within tolerance - * of each other - * - * @param state1, state2 states to compare - * @param linear_tolerance The tolerance to check positions and velocities with - * @param angular_tolerance The tolerance to check orientations and angular - * velocities with - * - * @return AssertionSuccess if the two values are within tolerance of each other - */ - ::testing::AssertionResult equalWithinTolerance(const BallState &state1, - const BallState &state2, - double tolerance); +/** + * Checks if two BallStates are within tolerance of each other. + * Two states are within tolerance if their states are within tolerance + * of each other + * + * @param state1, state2 states to compare + * @param linear_tolerance The tolerance to check positions and velocities with + * @param angular_tolerance The tolerance to check orientations and angular + * velocities with + * + * @return AssertionSuccess if the two values are within tolerance of each other + */ +::testing::AssertionResult equalWithinTolerance(const BallState &state1, + const BallState &state2, + double tolerance); - /** - * Checks if two matrices are within tolerance of each other. - * Two matrices are within tolerance if their norms are within tolerance of each - * other. - * - * @param matrix1, matrix2 The matrices to compare. - * @param tolerance The tolerance to check velocities with. - * @return AssertionSuccess if the two states are within tolerance of each other. - */ - ::testing::AssertionResult equalWithinTolerance(const Eigen::MatrixXd &matrix1, - const Eigen::MatrixXd &matrix2, - double tolerance); +/** + * Checks if two matrices are within tolerance of each other. + * Two matrices are within tolerance if their norms are within tolerance of each + * other. + * + * @param matrix1, matrix2 The matrices to compare. + * @param tolerance The tolerance to check velocities with. + * @return AssertionSuccess if the two states are within tolerance of each other. + */ +::testing::AssertionResult equalWithinTolerance(const Eigen::MatrixXd &matrix1, + const Eigen::MatrixXd &matrix2, + double tolerance); }; // namespace TestUtil diff --git a/src/software/test_util/test_util.cpp b/src/software/test_util/test_util.cpp index 088246cb07..0f8c2d527c 100644 --- a/src/software/test_util/test_util.cpp +++ b/src/software/test_util/test_util.cpp @@ -8,131 +8,130 @@ namespace TestUtil { - std::shared_ptr createBlankTestingWorld( - std::unique_ptr field_proto) - { - Field field = Field(*field_proto); - Team friendly_team = Team(Duration::fromMilliseconds(1000)); - Team enemy_team = Team(Duration::fromMilliseconds(1000)); - Ball ball = Ball(Point(), Vector(), Timestamp::fromSeconds(0)); +std::shared_ptr createBlankTestingWorld( + std::unique_ptr field_proto) +{ + Field field = Field(*field_proto); + Team friendly_team = Team(Duration::fromMilliseconds(1000)); + Team enemy_team = Team(Duration::fromMilliseconds(1000)); + Ball ball = Ball(Point(), Vector(), Timestamp::fromSeconds(0)); - return std::make_shared(World(field, ball, friendly_team, enemy_team)); - } + return std::make_shared(World(field, ball, friendly_team, enemy_team)); +} - std::shared_ptr createBlankTestingWorld(TbotsProto::FieldType field_type) - { - return createBlankTestingWorld(createField(Field::createField(field_type))); - } +std::shared_ptr createBlankTestingWorld(TbotsProto::FieldType field_type) +{ + return createBlankTestingWorld(createField(Field::createField(field_type))); +} - Team setRobotPositionsHelper(Team team, const std::vector &robot_positions, - const Timestamp ×tamp) +Team setRobotPositionsHelper(Team team, const std::vector &robot_positions, + const Timestamp ×tamp) +{ + std::vector robots; + unsigned int robot_id_index = 0; + for (const Point &robot_position : robot_positions) { - std::vector robots; - unsigned int robot_id_index = 0; - for (const Point &robot_position : robot_positions) - { - Robot robot = Robot(robot_id_index, robot_position, Vector(), Angle::zero(), - AngularVelocity::zero(), timestamp); - robots.emplace_back(robot); - - robot_id_index++; - } - - team.clearAllRobots(); - team.updateRobots(robots); + Robot robot = Robot(robot_id_index, robot_position, Vector(), Angle::zero(), + AngularVelocity::zero(), timestamp); + robots.emplace_back(robot); - return team; + robot_id_index++; } - void setFriendlyRobotPositions(const std::shared_ptr &world, - std::vector robot_positions, - const Timestamp ×tamp) - { - Team new_friendly_team = - setRobotPositionsHelper(world->friendlyTeam(), robot_positions, timestamp); - world->updateFriendlyTeamState(new_friendly_team); - } + team.clearAllRobots(); + team.updateRobots(robots); - void setEnemyRobotPositions(const std::shared_ptr &world, - std::vector robot_positions, - const Timestamp ×tamp) - { - Team new_enemy_team = - setRobotPositionsHelper(world->enemyTeam(), robot_positions, timestamp); - world->updateEnemyTeamState(new_enemy_team); - } + return team; +} - void setBallPosition(const std::shared_ptr &world, Point ball_position, - Timestamp timestamp) - { - BallState ball_state(ball_position, world->ball().velocity()); - world->updateBall(Ball(ball_state, timestamp)); - } +void setFriendlyRobotPositions(const std::shared_ptr &world, + std::vector robot_positions, + const Timestamp ×tamp) +{ + Team new_friendly_team = + setRobotPositionsHelper(world->friendlyTeam(), robot_positions, timestamp); + world->updateFriendlyTeamState(new_friendly_team); +} + +void setEnemyRobotPositions(const std::shared_ptr &world, + std::vector robot_positions, + const Timestamp ×tamp) +{ + Team new_enemy_team = + setRobotPositionsHelper(world->enemyTeam(), robot_positions, timestamp); + world->updateEnemyTeamState(new_enemy_team); +} - void setBallVelocity(const std::shared_ptr &world, Vector ball_velocity, - Timestamp timestamp) - { - BallState ball_state(world->ball().position(), ball_velocity); - world->updateBall(Ball(ball_state, timestamp)); - } +void setBallPosition(const std::shared_ptr &world, Point ball_position, + Timestamp timestamp) +{ + BallState ball_state(ball_position, world->ball().velocity()); + world->updateBall(Ball(ball_state, timestamp)); +} - Robot createRobotAtPos(const Point &pt) - { - static RobotId robot_id_counter = 0; - return Robot(robot_id_counter++, pt, Vector(), Angle(), AngularVelocity(), - Timestamp()); - } +void setBallVelocity(const std::shared_ptr &world, Vector ball_velocity, + Timestamp timestamp) +{ + BallState ball_state(world->ball().position(), ball_velocity); + world->updateBall(Ball(ball_state, timestamp)); +} - double secondsSince(std::chrono::time_point start_time) - { - return millisecondsSince(start_time) / MILLISECONDS_PER_SECOND; - } +Robot createRobotAtPos(const Point &pt) +{ + static RobotId robot_id_counter = 0; + return Robot(robot_id_counter++, pt, Vector(), Angle(), AngularVelocity(), + Timestamp()); +} - double millisecondsSince( - std::chrono::time_point start_time) - { - const auto end_time = std::chrono::system_clock::now(); - return static_cast(std::chrono::duration_cast( - end_time - start_time) - .count()) / - NANOSECONDS_PER_MILLISECOND; - } +double secondsSince(std::chrono::time_point start_time) +{ + return millisecondsSince(start_time) / MILLISECONDS_PER_SECOND; +} - std::vector createStationaryRobotStatesWithId( - const std::vector &positions) +double millisecondsSince(std::chrono::time_point start_time) +{ + const auto end_time = std::chrono::system_clock::now(); + return static_cast( + std::chrono::duration_cast(end_time - start_time) + .count()) / + NANOSECONDS_PER_MILLISECOND; +} + +std::vector createStationaryRobotStatesWithId( + const std::vector &positions) +{ + std::vector states; + for (RobotId id = 0; id < static_cast(positions.size()); id++) { - std::vector states; - for (RobotId id = 0; id < static_cast(positions.size()); id++) - { - states.push_back(RobotStateWithId{ - .id = id, - .robot_state = RobotState(positions[id], Vector(0, 0), Angle::zero(), - AngularVelocity::zero())}); - } - return states; + states.push_back(RobotStateWithId{ + .id = id, + .robot_state = RobotState(positions[id], Vector(0, 0), Angle::zero(), + AngularVelocity::zero())}); } + return states; +} - std::vector createMovingRobotStatesWithId( - const std::vector &positions, const std::vector &velocity) +std::vector createMovingRobotStatesWithId( + const std::vector &positions, const std::vector &velocity) +{ + std::vector states; + for (RobotId id = 0; id < static_cast(positions.size()); id++) { - std::vector states; - for (RobotId id = 0; id < static_cast(positions.size()); id++) - { - states.push_back(RobotStateWithId{ - .id = id, - .robot_state = RobotState(positions[id], velocity[id], Angle::zero(), - AngularVelocity::zero())}); - } - return states; + states.push_back(RobotStateWithId{ + .id = id, + .robot_state = RobotState(positions[id], velocity[id], Angle::zero(), + AngularVelocity::zero())}); } + return states; +} - GameState createGameState(const RefereeCommand ¤t_referee_command, - const RefereeCommand &previous_referee_command) - { - GameState game_state; - game_state.updateRefereeCommand(previous_referee_command); - game_state.updateRefereeCommand(current_referee_command); - return game_state; - } +GameState createGameState(const RefereeCommand ¤t_referee_command, + const RefereeCommand &previous_referee_command) +{ + GameState game_state; + game_state.updateRefereeCommand(previous_referee_command); + game_state.updateRefereeCommand(current_referee_command); + return game_state; +} }; // namespace TestUtil diff --git a/src/software/test_util/test_util.h b/src/software/test_util/test_util.h index 91e31fedca..e15bba2538 100644 --- a/src/software/test_util/test_util.h +++ b/src/software/test_util/test_util.h @@ -26,150 +26,149 @@ */ namespace TestUtil { - /** - * Creates a World object with a normal SSL Division B field, default (empty) - * teams with 1000 milliseconds expiry buffers, and the Ball at the center of the - * field with no velocity. - * - * @param field_type The field type - * - * @return a World object initialized with a Division B SSL field, empty teams - * with 1000 millisecond expiry buffers, and the Ball at the center of the field - * with no velocity. - */ - std::shared_ptr createBlankTestingWorld( - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B); - - /** - * Creates a blank testing World given a Field. - * - * @param field_proto field to use when creating a field - * - * @return a World object initialized with the given field, empty teams with 1000 - * millisecond expiry buffers, and the Ball at the centre of the field with no - * velocity. - */ - std::shared_ptr createBlankTestingWorld(TbotsProto::Field field_proto); - - /** - * Returns a new World object with friendly robots in the positions specified - * by the vector of Points. Robots will be created with consecutive increasing - * id's from 0 to robot_positions.size() - 1. Any friendly robots that already - * existed in the world are removed before the new ones are added. - * - * @param world The current world object - * @param robot_positions The positions to place friendly robots in - * @return A new world object with friendly robots in the given positions - */ - void setFriendlyRobotPositions(const std::shared_ptr &world, - std::vector robot_positions, - const Timestamp ×tamp); - - /** - * Returns a new World object with enemy robots in the positions specified - * by the vector of Points. Robots will be created with consecutive increasing - * id's from 0 to robot_positions.size() - 1. Any enemy robots that already - * existed in the world are removed before the new ones are added. - * - * @param world The current world object - * @param robot_positions The positions to place enemy robots in - * @return A new world object with enemy robots in the given positions - */ - void setEnemyRobotPositions(const std::shared_ptr &world, - std::vector robot_positions, - const Timestamp ×tamp); - - /** - * Returns a new World object with the Ball placed in the new position - * specified - * - * @param world The current world object - * @param ball_position The new position for the ball - * @return A new World object with the ball placed in the given position - */ - void setBallPosition(const std::shared_ptr &world, Point ball_position, - Timestamp timestamp); - - /** - * Returns a new World object with the Ball's velocity set to the new velocity - * - * @param world The current world object - * @param ball_velocity The new velocity for the ball - * @return A new World object with the ball's velocity set to the new velocity - */ - void setBallVelocity(const std::shared_ptr &world, Vector ball_velocity, - Timestamp timestamp); - - /** - * Returns a robot at the given position with zero velocity, - * facing zero radians, with zero rad/s of angular velocity, - * and an id monotonically increasing at each call of this - * function - * @param pt the point - * @return a robot at the point - */ - Robot createRobotAtPos(const Point &pt); - - /** - * Gets the number of milliseconds since the start_time - * - * @param start_time time point to calculate time since - * - * @return milliseconds since start time - */ - double millisecondsSince( - std::chrono::time_point start_time); - - /** - * Gets the number of seconds since the start_time - * - * @param start_time time point to calculate time since - * - * @return seconds since start time - */ - double secondsSince(std::chrono::time_point start_time); - - /** - * Returns a new team with robots placed at the given positions. Robots in the - * given team are removed before new ones are placed at the given positions, - * so pre-existing robots to not persist. - * - * @param team The team for which to set robot positions - * @param robot_positions The positions of the robots - * @return A new team with robots placed at the given positions - */ - Team setRobotPositionsHelper(Team team, const std::vector &robot_positions, - const Timestamp ×tamp); - - /** - * Creates a list of RobotStateWithId at given positions with 0 velocity, 0 angular - * velocity and 0 orientation. The id is set as the index in the list of positions. - * - * @param positions The positions to create robots at - */ - std::vector createStationaryRobotStatesWithId( - const std::vector &positions); - - /** - * Creates a list of RobotStateWithId at given positions with input velocity, 0 - * angular velocity and 0 orientation. The id is set as the index in the list of - * positions. - * - * @param positions The positions to create robots at - * @param velocity The velocities given to each robot - */ - std::vector createMovingRobotStatesWithId( - const std::vector &positions, const std::vector &velocity); - - /** - * Create a new GameState and update it with the previous_referee_command - * followed by the current_referee_command. This is so the GameState can internally - * be in the correct state for testing. - * - * @param current_referee_command The name of the current referee command to set - * @param previous_referee_command The name of the previous referee command to set - * @return the new GameState - */ - GameState createGameState(const RefereeCommand ¤t_referee_command, - const RefereeCommand &previous_referee_command); +/** + * Creates a World object with a normal SSL Division B field, default (empty) + * teams with 1000 milliseconds expiry buffers, and the Ball at the center of the + * field with no velocity. + * + * @param field_type The field type + * + * @return a World object initialized with a Division B SSL field, empty teams + * with 1000 millisecond expiry buffers, and the Ball at the center of the field + * with no velocity. + */ +std::shared_ptr createBlankTestingWorld( + TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B); + +/** + * Creates a blank testing World given a Field. + * + * @param field_proto field to use when creating a field + * + * @return a World object initialized with the given field, empty teams with 1000 + * millisecond expiry buffers, and the Ball at the centre of the field with no + * velocity. + */ +std::shared_ptr createBlankTestingWorld(TbotsProto::Field field_proto); + +/** + * Returns a new World object with friendly robots in the positions specified + * by the vector of Points. Robots will be created with consecutive increasing + * id's from 0 to robot_positions.size() - 1. Any friendly robots that already + * existed in the world are removed before the new ones are added. + * + * @param world The current world object + * @param robot_positions The positions to place friendly robots in + * @return A new world object with friendly robots in the given positions + */ +void setFriendlyRobotPositions(const std::shared_ptr &world, + std::vector robot_positions, + const Timestamp ×tamp); + +/** + * Returns a new World object with enemy robots in the positions specified + * by the vector of Points. Robots will be created with consecutive increasing + * id's from 0 to robot_positions.size() - 1. Any enemy robots that already + * existed in the world are removed before the new ones are added. + * + * @param world The current world object + * @param robot_positions The positions to place enemy robots in + * @return A new world object with enemy robots in the given positions + */ +void setEnemyRobotPositions(const std::shared_ptr &world, + std::vector robot_positions, + const Timestamp ×tamp); + +/** + * Returns a new World object with the Ball placed in the new position + * specified + * + * @param world The current world object + * @param ball_position The new position for the ball + * @return A new World object with the ball placed in the given position + */ +void setBallPosition(const std::shared_ptr &world, Point ball_position, + Timestamp timestamp); + +/** + * Returns a new World object with the Ball's velocity set to the new velocity + * + * @param world The current world object + * @param ball_velocity The new velocity for the ball + * @return A new World object with the ball's velocity set to the new velocity + */ +void setBallVelocity(const std::shared_ptr &world, Vector ball_velocity, + Timestamp timestamp); + +/** + * Returns a robot at the given position with zero velocity, + * facing zero radians, with zero rad/s of angular velocity, + * and an id monotonically increasing at each call of this + * function + * @param pt the point + * @return a robot at the point + */ +Robot createRobotAtPos(const Point &pt); + +/** + * Gets the number of milliseconds since the start_time + * + * @param start_time time point to calculate time since + * + * @return milliseconds since start time + */ +double millisecondsSince(std::chrono::time_point start_time); + +/** + * Gets the number of seconds since the start_time + * + * @param start_time time point to calculate time since + * + * @return seconds since start time + */ +double secondsSince(std::chrono::time_point start_time); + +/** + * Returns a new team with robots placed at the given positions. Robots in the + * given team are removed before new ones are placed at the given positions, + * so pre-existing robots to not persist. + * + * @param team The team for which to set robot positions + * @param robot_positions The positions of the robots + * @return A new team with robots placed at the given positions + */ +Team setRobotPositionsHelper(Team team, const std::vector &robot_positions, + const Timestamp ×tamp); + +/** + * Creates a list of RobotStateWithId at given positions with 0 velocity, 0 angular + * velocity and 0 orientation. The id is set as the index in the list of positions. + * + * @param positions The positions to create robots at + */ +std::vector createStationaryRobotStatesWithId( + const std::vector &positions); + +/** + * Creates a list of RobotStateWithId at given positions with input velocity, 0 + * angular velocity and 0 orientation. The id is set as the index in the list of + * positions. + * + * @param positions The positions to create robots at + * @param velocity The velocities given to each robot + */ +std::vector createMovingRobotStatesWithId( + const std::vector &positions, const std::vector &velocity); + +/** + * Create a new GameState and update it with the previous_referee_command + * followed by the current_referee_command. This is so the GameState can internally + * be in the correct state for testing. + * + * @param current_referee_command The name of the current referee command to set + * @param previous_referee_command The name of the previous referee command to set + * @return the new GameState + */ +GameState createGameState(const RefereeCommand ¤t_referee_command, + const RefereeCommand &previous_referee_command); }; // namespace TestUtil diff --git a/src/software/thunderscope/binary_context_managers/BUILD b/src/software/thunderscope/binary_context_managers/BUILD index 8396ae6351..835615fcfd 100644 --- a/src/software/thunderscope/binary_context_managers/BUILD +++ b/src/software/thunderscope/binary_context_managers/BUILD @@ -22,6 +22,7 @@ py_library( deps = [ "//proto:import_all_protos", "//software/networking:ssl_proto_communication", + "//software/thunderscope/common:thread_safe_circular_buffer", ], ) diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index d43bbece79..20c8f11033 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -1,5 +1,6 @@ from __future__ import annotations +import queue import random import logging import os @@ -17,8 +18,12 @@ from software.py_constants import * from software.thunderscope.binary_context_managers.util import * from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer +from software.thunderscope.common.thread_safe_circular_buffer import ( + ThreadSafeCircularBuffer, +) logger = logging.getLogger(__name__) +import itertools class Gamecontroller: @@ -29,7 +34,9 @@ class Gamecontroller: CI_MODE_OUTPUT_RECEIVE_BUFFER_SIZE = 9000 def __init__( - self, suppress_logs: bool = False, use_conventional_port: bool = False + self, + suppress_logs: bool = False, + use_conventional_port: bool = False, ) -> None: """Run Gamecontroller @@ -54,6 +61,14 @@ def __init__( buffer_size=2, protobuf_type=ManualGCCommand ) + self.simulator_proto_unix_io = None + self.blue_team_world_buffer = ThreadSafeCircularBuffer( + buffer_size=1, protobuf_type=World + ) + self.latest_world = None + self.blue_removed_robot_ids = queue.Queue() + self.yellow_removed_robot_ids = queue.Queue() + def get_referee_port(self) -> int: """Sometimes, the port that we are using changes depending on context. We want a getter function that returns the port we are using. @@ -95,7 +110,6 @@ def __exit__(self, type, value, traceback) -> None: """ self.gamecontroller_proc.terminate() self.gamecontroller_proc.wait() - self.ci_socket.close() def refresh(self): @@ -119,6 +133,104 @@ def refresh(self): ) manual_command = self.command_override_buffer.get(return_cached=False) + @staticmethod + def __update_robot_count( + robot_states, + team: Team, + max_robots: int, + removed_robot_ids: queue.Queue[int], + field_edge_y_meters: float, + ) -> None: + """Static method for updating the number of robots in the world state (i.e. robot_states) for a given Team. + This method is team & side agnostic. + + :param robot_states: WorldState map protobuf to be updated. + :param team: the Team of robots currently in play + :param max_robots: The number of robots we should have on the field. Must be >= 0 + :param removed_robot_ids: The robots (IDs) which have been removed already and can safely be re-added + :param field_edge_y_meters: Places new robots at this y position along the centerline + :return: + """ + # build the robot state for placing robots at the edge of field + place_state = RobotState( + global_position=Point(x_meters=0, y_meters=field_edge_y_meters) + ) + # Remove robots, as we have too many. Set robot velocities to zero to avoid any drift + for count, robot in enumerate(team.team_robots, start=1): + if count <= max_robots: + robot_states[robot.id].CopyFrom(robot.current_state) + velocity = robot_states[robot.id].global_velocity + velocity.x_component_meters = 0 + velocity.y_component_meters = 0 + else: + removed_robot_ids.put(robot.id) + # Add robots, since we are missing some + robots_diff: int = max_robots - len(team.team_robots) + if robots_diff <= 0: + return + for _ in range(robots_diff): + try: + robot_states[removed_robot_ids.get_nowait()].CopyFrom(place_state) + except queue.Empty: + return + + def handle_referee(self, referee: Referee) -> None: + """Updates the world state based on the referee message + :param referee: the referee protobuf message + """ + # Check that we are running with the simulator and have access to its + # proto unix io + if self.simulator_proto_unix_io is None: + return + + # Convert the latest blue world into a WorldState we can send to the simulator and update the robots + self.latest_world = self.blue_team_world_buffer.get( + block=False, return_cached=True + ) + + max_allowed_bots_yellow: int = referee.yellow.max_allowed_bots + max_allowed_bots_blue: int = referee.blue.max_allowed_bots + # Ignore if nothing needs to be updated + if ( + len(self.latest_world.friendly_team.team_robots) == max_allowed_bots_blue + and len(self.latest_world.enemy_team.team_robots) == max_allowed_bots_yellow + ): + return + + # Populate a blank WorldState with new updated robot information + world_state = WorldState() + field_edge_y_meters: Final[int] = ( + self.latest_world.field.field_y_length + - self.latest_world.field.boundary_buffer_size + ) + + self.__update_robot_count( + world_state.blue_robots, + self.latest_world.friendly_team, + max_allowed_bots_blue, + self.blue_removed_robot_ids, + field_edge_y_meters, + ) + self.__update_robot_count( + world_state.yellow_robots, + self.latest_world.enemy_team, + max_allowed_bots_yellow, + self.yellow_removed_robot_ids, + -field_edge_y_meters, + ) + + # Check if we need to invert the world state + if referee.blue_team_on_positive_half: + for robot in itertools.chain( + world_state.blue_robots, world_state.yellow_robots + ): + robot.current_state.global_position.x_meters *= -1 + robot.current_state.global_position.y_meters *= -1 + robot.current_state.global_orientation.radians += math.pi + + # Send out updated world state + self.simulator_proto_unix_io.send_proto(WorldState, world_state) + def is_valid_port(self, port): """Determine whether or not a given port is valid @@ -154,12 +266,14 @@ def setup_proto_unix_io( blue_full_system_proto_unix_io: ProtoUnixIO, yellow_full_system_proto_unix_io: ProtoUnixIO, autoref_proto_unix_io: ProtoUnixIO = None, + simulator_proto_unix_io: ProtoUnixIO = None, ) -> None: """Setup gamecontroller io :param blue_full_system_proto_unix_io: The proto unix io of the blue full system. :param yellow_full_system_proto_unix_io: The proto unix io of the yellow full system. :param autoref_proto_unix_io: The proto unix io for the autoref + :param simulator_proto_unix_io: The socket for the Gamecontroller to interact with the simulator """ def __send_referee_command(data: Referee) -> None: @@ -168,6 +282,7 @@ def __send_referee_command(data: Referee) -> None: :param data: The referee command to send """ + self.handle_referee(data) blue_full_system_proto_unix_io.send_proto(Referee, data) yellow_full_system_proto_unix_io.send_proto(Referee, data) if autoref_proto_unix_io is not None: @@ -187,6 +302,10 @@ def __send_referee_command(data: Referee) -> None: yellow_full_system_proto_unix_io.register_observer( ManualGCCommand, self.command_override_buffer ) + blue_full_system_proto_unix_io.register_observer( + World, self.blue_team_world_buffer + ) + self.simulator_proto_unix_io = simulator_proto_unix_io def send_gc_command( self, diff --git a/src/software/thunderscope/common/BUILD b/src/software/thunderscope/common/BUILD index 561886b9ad..34a7d0ac9b 100644 --- a/src/software/thunderscope/common/BUILD +++ b/src/software/thunderscope/common/BUILD @@ -57,3 +57,11 @@ py_library( requirement("pyqt-toast-notification"), ], ) + +py_library( + name = "thread_safe_circular_buffer", + srcs = ["thread_safe_circular_buffer.py"], + deps = [ + "//software/thunderscope:thread_safe_buffer", + ], +) diff --git a/src/software/thunderscope/common/proto_plotter.py b/src/software/thunderscope/common/proto_plotter.py index 81cf5fa0a1..c2ed21b256 100644 --- a/src/software/thunderscope/common/proto_plotter.py +++ b/src/software/thunderscope/common/proto_plotter.py @@ -100,7 +100,7 @@ def refresh(self) -> None: # Dump the entire buffer into a deque. This operation is fast because # its just consuming data from the buffer and appending it to a deque. for proto_class, buffer in self.buffers.items(): - for _ in range(buffer.queue.qsize()): + for _ in range(buffer.size()): data = self.configuration[proto_class](buffer.get(block=False)) # If named_value is new, create a plot and for the new value and diff --git a/src/software/thunderscope/common/thread_safe_circular_buffer.py b/src/software/thunderscope/common/thread_safe_circular_buffer.py new file mode 100644 index 0000000000..a977081eff --- /dev/null +++ b/src/software/thunderscope/common/thread_safe_circular_buffer.py @@ -0,0 +1,87 @@ +from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer +from collections import deque +from typing import Type, Optional +from google.protobuf.message import Message +from typing import override + + +class ThreadSafeCircularBuffer(ThreadSafeBuffer): + """Multiple producer, multiple consumer buffer. See: ThreadSafeBuffer + + │ buffer_size │ + ├──────────────────────────────────────────┤ + │ │ + + ┌──────┬──────┬──────┬──────┬──────┬───────┐ + put() │ │ │ │ │ │ │ get() + └──────┴──────┴──────┴──────┴──────┴───────┘ + ThreadSafeCircularBuffer + """ + + def __init__( + self, buffer_size: int, protobuf_type: Type[Message], log_overrun: bool = False + ) -> None: + """A circular buffer to hold data to be consumed. + + :param buffer_size: The max size of the buffer. + :param protobuf_type: To buffer + :param log_overrun: If True, warns when we lose protos during future operations + """ + super().__init__(buffer_size, protobuf_type) + self.log_overrun = log_overrun + self.buffer = deque(maxlen=buffer_size) + self.empty_exception = IndexError + + @override + def get( + self, block: bool = False, timeout: float = None, return_cached: bool = True + ) -> Optional[Message]: + """Get data from the buffer immediately. + + If the buffer is empty: + - Return cached message if return_cached is True, otherwise returns None + + :param block: This does nothing as all operations are immediate + :param timeout: This does nothing as all operations are immediate + :param return_cached: If buffer is empty, decides whether to + return cached value (True) or return None (false) + :return: protobuf (cached if there is no data in the buffer and return_cached is True) + """ + if ( + self.log_overrun + and self.protos_dropped > self.last_logged_protos_dropped + and self.protos_dropped > self.MIN_DROPPED_BEFORE_LOG + ): + self.logger.warn( + "packets dropped; thunderscope did not show {} protos".format( + self.protos_dropped + ) + ) + self.last_logged_protos_dropped = self.protos_dropped + + try: + self.cached_msg = self.buffer.popleft() + except self.empty_exception: + if not return_cached: + return None + + return self.cached_msg + + @override + def put(self, proto: Message, block: bool = False, timeout: float = None) -> None: + """Put data into the buffer. If the buffer is full, the proto may be logged. + + :param proto: The proto to place in the buffer + :param block: True blocks overwriting items in a full buffer, dropping the proto. False writes every time + :param timeout: This does nothing as all operations are immediate + """ + if len(self.buffer) == self.buffer.maxlen: + self.protos_dropped += 1 + if block: + return + self.buffer.append(proto) + + @override + def size(self) -> int: + """Returns the number of objects in the buffer""" + return len(self.buffer) diff --git a/src/software/thunderscope/gl/layers/gl_cost_vis_layer.py b/src/software/thunderscope/gl/layers/gl_cost_vis_layer.py index 9a0c06b2e7..633361e228 100644 --- a/src/software/thunderscope/gl/layers/gl_cost_vis_layer.py +++ b/src/software/thunderscope/gl/layers/gl_cost_vis_layer.py @@ -4,7 +4,6 @@ import pyqtgraph as pg import time -import queue import numpy as np from proto.world_pb2 import World @@ -101,11 +100,7 @@ def refresh_graphics(self) -> None: """Update graphics in this layer""" self.cached_world = self.world_buffer.get(block=False) field = self.cached_world.field - - try: - cost_vis = self.cost_visualization_buffer.queue.get_nowait() - except queue.Empty: - cost_vis = None + cost_vis = self.cost_visualization_buffer.get(block=False, return_cached=False) self.cost_vis_overlay_layer.refresh_graphics() diff --git a/src/software/thunderscope/gl/layers/gl_passing_layer.py b/src/software/thunderscope/gl/layers/gl_passing_layer.py index a8966f781b..0c40c57d18 100644 --- a/src/software/thunderscope/gl/layers/gl_passing_layer.py +++ b/src/software/thunderscope/gl/layers/gl_passing_layer.py @@ -1,7 +1,6 @@ from pyqtgraph.opengl import * import time -import queue from proto.visualization_pb2 import PassVisualization @@ -41,10 +40,7 @@ def __init__(self, name: str, buffer_size: int = 5) -> None: def refresh_graphics(self) -> None: """Update graphics in this layer""" - try: - pass_vis = self.pass_visualization_buffer.queue.get_nowait() - except queue.Empty: - pass_vis = None + pass_vis = self.pass_visualization_buffer.get(block=False, return_cached=False) if not pass_vis: pass_vis = self.cached_pass_vis diff --git a/src/software/thunderscope/gl/layers/gl_tactic_layer.py b/src/software/thunderscope/gl/layers/gl_tactic_layer.py index a7b5f3c671..a006ea0d7e 100644 --- a/src/software/thunderscope/gl/layers/gl_tactic_layer.py +++ b/src/software/thunderscope/gl/layers/gl_tactic_layer.py @@ -40,7 +40,7 @@ def __init__(self, name: str, buffer_size: int = 5) -> None: def refresh_graphics(self) -> None: """Update graphics in this layer""" self.cached_world = self.world_buffer.get(block=False) - play_info = self.play_info_buffer.get(block=False).bounds() + play_info = self.play_info_buffer.get(block=False) self.__update_tactic_name_graphics(self.cached_world.friendly_team, play_info) @@ -50,7 +50,7 @@ def __update_tactic_name_graphics(self, team: Team, play_info) -> None: :param team: The team proto :param play_info: The dictionary containing play/tactic info """ - tactic_assignments = play_info.robot_tactic_assignment() + tactic_assignments = play_info.robot_tactic_assignment # Ensure we have the same number of graphics as robots self.tactic_fsm_info_graphics.resize( @@ -68,8 +68,8 @@ def __update_tactic_name_graphics(self, team: Team, play_info) -> None: tactic_fsm_info_graphic.setData( text=textwrap.dedent( f""" - {tactic_assignments[str(robot.id)]["tacticName"]} - - {tactic_assignments[str(robot.id)]["tacticFsmState"]} + {tactic_assignments[robot.id].tactic_name} - + {tactic_assignments[robot.id].tactic_fsm_state} """ ), pos=[ diff --git a/src/software/thunderscope/gl/layers/gl_validation_layer.py b/src/software/thunderscope/gl/layers/gl_validation_layer.py index 41194dedbf..a1d1f571a4 100644 --- a/src/software/thunderscope/gl/layers/gl_validation_layer.py +++ b/src/software/thunderscope/gl/layers/gl_validation_layer.py @@ -101,7 +101,7 @@ def __init__( def refresh_graphics(self) -> None: """Update graphics in this layer""" # Consume the validation set buffer - for _ in range(self.validation_set_buffer.queue.qsize()): + for _ in range(self.validation_set_buffer.size()): self.validation_set = self.validation_set_buffer.get() if self.validation_set.validation_type == ValidationType.ALWAYS: diff --git a/src/software/thunderscope/log/g3log_widget.py b/src/software/thunderscope/log/g3log_widget.py index 447d7acb39..a6f155513b 100644 --- a/src/software/thunderscope/log/g3log_widget.py +++ b/src/software/thunderscope/log/g3log_widget.py @@ -1,5 +1,4 @@ from pyqtgraph.Qt.QtWidgets import * -import queue from software.py_constants import * import pyqtgraph.console as pg_console from proto.robot_log_msg_pb2 import RobotLog, LogLevel @@ -63,9 +62,9 @@ def __init__(self, buffer_size: int = 10): def refresh(self) -> None: """Update the log widget with another log message""" # Need to make sure the message is new before logging it - try: - log = self.log_buffer.queue.get_nowait() - except queue.Empty: + + log = self.log_buffer.get(block=False, return_cached=False) + if log is None: return # Checks whether this type of log is enabled from checkboxes diff --git a/src/software/thunderscope/thread_safe_buffer.py b/src/software/thunderscope/thread_safe_buffer.py index 94d37a1299..809c03797a 100644 --- a/src/software/thunderscope/thread_safe_buffer.py +++ b/src/software/thunderscope/thread_safe_buffer.py @@ -26,15 +26,17 @@ def __init__( :param buffer size: The size of the buffer. :param protobuf_type: To buffer - :param log_overrun: False + :param log_overrun: If True, warns when we lose protos during `put` operations """ self.logger = create_logger(protobuf_type.DESCRIPTOR.name + " Buffer") - self.queue = queue.Queue(buffer_size) + self.buffer = queue.Queue(buffer_size) self.protobuf_type = protobuf_type self.log_overrun = log_overrun self.cached_msg = protobuf_type() self.protos_dropped = 0 self.last_logged_protos_dropped = 0 + self.empty_exception = queue.Empty + self.full_exception = queue.Full def get( self, block: bool = False, timeout: float = None, return_cached: bool = True @@ -56,7 +58,7 @@ def get( comes through, or returned the cached msg if return_cached = True :param timeout: If block is True, then wait for this many seconds before throwing an error or returning cached - :param return_cached: If queue is empty, decides whether to + :param return_cached: If buffer is empty, decides whether to return cached value (True) or return None / throw an error (false) :return: protobuf (cached if block is False and there is no data @@ -76,14 +78,14 @@ def get( if block: try: - self.cached_msg = self.queue.get(timeout=timeout) - except queue.Empty as empty: + self.cached_msg = self.buffer.get(timeout=timeout) + except self.empty_exception as empty: if not return_cached: raise empty else: try: - self.cached_msg = self.queue.get_nowait() - except queue.Empty: + self.cached_msg = self.buffer.get_nowait() + except self.empty_exception: if not return_cached: return None @@ -98,10 +100,14 @@ def put(self, proto: Message, block: bool = False, timeout: float = None) -> Non :param timeout: If block is True, then wait for this many seconds """ if block: - self.queue.put(proto, block, timeout) + self.buffer.put(proto, block, timeout) return try: - self.queue.put_nowait(proto) - except queue.Full: + self.buffer.put_nowait(proto) + except self.full_exception: self.protos_dropped += 1 + + def size(self) -> int: + """Returns the number of objects in the buffer""" + return self.buffer.qsize() diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 951780cbb7..9a10c166f4 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -455,7 +455,7 @@ def __ticker(tick_rate_ms: int) -> None: run_sudo=args.sudo, running_in_realtime=(not args.ci_mode), ) as yellow_fs, Gamecontroller( - suppress_logs=(not args.verbose) + suppress_logs=(not args.verbose), ) as gamecontroller, ( # Here we only initialize autoref if the --enable_autoref flag is requested. # To avoid nested Python withs, the autoref is initialized as None when this flag doesn't exist. @@ -485,9 +485,14 @@ def __ticker(tick_rate_ms: int) -> None: autoref_proto_unix_io, ) gamecontroller.setup_proto_unix_io( - tscope.proto_unix_io_map[ProtoUnixIOTypes.BLUE], - tscope.proto_unix_io_map[ProtoUnixIOTypes.YELLOW], - autoref_proto_unix_io, + blue_full_system_proto_unix_io=tscope.proto_unix_io_map[ + ProtoUnixIOTypes.BLUE + ], + yellow_full_system_proto_unix_io=tscope.proto_unix_io_map[ + ProtoUnixIOTypes.YELLOW + ], + autoref_proto_unix_io=autoref_proto_unix_io, + simulator_proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.SIM], ) if args.enable_autoref: autoref.setup_ssl_wrapper_packets( diff --git a/src/software/util/make_enum/make_enum.hpp b/src/software/util/make_enum/make_enum.hpp index 39cb6eb431..8b26be8bc2 100644 --- a/src/software/util/make_enum/make_enum.hpp +++ b/src/software/util/make_enum/make_enum.hpp @@ -70,59 +70,59 @@ namespace reflective_enum { - /** - * Type trait that checks whether T is a reflective enum, i.e. whether it - * was created using the MAKE_ENUM macro. - */ - template - struct is_reflective_enum : std::false_type - { - }; +/** + * Type trait that checks whether T is a reflective enum, i.e. whether it + * was created using the MAKE_ENUM macro. + */ +template +struct is_reflective_enum : std::false_type +{ +}; - /** - * Returns the number of values in the reflective enum E. - * - * @return the number of values in the enum - */ - template - constexpr size_t size(); +/** + * Returns the number of values in the reflective enum E. + * + * @return the number of values in the enum + */ +template +constexpr size_t size(); - /** - * Returns an array containing the values of the reflective enum E. - * - * @return an array with the values of the enum - */ - template - constexpr auto values(); +/** + * Returns an array containing the values of the reflective enum E. + * + * @return an array with the values of the enum + */ +template +constexpr auto values(); - /** - * Returns an array contain the string representations of each value - * in the reflective enum E. - * - * @return an array with the names of the values in the enum - */ - template - constexpr auto valueNames(); +/** + * Returns an array contain the string representations of each value + * in the reflective enum E. + * + * @return an array with the names of the values in the enum + */ +template +constexpr auto valueNames(); - /** - * Returns the enum value with the given string representation. - * - * @param value_name the string representation of the enum value - * - * @return the enum value - */ - template - constexpr E fromName(const std::string value_name) +/** + * Returns the enum value with the given string representation. + * + * @param value_name the string representation of the enum value + * + * @return the enum value + */ +template +constexpr E fromName(const std::string value_name) +{ + constexpr size_t enum_size = size(); + constexpr auto enum_value_names = valueNames(); + for (size_t i = 0; i < enum_size; ++i) { - constexpr size_t enum_size = size(); - constexpr auto enum_value_names = valueNames(); - for (size_t i = 0; i < enum_size; ++i) + if (enum_value_names.at(i) == value_name) { - if (enum_value_names.at(i) == value_name) - { - return static_cast(i); - } + return static_cast(i); } - throw std::invalid_argument(value_name + " cannot be converted to enum value"); } + throw std::invalid_argument(value_name + " cannot be converted to enum value"); +} } // namespace reflective_enum diff --git a/src/software/world/field.h b/src/software/world/field.h index 9c3c0945f9..e9253dd5be 100644 --- a/src/software/world/field.h +++ b/src/software/world/field.h @@ -463,23 +463,22 @@ class Field namespace std { - // Implements the std::less function so Field can be used as the key in data - // structures, such as std::map. See: - // https://stackoverflow.com/questions/42762633/why-is-stdless-better-than and - // https://en.cppreference.com/w/cpp/utility/functional/less - template <> - struct less +// Implements the std::less function so Field can be used as the key in data +// structures, such as std::map. See: +// https://stackoverflow.com/questions/42762633/why-is-stdless-better-than and +// https://en.cppreference.com/w/cpp/utility/functional/less +template <> +struct less +{ + bool operator()(const Field &lhs, const Field &rhs) const { - bool operator()(const Field &lhs, const Field &rhs) const - { - return lhs.friendlyDefenseArea().halfPerimeter() + - lhs.enemyDefenseArea().halfPerimeter() + - lhs.fieldLines().halfPerimeter() + - lhs.fieldBoundary().halfPerimeter() < - rhs.friendlyDefenseArea().halfPerimeter() + - rhs.enemyDefenseArea().halfPerimeter() + - rhs.fieldLines().halfPerimeter() + - rhs.fieldBoundary().halfPerimeter(); - } - }; + return lhs.friendlyDefenseArea().halfPerimeter() + + lhs.enemyDefenseArea().halfPerimeter() + + lhs.fieldLines().halfPerimeter() + + lhs.fieldBoundary().halfPerimeter() < + rhs.friendlyDefenseArea().halfPerimeter() + + rhs.enemyDefenseArea().halfPerimeter() + + rhs.fieldLines().halfPerimeter() + rhs.fieldBoundary().halfPerimeter(); + } +}; } // namespace std