Skip to content

Commit

Permalink
Modified pass_and_receive.py and _turnAround_.py
Browse files Browse the repository at this point in the history
  • Loading branch information
@ultrainstinct30
ultrainstinct30 committed May 31, 2019
1 parent 74f50a4 commit 9a58288
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 74 deletions.
4 changes: 2 additions & 2 deletions role/_turnAround_.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ def reset():
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)

def execute(startTime):
def execute(startTime,thresh=2*ROTATION_FACTOR):
global start_time,FIRST_CALL,FLAG_turn,kub,prev_state

# print DIST_THRESH
Expand Down Expand Up @@ -92,7 +92,7 @@ def execute(startTime):
#print("rotate : ", rotate)
#print(" not rotate : ", kub.state.homePos[kub.kubs_id].theta)
print("diff : ",abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate)))
if (abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate))<2*ROTATION_FACTOR):
if (abs(normalize_angle(kub.state.homePos[kub.kubs_id].theta-rotate))<thresh):
kub.turn(0)
print("Angle completed")
FLAG_turn = True
Expand Down
207 changes: 135 additions & 72 deletions role/pass_and_receive.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
from enum import Enum
import logging
from role import pass_receive
from role import GoToBall, GoToPoint, KickToPoint, _GoToPoint_
from role import GoToBall, GoToPoint, KickToPoint, _GoToPoint_,_turnAround_
from utils.geometry import *
from utils.functions import *
from krssg_ssl_msgs.srv import bsServer
Expand Down Expand Up @@ -44,8 +44,10 @@ def __init__(self):

self.add_transition(behavior.Behavior.State.start,PassReceive.State.setup,
lambda:True,"immediately")
self.add_transition(PassReceive.State.setup, PassReceive.State.kicking,
lambda:self.passer_present() and self.kub_0_is_passer() and self.ready_to_kick(),"Direct kick")
self.add_transition(PassReceive.State.setup,PassReceive.State.passer,
lambda:self.passer_present() and self.kub_0_is_passer(),"passer found")
lambda:self.passer_present() and self.kub_0_is_passer() and not self.ready_to_kick(),"passer found")
self.add_transition(PassReceive.State.passer,PassReceive.State.waiting,
lambda:self.receiver_present(),"passer ready")
self.add_transition(PassReceive.State.waiting,PassReceive.State.kicking,
Expand All @@ -59,7 +61,7 @@ def __init__(self):
self.add_transition(PassReceive.State.going_to_receiving_point,PassReceive.State.receiving,
lambda:self.reciever_at_receiving_point(),"ball incoming")
self.add_transition(PassReceive.State.receiving,PassReceive.State.received,
lambda:self.ball_with_receiver(),"ball received")
lambda:self.ball_with_receiver() or not self.ball_moving(),"ball received")
self.add_transition(PassReceive.State.received,behavior.Behavior.State.completed,
lambda:True,"one pass completed")

Expand All @@ -80,130 +82,180 @@ def kub_0_is_receiver(self):
return self.receiver.kubs_id == self.kubs[0].kubs_id

def receiver_is_ready(self):
print("This conition is ",math.fabs(math.fabs(normalize_angle(self.receiver.get_theta()-self.pass_angle))-math.pi) < deg_2_radian(10.0))
return math.fabs(math.fabs(normalize_angle(self.receiver.get_theta()-self.pass_angle))-math.pi) < deg_2_radian(10.0)

def ready_to_kick(self):
return vicinity_points(getPointBehindTheBall(Vector2D(self.passer.state.ballPos),self.pass_angle), Vector2D(self.passer.get_pos()), thresh = 2*BOT_RADIUS)

def ball_moving_to_receive_point(self):
ball_vel = Vector2D()
ball_vel.x = self.receiver.state.ballVel.x
ball_vel.y = self.receiver.state.ballVel.y
if magnitute(ball_vel) <= 50:
return False
return math.fabs(ball_vel.angle()-self.pass_angle) < math.pi/2.0
if self.ball_moving():
ball_vel = Vector2D()
ball_vel.x = self.receiver.state.ballVel.x
ball_vel.y = self.receiver.state.ballVel.y
return math.fabs(ball_vel.angle()-self.pass_angle) < math.pi/2.0
return False

