From 818fc202ed0f9fe886905540264a01755155b053 Mon Sep 17 00:00:00 2001 From: SambhawDrag Date: Sun, 12 Jul 2020 20:41:42 +0530 Subject: [PATCH 1/8] Changes in Kick-To-Point Role, Changes in Driver File test_KickToP.py and added new Driver File for KickToPoint KickP.py --- KickP.py | 64 ++++++++++++++ role/KickToPoint.py | 209 ++++++++++++-------------------------------- test_KickToP.py | 194 +++++++++++++++++++++++----------------- 3 files changed, 238 insertions(+), 229 deletions(-) create mode 100644 KickP.py diff --git a/KickP.py b/KickP.py new file mode 100644 index 00000000..25c94510 --- /dev/null +++ b/KickP.py @@ -0,0 +1,64 @@ +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, KickToPointP +from multiprocessing import Process +from kubs import kubs +from krssg_ssl_msgs.srv import bsServer +from math import atan2,pi +from utils.functions import * +pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) + +BOT_ID=int(sys.argv[1]) +x=float(sys.argv[2]) +y=float(sys.argv[3]) + +xx=int(x) +yy=int(y) +target=Vector2D(x,y) + +def function(id_,state): + kub = kubs.kubs(id_,state,pub) + kub.update_state(state) + print(kub.kubs_id) + g_fsm = KickToPointP.KickToPoint(target) + # 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=normalize_angle(atan2(target.y - state.ballPos.y,target.x - state.ballPos.x))) + g_fsm.get_pos_as_vec2d(target) + #g_fsm.as_graphviz() + #g_fsm.write_diagram_png() + print('something before spin') + g_fsm.spin() + # + + + +#print str(kub.kubs_id) + str('***********') +rospy.init_node('node',anonymous=False) +start_time = rospy.Time.now() +start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + +# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000) + +while True: + state = None + rospy.wait_for_service('bsServer',) + getState = rospy.ServiceProxy('bsServer',bsServer) + try: + state = getState(state) + except rospy.ServiceException, e: + print("Error ",e) + if state: + print('lasknfcjscnajnstate',state.stateB.homePos) + function(BOT_ID,state.stateB) + print('chal ja') + # break +rospy.spin() + diff --git a/role/KickToPoint.py b/role/KickToPoint.py index a98cb1f1..ac60a5b9 100644 --- a/role/KickToPoint.py +++ b/role/KickToPoint.py @@ -16,8 +16,7 @@ class KickToPoint(behavior.Behavior): class State(Enum): normal = 1 setStance = 2 - turnAround = 3 - GoAndKick = 4 + GoAndKick = 3 def __init__(self,target,continuous=False): @@ -25,12 +24,14 @@ def __init__(self,target,continuous=False): self.name = "KickToPoint" - self.power = 7.0 - self.target_point = target self.go_at = None + self.power = 0 + + self.goto_kick=False; + self.setStanceThresh = 1.5*BOT_RADIUS self.ball_dist_thresh = BOT_BALL_THRESH @@ -42,9 +43,6 @@ def __init__(self,target,continuous=False): self.add_state(KickToPoint.State.setStance, behavior.Behavior.State.running) - - self.add_state(KickToPoint.State.turnAround, - behavior.Behavior.State.running) self.add_state(KickToPoint.State.GoAndKick, behavior.Behavior.State.running) @@ -56,24 +54,15 @@ def __init__(self,target,continuous=False): KickToPoint.State.setStance,lambda: self.setstance(),'far_away') self.add_transition(KickToPoint.State.normal, - KickToPoint.State.turnAround,lambda: self.turnAroundDirect(),'near_enough') - - self.add_transition(KickToPoint.State.normal, - KickToPoint.State.GoAndKick,lambda:self.GoAndKickDirect(),'very_close') - - self.add_transition(KickToPoint.State.setStance, - KickToPoint.State.turnAround,lambda:self.turnaround(),'turn') - - self.add_transition(KickToPoint.State.turnAround, - KickToPoint.State.GoAndKick,lambda: self.goandkick(),'kick') + KickToPoint.State.GoAndKick,lambda: self.GoAndKickDirect(),'very_close') self.add_transition(KickToPoint.State.GoAndKick, behavior.Behavior.State.completed,lambda: self.at_ball_pos(),'kicked!!!') self.add_transition(KickToPoint.State.setStance, - behavior.Behavior.State.failed,lambda: self.behavior_failed,'failed') + KickToPoint.State.GoAndKick,lambda: self.goto_kick,'kicking') - self.add_transition(KickToPoint.State.turnAround, + self.add_transition(KickToPoint.State.setStance, behavior.Behavior.State.failed,lambda: self.behavior_failed,'failed') self.add_transition(KickToPoint.State.GoAndKick, @@ -94,18 +83,14 @@ def add_target_theta(self): def setstance(self): global DIST_THRESH #print("hello") - theta = angle_diff(self.target_point, self.kub.state.ballPos) + theta = angle_diff(self.kub.state.ballPos,self.target_point) go_at = getPointToGo(self.kub.state.ballPos, theta) - return go_at.dist(self.get_pos_as_vec2d(self.kub.get_pos())) >= DIST_THRESH*0.85 + print "##########################" + return go_at.dist(self.get_pos_as_vec2d(self.kub.get_pos())) >= DIST_THRESH*1.00 - def turnaround(self): - return not self.bot_moving() #and not self.facing_the_target() and self.near_targetBall_line() and self.one_more() - - def turnAroundDirect(self): - return not self.setstance() and not self.bot_moving() def GoAndKickDirect(self): - return self.facing_the_target() and not self.setstance() + return not self.setstance() def goandkick(self): print("facing_the_target : ", self.facing_the_target()) @@ -126,7 +111,11 @@ def bot_moving(self): return True return False + def kicking_power(self): + return math.sqrt(dist(self.target_point,self.kub.state.ballPos)/6400.0)*5.0 + def ball_nearby(self, thresh = BOT_RADIUS*1.5): + if bot_in_front_of_ball(self.kub, thresh): return True else : @@ -135,7 +124,7 @@ def ball_nearby(self, thresh = BOT_RADIUS*1.5): def facing_the_target(self): print(" theta1 : ", self.theta) print(" bot theta : ", self.kub.get_theta()) - if vicinity_theta( self.theta, normalize_angle(self.kub.get_theta()), thresh=0.30 ): + if vicinity_theta( self.theta, self.kub.get_theta(), thresh=0.30 ): return True else : return False @@ -144,118 +133,61 @@ def at_ball_pos(self): error = 10 return vicinity_points(self.kub.get_pos(),self.kub.state.ballPos,thresh=BOT_BALL_THRESH+error) - - # def on_enter_setStance(self): - # global start_time - # start_time = rospy.Time.now() - # start_time = start_time.secs + 1.0*start_time.nsecs/pow(10,9) - - def on_enter_setStance(self): - print("entered setstance") - theta = angle_diff(self.target_point, self.kub.state.ballPos) - self.go_at = getPointToGo(self.kub.state.ballPos, theta) - _GoToPoint_.init(self.kub, self.go_at, self.theta) - pass - def reset(self): global start_time start_time = rospy.Time.now() start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + def on_enter_normal(self): + pass + def execute_normal(self): pass - # def execute_setStance(self): - # global start_time - # t = rospy.Time.now() - # t = t.secs + 1.0*t.nsecs/pow(10,9) - # #print(" t - start = ",t-start_time) - # theta = angle_diff(self.target_point, self.kub.state.ballPos) - # go_at = getPointToGo(self.kub.state.ballPos, theta) - # #print("goo", go_at) - # [vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, self.kub.kubs_id, Vector2D(go_at.x, go_at.y), self.kub.state.homePos, self.kub.state.awayPos, True) - # #vx, vy, vw, replanned - # #print("-------------------REPLANNED = ",REPLANNED) - # if(REPLANNED): - # self.reset() - - # # print("kubs_id = ",kub.kubs_id) - # curPos = self.kub.get_pos() - # #if vicinity_points(go_at, curPos, 4) == False: - # try: - # #print("vx = ",vx) - # #print("vy = ",vy) - # self.kub.move(vx, vy) - # #print(vw) - # #print("homePos") - # self.kub.turn(vw) - # self.kub.execute() - # except Exception as e: - # print("In except",e) - # pass - # print("exiting") + def on_exit_normal(self): + pass + def getPointToGo1(self,point, theta): + x = point.x - (1.5 * BOT_RADIUS) * (math.cos(theta)) + y = point.y - (1.5 * BOT_RADIUS) * (math.sin(theta)) + return Vector2D(int(x), int(y)) + + def on_enter_setStance(self): + print("entered setstance hello") + self.theta = normalize_angle(angle_diff( self.kub.state.ballPos,self.target_point)) + self.power = self.kicking_power() + self.go_at = self.getPointToGo1(self.kub.state.ballPos, self.theta) + _GoToPoint_.init(self.kub, self.go_at, self.theta) + pass + def execute_setStance(self): global DIST_THRESH - print("executing setstance") + print("executing setstance fnfnfj") start_time = rospy.Time.now() - start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + print "-----------------------##########-" generatingfunction = _GoToPoint_.execute(start_time,DIST_THRESH,True) + for gf in generatingfunction: self.kub,target_point = gf # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) theta = angle_diff(self.target_point, self.kub.state.ballPos) - self.go_at = getPointToGo(self.kub.state.ballPos, theta) - if not vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH): - self.behavior_failed = True - break - - - def on_enter_turnAround(self): - print("entered turnAround") - print(self.get_pos_as_vec2d(self.kub.get_pos()).dist(self.get_pos_as_vec2d(self.kub.state.ballPos))) - self.theta = normalize_angle(angle_diff(self.kub.state.ballPos,self.target_point)) - _turnAround_.init(self.kub, self.theta ) - pass - - # def execute_turnAround(self): - # print("TURNING") - # #print(data.homePos[BOT_ID].theta) - # #print(theta) - # theta = angle_diff(self.target_point, self.kub.state.ballpos) - # totalAngle = theta - # MAX_w = (MAX_BOT_OMEGA+MIN_BOT_OMEGA)/1.2 - # # theta_left = float(homePos[kub_id].theta-totalAngle) - # theta_lft = normalize_angle(normalize_angle(self.kub.state.homePos[self.kub.kubs_id].theta)-totalAngle)*-1.0+3.1412 - # vw = (theta_lft/2*math.pi)*MAX_w - # # print "totalAngle",radian_2_deg(totalAngle) - # # print "theta_left ",radian_2_deg(theta_lft),theta_lft - # # print "homePos theta ",radian_2_deg(normalize_angle(homePos[kub_id].theta)) - # # print "omega ",vw - # if abs(vw)<1*MIN_BOT_OMEGA: - # vw = 1*MIN_BOT_OMEGA*(1 if vw>0 else -1) - - # if abs(theta_lft)= 5.0*MAX_BOT_OMEGA: - # case = 1 - # ramp_upt = 5.0 - # ramp_dnt = 5.0 - # ramp_rampt = (theta_lft-5.0*MAX_BOT_OMEGA)*1.0/MAX_BOT_OMEGA - # elif theta_lft >= 2.5*MAX_BOT_OMEGA: - # case = 2 - # ramp_upt = 0.0 - # ramp_dnt = 5.0 - # ramp_rampt = (theta_lft-2.5*MAX_BOT_OMEGA)*1.0/MAX_BOT_OMEGA - # else: - # case = 3 - # ramp_rampt = 0.0 - # ramp_dnt = sqrt(2.0*theta_lft/MAX_BOT_OMEGA_ACC)*1.0 - # mvw = sqrt(2.0*theta_lft*MAX_BOT_OMEGA_ACC)*1.0 - # ramp_upt = 0.0 - # planned = True - # print(case) - # print(ramp_rampt) - # print(ramp_upt) - # print(ramp_dnt) - # print(mvw) + if vx == 0 and vy == 0: - print("TURNING") + + print(data.homePos[BOT_ID].theta) print(theta) totalAngle = theta MAX_w = (MAX_BOT_OMEGA+MIN_BOT_OMEGA)/1.2 # theta_left = float(homePos[kub_id].theta-totalAngle) theta_lft = normalize_angle(normalize_angle(data.homePos[BOT_ID].theta)-totalAngle)*-1.0+3.1412 - vw = (theta_lft/2*math.pi)*MAX_w - # print "totalAngle",radian_2_deg(totalAngle) - # print "theta_left ",radian_2_deg(theta_lft),theta_lft - # print "homePos theta ",radian_2_deg(normalize_angle(homePos[kub_id].theta)) - # print "omega ",vw - if abs(vw)<1*MIN_BOT_OMEGA: + + if abs(theta_lft)0 else -1) - if abs(theta_lft) 5.0 and diff <= 5.0+ramp_rampt: - # vw = MAX_BOT_OMEGA - # else: - # tim = 10.0+ramp_rampt-diff - # vw = tim*MAX_BOT_OMEGA_ACC - # elif case == 2: - # if diff <= ramp_rampt: - # vw = MAX_BOT_OMEGA - # else : - # tim = 5.0+ramp_rampt-diff - # vw = tim*MAX_BOT_OMEGA_ACC - # else : - # vw = mvw - MAX_BOT_OMEGA_ACC*diff - # if normalize_angle(homePos[BOT_ID].theta) < theta: - # vw = -1.0*vw - # print(vw) - # kub.reset() - # kub.turn(vw) - # kub.execute() + + if READY_TO_KICK : + print "\nAngle Remaining : ",theta_lft + print " ____________REACHED BALL, READY TO KICK____________\n" + rospy.signal_shutdown("=======ALIGNED======") + if __name__ == "__main__": global start_time, st, planned, case, mvw case = -1 - mvw = 0.0 st = None planned = False print("here") + + #Begin the process: Driver, START MOVING + rospy.init_node('node_new',anonymous=False) start_time = rospy.Time.now() start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + + pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000) print('testing...') - rospy.Subscriber('/belief_state', BeliefState, kp_callback, queue_size=1000) - rospy.Subscriber('/gui_params', point_SF, GUI_Callback, queue_size = 1000) + sub1 = rospy.Subscriber('/belief_state', BeliefState, kp_callback, queue_size=1000) + + sub2 = rospy.Subscriber('/gui_params', point_SF, GUI_Callback, queue_size = 1000) + + print("Ho gya!!") rospy.spin() + + sub1.unregister() + sub2.unregister() + print "AB MAIN KARUNGAAA -------- KICKKK !!!" + +command="python KickP.py %s %s %s"%(BOT_ID,target.x,target.y) +print(command) +os.system(command) + + + +#cmd = "date" +#returns output as byte string +#returned_output = subprocess.check_output(cmd) +#using decode() function to convert byte string to string +#print('Current date is:', returned_output.decode("utf-8")) From 917ff6edafe8b63761ec84fe241d6ea20c13b820 Mon Sep 17 00:00:00 2001 From: SambhawDrag Date: Sun, 12 Jul 2020 21:30:07 +0530 Subject: [PATCH 2/8] Changes in run.py to change factor; for KickToP --- test_KickToP.py | 1 + velocity/run.py | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/test_KickToP.py b/test_KickToP.py index 077964d5..b739de9d 100644 --- a/test_KickToP.py +++ b/test_KickToP.py @@ -1,3 +1,4 @@ + print "In test GoToPoint" from kubs import kubs, cmd_node from velocity.run import * diff --git a/velocity/run.py b/velocity/run.py index 09391357..35f1f9ef 100644 --- a/velocity/run.py +++ b/velocity/run.py @@ -30,7 +30,7 @@ def distance_(a, b): dy = a.y-b.y return sqrt(dx*dx+dy*dy) -def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False): +def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False, f=1.5): # print("Get_Vel: t - start = ",t-start) global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target REPLAN = 0 @@ -53,7 +53,7 @@ def Get_Vel(start, t, kub_id, target, homePos_, awayPos_,avoid_ball=False): findPath(startPt, target, avoid_ball) FIRST_CALL = 0 - if distance < 1.5*BOT_BALL_THRESH: + if distance < f*BOT_BALL_THRESH: return [0,0,0,0] # print("ex = ",expectedTraverseTime) # print("t = ",t," start = ",start) From d39d6535c39caebedeb57ccfb0b6fb8f50edc716 Mon Sep 17 00:00:00 2001 From: SambhawDrag Date: Thu, 16 Jul 2020 10:16:10 +0530 Subject: [PATCH 3/8] Modified turn-around and optimised alignment --- test_KickToP.py | 75 +++++++++++++++++++++++++++---------------------- 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/test_KickToP.py b/test_KickToP.py index b739de9d..eb47a1bc 100644 --- a/test_KickToP.py +++ b/test_KickToP.py @@ -1,7 +1,7 @@ - print "In test GoToPoint" from kubs import kubs, cmd_node from velocity.run import * +import velocity import rospy,sys,os import math import time @@ -73,7 +73,6 @@ def GUI_Callback(data): - def function(id_,state): kub = kubs.kubs(id_,state,pub) kub.update_state(state) @@ -90,15 +89,12 @@ def function(id_,state): g_fsm.spin() # - - - def kp_callback(data): global st, planned, ramp_rampt, ramp_dnt, ramp_upt, case, tnow ballpos = data.ballPos - theta = angle_diff(target, ballpos) - go_at = getPointBehindTheBall(ballpos, theta, 2) + theta = angle_diff(ballpos, target) #Point1: ballpos; Point2: target + go_at = getPointBehindTheBall(ballpos, theta, -2) #Modify getPoint*Behind*TheBall late on**. Factor should be negative. print(radian_2_deg(theta)) print(go_at.x) @@ -108,21 +104,19 @@ def kp_callback(data): t = t.secs + 1.0*t.nsecs/pow(10,9) print(" t - start = ",t-start_time) [vx, vy, vw, REPLANNED] = Get_Vel(start_time, t, BOT_ID, go_at, data.homePos, data.awayPos, True, 1.2) - #vx, vy, vw, replanned + #vx, vy, vw, replanned print("------------------- REPLANNED = ",REPLANNED) if(REPLANNED): reset() print("vx = ",vx) print("vy = ",vy) - - mvx=vx - mvy=vy # print("kubs_id = ",kub.kubs_id) curPos = Vector2D(int(data.homePos[BOT_ID].x),int(data.homePos[BOT_ID].y)) #if vicinity_points(go_at, curPos, 4) == False: + d=distance_(curPos,go_at) try: kub.move(vx, vy) print(vw) @@ -133,39 +127,50 @@ def kp_callback(data): print("In except",e) pass print(dist(go_at, curPos)) - - if vx == 0 and vy == 0: - + + #READY TO TAKE STANCE + if vx == 0 and vy == 0 and d < 1.2*BOT_BALL_THRESH: + print(data.homePos[BOT_ID].theta) - print(theta) + print("Target Alignment : ",theta) totalAngle = theta MAX_w = (MAX_BOT_OMEGA+MIN_BOT_OMEGA)/1.2 - # theta_left = float(homePos[kub_id].theta-totalAngle) - theta_lft = normalize_angle(normalize_angle(data.homePos[BOT_ID].theta)-totalAngle)*-1.0+3.1412 - - if abs(theta_lft)0 else -1) + vw = 3.0*(theta_lft)/(math.pi)*MAX_w + #vw = (theta_lft/2*math.pi)*MAX_w + if abs(vw)<1*MIN_w and READY_TO_KICK==False: + vw = 1*MIN_w*(1 if vw>0 else -1) + + if abs(vw) > MAX_w and READY_TO_KICK==False: + vw = (vw/abs(vw))*MAX_w if vw!=0: print("TURNING") + #print("vw Hoon Main =",vw) + print("BOT THETA: ",data.homePos[BOT_ID].theta) + print "\nAngle Remaining : ",theta_lft else: - print("DONE") + print("DONE !!") - mvw=vw - - - - print "Omega Return",vw - print "TRY: ",mvw + print "Omega Return: ",vw print READY_TO_KICK print "___________________________________________________________________" kub.reset() @@ -180,7 +185,7 @@ def kp_callback(data): if __name__ == "__main__": - global start_time, st, planned, case, mvw + global start_time, st, planned, case case = -1 st = None planned = False @@ -199,15 +204,17 @@ def kp_callback(data): sub2 = rospy.Subscriber('/gui_params', point_SF, GUI_Callback, queue_size = 1000) - print("Ho gya!!") + print("Ho gya align!!") rospy.spin() sub1.unregister() sub2.unregister() - print "AB MAIN KARUNGAAA -------- KICKKK !!!" + print "AB MAIN KARUNGA -------- KICKKK !!!" command="python KickP.py %s %s %s"%(BOT_ID,target.x,target.y) -print(command) +print("______________________________________________________________") +print("/n",command,"/n") +print("______________________________________________________________\n") os.system(command) From b3bb6b3566d19201c24fd31186e14b78866e16b8 Mon Sep 17 00:00:00 2001 From: SambhawDrag Date: Sun, 26 Jul 2020 22:52:49 +0530 Subject: [PATCH 4/8] Added role: DribbleKick.py and driver file test_DribbleKick.py --- role/DribbleKick.py | 510 ++++++++++++++++++++++++++++++++++++++++++++ test_DribbleKick.py | 52 +++++ 2 files changed, 562 insertions(+) create mode 100644 role/DribbleKick.py create mode 100644 test_DribbleKick.py diff --git a/role/DribbleKick.py b/role/DribbleKick.py new file mode 100644 index 00000000..4b1bdec8 --- /dev/null +++ b/role/DribbleKick.py @@ -0,0 +1,510 @@ +from enum import Enum +import behavior +import rospy +from role import GoToPoint, KickToPointP +import math +import kubs +from utils import math_functions +from utils.functions import * +from utils.state_functions import * +from utils.config import * +from velocity.run import * +from krssg_ssl_msgs.srv import * +import krssg_ssl_msgs.msg +import _turnAround_, _GoToPoint_ + +start_time = None +DIST_THRESH = 75 +dboxp = Vector2D((abs(OUR_DBOX_X)-(DBOX_HEIGHT/2)),0) + +class DribbleKick(behavior.Behavior): + """docstring for Dribble-and-Kick-into-Goal""" + class State(Enum): + + normal = 1 + movetoball = 2 + align = 3 + dribble = 4 + set_stance = 5 + goal = 6 + + def __init__(self,target): + + super(DribbleKick,self).__init__() + + self.name = "Dribble_Kick" + + self.goal_point = target + + self.go_at = None + + self.target_point = None + + self.dbox = dboxp + + self.power = 0 + + self.approachball = False; + + self.turn = False; + + self.goto_dribble = False; + + self.approachdbox = False; + + self.turndbox = False; + + self.ready_to_goal = False; + + self.theta = 0 + + self.behavior_failed = False + + + #States:- + self.add_state(DribbleKick.State.normal, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.movetoball, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.align, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.dribble, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.set_stance, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.goal, behavior.Behavior.State.running) + + + #Transition Functions:- + self.add_transition(behavior.Behavior.State.start, + DribbleKick.State.normal,lambda: True,'immediately') + + self.add_transition(DribbleKick.State.normal, + DribbleKick.State.movetoball, lambda: self.reachball(),'far_away_from_ball') + + self.add_transition(DribbleKick.State.movetoball, + DribbleKick.State.align, lambda: self.alignnow(),'Aligning_') + + self.add_transition(DribbleKick.State.align, + DribbleKick.State.dribble, lambda: self.goto_dribble,'Dribbling_') + + self.add_transition(DribbleKick.State.dribble, + DribbleKick.State.set_stance, lambda: self.at_DBox(), 'At D-Box_') + + self.add_transition(DribbleKick.State.set_stance, + DribbleKick.State.goal, lambda: self.readytogoal(), 'Goal!!') + + self.add_transition(DribbleKick.State.goal, + behavior.Behavior.State.completed, lambda: self.done(),'kicked!!!') + + #Behavior - Failed !!! + self.add_transition(DribbleKick.State.movetoball, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + self.add_transition(DribbleKick.State.align, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + self.add_transition(DribbleKick.State.dribble, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + self.add_transition(DribbleKick.State.set_stance, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + self.add_transition(DribbleKick.State.goal, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + + def add_kub(self,kub): + self.kub = kub + + def add_theta(self,theta): + self.theta = theta + + def get_pos_asvec2d(self, point2d): + return Vector2D(int(point2d.x),int(point2d.y)) + + def add_target_theta(self,theta): + self.theta = theta + + def point_on_D_Box(self): + return self.get_pos_asvec2d(self.dbox) + + def bot_moving(self): + if abs(self.kub.state.homeVel[self.kub.kubs_id].x) != 0 or abs(self.kub.state.homeVel[self.kub.kubs_id].y) != 0: + return True + return False + + def kicking_power(self): + return math.sqrt(dist(self.goal_point,self.kub.state.ballPos)/6400.0)*5.0 + +#LAMBDA FUNCTIONS:- + def reachball(self): + ballpos = self.kub.state.ballPos + theta = angle_diff(ballpos, self.point_on_D_Box()) + go_at = getPointBehindTheBall(ballpos, theta, -2) + return not(self.turn) and not(self.goto_dribble) and go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25 and self.approachball + + def alignnow(self): + ballpos = self.kub.state.ballPos + theta = angle_diff(ballpos, self.point_on_D_Box()) + go_at = getPointBehindTheBall(ballpos, theta, -2) + return not (go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25) and self.turn and not(self.goto_dribble) + + def at_DBox(self): + target = self.point_on_D_Box() + curPos = self.kub.get_pos() + return target.dist(self.get_pos_asvec2d(curPos)) <= DISTANCE_THRESH and self.turndbox + + def readytogoal(self): + bot_theta = self.kub.get_theta() + ballpos = self.get_pos_asvec2d(self.kub.state.ballPos) + totalAngle = angle_diff(ballpos, self.goal_point) + + deltheta = totalAngle-normalize_angle(bot_theta) + modtheta = min(abs(deltheta),abs((2*math.pi)-deltheta)) #shortest turn + sign = (normalize_angle(deltheta))/(abs(normalize_angle(deltheta))) + + theta_left = sign * modtheta + + return self.ready_to_goal and (abs(theta_left) < ROTATION_FACTOR/10) + + def done(self): + ballpos = self.get_pos_asvec2d(self.kub.state.ballPos) + condition = (ballpos.x >= 4500) and (ballpos.y > -300) and (ballpos.y < 300) + return condition + +# STATE FUNCTIONS:- + def on_enter_normal(self): + pass + + def execute_normal(self): + self.approachball=True; + + def on_exit_normal(self): + pass + + def on_enter_movetoball(self): + print("Entered movetoball") + self.theta = normalize_angle(angle_diff(self.kub.state.ballPos,self.point_on_D_Box())) + self.power = 0 + self.go_at = getPointBehindTheBall(self.kub.state.ballPos, self.theta, -2) + _GoToPoint_.init(self.kub, self.go_at, 0) + + def execute_movetoball(self): + + print("Executing MOVETOBALL ... ") + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + print "----------------------------------------------------------------" + + generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS, True) + + for gf in generatingfunction: + self.kub,target_point = gf + # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) + theta = angle_diff(self.kub.state.ballPos, self.point_on_D_Box()) + print(self.theta) + #print theta,"------------------------" + self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -1.5) + # #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH) + # if(dist(self.go_at,self.kub.get_pos())0 else -1) + + if abs(vw) > MAX_w and READY_TO_DRIBBLE==False: + vw = (vw/abs(vw))*MAX_w + + if vw!=0: + print("TURNING") + #print("vw Hoon Main =",vw) + #print("BOT THETA:",data.homePos[BOT_ID].theta) + print "\nAngle Remaining : ",theta_lft + else: + print("DONE !!") + break + + print "Omega Return: ",vw + print READY_TO_DRIBBLE + print "___________________________________________________________________" + self.kub.reset() + self.kub.turn(vw) + self.kub.execute() + + try: + prev_state = getState(prev_state).stateB + except rospy.ServiceException, e: + print("Error :: ROS ", e) + # print(kub.state) + self.kub.update_state(prev_state) + if not(prev_state == self.kub.state): + prev_state = self.kub.state + + self.turn = False; + self.goto_dribble = True; + print("HO GYA ALIGN !! ---------------------------------------") + + def on_exit_align(self): + print("EXITING ALIGN... goto_dribble:",self.goto_dribble) + self.goto_dribble = True; #Now, Ready to dribble after aligning + self.approachball = False; + self.turn = False; + + + def on_enter_dribble(self): + print("Enter DRIBBLE------------------------------") + self.go_at = self.point_on_D_Box() + ballpos = self.kub.state.ballPos + #self.theta = normalize_angle(angle_diff(self.point_on_D_Box(), self.get_pos_asvec2d(self.goal_point))) + self.theta = angle_diff(ballpos, self.point_on_D_Box()) + self.kub.reset() + self.approachball = False; + self.kub.dribbler = True; + self.kub.execute() + _GoToPoint_.init(self.kub, self.go_at, self.theta) + + + def execute_dribble(self): + + print("Executing DRIBBLE ... ") + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + print "----------------------------------------------------------------" + self.kub.dribbler = True; + self.kub.execute() + + generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS*2.0, False) + + for gf in generatingfunction: + self.kub,target_point = gf + # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) + print "--------------------------------------------------" + ballPos=self.kub.state.ballPos + print(ballPos) + print "--------------------------------------------------" + theta = angle_diff(self.kub.state.ballPos, self.point_on_D_Box()) + print(self.theta) + self.kub.dribbler = True; + #self.kub.execute() + #self.kub.kick(self.power) + #print theta,"------------------------" + #self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -2) + # #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH) + # if(dist(self.go_at,self.kub.get_pos())0 else -1) + + if abs(vw) > MAX_w and READY_TO_GOAL==False: + vw = (vw/abs(vw))*MAX_w + + if vw!=0: + print("TURNING") + #print("vw Hoon Main =",vw) + #print("BOT THETA:",data.homePos[BOT_ID].theta) + print "\nAngle Remaining : ",theta_lft + else: + print("DONE !!") + break + + print "Omega Return: ",vw + print READY_TO_GOAL + print "___________________________________________________________________" + self.kub.reset() + self.kub.turn(vw) + self.kub.execute() + + try: + prev_state = getState(prev_state).stateB + except rospy.ServiceException, e: + print("Error :: ROS ", e) + # print(kub.state) + self.kub.update_state(prev_state) + if not(prev_state == self.kub.state): + prev_state = self.kub.state + + self.turndbox = False; + self.ready_to_goal = True; + print("HO GYA GOAL KE TARAF ALIGN !! ---------------------------------------") + + + def exit_setstance(self): + print("EXIT SETSTANCE AT DBOX--------------------------") + print("turndbox :",self.turndbox) + + + def on_enter_goal(self): + print("Enter GOAL ...") + self.power=self.kicking_power() + KickToPointP.init(self.goal_point) + + + def execute_goal(self): + print("Goaling ... ") + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + print "----------------------------------------------------------------" + + self.kub.reset() + self.kub.kick(self.power) + self.execute() + + self.ready_to_goal = False; + + def on_exit_goal(self): + self.kub.reset() + self.kub.execute() + print("GOALLL !!!") + print("EXITING ROLE") + \ No newline at end of file diff --git a/test_DribbleKick.py b/test_DribbleKick.py new file mode 100644 index 00000000..d4071f64 --- /dev/null +++ b/test_DribbleKick.py @@ -0,0 +1,52 @@ +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, DribbleKick +from multiprocessing import Process +from kubs import kubs +from krssg_ssl_msgs.srv import bsServer +from math import atan2,pi +from utils.functions import * +pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) + +BOT_ID = int(sys.argv[1]) +x=int(sys.argv[2]) +y=int(sys.argv[3]) + +target=Vector2D(x,y) + +def function(id_,state): + kub = kubs.kubs(id_,state,pub) + kub.update_state(state) + print("BOT ",kub.kubs_id ," IN ACTION ------") + #ub.state.homePos[kub.kubs_id].theta += 3.1412 + g_fsm = DribbleKick.DribbleKick(target) + g_fsm.add_kub(kub) + #g_fsm.as_graphviz() + #g_fsm.write_diagram_png() + g_fsm.spin() + + +rospy.init_node('node',anonymous=False) +start_time = rospy.Time.now() +start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + +# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000) + +while True: + state=None + rospy.wait_for_service('bsServer',) + getState = rospy.ServiceProxy('bsServer',bsServer) + try: + state = getState(state) + except rospy.ServiceException, e: + print e + if state: + function(BOT_ID,state.stateB) + # break +rospy.spin() From b1bc672123679c5c6c58ba9f712d83f34bd68602 Mon Sep 17 00:00:00 2001 From: Sambhaw Kumar Date: Sat, 19 Sep 2020 14:16:03 +0530 Subject: [PATCH 5/8] Delete DribbleKick.py --- role/DribbleKick.py | 510 -------------------------------------------- 1 file changed, 510 deletions(-) delete mode 100644 role/DribbleKick.py diff --git a/role/DribbleKick.py b/role/DribbleKick.py deleted file mode 100644 index 4b1bdec8..00000000 --- a/role/DribbleKick.py +++ /dev/null @@ -1,510 +0,0 @@ -from enum import Enum -import behavior -import rospy -from role import GoToPoint, KickToPointP -import math -import kubs -from utils import math_functions -from utils.functions import * -from utils.state_functions import * -from utils.config import * -from velocity.run import * -from krssg_ssl_msgs.srv import * -import krssg_ssl_msgs.msg -import _turnAround_, _GoToPoint_ - -start_time = None -DIST_THRESH = 75 -dboxp = Vector2D((abs(OUR_DBOX_X)-(DBOX_HEIGHT/2)),0) - -class DribbleKick(behavior.Behavior): - """docstring for Dribble-and-Kick-into-Goal""" - class State(Enum): - - normal = 1 - movetoball = 2 - align = 3 - dribble = 4 - set_stance = 5 - goal = 6 - - def __init__(self,target): - - super(DribbleKick,self).__init__() - - self.name = "Dribble_Kick" - - self.goal_point = target - - self.go_at = None - - self.target_point = None - - self.dbox = dboxp - - self.power = 0 - - self.approachball = False; - - self.turn = False; - - self.goto_dribble = False; - - self.approachdbox = False; - - self.turndbox = False; - - self.ready_to_goal = False; - - self.theta = 0 - - self.behavior_failed = False - - - #States:- - self.add_state(DribbleKick.State.normal, behavior.Behavior.State.running) - - self.add_state(DribbleKick.State.movetoball, behavior.Behavior.State.running) - - self.add_state(DribbleKick.State.align, behavior.Behavior.State.running) - - self.add_state(DribbleKick.State.dribble, behavior.Behavior.State.running) - - self.add_state(DribbleKick.State.set_stance, behavior.Behavior.State.running) - - self.add_state(DribbleKick.State.goal, behavior.Behavior.State.running) - - - #Transition Functions:- - self.add_transition(behavior.Behavior.State.start, - DribbleKick.State.normal,lambda: True,'immediately') - - self.add_transition(DribbleKick.State.normal, - DribbleKick.State.movetoball, lambda: self.reachball(),'far_away_from_ball') - - self.add_transition(DribbleKick.State.movetoball, - DribbleKick.State.align, lambda: self.alignnow(),'Aligning_') - - self.add_transition(DribbleKick.State.align, - DribbleKick.State.dribble, lambda: self.goto_dribble,'Dribbling_') - - self.add_transition(DribbleKick.State.dribble, - DribbleKick.State.set_stance, lambda: self.at_DBox(), 'At D-Box_') - - self.add_transition(DribbleKick.State.set_stance, - DribbleKick.State.goal, lambda: self.readytogoal(), 'Goal!!') - - self.add_transition(DribbleKick.State.goal, - behavior.Behavior.State.completed, lambda: self.done(),'kicked!!!') - - #Behavior - Failed !!! - self.add_transition(DribbleKick.State.movetoball, - behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') - - self.add_transition(DribbleKick.State.align, - behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') - - self.add_transition(DribbleKick.State.dribble, - behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') - - self.add_transition(DribbleKick.State.set_stance, - behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') - - self.add_transition(DribbleKick.State.goal, - behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') - - - def add_kub(self,kub): - self.kub = kub - - def add_theta(self,theta): - self.theta = theta - - def get_pos_asvec2d(self, point2d): - return Vector2D(int(point2d.x),int(point2d.y)) - - def add_target_theta(self,theta): - self.theta = theta - - def point_on_D_Box(self): - return self.get_pos_asvec2d(self.dbox) - - def bot_moving(self): - if abs(self.kub.state.homeVel[self.kub.kubs_id].x) != 0 or abs(self.kub.state.homeVel[self.kub.kubs_id].y) != 0: - return True - return False - - def kicking_power(self): - return math.sqrt(dist(self.goal_point,self.kub.state.ballPos)/6400.0)*5.0 - -#LAMBDA FUNCTIONS:- - def reachball(self): - ballpos = self.kub.state.ballPos - theta = angle_diff(ballpos, self.point_on_D_Box()) - go_at = getPointBehindTheBall(ballpos, theta, -2) - return not(self.turn) and not(self.goto_dribble) and go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25 and self.approachball - - def alignnow(self): - ballpos = self.kub.state.ballPos - theta = angle_diff(ballpos, self.point_on_D_Box()) - go_at = getPointBehindTheBall(ballpos, theta, -2) - return not (go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25) and self.turn and not(self.goto_dribble) - - def at_DBox(self): - target = self.point_on_D_Box() - curPos = self.kub.get_pos() - return target.dist(self.get_pos_asvec2d(curPos)) <= DISTANCE_THRESH and self.turndbox - - def readytogoal(self): - bot_theta = self.kub.get_theta() - ballpos = self.get_pos_asvec2d(self.kub.state.ballPos) - totalAngle = angle_diff(ballpos, self.goal_point) - - deltheta = totalAngle-normalize_angle(bot_theta) - modtheta = min(abs(deltheta),abs((2*math.pi)-deltheta)) #shortest turn - sign = (normalize_angle(deltheta))/(abs(normalize_angle(deltheta))) - - theta_left = sign * modtheta - - return self.ready_to_goal and (abs(theta_left) < ROTATION_FACTOR/10) - - def done(self): - ballpos = self.get_pos_asvec2d(self.kub.state.ballPos) - condition = (ballpos.x >= 4500) and (ballpos.y > -300) and (ballpos.y < 300) - return condition - -# STATE FUNCTIONS:- - def on_enter_normal(self): - pass - - def execute_normal(self): - self.approachball=True; - - def on_exit_normal(self): - pass - - def on_enter_movetoball(self): - print("Entered movetoball") - self.theta = normalize_angle(angle_diff(self.kub.state.ballPos,self.point_on_D_Box())) - self.power = 0 - self.go_at = getPointBehindTheBall(self.kub.state.ballPos, self.theta, -2) - _GoToPoint_.init(self.kub, self.go_at, 0) - - def execute_movetoball(self): - - print("Executing MOVETOBALL ... ") - start_time = rospy.Time.now() - start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) - print "----------------------------------------------------------------" - - generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS, True) - - for gf in generatingfunction: - self.kub,target_point = gf - # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) - theta = angle_diff(self.kub.state.ballPos, self.point_on_D_Box()) - print(self.theta) - #print theta,"------------------------" - self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -1.5) - # #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH) - # if(dist(self.go_at,self.kub.get_pos())0 else -1) - - if abs(vw) > MAX_w and READY_TO_DRIBBLE==False: - vw = (vw/abs(vw))*MAX_w - - if vw!=0: - print("TURNING") - #print("vw Hoon Main =",vw) - #print("BOT THETA:",data.homePos[BOT_ID].theta) - print "\nAngle Remaining : ",theta_lft - else: - print("DONE !!") - break - - print "Omega Return: ",vw - print READY_TO_DRIBBLE - print "___________________________________________________________________" - self.kub.reset() - self.kub.turn(vw) - self.kub.execute() - - try: - prev_state = getState(prev_state).stateB - except rospy.ServiceException, e: - print("Error :: ROS ", e) - # print(kub.state) - self.kub.update_state(prev_state) - if not(prev_state == self.kub.state): - prev_state = self.kub.state - - self.turn = False; - self.goto_dribble = True; - print("HO GYA ALIGN !! ---------------------------------------") - - def on_exit_align(self): - print("EXITING ALIGN... goto_dribble:",self.goto_dribble) - self.goto_dribble = True; #Now, Ready to dribble after aligning - self.approachball = False; - self.turn = False; - - - def on_enter_dribble(self): - print("Enter DRIBBLE------------------------------") - self.go_at = self.point_on_D_Box() - ballpos = self.kub.state.ballPos - #self.theta = normalize_angle(angle_diff(self.point_on_D_Box(), self.get_pos_asvec2d(self.goal_point))) - self.theta = angle_diff(ballpos, self.point_on_D_Box()) - self.kub.reset() - self.approachball = False; - self.kub.dribbler = True; - self.kub.execute() - _GoToPoint_.init(self.kub, self.go_at, self.theta) - - - def execute_dribble(self): - - print("Executing DRIBBLE ... ") - start_time = rospy.Time.now() - start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) - print "----------------------------------------------------------------" - self.kub.dribbler = True; - self.kub.execute() - - generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS*2.0, False) - - for gf in generatingfunction: - self.kub,target_point = gf - # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) - print "--------------------------------------------------" - ballPos=self.kub.state.ballPos - print(ballPos) - print "--------------------------------------------------" - theta = angle_diff(self.kub.state.ballPos, self.point_on_D_Box()) - print(self.theta) - self.kub.dribbler = True; - #self.kub.execute() - #self.kub.kick(self.power) - #print theta,"------------------------" - #self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -2) - # #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH) - # if(dist(self.go_at,self.kub.get_pos())0 else -1) - - if abs(vw) > MAX_w and READY_TO_GOAL==False: - vw = (vw/abs(vw))*MAX_w - - if vw!=0: - print("TURNING") - #print("vw Hoon Main =",vw) - #print("BOT THETA:",data.homePos[BOT_ID].theta) - print "\nAngle Remaining : ",theta_lft - else: - print("DONE !!") - break - - print "Omega Return: ",vw - print READY_TO_GOAL - print "___________________________________________________________________" - self.kub.reset() - self.kub.turn(vw) - self.kub.execute() - - try: - prev_state = getState(prev_state).stateB - except rospy.ServiceException, e: - print("Error :: ROS ", e) - # print(kub.state) - self.kub.update_state(prev_state) - if not(prev_state == self.kub.state): - prev_state = self.kub.state - - self.turndbox = False; - self.ready_to_goal = True; - print("HO GYA GOAL KE TARAF ALIGN !! ---------------------------------------") - - - def exit_setstance(self): - print("EXIT SETSTANCE AT DBOX--------------------------") - print("turndbox :",self.turndbox) - - - def on_enter_goal(self): - print("Enter GOAL ...") - self.power=self.kicking_power() - KickToPointP.init(self.goal_point) - - - def execute_goal(self): - print("Goaling ... ") - start_time = rospy.Time.now() - start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) - print "----------------------------------------------------------------" - - self.kub.reset() - self.kub.kick(self.power) - self.execute() - - self.ready_to_goal = False; - - def on_exit_goal(self): - self.kub.reset() - self.kub.execute() - print("GOALLL !!!") - print("EXITING ROLE") - \ No newline at end of file From 9f9203074e01e0b93d73927b2af3175e2d1a549a Mon Sep 17 00:00:00 2001 From: Sambhaw Kumar Date: Sat, 19 Sep 2020 14:16:47 +0530 Subject: [PATCH 6/8] Add files via upload --- role/Dribble_Kick.py | 372 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 372 insertions(+) create mode 100644 role/Dribble_Kick.py diff --git a/role/Dribble_Kick.py b/role/Dribble_Kick.py new file mode 100644 index 00000000..249c9119 --- /dev/null +++ b/role/Dribble_Kick.py @@ -0,0 +1,372 @@ +from enum import Enum +import behavior +import rospy +from role import GoToPoint +import math +import kubs +from utils import math_functions +from utils.functions import * +from utils.state_functions import * +from utils.config import * +from velocity.run import * +from krssg_ssl_msgs.srv import * +import krssg_ssl_msgs.msg +import _turnAround_, _GoToPoint_ + +start_time = None +DIST_THRESH = 75 +dboxp = Vector2D((abs(OUR_DBOX_X)-(DBOX_HEIGHT/2)),0) + +class DribbleKick(behavior.Behavior): + """docstring for Dribble-and-Kick-into-Goal""" + class State(Enum): + + normal = 1 + movetoball = 2 + align = 3 + dribble = 4 + goal = 5 + + def __init__(self,target): + + super(DribbleKick,self).__init__() + + self.name = "Dribble_Kick" + + self.goal_point = target + + self.go_at = None + + self.target_point = None + + self.dbox = dboxp + + self.power = 0 + + self.approachball = False + + self.turn = False + + self.goto_dribble = False + + self.approachdbox = False + + self.readytogoal = False + + self.theta = 0 + + self.behavior_failed = False + + + #States:- + self.add_state(DribbleKick.State.normal, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.movetoball, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.align, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.dribble, behavior.Behavior.State.running) + + self.add_state(DribbleKick.State.goal, behavior.Behavior.State.running) + + + #Transition Functions:- + self.add_transition(behavior.Behavior.State.start, + DribbleKick.State.normal,lambda: True,'immediately') + + self.add_transition(DribbleKick.State.normal, + DribbleKick.State.movetoball, lambda: self.reachball(),'far_away_from_ball') + + self.add_transition(DribbleKick.State.movetoball, + DribbleKick.State.align, lambda: self.alignnow(),'Aligning_') + + self.add_transition(DribbleKick.State.align, + DribbleKick.State.dribble, lambda: self.goto_dribble,'Dribbling_') + + self.add_transition(DribbleKick.State.dribble, + DribbleKick.State.goal, lambda: self.readytogoal, 'Goaling!') + + self.add_transition(DribbleKick.State.goal, + behavior.Behavior.State.completed, lambda: self.done(),'Ball inside Goal') + + #Behavior - Failed !!! + self.add_transition(DribbleKick.State.movetoball, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + self.add_transition(DribbleKick.State.align, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + self.add_transition(DribbleKick.State.dribble, + behavior.Behavior.State.failed, lambda: self.behavior_failed, 'failed') + + + def add_kub(self,kub): + self.kub = kub + + def add_theta(self,theta): + self.theta = theta + + def get_pos_asvec2d(self, point2d): + return Vector2D(int(point2d.x),int(point2d.y)) + + def add_target_theta(self,theta): + self.theta = theta + + def point_on_D_Box(self): + return self.get_pos_asvec2d(self.dbox) + + def bot_moving(self): + if abs(self.kub.state.homeVel[self.kub.kubs_id].x) != 0 or abs(self.kub.state.homeVel[self.kub.kubs_id].y) != 0: + return True + return False + + def kicking_power(self): + return math.sqrt(dist(self.goal_point,self.kub.state.ballPos)/6400.0)*5.0 + +#LAMBDA FUNCTIONS:- + def reachball(self): + ballpos = self.kub.state.ballPos + theta = angle_diff(ballpos, self.point_on_D_Box()) + go_at = getPointBehindTheBall(ballpos, theta, -2) + return not(self.turn) and not(self.goto_dribble) and go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25 and self.approachball + + def alignnow(self): + ballpos = self.kub.state.ballPos + theta = angle_diff(ballpos, self.point_on_D_Box()) + go_at = getPointBehindTheBall(ballpos, theta, -2) + return not (go_at.dist(self.get_pos_asvec2d(self.kub.get_pos())) >= DISTANCE_THRESH*0.25) and self.turn and not(self.goto_dribble) + + def done(self): + ballpos = self.get_pos_asvec2d(self.kub.state.ballPos) + condition = (ballpos.x >= 4500) and (ballpos.y > -300) and (ballpos.y < 300) + return condition + +# STATE FUNCTIONS:- + def on_enter_normal(self): + pass + + def execute_normal(self): + self.approachball=True + + def on_exit_normal(self): + pass + + def on_enter_movetoball(self): + print("Entered movetoball") + self.theta = normalize_angle(angle_diff(self.kub.state.ballPos,self.point_on_D_Box())) + self.power = 0 + self.go_at = getPointBehindTheBall(self.kub.state.ballPos, self.theta, -2) + _GoToPoint_.init(self.kub, self.go_at, 0) + + def execute_movetoball(self): + + print("Executing MOVETOBALL ... ") + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + print "----------------------------------------------------------------" + + generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS, True) + + for gf in generatingfunction: + self.kub,target_point = gf + # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) + theta = angle_diff(self.kub.state.ballPos, self.point_on_D_Box()) + print(self.theta) + #print theta,"------------------------" + self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -1.5) + # #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH) + # if(dist(self.go_at,self.kub.get_pos())0 else -1) + + if abs(vw) > MAX_w and READY_TO_DRIBBLE==False: + vw = (vw/abs(vw))*MAX_w + + if vw!=0: + print("TURNING") + #print("vw Hoon Main =",vw) + #print("BOT THETA:",data.homePos[BOT_ID].theta) + print "\nAngle Remaining : ",theta_lft + else: + print("DONE !!") + break + + print "Omega Return: ",vw + print READY_TO_DRIBBLE + print "___________________________________________________________________" + self.kub.reset() + self.kub.turn(vw) + self.kub.execute() + + try: + prev_state = getState(prev_state).stateB + except rospy.ServiceException, e: + print("Error :: ROS ", e) + # print(kub.state) + self.kub.update_state(prev_state) + if not(prev_state == self.kub.state): + prev_state = self.kub.state + + self.turn = False + self.goto_dribble = True + print("HO GYA ALIGN !! ---------------------------------------") + + def on_exit_align(self): + print("EXITING ALIGN... goto_dribble:",self.goto_dribble) + self.goto_dribble = True #Now, Ready to dribble after aligning + self.approachball = False + self.turn = False + + + def on_enter_dribble(self): + print("Enter DRIBBLE------------------------------") + self.go_at = self.point_on_D_Box() + ballpos = self.kub.state.ballPos + #self.theta = normalize_angle(angle_diff(self.point_on_D_Box(), self.get_pos_asvec2d(self.goal_point))) + self.theta = angle_diff(ballpos, self.get_pos_asvec2d(self.goal_point)) + print "Required Rotation while dribble: ",self.theta + print "Goal Point: ",self.goal_point + self.kub.reset() + self.approachball = False + self.kub.dribbler = True + self.kub.execute() + _GoToPoint_.init(self.kub, self.go_at, 0) + + + def execute_dribble(self): + + print("Executing DRIBBLE ... ") + start_time = rospy.Time.now() + start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + print "----------------------------------------------------------------" + self.kub.dribbler = True + self.kub.execute() + + generatingfunction = _GoToPoint_.execute(start_time, BOT_RADIUS*2.0, False) + + for gf in generatingfunction: + self.kub,target_point = gf + # self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta) + print "--------------------------------------------------" + ballPos=self.kub.state.ballPos + print(ballPos) + print "--------------------------------------------------" + theta = angle_diff(self.kub.state.ballPos, self.goal_point) + print(self.theta) + self.kub.dribbler = True + #self.kub.execute() + #self.kub.kick(self.power) + #print theta,"------------------------" + #self.go_at = getPointBehindTheBall(self.kub.state.ballPos, theta, -2) + # #print vicinity_points(self.go_at,target_point,thresh=BOT_BALL_THRESH) + # if(dist(self.go_at,self.kub.get_pos()) Date: Sat, 19 Sep 2020 14:17:22 +0530 Subject: [PATCH 7/8] Delete test_DribbleKick.py --- test_DribbleKick.py | 52 --------------------------------------------- 1 file changed, 52 deletions(-) delete mode 100644 test_DribbleKick.py diff --git a/test_DribbleKick.py b/test_DribbleKick.py deleted file mode 100644 index d4071f64..00000000 --- a/test_DribbleKick.py +++ /dev/null @@ -1,52 +0,0 @@ -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, DribbleKick -from multiprocessing import Process -from kubs import kubs -from krssg_ssl_msgs.srv import bsServer -from math import atan2,pi -from utils.functions import * -pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) - -BOT_ID = int(sys.argv[1]) -x=int(sys.argv[2]) -y=int(sys.argv[3]) - -target=Vector2D(x,y) - -def function(id_,state): - kub = kubs.kubs(id_,state,pub) - kub.update_state(state) - print("BOT ",kub.kubs_id ," IN ACTION ------") - #ub.state.homePos[kub.kubs_id].theta += 3.1412 - g_fsm = DribbleKick.DribbleKick(target) - g_fsm.add_kub(kub) - #g_fsm.as_graphviz() - #g_fsm.write_diagram_png() - g_fsm.spin() - - -rospy.init_node('node',anonymous=False) -start_time = rospy.Time.now() -start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) - -# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000) - -while True: - state=None - rospy.wait_for_service('bsServer',) - getState = rospy.ServiceProxy('bsServer',bsServer) - try: - state = getState(state) - except rospy.ServiceException, e: - print e - if state: - function(BOT_ID,state.stateB) - # break -rospy.spin() From a426617a28b1721b846e9599404bff85155a3841 Mon Sep 17 00:00:00 2001 From: Sambhaw Kumar Date: Sat, 19 Sep 2020 14:18:46 +0530 Subject: [PATCH 8/8] Dribble_Kick completed and improved. Test file for the same. --- test_Dribble_Kick.py | 55 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 test_Dribble_Kick.py diff --git a/test_Dribble_Kick.py b/test_Dribble_Kick.py new file mode 100644 index 00000000..a69ba86c --- /dev/null +++ b/test_Dribble_Kick.py @@ -0,0 +1,55 @@ +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, Dribble_Kick +from multiprocessing import Process +from kubs import kubs +from krssg_ssl_msgs.srv import bsServer +from math import atan2,pi +from utils.functions import * +pub = rospy.Publisher('/grsim_data',gr_Commands,queue_size=1000) + +'''Arguments : (BOT_ID) (x coordinate of goal point) (y coordinate of goal point)''' +# Can try making it dynamic according to goalie's position, during gameplay... + +BOT_ID = int(sys.argv[1]) +x=int(sys.argv[2]) +y=int(sys.argv[3]) + +target=Vector2D(x,y) + +def function(id_,state): + kub = kubs.kubs(id_,state,pub) + kub.update_state(state) + print("BOT ",kub.kubs_id ," IN ACTION ------") + #ub.state.homePos[kub.kubs_id].theta += 3.1412 + g_fsm = Dribble_Kick.DribbleKick(target) + g_fsm.add_kub(kub) + #g_fsm.as_graphviz() + #g_fsm.write_diagram_png() + g_fsm.spin() + + +rospy.init_node('node',anonymous=False) +start_time = rospy.Time.now() +start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9) + +# rospy.Subscriber('/belief_state', BeliefState, BS_callback, queue_size=1000) + +while True: + state=None + rospy.wait_for_service('bsServer',) + getState = rospy.ServiceProxy('bsServer',bsServer) + try: + state = getState(state) + except rospy.ServiceException, e: + print e + if state: + function(BOT_ID,state.stateB) + # break +rospy.spin()