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)
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)
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)
