Fawkes API  Fawkes Development Version
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 }
virtual void init()
Initialize the thread.
Definition: comm_thread.cpp:60
Blue goal.
Definition: enums.h:64
void set_game_state(const uint32_t new_game_state)
Set game_state value.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
void set_score_cyan(const uint32_t new_score_cyan)
Set score_cyan value.
void set_our_goal_color(const if_gamestate_goalcolor_t new_our_goal_color)
Set our_goal_color value.
No team, not team-specific.
Definition: enums.h:55
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void set_state_team(const if_gamestate_team_t new_state_team)
Set state_team value.
void set_penalty(const uint16_t new_penalty)
Set penalty value.
virtual void set_gamestate(int game_state, fawkes::worldinfo_gamestate_team_t state_team)
Set current game state.
virtual void handle_refbox_state()
Process the information set up to now.
Yellow goal.
Definition: enums.h:65
if_gamestate_team_t our_team() const
Get our_team value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Mid-size league refbox repeater.
Definition: msl2010.h:42
virtual void set_team_goal(fawkes::worldinfo_gamestate_team_t our_team, fawkes::worldinfo_gamestate_goalcolor_t goal_color)
Set team and goal info.
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
virtual void set_handler(RefBoxStateHandler *rsh)
Set handler.
Definition: processor.cpp:53
virtual bool check_connection()=0
Check if the connection is alive and reconnect.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Magenta team.
Definition: enums.h:57
virtual void loop()
Code to execute in the thread.
SoccerPenaltyInterface Fawkes BlackBoard Interface.
First half.
Definition: enums.h:71
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:70
SetPenaltyMessage Fawkes BlackBoard Interface Message.
virtual void add_penalty(unsigned int penalty, unsigned int seconds_remaining)
Add penalty.
Thread aspect to use blocked timing.
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
RefBoxCommThread()
Constructor.
Definition: comm_thread.cpp:51
SwitchInterface Fawkes BlackBoard Interface.
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
Definition: enums.cpp:75
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void refbox_process()=0
Process incoming refbox communication.
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
SetTeamColorMessage Fawkes BlackBoard Interface Message.
Both teams.
Definition: enums.h:58
virtual void set_score(unsigned int score_cyan, unsigned int score_magenta)
Set score.
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
Second half.
Definition: enums.h:72
SetKickoffMessage Fawkes BlackBoard Interface Message.
if_gamestate_team_t state_team() const
Get state_team value.
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
Definition: enums.cpp:92
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
SetStateTeamMessage Fawkes BlackBoard Interface Message.
Remote BlackBoard refbox repeater.
Definition: remotebb.h:35
Cyan team.
Definition: enums.h:56
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
worldinfo_gamestate_team_t
Team.
Definition: enums.h:54
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void finalize()
Finalize the thread.
GameStateInterface Fawkes BlackBoard Interface.
EnableDurationMessage Fawkes BlackBoard Interface Message.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
void set_score_magenta(const uint32_t new_score_magenta)
Set score_magenta value.
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
Definition: enums.cpp:108
virtual void set_half(fawkes::worldinfo_gamestate_half_t half, bool kickoff)
Set current half of the game time.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:63
void set_kickoff(const bool new_kickoff)
Set kickoff value.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
void set_half(const if_gamestate_half_t new_half)
Set half value.
SPL league refbox repeater.
Definition: spl.h:78
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
void set_our_team(const if_gamestate_team_t new_our_team)
Set our_team value.
virtual void close(Interface *interface)=0
Close interface.