Add pass trajectory avoidance for non-passers

RoboTeamTwente · Nov 20, 2023 · 0263b74 · 0263b74
1 parent 6d24920
commit 0263b74
Show file tree

Hide file tree

Showing 7 changed files with 53 additions and 0 deletions.
diff --git a/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h b/roboteam_ai/include/roboteam_ai/stp/computations/PositionComputations.h
@@ -21,6 +21,8 @@
 #include "world/FieldComputations.h"
 #include "world/World.hpp"
 #include "world/views/WorldDataView.hpp"
+#include "stp/computations/PassComputations.h"
+
 namespace rtt::ai::stp {
 
 /**
@@ -136,6 +138,17 @@ class PositionComputations {
     static void calculateInfoForFormationOurSide(std::unordered_map<std::string, StpInfo> &stpInfos,
                                                  std::array<std::unique_ptr<Role>, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field,
                                                  world::World *world) noexcept;
+    /**
+     * @brief Recalculates info for the position of our robots to not interfere with passing
+     * @param stpInfos The current stpInfos
+     * @param roles The current roles
+     * @param field The current field
+     * @param world The current world
+     * @param passInfo The current passInfo
+     */
+    static void recalculateInfoForNonPassers(std::unordered_map<std::string, StpInfo> &stpInfos,
+                                                 std::array<std::unique_ptr<Role>, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field,
+                                                 world::World *world, rtt::ai::stp::PassInfo passInfo) noexcept;
 
    private:
     /**

diff --git a/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h b/roboteam_ai/include/roboteam_ai/stp/constants/ControlConstants.h
@@ -66,6 +66,7 @@ constexpr double DISTANCE_TO_ROBOT_FAR = 5 * ROBOT_RADIUS;    /**< Distance from
 constexpr double ROBOT_CLOSE_TO_POINT = 0.2;                  /**< Distance from the robot to a position at which the robot is considered close to that position */
 constexpr double DISTANCE_TO_ROBOT_NEAR = 2.2 * ROBOT_RADIUS; /**< Distance from the robot to another robot at which the robot is considered near that other robot */
 constexpr double DEFENSE_AREA_AVOIDANCE_MARGIN = 0.1;         /**< Distance error for avoiding the defense area */
+constexpr double DISTANCE_TO_PASS_TRAJECTORY = 0.5;                  /**< Distance from the robot to the pass trajectory at which the robot is considered too close to the pass trajectory */
 
 /// Keeper constants
 constexpr double DISTANCE_FROM_GOAL_CLOSE = 2 * ROBOT_RADIUS; /**< Distance from the keeper to the goal at which the keeper is considered close to that goal */

diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp
@@ -13,6 +13,7 @@
 #include "stp/computations/ComputationManager.h"
 #include "stp/computations/PositionScoring.h"
 #include "world/World.hpp"
+#include "stp/computations/PassComputations.h"
 
 namespace rtt::ai::stp {
 
@@ -473,4 +474,38 @@ void PositionComputations::calculateInfoForFormationOurSide(std::unordered_map<s
     }
 }
 
+void PositionComputations::recalculateInfoForNonPassers(std::unordered_map<std::string, StpInfo> &stpInfos,
+                                                            std::array<std::unique_ptr<Role>, stp::control_constants::MAX_ROBOT_COUNT> &roles, const Field &field,
+                                                            world::World *world, rtt::ai::stp::PassInfo passInfo) noexcept {
+    auto ballPosition = world->getWorld()->getBall()->get()->position;
+    auto passLocation = passInfo.passLocation;
+    auto passerId = passInfo.passerId;
+    auto receiverId = passInfo.receiverId;
+    // Make a list of all robots that are not the passer, receiver or keeper, which need to make sure they are not in the way of the pass
+    auto toBeCheckedRobots = std::vector<std::string>{};
+    for (auto& role : stpInfos) {
+        if (role.second.getRobot().has_value()) {
+            auto robotId = role.second.getRobot()->get()->getId();
+            auto robotName = role.first;
+            if (robotId != passerId && robotId != receiverId && robotName != "keeper") {
+                toBeCheckedRobots.emplace_back(role.first);
+            }
+        }
+    }
+    // Make a tube around the pass trajectory, and make sure all robots outside of this tube
+    std::unique_ptr<Shape> avoidShape = std::make_unique<Tube>(Tube(ballPosition, passLocation, control_constants::DISTANCE_TO_PASS_TRAJECTORY));
+    for (auto& robot : toBeCheckedRobots) {
+        auto robotPositionToMoveTo = stpInfos[robot].getPositionToMoveTo();
+        if (robotPositionToMoveTo == std::nullopt || !robotPositionToMoveTo.has_value()) {
+            continue;
+        }
+        if (!avoidShape->contains(robotPositionToMoveTo.value())) {
+            continue;
+        }
+        auto newRobotPositionToMoveTo = calculatePositionOutsideOfShape(ballPosition, field, avoidShape);
+        stpInfos[robot].setPositionToMoveTo(newRobotPositionToMoveTo);
+    }
+
+}
+
 }  // namespace rtt::ai::stp
diff --git a/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp b/roboteam_ai/src/stp/plays/defensive/KeeperKickBall.cpp
@@ -69,6 +69,7 @@ Dealer::FlagMap KeeperKickBall::decideRoleFlags() const noexcept {
 void KeeperKickBall::calculateInfoForRoles() noexcept {
     PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world);
     PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world);
+    PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo);
 
     if (!ballKicked()) {
         stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation);

diff --git a/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp b/roboteam_ai/src/stp/plays/offensive/AttackingPass.cpp
@@ -77,6 +77,7 @@ void AttackingPass::calculateInfoForRoles() noexcept {
     PositionComputations::calculateInfoForKeeper(stpInfos, field, world);
     PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world);
     PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world);
+    PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo);
 
     if (!ballKicked()) {
         stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation);

diff --git a/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp b/roboteam_ai/src/stp/plays/offensive/ChippingPass.cpp
@@ -76,6 +76,7 @@ void ChippingPass::calculateInfoForRoles() noexcept {
     PositionComputations::calculateInfoForKeeper(stpInfos, field, world);
     PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world);
     PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world);
+    PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo);
 
     if (!ballKicked()) {
         stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation);

diff --git a/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp b/roboteam_ai/src/stp/plays/referee_specific/FreeKickUsPass.cpp
@@ -69,6 +69,7 @@ void FreeKickUsPass::calculateInfoForRoles() noexcept {
     PositionComputations::calculateInfoForKeeper(stpInfos, field, world);
     PositionComputations::calculateInfoForDefendersAndWallers(stpInfos, roles, field, world);
     PositionComputations::calculateInfoForAttackers(stpInfos, roles, field, world);
+    PositionComputations::recalculateInfoForNonPassers(stpInfos, roles, field, world, passInfo);
 
     if (!ballKicked()) {
         stpInfos["receiver"].setPositionToMoveTo(passInfo.passLocation);