def ball_with_receiver(self):
return self.receiver.has_ball()

def reciever_at_receiving_point(self):
if vicinity_points(self.receiver.get_pos(),self.receiving_point,BOT_BALL_THRESH):
if vicinity_points(self.receiver.get_pos(),self.receiving_point,0.5*BOT_RADIUS):
return True
return False

def ball_moving(self):
if magnitute(self.receiver.state.ballVel) <= 50:
if magnitute(self.receiver.state.ballVel) <= 50 or vicinity_points(self.receiver.state.homePos[self.passer.kubs_id],self.receiver.state.ballPos,200):
return False
return True

def add_kub(self,kub):
self.kubs += [kub]

def kicking_power(self):
return math.sqrt(dist(self.receiving_point,self.passer.state.ballPos)/6750.0)*5.0
return math.sqrt(dist(self.receiving_point,self.passer.state.ballPos)/6400.0)*5.0

def calculate_receiving_point(self):
ball_pos = Vector2D(self.receiver.state.ballPos.x,self.receiver.state.ballPos.y)
ball_vel = Vector2D(self.receiver.state.ballVel.x,self.receiver.state.ballVel.y)
self.pass_line = Line(point1=ball_pos,angle=ball_vel.angle())
receiver_pos = Vector2D(self.receiver.get_pos().x,self.receiver.get_pos().y)
line2 = Line(point1=receiver_pos,angle = normalize_angle(self.pass_line.angle+math.pi/2.0))
line2 = Line(point1=receiver_pos,angle = normalize_angle(ball_vel.angle()+math.pi/2.0))
# print(magnitute(ball_vel))
return self.pass_line.intersection_with_line(line2)

def on_enter_setup(self):
print("Entering setup")
#print("Entering setup")
pass

def execute_setup(self):
print("Executing setup")
if len(self.kubs) != 2:
self.behavior_failed = True
return
if dist(self.kubs[0].state.ballPos,self.kubs[0].get_pos())< dist(self.kubs[0].state.ballPos,self.kubs[1].get_pos()):
self.passer = self.kubs[0]
self.receiver = self.kubs[1]
self.receiving_point = Vector2D(self.kubs[1].get_pos().x,self.kubs[1].get_pos().y)
self.receiving_point = Vector2D(self.kubs[1].get_pos())
else:
self.passer = self.kubs[1]
self.receiver = self.kubs[0]
self.receiving_point = Vector2D(self.kubs[0].get_pos().x,self.kubs[0].get_pos().y)
self.pass_angle = angle_diff(self.receiver.state.ballPos,self.receiver.get_pos())
self.receiving_point = Vector2D(self.kubs[0].get_pos())
self.pass_angle = angle_diff(self.receiver.state.ballPos,self.receiving_point)
# self.pass_angle = normalize_angle(self.pass_angle)
state = None
state = getState(state)
state = state.stateB
self.passer.update_state(state)
self.receiver.update_state(state)
print("Pass angle is",self.pass_angle)
print("receiver id is",self.receiver.kubs_id)

def on_exit_setup(self):
#print("exiting setup")
pass

def on_enter_passer(self):
print("Entering passer")
# print("############################# GOING TO POINT ###################################")
target_point = getPointBehindTheBall(Vector2D(self.passer.state.ballPos),normalize_angle(self.pass_angle+math.pi))
_GoToPoint_.init(self.passer,target_point,self.passer.get_theta())
pass

