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 goToPoint(self,kubs_id):
global BState,kubs_pub
if not BState:
kub = kubs.kubs(kubs_id,BState,kubs_pub)
g_fsm = GoToPoint.GoToPoint()
g_fsm.add_kub(kub)
g_fsm.add_point(point=BState.ballPos,orient=normalize_angle(pi+atan2(BState.ballPos.y,BState.ballPos.x-3000)))
g_fsm.spin()
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 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()
def function(id_, state):
global flag
kub = kubs.kubs(id_, state, pub)
# print(kub.kubs_id)
g_fsm = sample_tactic.SampleTactic()
# print(kub.kubs_id+1)
g_fsm.add_kub(kub)
# print(kub.kubs_id+2)
g_fsm.as_graphviz()
g_fsm.write_diagram_png()
g_fsm.spin_cb()
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()
def function(id_, state):
kub = kubs.kubs(id_, state, pub)
print(kub.kubs_id)
g_fsm = GoToBall.GoToBall()
# 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(pi +
atan2(state.ballPos.y, state.ballPos.y - 3000)))
g_fsm.as_graphviz()
g_fsm.write_diagram_png()
g_fsm.spin()
def goToBall(self):
global BState,kubs_pub
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
msg=point_SF()
kubs_id = int(self.textBotId.text())
# msg.bot_id = kubs_id
# pub.publish(msg)
print "in goToBall",BState
if BState:
print "belief_state Subscriber"
kub = kubs.kubs(kubs_id,BState,kubs_pub)
g_fsm = GoToBall.GoToBall()
g_fsm.add_kub(kub)
g_fsm.add_theta(theta=normalize_angle(pi+atan2(BState.ballPos.y,BState.ballPos.y-3000)))
g_fsm.spin()
if not (self.t1 == None):
self.t1.terminate()
self.t1 = multiprocessing.Process(target=g_fsm.spin)
self.t1.start()
def GUI_Callback(data): global BOT_ID, kub, BState, pub BOT_ID = data.bot_id print BOT_ID, "_____________________________" kub = kubs.kubs(BOT_ID, BState, pub)
REPLANNED = 0
homePos = None
awayPos = None
state = None
rospy.wait_for_service('bsServer',)
getState = rospy.ServiceProxy('bsServer',bsServer)
try:
state = getState(state)
except rospy.ServiceException, e:
print e
if state:
BState = state.stateB
kub = kubs.kubs(BOT_ID, BState, pub)
st = None
tnow = None
planned = False
ramp_upt = 5.0
ramp_dnt = 0.0
ramp_rampt = 0.0
case = -1
mvw=1.0
mvx=1.0
mvy=1.0
# 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()*
def GUI_Callback(data):
global BOT_ID, BState, kub
BOT_ID = data.bot_id
kub = kubs.kubs(BOT_ID, BState, pub)