14.2 SmartGoToTarget

This method was implemented by the UTAutstinVilla Team in order to solve the issue of agents running into each other when moving across the field. It works by adjusting the intermediate target points along its journey to its final destination. It adjusts for any other obstacle along the way using a proximity threshold which is simply a parameter you can define. You can also indicate which objects you want to avoid, either teammates, opponents or the ball.

SkillType NaoBehavior::SmartGoToTarget(VecPosition target, double proximity){
    SIM::AngDeg localAngle = atan2Deg(ball.getY(), ball.getX());

    // Adjust target to not be too close to teammates or the ball
        target = collisionAvoidance(true /*teammate*/, true/*opponent*/, false/*ball*/, proximity/*proximity thresh*/, .5/*collision thresh*/, target, true/*keepDistance*/);

        if (me.getDistanceTo(target) < .25 && abs(localAngle) <= 10) {
            // Close enough to desired position and orientation so just stand
            return SKILL_STAND;
        } else if (me.getDistanceTo(target) < .5) {
            // Close to desired position so start turning to face ball
            return goToTargetRelative(worldModel->g2l(target), localAngle);
        } else {
            // Move toward target location
            return goToTarget(target);
        }
}