def execute_passer(self):
print("Executing passer")
target_point = getPointBehindTheBall(self.passer.state.ballPos,normalize_angle(self.pass_angle+math.pi))
g_fsm = GoToPoint.GoToPoint()
g_fsm.add_kub(self.passer)
g_fsm.add_point(target_point,self.pass_angle)
g_fsm.avoid_ball = True
g_fsm.spin()
self.behavior_failed = g_fsm.behavior_failed
if vicinity_points(Vector2D(self.passer.get_pos()),Vector2D(self.passer.state.ballPos),4.5*BOT_RADIUS):
pass
# target_point = getPointBehindTheBall(self.passer.state.ballPos,normalize_angle(self.pass_angle+math.pi))
# g_fsm = GoToPoint.GoToPoint()
# g_fsm.add_kub(self.passer)
# g_fsm.add_point(target_point,self.pass_angle)
# g_fsm.avoid_ball = True
# g_fsm.spin()
# self.behavior_failed = g_fsm.behavior_failed
print(self.passer.kubs_id)
if self.ready_to_kick():
pass
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,BOT_RADIUS,avoid_ball=True)
for gf in generatingfunction:
self.passer,target_point = gf
if not vicinity_points(target_point,getPointBehindTheBall(Vector2D(self.passer.state.ballPos),normalize_angle(self.pass_angle+math.pi)),BOT_RADIUS):
self.behavior_failed = True
break

def on_exit_passer(self):
print("exiting passer")
pass

def on_enter_waiting(self):
print("Enter waiting")
self.pass_angle = angle_diff(self.receiver.state.ballPos,self.receiver.get_pos())
_turnAround_.init(self.passer,self.pass_angle)
pass

def execute_waiting(self):
# print("/////////////////////// waiting //////////////////")
if not vicinity_theta(self.passer.get_theta(),self.pass_angle,thresh=0.05):
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
generatingfunction = _turnAround_.execute(start_time,0.1)
for gf in generatingfunction:
self.passer,final_theta = gf
# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
if not vicinity_theta(self.pass_angle,final_theta,thresh=0.1):
self.behavior_failed = True
break
# print("[[[[[[[[[[[[[[[[[[[[[[[[[[[oriented]]]]]]]]]]]]]]]]]]]]]]]]]]]]]")
print("in waiting, oriented")
while True:
state = None
state = getState(state)
state = state.stateB
self.passer.update_state(state)
self.receiver.update_state(state)
if math.fabs(math.fabs(normalize_angle(self.receiver.get_theta()-self.pass_angle))-math.pi) < deg_2_radian(10.0):
if self.receiver_is_ready():
break

def on_exit_waiting(self):
print("exiting waiting")
pass

def on_enter_kicking(self):
print("Enter kicking")
self.kick_power = self.kicking_power()
_GoToPoint_.init(self.passer,Vector2D(self.passer.state.ballPos),self.passer.get_theta())
pass

def execute_kicking(self):
g_fsm = KickToPoint.KickToPoint(self.receiving_point)
g_fsm.power = self.kick_power
state = None
state = getState(state)
state = state.stateB
self.passer.update_state(state)
self.receiver.update_state(state)
g_fsm.add_kub(self.passer)
g_fsm.add_theta(self.pass_angle)
g_fsm.spin()
self.behavior_failed = g_fsm.behavior_failed
if g_fsm.state == behavior.Behavior.State.completed:
self.ball_kicked = True
# g_fsm = KickToPoint.KickToPoint(self.receiving_point)
# g_fsm.power = self.kick_power
# state = None
# state = getState(state)
# state = state.stateB
# self.passer.update_state(state)
# self.receiver.update_state(state)
# g_fsm.add_kub(self.passer)
# g_fsm.add_theta(self.pass_angle)
# print("***************************** KICKING **************************")
# g_fsm.spin()
# # self.behavior_failed = g_fsm.behavior_failed
# if g_fsm.state == behavior.Behavior.State.completed:
# self.ball_kicked = True
# else:
# self.behavior_failed = True
print(self.kicking_power())
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,BOT_RADIUS)
for gf in generatingfunction:
self.passer,ballPos = gf
self.passer.kick(self.kicking_power())
if not vicinity_points(ballPos,Vector2D(self.passer.state.ballPos),thresh=BOT_RADIUS):
self.behavior_failed = True
break

