diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..b92cd58 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,20 @@ +FROM ubuntu:22.04 + +RUN apt-get update && export DEBIAN_FRONTEND=noninteractive \ + && apt-get -y install --no-install-recommends \ + libqt5opengl5-dev \ + qtbase5-dev \ + build-essential \ + freeglut3-dev \ + protobuf-compiler \ + libprotobuf-dev + +COPY . /opt/vssreferee + +WORKDIR /opt/vssreferee/build + +RUN qmake .. && make -j4 + +WORKDIR /opt/vssreferee/bin + +CMD ["./VSSReferee","--3v3","--record","false"] \ No newline at end of file diff --git a/VSSReferee.pro b/VSSReferee.pro index 7315381..be9cf40 100644 --- a/VSSReferee.pro +++ b/VSSReferee.pro @@ -42,6 +42,9 @@ SOURCES += \ include/vssref_command.pb.cc \ include/vssref_common.pb.cc \ include/vssref_placement.pb.cc \ + include/messages_robocup_ssl_detection.pb.cc \ + include/messages_robocup_ssl_geometry.pb.cc \ + include/wrapper.pb.cc \ main.cpp \ src/constants/constants.cpp \ src/recorder/recorder.cpp \ @@ -90,6 +93,9 @@ HEADERS += \ include/vssref_command.pb.h \ include/vssref_common.pb.h \ include/vssref_placement.pb.h \ + include/messages_robocup_ssl_detection.pb.h \ + include/messages_robocup_ssl_geometry.pb.h \ + include/wrapper.pb.h \ src/constants/constants.h \ src/recorder/recorder.h \ src/refereecore.h \ diff --git a/docker_run b/docker_run new file mode 100755 index 0000000..6a3c2a7 --- /dev/null +++ b/docker_run @@ -0,0 +1,5 @@ +#!/bin/bash + +xhost + +docker run -e DISPLAY=$DISPLAY --network=host -it vssreferee +xhost - diff --git a/include/proto/messages_robocup_ssl_detection.proto b/include/proto/messages_robocup_ssl_detection.proto new file mode 100644 index 0000000..918748c --- /dev/null +++ b/include/proto/messages_robocup_ssl_detection.proto @@ -0,0 +1,32 @@ +syntax = "proto2"; + +message SSL_DetectionBall { + required float confidence = 1; + optional uint32 area = 2; + required float x = 3; + required float y = 4; + optional float z = 5; + required float pixel_x = 6; + required float pixel_y = 7; +} + +message SSL_DetectionRobot { + required float confidence = 1; + optional uint32 robot_id = 2; + required float x = 3; + required float y = 4; + optional float orientation = 5; + required float pixel_x = 6; + required float pixel_y = 7; + optional float height = 8; +} + +message SSL_DetectionFrame { + required uint32 frame_number = 1; + required double t_capture = 2; + required double t_sent = 3; + required uint32 camera_id = 4; + repeated SSL_DetectionBall balls = 5; + repeated SSL_DetectionRobot robots_yellow = 6; + repeated SSL_DetectionRobot robots_blue = 7; +} diff --git a/include/proto/messages_robocup_ssl_geometry.proto b/include/proto/messages_robocup_ssl_geometry.proto new file mode 100644 index 0000000..0138ab1 --- /dev/null +++ b/include/proto/messages_robocup_ssl_geometry.proto @@ -0,0 +1,151 @@ +syntax = "proto2"; +// A 2D float vector. +message Vector2f { + // X-coordinate in mm + required float x = 1; + // Y-coordinate in mm + required float y = 2; +} + +// Represents a field marking as a line segment represented by a start point p1, +// and end point p2, and a line thickness. The start and end points are along +// the center of the line, so the thickness of the line extends by thickness / 2 +// on either side of the line. +message SSL_FieldLineSegment { + // Name of this field marking. + required string name = 1; + // Start point of the line segment. + required Vector2f p1 = 2; + // End point of the line segment. + required Vector2f p2 = 3; + // Thickness of the line segment. + required float thickness = 4; + // The type of this shape + optional SSL_FieldShapeType type = 5; +} + +// Represents a field marking as a circular arc segment represented by center point, a +// start angle, an end angle, and an arc thickness. +message SSL_FieldCircularArc { + // Name of this field marking. + required string name = 1; + // Center point of the circular arc. + required Vector2f center = 2; + // Radius of the arc. + required float radius = 3; + // Start angle in counter-clockwise order. + required float a1 = 4; + // End angle in counter-clockwise order. + required float a2 = 5; + // Thickness of the arc. + required float thickness = 6; + // The type of this shape + optional SSL_FieldShapeType type = 7; +} + +message SSL_GeometryFieldSize { + // Field length (distance between goal lines) in mm + required int32 field_length = 1; + // Field width (distance between touch lines) in mm + required int32 field_width = 2; + // Goal width (distance between inner edges of goal posts) in mm + required int32 goal_width = 3; + // Goal depth (distance from outer goal line edge to inner goal back) in mm + required int32 goal_depth = 4; + // Boundary width (distance from touch/goal line centers to boundary walls) in mm + required int32 boundary_width = 5; + // Generated line segments based on the other parameters + repeated SSL_FieldLineSegment field_lines = 6; + // Generated circular arcs based on the other parameters + repeated SSL_FieldCircularArc field_arcs = 7; + // Depth of the penalty/defense area (measured between line centers) in mm + optional int32 penalty_area_depth = 8; + // Width of the penalty/defense area (measured between line centers) in mm + optional int32 penalty_area_width = 9; + // Radius of the center circle (measured between line centers) in mm + optional int32 center_circle_radius = 10; + // Thickness/width of the lines on the field in mm + optional int32 line_thickness = 11; + // Distance between the goal center and the center of the penalty mark in mm + optional int32 goal_center_to_penalty_mark = 12; + // Goal height in mm + optional int32 goal_height = 13; + // Ball radius in mm (note that this is a float type to represent sub-mm precision) + optional float ball_radius = 14; + // Max allowed robot radius in mm (note that this is a float type to represent sub-mm precision) + optional float max_robot_radius = 15; +} + +message SSL_GeometryCameraCalibration { + required uint32 camera_id = 1; + required float focal_length = 2; + required float principal_point_x = 3; + required float principal_point_y = 4; + required float distortion = 5; + required float q0 = 6; + required float q1 = 7; + required float q2 = 8; + required float q3 = 9; + required float tx = 10; + required float ty = 11; + required float tz = 12; + optional float derived_camera_world_tx = 13; + optional float derived_camera_world_ty = 14; + optional float derived_camera_world_tz = 15; + optional uint32 pixel_image_width = 16; + optional uint32 pixel_image_height = 17; +} + +// Two-Phase model for straight-kicked balls. +// There are two phases with different accelerations during the ball kicks: +// 1. Sliding +// 2. Rolling +// The full model is described in the TDP of ER-Force from 2016, which can be found here: +// https://ssl.robocup.org/wp-content/uploads/2019/01/2016_ETDP_ER-Force.pdf +message SSL_BallModelStraightTwoPhase { + // Ball sliding acceleration [m/s^2] (should be negative) + required double acc_slide = 1; + // Ball rolling acceleration [m/s^2] (should be negative) + required double acc_roll = 2; + // Fraction of the initial velocity where the ball starts to roll + required double k_switch = 3; +} + +// Fixed-Loss model for chipped balls. +// Uses fixed damping factors for xy and z direction per hop. +message SSL_BallModelChipFixedLoss { + // Chip kick velocity damping factor in XY direction for the first hop + required double damping_xy_first_hop = 1; + // Chip kick velocity damping factor in XY direction for all following hops + required double damping_xy_other_hops = 2; + // Chip kick velocity damping factor in Z direction for all hops + required double damping_z = 3; +} + +message SSL_GeometryModels { + optional SSL_BallModelStraightTwoPhase straight_two_phase = 1; + optional SSL_BallModelChipFixedLoss chip_fixed_loss = 2; +} + +message SSL_GeometryData { + required SSL_GeometryFieldSize field = 1; + repeated SSL_GeometryCameraCalibration calib = 2; + optional SSL_GeometryModels models = 3; +} + +enum SSL_FieldShapeType { + Undefined = 0; + CenterCircle = 1; + TopTouchLine = 2; + BottomTouchLine = 3; + LeftGoalLine = 4; + RightGoalLine = 5; + HalfwayLine = 6; + CenterLine = 7; + LeftPenaltyStretch = 8; + RightPenaltyStretch = 9; + LeftFieldLeftPenaltyStretch = 10; + LeftFieldRightPenaltyStretch = 11; + RightFieldLeftPenaltyStretch = 12; + RightFieldRightPenaltyStretch = 13; +} \ No newline at end of file diff --git a/include/proto/wrapper.proto b/include/proto/wrapper.proto new file mode 100644 index 0000000..4b4e3e2 --- /dev/null +++ b/include/proto/wrapper.proto @@ -0,0 +1,8 @@ +syntax = "proto2"; +import "messages_robocup_ssl_detection.proto"; +import "messages_robocup_ssl_geometry.proto"; + +message SSL_WrapperPacket { + optional SSL_DetectionFrame detection = 1; + optional SSL_GeometryData geometry = 2; +} diff --git a/main.cpp b/main.cpp index 9bb900d..fba84ef 100644 --- a/main.cpp +++ b/main.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include @@ -71,7 +72,7 @@ int main(int argc, char *argv[]) } else if(parser.value(record).toLower() == "true") { // Allocate recorder - QString logFileName = PROJECT_PATH + QString("/logs/") + Timer::getActualTime() + QString("|%1 - %2_%3").arg(constants->gameType()).arg(constants->blueTeamName()).arg(constants->yellowTeamName()) + ".log"; + QString logFileName = PROJECT_PATH + QString("/logs/") + Timer::getActualTime() + QString("|%1 - %2_%3").arg(constants->gameType()).arg("Blue Team").arg("Yellow Team") + ".log"; recorder = new Recorder(logFileName, constants->visionAddress(), constants->visionPort(), constants->refereeAddress(), constants->refereePort()); } } diff --git a/src/constants/constants.cpp b/src/constants/constants.cpp index 9899be7..27a9a13 100644 --- a/src/constants/constants.cpp +++ b/src/constants/constants.cpp @@ -111,6 +111,15 @@ void Constants::readVisionConstants() { _visionPort = visionMap["visionPort"].toUInt(); std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded visionPort: " + std::to_string(_visionPort)) + '\n'; + + _firaVisionAddress = visionMap["firaVisionAddress"].toString(); + std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded firaVisionAddress: '" + _firaVisionAddress.toStdString() + "'\n"); + + _firaVisionPort = visionMap["firaVisionPort"].toUInt(); + std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded firaVisionPort: " + std::to_string(_firaVisionPort)) + '\n'; + + _isFIRAVision = visionMap["isFIRAVision"].toUInt(); + std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded isFIRAVision: " + std::to_string(_isFIRAVision)) + '\n'; // Filter constants QVariantMap filterMap = visionMap["filters"].toMap(); @@ -150,16 +159,21 @@ void Constants::readReplacerConstants() { void Constants::readTeamConstants() { // Taking team mapping in json QVariantMap teamMap = documentMap()["Team"].toMap(); + QVariantList teams = teamMap["teams"].toList(); // Filling vars _qtPlayers = teamMap["qtPlayers"].toInt(); std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded qtPlayers: " + std::to_string(_qtPlayers)) + '\n'; - _blueTeamName = teamMap["blueTeamName"].toString(); - std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded blueTeamName: '" + _blueTeamName.toStdString() + "'\n"); + _teams.clear(); - _yellowTeamName = teamMap["yellowTeamName"].toString(); - std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded yellowTeamName: '" + _yellowTeamName.toStdString() + "'\n"); + foreach (const QVariant &t, teams) { + if (t.canConvert()) { + QString teamName = t.toString(); + _teams.append(teamName); + std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded team: " + teamName.toStdString()) + '\n'; + } + } _blueIsLeftSide = teamMap["blueIsLeftSide"].toInt(); std::cout << Text::purple("[CONSTANTS] ", true) << Text::bold("Loaded blueIsLeftSide: " + ((_blueIsLeftSide) ? QString("true").toStdString() : QString("false").toStdString()) + '\n'); @@ -253,6 +267,18 @@ quint16 Constants::visionPort() { return _visionPort; } +QString Constants::firaVisionAddress() { + return _firaVisionAddress; +} + +quint16 Constants::firaVisionPort() { + return _firaVisionPort; +} + +bool Constants::isFIRAVision() { + return _isFIRAVision; +} + bool Constants::useKalman() { return _useKalman; } @@ -265,6 +291,10 @@ int Constants::lossTime() { return _lossTime; } +QStringList Constants::teams() { + return _teams; +} + QString Constants::replacerAddress() { return _replacerAddress; } @@ -285,20 +315,11 @@ int Constants::qtPlayers() { return _qtPlayers; } -QString Constants::blueTeamName() { - return _blueTeamName; -} - -QString Constants::yellowTeamName() { - return _yellowTeamName; -} - bool Constants::blueIsLeftSide() { return _blueIsLeftSide; } void Constants::swapSides() { - std::swap(_blueTeamName, _yellowTeamName); _blueIsLeftSide = !_blueIsLeftSide; } @@ -309,3 +330,29 @@ QString Constants::getHID() { QString Constants::getHToken() { return _hToken; } + + + +int Constants::varsPerTeam() { + QString varsPerTeam = _documentMap["Referee"].toMap()["game"].toMap()["varPerTeam"].toString(); + bool converted = false; + int varsPerTeamInt = varsPerTeam.toInt(&converted); + + return varsPerTeamInt; +} + +int Constants::timeoutsPerTeam() { + QString timeoutsPerTeam = _documentMap["Referee"].toMap()["game"].toMap()["timeoutsPerTeam"].toString(); + bool converted = false; + int timeoutsPerTeamInt = timeoutsPerTeam.toInt(&converted); + + return timeoutsPerTeamInt; +} + +float Constants::timeoutLength() { + QString timeoutLength = _documentMap["Referee"].toMap()["game"].toMap()["timeoutLength"].toString(); + bool converted = false; + float timeoutLengthFloat = timeoutLength.toFloat(&converted); + + return timeoutLengthFloat; +} diff --git a/src/constants/constants.h b/src/constants/constants.h index ff0ecf7..0e6771f 100644 --- a/src/constants/constants.h +++ b/src/constants/constants.h @@ -40,6 +40,11 @@ class Constants // Vision constants getters QString visionAddress(); quint16 visionPort(); + QString firaVisionAddress(); + quint16 firaVisionPort(); + + bool isFIRAVision(); + bool useKalman(); int noiseTime(); int lossTime(); @@ -52,15 +57,19 @@ class Constants // Teams constants getters int qtPlayers(); - QString blueTeamName(); - QString yellowTeamName(); bool blueIsLeftSide(); void swapSides(); + QStringList teams(); // Discord webhook token QString getHToken(); QString getHID(); + int timeoutsPerTeam(); + float timeoutLength(); + int varsPerTeam(); + + protected: QVariantMap documentMap() { return _documentMap; } @@ -102,6 +111,9 @@ class Constants // Vision constants QString _visionAddress; quint16 _visionPort; + QString _firaVisionAddress; + quint16 _firaVisionPort; + bool _isFIRAVision; bool _useKalman; int _noiseTime; int _lossTime; @@ -116,8 +128,7 @@ class Constants // Teams constants int _qtPlayers; - QString _blueTeamName; - QString _yellowTeamName; + QStringList _teams; bool _blueIsLeftSide; void readTeamConstants(); diff --git a/src/constants/constants.json b/src/constants/constants.json index 0653446..458408d 100644 --- a/src/constants/constants.json +++ b/src/constants/constants.json @@ -4,8 +4,11 @@ }, "Vision":{ - "visionAddress": "224.0.0.1", - "visionPort": 10002, + "isFIRAVision": true, + "visionAddress": "224.5.23.2", + "visionPort": 10015, + "firaVisionAddress": "224.0.0.1", + "firaVisionPort": 10002, "filters":{ "useKalman": false, "noiseTime": 100, @@ -25,25 +28,32 @@ }, "Team":{ - "qtPlayers": 5, - "blueTeamName": "Team Blue", - "yellowTeamName": "Team Yellow", + "qtPlayers": 20, + "teams": ["None", "RoboCIn", "ITAndroids", "RobotBulls", "Maracatronics", "Rodetas" + ,"RoboTech", "RoboIME", "Rinobot", "PinguimBots", "Pequi Mecanico" + ,"GER", "FURGBot", "UnBall", "Poti", "Warthog", "Nhao","Ararabots" + ,"Neon", "UaiSoccer", "Troia", "Yapira", "ThunderVolt", "RedDragons" + ,"UFRBots", "UTBots", "LambeSujo", "Carrossel", "BahiaRT", "CIMAT-UAZ" + ,"Trevinho", "HumaMaquina", "Drumonsters", "BDP", "UERJ"], "blueIsLeftSide": true }, "Referee":{ "refereeAddress": "224.5.23.2", "refereePort": 10003, - "transitionTime": 4.0, + "transitionTime": 8.0, "game":{ "gameType": "Group_Phase", "ballRadius": 0.0215, "robotLength": 0.075, "halfTime": 300.0, - "overtimeHalfTime": 180.0 + "overtimeHalfTime": 180.0, + "timeoutsPerTeam": 2, + "timeoutLength": 120.0, + "varPerTeam": 4 }, "fouls":{ - "useRefereeSuggestions": false, + "useRefereeSuggestions": true, "maintainSpeedAtSuggestions": true, "ballMinSpeedForStuck": 0.1, "stuckedBallTime": 5.0, diff --git a/src/refereecore.cpp b/src/refereecore.cpp index 5ce4b01..8b2c3a1 100644 --- a/src/refereecore.cpp +++ b/src/refereecore.cpp @@ -63,6 +63,8 @@ void RefereeCore::start() { QObject::connect(_referee, SIGNAL(sendTimestamp(float, float, VSSRef::Half, bool)), _soccerView, SLOT(takeTimeStamp(float, float, VSSRef::Half, bool))); QObject::connect(_soccerView, SIGNAL(sendManualFoul(VSSRef::Foul, VSSRef::Color, VSSRef::Quadrant, bool)), _referee, SLOT(takeManualFoul(VSSRef::Foul, VSSRef::Color, VSSRef::Quadrant, bool))); QObject::connect(_vision, SIGNAL(visionUpdated()), _soccerView->getFieldView(), SLOT(updateField())); + QObject::connect(_soccerView, SIGNAL(changeVision(bool)), _vision, SLOT(visionPacketChanged(bool))); + QObject::connect(_soccerView, SIGNAL(changeVision(bool)), _referee, SLOT(visionPacketChanged(bool))); // Show GUI _soccerView->show(); diff --git a/src/soccerview/soccerview.cpp b/src/soccerview/soccerview.cpp index 6ce6996..e2ca0b0 100644 --- a/src/soccerview/soccerview.cpp +++ b/src/soccerview/soccerview.cpp @@ -15,6 +15,7 @@ SoccerView::SoccerView(Constants *constants, QWidget *parent) : // Setup UI ui->setupUi(this); setDarkTheme(); + setTeams(); setupTeams(); setupGoals(); setupButtons(); @@ -55,17 +56,20 @@ int SoccerView::getRightTeamGoals() { return _rightTeamGoals; } -void SoccerView::setupTeams() { - QString leftName, rightName; +void SoccerView::setTeams() { - leftName = getConstants()->blueIsLeftSide() ? getConstants()->blueTeamName() : getConstants()->yellowTeamName(); - rightName = getConstants()->blueIsLeftSide() ? getConstants()->yellowTeamName() : getConstants()->blueTeamName(); + _teams = getConstants()->teams(); + for(auto a : _teams) { + ui->leftTeamBox->addItem(a); + ui->rightTeamBox->addItem(a); + } +} - // Set names - ui->leftTeamName->setText(leftName); - ui->leftTeamName->setStyleSheet(getConstants()->blueIsLeftSide() ? "color: #779FFF;" : "color: #FCEE44;"); - ui->rightTeamName->setText(rightName); - ui->rightTeamName->setStyleSheet(getConstants()->blueIsLeftSide() ? "color: #FCEE44;" : "color: #779FFF;"); +void SoccerView::setupTeams() { + QString leftName, rightName; + + leftName = ui->leftTeamBox->currentText(); + rightName = ui->rightTeamBox->currentText(); // Remove spaces to take logo leftName.remove(" "); @@ -95,6 +99,13 @@ void SoccerView::setupTeams() { _leftTeamGoals = 0; _rightTeamGoals = 0; setupGoals(); + + // Set initial timeouts and vars per team + _leftTeamVars = _rightTeamVars = getConstants()->varsPerTeam(); + _leftTeamTimeouts = _rightTeamTimeouts = getConstants()->timeoutsPerTeam(); + + // Setup timeout + _timeoutSet = false; } void SoccerView::setupGoals() { @@ -105,9 +116,25 @@ void SoccerView::setupGoals() { ui->leftTeamGoals->setText(QString("%1").arg(leftGoal)); ui->rightTeamGoals->setText(QString("%1").arg(rightGoal)); + + std::cout << Text::blue("[SOCCERVIEW] ", true) + Text::red("Goals set to ", true) + Text::bold(QString("%1 x %2").arg(_leftTeamGoals).arg(_rightTeamGoals).toStdString()) + '\n'; } void SoccerView::setupButtons() { + + // Connecting vision buttons + connect(ui->SSLVision, &QPushButton::released, [this](){ + ui->SSLVision->setChecked(true); + ui->FIRAVision->setChecked(false); + emit changeVision(false); + }); + + connect(ui->FIRAVision, &QPushButton::released, [this](){ + ui->SSLVision->setChecked(false); + ui->FIRAVision->setChecked(true); + emit changeVision(true); + }); + // Setup buttons mapper _buttonsMapper = new QSignalMapper(); @@ -167,6 +194,19 @@ void SoccerView::setupButtons() { emit addTime(1); }); + // Connecting teleport button to lambda + connect(ui->enableTeleport, &QPushButton::released, [this](){ + ui->enableTeleport->setChecked(true); + ui->disableTeleport->setChecked(false); + emit sendTeleport(true); + }); + + connect(ui->disableTeleport, &QPushButton::released, [this](){ + ui->enableTeleport->setChecked(false); + ui->disableTeleport->setChecked(true); + emit sendTeleport(false); + }); + // Connecting command buttons that need to place outside connect(ui->penaltyShootout, &QPushButton::released, [this](){ VSSRef::Color calledColor = VSSRef::Color(); @@ -191,10 +231,12 @@ void SoccerView::setupButtons() { connect(_buttonsMapper, SIGNAL(mapped(QWidget *)), this, SLOT(processButton(QWidget *)), Qt::UniqueConnection); } + std::cout << Text::blue("[SOCCERVIEW] ", true) + Text::red("Modules connection done.", true) + '\n'; + // Connect Send To Discord button connect(ui->sendToDiscord, &QPushButton::released, [this](){ - QString leftTeamName = getConstants()->blueTeamName(); - QString rightTeamName = getConstants()->yellowTeamName(); + QString leftTeamName = ui->leftTeamBox->currentText(); + QString rightTeamName = ui->rightTeamBox->currentText(); QString leftTeamScore = QString("%1").arg(getLeftTeamGoals()); QString rightTeamScore = QString("%1").arg(getRightTeamGoals()); @@ -205,6 +247,40 @@ void SoccerView::setupButtons() { system(cmd.toStdString().c_str()); ui->sendToDiscord->setEnabled(false); }); + connect(ui->leftTeamBox, QOverload::of(&QComboBox::currentIndexChanged), [this](int index){ + setupTeams(); + }); + connect(ui->rightTeamBox, QOverload::of(&QComboBox::currentIndexChanged), [this](int index){ + setupTeams(); + }); + + // var + connect(ui->var, &QPushButton::released, [this]() { + emit sendManualFoul(VSSRef::Foul::STOP, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); + bool hasTeamBlue = ui->forBlue->isChecked(); + + QString leftTeamName = ui->leftTeamBox->currentText(); + QString rightTeamName = ui->rightTeamBox->currentText(); + + QString forBlue = QString("%1").arg(leftTeamName); + QString forYellow = QString("%1").arg(rightTeamName); + + ui->statusColor->setText(QString("Game is stopped due to VAR for %2.").arg(hasTeamBlue ? forBlue : forYellow)); + }); + + //timeout + connect(ui->timeout, &QPushButton::released, [this]() { + emit sendManualFoul(VSSRef::Foul::STOP, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); + bool hasTeamBlue = ui->forBlue->isChecked(); + + QString leftTeamName = ui->leftTeamBox->currentText(); + QString rightTeamName = ui->rightTeamBox->currentText(); + + QString forBlue = QString("%1").arg(leftTeamName); + QString forYellow = QString("%1").arg(rightTeamName); + + ui->statusColor->setText(QString("Game is stopped due to TIMEOUT for %2.").arg(hasTeamBlue ? forBlue : forYellow)); + }); } void SoccerView::animateWidget(QWidget *widget, QColor desiredColor, int animationTime) { @@ -328,8 +404,8 @@ void SoccerView::takeFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Qu else { desiredColor = QColor(238, 0, 34, 255); if(foulColor != VSSRef::Color::NONE) { - QString forBlue = QString("%1").arg(getConstants()->blueTeamName()); - QString forYellow = QString("%1").arg(getConstants()->yellowTeamName()); + QString forBlue = QString("%1").arg(getConstants()->blueIsLeftSide() ? ui->leftTeamBox->currentText() : ui->rightTeamBox->currentText()); + QString forYellow = QString("%1").arg(getConstants()->blueIsLeftSide() ? ui->rightTeamBox->currentText() : ui->leftTeamBox->currentText()); ui->statusColor->setText(QString("%1 for %2").arg(VSSRef::Foul_Name(foul).c_str()).arg(foulColor == VSSRef::Color::BLUE ? forBlue : forYellow)); } @@ -343,7 +419,7 @@ void SoccerView::takeFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Qu // Animate statusboard animateWidget(ui->statusColor, desiredColor, 500); - + // Animate timer // Taking desired color for timer if(foul == VSSRef::Foul::GAME_ON){ @@ -374,6 +450,24 @@ void SoccerView::takeTimeStamp(float halftime, float timestamp, VSSRef::Half hal return ; } + // Check if timeout is set + if(_timeoutSet) { + halftime = getConstants()->timeoutLength(); + timestamp = _timeoutTimestamp; + + _timeoutTimestamp += _timeoutTimer.getSeconds(); + _timeoutTimer.start(); + + if(_timeoutTimestamp >= halftime) { + _timeoutTimestamp = halftime; + + // Disable timeout + _timeoutSet = false; + emit sendManualFoul(VSSRef::Foul::HALT, VSSRef::Color::NONE, VSSRef::NO_QUADRANT); + } + } + + int min = (halftime - timestamp) / 60.0; int sec = (halftime - timestamp) - (min * 60.0); @@ -455,7 +549,7 @@ void SoccerView::processButton(QWidget *button) { void SoccerView::addSuggestion(QString suggestion, VSSRef::Color forColor, VSSRef::Quadrant atQuadrant) { // Animate flag - animateFlag(true, 150); + // animateFlag(true, 150); // Avoid to add if not using ref suggestions if(!getConstants()->useRefereeSuggestions() && !suggestion.contains("Collision")) return; @@ -480,9 +574,12 @@ void SoccerView::addSuggestion(QString suggestion, VSSRef::Color forColor, VSSRe } else if(label->whatsThis() == "GOAL") { addGoal(forColor); - emit sendManualFoul(VSSRef::Foul::KICKOFF, (forColor == VSSRef::Color::BLUE) ? VSSRef::Color::YELLOW : VSSRef::Color::BLUE, VSSRef::Quadrant::NO_QUADRANT); } + else if(label->whatsThis() == "GAME_ON"){ + printf("Estou aqui no GAME ON"); + emit sendManualFoul(VSSRef::Foul::GAME_ON, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); + } else { VSSRef::Foul suggestedFoul = VSSRef::Foul(); VSSRef::Foul_Parse(label->whatsThis().toStdString(), &suggestedFoul); @@ -499,9 +596,7 @@ void SoccerView::addSuggestion(QString suggestion, VSSRef::Color forColor, VSSRe _suggestionsMutex.unlock(); - if(suggestion.contains("Collision")) { - showSuggestions(); - } + showSuggestions(); } void SoccerView::showSuggestions() { diff --git a/src/soccerview/soccerview.h b/src/soccerview/soccerview.h index e6c97e0..d77a586 100644 --- a/src/soccerview/soccerview.h +++ b/src/soccerview/soccerview.h @@ -9,6 +9,7 @@ #include #include + namespace Ui { class SoccerView; } @@ -35,6 +36,19 @@ class SoccerView : public QMainWindow void setupTeams(); void setupGoals(); void setupButtons(); + void setTeams(); + + // Timeouts + int _leftTeamTimeouts; + int _rightTeamTimeouts; + bool _timeoutSet; + float _timeoutTimestamp; + Timer _timeoutTimer; + + // VARs + int _leftTeamVars; + int _rightTeamVars; + // Animations void animateWidget(QWidget *widget, QColor desiredColor, int animationTime); @@ -43,14 +57,15 @@ class SoccerView : public QMainWindow // Mapping buttons QList _buttons; QSignalMapper *_buttonsMapper; + QStringList _teams; // Constants Constants *_constants; Constants* getConstants(); // Goals - int _leftTeamGoals; - int _rightTeamGoals; + int _leftTeamGoals = 0; + int _rightTeamGoals = 0; // Suggestions QList _widgets; @@ -62,6 +77,8 @@ class SoccerView : public QMainWindow void sendCollisionDecision(); void sendManualFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quadrant foulQuadrant, bool isToPlaceOutside = false); void addTime(int seconds); + void sendTeleport(bool teleport); + void changeVision(bool isFIRAvision); public slots: void takeFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quadrant foulQuadrant); diff --git a/src/soccerview/soccerview.ui b/src/soccerview/soccerview.ui index 72de321..b11487c 100644 --- a/src/soccerview/soccerview.ui +++ b/src/soccerview/soccerview.ui @@ -10,7 +10,7 @@ 0 0 1066 - 609 + 693 @@ -26,7 +26,7 @@ 10 180 600 - 420 + 431 @@ -107,7 +107,7 @@ FreeMono - 58 + 50 50 false @@ -140,7 +140,7 @@ FreeMono - 58 + 50 50 false @@ -236,65 +236,41 @@ color: white; Qt::AlignBottom|Qt::AlignHCenter - + - 5 - 27 - 96 - 20 + 238 + 104 + 131 + 21 - - - 10 - 75 - true - - - Default Team - - - Qt::AlignCenter + Send to Discord - + - 509 - 27 - 96 - 20 + 470 + 20 + 141 + 25 - - - 10 - 75 - true - - - - Default Team - - - Qt::AlignCenter - - + - 238 - 104 - 131 - 21 + 0 + 20 + 141 + 25 - - Send to Discord - + rightTeamBox + leftTeamBox statusColor leftTeamLogo rightTeamLogo @@ -302,17 +278,15 @@ color: white; rightTeamGoals gameTime halfTime - leftTeamName - rightTeamName sendToDiscord 630 - 310 + 280 421 - 290 + 331 @@ -721,11 +695,37 @@ color: white; Stop - + 180 230 + 100 + 25 + + + + VAR + + + + + + 305 + 230 + 100 + 25 + + + + Timeout + + + + + + 180 + 290 226 25 @@ -752,9 +752,9 @@ color: white; 630 - 10 + 5 421 - 290 + 261 @@ -764,7 +764,7 @@ color: white; 0 - 20 + 15 421 271 @@ -772,6 +772,120 @@ color: white; + + + + 630 + 620 + 421 + 65 + + + + Teleport + + + + + 110 + 30 + 90 + 25 + + + + ENABLE TELEPORT + + + Enable + + + true + + + false + + + + + + 210 + 30 + 90 + 25 + + + + DISABLE TELEPORT + + + Disable + + + true + + + true + + + + + + + 10 + 620 + 601 + 65 + + + + Vision Packets + + + + + 90 + 30 + 151 + 25 + + + + SSL VISION + + + VSS-Vision + + + true + + + false + + + + + + 360 + 30 + 151 + 25 + + + + FIRA VISION + + + FIRASim + + + true + + + true + + + @@ -800,7 +914,7 @@ color: white; - + diff --git a/src/world/entities/referee/checkers/ballplay/checker_ballplay.cpp b/src/world/entities/referee/checkers/ballplay/checker_ballplay.cpp index 4cc0380..1e93cd8 100644 --- a/src/world/entities/referee/checkers/ballplay/checker_ballplay.cpp +++ b/src/world/entities/referee/checkers/ballplay/checker_ballplay.cpp @@ -96,30 +96,46 @@ void Checker_BallPlay::run() { // Check if not occurred possible penalty or goal kick if(!_possiblePenalty && !_possibleGoalKick) { // Send as valid goal - emit emitGoal(_possibleGoalTeam); + // emit emitGoal(_possibleGoalTeam); // If is penalty shootout if(_isPenaltyShootout) { setNextTeam(); setPenaltiesInfo(VSSRef::Foul::PENALTY_KICK, _penaltyTeam, VSSRef::Quadrant::NO_QUADRANT); - emit foulOccured(); + if(!_ballInside){ + emit emitSuggestion("GOAL", _possibleGoalTeam); + _ballInside = true; + } + // emit foulOccured(); return ; } else { setPenaltiesInfo(VSSRef::Foul::KICKOFF, VSSRef::Color(i), VSSRef::Quadrant::NO_QUADRANT); - emit foulOccured(); + if(!_ballInside){ + emit emitSuggestion("GOAL", _possibleGoalTeam); + _ballInside = true; + } + // emit foulOccured(); return ; } } else{ if(getConstants()->useRefereeSuggestions()) { // If occurred penalty or goal kick, send an goal suggestion - emit emitSuggestion("GOAL", _possibleGoalTeam); + if(!_ballInside){ + emit emitSuggestion("GOAL", _possibleGoalTeam); + emit emitSuggestion("FREE_BALL", VSSRef::Color::NONE, Utils::getBallQuadrant(getVision()->getBallPosition())); + _ballInside = true; + } // Send also an suggestion of free ball - emit emitSuggestion("FREE_BALL", VSSRef::Color::NONE, Utils::getBallQuadrant(getVision()->getBallPosition())); + return ; } } + } else { + if(_possibleGoalTeam != i){ + _ballInside = false; + } } } @@ -131,8 +147,9 @@ void Checker_BallPlay::run() { // If any of them occurred, send HALT command if(getConstants()->useRefereeSuggestions()) { - setPenaltiesInfo(VSSRef::Foul::HALT); - emit foulOccured(); + // setPenaltiesInfo(VSSRef::Foul::HALT); + // emit foulOccured(); + _isPlayRunning = false; return ; } else { diff --git a/src/world/entities/referee/checkers/ballplay/checker_ballplay.h b/src/world/entities/referee/checkers/ballplay/checker_ballplay.h index 0ade8a7..d8c105f 100644 --- a/src/world/entities/referee/checkers/ballplay/checker_ballplay.h +++ b/src/world/entities/referee/checkers/ballplay/checker_ballplay.h @@ -38,6 +38,7 @@ class Checker_BallPlay : public Checker // Ball in area management Timer _areaTimer; bool _areaTimerControl; + bool _ballInside = false; // Checkers Checker_TwoAttackers *_checkerTwoAtk; diff --git a/src/world/entities/referee/referee.cpp b/src/world/entities/referee/referee.cpp index 5c3c509..bba4591 100644 --- a/src/world/entities/referee/referee.cpp +++ b/src/world/entities/referee/referee.cpp @@ -25,10 +25,12 @@ Referee::Referee(Vision *vision, Replacer *replacer, SoccerView *soccerView, Con // Taking network data _refereeAddress = getConstants()->refereeAddress(); _refereePort = getConstants()->refereePort(); + _isFIRAVision = getConstants()->isFIRAVision(); // Connecting referee to replacer connect(_replacer, SIGNAL(teamsPlaced()), this, SLOT(teamsPlaced())); connect(_replacer, SIGNAL(teamsCollided(VSSRef::Foul, VSSRef::Color, VSSRef::Quadrant, bool)), this, SLOT(processCollision(VSSRef::Foul, VSSRef::Color, VSSRef::Quadrant, bool))); + connect(_soccerView, SIGNAL(sendTeleport(bool)), _replacer, SLOT(takeTeleport(bool))); connect(this, SIGNAL(sendFoul(VSSRef::Foul, VSSRef::Color, VSSRef::Quadrant)), _replacer, SLOT(takeFoul(VSSRef::Foul, VSSRef::Color, VSSRef::Quadrant))); connect(this, SIGNAL(callReplacer(bool, bool)), _replacer, SLOT(placeTeams(bool, bool)), Qt::DirectConnection); connect(this, SIGNAL(saveFrame()), _replacer, SLOT(saveFrameAndBall()), Qt::DirectConnection); @@ -95,7 +97,7 @@ void Referee::initialization() { std::cout << Text::blue("[REFEREE] ", true) + Text::bold("Module started at address '" + _refereeAddress.toStdString() + "' and port '" + std::to_string(_refereePort) + "'.") + '\n'; // Call check collision - checkIfTeamsAreColliding(); + // checkIfTeamsAreColliding(); // Call halfPassed (start game) halfPassed(); @@ -185,13 +187,16 @@ void Referee::loop() { _resetedTimer = false; // Call replacer (place teams) - emit callReplacer(_forceDefault, _isToPlaceOutside); - _forceDefault = false; - _isToPlaceOutside = false; + if(_isFIRAVision){ + emit callReplacer(_forceDefault, _isToPlaceOutside); + _forceDefault = false; + _isToPlaceOutside = false; + } // Update sent foul to STOP updatePenaltiesInfo(VSSRef::Foul::STOP, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); sendPenaltiesToNetwork(); + _wait = true; } } else { @@ -206,10 +211,13 @@ void Referee::loop() { _transitionTimer.stop(); // Check if passed transition time - if(_transitionTimer.getSeconds() >= getConstants()->transitionTime()) { + if(_wait) { // Update sent foul to GAME_ON - updatePenaltiesInfo(VSSRef::Foul::GAME_ON, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); - sendPenaltiesToNetwork(); + + updatePenaltiesInfo(VSSRef::Foul::HALT, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); + emit emitSuggestion("GAME_ON"); + _wait = false; + // sendPenaltiesToNetwork(); } } } @@ -243,7 +251,7 @@ void Referee::connectClient() { // Set multicast options _refereeClient->setSocketOption(QAbstractSocket::MulticastTtlOption, 1); - _refereeClient->setMulticastInterface(QNetworkInterface::interfaceFromName(getConstants()->networkInterface())); + // _refereeClient->setMulticastInterface(QNetworkInterface::interfaceFromName(getConstants()->networkInterface())); } void Referee::disconnectClient() { @@ -326,7 +334,7 @@ void Referee::checkIfTeamsAreColliding() { updatePenaltiesInfo(VSSRef::Foul(i), VSSRef::Color::NONE, VSSRef::Quadrant(j)); sendPenaltiesToNetwork(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); - emit callReplacer(false, false); + if(_isFIRAVision) emit callReplacer(false, false); } } else { @@ -335,7 +343,7 @@ void Referee::checkIfTeamsAreColliding() { updatePenaltiesInfo(VSSRef::Foul(i), VSSRef::Color(j), VSSRef::Quadrant::NO_QUADRANT); sendPenaltiesToNetwork(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); - emit callReplacer(false, false); + if(_isFIRAVision) emit callReplacer(false, false); } } } @@ -379,8 +387,10 @@ void Referee::sendPenaltiesToNetwork() { command.SerializeToString(&datagram); // Send via socket - if(_refereeClient->write(datagram.c_str(), static_cast(datagram.length())) == -1) { - std::cout << Text::cyan("[REFEREE] ", true) + Text::red("Failed to write to socket.", true) + '\n'; + for (int i = 0; i < MAX_PACKETS; i++) { + if(_refereeClient->write(datagram.c_str(), static_cast(datagram.length())) == -1) { + std::cout << Text::cyan("[REFEREE] ", true) + Text::red("Failed to write to socket.", true) + '\n'; + } } // Debug sent foul @@ -515,6 +525,7 @@ void Referee::sendControlFoul(VSSRef::Foul foul) { } void Referee::takeManualFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quadrant foulQuadrant, bool isToPlaceOutside) { + printf("%d\n", foul); if(foul == VSSRef::Foul::GAME_ON) { // Reset transitions vars resetTransitionVars(); @@ -531,6 +542,8 @@ void Referee::takeManualFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef: } // Re-send last penalties to network + + updatePenaltiesInfo(VSSRef::Foul::GAME_ON, VSSRef::Color::NONE, VSSRef::Quadrant::NO_QUADRANT); sendPenaltiesToNetwork(); } else if(foul == VSSRef::Foul::HALT) { @@ -595,3 +608,7 @@ Constants* Referee::getConstants() { return nullptr; } + +void Referee::visionPacketChanged(bool isFIRAVision) { + _isFIRAVision = isFIRAVision; +} \ No newline at end of file diff --git a/src/world/entities/referee/referee.h b/src/world/entities/referee/referee.h index e97e1c2..1489bda 100644 --- a/src/world/entities/referee/referee.h +++ b/src/world/entities/referee/referee.h @@ -8,6 +8,8 @@ #include #include +#define MAX_PACKETS 10 + // Abstract SoccerView class SoccerView; @@ -109,6 +111,8 @@ class Referee : public Entity Position _lastBallPosition; Velocity _lastBallVelocity; bool _placedLast; + bool _isFIRAVision; + bool _wait = false; signals: void sendFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quadrant foulQuadrant); @@ -128,6 +132,7 @@ public slots: void takeStuckedTime(float time); void processCollision(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quadrant foulQuadrant, bool isToPlaceOutside); void processCollisionDecision(); + void visionPacketChanged(bool isFIRAVision); }; #endif // REFEREE_H diff --git a/src/world/entities/replacer/replacer.cpp b/src/world/entities/replacer/replacer.cpp index cca9253..1e10ec8 100644 --- a/src/world/entities/replacer/replacer.cpp +++ b/src/world/entities/replacer/replacer.cpp @@ -21,6 +21,11 @@ void debugReplacePosition(double x, double y, int num = 0, std::string side = "" std::cout << Text::blue("[REPLACER] ", true) + Text::bold("Moving player " + std::to_string(num) + " from team \"" + side + "\" " + " to position ("+ std::to_string(x) + ", " + std::to_string(y) + ")") + '\n'; } +void debugTeleport(bool teleport) { + std::string const str_teleport = (teleport ? "enabled." : "disabled."); + std::cout << Text::yellow("[TELEPORT] ", true) + Text::bold("Teleport is " + str_teleport) + '\n'; +} + Replacer::Replacer(QString replaceFileName, Vision *vision, Field *field, Constants *constants) : Entity(ENT_REPLACER){ // Take pointers _vision = vision; @@ -455,6 +460,11 @@ void Replacer::takeFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quad _foulMutex.unlock(); } +void Replacer::takeTeleport(bool teleport) { + _teleport = teleport; + debugTeleport(teleport); +} + Position Replacer::getBallPlaceByFoul(VSSRef::Foul foul, VSSRef::Color color, VSSRef::Quadrant quadrant){ float goalKickX = (getField()->fieldLength() / 1000.0)/2.0 - 0.15; float markX = (getField()->fieldLength() / 1000.0)/2.0 - getField()->fieldFBMarkX()/1000.0; @@ -735,9 +745,11 @@ void Replacer::placeTeams(bool forceDefault, bool isToPlaceOutside) { emit teamsCollided(_foul, _foulColor, _foulQuadrant, isToPlaceOutside); } else { - // If frames not collides, just place it - placeFrame(getTeamFrame(VSSRef::Color::BLUE)); - placeFrame(getTeamFrame(VSSRef::Color::YELLOW)); + // If frames not collides and the teleport is enabled, just place it + if(_teleport){ + placeFrame(getTeamFrame(VSSRef::Color::BLUE)); + placeFrame(getTeamFrame(VSSRef::Color::YELLOW)); + } // Mark foul as processed _foulMutex.lock(); diff --git a/src/world/entities/replacer/replacer.h b/src/world/entities/replacer/replacer.h index bc136fb..9abe07f 100644 --- a/src/world/entities/replacer/replacer.h +++ b/src/world/entities/replacer/replacer.h @@ -69,6 +69,9 @@ class Replacer : public Entity quint8 getGoalie(VSSRef::Color color); QMutex _goalieMutex; + // Teleport + bool _teleport; + // Fouls management VSSRef::Foul _foul; VSSRef::Color _foulColor; @@ -110,6 +113,7 @@ class Replacer : public Entity public slots: void takeGoalie(VSSRef::Color color, quint8 playerId); void takeFoul(VSSRef::Foul foul, VSSRef::Color foulColor, VSSRef::Quadrant foulQuadrant); + void takeTeleport(bool teleport); void placeTeams(bool forceDefault, bool isToPlaceOutside); void saveFrameAndBall(); void placeLastFrameAndBall(); diff --git a/src/world/entities/vision/vision.cpp b/src/world/entities/vision/vision.cpp index 9b3611b..175214b 100644 --- a/src/world/entities/vision/vision.cpp +++ b/src/world/entities/vision/vision.cpp @@ -9,6 +9,7 @@ Vision::Vision(Constants *constants) : Entity(ENT_VISION) { // Taking network data _visionAddress = getConstants()->visionAddress(); _visionPort = getConstants()->visionPort(); + _isFIRAVision = getConstants()->isFIRAVision(); // Init objects initObjects(); @@ -17,15 +18,123 @@ Vision::Vision(Constants *constants) : Entity(ENT_VISION) { Vision::~Vision() { deleteObjects(); } - + void Vision::initialization() { // Binding and connecting in network bindAndConnect(); - - std::cout << Text::blue("[VISION] ", true) + Text::bold("Module started at address '" + _visionAddress.toStdString() + "' and port '" + std::to_string(_visionPort) + "'.") + '\n'; + visionType = _isFIRAVision ? "[FIRA_VISION] " : "[SSL_VISION] "; + std::cout << Text::green(visionType + "Module started at address '" + _visionAddress.toStdString() + "' and port '" + std::to_string(_visionPort) + "'.", true) + '\n'; + std::cout << Text::green(visionType + "Receiving for packets of " + (_isFIRAVision ? "FIRA Simulation!" : "SSLVision!"), true) + '\n'; } void Vision::loop() { + if(_isFIRAVision) FIRAVisionPackets(); + else SSLVisionPackets(); +} + +void Vision::SSLVisionPackets(){ + while(_visionClient->hasPendingDatagrams()) { + + // Creating auxiliary vars + SSL_WrapperPacket wrapper; + QNetworkDatagram datagram; + + // Reading datagram and checking if it is valid + datagram = _visionClient->receiveDatagram(); + if(!datagram.isValid()) { + continue; + } + + // Parsing datagram and checking if it worked properly + if(wrapper.ParseFromArray(datagram.data().data(), datagram.data().size()) == false) { + std::cout << Text::green(visionType, true) << Text::green("Wrapper packet parsing error.", true) + '\n'; + continue; + } + + if(wrapper.has_detection()) { + // Lock mutex for write + _dataMutex.lockForWrite(); + + // Clear objects control + clearObjectsControl(); + + // Take frame + const auto& detection = wrapper.detection(); + + // Parse ball + { + using BALL = std::pair; + QVector balls; + for (const auto& ball : detection.balls()) { + balls += BALL(ball.confidence(), Position(true, ball.x() / 1000., ball.y() / 1000.)); + } + /* TODO: How we can do it better? (ball confidence) */ + std::sort(balls.begin(), balls.end(), [](BALL a, BALL b) { return a.first > b.first; }); + + if (!balls.empty()) { + _ballObject->updateObject(balls.front().first/ 1000., balls.front().second); + } + else { + _ballObject->updateObject(0.0f, Position(false, 0.0, 0.0)); + } + } + + // Parse blue robots + for (const auto& robot : detection.robots_blue()) { + // Take id + quint8 robotId = robot.robot_id(); + + // Get object + Object *robotObject = _objects.value(VSSRef::Color::BLUE)->value(robotId); + robotObject->updateObject(robot.confidence(), Position(true, robot.x() / 1000., robot.y() / 1000.), Angle(true, robot.orientation())); + + // Update control to true + _objectsControl.value(VSSRef::Color::BLUE)->insert(robotId, true); + } + + // Parse yellow robots + for (const auto& robot : detection.robots_yellow()) { + // Take id + quint8 robotId = robot.robot_id(); + + // Get object + Object *robotObject = _objects.value(VSSRef::Color::YELLOW)->value(robotId); + robotObject->updateObject(robot.confidence(), Position(true, robot.x() / 1000., robot.y() / 1000.), Angle(true, robot.orientation())); + + // Update control to true + _objectsControl.value(VSSRef::Color::YELLOW)->insert(robotId, true); + } + + // Parse robots that didn't appeared + for(int i = VSSRef::Color::BLUE; i <= VSSRef::Color::YELLOW; i++) { + // Take control hash + QHash *idsControl = _objectsControl.value(VSSRef::Color(i)); + + // Take ids list and iterate on it + QList idList = idsControl->keys(); + QList::iterator it; + + for(it = idList.begin(); it != idList.end(); it++) { + // If not updated (== false) + if(idsControl->value((*it)) == false) { + // Take object + Object *robotObject = _objects.value(VSSRef::Color(i))->value((*it)); + + // Update it with invalid values + robotObject->updateObject(0.0f, Position(false, 0.0, 0.0), Angle(false, 0.0)); + } + } + } + + // Release mutex + _dataMutex.unlock(); + + emit visionUpdated(); + } + } +} + +void Vision::FIRAVisionPackets(){ while(_visionClient->hasPendingDatagrams()) { // Creating auxiliary vars fira_message::sim_to_ref::Environment environmentData; @@ -39,7 +148,7 @@ void Vision::loop() { // Parsing datagram and checking if it worked properly if(environmentData.ParseFromArray(datagram.data().data(), datagram.data().size()) == false) { - std::cout << Text::blue("[VISION] ", true) << Text::red("Wrapper packet parsing error.", true) + '\n'; + std::cout << Text::green(visionType + "Wrapper packet parsing error.", true) + '\n'; continue; } @@ -132,7 +241,7 @@ void Vision::finalization() { // Deleting vision client delete _visionClient; - std::cout << Text::blue("[VISION] ", true) + Text::bold("Module finished.") + '\n'; + std::cout << Text::green(visionType + "Module finished.", true) + '\n'; } void Vision::bindAndConnect() { @@ -141,13 +250,13 @@ void Vision::bindAndConnect() { // Binding in defined network data if(_visionClient->bind(QHostAddress(_visionAddress), _visionPort, QUdpSocket::ShareAddress) == false) { - std::cout << Text::blue("[VISION] " , true) << Text::red("Error while binding socket.", true) + '\n'; + std::cout << Text::green(visionType + "Error while binding socket.", true) + '\n'; return ; } // Joining multicast group if(_visionClient->joinMulticastGroup(QHostAddress(_visionAddress)) == false) { - std::cout << Text::blue("[VISION] ", true) << Text::red("Error while joining multicast.", true) + '\n'; + std::cout << Text::green(visionType + "Error while joining multicast.", true) + '\n'; return ; } } @@ -285,7 +394,7 @@ Velocity Vision::getBallVelocity() { Constants* Vision::getConstants() { if(_constants == nullptr) { - std::cout << Text::red("[ERROR] ", true) << Text::bold("Constants with nullptr value at Vision") + '\n'; + std::cout << Text::red("[ERROR] Constants with nullptr value at Vision", true) + '\n'; } else { return _constants; @@ -293,3 +402,19 @@ Constants* Vision::getConstants() { return nullptr; } + +void Vision::visionPacketChanged(bool isFIRAVision){ + _isFIRAVision = isFIRAVision; + if(isFIRAVision){ + _visionAddress = getConstants()->firaVisionAddress(); + _visionPort = getConstants()->firaVisionPort(); + } + else{ + _visionAddress = getConstants()->visionAddress(); + _visionPort = getConstants()->visionPort(); + } + _visionClient->close(); + deleteObjects(); + initObjects(); + initialization(); +} diff --git a/src/world/entities/vision/vision.h b/src/world/entities/vision/vision.h index 30ee1f5..c183a18 100644 --- a/src/world/entities/vision/vision.h +++ b/src/world/entities/vision/vision.h @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -42,6 +43,7 @@ class Vision : public Entity // Network QString _visionAddress; quint16 _visionPort; + bool _isFIRAVision; // Objects Object *_ballObject; @@ -54,8 +56,16 @@ class Vision : public Entity // Data management QReadWriteLock _dataMutex; + void SSLVisionPackets(); + void FIRAVisionPackets(); + + std::string visionType = "[VISION] "; + signals: void visionUpdated(); + +public slots: + void visionPacketChanged(bool isFIRAVision); }; #endif // VISION_H