diff --git a/PlaySelector.py b/PlaySelector.py index 7841b13..4ee9d51 100644 --- a/PlaySelector.py +++ b/PlaySelector.py @@ -3,7 +3,6 @@ import rospy from krssg_ssl_msgs.msg import BeliefState from krssg_ssl_msgs.msg import gr_Commands -from krssg_ssl_msgs.msg import Referee import threading import time from std_msgs.msg import Int8 @@ -18,8 +17,11 @@ from tactics import TDTP,TAttacker from tactics import TTestIt +from skills import skills_union from skills import sStop -from skills import skill_node +from skills import sGoToPoint +from utils.role_selector import * + from plays import pStall from plays import DTP_Play from plays import pCordinatedPass @@ -35,6 +37,7 @@ RDefender_tac = None cur_goalie = 0 +prev_attacker = 3 @@ -56,20 +59,9 @@ def select_play(state): # play_Defence = pDefence.pDefence() # play_Defence.execute() -def referee_callback(msg): - print "Time: {}, Stage: {}, Command: {}".format(msg.packet_timestamp, msg.stage, msg.command) - # global pub, ref_command - # if msg.command == 0: - # ref_command = True - # for i in xrange(6): - # os.environ['bot'+str(i)]=str(start_time) - # skill_node.send_command(pub, isteamyellow, i, 0, 0, 0, 0, False) - # else: - # ref_command = False - # print ("Received referee message",msg.command) - # print ("packet_timestamp ",msg.packet_timestamp) - # global ref_play_id - # ref_play_id = play_id +def ref_callback(play_id): + global ref_play_id + ref_play_id = play_id def goalKeeper_callback(state): global pub,goalie_tac,cur_goalie @@ -83,11 +75,16 @@ def goalKeeper_callback(state): def attacker_callback(state): - global pub - attacker_id = 5 + global pub,prev_attacker + # print "bef as" + attacker_id = attacker_selector(state) param = skills_union.SParam() - cur_tactic = TAttacker.TAttacker(attacker_id,state,param) - cur_tactic.execute(state,pub) + if prev_attacker != attacker_id : + sStop.execute(param, state, prev_attacker, pub) + else: + cur_tactic = TAttacker.TAttacker(attacker_id,state,param) + cur_tactic.execute(state,pub) + prev_attacker = attacker_id print ("attacker : ",attacker_id) def LDefender_callback(state): @@ -112,16 +109,15 @@ def RDefender_callback(state): RDefender_tac.execute(state,pub) def planner_callback(state): - global ref_command, pub - if not ref_command: - # print("incoming planner_callback") - bot_id = 0 - ballPos = Vector2D(state.ballPos.x, state.ballPos.y) - botpos = Vector2D(state.homePos[bot_id].x,state.homePos[bot_id].y) - # print("dist is ",ballPos.dist(botpos)) - new = TestTac.TestTac(bot_id,state) - new.execute(state,pub) - # print(" outgoing planner_callback") + print("incoming planner_callback") + global pub + bot_id = 0 + ballPos = Vector2D(state.ballPos.x, state.ballPos.y) + botpos = Vector2D(state.homePos[bot_id].x,state.homePos[bot_id].y) + print("dist is ",ballPos.dist(botpos)) + new = TestTac.TestTac(bot_id,state) + new.execute(state,pub) + print(" outgoing planner_callback") def bs_callback(state): global cur_play,start_time @@ -182,6 +178,7 @@ def main(): # gv.append(GetVelocity(start_time = start_time,kubs_id = i)) pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000) + # rospy.Subscriber("/ref_data", Referee, referee_callback, queue_size=1000) # rospy.Subscriber('/belief_state', BeliefState, bs_callback, queue_size=1000) # rospy.Subscriber('/belief_state', BeliefState, goalKeeper_callback, queue_size=1000) @@ -190,7 +187,7 @@ def main(): # rospy.Subscriber('/belief_state', BeliefState, LDefender_callback, queue_size=1000) # rospy.Subscriber('/belief_state', BeliefState, planner_callback, queue_size=1000) # rospy.Subscriber('/belief_state', BeliefState, RDefender_callback, queue_size=1000) - # rospy.Subscriber('/belief_state', BeliefState, attacker_callback, queue_size=1000) + rospy.Subscriber('/belief_state', BeliefState, attacker_callback, queue_size=1000) #rospy.Subscriber('/ref_play', Int8, ref_callback, queue_size=1000) rospy.spin() diff --git a/skills/sSpin.py b/skills/sSpin.py index 7826484..4886cc6 100644 --- a/skills/sSpin.py +++ b/skills/sSpin.py @@ -1,4 +1,4 @@ import skill_node -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,pub): + skill_node.send_command(pub , state.isteamyellow , bot_id , 0 , 0 , param.SpinP.radPerSec , 0 , False) diff --git a/tactics/TAttacker.py b/tactics/TAttacker.py index 120e08c..e9996e5 100644 --- a/tactics/TAttacker.py +++ b/tactics/TAttacker.py @@ -3,6 +3,11 @@ import sys from math import * +import enum + +##################### +#change opp_goalie## +#################### from utils.config import * from utils.geometry import * @@ -14,481 +19,172 @@ from skills import sStop from tactic import Tactic from skills import sGoToBall +from skills import sTurnToPoint +from evo_fuzz import * import numpy as np from numpy import array,inf from isPossible import isPossible -KICK_RANGE_THRESH = MAX_DRIBBLE_R #ASK -THRES = 0.8 -THETA_THRESH = 0.005 -TURNING_THRESH = 10 -# k=k2=50000 no threshold -THRESH = pi**2 /180 -BOT_OPP_DIST_THRESH = 500 - -ROWS = 33 -COLS = 22 -OPTIMAL_DISTANCE = 3.0*HALF_FIELD_MAXX/5 -PASSING_PARAMETER = 5 -BISECTOR_CONST = 70 -OUR_BOT_APPROACHABILITY_CONST = 35 -OPTIMAL_DISTANCE_CONST = 10 -AWAY_BOT_APP_GWT = 30 -BOT_ORIENT_CONST = 0.2 -KICK_DISTANCE_CONST = 90 -PASS_PROB_THRESH = 2.0 -k=k2=20*pi/180 +k1 = True +k2 = True class TAttacker(Tactic): - def __init__(self, bot_id, state, param=None): - super(TAttacker, self).__init__( bot_id, state, param) - self.oh_my_bot =-1 - self.bot_id = bot_id - self.sParams = skills_union.SParam() - self.UPPER_HALF = Vector2D(-HALF_FIELD_MAXX,OUR_GOAL_MAXY) - self.LOWER_HALF = Vector2D(-HALF_FIELD_MAXX,OUR_GOAL_MINY) - - self.GOAL_UPPER = Vector2D(HALF_FIELD_MAXX,OUR_GOAL_MAXY*3) - self.GOAL_LOWER = Vector2D(HALF_FIELD_MAXX,OUR_GOAL_MINY*3) - - - def Dist(self,a,state): - a=a[0] - return sqrt(pow((a.x-state.homePos[self.bot_id].x),2)+ pow((a.y-(state.homePos[self.bot_id].y)),2)) - - - def bisector_bot_wt(self,state): - - botPos = Vector2D(int(state.homePos[self.bot_id].x), int(state.homePos[self.bot_id].y)) - ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) - DistancesFromBot=999999999999 - count=0 - closest_opp=0 - for i in state.awayPos: - botIDs=Vector2D(int(i.x),int(i.y)) - dist=botIDs.dist(ballPos) - if dist=len(self.Opp_sorted_Angles): j=0 - # if self.Opp_sorted_Angles[j][0]-self.Opp_sorted_Angles[i][1]>angle_gap: - # angle_here=self.Opp_sorted_Angles[j][0]-self.Opp_sorted_Angles[i][1] - # if angle_here>pi: - # angle_to_shoot.extend([origin_angle+(angle_here/3),origin_angle+(angle_here*2/3)]) - # else: - # angle_to_shoot.append(origin_angle+angle_here/2) - - - for i in xrange(len(self.Opp_sorted_Angles)): - origin_angle=self.Opp_sorted_Angles[i][1] - j=i+1 - flag=0 - if j>=len(self.Opp_sorted_Angles): j=0; flag=1 - diff=self.Opp_sorted_Angles[j][0]-self.Opp_sorted_Angles[i][1] - - if flag: - diff*=-1 - diff = 2*pi -diff - - - if (diff>angle_gap) : - angle_here=diff - ##print "angle here inside: "+str(array(angle_here)*(180/pi)) - if angle_here>pi: - angle_to_shoot.extend([origin_angle+(angle_here/3),origin_angle+(angle_here*2/3)]) - else: - angle_to_shoot.append(origin_angle+angle_here/2) - - ##print("Angle to shoot "+ str(array(angle_to_shoot)*(180/pi))) - - for i in xrange(len(state.homePos)): - if i==self.bot_id or i==state.our_goalie: - continue - dist=fabs(Vector2D(int(state.homePos[i].x),int(state.homePos[i].y)).dist(Vector2D(int(state.homePos[self.bot_id].x),int(state.homePos[self.bot_id].y)))) - angle_here=Vector2D(int(state.homePos[i].x),int(state.homePos[i].y)).angle(Vector2D(int(state.homePos[self.bot_id].x),int(state.homePos[self.bot_id].y))) - state.homePos[self.bot_id].theta - min_dist= np.inf - # #print "angles to shoot "+str(angle_to_shoot) - for angles in angle_to_shoot: - angleneeded=fabs(angle_here-angles) - min_dist_here=dist*sin(angleneeded) - if min_dist_here=pi: - # goal_angle_upper,goal_angle_lower=goal_angle_lower,goal_angle_upper - # #print "goal angle lower : "+str(goal_angle_lower*180/pi) - # #print "goal angle upper: "+str(goal_angle_upper*180/pi) - # #goal_angle_lower = atan2(LOWER_HALF.y-state.homePos[self.bot_id].y, LOWER_HALF.x-state.homePos[self.bot_id].x) - for i in xrange(len(state.homePos)): - if i==self.bot_id or i==state.our_goalie: - continue - myBotPos=Vector2D(int(state.homePos[i].x),int(state.homePos[i].y)) - angle=myBotPos.angle(botPos)-state.homePos[self.bot_id].theta - ourGoal=Vector2D(int(-HALF_FIELD_MAXX),int(0)) - ##print "angle for bot "+str(i)+" is "+str(angle*180/pi) - flag1 =0 - for angles in self.Opp_sorted_Angles: - if (angle>=angles[0] and angle<=angles[1]) : - - dist_of_homeBot=myBotPos.dist(botPos) - dist_of_awayBot=Vector2D(int(state.awayPos[angles[2]].x),int(state.awayPos[angles[2]].y)).dist(botPos) - - if fabs(dist_of_awayBot) goal_angle_upper and bot_angle < goal_angle_lower and state.homePos[our_bot].x < botPos.x : - self.bots_to_remove.append(our_bot) - else: - if bot_angle > goal_angle_upper and bot_angle < goal_angle_lower and state.homePos[our_bot].x > botPos.x : - self.bots_to_remove.append(our_bot) - - ##print("bots to remove ", self.bots_to_remove) - # print("printing weights") - # for i in xrange(len(weights)): - # print weights[i] - - # print("printing bots to remove") - # for i in xrange(len(self.bots_to_remove)): - # print self.bots_to_remove[i] - if state.our_goalie in self.bots_to_remove: - pass - else: - self.bots_to_remove.append(state.our_goalie) - # print("printing bots to remove") - # for i in xrange(len(self.bots_to_remove)): - # print self.bots_to_remove[i] - return weights - - - def bot_orient_wt(self,state): - orient_angles= [] - # deviation=[] - ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) - - no_use = Vector2D() - for our_bot in xrange(len(state.homePos)): - if our_bot is self.bot_id or our_bot == state.our_goalie: - continue - angle_needed = no_use.normalizeAngle(state.homePos[our_bot].theta-state.homePos[self.bot_id].theta) - # deviation.append([our_bot, normalizeDistance]) - if fabs(angle_needed) < 90 : - parameter = fabs(angle_needed) * BOT_ORIENT_CONST - else: - parameter = -1*fabs(angle_needed) * BOT_ORIENT_CONST - orient_angles.append([our_bot, parameter]) - - return orient_angles - - def bot_ourgoal_angle(self,state): - angles=[] - botPos = Vector2D(int(state.homePos[self.bot_id].x),int(state.homePos[self.bot_id].y)) - Goal = Vector2D(int(-HALF_FIELD_MAXY),int(0)) - for i in xrange(len(state.homePos)): - if i is self.bot_id or i == state.our_goalie: - continue - anglediff = fabs(botPos.angle(Goal) - botPos.angle(Vector2D(int(state.homePos[i].x),int(state.homePos[i].y)))) - angles.append([i,anglediff]) - return angles - - def velocity_dir_wt(self,state): - #not completed - wt = [[i,1] for i in xrange(5)] - return wt - def optimal_dist_wt(self,state): - - deviation = [] - AttackPos = Vector2D(int(state.homePos[self.bot_id].x), int(state.homePos[self.bot_id].y)) - for our_bot in xrange(len(state.homePos)): - if our_bot is self.bot_id: - continue - distance = fabs(Vector2D(int(state.homePos[our_bot].x),int(state.homePos[our_bot].y)).dist(Vector2D(int(state.homePos[self.bot_id].x),int(state.homePos[self.bot_id].y)))) - normalizeDistance = OPTIMAL_DISTANCE_CONST * exp(-1*(OPTIMAL_DISTANCE - distance)/HALF_FIELD_MAXY) - #if distance is greater than optimal distance then score will be greater than 1.... ERROR - deviation.append([our_bot, normalizeDistance]) - return deviation - - def ourBot_approachability_wt(self, state): - ourBot_app_wt = [] - - for our_bot in xrange(len(state.homePos)): - if our_bot is self.bot_id or our_bot == state.our_goalie: - continue - distance = fabs(Vector2D(int(state.homePos[our_bot].x),int(state.homePos[our_bot].y)).dist(Vector2D(int(state.homePos[self.bot_id].x),int(state.homePos[self.bot_id].y)))) - normalizeDistance = OUR_BOT_APPROACHABILITY_CONST * exp(-1*distance/HALF_FIELD_MAXY) - ourBot_app_wt.append([our_bot, normalizeDistance]) - ##print(ourBot_app_wt) - ##print("bod_id ",self.bot_id) - return ourBot_app_wt - - - - - def calculate_score(self, state,BOTiD): - - weights = [0.35, 0.25, 0.15, 0.35, 0.15,0.6] - dist_weight_partition = [0.25, 0.75] - - score_bisect = self.bisector_bot_wt(state) - score_orient = self.bot_orient_wt(state) - #score_velo_dir = self.velocity_dir_wt(state) - score_opt_dist = self.optimal_dist_wt(state) - score_bot_app = self.ourBot_approachability_wt(state) - score_bot_goalangle = self.bot_ourgoal_angle(state) - - - # #print "score opt distance: "+str(score_opt_dist[1]) - - score = [] - - our_bot=0 - for i in xrange(len(state.homePos)): - if i is BOTiD or i == state.our_goalie: - - continue - - botid = score_bisect[our_bot][0] - bot_score = (score_bisect[our_bot][1]*weights[0]*dist_weight_partition[0] + score_bisect[our_bot][2]*weights[0]*dist_weight_partition[1]) +\ - (score_orient[our_bot][1]*weights[1])+\ - (score_opt_dist[our_bot][1]*weights[3])+\ - (score_bot_app[our_bot][1]*weights[4]) - - - if botid in self.bots_to_remove: - score.append([botid,0]) - else: - score.append([botid, bot_score]) - our_bot+=1 - score.sort(reverse= True ,key = lambda x:x[1] ) - - return score - - - def goal_possible(self,state): - - botPos = Vector2D(int(state.homePos[self.bot_id].x), int(state.homePos[self.bot_id].y)) - - goal_angle_upper = atan((self.GOAL_UPPER.y-botPos.y)*1.0/ (self.GOAL_UPPER.x-botPos.x)) - goal_angle_lower = atan((self.GOAL_LOWER.y-botPos.y)*1.0/ (self.GOAL_LOWER.x-botPos.x)) - goal_angle_mid = 0.5*(goal_angle_lower+goal_angle_upper) - - - k1 = False - k2 = False - for away_bot in xrange(len(state.awayPos)): - - if state.awayPos[away_bot].x > state.homePos[self.bot_id].x: - ##print (state.awayPos[away_bot].x, state.homePos[self.bot_id].x, "chutiya.....") - away_BOT = Vector2D (int(state.awayPos[away_bot].x), int(state.awayPos[away_bot].y)) - try: - bot_angle = atan((away_BOT.y - botPos.y)*1.0/ (away_BOT.x - botPos.x)) - except: - bot_angle = fabs((away_BOT.y - botPos.y)) - - ##print("bhosdiwala angles ",bot_angle, goal_angle_lower, goal_angle_mid, goal_angle_upper, away_bot) - if bot_angle > goal_angle_lower and bot_angle < goal_angle_mid: - k1=True - - if bot_angle > goal_angle_mid and bot_angle < goal_angle_upper: - k2=True - #print(k1,k2) - for home_bot in xrange(len(state.homePos)): - if home_bot == state.our_goalie: - continue - - if state.homePos[home_bot].x > state.homePos[self.bot_id].x: - ##print (state.homePos[home_bot].x, state.homePos[self.bot_id].x, "chutiya.....") - home_BOT = Vector2D (int(state.homePos[home_bot].x), int(state.homePos[home_bot].y)) - try: - bot_angle = atan((home_BOT.y - botPos.y)*1.0/ (home_BOT.x - botPos.x)) - except: - bot_angle = fabs((home_BOT.y - botPos.y)) - - ##print("bhosdiwala angles ",bot_angle, goal_angle_lower, goal_angle_mid, goal_angle_upper, home_bot) - if bot_angle > goal_angle_lower and bot_angle < goal_angle_mid: - k1=True - - if bot_angle > goal_angle_mid and bot_angle < goal_angle_upper: - k2=True - - if k1 and k2: - return [False, -1] - if k1 : - return [True, 2] - if k2 : - return [True, 1] - if not (k1 or k2): - return [True, 0] - - - def execute(self, state, pub, receiver_bot_id = [-1]): - ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) - DistancesFromBot = inf - - if not (state.ballPos.x < -HALF_FIELD_MAXX + DBOX_WIDTH and fabs(state.ballPos.y) < OUR_GOAL_MAXY*0.8) : - # print(ballPos.x,ballPos.y) - if fabs(ballPos.dist(Vector2D(int(state.homePos[self.bot_id].x),int(state.homePos[self.bot_id].y)))>BOT_BALL_THRESH): - sGoToBall.execute(self.sParams, state, self.bot_id, pub) - else: - return - flaggu=0 - possibility = isPossible(self.bot_id,state) - bool_goal = possibility.isGoalPossible(state) - goal_chck = self.goal_possible(state) - - - - - scores_of_bot = self.calculate_score(state,self.bot_id) - AttackBot_ID = self.bot_id - - self.oh_my_bot = scores_of_bot[0][0] - self.oh_my_bot =1 - #print("\n\n\n\n\n\n\n\n\n\nScore: "+str(scores_of_bot)) - # print("kickinf to",self.oh_my_bot) - passprobable = possibility.isPassPossible(state, self.oh_my_bot) - geom = Vector2D() - angle2 = -geom.normalizeAngle(passprobable[1]) - receiveprobable = possibility.isReceivePossible(state, self.oh_my_bot,angle2) - # print (passprobable[0]) - if not flaggu : - if passprobable[0]: - print("Passing",self.bot_id,self.oh_my_bot) - if receiveprobable: - print("Receiving",self.bot_id,self.oh_my_bot) - receiverbot = Vector2D(int(state.homePos[self.oh_my_bot].x),int(state.homePos[self.oh_my_bot].y)) - receiver_bot_id[0] = self.oh_my_bot - - - distance = ballPos.dist(receiverbot) - - point =Vector2D(int(ballPos.x - distance*cos(angle2)),int(ballPos.y - distance*sin(angle2))) - - # print(self.oh_my_bot) - # ob =Vector2D() - # finalSlope = ballPos.angle(Vector2D(int(state.homePos[self.oh_my_bot].x),int(state.homePos[self.oh_my_bot].y))) - # turnAngleLeft = ob.normalizeAngle(finalSlope - state.homePos[self.bot_id].theta) - botPos=Vector2D(int(state.homePos[self.bot_id].x), int(state.homePos[self.bot_id].y)) - # ballPos=Vector2D(int(state.ballPos.x), int(state.ballPos.y)) - dist = ballPos.dist(botPos) - # finalSlope1 = ballPos.angle(botPosition) - # anglediff = state.homePos[self.bot_id].theta - ballPos.angle(Vector2D(botPosition)) - # turnAngleLeft1 = ob.normalizeAngle(finalSlope1 - state.homePos[self.bot_id].theta) - if dist > BOT_BALL_THRESH+10: - - print('sGoToBall',dist,BOT_BALL_THRESH) - sGoToBall.execute(self.sParams, state,self.bot_id, pub) - return - # elif math.fabs(turnAngleLeft1) > SATISFIABLE_THETA/2 : # SATISFIABLE_THETA in config file - # sParam = skills_union.SParam() - # import sTurnToPoint - # sParam.TurnToPointP.x = botPosition.x - # sParam.TurnToPointP.y = botPosition.y - # sParam.TurnToPointP.max_omega = MAX_BOT_OMEGA - # print("before turn") - # sTurnToPoint.execute(self.sParams, state,self.bot_id, pub) - # print("after turn") - # return - # elif fabs(anglediff)>SATISFIABLE_THETA: - # argument = "align_properly" - # print(argument) - # return - else: - self.sParams.KickToPointP.x = 3000 - self.sParams.KickToPointP.y = 0 - # self.sParams.KickToPointP.x = point.x - # self.sParams.KickToPointP.y = point.y - # for i in xrange(10): - # print("cordadfsd $$$$$$$$$$$$$$$$$$$$$$$$ ",point.x," , ",point.y) - self.sParams.KickToPointP.power = 7 - sKickToPoint.execute(self.sParams, state, self.bot_id, pub) - # argument = "kicking" - # print(argument) - return - - - - def isComplete(self, state): - if self.destination.dist(state.homePos[self.bot_id]) < self.threshold: - return True - elif time.time()-self.begin_time > self.time_out: - return True - else: - return False - - def updateParams(self, state): - # No parameter to update here - pass - - + def __init__(self, bot_id, state, params=None): + super(TAttacker, self).__init__( bot_id, state, params) + self.sParams = skills_union.SParam() + self.bot_id = bot_id + self.receive_bot_id = -1 + self.passer_bot_id = -1 + self.UPPER_HALF = Vector2D(-HALF_FIELD_MAXX,OUR_GOAL_MAXY) + self.LOWER_HALF = Vector2D(-HALF_FIELD_MAXX,OUR_GOAL_MINY) + + self.GOAL_UPPER = Vector2D(HALF_FIELD_MAXX,OUR_GOAL_MAXY*3) + self.GOAL_LOWER = Vector2D(HALF_FIELD_MAXX,OUR_GOAL_MINY*3) + + class State(enum.Enum): + #shoot towards goal + shoot = 1 + #cross pass + cross_pass = 2 + #cross receive + cross_receive = 3 + + def getState(self,state): + botPos = Vector2D(int(state.homePos[self.bot_id].x), int(state.homePos[self.bot_id].y)) + ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) + ballVel = Vector2D(int(state.ballVel.x) , int(state.ballVel.y)) + goal_angle_upper = atan2((self.GOAL_UPPER.y-botPos.y)*1.0, (self.GOAL_UPPER.x-botPos.x)) + goal_angle_lower = atan2((self.GOAL_LOWER.y-botPos.y)*1.0, (self.GOAL_LOWER.x-botPos.x)) + goal_angle_mid = 0.5*(goal_angle_lower+goal_angle_upper) + global k1 + global k2 + for away_bot in xrange(len(state.awayPos)): + + if state.awayPos[away_bot].x > state.homePos[self.bot_id].x: + if away_bot == 4: + continue + ###print (state.awayPos[away_bot].x, state.homePos[self.bot_id].x, "chutiya.....") + away_BOT = Vector2D (int(state.awayPos[away_bot].x), int(state.awayPos[away_bot].y)) + try: + bot_angle = atan2((away_BOT.y - botPos.y)*1.0, (away_BOT.x - botPos.x)) + except: + bot_angle = fabs((away_BOT.y - botPos.y)) + + ###print("bhosdiwala angles ",bot_angle, goal_angle_lower, goal_angle_mid, goal_angle_upper, away_bot) + if bot_angle > goal_angle_lower and bot_angle < goal_angle_mid: + k1=False + break + + if bot_angle > goal_angle_mid and bot_angle < goal_angle_upper: + k2=False + break + ##print(k1,k2) + for home_bot in xrange(len(state.homePos)): + if home_bot == state.our_goalie: + continue + + if state.homePos[home_bot].x > state.homePos[self.bot_id].x: + ###print (state.homePos[home_bot].x, state.homePos[self.bot_id].x, "chutiya.....") + home_BOT = Vector2D (int(state.homePos[home_bot].x), int(state.homePos[home_bot].y)) + try: + bot_angle = atan2((home_BOT.y - botPos.y)*1.0, (home_BOT.x - botPos.x)) + except: + bot_angle = fabs((home_BOT.y - botPos.y)) + + ###print("bhosdiwala angles ",bot_angle, goal_angle_lower, goal_angle_mid, goal_angle_upper, home_bot) + if bot_angle > goal_angle_lower and bot_angle < goal_angle_mid: + k1=False + break + if bot_angle > goal_angle_mid and bot_angle < goal_angle_upper: + k2=False + break + + if k1 or k2 or ballPos.x > HALF_FIELD_MAXX - DBOX_HEIGHT: + print "shoot_State" + return TAttacker.State.shoot + + if fabs(atan2(ballVel.y,ballVel.x) - atan2((botPos.y-ballPos.y),(botPos.x-ballPos.x))) < 0.01 : + print 'cross_receive' + return TAttacker.State.cross_receive + print 'cross_pass' + return TAttacker.State.cross_pass + + + def execute(self,state,pub): + ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y)) + botPos = Vector2D(int(state.homePos[self.bot_id].x), int(state.homePos[self.bot_id].y)) + ballVel = Vector2D(int(state.ballVel.x) , int(state.ballVel.y)) + gameState = self.getState(state) + global k1,k2 + + if gameState == TAttacker.State.shoot : + # if fabs(ballPos.dist(botPos)>BOT_BALL_THRESH/400): + # sGoToBall.execute(self.sParams, state, self.bot_id, pub) + # #fuzzy logic to find best angle + # #print "shoot_State" + if k1 and not k2: + self.sParams.KickToPointP.x = HALF_FIELD_MAXX + self.sParams.KickToPointP.y = 0.8*OUR_GOAL_MINY + self.sParams.KickToPointP.power = 7 + sKickToPoint.execute(self.sParams, state, self.bot_id, pub) + #print "shot" + elif k2 and not k1: + self.sParams.KickToPointP.x = HALF_FIELD_MAXX + self.sParams.KickToPointP.y = 0.8*OUR_GOAL_MAXY + self.sParams.KickToPointP.power = 7 + sKickToPoint.execute(self.sParams, state, self.bot_id, pub) + #print "shot" + else: + if state.awayPos[4].y < 0 : + self.sParams.KickToPointP.x = HALF_FIELD_MAXX + self.sParams.KickToPointP.y = 0.8*OUR_GOAL_MAXY + self.sParams.KickToPointP.power = 7 + sKickToPoint.execute(self.sParams, state, self.bot_id, pub) + #print "shot" + if state.awayPos[4].y > 0: + self.sParams.KickToPointP.x = HALF_FIELD_MAXX + self.sParams.KickToPointP.y = 0.8*OUR_GOAL_MINY + self.sParams.KickToPointP.power = 7 + sKickToPoint.execute(self.sParams, state, self.bot_id, pub) + #print "shot" + + + + if gameState == TAttacker.State.cross_pass : + # if fabs(ballPos.dist(botPos)>BOT_BALL_THRESH/4): + # sGoToBall.execute(self.sParams, state, self.bot_id, pub) + print "pass_state" + p = get_all(state,self.bot_id) + self.receive_bot_id = p[0] + print "receiver =" , p[0] + self.sParams.KickToPointP.x = state.homePos[self.receive_bot_id].x + self.sParams.KickToPointP.y = state.homePos[self.receive_bot_id].y + self.sParams.KickToPointP.power = 7 + sKickToPoint.execute(self.sParams,state,self.bot_id,pub) + + if gameState == TAttacker.State.cross_receive : + mindist = 9999999 + print "recieve_state" + for our_bot in xrange(len(state.homePos)): + if state.homePos[our_bot].x > state.homePos[bots].x: + curr_home_bot = Vector2D(int(state.homePos[our_bot].x),int(state.homePos[our_bot].y)) + # #print'Bot id: {}, Pos {},{}'.format(our_bot,curr_opp_bot.x, curr_opp_bot.y) + dist_bot = ballPos.dist(curr_home_bot) + # #print our_bot , " " ,mindist , " " , dist_bot + # #print int(state.homePos[our_bot].x)," " ,int(state.homePos[our_bot].y) + if dist_bot < mindist: + + mindist = dist_bot + closest_home_bot = our_bot + passer_bot_id = closest_home_bot + self.SParams.sTurnToPoint.x = state.homePos[self.passer_bot_id].x + self.SParams.sTurnToPoint.y = state.homePos[self.passer_bot_id].y + self.SParams.sTurnToPoint.max_omega = MAX_BOT_OMEGA + + def isComplete(self, state): + return False + + def updateParams(self, state): + pass