Fawkes API  Fawkes Development Version
spl.cpp
1 
2 /***************************************************************************
3  * spl.cpp - Fawkes SPL refbox repeater
4  *
5  * Created: Tue Jul 08 13:50:06 2008
6  * Copyright 2008 Tim Niemueller [www.niemueller.de]
7  * 2009 Tobias Kellner
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #include "spl.h"
25 #include "state_handler.h"
26 #include <core/exception.h>
27 #include <netcomm/socket/datagram.h>
28 #include <logging/logger.h>
29 
30 #ifdef USE_SPL_GC6
31 # include <interfaces/SoccerPenaltyInterface.h>
32 #endif
33 
34 #include <cstring>
35 #include <cstdio>
36 #include <unistd.h>
37 #include <cerrno>
38 // it it was defined, Exception::errno() could not be called...
39 #ifdef errno
40 # undef errno
41 #endif
42 using namespace fawkes;
43 
44 #ifdef USE_SPL_GC6
45 static const uint32_t SPL_STRUCT_VERSION = 6;
46 #else
47 static const uint32_t SPL_STRUCT_VERSION = 7;
48 #endif
49 
50 static const uint8_t SPL_STATE_INITIAL = 0;
51 static const uint8_t SPL_STATE_READY = 1;
52 static const uint8_t SPL_STATE_SET = 2;
53 static const uint8_t SPL_STATE_PLAYING = 3;
54 static const uint8_t SPL_STATE_FINISHED = 4;
55 
56 /*
57 static const uint8_t SPL_STATE2_NORMAL = 0;
58 static const uint8_t SPL_STATE2_PENALTYSHOOT = 1;
59 */
60 
61 static const uint8_t SPL_PENALTY_NONE = 0;
62 #ifdef USE_SPL_GC6
63 static const uint8_t SPL_PENALTY_BALL_HOLDING = 1;
64 static const uint8_t SPL_PENALTY_GOALIE_PUSHING = 2;
65 static const uint8_t SPL_PENALTY_PLAYER_PUSHING = 3;
66 static const uint8_t SPL_PENALTY_ILLEGAL_DEFENDER = 4;
67 static const uint8_t SPL_PENALTY_ILLEGAL_DEFENSE = 5;
68 static const uint8_t SPL_PENALTY_OBSTRUCTION = 6;
69 static const uint8_t SPL_PENALTY_REQ_FOR_PICKUP = 7;
70 static const uint8_t SPL_PENALTY_LEAVING = 8;
71 static const uint8_t SPL_PENALTY_DAMAGE = 9;
72 static const uint8_t SPL_PENALTY_MANUAL = 10;
73 #else
74 /*
75 static const uint8_t SPL_PENALTY_BALL_HOLDING = 1;
76 static const uint8_t SPL_PENALTY_PLAYER_PUSHING = 2;
77 static const uint8_t SPL_PENALTY_OBSTRUCTION = 3;
78 static const uint8_t SPL_PENALTY_INACTIVE_PLAYER = 4;
79 static const uint8_t SPL_PENALTY_ILLEGAL_DEFENDER = 5;
80 static const uint8_t SPL_PENALTY_LEAVING_THE_FIELD = 6;
81 static const uint8_t SPL_PENALTY_PLAYING_WITH_HANDS = 7;
82 static const uint8_t SPL_PENALTY_REQ_FOR_PICKUP = 8;
83 static const uint8_t SPL_PENALTY_MANUAL = 15;
84 */
85 #endif
86 
87 // team numbers
88 static const uint8_t SPL_TEAM_BLUE = 0;
89 static const uint8_t SPL_TEAM_RED = 1;
90 
91 /*
92 static const uint8_t SPL_GOAL_BLUE = 0;
93 static const uint8_t SPL_GOAL_YELLOW = 1;
94 */
95 
96 static const char SPL_GAMECONTROL_HEADER[SPL_HEADER_SIZE] = {'R','G','m','e'};
97 
98 
99 /** @class SplRefBoxProcessor "processor/spl.h"
100  * SPL league refbox repeater.
101  * This class will listen to SPL refbox commands and derive matching
102  * game states from the communication stream and send this via the world info.
103  * @author Tim Niemueller
104  */
105 
106 /** Constructor.
107  * @param logger Logger
108  * @param broadcast_port Broadcast port
109  * @param team_number our team number
110  * @param player_number individual player number
111  */
113  unsigned short int broadcast_port,
114  unsigned int team_number,
115  unsigned int player_number)
116 {
117  __player_number = player_number;
118  __team_number = team_number;
119  __logger = logger;
120  __quit = false;
121  __s = new DatagramSocket(Socket::IPv4, 0.0000000001);
122  __s->bind(broadcast_port);
123 
124  __penalty = SPL_PENALTY_NONE;
125 }
126 
127 
128 /** Destructor. */
130 {
131  __s->close();
132  delete __s;
133 }
134 
135 
136 /** Process received struct. */
137 void
138 SplRefBoxProcessor::process_struct(spl_gamecontrol_t *msg)
139 {
141  //fawkes::worldinfo_gamestate_goalcolor_t our_goal;
142 
143  int team_index;
144  if (msg->teams[0].team_number == __team_number) team_index = 0;
145  else if (msg->teams[1].team_number == __team_number) team_index = 1;
146  else return; //Message doesn't concern us
147 
148  switch (msg->teams[team_index].team_color) {
149  case SPL_TEAM_BLUE:
150  our_team = TEAM_CYAN;
151  break;
152  case SPL_TEAM_RED:
153  our_team = TEAM_MAGENTA;
154  break;
155  default:
156  printf("Ignoring faulty packet\n");
157  return;
158  }
159 
160  _rsh->set_score(msg->teams[team_index].score, msg->teams[(team_index == 1 ? 0 : 1)].score);
161  _rsh->set_team_goal(our_team, (our_team == TEAM_CYAN ? GOAL_BLUE : GOAL_YELLOW)); //blue team defends blue goal
162 
163  for (unsigned int pl_num = 0; pl_num < SPL_MAX_NUM_PLAYERS; ++pl_num)
164  {
165  if ((pl_num + 1) == __player_number)
166  {
167  if ((msg->teams[team_index].players[pl_num].penalty != __penalty) ||
168  (msg->teams[team_index].players[pl_num].penalty != PENALTY_NONE))
169  {
170  __penalty = msg->teams[team_index].players[pl_num].penalty;
171 
172 #ifdef USE_SPL_GC6
173  // convert GC6 codes to new GC7 codes, "closest match"
174  switch (__penalty) {
175  case SPL_PENALTY_BALL_HOLDING:
177  case SPL_PENALTY_GOALIE_PUSHING:
178  case SPL_PENALTY_PLAYER_PUSHING:
180  case SPL_PENALTY_ILLEGAL_DEFENDER:
181  case SPL_PENALTY_ILLEGAL_DEFENSE:
183  case SPL_PENALTY_OBSTRUCTION:
185  case SPL_PENALTY_REQ_FOR_PICKUP:
187  case SPL_PENALTY_LEAVING:
189  case SPL_PENALTY_DAMAGE:
190  case SPL_PENALTY_MANUAL:
192  default:
193  __penalty = SoccerPenaltyInterface::SPL_PENALTY_NONE; break;
194  }
195 #endif
196 
197  _rsh->add_penalty(__penalty,
198  msg->teams[team_index].players[pl_num].secs_till_unpenalized);
199  }
200  break;
201  }
202  }
203 
204  switch (msg->state) {
205  case SPL_STATE_INITIAL:
206  _rsh->set_gamestate(GS_SPL_INITIAL, TEAM_BOTH);
207  break;
208  case SPL_STATE_READY:
209  _rsh->set_gamestate(GS_SPL_READY, TEAM_BOTH);
210  break;
211  case SPL_STATE_SET:
212  _rsh->set_gamestate(GS_SPL_SET, TEAM_BOTH);
213  break;
214  case SPL_STATE_PLAYING:
215  _rsh->set_gamestate(GS_SPL_PLAY, TEAM_BOTH);
216  break;
217  case SPL_STATE_FINISHED:
218  _rsh->set_gamestate(GS_SPL_FINISHED, TEAM_BOTH);
219  break;
220  default:
221  _rsh->set_gamestate(GS_SPL_FINISHED, TEAM_BOTH);
222  break;
223  }
224 
225  _rsh->set_half((msg->first_half == 1) ? HALF_FIRST : HALF_SECOND,
226  msg->kick_off_team == team_index);
227 }
228 
229 
230 void
232 {
233  try {
234  spl_gamecontrol_t ctrlmsg;
235  size_t bytes_read = __s->recv((void *)&ctrlmsg, sizeof(ctrlmsg));
236  if ( bytes_read == sizeof(ctrlmsg) ) {
237  if ((strncmp(ctrlmsg.header, SPL_GAMECONTROL_HEADER, SPL_HEADER_SIZE) == 0) &&
238  (ctrlmsg.version == SPL_STRUCT_VERSION) ) {
239  process_struct(&ctrlmsg);
240  }
241  }
242  } catch (fawkes::Exception &e) {
243  if ( e.get_errno() != EAGAIN ) {
244  __logger->log_warn("SplRefBoxProcessor", "Receiving failed, exception follows");
245  __logger->log_warn("SplRefBoxProcessor", e);
246  } // else just no data available this time
247  }
248 }
249 
250 bool
252 {
253  return true;
254 }
255 
256 
257 /** Run.
258  * Reads messages from the network, processes them and calls the refbox state sender.
259  */
260 void
262 {
263  spl_gamecontrol_t ctrlmsg;
264  while ( ! __quit ) {
265  size_t bytes_read = __s->recv((void *)&ctrlmsg, sizeof(ctrlmsg));
266  if ( bytes_read == sizeof(ctrlmsg) ) {
267  if ( (strncmp(ctrlmsg.header, SPL_GAMECONTROL_HEADER, SPL_HEADER_SIZE) == 0) &&
268  (ctrlmsg.version == SPL_STRUCT_VERSION) ) {
269  process_struct(&ctrlmsg);
270  _rsh->handle_refbox_state();
271  } else {
272  printf("Received illegal package\n");
273  }
274  }
275  }
276 }
static const uint16_t SPL_PENALTY_ILLEGAL_DEFENDER
SPL_PENALTY_ILLEGAL_DEFENDER constant.
uint8_t first_half
1 = game in first half, 0 otherwise
Definition: spl.h:68
Blue goal.
Definition: enums.h:64
int get_errno()
Get errno.
Definition: exception.cpp:643
Fawkes library namespace.
Yellow goal.
Definition: enums.h:65
static const uint16_t SPL_PENALTY_NONE
SPL_PENALTY_NONE constant.
spl_robotinfo_t players[SPL_MAX_NUM_PLAYERS]
the team&#39;s players
Definition: spl.h:59
void run()
Run.
Definition: spl.cpp:261
Magenta team.
Definition: enums.h:57
Datagram socket.
Definition: datagram.h:31
First half.
Definition: enums.h:71
static const uint16_t SPL_PENALTY_LEAVING_THE_FIELD
SPL_PENALTY_LEAVING_THE_FIELD constant.
Base class for exceptions in Fawkes.
Definition: exception.h:36
static const uint16_t SPL_PENALTY_REQ_FOR_PICKUP
SPL_PENALTY_REQ_FOR_PICKUP constant.
Both teams.
Definition: enums.h:58
SplRefBoxProcessor(fawkes::Logger *logger, unsigned short int broadcast_port, unsigned int team_number, unsigned int player_number)
Constructor.
Definition: spl.cpp:112
uint8_t score
team&#39;s score
Definition: spl.h:57
static const uint16_t SPL_PENALTY_MANUAL
SPL_PENALTY_MANUAL constant.
Second half.
Definition: enums.h:72
SPL RefBox protocol game control struct.
Definition: spl.h:63
uint8_t state
state of the game (STATE_READY, STATE_PLAYING, etc.)
Definition: spl.h:67
static const uint16_t SPL_PENALTY_PLAYER_PUSHING
SPL_PENALTY_PLAYER_PUSHING constant.
Corner kick.
Definition: enums.h:49
Cyan team.
Definition: enums.h:56
uint8_t team_number
unique team number
Definition: spl.h:51
uint16_t penalty
penalty state of the player
Definition: spl.h:45
worldinfo_gamestate_team_t
Team.
Definition: enums.h:54
No penalty.
Definition: enums.h:77
~SplRefBoxProcessor()
Destructor.
Definition: spl.cpp:129
Wait for kick-off.
Definition: enums.h:47
uint8_t kick_off_team
the next team to kick off
Definition: spl.h:69
static const uint16_t SPL_PENALTY_BALL_HOLDING
SPL_PENALTY_BALL_HOLDING constant.
Move to kick-off positions.
Definition: enums.h:46
bool check_connection()
Check if the connection is alive and reconnect.
Definition: spl.cpp:251
static const uint16_t SPL_PENALTY_OBSTRUCTION
SPL_PENALTY_OBSTRUCTION constant.
Initial setup phase.
Definition: enums.h:45
void refbox_process()
Process incoming refbox communication.
Definition: spl.cpp:231
uint16_t secs_till_unpenalized
estimate of time till unpenalised
Definition: spl.h:46
uint8_t team_color
colour of the team
Definition: spl.h:52
spl_teaminfo_t teams[2]
Info about the teams.
Definition: spl.h:74
Interface for logging.
Definition: logger.h:34