diff --git a/defending.py b/defending.py new file mode 100644 index 00000000..fbbbd6ec --- /dev/null +++ b/defending.py @@ -0,0 +1,113 @@ +import rospy,sys +from utils.geometry import Vector2D +from utils.functions import * +from krssg_ssl_msgs.msg import point_2d +from krssg_ssl_msgs.msg import BeliefState +from krssg_ssl_msgs.msg import gr_Commands +from krssg_ssl_msgs.msg import gr_Robot_Command +from krssg_ssl_msgs.msg import BeliefState +from role import GoToBall, GoToPoint, KickToPoint, top , bottom +from multiprocessing import Process +from kubs import kubs +from krssg_ssl_msgs.srv import bsServer +from math import atan2,pi +from utils.functions import * +from tactics import Goalie +import multiprocessing +import threading +#pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) + + +import memcache +shared = memcache.Client(['127.0.0.1:11211'],debug=True) + + +def function1(id_,state,pub): + kub = kubs.kubs(id_,state,pub) + kub.update_state(state) + print(kub.kubs_id) + g_fsm = top.defender() + # g_fsm = GoToPoint.GoToPoint() + g_fsm.add_kub(kub) + # g_fsm.add_point(point=kub.state.ballPos,orient=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x-3000))) + g_fsm.add_theta(theta=math.pi) + print('something before spin') + g_fsm.spin() + +def function2(id_,state,pub): + kub = kubs.kubs(id_,state,pub) + kub.update_state(state) + print(kub.kubs_id) + g_fsm = bottom.defender() + # g_fsm = GoToPoint.GoToPoint() + g_fsm.add_kub(kub) + # g_fsm.add_point(point=kub.state.ballPos,orient=normalize_angle(pi+atan2(state.ballPos.y,state.ballPos.x-3000))) + g_fsm.add_theta(theta=math.pi) + print('something before spin') + g_fsm.spin() + +def main1(process_id): + pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) + rospy.init_node('node' + str(process_id),anonymous=False) + + while True: + state = None + # state=shared.get('state') + rospy.wait_for_service('bsServer',) + getState = rospy.ServiceProxy('bsServer',bsServer) + try: + state = getState(state) + except rospy.ServiceException, e: + print("chutiya") + if state: + #print('lasknfcjscnajnstate',state.stateB.homePos) + #p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, )) + print("process 1") + function1(1,state.stateB,pub) + #p2.start() + #p1.join() + #p2.join() + # print('chal ja') + # break + #rospy.spin() + +def main2(process_id): + pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) + rospy.init_node('node' + str(process_id),anonymous=False) + + while True: + state = None + # state=shared.get('state') + rospy.wait_for_service('bsServer',) + getState = rospy.ServiceProxy('bsServer',bsServer) + try: + state = getState(state) + except rospy.ServiceException, e: + print("chutiya") + if state: + #print('lasknfcjscnajnstate',state.stateB.homePos) + #p2 = multiprocessing.Process(target=function2, args=(2,state.stateB, )) + print("process 2") + function2(2,state.stateB,pub) + #p2.start() + #p1.join() + #p2.join() + # print('chal ja') + # break + #rospy.spin() + + +#print str(kub.kubs_id) + str('***********') +p1 = multiprocessing.Process(target=main1, args=(0,)) +p2 = multiprocessing.Process(target=main2, args=(1,)) +p1.start() +p2.start() +p1.join() +p2.join() +#main1() + +# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000) + + + + diff --git a/role/bottom.py b/role/bottom.py new file mode 100644 index 00000000..3a1eb3ab --- /dev/null +++ b/role/bottom.py @@ -0,0 +1,232 @@ +from enum import Enum +import behavior +import _GoToPoint_ +import rospy +from utils.functions import * +from utils.geometry import * +from utils.config import * +from math import * +from velocity.run import * +import time +from krssg_ssl_msgs.srv import * + +rospy.wait_for_service('bsServer',) +getState = rospy.ServiceProxy('bsServer',bsServer) +center_arc=Vector2D(-HALF_FIELD_MAXX,0) +point=Vector2D((-HALF_FIELD_MAXX + DBOX_HEIGHT)+BOT_RADIUS , 0.5*DBOX_WIDTH) +class defender(behavior.Behavior): + + class State(Enum): + setup = 1 + block = 2 + intercept = 3 + clear = 4 + + def __init__(self,course_approch_thresh = DISTANCE_THRESH/3,continuous=False): + + super(defender,self).__init__() + self.course_approch_thresh = course_approch_thresh + self.name = "Square" + self.kubs = [] + self.blocker = None + self.intercepter = None + self.intercepting_point = Vector2D() + self.pass_angle = None + self.behavior_failed = False + self.ball_kicked = False + self.kicking_point=Vector2D(0,0) + self.line = Line(point1=Vector2D((-HALF_FIELD_MAXX + DBOX_HEIGHT)+5*BOT_RADIUS,0),angle=math.pi/2.0) + self.line1= Line(point1=Vector2D(0,-0.5*DBOX_WIDTH-5*BOT_RADIUS),angle=0) + self.line2= Line(point1=Vector2D(0,0.5*DBOX_WIDTH+5*BOT_RADIUS),angle=0) + self.goal_line= Line(point1=Vector2D(-HALF_FIELD_MAXX,0),angle=math.pi/2.0) + for state in defender.State: + self.add_state(state,behavior.Behavior.State.running) + + self.add_transition(behavior.Behavior.State.start,defender.State.setup, + lambda:True,"immediately") + self.add_transition(defender.State.setup, defender.State.clear, + lambda:self.ready_to_kick(),"Direct kick") + self.add_transition(defender.State.setup,defender.State.intercept, + lambda:self.should_intercept(),"passer ready") + self.add_transition(defender.State.setup,defender.State.block, + lambda:not self.should_intercept(),"threat") + self.add_transition(defender.State.clear,defender.State.intercept, + lambda:self.should_intercept() and not self.ready_to_kick(),"kicking") + self.add_transition(defender.State.clear,defender.State.block, + lambda:not self.ready_to_kick() or self.bot_infrontof_ball(),"ball kicked") + self.add_transition(defender.State.block,defender.State.clear, + lambda:self.ready_to_kick(),"receiver found") + self.add_transition(defender.State.block,defender.State.intercept, + lambda:self.should_intercept(),"moving to receiving point") + self.add_transition(defender.State.intercept,defender.State.clear, + lambda:self.ready_to_kick(),"ball incoming") + self.add_transition(defender.State.intercept,defender.State.block, + lambda:not self.should_intercept(),"ball received") + + for state in defender.State: + self.add_transition(state,defender.State.setup, + lambda:self.behavior_failed,"failed") + + + def add_kub(self,kub): + self.kub = kub + + def add_theta(self,theta): + self.theta = theta + + def ready_to_kick(self): + return vicinity_points(getPointBehindTheBall(Vector2D(self.kub.state.ballPos),0), Vector2D(self.kub.get_pos()), thresh = 4*BOT_RADIUS) + + def bot_infrontof_ball(self): + if(self.kub.get_pos().x>self.kub.state.ballPos.x): + return True + return False + + def ball_moving(self): + if magnitute(self.kub.state.ballVel) <= 50: + return False + return True + + def on_enter_setup(self): + self.behavior_failed=False + pass + + def execute_setup(self): + pass + + def on_exit_setup(self): + pass + + def on_enter_block(self): + pass + + def execute_block(self): + ball=Vector2D(self.kub.state.ballPos) + goal_line=Line(point1=Vector2D(self.kub.state.ballPos),point2=Vector2D(-HALF_FIELD_MAXX,0)) + target_point=self.line.intersection_with_line(goal_line) + point=Vector2D() + target_point.y-=BOT_RADIUS + if target_point.y > 0.5*DBOX_WIDTH+5*BOT_RADIUS: + target_point=self.line2.intersection_with_line(goal_line) + if target_point.y < -0.5*DBOX_WIDTH-5*BOT_RADIUS: + target_point=self.line1.intersection_with_line(goal_line) + if ball_moving_towards_our_goal(self.kub.state): + ball_line=Line(point1=Vector2D(self.kub.state.ballPos),angle=math.atan2(self.kub.state.ballVel.y,self.kub.state.ballVel.x)) + point=ball_line.intersection_with_line(self.goal_line) + if point.y*point.y<0.25*DBOX_WIDTH*DBOX_WIDTH : + target_point=self.line.intersection_with_line(ball_line) + if target_point.y > 0.5*DBOX_WIDTH+5*BOT_RADIUS: + target_point=self.line2.intersection_with_line(ball_line) + if target_point.y < -0.5*DBOX_WIDTH-5*BOT_RADIUS: + target_point=self.line1.intersection_with_line(ball_line) + _GoToPoint_.init(self.kub, target_point, 0) + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + generatingfunction = _GoToPoint_.execute(start_time,self.course_approch_thresh,True) + #self.behavior_failed = False + prev_point=Vector2D(self.kub.state.ballPos) + next_point = prev_point + movement=self.kub.get_pos() + for gf in generatingfunction: + # print (target_point.x) + next_point = Vector2D(self.kub.state.ballPos) + self.kub,point = gf + if dist(next_point,prev_point) > 3*BOT_RADIUS: + break + if not vicinity_points(point,target_point,thresh=BOT_RADIUS*2.0): + break + if self.should_intercept(): + break + if self.ready_to_kick(): + break + prev_point=next_point + + def on_exit_block(self): + pass + + def intercept_complete(self): + ball_vel = Vector2D(self.kub.state.ballVel.y,self.kub.state.ballVel.x) + ball_vel_angle = ball_vel.tan_inverse() + bot_ball_dir = Vector2D(self.kub.state.ballPos.y-self.kub.state.homePos[self.kub.kubs_id].y , self.kub.state.ballPos.x-self.kub.state.homePos[self.kub.kubs_id].x) + if ( abs(ball_vel_angle - bot_ball_dir.tan_inverse() )< 0.0523599): + return 1 + return 0 + + def should_intercept(self): + ball=Vector2D(self.kub.state.ballPos) + if not self.ball_moving: + ball_line=Line(point1=ball,angle=math.atan2(self.kub.state.ballVel.y,self.kub.state.ballVel.x)) + ball_path_point=ball + if not self.ball_moving(): + opp_time=1000000000000000 + for opp_id, awayPos in enumerate(self.kub.state.awayPos): + if dist(awayPos,getPointBehindTheBall(ball,math.pi))/MAX_BOT_SPEED dist(self.kub.state.homePos[1],getPointBehindTheBall(ball,0))/MAX_BOT_SPEED: + return False + if our_time < 0.8*opp_time: + self.intercepting_point=getPointBehindTheBall(ball,0) + return True + # for x in range(1, 31): + # ball_path_point.x=ball_path_point.x+DBOX_HEIGHT*0.2*x*math.cos(ball_line.angle) + # ball_path_point.y=ball_path_point.y+DBOX_HEIGHT*0.2*x*math.cos(ball_line.angle) + # ball_time=abs(dist(ball_path_point,ball)/magnitute(self.intercepter.state.ballVel)) + # opp_time=1000000000000000 + # for opp_id, awayPos in enumerate(self.kubs[0].state.awayPos) + # if dist(awayPos,getPointBehindTheBall(ball_path_point,math.pi))/MAX_BOT_SPEEDself.kub.state.ballPos.x): + return True + return False + + + def on_enter_setup(self): + self.behavior_failed=False + pass + + def execute_setup(self): + pass + + def on_exit_setup(self): + pass + + def on_enter_block(self): + pass + + def execute_block(self): + ball=Vector2D(self.kub.state.ballPos) + goal_line=Line(point1=Vector2D(self.kub.state.ballPos),point2=Vector2D(-HALF_FIELD_MAXX,0)) + target_point=self.line.intersection_with_line(goal_line) + target_point.y+=BOT_RADIUS + if target_point.y > 0.5*DBOX_WIDTH+5*BOT_RADIUS: + target_point=self.line2.intersection_with_line(goal_line) + if target_point.y < -0.5*DBOX_WIDTH-5*BOT_RADIUS: + target_point=self.line1.intersection_with_line(goal_line) + if ball_moving_towards_our_goal(self.kub.state): + ball_line=Line(point1=Vector2D(self.kub.state.ballPos),angle=math.atan2(self.kub.state.ballVel.y,self.kub.state.ballVel.x)) + point=ball_line.intersection_with_line(self.goal_line) + if point.y*point.y<0.25*DBOX_WIDTH*DBOX_WIDTH : + target_point=self.line.intersection_with_line(ball_line) + if target_point.y > 0.5*DBOX_WIDTH+5*BOT_RADIUS: + target_point=self.line2.intersection_with_line(ball_line) + if target_point.y < -0.5*DBOX_WIDTH-5*BOT_RADIUS: + target_point=self.line1.intersection_with_line(ball_line) + + _GoToPoint_.init(self.kub, target_point, 0) + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + generatingfunction = _GoToPoint_.execute(start_time,self.course_approch_thresh,True) + #self.behavior_failed = False + prev_point=Vector2D(self.kub.state.ballPos) + next_point = prev_point + movement=self.kub.get_pos() + point=Vector2D() + for gf in generatingfunction: + # print (target_point.x) + next_point = Vector2D(self.kub.state.ballPos) + self.kub,point = gf + if dist(next_point,prev_point) > 3*BOT_RADIUS: + break + if not vicinity_points(point,target_point,thresh=BOT_RADIUS*2.0): + break + if self.should_intercept(): + break + if self.ready_to_kick(): + break + prev_point=next_point + + def on_exit_block(self): + pass + + def intercept_complete(self): + ball_vel = Vector2D(self.kub.state.ballVel.y,self.kub.state.ballVel.x) + ball_vel_angle = ball_vel.tan_inverse() + bot_ball_dir = Vector2D(self.kub.state.ballPos.y-self.kub.state.homePos[self.kub.kubs_id].y , self.kub.state.ballPos.x-self.kub.state.homePos[self.kub.kubs_id].x) + if ( abs(ball_vel_angle - bot_ball_dir.tan_inverse() )< 0.0523599): + return 1 + return 0 + + def should_intercept(self): + ball=Vector2D(self.kub.state.ballPos) + if not self.ball_moving: + ball_line=Line(point1=ball,angle=math.atan2(self.kub.state.ballVel.y,self.kub.state.ballVel.x)) + ball_path_point=ball + if not self.ball_moving(): + opp_time=1000000000000000 + for opp_id, awayPos in enumerate(self.kub.state.awayPos): + if dist(awayPos,getPointBehindTheBall(ball,math.pi))/MAX_BOT_SPEED dist(self.kub.state.homePos[2],getPointBehindTheBall(ball,0))/MAX_BOT_SPEED: + return False + if our_time < 0.8*opp_time: + self.intercepting_point=getPointBehindTheBall(ball,0) + return True + # for x in range(1, 31): + # ball_path_point.x=ball_path_point.x+DBOX_HEIGHT*0.2*x*math.cos(ball_line.angle) + # ball_path_point.y=ball_path_point.y+DBOX_HEIGHT*0.2*x*math.cos(ball_line.angle) + # ball_time=abs(dist(ball_path_point,ball)/magnitute(self.intercepter.state.ballVel)) + # opp_time=1000000000000000 + # for opp_id, awayPos in enumerate(self.kubs[0].state.awayPos) + # if dist(awayPos,getPointBehindTheBall(ball_path_point,math.pi))/MAX_BOT_SPEED SLOPE def intersection_with_line(self, line2): - if not isinstance(line, line2): - raise ValueError("Expected Line instance, got %s" %type(line2).__name__) + # if not isinstance(line, line2): + # raise ValueError("Expected Line instance, got %s" %type(line2).__name__) c1 = self.point.y - self.slope * self.point.x c2 = line2.point.y - line2.slope * line2.point.x @@ -212,8 +212,8 @@ def direction(vector): def getPointBehindTheBall(point, theta): - x = point.x - (3.5 * BOT_RADIUS) * (math.cos(theta)) - y = point.y - (3.5 * BOT_RADIUS) * (math.sin(theta)) + x = point.x - (1*BOT_RADIUS) * (math.cos(theta)) + y = point.y - (1*BOT_RADIUS) * (math.sin(theta)) return Vector2D(int(x), int(y)) def getPointToGo(point, theta): @@ -245,10 +245,10 @@ def stan_inverse(self,y,x): return atan2(y,x)+3.14159265 -def getPointBehindTheBall(point ,theta): - x = point.x -(3.5 * BOT_RADIUS) *(math.cos(theta)) - y = point.y -(3.5 * BOT_RADIUS) *(math.sin(theta)) - return Vector2D(int(x), int(y)) +def getPointBehindTheBall(point, theta): + x = point.x - (1*BOT_RADIUS) * (math.cos(theta)) + y = point.y - (1*BOT_RADIUS) * (math.sin(theta)) + return Vector2D(int(x), int(y)) def vicinity_points(point1, point2, thresh=10): return dist(point1, point2) < thresh diff --git a/utils/state_functions.py b/utils/state_functions.py index dc61734c..8efbd1d6 100644 --- a/utils/state_functions.py +++ b/utils/state_functions.py @@ -31,9 +31,9 @@ def kub_has_ball(state, kub_id, is_opponent=False): ## def ball_moving_towards_our_goal(state): ballPos = Vector2D(state.ballPos.x, state.ballPos.y) - ballvel = Vector2D(state.ballvel.x, state.ballvel.y) + ballvel = Vector2D(state.ballVel.x, state.ballVel.y) if ballvel.absSq(ballvel) > 0.1: - ball_movement = Line(ballPos, ballPos + ballvel) + ball_movement = Line(point1=Vector2D(ballPos),point2= Vector2D(ballPos + ballvel)) ptA = Vector2D(-HALF_FIELD_MAXX, DBOX_HEIGHT) ptB = Vector2D(-HALF_FIELD_MAXX, -DBOX_HEIGHT) defend_line = Line(point1=ptA,point2=ptB)