-
Notifications
You must be signed in to change notification settings - Fork 6
/
robocup.py
149 lines (131 loc) · 5.74 KB
/
robocup.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
import sys, os
import rospy
from krssg_ssl_msgs.msg import BeliefState
from krssg_ssl_msgs.msg import Referee
from krssg_ssl_msgs.msg import gr_Commands
from plays.pSelect import pSelect
from velocity.getVel import GetVelocity
bs_msg = BeliefState()
def referee_callback(msg):
global bs_msg
print msg.command
bs_msg.ref_time_stamp = msg.packet_timestamp
bs_msg.ref_stage = msg.stage
bs_msg.ref_stage_time_left = msg.stage_time_left
bs_msg.ref_command = msg.command
bs_msg.ref_command_counter = msg.command_counter
bs_msg.ref_command_timestamp = msg.command_timestamp
if bs_msg.isteamyellow:
bs_msg.opp_name = msg.blue.name
bs_msg.opp_score = msg.blue.score
bs_msg.opp_redcards = msg.blue.red_cards
bs_msg.opp_yellow_card_times = msg.blue.yellow_card_times
bs_msg.opp_yellow_cards = msg.blue.yellow_cards
bs_msg.opp_timeouts = msg.blue.timeouts
bs_msg.opp_goalie = msg.blue.goalie
bs_msg.our_name = msg.yellow.name
bs_msg.our_score = msg.yellow.score
bs_msg.our_redcards = msg.yellow.red_cards
bs_msg.our_yellow_card_times = msg.yellow.yellow_card_times
bs_msg.our_yellow_cards = msg.yellow.yellow_cards
bs_msg.our_timeouts = msg.yellow.timeouts
bs_msg.our_goalie = msg.yellow.goalie
if msg.command==4:
bs_msg.our_kickoff = 1
if msg.command == 5:
bs_msg.opp_kickoff = 1
if msg.command == 6:
bs_msg.our_penalty = 1
if msg.command == 7:
bs_msg.opp_penalty = 1
if msg.command == 8:
bs_msg.our_direct_free_kick = 1
if msg.command == 9:
bs_msg.opp_direct_free_kick = 1
if msg.command == 10:
bs_msg.our_indirect_free_kick = 1
if msg.command == 11:
bs_msg.opp_indirect_free_kick = 1
else:
bs_msg.our_name = msg.blue.name
bs_msg.our_score = msg.blue.score
bs_msg.our_redcards = msg.blue.red_cards
bs_msg.our_yellow_card_times = msg.blue.yellow_card_times
bs_msg.our_yellow_cards = msg.blue.yellow_cards
bs_msg.our_timeouts = msg.blue.timeouts
bs_msg.our_goalie = msg.blue.goalie
bs_msg.opp_name = msg.yellow.name
bs_msg.opp_score = msg.yellow.score
bs_msg.opp_redcards = msg.yellow.red_cards
bs_msg.opp_yellow_card_times = msg.yellow.yellow_card_times
bs_msg.opp_yellow_cards = msg.yellow.yellow_cards
bs_msg.opp_timeouts = msg.yellow.timeouts
bs_msg.opp_goalie = msg.yellow.goalie
if msg.command==4:
bs_msg.opp_kickoff = 1
if msg.command == 5:
bs_msg.our_kickoff = 1
if msg.command == 6:
bs_msg.opp_penalty = 1
if msg.command == 7:
bs_msg.our_penalty = 1
if msg.command == 8:
bs_msg.opp_direct_free_kick = 1
if msg.command == 9:
bs_msg.our_direct_free_kick = 1
if msg.command == 10:
bs_msg.opp_indirect_free_kick = 1
if msg.command == 11:
bs_msg.our_indirect_free_kick = 1
bs_msg.blueTeamOnPositiveHalf = msg.blueTeamOnPositiveHalf
def bs_callback(msg):
# print "BS Callback"
global bs_msg
bs_msg.isteamyellow = msg.isteamyellow
bs_msg.frame_number = msg.frame_number
bs_msg.camera_id = msg.camera_id
bs_msg.t_capture = msg.t_capture
bs_msg.t_sent = msg.t_sent
bs_msg.ballPos = msg.ballPos
bs_msg.ballVel = msg.ballVel
bs_msg.awayPos = msg.awayPos
bs_msg.homePos = msg.homePos
bs_msg.awayVel = msg.awayVel
bs_msg.homeVel = msg.homeVel
bs_msg.ballDetected = msg.ballDetected
bs_msg.homeDetected = msg.homeDetected
bs_msg.awayDetected = msg.awayDetected
bs_msg.our_bot_closest_to_ball = msg.our_bot_closest_to_ball
bs_msg.opp_bot_closest_to_ball = msg.opp_bot_closest_to_ball
bs_msg.opp_bot_marking_our_attacker = msg.opp_bot_marking_our_attacker
bs_msg.ball_at_corners = msg.ball_at_corners
bs_msg.ball_in_our_half = msg.ball_in_our_half
bs_msg.ball_in_our_possession = msg.ball_in_our_possession
bs_msg.ball_in_our_dbox = msg.ball_in_our_dbox
playSelector = pSelect()
play = playSelector.selectPlay(bs_msg, pub)
if play is not None:
play.execute(gv)
# pass
gv = []
def main():
global pub, gv
print "Initializing the node "
# rospy.init_node('play_py_node',anonymous=False)
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
for i in xrange(6):
os.environ['bot'+str(i)]=str(start_time)
os.environ['fc'+str(i)]='1'
for i in xrange(6):
print os.environ.get('bot'+str(i))
for i in xrange(6):
start_time = float(os.environ.get('bot'+str(i)))
gv.append(GetVelocity(start_time = start_time,kubs_id = i))
pub = rospy.Publisher('/grsim_data', gr_Commands, queue_size=1000)
rospy.Subscriber("/ref_data", Referee, referee_callback, queue_size=1000)
rospy.Subscriber('/belief_state', BeliefState, bs_callback, queue_size=1000)
rospy.spin()
if __name__=='__main__':
rospy.init_node('play_py_node',anonymous=False)
main()