Skip to content

Commit

Permalink
improve harasser and collisions
Browse files Browse the repository at this point in the history
  • Loading branch information
@JornJorn
JornJorn committed Jul 19, 2024
1 parent 7c22f84 commit 59da6b4
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 35 deletions.
44 changes: 22 additions & 22 deletions roboteam_ai/src/control/positionControl/CollisionCalculations.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<size_t>(7));
auto pathPoints = Trajectory.getPathApproach(0.05);
auto maxCheckPoints = std::min(pathPoints.size(), static_cast<size_t>(14));

auto [theirDefenseAreaMargin, ourDefenseAreaMargin] = FieldComputations::getDefenseAreaMargin();
const auto &ourDefenseArea = FieldComputations::getDefenseArea(field, true, ourDefenseAreaMargin, 1);
Expand All @@ -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;
}
}
}
Expand All @@ -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<int, std::vector<Vector2>> &computedPaths) {
auto pathPoints = Trajectory.getPathApproach(0.1);
auto pathPoints = Trajectory.getPathApproach(0.05);
auto maxCheckPoints = std::min(pathPoints.size(), static_cast<size_t>(7));

const std::vector<world::view::RobotView> &theirRobots = world->getWorld()->getThem();
Expand All @@ -70,8 +70,8 @@ double CollisionCalculations::getFirstCollisionTimeMovingObject(const Trajectory

for (int checkPoint = 1; checkPoint < static_cast<int>(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) {
Expand All @@ -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;
Expand All @@ -95,60 +95,60 @@ 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) {
continue;
}
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<int>(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;
break;
}
}
if (isBallPlacementCollision) {
return checkPoint * 0.1;
return checkPoint * 0.05;
}
}
}
Expand Down
26 changes: 13 additions & 13 deletions roboteam_ai/src/stp/computations/PositionComputations.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -338,23 +338,23 @@ void PositionComputations::calculateInfoForHarasser(std::unordered_map<std::stri
auto enemyAngle = enemyClosestToBall->get()->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> &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> &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();
}
}
}
Expand Down

0 comments on commit 59da6b4

Please sign in to comment.