def execute(param, state, bot_id):
point = Vector2D(param.DribbleTurnP.x, param.DribbleTurnP.y)
botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y)
ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
radius = param.DribbleTurnP.turn_radius
ob = Vector2D()
finalSlope = ob.angle(point, ballPos)
turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[bot_id].theta)
if turnAngleLeft >= -ANGLE_THRES * PI / 180 and turnAngleLeft <= ANGLE_THRES * PI / 180:
omega = ((turnAngleLeft) * 180 /
(ANGLE_THRES * PI)) * param.DribbleTurnP.max_omega
else:
omega = param.DribbleTurnP.max_omega * (-1 if turnAngleLeft < 0 else 1)
phi = ob.normalizeAngle(
atan2(omega * omega * radius, FRICTION_COEFF * ACCN_GRAVITY))
sphi = sin(phi)
cphi = cos(phi)
if omega >= 0:
v_x = omega * radius * sphi
v_y = -omega * radius * cphi
else:
v_x = -omega * radius * sphi
v_y = -omega * radius * cphi
skill_node.send_command(state.isteamyellow, botID, v_x, v_y, omega, 0,
True)
def execute(param, state, bot_id, pub):
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
destPoint = Vector2D(int(param.KickToPointP.x), int(param.KickToPointP.y))
ob = Vector2D()
finalSlope = destPoint.angle(botPos)
turnAngleLeft = ob.normalizeAngle(
finalSlope - state.homePos[bot_id].theta) #Angle left to turn
dist = ballPos.dist(botPos)
if dist > BOT_BALL_THRESH:
sGoToBall.execute(param, state, bot_id, pub)
return
if math.fabs(turnAngleLeft
) > SATISFIABLE_THETA / 2: # SATISFIABLE_THETA in config file
sParam = skills_union.SParam()
sParam.TurnToPointP.x = destPoint.x
sParam.TurnToPointP.y = destPoint.y
sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA * 3
sTurnToPoint.execute(sParam, state, bot_id, pub)
return
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0,
param.KickToPointP.power, False)
def execute(param, state, bot_id, pub):
point = Vector2D(int(param.TurnToPointP.x), int(param.TurnToPointP.y))
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
vec = Vector2D()
finalslope = point.angle(botPos)
turnAngleLeft = vec.normalizeAngle(
finalslope - state.homePos[bot_id].theta) #Angle left to turn
omega = 2.8 * turnAngleLeft * param.TurnToPointP.max_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
v_x = omega * BOT_BALL_THRESH * 1.5
dist = ballPos.dist(botPos)
if dist < DRIBBLER_BALL_THRESH * 4:
skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, 0, omega,
0, True)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega,
0, False)
def execute(param, state, bot_id, pub):
debug(param,state,bot_id)
botPos = Vector2D(int(state.homePos[bot_id].x), int(state.homePos[bot_id].y))
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
vec = Vector2D()
finalslope = botPos.angle(ballPos)
turnAngleLeft = vec.normalizeAngle(finalslope - state.homePos[bot_id].theta) #Angle left to turn
omega = 1*(math.pi)*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
if turnAngleLeft == 0:
omega = 0
v_x = omega * BOT_BALL_THRESH * 1.5
dist = ballPos.dist(botPos)
if(fabs(turnAngleLeft)<SATISFIABLE_THETA/2.0):
omega=0
if dist < DRIBBLER_BALL_THRESH * 2:
skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, 0, omega, 0, True)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0, False)
def execute(param, state, bot_id, pub):
print("Testing Motion")
speed = param.speed
motionAngle = param.motionAngle
omega = param.omega
theta = state.homePos[bot_id].theta - motionAngle
print("Velocity Tangent", speed * math.cos(theta))
print("Velocity Normal", -speed * math.sin(theta))
skill_node.send_command(pub, state.isteamyellow, bot_id,
-speed * math.sin(theta), -speed * math.cos(theta),
omega, 0, False)
def execute(param, state, bot_id, pub,dribbler = False):
t = rospy.Time.now()
t = t.secs + 1.0*t.nsecs/pow(10,9)
start_time = float(os.environ.get('bot'+str(bot_id)))
[vx, vy, vw, REPLANNED, remainingPath] = Get_Vel(start_time, t, bot_id, state.ballPos, state.homePos, state.awayPos) #vx, vy, vw, replanned, remainingPath
if(REPLANNED):
reset(bot_id)
try:
skill_node.send_command(pub, False, bot_id, vx, vy, vw, 0,0)
except Exception as e:
print("In except",e)
pass
def execute(param, state, bot_id, pub):
point = Vector2D(int(param.GoWithDribbleP.x), int(param.GoWithDribbleP.y))
myPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
distance = point.dist(myPos)
MAX_BOT_SPEED = 0.7 * 1800
speed = 2 * distance * MAX_BOT_SPEED / (HALF_FIELD_MAXX)
from math import *
theta = myPos.angle(point)
print(theta)
theta = point.normalizeAngle(theta + state.homePos[bot_id].theta)
vel_x = speed * cos(theta)
vel_y = speed * sin(theta)
skill_node.send_command(pub, state.isteamyellow, bot_id, vel_x, vel_y, 0,
0, True)
def execute(param, state, bot_id, pub):
# print "in kicktopoint"
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
destPoint = Vector2D(int(param.KickToPointP.x), int(param.KickToPointP.y))
ob = Vector2D()
finalSlope = destPoint.angle(botPos)
turnAngleLeft = ob.normalizeAngle(
finalSlope - state.homePos[bot_id].theta) #Angle left to turn
ballBotAngle = ob.normalizeAngle(
ballPos.angle(botPos) - state.homePos[bot_id].theta)
dist = ballPos.dist(botPos)
if dist > BOT_BALL_THRESH + 10:
print("before kick (GoToBall) dist remaining : ",
dist - BOT_BALL_THRESH, " : ", BOT_BALL_THRESH)
param.GoToPointP.x = state.ballPos.x
param.GoToPointP.y = state.ballPos.y
param.GoToPointP.finalSlope = ballPos.angle(state.homePos[bot_id])
sGoToPoint.execute(param, state, bot_id, pub)
return
# print("After gotoball")
if ballBotAngle > SATISFIABLE_THETA / 2:
sParam = skills_union.SParam()
sParam.TurnToPointP.x = ballPos.x
sParam.TurnToPointP.y = ballPos.y
sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA
print("before kick (TurnToBall) angle remaining : ", ballBotAngle)
sTurnToPoint.execute(sParam, state, bot_id, pub)
return
if math.fabs(turnAngleLeft
) > SATISFIABLE_THETA / 2: # SATISFIABLE_THETA in config file
sParam = skills_union.SParam()
sParam.TurnToPointP.x = destPoint.x
sParam.TurnToPointP.y = destPoint.y
sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA
print("before kick (Turn) angle remaining : ", turnAngleLeft)
sTurnToPoint.execute(sParam, state, bot_id, pub)
return
# print("after turn")
print("____KICK____ DIST :", dist, " ANGLE : ", turnAngleLeft)
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0,
param.KickToPointP.power, False)
def execute(param, state, bot_id,pub, dribbler=False): debug(param,state,bot_id) finalSlope = param.TurnToAngleP.finalSlope # Yet to be defined ballPos = Vector2D(int(state.ballPos.x),int(state.ballPos.y)) botPos = Vector2D(int(state.homePos[bot_id].x),int(state.homePos[bot_id].y)) obj=Vector2D() turnAngleLeft = obj.normalizeAngle(finalSlope - state.homePos[bot_id].theta); # Angle left to turn omega = 3*turnAngleLeft * MAX_BOT_OMEGA/(2*math.pi); # Speedup turn if(omega < 1.43*MIN_BOT_OMEGA and omega > -1.43*MIN_BOT_OMEGA): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega if(omega < 0): omega = -1.43*MIN_BOT_OMEGA else: omega = 1.43*MIN_BOT_OMEGA dist = ballPos.dist(botPos) if(dist < DRIBBLER_BALL_THRESH): skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,True) else: skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,False)
def execute(param, state, bot_id, pub):
R = param.DribbleTurnP.turn_radius
v = param.DribbleTurnP.max_velocity
K = 1000
fp = Vector2D(param.DribbleTurnP.x,param.DribbleTurnP.y)
botpointangle = fp.normalizeAngle(fp.angle(state.homePos[bot_id]))
print "Bot Pos: {} {}".format(state.homePos[bot_id].x,state.homePos[bot_id].y)
print "Bot Point angle: {}".format(fp.angle(state.homePos[bot_id])*180/math.pi)
sign = -1 if fp.normalizeAngle(fp.normalizeAngle(state.homePos[bot_id].theta)-fp.normalizeAngle(fp.angle(state.homePos[bot_id]))) >= 0 else 1
print "Sign: {}".format(sign)
ta = (v*v)/(K*R)
a = sign*abs(math.atan(ta))
print "tananlpha: {} alpha: {}".format(ta,a*180/math.pi)
v_y = v*math.cos(a)
v_x = v*math.sin(a)
w = sign*v/R
print "w: {}".format(w)
skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, v_y, w, 0, 1)
def execute(param, state, bot_id):
finalSlope = param.TurnToAngleP.finalslope # Yet to be defined
obj = Vector2D()
turnAngleLeft = obj.normalizeAngle(finalSlope - state.homePos[botID].theta)
# Angle left to turn
omega = turnAngleLeft * MAX_BOT_OMEGA / (2 * math.pi)
# Speedup turn
if (
omega < MIN_BOT_OMEGA / 2 and omega > -MIN_BOT_OMEGA / 2
): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega
if (omega < 0): omega = -MIN_BOT_OMEGA / 2
else: omega = MIN_BOT_OMEGA / 2
dist = Vector2D(ballPos, botPos)
if (dist < DRIBBLER_BALL_THRESH):
skill_node.send_command(state.isteamyellow, bot_id, 0, 0, omega, 0,
true)
else:
skill_node.send_command(state.isteamyellow, bot_id, 0, 0, omega, 0,
false)
def execute(param, state, bot_id):
botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y)
ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
destPoint = Vector2D(param.KickToPointP.x, param.KickToPointP.y)
ob = Vector2D()
finalSlope = ob.angle(destPoint, botPos)
turnAngleLeft = ob.normalizeAngle(
finalSlope - state.homePos[bot_id].theta) #Angle left to turn
dist = ob.dist(ballPos, botPos)
if dist > BOT_BALL_THRESH:
sGoToBall.execute(param, state, bot_id)
if math.fabs(turnAngleLeft
) > SATISFIABLE_THETA / 2: # SATISFIABLE_THETA in config file
paramt = Sparam()
paramt.TurnToPointP.x = destPoint.x
paramt.TurnToPointP.y = destPoint.y
paramt.TurnToPointP.max_omega = MAX_BOT_OMEGA * 3
sTurnToPoint.execute(paramt, state, bot_id)
skill_node.send_command(state.isteamyellow, bot_id, 0, 0, 0,
param.KickToPointP.power, False)
def execute(param, state, bot_id, gv, pub, dribller=False):
# debug(param,state, bot_id)
pointPos = Vector2D()
pointPos.x = int(param.GoToPointP.x)
pointPos.y = int(param.GoToPointP.y)
t = rospy.Time.now()
t = t.secs + 1.0 * t.nsecs / pow(10, 9)
[vx, vy, vw, REPLANNED,
maxDisToTurn] = gv[bot_id].execute(t, bot_id, pointPos, state.homePos,
state.awayPos) #vx, vy, vw, replanned
if (REPLANNED):
print("REPLANNED {}".format(bot_id))
reset(bot_id)
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
v = Vector2D()
distan = botPos.dist(pointPos)
ballPos = Vector2D(state.ballPos)
# maxDisToTurn = distan
if param.GoToPointP.align == True:
angleToTurn = pointPos.normalizeAngle(param.GoToPointP.finalSlope -
state.homePos[bot_id].theta)
else:
angleToTurn = pointPos.normalizeAngle(
botPos.angle(ballPos) - state.homePos[bot_id].theta)
# 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
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
if angleToTurn == 0:
omega = 0
print("bot id{} vx: {}, vy:{}".format(bot_id, vx, vy))
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, vx,
vy, omega, 0, dribller)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy,
omega, 0, dribller)
else:
if distan > BOT_BALL_THRESH:
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy,
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, kicking=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 - 5.0 * BOT_BALL_THRESH / 4
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 = 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 = distan / MAX_FIELD_DIST * MAX_BOT_SPEED
vec = Vector2D()
motionAngle = nextWP.angle(botPos)
theta = motionAngle - state.homePos[bot_id].theta
if math.fabs(speed) < 64 * MIN_BOT_SPEED:
speed = 64 * MIN_BOT_SPEED
if kicking == True:
force = 7
else:
force = 0
# debug(param, state, botPos, pointPos, nextWP, nextNWP, speed, theta, omega, obs)
# print "Motion angle : {}".format(motionAngle)
speed = MAX_BOT_SPEED / 1.2
# omega = 0
if param.GoToPointP.align == False:
if distan < DRIBBLER_BALL_THRESH:
print "here"
if distan < 0.9 * BOT_BALL_THRESH:
skill_node.send_command(pub, state, bot_id, 0, 0, 0, 0, False)
else:
skill_node.send_command(pub, state, bot_id,
speed * math.sin(-theta),
speed * math.cos(-theta), omega, force,
False)
else:
skill_node.send_command(pub, state, bot_id,
speed * math.sin(-theta),
speed * math.cos(-theta), omega, force,
False)
else:
if distan > BOT_BALL_THRESH / 4:
skill_node.send_command(pub, state, bot_id,
speed * math.sin(-theta),
speed * math.cos(-theta), 0, force, False)
else:
skill_node.send_command(pub, state, bot_id, 0, 0, 0, force, False)
def execute(param, state, bot_id, pub, dribbler=False):
debug(param, state, bot_id)
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 = 2.5 * 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 = 2.5 * 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,
not state.isteamyellow)
# Komega = 1.5
# Kvel = 1.7
# netDist = pointPos.dist(state.homePos[bot_id])
# curDist = nextWP.dist(state.homePos[bot_id])
# theta_cov = nextWP.normalizeAngle(nextWP.normalizeAngle(param.GoToPointP.finalSlope) - nextWP.normalizeAngle(state.homePos[bot_id].theta))
# theta_cur = theta_cov * curDist/netDist
# v_x = -25*Kvel*curDist*math.sin(theta_cur)/HALF_FIELD_MAXX
# v_y = 25*Kvel*curDist*math.cos(theta_cur)/HALF_FIELD_MAXX
# omega = Komega*theta_cur
# print v_x,",",v_y
# skill_node.send_command(pub, state.isteamyellow, bot_id, v_x, v_y, omega, 0, dribbler)
dist = pointPos.dist(botPos)
maxDisToTurn = dist - DRIBBLER_BALL_THRESH
angleToTurn = 0
ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
if param.GoToPointP.align == True:
angleToTurn = pointPos.normalizeAngle(param.GoToPointP.finalSlope -
state.homePos[bot_id].theta)
else:
angleToTurn = pointPos.normalizeAngle(
botPos.angle(ballPos) - state.homePos[bot_id].theta)
# angleToTurn = pointPos.normalizeAngle(-math.pi+(pointPos.angle(botPos))-(state.homePos[bot_id].theta))
minReachTime = maxDisToTurn / MAX_BOT_SPEED
maxReachTime = maxDisToTurn / MIN_BOT_SPEED
minTurnTime = angleToTurn / MAX_BOT_OMEGA
maxTurnTime = angleToTurn / MIN_BOT_OMEGA
speed = 0.0
omega = angleToTurn * MAX_BOT_OMEGA / (2 * math.pi)
if angleToTurn:
if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
if omega < 0:
omega = -MIN_BOT_OMEGA
else:
omega = MIN_BOT_OMEGA
else:
omega = 0.0
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 = -state.homePos[bot_id].theta + motionAngle
if dist < DRIBBLER_BALL_THRESH:
if dist < 1 * BOT_BALL_THRESH:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
omega, 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)
def execute(param, state, bot_id, pub, dribller=False):
pointPos = Vector2D()
pointPos.x = int(state.ballPos.x)
pointPos.y = int(state.ballPos.y)
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballPos = Vector2D(state.ballPos)
oppGoal = Vector2D(HALF_FIELD_MAXX, 0)
v = Vector2D()
targetPoint = Vector2D()
# maxDisToTurn = distan
if param.GoToBallP.align:
angle = param.GoToBallP.finalslope
angleToTurn = v.normalizeAngle(param.GoToBallP.finalslope -
state.homePos[bot_id].theta)
else:
angle = ballPos.angle(oppGoal)
angleToTurn = v.normalizeAngle(
ballPos.angle(oppGoal) - state.homePos[bot_id].theta)
# angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta))
targetPoint.x = ballPos.x - 2 * DRIBBLER_BALL_THRESH * cos(angle)
targetPoint.y = ballPos.y - 2 * DRIBBLER_BALL_THRESH * sin(angle)
distan = botPos.dist(targetPoint)
t = rospy.Time.now()
t = t.secs + 1.0 * t.nsecs / pow(10, 9)
start_time = float(os.environ.get('bot' + str(bot_id)))
if distan <= DRIBBLER_BALL_THRESH:
# print("in DRIBBLER_BALL_THRESH")
[vx, vy, vw, REPLANNED,
maxDisToTurn] = Get_Vel(start_time,
t,
bot_id,
state.ballPos,
state.homePos,
state.awayPos,
avoid_ball=False) #vx, vy, vw, replanned
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
if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP:
omega = 0
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, vx, vy,
omega, 0, dribller)
else:
[vx, vy, vw, REPLANNED,
maxDisToTurn] = Get_Vel(start_time,
t,
bot_id,
targetPoint,
state.homePos,
state.awayPos,
avoid_ball=True) #vx, vy, vw, replanned
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
if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP:
omega = 0
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega,
0, dribller)
if (REPLANNED):
reset(bot_id)
def execute(param, state, bot_id):
skill_node.send_command(state.isteamyellow, bot_id, 0, 0, 0,
param.KickP.power, False)
def execute(param, state, bot_id, pub):
debug(param, state, bot_id)
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, not state.isteamyellow)
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
dist = ballPos.dist(botPos)
maxDisToTurn = dist - DRIBBLER_BALL_THRESH
angleToTurn = ballPos.normalizeAngle(-math.pi + (ballPos.angle(botPos)) -
(state.homePos[bot_id].theta))
minReachTime = maxDisToTurn / MAX_BOT_SPEED
maxReachTime = maxDisToTurn / MIN_BOT_SPEED
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
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 = -state.homePos[bot_id].theta + motionAngle
if dist < DRIBBLER_BALL_THRESH:
if dist < 1 * BOT_BALL_THRESH:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
omega, 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)
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):
obs = []
for i in range(0, len(state.homeDetected)):
if state.homeDetected[i]:
o = obstacle()
o.x = state.homePos[i].x
o.y = state.homePos[i].y
o.radius = 2.5 * BOT_RADIUS
obs.append(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 = 2.5 * BOT_RADIUS
obs.append(o)
pointPos = Vector2D()
pointPos.x = param.GoToPointP.x
pointPos.y = param.GoToPointP.y
point = Vector2D()
nextWP = Vector2D()
nextNWP = Vector2D()
pathplanner = MergeSCurve()
botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y)
pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), bot_id,
True)
v = Vector2D()
dist = v.dist(botPos, pointPos)
maxDisToTurn = dist - 3.5 * BOT_BALL_THRESH
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 = 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 = vec.angle(nextWP, botPos)
theta = motionAngle - state.homePos[bot_id].theta
if param.GoToPointP.align == False:
if dist < DRIBBLER_BALL_THRESH:
if dist < BOT_BALL_THRESH:
skill_node.send_command(bot_id, 0, 0, 0, 0, True)
else:
skill_node.send_command(bot_id, speed * math.sin(-theta),
speed * math.cos(-theta), omega, 0,
True)
else:
skill_node.send_command(bot_id, speed * math.sin(-theta),
speed * math.cos(-theta), omega, 0, False)
else:
if dist > BOT_BALL_THRESH:
skill_node.send_command(bot_id, speed * math.sin(-theta),
speed * math.cos(-theta), 0, 0, False)
else:
skill_node.send_command(bot_id, 0, 0, 0, 0, True)
def execute(param,state,bot_id, pub,dribbler = 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=2.5*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=2.5*BOT_RADIUS
obs.push_back(o)
pointPos = Vector2D()
pointPos.x = int(param.GoToPointP.x)
pointPos.y = int(param.GoToPointP.y)
point = Vector2D()
nextWP = pointPos
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)
speed= 4*maxDisToTurn*MAX_BOT_SPEED/(HALF_FIELD_MAXX)
if (speed)< 2*MIN_BOT_SPEED:
speed=2*MIN_BOT_SPEED
if (speed >= MAX_BOT_SPEED/2.0):
speed=MAX_BOT_SPEED
# omega = 0
vec = Vector2D()
motionAngle = nextWP.angle(botPos)
theta = motionAngle - state.homePos[bot_id].theta
if distan >= 20 * BALL_RADIUS :
speed = MAX_BOT_SPEED
skill_node.send_command(pub, state.isteamyellow, bot_id, speed * math.sin(-theta), speed * math.cos(-theta), omega, 0, dribbler)
def execute(param, state, bot_id, pub):
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, 0, 0, False)
def execute(param, state, bot_id, pub):
centre = Vector2D(int(param.MoveOnArcP.centrex),
int(param.MoveOnArcP.centrey))
finalPoint = Vector2D(int(param.MoveOnArcP.finalx),
int(param.MoveOnArcP.finaly))
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
print "\n\n\n\n\n\n"
print " in MoveOnArc bot no " + str(bot_id) + " got final point " + str(
finalPoint.x) + str(finalPoint.y)
if (math.fabs(botPos.dist(centre) - finalPoint.dist(centre)) >
RADIUS_THRESH):
param.GoToPointP.x = finalPoint.x
param.GoToPointP.y = finalPoint.y
param.GoToPointP.finalslope = ballPos.angle(finalPoint)
sGoToPoint.execute(param, state, bot_id, pub)
return
print "HEEEEEEEEEEEEEEEEEEEEE"
Radius = botPos.dist(centre)
AngleLeft = botPos.normalizeAngle(
finalPoint.angle(centre) - botPos.angle(centre))
print " botPos=" + str(botPos.x) + "," + str(botPos.y)
print " finalPoint=" + str(finalPoint.x) + "," + str(finalPoint.y)
print " AngleLeft=" + str(AngleLeft * 180 / math.pi)
print " Radius=" + str(Radius)
print " centre=" + str(centre.x) + "," + str(centre.y)
speed = MAX_BOT_SPEED * AngleLeft / (2 * math.pi)
if (speed < 16 * MIN_BOT_SPEED):
speed = 16 * MIN_BOT_SPEED
v_x = -speed * math.sin(botPos.angle(centre))
v_y = +speed * math.cos(botPos.angle(centre))
if (AngleLeft < 0):
v_y *= -1
v_x *= -1
theta = state.homePos[bot_id].theta
v_oy = v_x * math.cos(theta) + v_y * math.sin(theta)
v_ox = v_x * math.sin(theta) - v_y * math.cos(theta)
print " v_x=" + str(v_x) + " v_y=" + str(v_y)
turnAngleLeft = botPos.normalizeAngle(
ballPos.angle(finalPoint) -
(state.homePos[bot_id].theta)) # Angle left to turn
omega = turnAngleLeft * MAX_BOT_OMEGA / (2 * math.pi)
# Speedup turn
if (
omega < MIN_BOT_OMEGA / 2 and omega > -MIN_BOT_OMEGA / 2
): # This is a rare used skill so believe in Accuracy more than speed. Hence reducing minimum Omega
if (omega < 0): omega = -MIN_BOT_OMEGA / 2
else: omega = MIN_BOT_OMEGA / 2
#dist = ballPos.dist(botPos)
#if(dist < DRIBBLER_BALL_THRESH):
#skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega, 0,False)
#omega=0
if math.fabs(AngleLeft) < ANGLE_THRES:
print "0 vel "
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0, omega,
0, True)
else:
print "v_ox=" + str(v_ox) + " v_oy=" + str(v_oy)
skill_node.send_command(pub, state.isteamyellow, bot_id, v_ox, v_oy,
omega, 0, True)
def execute(param, state, bot_id, pub):
skill_node.send_command(pub, state.isteamyellow, bot_id,
param.VelocityP.v_x, param.VelocityP.v_y,
param.VelocityP.v_t, 0, True)
def execute(param, state, bot_id):
skill_node.send_command(state.isteamyellow, bot_id, 0, 0,
param.SpinP.radPerSec, 0, false)
def execute(param, state, bot_id):
point = Vector2D(int(param.DribbleToPointP.x),
int(param.DribbleToPointP.y))
botPos = Vector2D(int(state.homePos[botID].x), int(state.homePos[botID].y))
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
ballSlope = ballPos.angle(botPos)
ballDist = ballPos.dist(botPos)
ballTheta = math.fabs(
ballDist.normalizeAngle(state.homePos[botID].theta - ballSlope))
# if(ballDist > BOT_BALL_THRESH || ballTheta > DRIBBLER_BALL_ANGLE_RANGE):
# return goToBall(param, state, botID) # No python equivalent found
obs = []
j = 0
for i, bot in enumerate(state.homePos):
if state.homeDetected[i]:
obs.append(Obstacle(bot.x, bot.y, 0, 0, 3 * BOT_RADIUS))
j += 1
for i in state.homePos:
if state.homeDetected[i]:
x = state.awayPos[i - (j - 1)].x
y = state.awayPos[i - (j - 1)].y
radius = 3 * BOT_RADIUS
o = Obstacle(x, y, 0, 0, radius)
obs.append(o)
nextWP = Vector2D()
nextNWP = Vector2D
pathplanner = MergeSCurve()
pathplanner.plan(botPos, pointPos, nextWP, nextNWP, obs, len(obs), botID,
True)
if (nextWP.valid()):
pass
# comm.addCircle(nextWP.x, nextWP.y, 50)
# comm.addLine(state.homePos[botID].x, state.homePos[botID].y, nextWP.x, nextWP.y) #Python equivalent for comm not found
if (nextNWP.valid()):
pass
# comm.addCircle(nextNWP.x, nextNWP.y, 50)
# comm.addLine(nextWP.x, nextWP.y, nextNWP.x, nextNWP.y)
# rospy.loginfo("Dribbling\n")
dist = nextWP.dist(botPos)
# angle = nextWP.angle(botPos)
angle = param.DribbleToPointP.finalslope
theta = nextWP(normalizeAngle(state.homePos[botID].theta - angle))
profileFactor = 2 * dist * MAX_BOT_SPEED / MAX_FIELD_DIST
if (math.fabs(profileFactor) < MIN_BOT_SPEED):
if (profileFactor < 0):
profileFactor = -MIN_BOT_SPEED
else:
profileFactor = MIN_BOT_SPEED
v_y = profileFactor * math.cos(theta)
v_x = profileFactor * math.sin(theta)
romega = theta / (2 * math.pi) * MAX_BOT_OMEGA
if (v_y < -MAX_BACK_DRIBBLE_V_Y):
v_x /= (v_y / -MAX_BACK_DRIBBLE_V_Y)
v_y = -MAX_BACK_DRIBBLE_V_Y
elif (v_y > MAX_FRONT_DRIBBLE_V_Y):
v_x /= (v_y / MAX_FRONT_DRIBBLE_V_Y)
v_y = MAX_FRONT_DRIBBLE_V_Y
if (v_x > MAX_DRIBBLE_V_X):
v_y /= (v_x / MAX_DRIBBLE_V_X)
v_x = MAX_DRIBBLE_V_X
elif (v_x < -MAX_DRIBBLE_V_X):
v_y /= (v_x / -MAX_DRIBBLE_V_X)
v_x = -MAX_DRIBBLE_V_X
if (math.fabs(theta) > SATISFIABLE_THETA):
if (math.fabs(romega < MIN_BOT_OMEGA)):
if (romega > 0):
romega = MIN_BOT_OMEGA
else:
romega = -MIN_BOT_OMEGA
elif (fabs(romega) > MAX_DRIBBLE_R):
if (romega > 0):
romega = MAX_DRIBBLE_R
else:
romega = -MAX_DRIBBLE_R
else:
romega = 0
if (dist < BOT_BALL_THRESH):
skill_node.send_command(state.isteamyellow, bot_id, 0, 0, 0, 0, True)
else:
skill_node.send_command(state.isteamyellow, bot_id, v_x, v_y, -romega,
0, True)
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)
import skill_node
import maths
import sGoToBall
import sTurnToPoint
def execute(param, state, bot_id):
botPos=Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y)
ballPos=Vector2D(state.ballPos.x, state.ballPos.y)
destPoint=Vector2D(param.KickToPointP.x, param.KickToPointP.y)
ob = Vector2D()
finalSlope = ob.angle(destPoint, botPos)
turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[bot_id].theta) #Angle left to turn
dist = ob.dist(ballPos, botPos)
if dist > BOT_BALL_THRESH :
return sGoToBall.execute(param,state,bot_id)
if math.fabs(turnAngleLeft) > SATISFIABLE_THETA/2 : # SATISFIABLE_THETA in config file
Sparam paramt()
paramt.TurnToPointP.x = destPoint.x
paramt.TurnToPointP.y = destPoint.y
paramt.TurnToPointP.max_omega = MAX_BOT_OMEGA*3
return sTurnToPoint.execute(paramt, state, bot_id)
skill_node.send_command(state.isteamyellow, bot_id ,0, 0, 0, param.KickToPointP.power, False)
