Esempio n. 1
0
def execute(param, state, bot_id, pub):
    DEFEND_RADIUS = 50.0
    ob = Vector2D()
    botPos = Vector2D(int(state.homePos[bot_id].x),
                      int(state.homePos[bot_id].y))
    point_ball_angle = math.atan2(state.ballPos.y - param.DefendPointP.y,
                                  state.ballPos.x - param.DefendPointP.x)
    dpoint = Vector2D(
        int(param.DefendPointP.x +
            (param.DefendPointP.radius) * math.cos(point_ball_angle)),
        int(param.DefendPointP.y +
            (param.DefendPointP.radius) * math.sin(point_ball_angle)))
    obs = Vector_Obstacle()
    for i in range(0, len(state.homeDetected)):
        if state.homeDetected[i] and i != bot_id:
            o = Obstacle()
            o.x = state.homePos[i].x
            o.y = state.homePos[i].y
            o.radius = 3.3 * BOT_RADIUS
            obs.push_back(o)

    for j in range(0, len(state.awayDetected)):
        if state.awayDetected[j]:
            o = Obstacle()
            o.x = state.awayPos[j].x
            o.y = state.awayPos[j].y
            o.radius = 3.3 * BOT_RADIUS
            obs.push_back(o)

    point = Vector2D()
    nextWP = Vector2D()
    nextNWP = Vector2D()

    # calling of pathplanner has to be noticed later

    pathPlanner = MergeSCurve()
    pathPlanner.plan(botPos, dpoint, nextWP, nextNWP, obs,
                     len(state.homeDetected) + len(state.awayDetected), bot_id,
                     True)
    motionAngle = nextWP.angle(botPos)

    if nextNWP.valid() is True:  # this has to be checked
        finalSlope = nextNWP.angle(nextWP)
    else:
        finalSlope = nextWP.angle(botPos)

    finalSlope = point_ball_angle

    turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[bot_id].theta)
    omega = turnAngleLeft * MAX_BOT_OMEGA / (2 * math.pi)  #Speedup turn
    if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
        if omega < 0:
            omega = -MIN_BOT_OMEGA
        else:
            omega = MIN_BOT_OMEGA
    dist = nextWP.dist(botPos)  # Distance of next waypoint from the bot
    theta = motionAngle - state.homePos[
        bot_id].theta  #Angle of dest with respect to bot's frame of reference
    profileFactor = (dist * 2 / MAX_FIELD_DIST) * MAX_BOT_SPEED

    if profileFactor < MIN_BOT_SPEED:
        profileFactor = MIN_BOT_SPEED
    elif profileFactor > MAX_BOT_SPEED:
        profileFactor = MAX_BOT_SPEED

    if dist < BOT_POINT_THRESH:
        if (turnAngleLeft) > -DRIBBLER_BALL_ANGLE_RANGE and (
                turnAngleLeft) < DRIBBLER_BALL_ANGLE_RANGE:
            skill_node.send_command(
                pub, state.isteamyellow, bot_id, 0, 0, 0, 0,
                True)  # previous comment here was kick , needs be checked
        else:
            skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
                                    omega, 0, True)
    else:
        skill_node.send_command(pub, state.isteamyellow, bot_id,
                                profileFactor * math.sin(-theta),
                                profileFactor * math.cos(-theta), omega, 0,
                                False)
Esempio n. 2
0
def execute(param, state, bot_id, pub, dribller=False):
    obs = Vector_Obstacle()
    for i in range(0, len(state.homeDetected)):
        if state.homeDetected[i] and i != bot_id:
            o = Obstacle()
            o.x = state.homePos[i].x
            o.y = state.homePos[i].y
            o.radius = 3.3 * BOT_RADIUS
            obs.push_back(o)

    for j in range(0, len(state.awayDetected)):
        if state.awayDetected[j]:
            o = Obstacle()
            o.x = state.awayPos[j].x
            o.y = state.awayPos[j].y
            o.radius = 3.3 * BOT_RADIUS
            obs.push_back(o)

    pointPos = Vector2D()
    pointPos.x = int(param.GoToPointP.x)
    pointPos.y = int(param.GoToPointP.y)
    point = Vector2D()
    nextWP = Vector2D()
    nextNWP = Vector2D()

    pathplanner = MergeSCurve()

    botPos = Vector2D(int(state.homePos[bot_id].x),
                      int(state.homePos[bot_id].y))

    pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), bot_id,
                     True)
    v = Vector2D()
    distan = botPos.dist(pointPos)
    maxDisToTurn = distan
    angleToTurn = v.normalizeAngle((param.GoToPointP.finalslope) -
                                   (state.homePos[bot_id].theta))

    minReachTime = maxDisToTurn / MAX_BOT_OMEGA
    maxReachTime = maxDisToTurn / MIN_BOT_OMEGA

    minTurnTime = angleToTurn / MAX_BOT_OMEGA
    maxTurnTime = angleToTurn / MIN_BOT_OMEGA

    speed = 0.0
    omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi)

    if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
        if omega < 0:
            omega = -MIN_BOT_OMEGA
        else:
            omega = MIN_BOT_OMEGA

    from math import exp

    speed = 2 * maxDisToTurn * MAX_BOT_SPEED / (HALF_FIELD_MAXX)
    if (speed) < 2 * MIN_BOT_SPEED:
        speed = 2 * MIN_BOT_SPEED
    if (speed > MAX_BOT_SPEED):
        speed = MAX_BOT_SPEED

    vec = Vector2D()

    motionAngle = nextWP.angle(botPos)
    theta = motionAngle - state.homePos[bot_id].theta
    if param.GoToPointP.align == False:
        if distan < DRIBBLER_BALL_THRESH:
            if distan < BOT_BALL_THRESH:
                skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
                                        omega, 0, dribller)

            else:
                skill_node.send_command(pub, state.isteamyellow, bot_id,
                                        speed * math.sin(-theta),
                                        speed * math.cos(-theta), omega, 0,
                                        dribller)
        else:
            skill_node.send_command(pub, state.isteamyellow, bot_id,
                                    speed * math.sin(-theta),
                                    speed * math.cos(-theta), omega, 0,
                                    dribller)
    else:
        if distan > BOT_BALL_THRESH:
            skill_node.send_command(pub, state.isteamyellow, bot_id,
                                    speed * math.sin(-theta),
                                    speed * math.cos(-theta), omega, 0,
                                    dribller)
        else:
            skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
                                    omega, 0, dribller)
