From 59da6b4bccf2c24b747eb015619d97f36f8c8fc0 Mon Sep 17 00:00:00 2001 From: Jorn Date: Fri, 19 Jul 2024 09:29:26 +0200 Subject: [PATCH] improve harasser and collisions --- .../positionControl/CollisionCalculations.cpp | 44 +++++++++---------- .../stp/computations/PositionComputations.cpp | 26 +++++------ 2 files changed, 35 insertions(+), 35 deletions(-) diff --git a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp index 0f485e081..d7fbe7d4b 100644 --- a/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp +++ b/roboteam_ai/src/control/positionControl/CollisionCalculations.cpp @@ -9,8 +9,8 @@ namespace rtt::ai::control { double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajectory2D &Trajectory, stp::AvoidObjects avoidObjects, const Field &field) { - auto pathPoints = Trajectory.getPathApproach(0.1); - auto maxCheckPoints = std::min(pathPoints.size(), static_cast(7)); + auto pathPoints = Trajectory.getPathApproach(0.05); + auto maxCheckPoints = std::min(pathPoints.size(), static_cast(14)); auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = FieldComputations::getDefenseAreaMargin(); const auto &ourDefenseArea = FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1); @@ -36,17 +36,17 @@ double CollisionCalculations::getFirstCollisionTimeMotionlessObject(const Trajec } if (avoidObjects.shouldAvoidOurDefenseArea) { if (ourDefenseArea.contains(pathPoints[checkPoint]) || ourDefenseArea.contains(pathPoints[checkPoint - 1]) || ourDefenseArea.doesIntersect(pathLine)) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } if (avoidObjects.shouldAvoidTheirDefenseArea && theirDefenseAreaMargin > constants::ROBOT_RADIUS + constants::GO_TO_POS_ERROR_MARGIN) { if (theirDefenseArea.contains(pathPoints[checkPoint]) || theirDefenseArea.contains(pathPoints[checkPoint - 1]) || theirDefenseArea.doesIntersect(pathLine)) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } if (avoidObjects.shouldAvoidOutOfField) { if (!field.playArea.contains(pathPoints[checkPoint], constants::OUT_OF_FIELD_MARGIN)) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } } @@ -60,7 +60,7 @@ bool CollisionCalculations::isCollidingWithMotionlessObject(const Trajectory2D & double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory2D &Trajectory, stp::AvoidObjects avoidObjects, int &robotId, const world::World *world, const std::unordered_map> &computedPaths) { - auto pathPoints = Trajectory.getPathApproach(0.1); + auto pathPoints = Trajectory.getPathApproach(0.05); auto maxCheckPoints = std::min(pathPoints.size(), static_cast(7)); const std::vector &theirRobots = world->getWorld()->getThem(); @@ -70,8 +70,8 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory for (int checkPoint = 1; checkPoint < static_cast(maxCheckPoints); checkPoint += 1) { auto pathLine = LineSegment(pathPoints[checkPoint - 1], pathPoints[checkPoint]); - double velocityOurRobot = Trajectory.getVelocity(checkPoint * 0.1).length(); - auto positionOurRobot = Trajectory.getPosition(checkPoint * 0.1); + double velocityOurRobot = Trajectory.getVelocity(checkPoint * 0.05).length(); + auto positionOurRobot = Trajectory.getPosition(checkPoint * 0.05); double additionalMargin = std::pow(std::min(3.5, velocityOurRobot) / 3.5, 2) * 0.2; if (velocityOurRobot > 0.7 && avoidObjects.shouldAvoidOurRobots) { for (const auto &ourOtherRobot : ourRobots) { @@ -85,7 +85,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory if (computedPathsIt == computedPaths.end()) { const Vector2 &ourOtherRobotPos = ourOtherRobot->getPos(); if ((ourOtherRobotPos - positionOurRobot).length() < 2 * constants::ROBOT_RADIUS + additionalMargin) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } else { LineSegment pathLineOtherRobot; @@ -95,11 +95,11 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory pathLineOtherRobot = LineSegment(computedPathsIt->second.back(), computedPathsIt->second.back()); } if (pathLineOtherRobot.closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS + additionalMargin) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } } - } else { + } else if (avoidObjects.shouldAvoidOurRobots) { for (const auto &ourOtherRobot : ourRobots) { const int &ourOtherRobotId = ourOtherRobot->getId(); if (ourOtherRobotId == robotId) { @@ -107,40 +107,40 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory } const Vector2 &ourOtherRobotPos = ourOtherRobot->getPos(); if ((ourOtherRobotPos - positionOurRobot).length() < 1.4 * constants::ROBOT_RADIUS) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } } if (velocityOurRobot > 0.7 && avoidObjects.shouldAvoidTheirRobots) { if (std::any_of(theirRobots.begin(), theirRobots.end(), [&](const auto &theirRobot) { - return LineSegment(theirRobot->getPos() + theirRobot->getVel() * 0.1 * (checkPoint - 1), theirRobot->getPos() + theirRobot->getVel() * 0.1 * checkPoint) + return LineSegment(theirRobot->getPos() + theirRobot->getVel() * 0.05 * (checkPoint - 1), theirRobot->getPos() + theirRobot->getVel() * 0.05 * checkPoint) .closestDistanceToLineSegment(pathLine) < 2 * constants::ROBOT_RADIUS + additionalMargin; })) { - return checkPoint * 0.1; + return checkPoint * 0.05; } - } else { + } else if (avoidObjects.shouldAvoidTheirRobots) { if (std::any_of(theirRobots.begin(), theirRobots.end(), [&](const auto &theirRobot) { return (theirRobot->getPos() - positionOurRobot).length() < 1.8 * constants::CENTER_TO_FRONT; })) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } if (avoidObjects.shouldAvoidBall) { - auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1); + auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.05); if ((ballPosition - positionOurRobot).length() < constants::AVOID_BALL_DISTANCE + additionalMargin) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } if (GameStateManager::getCurrentGameState().getCommandId() == RefCommand::BALL_PLACEMENT_THEM || GameStateManager::getCurrentGameState().getCommandId() == RefCommand::PREPARE_FORCED_START) { auto ballPlacementPosition = GameStateManager::getRefereeDesignatedPosition(); bool isBallPlacementCollision = true; - for (int i = checkPoint; i < checkPoint + 10; i++) { - auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.1); + for (int i = checkPoint; i < checkPoint + 20; i++) { + auto ballPosition = FieldComputations::getBallPositionAtTime(*world->getWorld()->getBall()->get(), checkPoint * 0.05); if (i >= static_cast(pathPoints.size())) { isBallPlacementCollision = false; break; } - auto positionOurRobot = Trajectory.getPosition(i * 0.1); + auto positionOurRobot = Trajectory.getPosition(i * 0.05); auto ballPlacementLine = LineSegment(ballPlacementPosition, ballPosition); if (ballPlacementLine.distanceToLine(positionOurRobot) > constants::AVOID_BALL_DISTANCE + additionalMargin) { isBallPlacementCollision = false; @@ -148,7 +148,7 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory } } if (isBallPlacementCollision) { - return checkPoint * 0.1; + return checkPoint * 0.05; } } } diff --git a/roboteam_ai/src/stp/computations/PositionComputations.cpp b/roboteam_ai/src/stp/computations/PositionComputations.cpp index f8009ebd1..1ab2d1628 100644 --- a/roboteam_ai/src/stp/computations/PositionComputations.cpp +++ b/roboteam_ai/src/stp/computations/PositionComputations.cpp @@ -338,23 +338,23 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_mapget()->getYaw(); auto harasserAngle = stpInfos["harasser"].getYaw(); // If enemy is not facing our goal AND does have the ball, stand between the enemy and our goal - if (enemyClosestToBall->get()->hasBall() && enemyAngle.shortestAngleDiff(harasserAngle) < M_PI / 1.5) { + LineSegment ourToBall = LineSegment(stpInfos["harasser"].getRobot()->get()->getPos(), world->getWorld()->getBall()->get()->position); + // project their point on the line + Vector2 projectedPoint = ourToBall.project(enemyClosestToBall->get()->getPos()); + bool theyAreBetween = (projectedPoint != world->getWorld()->getBall()->get()->position); + bool theyAreCloser = (enemyClosestToBall->get()->getPos() - world->getWorld()->getBall()->get()->position).length() < (stpInfos["harasser"].getRobot()->get()->getPos() - world->getWorld()->getBall()->get()->position).length(); + auto harasser = std::find_if(roles->begin(), roles->end(), [](const std::unique_ptr &role) { return role != nullptr && role->getName() == "harasser"; }); + if (theyAreBetween && theyAreCloser) { auto enemyPos = enemyClosestToBall->get()->getPos(); - auto targetPos = enemyPos + (field.leftGoalArea.leftLine().center() - enemyPos).stretchToLength(constants::ROBOT_RADIUS * 4); + auto targetPos = world->getWorld()->getBall()->get()->position - (enemyPos - world->getWorld()->getBall()->get()->position).stretchToLength(constants::ROBOT_RADIUS * 3); + if (harasser != roles->end()) { + harasser->get()->reset(); + } stpInfos["harasser"].setPositionToMoveTo(targetPos); stpInfos["harasser"].setYaw((world->getWorld()->getBall()->get()->position - targetPos).angle()); } else { - auto harasser = std::find_if(roles->begin(), roles->end(), [](const std::unique_ptr &role) { return role != nullptr && role->getName() == "harasser"; }); - if (harasser != roles->end() && !harasser->get()->finished() && strcmp(harasser->get()->getCurrentTactic()->getName(), "Formation") == 0) { - auto enemyPos = enemyClosestToBall->get()->getPos(); - auto targetPos = enemyPos + (world->getWorld()->getBall()->get()->position - enemyPos).stretchToLength(constants::ROBOT_RADIUS * 3); - // prevent the harasser from being stuck on the side of the enemy - if (enemyClosestToBall->get()->hasBall() && ((stpInfos["harasser"].getRobot()->get()->getPos() - targetPos).length() > constants::ROBOT_RADIUS)) { - stpInfos["harasser"].setPositionToMoveTo(targetPos); - stpInfos["harasser"].setYaw((world->getWorld()->getBall()->get()->position - targetPos).angle()); - } else { - harasser->get()->forceNextTactic(); - } + if (harasser != roles->end()) { + harasser->get()->forceLastTactic(); } } }