Path Planning

Armorial-Suassuna.pro

@@ -51,6 +51,8 @@ SOURCES += \
     src/entities/player/behavior/default/behavior_default.cpp \
     src/entities/player/behavior/intercept/behavior_intercept.cpp \
     src/entities/player/behavior/move/behavior_move.cpp \
+    src/entities/player/navigation/navigation.cpp \
+    src/entities/player/navigation/univector/univector.cpp \
     src/entities/player/role/default/role_default.cpp \
     src/entities/player/role/goalkeeper/role_goalkeeper.cpp \
     src/entities/player/role/role.cpp \
@@ -88,6 +90,8 @@ HEADERS += \
     src/entities/player/behavior/default/behavior_default.h \
     src/entities/player/behavior/intercept/behavior_intercept.h \
     src/entities/player/behavior/move/behavior_move.h \
+    src/entities/player/navigation/navigation.h \
+    src/entities/player/navigation/univector/univector.h \
     src/entities/player/role/default/role_default.h \
     src/entities/player/role/goalkeeper/role_goalkeeper.h \
     src/entities/player/role/role.h \

src/common/constants/constants.cpp

@@ -182,3 +182,63 @@ quint16 Constants::maxNumPlayers() {
 
     return maxPlayers;
 }
+
+float Constants::univectorRadius() {
+    bool converted;
+    float univectorRadius = _parameterHandler["Univector"].getAsMap()["radius"].toFloat(&converted);
+
+    if(!converted) {
+        spdlog::error("[Constants] Failed to read a valid float in '{}'.", __FUNCTION__);
+        exit(-1);
+    }
+
+    return univectorRadius;
+}
+
+float Constants::univectorKr() {
+    bool converted;
+    float univectorKr = _parameterHandler["Univector"].getAsMap()["kr"].toFloat(&converted);
+
+    if(!converted) {
+        spdlog::error("[Constants] Failed to read a valid float in '{}'.", __FUNCTION__);
+        exit(-1);
+    }
+
+    return univectorKr;
+}
+
+float Constants::univectorDelta() {
+    bool converted;
+    float univectorDelta = _parameterHandler["Univector"].getAsMap()["delta"].toFloat(&converted);
+
+    if(!converted) {
+        spdlog::error("[Constants] Failed to read a valid float in '{}'.", __FUNCTION__);
+        exit(-1);
+    }
+
+    return univectorDelta;
+}
+
+float Constants::univectorDmin() {
+    bool converted;
+    float univectorDMin = _parameterHandler["Univector"].getAsMap()["dMin"].toFloat(&converted);
+
+    if(!converted) {
+        spdlog::error("[Constants] Failed to read a valid float in '{}'.", __FUNCTION__);
+        exit(-1);
+    }
+
+    return univectorDMin;
+}
+
+float Constants::univectorKo() {
+    bool converted;
+    float univectorKo = _parameterHandler["Univector"].getAsMap()["ko"].toFloat(&converted);
+
+    if(!converted) {
+        spdlog::error("[Constants] Failed to read a valid float in '{}'.", __FUNCTION__);
+        exit(-1);
+    }
+
+    return univectorKo;
+}

src/common/constants/constants.h

@@ -103,6 +103,31 @@ class Constants
      */
     static quint16 maxNumPlayers();
 
+    /*!
+     * \return Univector Field's Radius value.
+     */
+    static float univectorRadius();
+
+    /*!
+     * \return Univector Field's Kr value.
+     */
+    static float univectorKr();
+
+    /*!
+     * \return Univector Field's Delta value.
+     */
+    static float univectorDelta();
+
+    /*!
+     * \return Univector Field's D minimum.
+     */
+    static float univectorDmin();
+
+    /*!
+     * \return Univector Field's Ko.
+     */
+    static float univectorKo();
+
 private:
     static Utils::ParameterHandler _parameterHandler;
 };

src/common/constants/constants.json

@@ -19,8 +19,16 @@
 	},
 
 	"Team": {
-                "teamColor": "yellow",
-                "teamPlaySide": "left",
+		"teamColor": "yellow",
+		"teamPlaySide": "left",
 		"maxNumPlayers": 3
+	},
+
+	"Univector": {
+                "radius": 350.0,
+                "kr": 500.0,
+                "delta": 50.0,
+                "dMin": 75.0,
+				"ko": 0.21
 	}
 }

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/navigation/navigation.cpp

