def main():
import memcache
shared = memcache.Client(['127.0.0.1:11211'], debug=False)
paddingX = 300
paddingY = 200
points = [
Vector2D((HALF_FIELD_MAXX - paddingX), (HALF_FIELD_MAXY - paddingY)),
Vector2D((HALF_FIELD_MAXX - paddingX), -(HALF_FIELD_MAXY - paddingY)),
Vector2D(-(HALF_FIELD_MAXX - paddingX), -(HALF_FIELD_MAXY - paddingY)),
Vector2D(-(HALF_FIELD_MAXX - paddingX), (HALF_FIELD_MAXY - paddingY)),
]
BOT_ID = 0
pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
state = shared.get('state')
kub = kubs.kubs(BOT_ID, state, pub)
while True:
for p in points:
state = shared.get('state')
kub.update_state(state)
g_fsm = GoToPoint.GoToPoint()
g_fsm.add_kub(kub)
g_fsm.add_point(point=p, orient=state.homePos[kub.kubs_id].theta)
g_fsm.spin()
def planner_callback(state):
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 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)
def skills_GoToPoint(state, bot_id, point):
sParams = skills_union.SParam()
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballVel = Vector2D(int(state.ballVel.x), int(state.ballVel.y))
distance = botPos.dist(ballPos)
sParams.GoToPointP.x = point.x
sParams.GoToPointP.y = point.y
sGoToPoint.execute(sParams, state, bot_id, pub)
def LDefender_callback(state):
global pub, LDefender_tac
# LDefender_id = 0
LDefender_id = 1
ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
botpos = Vector2D(state.homePos[0].x, state.homePos[0].y)
print("dist ", ballPos.dist(botpos))
# return
if LDefender_tac == None:
LDefender_tac = TLDefender.TLDefender(LDefender_id, state)
LDefender_tac.execute(state, pub)
def get_all(state, bot_id):
global fuzz_passing, HALF_FIELD_MAXX_REAL
team = [x for x in xrange(6)]
opp_player = pass_analysis(bot_id, state)
param = ()
max_score = -1
index = -1
botPos = Vector2D(state.awayPos[bot_id].x, state.awayPos[bot_id].y)
ourgoalPos = Vector2D(-HALF_FIELD_MAXX_REAL, 0)
for player in team:
if player == bot_id:
continue
else:
recPos = Vector2D(state.awayPos[player].x, state.awayPos[player].y)
#param = our_player.ratios( state, player)
sep = botPos.dist(recPos)
sep_from_goal = recPos.dist(ourgoalPos)
if sep_from_goal < HALF_FIELD_MAXX_REAL * 2.0:
fuzz_passing.input['dis_from_goal'] = sep_from_goal
else:
fuzz_passing.input['dis_from_goal'] = HALF_FIELD_MAXX_REAL * 2
if sep < HALF_FIELD_MAXX_REAL * 2.0:
fuzz_passing.input['dis_bn_p_r'] = sep
else:
fuzz_passing.input['dis_bn_p_r'] = HALF_FIELD_MAXX_REAL * 2.0
t = time.time()
param = opp_player.evr_params(state, player, botPos, recPos, sep)
print "$" * 20, " ", time.time() - t, " ", "$" * 20
fuzz_passing.input['danger_no'] = param[1]
fuzz_passing.input['net_ratio'] = param[0]
fuzz_passing.compute()
print "$" * 20, " ", time.time() - t, " ", "$" * 20
if len(T) < 5:
T.append([time.time() - t, player])
score = fuzz_passing.output['pass_score']
#print player, state.homePos[player].x, state.homePos[player].y
print " --------- "
print "dis_from_goal: ", recPos.dist(
oppgoalPos), "\ndis_bn_p_r: ", botPos.dist(
recPos), "\nintercept_ratio: ", param[
0], "\nnearness_ratio: ", param[1]
print "SCORE--> ", score
print recPos.x, " ", recPos.y
if score > max_score:
max_score = score
index = player
print "#" * 50
print T
print "BOT_TO_PASS---->"
return index, max_score
def init(_kub, ballpos, pointB, theta_whiledribble):
global kub, APPROACH_POINT, BALLPOS, GOAL_POINT, rotate, FLAG_turn, FLAG_dribble, FLAG_move, FIRST_CALL
kub = _kub
GOAL_POINT = Vector2D()
APPROACH_POINT = Vector2D()
BALLPOS = Vector2D()
BALLPOS.x = ballpos.x
BALLPOS.y = ballpos.y
rotate = theta_whiledribble
GOAL_POINT.x = pointB.x
GOAL_POINT.y = pointB.y
FLAG_move = False
FLAG_dribble = False
FLAG_turn = False
FIRST_CALL = True
def findPath(startPoint, end, avoid_ball=False):
global FLAG_PATH_RECEIVED, REPLAN
FLAG_PATH_RECEIVED = 1
REPLAN = 1
global v, expectedTraverseTime, kubid
global start, target
global pso, errorInfo
startPt = point_2d()
target = point_2d()
startPt.x = startPoint.x
startPt.y = startPoint.y
target.x = end.x
target.y = end.y
# print("Start Point ",startPt.x,startPt.y)
# print("Target Point",target.x,target.y)
# print("Waiting for service")
rospy.wait_for_service('planner')
planner = rospy.ServiceProxy('planner', path_plan)
message = planner(kubid, startPt, target, avoid_ball)
path = []
for i in xrange(len(message.path)):
path = path + [
Vector2D(int(message.path[i].x), int(message.path[i].y))
]
start = rospy.Time.now()
start = 1.0 * start.secs + 1.0 * start.nsecs / pow(10, 9)
v = Velocity(path, start, startPt)
v.updateAngle()
expectedTraverseTime = v.getTime(v.GetPathLength())
global time_cal
time_cal = expectedTraverseTime
# pso = PSO(5,20,1000,1,1,0.5)
errorInfo = Error()
def skills_GoToBall(state, bot_id, slope=None):
sParams = skills_union.SParam()
ballPos = Vector2D(int(state.ballPos.x), int(state.ballPos.y))
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballVel = Vector2D(int(state.ballVel.x), int(state.ballVel.y))
distance = botPos.dist(ballPos)
sParams.GoToPointP.x = ballPos.x
sParams.GoToPointP.y = ballPos.y
if slope:
sParams.GoToBallP.align = True
sParams.GoToBallP.finalslope = slope
else:
sParams.GoToBallP.align = False
sGoToBall.execute(sParams, state, bot_id, pub)
def findPath(self,startPoint,end,avoid_ball=False):
print("Bot id: {}, calculating path".format(self.kubid))
# global FLAG_PATH_RECEIVED, self.REPLAN
# self.FLAG_PATH_RECEIVED = 1
self.REPLAN = 1
startPt = point_2d()
self.target = point_2d()
startPt.x = startPoint.x
startPt.y = startPoint.y
self.target.x = end.x
self.target.y = end.y
# print("Start Point ",startPt.x,startPt.y)
# print("self.Target Point",self.target.x,self.target.y)
# print("Waiting for service")
rospy.wait_for_service('planner')
planner = rospy.ServiceProxy('planner', path_plan)
message = planner(self.kubid,startPt,self.target,avoid_ball)
path = []
for i in xrange(len(message.path)):
path = path + [Vector2D(int(message.path[i].x),int(message.path[i].y))]
# start = rospy.Time.now()
# start = 1.0*start.secs + 1.0*start.nsecs/pow(10,9)
self.start_time = rospy.Time.now()
self.start_time = 1.0*self.start_time.secs + 1.0*self.start_time.nsecs/pow(10,9)
os.environ['bot'+str(self.kubid)]=str(self.start_time)
self.v = Velocity(path,self.start_time,startPt)
self.v.updateAngle()
self.expectedTraverseTime = self.v.getTime(self.v.GetPathLength())
time_cal = self.expectedTraverseTime
# self.pso = self.PSO(5,20,1000,1,1,0.5)
self.errorInfo = Error()
def intercept_ratio(self, state, reciever_id):
rivals = [x for x in range(6)]
min_ratio = 9999999
index = -1
botPos = Vector2D(state.homePos[self.bot_id].x,
state.homePos[self.bot_id].y)
for player in rivals:
distance = botPos.dist(
Vector2D(state.awayPos[player].x, state.awayPos[player].y))
dist = self.line(player, self.bot_id, reciever_id, state)
in_line_dist = (distance**2 - dist**2)**(0.5)
if in_line_dist * 1.0 / dist < min_ratio:
min_ratio = in_line_dist * 1.0 / dist
index = player
else:
continue
return min_ratio
def debug(param, state, bot_id):
botPos = Vector2D(state.homePos[bot_id].x, state.homePos[bot_id].y)
destination = Vector2D(param.GoToPointP.x, param.GoToPointP.y)
align = param.GoToPointP.align
finalSlope = param.GoToPointP.finalSlope
dist = botPos.dist(destination)
print '#' * 50
print 'In sGoToPoint'
print 'Current bot pos: {}, {}'.format(state.homePos[bot_id].x,
state.homePos[bot_id].y)
print 'Target pos: {}, {}'.format(destination.x, destination.y)
print 'Align: {}'.format(align)
if align:
print 'Finalslope: {}'.format(finalSlope)
print 'Bot Orientation {}'.format(state.homePos[bot_id].theta)
print 'Distance: {}'.format(dist)
print '#' * 50
def assign(self, state, botL, taskD):
Mat = {}
for x in botL:
Mat[x] = {}
botPos = Vector2D(state.homePos[x].x, state.homePos[x].y)
for y in taskD.keys():
Mat[x][y] = botPos.dist(taskD[y])
return Mat
def RDefender_callback(state):
global pub, RDefender_tac
# RDefender_id = 1
RDefender_id = 2
ballPos = Vector2D(state.ballPos.x, state.ballPos.y)
if RDefender_tac == None:
RDefender_tac = TRDefender.TRDefender(RDefender_id, state)
RDefender_tac.execute(state, pub)
def nearness_ratio(self, state, reciever_id):
rivals = [x for x in range(6)]
min_ratio = 9999999
index = -1
bot_Pos = Vector2D(state.homePos[reciever_id].x,
state.homePos[reciever_id].y)
passerPos = Vector2D(state.homePos[self.bot_id].x,
state.homePos[self.bot_id].y)
distance = bot_Pos.dist(passerPos)
for player in rivals:
dist = bot_Pos.dist(
Vector2D(state.awayPos[player].x, state.awayPos[player].y))
if distance * 1.0 / dist < min_ratio:
min_ratio = distance * 1.0 / dist
index = player
else:
continue
return min_ratio
def g(id_):
kub = kubs.kubs(id_, pub)
print(kub.kubs_id)
m = -1 if id_ % 2 == 0 else 1
g_fsm = GoToPoint.GoToPoint(kub, Vector2D(400 * m * id_ * 0,
800 * id_ * m), 1.57)
# g_fsm.as_graphviz()
# g_fsm.write_diagram_png()
g_fsm.spin()
def ratios(self, state, reciever_id):
rivals = [x for x in range(6)]
intercept_ratio = -1
nearness_ratio = -1
#index = -1
botPos = Vector2D(state.homePos[self.bot_id].x,
state.homePos[self.bot_id].y)
recieverPos = Vector2D(state.homePos[reciever_id].x,
state.homePos[reciever_id].y)
sep = botPos.dist(recieverPos)
print "#" * 50
print "PLAYER: ", reciever_id, " in_line_dist sep sep_from_rival perp_dist in_line_dist2"
for player in rivals:
sep_from_rival = recieverPos.dist(
Vector2D(state.awayPos[player].x, state.awayPos[player].y))
distance = botPos.dist(
Vector2D(state.awayPos[player].x, state.awayPos[player].y))
distance2 = recieverPos.dist(
Vector2D(state.awayPos[player].x, state.awayPos[player].y))
dist = self.line(player, self.bot_id, reciever_id, state)
in_line_dist = (distance**2 - dist**2)**(0.5)
in_line_dist2 = (distance2**2 - dist**2)**(0.5)
print "DETAILS--> ", player, in_line_dist, sep, sep_from_rival, dist, in_line_dist2
if in_line_dist * 1.0 / dist > intercept_ratio and in_line_dist < sep and in_line_dist2 < sep:
intercept_ratio = in_line_dist * 1.0 / dist
#index = player
if sep * 1.0 / sep_from_rival > nearness_ratio:
nearness_ratio = sep * 1.0 / sep_from_rival
if nearness_ratio > 400.0 / 27:
nearness_ratio = 400.0 / 27
# if self.if_intercept_poss( self, state, rival, reciever_id, in_line_dist, perp_dist, sep, in_line_dist2) == 0:
# intercept_ratio = 0
# return intercept_ratio, nearness_ratio
if intercept_ratio * intercept_ratio < RATIO * RATIO - 1:
intercept_ratio = 0
return intercept_ratio, nearness_ratio
if intercept_ratio == -1:
print "GOTCHA...."
intercept_ratio = 0
return intercept_ratio, nearness_ratio
if intercept_ratio > 400.0 / 27:
intercept_ratio = 400.0 / 27
return intercept_ratio, nearness_ratio
def evr_params(self, state, reciever_id, bp, rp, sp):
global RATIO
ourteam = [x for x in xrange(6)]
botPos = bp
recieverPos = rp
sep = sp
nearness_ratio = -1
intercept_ratio = -1
count1 = 0
count2 = 0
print "#" * 50
print "PLAYER: ", reciever_id, " in_line_dist sep sep_from_rival perp_dist in_line_dist2"
for player in ourteam:
distance = botPos.dist(
Vector2D(state.homePos[player].x, state.homePos[player].y))
distance2 = recieverPos.dist(
Vector2D(state.homePos[player].x, state.homePos[player].y))
if distance2 > HALF_FIELD_MAXX * 0.5 and distance > HALF_FIELD_MAXX * 0.75:
continue
dist = self.line(player, self.bot_id, reciever_id, state)
in_line_dist = (distance**2 - dist**2)**(0.5)
in_line_dist2 = (distance2**2 - dist**2)**(0.5)
if in_line_dist > sep or in_line_dist2 > sep:
if nearness_ratio < sep * 1.0 / distance2:
nearness_ratio = sep * 1.0 / distance2
if sep * 1.0 / distance2 > RATIO * 0.75:
count1 = count1 + 1
else:
if intercept_ratio < in_line_dist * 1.0 / dist:
intercept_ratio = in_line_dist * 1.0 / dist
# if self.prod(in_line_dist, dist, sep) < 0:
# count2 = count2 + 1
print "DETAILS--> ", player, in_line_dist, sep, distance2, dist, in_line_dist2
if intercept_ratio == -1 and nearness_ratio == -1:
return 0.1, 0
if intercept_ratio > 400.0 / 27:
intercept_ratio = 400.0 / 27
if nearness_ratio > 400.0 / 27:
nearness_ratio = 400.0 / 27
if intercept_ratio > nearness_ratio:
return intercept_ratio, count1 + count2
else:
return nearness_ratio, count2 + count1
def shouldReplan(self):
# print("velocity = ",self.v.velocity)
if self.v.velocity < 10:
return False
myPos = Vector2D(int(self.homePos[self.kubid].x),int(self.homePos[self.kubid].y))
obsPos = Vector2D()
index = self.v.GetExpectedPositionIndex()
for i in xrange(len(self.homePos)):
if i == self.kubid:
pass
else:
obsPos.x = int(self.homePos[i].x)
obsPos.y = int(self.homePos[i].y)
if self.v.ellipse(myPos,obsPos,self.v.motionAngle[index]):
return True
for i in xrange(len(self.awayPos)):
obsPos.x = int(self.awayPos[i].x)
obsPos.y = int(self.awayPos[i].y)
if self.v.ellipse(myPos,obsPos,self.v.motionAngle[index]):
return True
return False
def shouldReplan():
global homePos, awayPos, kubid
if v.velocity < 10:
return False
myPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y))
obsPos = Vector2D()
index = v.GetExpectedPositionIndex()
for i in xrange(len(homePos)):
if i == kubid:
pass
else:
obsPos.x = int(homePos[i].x)
obsPos.y = int(homePos[i].y)
if v.ellipse(myPos, obsPos, v.motionAngle[index]):
return True
for i in xrange(len(awayPos)):
obsPos.x = int(awayPos[i].x)
obsPos.y = int(awayPos[i].y)
if v.ellipse(myPos, obsPos, v.motionAngle[index]):
return True
return False
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()
def execute(param, state, bot_id, gv, pub, dribller=False):
# debug(param,state, bot_id)
pointPos = Vector2D()
pointPos.x = int(param.GoToPointP.x)
pointPos.y = int(param.GoToPointP.y)
t = rospy.Time.now()
t = t.secs + 1.0 * t.nsecs / pow(10, 9)
[vx, vy, vw, REPLANNED,
maxDisToTurn] = gv[bot_id].execute(t, bot_id, pointPos, state.homePos,
state.awayPos) #vx, vy, vw, replanned
if (REPLANNED):
print("REPLANNED {}".format(bot_id))
reset(bot_id)
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
v = Vector2D()
distan = botPos.dist(pointPos)
ballPos = Vector2D(state.ballPos)
# maxDisToTurn = distan
if param.GoToPointP.align == True:
angleToTurn = pointPos.normalizeAngle(param.GoToPointP.finalSlope -
state.homePos[bot_id].theta)
else:
angleToTurn = pointPos.normalizeAngle(
botPos.angle(ballPos) - state.homePos[bot_id].theta)
# angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta))
minReachTime = maxDisToTurn / MAX_BOT_OMEGA
maxReachTime = maxDisToTurn / MIN_BOT_OMEGA
minTurnTime = angleToTurn / MAX_BOT_OMEGA
maxTurnTime = angleToTurn / MIN_BOT_OMEGA
omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi)
if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
if omega < 0:
omega = -MIN_BOT_OMEGA
else:
omega = MIN_BOT_OMEGA
if angleToTurn == 0:
omega = 0
print("bot id{} vx: {}, vy:{}".format(bot_id, vx, vy))
if param.GoToPointP.align == False:
if distan < DRIBBLER_BALL_THRESH:
if distan < BOT_BALL_THRESH:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
omega, 0, dribller)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, vx,
vy, omega, 0, dribller)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy,
omega, 0, dribller)
else:
if distan > BOT_BALL_THRESH:
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy,
omega, 0, dribller)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
omega, 0, dribller)
"""
This is a test file for optimalAssignment
"""
from utils.geometry import Vector2D
from optimalAssignment import *
botL = [1,2,3,4,5]
taskD = { 't1': Vector2D(500,500), 't2': Vector2D(1000,1000), 't3': Vector2D(2000,2000) }
obj = optimalAssignment(state, botL, taskD)
print obj.decider(botL, taskD)
def execute(param, state, bot_id, pub, dribller=False):
pointPos = Vector2D()
pointPos.x = int(state.ballPos.x)
pointPos.y = int(state.ballPos.y)
botPos = Vector2D(int(state.homePos[bot_id].x),
int(state.homePos[bot_id].y))
ballPos = Vector2D(state.ballPos)
oppGoal = Vector2D(HALF_FIELD_MAXX, 0)
v = Vector2D()
targetPoint = Vector2D()
# maxDisToTurn = distan
if param.GoToBallP.align:
angle = param.GoToBallP.finalslope
angleToTurn = v.normalizeAngle(param.GoToBallP.finalslope -
state.homePos[bot_id].theta)
else:
angle = ballPos.angle(oppGoal)
angleToTurn = v.normalizeAngle(
ballPos.angle(oppGoal) - state.homePos[bot_id].theta)
# angleToTurn = v.normalizeAngle((param.GoToPointP.finalSlope)-(state.homePos[bot_id].theta))
targetPoint.x = ballPos.x - 2 * DRIBBLER_BALL_THRESH * cos(angle)
targetPoint.y = ballPos.y - 2 * DRIBBLER_BALL_THRESH * sin(angle)
distan = botPos.dist(targetPoint)
t = rospy.Time.now()
t = t.secs + 1.0 * t.nsecs / pow(10, 9)
start_time = float(os.environ.get('bot' + str(bot_id)))
if distan <= DRIBBLER_BALL_THRESH:
# print("in DRIBBLER_BALL_THRESH")
[vx, vy, vw, REPLANNED,
maxDisToTurn] = Get_Vel(start_time,
t,
bot_id,
state.ballPos,
state.homePos,
state.awayPos,
avoid_ball=False) #vx, vy, vw, replanned
omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi)
if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
if omega < 0:
omega = -MIN_BOT_OMEGA
else:
omega = MIN_BOT_OMEGA
if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP:
omega = 0
if distan < BOT_BALL_THRESH:
skill_node.send_command(pub, state.isteamyellow, bot_id, 0, 0,
omega, 0, dribller)
else:
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy,
omega, 0, dribller)
else:
[vx, vy, vw, REPLANNED,
maxDisToTurn] = Get_Vel(start_time,
t,
bot_id,
targetPoint,
state.homePos,
state.awayPos,
avoid_ball=True) #vx, vy, vw, replanned
omega = 2 * angleToTurn * MAX_BOT_OMEGA / (2 * math.pi)
if omega < MIN_BOT_OMEGA and omega > -MIN_BOT_OMEGA:
if omega < 0:
omega = -MIN_BOT_OMEGA
else:
omega = MIN_BOT_OMEGA
if fabs(angleToTurn) < SATISFIABLE_THETA_SHARP:
omega = 0
skill_node.send_command(pub, state.isteamyellow, bot_id, vx, vy, omega,
0, dribller)
if (REPLANNED):
reset(bot_id)
def execute(self, t, kub_id, target, homePos_, awayPos_,avoid_ball=False):
# Return vx,vy,vw,self.replan,remainingDistance
self.target = target
self.REPLAN = 0
self.homePos = homePos_
self.awayPos = awayPos_
self.kubid = kub_id
#self.FIRST_CALL = int(os.environ.get('fc'+str(self.kubid)))
print(self.FIRST_CALL)
# if not self.prev_target==None:
if isinstance(self.prev_target, Vector2D):
dist = distance_(self.target, self.prev_target)
if(dist>DESTINATION_THRESH):
self.REPLAN = 1
self.prev_target = self.target
# print("in getVelocity, self.FIRST_CALL = ",self.FIRST_CALL)
curPos = Vector2D(int(self.homePos[self.kubid].x),int(self.homePos[self.kubid].y))
distance = sqrt(pow(self.target.x - self.homePos[self.kubid].x,2) + pow(self.target.y - self.homePos[self.kubid].y,2))
if(self.FIRST_CALL):
print("BOT id:{}, in first call, timeIntoLap: {}".format(self.kubid, t-self.start_time))
startPt = point_2d()
startPt.x = self.homePos[self.kubid].x
startPt.y = self.homePos[self.kubid].y
self.findPath(startPt, self.target, avoid_ball)
#os.environ['fc'+str(self.kubid)]='0'
self.FIRST_CALL = 0
else:
print("Bot id:{}, not first call, timeIntoLap: {}".format(self.kubid,t-self.start_time))
if distance < 1.5*BOT_BALL_THRESH:
return [0,0,0,0,0]
# print("ex = ",self.expectedTraverseTime)
# print("t = ",t," start = ",start)
remainingDistance = 0
# print("ex = ",self.expectedTraverseTime)
# print("t = ",t," start = ",start)
if (t-self.start_time< self.expectedTraverseTime):
if self.v.trapezoid(t-self.start_time,curPos):
index = self.v.GetExpectedPositionIndex()
if index == -1:
vX,vY,eX,eY = self.v.sendVelocity(self.v.getVelocity(),self.v.motionAngle[index],index)
vX,vY = 0,0
else:
remainingDistance = self.v.GetPathLength(startIndex=index)
vX,vY,eX,eY = self.v.sendVelocity(self.v.getVelocity(),self.v.motionAngle[index],index)
else:
# print(t-self.start_time, self.expectedTraverseTime)
if self.expectedTraverseTime == 'self.REPLAN':
self.REPLAN = 1
# print("Motion Not Possible")
vX,vY,eX,eY = 0,0,0,0
flag = 1
else:
# print("TimeOUT, self.REPLANNING")
vX,vY,eX,eY = 0,0,0,0
self.errorInfo.errorIX = 0.0
self.errorInfo.errorIY = 0.0
self.errorInfo.lastErrorX = 0.0
self.errorInfo.lastErrorY = 0.0
startPt = point_2d()
startPt.x = self.homePos[self.kubid].x
startPt.y = self.homePos[self.kubid].y
self.findPath(startPt,self.target, avoid_ball)
errorMag = sqrt(pow(eX,2) + pow(eY,2))
should_replan = self.shouldReplan()
if(should_replan == True):
self.v.velocity = 0
# print("self.v.velocity now = ",self.v.velocity)
# print("entering if, should_replan = ", should_replan)
if should_replan or \
(errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) or \
self.REPLAN == 1:
# print("______________here____________________")
# print("condition 1",should_replan)
# print("error magnitude", errorMag)
# print("distance threshold",distance > 1.5* BOT_BALL_THRESH)
# print("condition 2",errorMag > 350 and distance > 1.5* BOT_BALL_THRESH)
# print("condition 3",self.REPLAN)
# print("Should self.Replan",should_replan)
# print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH)
self.REPLAN = 1
startPt = point_2d()
startPt.x = self.homePos[self.kubid].x
startPt.y = self.homePos[self.kubid].y
self.findPath(startPt,self.target, avoid_ball)
return [0,0,0, self.REPLAN,0]
else:
self.errorInfo.errorX = eX
self.errorInfo.errorY = eY
vX,vY = pid(vX,vY,self.errorInfo,self.pso)
botAngle = self.homePos[self.kubid].theta
vXBot = vX*cos(botAngle) + vY*sin(botAngle)
vYBot = -vX*sin(botAngle) + vY*cos(botAngle)
return [vXBot, vYBot, 0, self.REPLAN,remainingDistance]
return [vXBot, vYBot, 0, self.REPLAN]
def Get_Vel(start, t, kub_id, target, homePos_, awayPos_, avoid_ball=False):
global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid
REPLAN = 0
homePos = homePos_
awayPos = awayPos_
kubid = kub_id
curPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y))
distance = sqrt(
pow(target.x - homePos[kubid].x, 2) +
pow(target.y - homePos[kubid].y, 2))
if (FIRST_CALL):
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
FIRST_CALL = 0
if distance < 1.5 * BOT_BALL_THRESH:
return [0, 0, 0, 0]
# print("ex = ",expectedTraverseTime)
# print("t = ",t," start = ",start)
# print("t - start = ",t-start)
if (t - start < expectedTraverseTime):
if v.trapezoid(t - start, curPos):
index = v.GetExpectedPositionIndex()
if index == -1:
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
vX, vY = 0, 0
else:
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
else:
# print(t-start, expectedTraverseTime)
if expectedTraverseTime == 'REPLAN':
REPLAN = 1
# print("Motion Not Possible")
vX, vY, eX, eY = 0, 0, 0, 0
flag = 1
else:
# print("TimeOUT, REPLANNING")
vX, vY, eX, eY = 0, 0, 0, 0
errorInfo.errorIX = 0.0
errorInfo.errorIY = 0.0
errorInfo.lastErrorX = 0.0
errorInfo.lastErrorY = 0.0
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
errorMag = sqrt(pow(eX, 2) + pow(eY, 2))
if shouldReplan() or \
(errorMag > 350 and distance > 2* BOT_BALL_THRESH) or \
REPLAN == 1:
# print("Should Replan",shouldReplan())
# print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH)
REPLAN = 1
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
return [0, 0, 0, REPLAN]
else:
errorInfo.errorX = eX
errorInfo.errorY = eY
vX, vY = pid(vX, vY, errorInfo, pso)
botAngle = homePos[kubid].theta
vXBot = vX * cos(botAngle) + vY * sin(botAngle)
vYBot = -vX * sin(botAngle) + vY * cos(botAngle)
return [vXBot, vYBot, 0, REPLAN]
def Get_Vel(start, t, kub_id, target, homePos_, awayPos_, avoid_ball=False):
# Return vx,vy,vw,replan,remainingDistance
global expectedTraverseTime, REPLAN, v, errorInfo, pso, FIRST_CALL, homePos, awayPos, kubid, prev_target
REPLAN = 0
homePos = homePos_
awayPos = awayPos_
kubid = kub_id
# if not prev_target==None:
if isinstance(prev_target, Vector2D):
dist = distance_(target, prev_target)
if (dist > DESTINATION_THRESH):
REPLAN = 1
prev_target = target
# print("in getVelocity, FIRST_CALL = ",FIRST_CALL)
curPos = Vector2D(int(homePos[kubid].x), int(homePos[kubid].y))
distance = sqrt(
pow(target.x - homePos[kubid].x, 2) +
pow(target.y - homePos[kubid].y, 2))
if (FIRST_CALL):
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
FIRST_CALL = 0
if distance < 1.5 * BOT_BALL_THRESH:
return [0, 0, 0, 0, 0]
# print("ex = ",expectedTraverseTime)
# print("t = ",t," start = ",start)
remainingDistance = 0
# print("ex = ",expectedTraverseTime)
# print("t = ",t," start = ",start)
if (t - start < expectedTraverseTime):
if v.trapezoid(t - start, curPos):
index = v.GetExpectedPositionIndex()
if index == -1:
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
vX, vY = 0, 0
else:
remainingDistance = v.GetPathLength(startIndex=index)
vX, vY, eX, eY = v.sendVelocity(v.getVelocity(),
v.motionAngle[index], index)
else:
# print(t-start, expectedTraverseTime)
if expectedTraverseTime == 'REPLAN':
REPLAN = 1
# print("Motion Not Possible")
vX, vY, eX, eY = 0, 0, 0, 0
flag = 1
else:
# print("TimeOUT, REPLANNING")
vX, vY, eX, eY = 0, 0, 0, 0
errorInfo.errorIX = 0.0
errorInfo.errorIY = 0.0
errorInfo.lastErrorX = 0.0
errorInfo.lastErrorY = 0.0
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
errorMag = sqrt(pow(eX, 2) + pow(eY, 2))
should_replan = shouldReplan()
if (should_replan == True):
v.velocity = 0
# print("v.velocity now = ",v.velocity)
# print("entering if, should_replan = ", should_replan)
if should_replan or \
(errorMag > 350 and distance > 1.5* BOT_BALL_THRESH) or \
REPLAN == 1:
# print("______________here____________________")
# print("condition 1",should_replan)
# print("error magnitude", errorMag)
# print("distance threshold",distance > 1.5* BOT_BALL_THRESH)
# print("condition 2",errorMag > 350 and distance > 1.5* BOT_BALL_THRESH)
# print("condition 3",REPLAN)
# print("Should Replan",should_replan)
# print("ErrorMag",errorMag > 350 and distance > 2*BOT_BALL_THRESH)
REPLAN = 1
startPt = point_2d()
startPt.x = homePos[kubid].x
startPt.y = homePos[kubid].y
findPath(startPt, target, avoid_ball)
return [0, 0, 0, REPLAN, 0]
else:
errorInfo.errorX = eX
errorInfo.errorY = eY
vX, vY = pid(vX, vY, errorInfo, pso)
botAngle = homePos[kubid].theta
vXBot = vX * cos(botAngle) + vY * sin(botAngle)
vYBot = -vX * sin(botAngle) + vY * cos(botAngle)
return [vXBot, vYBot, 0, REPLAN, remainingDistance]
return [vXBot, vYBot, 0, REPLAN]
def execute(startTime, DIST_THRESH, avoid_ball=False, factor=0.7):
global kub, start_time, getState, BALLPOS, APPROACH_POINT, GOAL_POINT, rotate, FLAG_dribble, FLAG_turn, FLAG_move, FIRST_CALL, prev_state, vx_end, vy_end
if FIRST_CALL:
print("First Call of _Dribble_ ---")
start_time = start_time
FIRST_CALL = False
vx_end, vy_end = 0, 0
#finding APPROACH_POINT:
phi = kub.state.homePos[kub.kubs_id].theta
APPROACH_POINT = getPointBehindTheBall(BALLPOS, phi, -4)
if not FLAG_dribble:
while not (FLAG_move):
try:
kub.state = getState(prev_state).stateB
except:
print("Error", e)
if not (prev_state == kub.state):
prev_state = kub.state
t = rospy.Time.now()
t = t.secs + 1.0 * t.nsecs / pow(10, 9)
[vx, vy, vw, REPLANNED
] = velocity.run.Get_Vel(start_time, t, kub.kubs_id,
APPROACH_POINT, kub.state.homePos,
kub.state.awayPos, avoid_ball)
velocity_magnitude = Vector2D(vx, vy).abs(Vector2D(vx, vy))
if velocity_magnitude > MAX_BOT_SPEED:
angle_movement = math.atan2(vy, vx)
# print("_____________Velocity Changed____________")
vy = MAX_BOT_SPEED * math.sin(angle_movement)
vx = MAX_BOT_SPEED * math.cos(angle_movement)
if (REPLANNED):
reset()
if not vx and not vy:
vx, vy = vx_end, vy_end
else:
vx_end, vy_end = vx, vy
if dist(kub.state.homePos[kub.kubs_id],
APPROACH_POINT) < BOT_RADIUS * factor:
kub.move(0, 0)
# print("Distance completed"*200)
FLAG_move = True
FLAG_dribble = True
else:
# print("Sending velocity",vx,vy)
kub.move(vx, vy)
kub.execute()
yield kub, APPROACH_POINT
else:
kub.dribbler(FLAG_dribble)
kub.execute()
FLAG_move = False
FLAG_turn = False
while not (FLAG_move and FLAG_turn):
try:
kub.state = getState(prev_state).stateB
except:
print("Error", e)
if not (prev_state == kub.state):
prev_state = kub.state
t = rospy.Time.now()
t = t.secs + 1.0 * t.nsecs / pow(10, 9)
[vx, vy, vw,
REPLANNED] = velocity.run.Get_Vel(start_time, t, kub.kubs_id,
GOAL_POINT,
kub.state.homePos,
kub.state.awayPos, False)
velocity_magnitude = Vector2D(vx, vy).abs(Vector2D(vx, vy))
if velocity_magnitude > MAX_BOT_SPEED:
angle_movement = math.atan2(vy, vx)
# print("_____________Velocity Changed____________")
vy = MAX_BOT_SPEED * math.sin(angle_movement)
vx = MAX_BOT_SPEED * math.cos(angle_movement)
vw = Get_Omega(kub.kubs_id, rotate, kub.state.homePos)
if not vw:
vw = 0
if (REPLANNED):
reset()
if not vx and not vy:
vx, vy = vx_end, vy_end
else:
vx_end, vy_end = vx, vy
if abs(
normalize_angle(kub.state.homePos[kub.kubs_id].theta -
rotate)) < ROTATION_FACTOR:
kub.turn(0)
# print("Angle completed")
FLAG_turn = True
else:
kub.turn(vw)
if dist(kub.state.homePos[kub.kubs_id],
GOAL_POINT) < BOT_RADIUS * factor:
kub.move(0, 0)
# print("Distance completed"*200)
FLAG_move = True
else:
# print("Sending velocity",vx,vy)
kub.move(vx, vy)
kub.execute()
yield kub, GOAL_POINT
kub.dribble(FLAG_dribble)
kub.execute()
yield kub, GOAL_POINT
from kubs import kubs, cmd_node
from pid.run import *
from pid.run_w import *
import rospy,sys
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 utils.geometry import Vector2D
from utils.config import *
from utils.math_functions import *
kub = None
start_time = None
start_time_w = None
GOAL_POINT = Vector2D(-3000,-2000)
point_to = Vector2D(0,0)
BScall = None
theta = None
FLAG_move = True
FLAG_turn = True
totalAngle = 0
init_angle = 0
FIRST_CALL = True
def init(_kub,_point_to):
global kub,point_to
kub = _kub
start_time = None
point_to = _point_to
# g_fsm.as_graphviz()
# g_fsm.write_diagram_png()
g_fsm.spin()
# print
# print kub.state.homePos[kub.kubs_id].theta,t
# g(0)
# for i in xrange(0):
# p = Process(target=g,args=(i,))
# p.start()
#g_fsm1 = GoToBall.GoToBall(kub,deg_2_radian(45))
kub1 = kubs.kubs(0, pub)
# print kub1.state.ballPos.y,kub1.state.ballPos.x
# while True:
# pass
g1_fsm = GoToBall.GoToBall(kub1, Vector2D(0, 3000))
g1_fsm.as_graphviz()
g1_fsm.write_diagram_png()
# g_fsm1.spin()
g1_fsm.spin()
#t = threading.Thread(target=g_fsm.spin())
#t1 = threading.Thread(target=g1_fsm.spin())
#t.start()
#t1.start()*