Esempio n. 3
0
def execute(param, state, bot_id, pub):
    obs = Vector_Obstacle()
    for i in range(0, len(state.homeDetected)):
        if state.homeDetected[i] and i != bot_id:
            o = Obstacle()
            o.x = state.homePos[i].x
            o.y = state.homePos[i].y
            o.radius = 3.3 * BOT_RADIUS
            obs.push_back(o)

    for j in range(0, len(state.awayDetected)):
        if state.awayDetected[j]:
            o = Obstacle()
            o.x = state.awayPos[j].x
            o.y = state.awayPos[j].y
            o.radius = 3.3 * BOT_RADIUS
            obs.push_back(o)

    ballfinalpos = Vector2D()
    ballfinalpos.x = int(state.ballPos.x +
                         (state.ballVel.x) / POINTPREDICTIONFACTOR)
    ballfinalpos.y = int(state.ballPos.y +
                         (state.ballVel.y) / POINTPREDICTIONFACTOR)

    point = Vector2D()
    nextWP = Vector2D()
    nextNWP = Vector2D()

    pathplanner = MergeSCurve()

    botPos = Vector2D(int(state.homePos[bot_id].x),
                      int(state.homePos[bot_id].y))

    pathplanner.plan(botPos, ballfinalpos, nextWP, nextNWP, obs, len(obs),
                     bot_id, True)

    ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
    dist = ballPos.dist(botPos)
    maxDisToTurn = dist - 3.5 * BOT_BALL_THRESH
    angleToTurn = ballPos.normalizeAngle((ballPos.angle(botPos)) -
                                         (state.homePos[bot_id].theta))

    minReachTime = maxDisToTurn / MAX_BOT_OMEGA
    maxReachTime = maxDisToTurn / MIN_BOT_OMEGA

    minTurnTime = angleToTurn / MAX_BOT_OMEGA
    maxTurnTime = angleToTurn / MIN_BOT_OMEGA

    speed = 0.0
    omega = angleToTurn * MAX_BOT_OMEGA / (2 * math.pi)

    if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
        if omega < 0:
            omega = -MIN_BOT_OMEGA
        else:
            omega = MIN_BOT_OMEGA

    if maxDisToTurn > 0:
        if minTurnTime > maxReachTime:
            speed = MIN_BOT_SPEED
        elif minReachTime > maxTurnTime:
            speed = MAX_BOT_SPEED
        elif minReachTime < minTurnTime:
            speed = maxDisToTurn / minTurnTime
        elif minTurnTime < minReachTime:
            speed = MAX_BOT_SPEED
    else:
        speed = dist / MAX_FIELD_DIST * MAX_BOT_SPEED

    vec = Vector2D()
    motionAngle = nextWP.angle(botPos)
    theta = motionAngle - state.homePos[bot_id].theta

    if param.GoToBallP.intercept == False:
        if dist < DRIBBLER_BALL_THRESH:
            if dist < 2 * BOT_BALL_THRESH:
                skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
                                        0, 0, True)
            else:
                skill_node.send_command(pub, state.isteamyellow, bot_id,
                                        speed * math.sin(-theta),
                                        speed * math.cos(-theta), omega, 0,
                                        True)

        else:
            skill_node.send_command(pub, state.isteamyellow, bot_id,
                                    speed * math.sin(-theta),
                                    speed * math.cos(-theta), omega, 0, False)

    else:
        if dist > BOT_BALL_THRESH:
            skill_node.send_command(pub, state.isteamyellow, bot_id,
                                    speed * math.sin(-theta),
                                    speed * math.cos(-theta), 0, 0, False)
        else:
            skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0,
                                    0, True)