@@ -0,0 +1,77 @@
+#include "navigation.h"
+
+Navigation::Navigation(quint8 playerId, WorldMap *worldMap) {
+    _worldMap = worldMap;
+    _playerId = playerId;
+    _univector = new Univector(playerId);
+    _gui = nullptr;
+}
+
+QList<Geometry::Vector2D> Navigation::getObstacles(bool avoidOpponents, bool avoidTeammates, bool avoidBall){
+    Common::Enums::Color ourColor = Suassuna::Constants::teamColor();
+
+    QList<Geometry::Vector2D> obstacles;
+    if (avoidOpponents) {
+        Common::Enums::Color theirColor;
+        if (ourColor == Common::Enums::Color::YELLOW) {
+            theirColor = Common::Enums::Color::BLUE;
+        } else {
+            theirColor = Common::Enums::Color::YELLOW;
+        }
+
+        QList<quint8> opPlayers = getWorldMap()->getPlayersIds(theirColor);
+        for(int i = 0; i < opPlayers.size(); i++) {
+            QPair<Geometry::Vector2D, bool> pos = getWorldMap()->getPlayerPosition(theirColor, opPlayers[i]);
+            if(pos.second){
+                obstacles.push_back(Geometry::Vector2D(pos.first.x() * 1000.0, pos.first.y() * 1000.0));
+            }
+        }
+    }
+
+    if (avoidTeammates) {
+        QList<quint8> allyPlayers = getWorldMap()->getPlayersIds(ourColor);
+        for(int i = 0; i < allyPlayers.size(); i++) {
+            if(allyPlayers[i] != _playerId) {
+                QPair<Geometry::Vector2D, bool> pos = getWorldMap()->getPlayerPosition(ourColor, allyPlayers[i]);
+                if(pos.second){
+                    obstacles.push_back(Geometry::Vector2D(pos.first.x() * 1000.0, pos.first.y() * 1000.0));
+                }
+            }
+        }
+    }
+
+    if (avoidBall) {
+        Geometry::Vector2D ballPos = getWorldMap()->getBall().getPosition();
+        obstacles.push_back(ballPos);
+    }
+
+    return obstacles;
+}
+
+QVector<float> Navigation::getVector(Geometry::Vector2D robotPos, Geometry::Vector2D targetPos, bool avoidOpponents, bool avoidTeammates, bool avoidBall){
+    QList<Geometry::Vector2D> obstacles = getObstacles(avoidOpponents, avoidTeammates, avoidBall);
+    QVector<float> uni = _univector->generateUnivectorField(robotPos, targetPos, obstacles, QPair<float, float>(0, 0), QPair<float, float>(0, 0));
+//    QVector<float> uni = _univector->generateHyperbolicField(targetPos, robotPos);
+    return uni;
+}
+
+Geometry::Angle Navigation::angleToTarget(Geometry::Vector2D robotPos, Geometry::Vector2D targetPos, bool avoidOpponents, bool avoidTeammates, bool avoidBall){
+    robotPos = Geometry::Vector2D(1000 * robotPos.x(), 1000 * robotPos.y());
+    targetPos = Geometry::Vector2D(1000 * targetPos.x(), 1000 * targetPos.y());
+    //std::cout << "robotPos: " << robotPos.x() << " " << robotPos.y() << " targetPos: " << targetPos.x() << " " << targetPos.y() << std::endl;
+    QVector<float> v = getVector(robotPos, targetPos, avoidOpponents, avoidTeammates, avoidBall);
+    Geometry::Vector2D resultPos = Geometry::Vector2D(robotPos.x() + 100 * v[0], robotPos.y() + 100 * v[1]);
+    Geometry::Angle angle = Geometry::Angle(atan2(resultPos.y() - robotPos.y(), resultPos.x() - robotPos.x()));
+    //std::cout << "V: " << v[0] << " " << v[1] << std::endl;
+    return angle;
+}
+
+WorldMap *Navigation::getWorldMap(){
+    if(_worldMap == nullptr) {
+        spdlog::info("[ERROR] WorldMap with nullptr value at Navigation.");
+    } else {
+        return _worldMap;
+    }
+
+    return nullptr;
+}

src/entities/player/navigation/navigation.h

@@ -0,0 +1,37 @@
+#ifndef NAVIGATION_H
+#define NAVIGATION_H
+
+#include <src/entities/player/player.h>
+#include <src/common/constants/constants.h>
+#include <src/entities/player/navigation/univector/univector.h>
+#include <src/entities/worldmap/worldmap.h>
+#include <src/gui/gui.h>
+#include <Armorial/Common/Enums/Color/Color.h>
+#include <Armorial/Common/Types/Field/Field.h>
+
+class Navigation
+{
+
+public:
+    Navigation(quint8 playerId, WorldMap *worldMap);
+    QVector<float> getVector(Geometry::Vector2D robotPos, Geometry::Vector2D targetPos, bool avoidOpponents, bool avoidTeammates, bool avoidBall);
+    Geometry::Angle angleToTarget(Geometry::Vector2D robotPos, Geometry::Vector2D targetPos, bool avoidOpponents, bool avoidTeammates, bool avoidBall);
+
+    void setGUI(GUI *gui) { _gui = gui; }
+    GUI *getGUI() { return _gui; }
+
+private:
+    Univector *_univector;
+    WorldMap *_worldMap;
+    quint8 _playerId;
+
+    QVector<float> _univectorOutput;
+
+    GUI *_gui;
+
+    QList<Geometry::Vector2D> getObstacles(bool avoidOpponents, bool avoidTeammates, bool avoidBall);
+    WorldMap *getWorldMap();
+
+};
+
+#endif // NAVIGATION_H