def on_exit_kicking(self):
print("exiting kicking")
self.passer.reset()
self.passer.execute()
# # if not self.behavior_failed:
# self.ball_kicked = True
# # else:
Expand All @@ -212,48 +264,59 @@ def on_exit_kicking(self):

def on_enter_receiver(self):
print("Entering receiver")
_turnAround_.init(self.receiver,normalize_angle(self.pass_angle+math.pi))
pass

def execute_receiver(self):
print("Executing receiver")
target_point = Vector2D(self.receiver.get_pos().x,self.receiver.get_pos().y)
g_fsm = GoToPoint.GoToPoint()
g_fsm.add_kub(self.receiver)
g_fsm.add_point(target_point,normalize_angle(self.pass_angle+math.pi))
g_fsm.spin()
self.behavior_failed = g_fsm.behavior_failed
# target_point = Vector2D(self.receiver.get_pos().x,self.receiver.get_pos().y)
# g_fsm = GoToPoint.GoToPoint()
# g_fsm.add_kub(self.receiver)
# g_fsm.add_point(target_point,normalize_angle(self.pass_angle+math.pi))
# g_fsm.spin()
# self.behavior_failed = g_fsm.behavior_failed
print(self.receiver.kubs_id)
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
generatingfunction = _turnAround_.execute(start_time,0.1)
for gf in generatingfunction:
self.receiver,final_theta = gf
# self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
if not vicinity_theta(angle_diff(Vector2D(self.receiver.get_pos()),Vector2D(self.receiver.state.ballPos)),final_theta,thresh=0.1):
self.behavior_failed = True
break

def on_exit_receiver(self):
print("exiting receiver")
pass

def on_enter_going_to_receiving_point(self):
print("going to receiving point")
self.receiving_point = self.calculate_receiving_point()
# _GoToPoint_.init(self.receiver,self.receiving_point,self.receiver.get_theta())
if not vicinity_points(self.receiving_point,self.calculate_receiving_point(),thresh=0.5*BOT_RADIUS):
self.receiving_point = self.calculate_receiving_point()
_GoToPoint_.init(self.receiver,self.receiving_point,self.receiver.get_theta())
pass

def execute_going_to_receiving_point(self):
print("Executing receiving", self.passer.kubs_id, self.receiver.kubs_id)
# 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,BOT_BALL_THRESH*2.0)
# for gf in generatingfunction:
# self.receiver,receiving_point = gf
# if not vicinity_points(self.receiving_point,receiving_point,BOT_BALL_THRESH):
# self.behavior_failed = True
# break
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,BOT_RADIUS)
for gf in generatingfunction:
self.receiver,receiving_point = gf
if not vicinity_points(self.calculate_receiving_point(),receiving_point,BOT_RADIUS):
self.behavior_failed = True
break

g_fsm = GoToPoint.GoToPoint()
state = None
state = getState(state)
state = state.stateB
self.passer.update_state(state)
self.receiver.update_state(state)
g_fsm.add_kub(self.receiver)
g_fsm.add_point(self.receiving_point,self.receiver.get_theta())
g_fsm.spin()
self.behavior_failed = g_fsm.behavior_failed
# g_fsm = GoToPoint.GoToPoint()
# state = None
# state = getState(state)
# state = state.stateB
# self.passer.update_state(state)
# self.receiver.update_state(state)
# g_fsm.add_kub(self.receiver)
# g_fsm.add_point(self.receiving_point,self.receiver.get_theta())
# print(self.receiving_point.x, self.receiving_point.y)
# g_fsm.spin()
# self.behavior_failed = g_fsm.behavior_failed

# while True:
# state = None
Expand Down Expand Up @@ -285,7 +348,7 @@ def execute_receiving(self):
break

def on_exit_receiving(self):
print("ball received")
print("exiting receiving")
pass

def on_enter_received(self):
Expand All @@ -296,7 +359,7 @@ def on_enter_received(self):
def execute_received(self):
self.receiver.execute()

def on_exit_receiving(self):
def on_exit_received(self):
print("exiting received")
pass

Expand Down

0 comments on commit 9a58288

Please sign in to comment.