Setting GUI for Navigation Check

<TBC>

src/entities/player/behavior/behavior.cpp

@@ -70,6 +70,10 @@ void Behavior::setPlayer(Player *player) {
     }
 }
 
+void Behavior::setGUI(GUI *gui) {
+    _gui = gui;
+}
+
 void Behavior::runBehavior() {
     // Check if behavior have at least one skill
     if(_skillList.size() == 0) {
@@ -118,6 +122,10 @@ Player* Behavior::player() {
     return _player;
 }
 
+GUI* Behavior::gui() {
+    return _gui;
+}
+
 WorldMap* Behavior::getWorldMap() {
     if(_worldMap == nullptr) {
         spdlog::error("[{}] WorldMap with nullptr value.", name().toStdString());

src/entities/player/behavior/behavior.h

@@ -24,6 +24,7 @@
 #define BEHAVIOR_H
 
 #include <src/entities/basesuassuna.h>
+#include <src/gui/gui.h>
 
 class Behavior
 {
@@ -41,6 +42,9 @@ class Behavior
     void initialize(WorldMap* worldMap);
     void setPlayer(Player* player);
 
+    void setGUI(GUI* gui);
+
+
     // Method to run in role
     void runBehavior();
 
@@ -53,6 +57,8 @@ class Behavior
     WorldMap* getWorldMap();
     Player* player();
 
+    GUI* gui();
+
 private:
 
     // Virtual implementation in inherited classes
@@ -71,6 +77,8 @@ class Behavior
 
     // Initialize control
     bool _initialized;
+
+    GUI *_gui;
 };
 
 #endif // BEHAVIOR_H

src/entities/player/behavior/default/behavior_default.cpp

@@ -56,6 +56,7 @@ void Behavior_Default::run() {
         // Move the robot to other positions and see if it moves
         // to the target position using the best face no move
         _skill_goTo->setTargetPosition(_targetPosition);
+        _skill_goTo->setGUI(gui());
         runSkill(SKILL_GOTO);
     } else {
         // Move the robot to positions where you can see both spin directions

src/entities/player/behavior/intercept/behavior_intercept.cpp

@@ -71,6 +71,7 @@ void Behavior_Intercept::run() {
     }
 
     _skill_goTo->setTargetPosition(_interceptPosition);
+    _skill_goTo->setGUI(gui());
     runSkill(SKILL_GOTO);
 }
 

src/entities/player/behavior/move/behavior_move.cpp

@@ -49,6 +49,7 @@ void Behavior_Move::run() {
     switch(_movementType) {
         case MOVEMENT::MOVE:
             _skill_goTo->setTargetPosition(_targetPosition);
+            _skill_goTo->setGUI(gui());
             runSkill(SKILL_GOTO);
             break;
         case MOVEMENT::ROTATE:

src/entities/player/player.cpp

@@ -70,6 +70,10 @@ void Player::setRole(Role *role) {
     _mutexRole.unlock();
 }
 
+void Player::setGUI(GUI *gui) {
+    _gui = gui;
+}
+
 void Player::updatePlayer(Common::Types::Object playerData) {
     setPosition(playerData.getPosition());
     setVelocity(playerData.getVelocity());
@@ -111,6 +115,7 @@ void Player::loop() {
                 _playerRole->initialize(_worldMap);
             }
             _playerRole->setPlayer(this);
+            _playerRole->setGUI(_gui);
             _playerRole->runRole();
         }
         else {

src/entities/player/player.h

@@ -24,6 +24,8 @@
 
 #include <src/entities/basesuassuna.h>
 #include <src/common/types/robot/robot.h>
+#include <src/entities/player/navigation/navigation.h>
+#include <src/gui/gui.h>
 
 class Player : public Suassuna::Robot, public Threaded::Entity
 {
@@ -39,6 +41,7 @@ class Player : public Suassuna::Robot, public Threaded::Entity
      * \param role
      */
     void setRole(Role* role);
+    void setGUI(GUI* gui);
 
 protected:
     // Friend classes for update and internal control of the player
@@ -92,6 +95,9 @@ class Player : public Suassuna::Robot, public Threaded::Entity
     // Idle control
     Utils::Timer _idleTimer;
     bool _firstIt;
+
+    GUI *_gui;
+
 };
 
 #endif // PLAYER_H

src/entities/player/role/default/role_default.cpp

@@ -45,18 +45,21 @@ void Role_Default::run() {
         // Move the robot to other positions and see if it moves to the target position using the best face no move
         _behavior_move->setMovementType(Behavior_Move::MOVEMENT::MOVE);
         _behavior_move->setTargetPosition(getWorldMap()->getBall().getPosition());
+        _behavior_move->setGUI(gui());
         setBehavior(BEHAVIOR_MOVE);
     } else if (player()->robotId() == 1) {
         // Move the robot to other positions and see if it rotate to the target position using the smarter rotation diretion route
         _behavior_move->setMovementType(Behavior_Move::MOVEMENT::ROTATE);
         _behavior_move->setTargetAngle((getWorldMap()->getBall().getPosition() - player()->getPosition()).angle());
+        _behavior_move->setGUI(gui());
         setBehavior(BEHAVIOR_MOVE);
     } else {
         // Move the robot to positions where you can see both spin directions
         _behavior_intercept->setInterceptSegment(Geometry::LineSegment(Geometry::Vector2D(0.4f, -0.5f), Geometry::Vector2D(0.4f, 0.5f)));
         _behavior_intercept->setObjectPosition(getWorldMap()->getBall().getPosition());
         _behavior_intercept->setObjectVelocity(getWorldMap()->getBall().getVelocity());
         _behavior_intercept->setIntersectionAccuracy(0.3f);
+        _behavior_intercept->setGUI(gui());
         setBehavior(BEHAVIOR_INTERCEPT);
     }
 }

src/entities/player/role/role.cpp

@@ -64,6 +64,10 @@ void Role::setPlayer(Player *player) {
     _player = player;
 }
 
+void Role::setGUI(GUI *gui) {
+    _gui = gui;
+}
+
 QString Role::actualBehaviorName() {
     if(_actualBehavior == nullptr) {
         return "NONE";
@@ -133,3 +137,7 @@ WorldMap* Role::getWorldMap() {
 
     return nullptr;
 }
+
+GUI* Role::gui() {
+    return _gui;
+}

src/entities/player/role/role.h

@@ -24,6 +24,7 @@
 #define ROLE_H
 
 #include <src/entities/basesuassuna.h>
+#include <src/gui/gui.h>
 
 class Role : public QObject
 {
@@ -42,6 +43,8 @@ class Role : public QObject
     void initialize(WorldMap *worldMap);
     void setPlayer(Player *player);
 
+    void setGUI(GUI *gui);
+
     // Actual Behavior name
     QString actualBehaviorName();
 
@@ -57,6 +60,8 @@ class Role : public QObject
     Player* player();
     WorldMap* getWorldMap();
 
+    GUI* gui();
+
 private:
     // Virtual implementation in inherited classes
     virtual void configure() = 0;
@@ -74,6 +79,8 @@ class Role : public QObject
 
     // Initialize control
     bool _initialized;
+
+    GUI *_gui;
 };
 
 #endif // ROLE_H

src/entities/player/skill/goto/skill_goto.cpp

@@ -44,6 +44,8 @@ float smallestAngleDiff(float target, float source) {
 }
 
 void Skill_GoTo::run() {
+    nav()->setGUI(gui());
+
     float Kp = 20.0f;
     float Ki = 0.0f;
     float Kd = 2.5f;
@@ -55,16 +57,22 @@ void Skill_GoTo::run() {
     bool reversed = false;
 
     float robotAngle = player()->getOrientation().value();
+
     //float angleToTarget = (_targetPosition - player()->getPosition()).angle();
-    float angleToTarget = atan2(_targetPosition.y() - player()->getPosition().y(),
-                                _targetPosition.x() - player()->getPosition().x());
 
-    float angError = smallestAngleDiff(robotAngle, angleToTarget);
+//    float angleToTarget = atan2(_targetPosition.y() - player()->getPosition().y(),
+//                                _targetPosition.x() - player()->getPosition().x());
+
+    // Navigation output here
+    // inputs: curr pos, goal pos, to avoid opponents, to avoid teammates, to avoid ball;
+    Geometry::Angle angleToTarget = nav()->angleToTarget(player()->getPosition(), _targetPosition, true, true, false);
+
+    float angError = smallestAngleDiff(robotAngle, angleToTarget.value());
     if(fabs(angError) > M_PI/2.0 + M_PI/20.0) {
         reversed = true;
         robotAngle = Geometry::Angle(robotAngle + M_PI).value();
         // Calculates the error and reverses the front of the robot
-        angError = smallestAngleDiff(robotAngle, angleToTarget);
+        angError = smallestAngleDiff(robotAngle, angleToTarget.value());
     }
 
     float motorSpeed = (Kp*angError) + (Ki*cumulativeError) + (Kd * (angError - lastError));// + 0.2 * sumErr;
@@ -96,6 +104,17 @@ void Skill_GoTo::run() {
     }
 
     setWheelsSpeed(leftMotorSpeed, rightMotorSpeed);
+
+    if (gui() != nullptr) {
+        QList<Geometry::Vector2D> interestPoints;
+        interestPoints.push_back(_targetPosition);
+        QVector<Geometry::Vector2D> univector;
+        Geometry::Vector2D projection = Geometry::Vector2D(angleToTarget, 1.0f);
+        projection = Geometry::Vector2D(player()->getPosition().x() + projection.x(), player()->getPosition().y() + projection.y());
+        univector.push_back(projection);
+        gui()->updateInterestPoints(player()->identifier().robotid(), interestPoints);
+        gui()->updateVectorsAngles(player()->identifier().robotid(), univector);
+    }
 }
 
 void Skill_GoTo::setTargetPosition(const Geometry::Vector2D &targetPosition) {

src/entities/player/skill/goto/skill_goto.h

@@ -39,6 +39,7 @@ class Skill_GoTo : public Skill
     // Internal
     Geometry::Vector2D _targetPosition;
     bool _isSimulation;
+    void sendTargetPosition(const QList<Geometry::Vector2D>& targets);
 };
 
 #endif // SKILL_GOTO_H

src/entities/player/skill/skill.cpp

@@ -32,6 +32,8 @@ Skill::Skill() {
     _worldMap = nullptr;
     _player = nullptr;
     _initialized = false;
+    _nav = nullptr;
+    _gui = nullptr;
 }
 
 bool Skill::isInitialized() {
@@ -48,14 +50,28 @@ void Skill::setPlayer(Player *player) {
     _player = player;
 }
 
+void Skill::setGUI(GUI *gui) {
+    _gui = gui;
+}
+
 void Skill::runSkill() {
+    _nav = new Navigation(_player->identifier().robotid(), _worldMap);
+    _nav->setGUI(gui());
     run();
 }
 
 Player* Skill::player() {
     return _player;
 }
 
+Navigation* Skill::nav() {
+    return _nav;
+}
+
+GUI* Skill::gui() {
+    return _gui;
+}
+
 void Skill::setWheelsSpeed(const float &wheelLeft, const float &wheelRight) {
     _player->setWheelsSpeed(wheelLeft, wheelRight);
 }

src/entities/player/skill/skill.h

@@ -23,6 +23,8 @@
 #define SKILL_H
 
 #include <src/entities/basesuassuna.h>
+#include <src/entities/player/navigation/navigation.h>
+#include <src/gui/gui.h>
 #include <math.h>
 
 class Skill
@@ -41,13 +43,18 @@ class Skill
     void initialize(WorldMap* worldMap);
     void setPlayer(Player *player);
 
+    void setGUI(GUI *gui);
+
     // Run
     void runSkill();
 
 protected:
     // Player and worldmap getters
     WorldMap* getWorldMap();
     Player* player();
+    Navigation* nav();
+
+    GUI* gui();
 
     // Interface with Player
     void setWheelsSpeed(const float& wheelLeft, const float& wheelRight);
@@ -65,6 +72,9 @@ class Skill
 
     // Internal control
     bool _initialized;
+
+    Navigation *_nav;
+    GUI *_gui;
 };
 
 #endif // SKILL_H

src/entities/worldmap/worldmap.cpp

@@ -25,7 +25,6 @@
 
 #include <src/common/constants/constants.h>
 #include <src/common/utils/utils.h>
-#include <src/entities/coach/team/team.h>
 
 WorldMap::WorldMap() {
     // Setup default field
@@ -52,6 +51,35 @@ Common::Types::Object WorldMap::getBall() {
     return ball;
 }
 
+QPair<Geometry::Vector2D, bool> WorldMap::getPlayerPosition(const Common::Enums::Color& teamColor, quint8 playerId) {
+    _mutex.lockForWrite();
+    QList<Suassuna::Robot*> team = _teams[teamColor]->getPlayers();
+    _mutex.unlock();
+
+    // Return True in pair if robot found
+    for (Suassuna::Robot* player : team) {
+        if (player->identifier().robotid() == playerId) {
+            return QPair(player->getPosition(), true);
+        }
+    }
+
+    return QPair(Geometry::Vector2D(), false);
+}
+
+QList<quint8> WorldMap::getPlayersIds(const Common::Enums::Color &teamColor) {
+    QList<quint8> playersIds;
+
+    _mutex.lockForWrite();
+    QList<Suassuna::Robot*> team = _teams[teamColor]->getPlayers();
+    _mutex.unlock();
+
+    for (auto robot : team) {
+        playersIds.append(robot->identifier().robotid());
+    }
+
+    return playersIds;
+}
+
 void WorldMap::updatePlayers(const QList<Armorial::Robot>& robots) {
     _mutex.lockForWrite();
     for(const auto& r : robots) {