Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
comm_thread.cpp
1 
2 /***************************************************************************
3  * comm_thread.cpp - Fawkes RefBox Communication Thread
4  *
5  * Created: Sun Apr 19 13:13:43 2009 (on way to German Open 2009)
6  * Copyright 2009 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 "comm_thread.h"
25 #include "processor/remotebb.h"
26 #ifdef HAVE_MSL2010
27 # include "processor/msl2010.h"
28 #endif
29 #ifdef HAVE_SPL
30 # include "processor/spl.h"
31 #endif
32 
33 #include <interfaces/GameStateInterface.h>
34 #include <interfaces/SwitchInterface.h>
35 #ifdef HAVE_SPL
36 # include <interfaces/SoccerPenaltyInterface.h>
37 #endif
38 
39 #define CONFPREFIX "/plugins/refboxcomm"
40 
41 using namespace fawkes;
42 
43 /** @class RefBoxCommThread "comm_thread.h"
44  * Referee Box Communication Thread for robotic soccer.
45  * This thread communicates with the refbox.
46  * @author Tim Niemueller
47  */
48 
49 
50 /** Constructor. */
52  : Thread("RefBoxCommThread", Thread::OPMODE_WAITFORWAKEUP),
53  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
54 {
55  __refboxproc = NULL;
56 }
57 
58 
59 void
61 {
62  try {
63  __refboxproc = NULL;
64  __gamestate_if = NULL;
65  __beep_if = NULL;
66 #ifdef HAVE_SPL
67  __penalty_if = NULL;
68 #endif
69  __last_half = (worldinfo_gamestate_half_t)-1;
70  __last_score_cyan = 0xFFFFFFFF;
71  __last_score_magenta = 0xFFFFFFFF;
72  __last_gamestate = -1;
73  __our_team = TEAM_NONE;
74  __our_goal_color = GOAL_BLUE;
75  __kickoff = false;
76  __gamestate_modified = false;
77 
78  std::string processor = "";
79  try {
80  processor = config->get_string(CONFPREFIX"/processor");
81  } catch (Exception &e) {
82  // try to get league
83  std::string league = config->get_string("/general/league");
84  if (league == "MSL" || league == "SPL") {
85  processor = league;
86  }
87  }
88  if (processor == "") {
89  throw Exception("No valid processor defined");
90  }
91 
92  __cfg_beep_on_change = true;
93  __cfg_beep_frequency = 1000.;
94  __cfg_beep_duration = 0.5;
95  try {
96  __cfg_beep_on_change = config->get_bool(CONFPREFIX"/beep_on_change");
97  } catch (Exception &e) {} // ignored
98  try {
99  __cfg_beep_frequency = config->get_float(CONFPREFIX"/beep_frequency");
100  } catch (Exception &e) {} // ignored
101  try {
102  __cfg_beep_duration = config->get_float(CONFPREFIX"/beep_duration");
103  } catch (Exception &e) {} // ignored
104  if (__cfg_beep_on_change) {
105  __beep_if = blackboard->open_for_reading<SwitchInterface>("Beep");
106  }
107 
108  if ( processor == "MSL" ) {
109 #ifdef HAVE_MSL2010
110  std::string refbox_host = config->get_string(CONFPREFIX"/MSL/host");
111  unsigned int refbox_port = config->get_uint(CONFPREFIX"/MSL/port");
112  __refboxproc = new Msl2010RefBoxProcessor(logger,
113  refbox_host.c_str(), refbox_port);
114 #else
115  throw Exception("MSL2010 support not available at compile time");
116 #endif
117  } else if ( processor == "SPL" ) {
118 #ifdef HAVE_SPL
119  unsigned int refbox_port = config->get_uint(CONFPREFIX"/SPL/port");
120  __team_number = config->get_uint("/general/team_number");
121  __player_number = config->get_uint("/general/player_number");
122  __refboxproc = new SplRefBoxProcessor(logger, refbox_port,
123  __team_number, __player_number);
124 #else
125  throw Exception("SPL support not available at compile time");
126 #endif
127  } else if ( processor == "RemoteBB" ) {
128  std::string bb_host = config->get_string(CONFPREFIX"/RemoteBB/host");
129  unsigned int bb_port = config->get_uint(CONFPREFIX"/RemoteBB/port");
130  std::string iface_id = config->get_string(CONFPREFIX"/RemoteBB/interface_id");
131  __refboxproc = new RemoteBlackBoardRefBoxProcessor(logger,
132  bb_host.c_str(), bb_port,
133  iface_id.c_str());
134  } else {
135  throw Exception("Processor %s is not supported by refboxcomm plugin",
136  processor.c_str());
137  }
138  __refboxproc->set_handler(this);
139  __gamestate_if = blackboard->open_for_writing<GameStateInterface>("RefBoxComm");
140 #ifdef HAVE_SPL
141  __penalty_if = blackboard->open_for_writing<SoccerPenaltyInterface>("SPL Penalty");
142 #endif
143  } catch (Exception &e) {
144  finalize();
145  throw;
146  }
147 }
148 
149 
150 void
152 {
153  delete __refboxproc;
154  blackboard->close(__gamestate_if);
155  blackboard->close(__beep_if);
156 #ifdef HAVE_SPL
157  blackboard->close(__penalty_if);
158 #endif
159 }
160 
161 void
163 {
164  while (!__gamestate_if->msgq_empty()) {
167  msg = __gamestate_if->msgq_first<GameStateInterface::SetTeamColorMessage>();
168  __gamestate_if->set_our_team(msg->our_team());
169  __gamestate_modified = true;
170  } else if (__gamestate_if->msgq_first_is<GameStateInterface::SetStateTeamMessage>()) {
172  msg = __gamestate_if->msgq_first<GameStateInterface::SetStateTeamMessage>();
173  __gamestate_if->set_state_team(msg->state_team());
174  __gamestate_modified = true;
175  } else if (__gamestate_if->msgq_first_is<GameStateInterface::SetKickoffMessage>()) {
177  msg = __gamestate_if->msgq_first<GameStateInterface::SetKickoffMessage>();
178  __gamestate_if->set_kickoff(msg->is_kickoff());
179  __gamestate_modified = true;
180  }
181  __gamestate_if->msgq_pop();
182  }
183 #ifdef HAVE_SPL
184  while (!__penalty_if->msgq_empty()) {
185  if (__penalty_if->msgq_first_is<SoccerPenaltyInterface::SetPenaltyMessage>()) {
187  msg = __penalty_if->msgq_first<SoccerPenaltyInterface::SetPenaltyMessage>();
188  __penalty_if->set_penalty(msg->penalty());
189  __gamestate_modified = true;
190  }
191  __penalty_if->msgq_pop();
192  }
193 #endif
194  if (__refboxproc->check_connection()) {
195  __refboxproc->refbox_process();
196  }
197  if (__gamestate_modified) {
198  if (__cfg_beep_on_change && __beep_if->has_writer()) {
199  try {
200  __beep_if->msgq_enqueue(
201  new SwitchInterface::EnableDurationMessage(__cfg_beep_duration,
202  __cfg_beep_frequency));
203  } catch (Exception &e) {} // ignored
204  }
205 
206  __gamestate_if->write();
207 #ifdef HAVE_SPL
208  __penalty_if->write();
209 #endif
210  __gamestate_modified = false;
211  }
212 }
213 
214 
215 void
218 {
219  if (game_state != __last_gamestate) {
220  __last_gamestate = game_state;
221  __gamestate_modified = true;
222 
223  logger->log_debug("RefBoxCommThread", "Gamestate: %d State team: %s",
224  game_state, worldinfo_gamestate_team_tostring(state_team));
225  __gamestate_if->set_game_state(game_state);
226  switch (state_team) {
227  case TEAM_NONE:
228  __gamestate_if->set_state_team(GameStateInterface::TEAM_NONE); break;
229  case TEAM_CYAN:
230  __gamestate_if->set_state_team(GameStateInterface::TEAM_CYAN); break;
231  case TEAM_MAGENTA:
232  __gamestate_if->set_state_team(GameStateInterface::TEAM_MAGENTA); break;
233  case TEAM_BOTH:
234  __gamestate_if->set_state_team(GameStateInterface::TEAM_BOTH); break;
235  }
236  }
237 }
238 
239 void
240 RefBoxCommThread::set_score(unsigned int score_cyan, unsigned int score_magenta)
241 {
242  if ( (score_cyan != __last_score_cyan) || (score_magenta != __last_score_magenta) ) {
243  __last_score_cyan = score_cyan;
244  __last_score_magenta = score_magenta;
245  __gamestate_modified = true;
246 
247  logger->log_debug("RefBoxCommThread", "Score (cyan:magenta): %u:%u",
248  score_cyan, score_magenta);
249  __gamestate_if->set_score_cyan(score_cyan);
250  __gamestate_if->set_score_magenta(score_magenta);
251  }
252 }
253 
254 
255 void
258 {
259  if (our_team != __our_team)
260  {
261  logger->log_debug("RefBoxCommThread", "Team: %s",
263 
264  __our_team = our_team;
265  switch (our_team) {
266  case TEAM_CYAN:
267  __gamestate_if->set_our_team(GameStateInterface::TEAM_CYAN);
268  break;
269  case TEAM_MAGENTA:
270  __gamestate_if->set_our_team(GameStateInterface::TEAM_MAGENTA);
271  break;
272  default:
273  break;
274  }
275  __gamestate_modified = true;
276  }
277 
278  if (goal_color != __our_goal_color)
279  {
280  logger->log_debug("RefBoxCommThread", "Our Goal: %s",
282  __our_goal_color = goal_color;
283  switch (goal_color)
284  {
285  case GOAL_BLUE:
286  __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_BLUE);
287  break;
288  case GOAL_YELLOW:
289  __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_YELLOW);
290  break;
291  }
292  __gamestate_modified = true;
293  }
294 }
295 
296 
297 void
299  bool kickoff)
300 {
301  if (half != __last_half) {
302  __last_half = half;
303  __gamestate_modified = true;
304 
305  logger->log_debug("RefBoxCommThread", "Half time: %s (Kickoff? %s)",
307  kickoff ? "yes" : "no");
308 
309  switch (half) {
310  case HALF_FIRST:
311  __gamestate_if->set_half(GameStateInterface::HALF_FIRST); break;
312  case HALF_SECOND:
313  __gamestate_if->set_half(GameStateInterface::HALF_SECOND); break;
314  }
315  }
316 
317  if (kickoff != __kickoff)
318  {
319  __kickoff = kickoff;
320  __gamestate_modified = true;
321  __gamestate_if->set_kickoff(kickoff);
322  }
323 }
324 
325 
326 void
327 RefBoxCommThread::add_penalty(unsigned int penalty,
328  unsigned int seconds_remaining)
329 {
330 #ifdef HAVE_SPL
331  if ((penalty != __penalty_if->penalty()) ||
332  (seconds_remaining != __penalty_if->remaining()))
333  {
334  __gamestate_modified = true;
335  logger->log_debug("RefBoxCommThread", "Penalty %u (%u sec remaining)",
336  penalty, seconds_remaining);
337  __penalty_if->set_penalty(penalty);
338  __penalty_if->set_remaining(seconds_remaining);
339  }
340 #endif
341 }
342 
343 
344 
345 void
347 {
348  __gamestate_if->write();
349 }