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 
26 #include "processor/remotebb.h"
27 #ifdef HAVE_MSL2010
28 # include "processor/msl2010.h"
29 #endif
30 #ifdef HAVE_SPL
31 # include "processor/spl.h"
32 #endif
33 
34 #include <interfaces/GameStateInterface.h>
35 #include <interfaces/SwitchInterface.h>
36 #ifdef HAVE_SPL
37 # include <interfaces/SoccerPenaltyInterface.h>
38 #endif
39 
40 #define CONFPREFIX "/plugins/refboxcomm"
41 
42 using namespace fawkes;
43 
44 /** @class RefBoxCommThread "comm_thread.h"
45  * Referee Box Communication Thread for robotic soccer.
46  * This thread communicates with the refbox.
47  * @author Tim Niemueller
48  */
49 
50 /** Constructor. */
52 : Thread("RefBoxCommThread", Thread::OPMODE_WAITFORWAKEUP),
53  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE)
54 {
55  refboxproc_ = NULL;
56 }
57 
58 void
60 {
61  try {
62  refboxproc_ = NULL;
63  gamestate_if_ = NULL;
64  beep_if_ = NULL;
65 #ifdef HAVE_SPL
66  penalty_if_ = NULL;
67 #endif
68  last_half_ = (worldinfo_gamestate_half_t)-1;
69  last_score_cyan_ = 0xFFFFFFFF;
70  last_score_magenta_ = 0xFFFFFFFF;
71  last_gamestate_ = -1;
72  our_team_ = TEAM_NONE;
73  our_goal_color_ = GOAL_BLUE;
74  kickoff_ = false;
75  gamestate_modified_ = false;
76 
77  std::string processor = "";
78  try {
79  processor = config->get_string(CONFPREFIX "/processor");
80  } catch (Exception &e) {
81  // try to get league
82  std::string league = config->get_string("/general/league");
83  if (league == "MSL" || league == "SPL") {
84  processor = league;
85  }
86  }
87  if (processor == "") {
88  throw Exception("No valid processor defined");
89  }
90 
91  cfg_beep_on_change_ = true;
92  cfg_beep_frequency_ = 1000.;
93  cfg_beep_duration_ = 0.5;
94  try {
95  cfg_beep_on_change_ = config->get_bool(CONFPREFIX "/beep_on_change");
96  } catch (Exception &e) {
97  } // ignored
98  try {
99  cfg_beep_frequency_ = config->get_float(CONFPREFIX "/beep_frequency");
100  } catch (Exception &e) {
101  } // ignored
102  try {
103  cfg_beep_duration_ = config->get_float(CONFPREFIX "/beep_duration");
104  } catch (Exception &e) {
105  } // ignored
106  if (cfg_beep_on_change_) {
107  beep_if_ = blackboard->open_for_reading<SwitchInterface>("Beep");
108  }
109 
110  if (processor == "MSL") {
111 #ifdef HAVE_MSL2010
112  std::string refbox_host = config->get_string(CONFPREFIX "/MSL/host");
113  unsigned int refbox_port = config->get_uint(CONFPREFIX "/MSL/port");
114  refboxproc_ = new Msl2010RefBoxProcessor(logger, refbox_host.c_str(), refbox_port);
115 #else
116  throw Exception("MSL2010 support not available at compile time");
117 #endif
118  } else if (processor == "SPL") {
119 #ifdef HAVE_SPL
120  unsigned int refbox_port = config->get_uint(CONFPREFIX "/SPL/port");
121  team_number_ = config->get_uint("/general/team_number");
122  player_number_ = config->get_uint("/general/player_number");
123  refboxproc_ = new SplRefBoxProcessor(logger, refbox_port, 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_ =
132  new RemoteBlackBoardRefBoxProcessor(logger, bb_host.c_str(), bb_port, iface_id.c_str());
133  } else {
134  throw Exception("Processor %s is not supported by refboxcomm plugin", processor.c_str());
135  }
136  refboxproc_->set_handler(this);
137  gamestate_if_ = blackboard->open_for_writing<GameStateInterface>("RefBoxComm");
138 #ifdef HAVE_SPL
139  penalty_if_ = blackboard->open_for_writing<SoccerPenaltyInterface>("SPL Penalty");
140 #endif
141  } catch (Exception &e) {
142  finalize();
143  throw;
144  }
145 }
146 
147 void
149 {
150  delete refboxproc_;
151  blackboard->close(gamestate_if_);
152  blackboard->close(beep_if_);
153 #ifdef HAVE_SPL
154  blackboard->close(penalty_if_);
155 #endif
156 }
157 
158 void
160 {
161  while (!gamestate_if_->msgq_empty()) {
164  msg = gamestate_if_->msgq_first<GameStateInterface::SetTeamColorMessage>();
165  gamestate_if_->set_our_team(msg->our_team());
166  gamestate_modified_ = true;
167  } else if (gamestate_if_->msgq_first_is<GameStateInterface::SetStateTeamMessage>()) {
169  msg = gamestate_if_->msgq_first<GameStateInterface::SetStateTeamMessage>();
170  gamestate_if_->set_state_team(msg->state_team());
171  gamestate_modified_ = true;
172  } else if (gamestate_if_->msgq_first_is<GameStateInterface::SetKickoffMessage>()) {
174  msg = gamestate_if_->msgq_first<GameStateInterface::SetKickoffMessage>();
175  gamestate_if_->set_kickoff(msg->is_kickoff());
176  gamestate_modified_ = true;
177  }
178  gamestate_if_->msgq_pop();
179  }
180 #ifdef HAVE_SPL
181  while (!penalty_if_->msgq_empty()) {
182  if (penalty_if_->msgq_first_is<SoccerPenaltyInterface::SetPenaltyMessage>()) {
184  msg = penalty_if_->msgq_first<SoccerPenaltyInterface::SetPenaltyMessage>();
185  penalty_if_->set_penalty(msg->penalty());
186  gamestate_modified_ = true;
187  }
188  penalty_if_->msgq_pop();
189  }
190 #endif
191  if (refboxproc_->check_connection()) {
192  refboxproc_->refbox_process();
193  }
194  if (gamestate_modified_) {
195  if (cfg_beep_on_change_ && beep_if_->has_writer()) {
196  try {
197  beep_if_->msgq_enqueue(
198  new SwitchInterface::EnableDurationMessage(cfg_beep_duration_, cfg_beep_frequency_));
199  } catch (Exception &e) {
200  } // ignored
201  }
202 
203  gamestate_if_->write();
204 #ifdef HAVE_SPL
205  penalty_if_->write();
206 #endif
207  gamestate_modified_ = false;
208  }
209 }
210 
211 void
213 {
214  if (game_state != last_gamestate_) {
215  last_gamestate_ = game_state;
216  gamestate_modified_ = true;
217 
218  logger->log_debug("RefBoxCommThread",
219  "Gamestate: %d State team: %s",
220  game_state,
222  gamestate_if_->set_game_state(game_state);
223  switch (state_team) {
224  case TEAM_NONE: gamestate_if_->set_state_team(GameStateInterface::TEAM_NONE); break;
225  case TEAM_CYAN: gamestate_if_->set_state_team(GameStateInterface::TEAM_CYAN); break;
226  case TEAM_MAGENTA: gamestate_if_->set_state_team(GameStateInterface::TEAM_MAGENTA); break;
227  case TEAM_BOTH: gamestate_if_->set_state_team(GameStateInterface::TEAM_BOTH); break;
228  }
229  }
230 }
231 
232 void
233 RefBoxCommThread::set_score(unsigned int score_cyan, unsigned int score_magenta)
234 {
235  if ((score_cyan != last_score_cyan_) || (score_magenta != last_score_magenta_)) {
236  last_score_cyan_ = score_cyan;
237  last_score_magenta_ = score_magenta;
238  gamestate_modified_ = true;
239 
240  logger->log_debug("RefBoxCommThread", "Score (cyan:magenta): %u:%u", score_cyan, score_magenta);
241  gamestate_if_->set_score_cyan(score_cyan);
242  gamestate_if_->set_score_magenta(score_magenta);
243  }
244 }
245 
246 void
249 {
250  if (our_team != our_team_) {
251  logger->log_debug("RefBoxCommThread", "Team: %s", worldinfo_gamestate_team_tostring(our_team));
252 
253  our_team_ = our_team;
254  switch (our_team) {
255  case TEAM_CYAN: gamestate_if_->set_our_team(GameStateInterface::TEAM_CYAN); break;
256  case TEAM_MAGENTA: gamestate_if_->set_our_team(GameStateInterface::TEAM_MAGENTA); break;
257  default: break;
258  }
259  gamestate_modified_ = true;
260  }
261 
262  if (goal_color != our_goal_color_) {
263  logger->log_debug("RefBoxCommThread",
264  "Our Goal: %s",
266  our_goal_color_ = goal_color;
267  switch (goal_color) {
268  case GOAL_BLUE: gamestate_if_->set_our_goal_color(GameStateInterface::GOAL_BLUE); break;
269  case GOAL_YELLOW: gamestate_if_->set_our_goal_color(GameStateInterface::GOAL_YELLOW); break;
270  }
271  gamestate_modified_ = true;
272  }
273 }
274 
275 void
277 {
278  if (half != last_half_) {
279  last_half_ = half;
280  gamestate_modified_ = true;
281 
282  logger->log_debug("RefBoxCommThread",
283  "Half time: %s (Kickoff? %s)",
285  kickoff ? "yes" : "no");
286 
287  switch (half) {
288  case HALF_FIRST: gamestate_if_->set_half(GameStateInterface::HALF_FIRST); break;
289  case HALF_SECOND: gamestate_if_->set_half(GameStateInterface::HALF_SECOND); break;
290  }
291  }
292 
293  if (kickoff != kickoff_) {
294  kickoff_ = kickoff;
295  gamestate_modified_ = true;
296  gamestate_if_->set_kickoff(kickoff);
297  }
298 }
299 
300 void
301 RefBoxCommThread::add_penalty(unsigned int penalty, unsigned int seconds_remaining)
302 {
303 #ifdef HAVE_SPL
304  if ((penalty != penalty_if_->penalty()) || (seconds_remaining != penalty_if_->remaining())) {
305  gamestate_modified_ = true;
306  logger->log_debug("RefBoxCommThread",
307  "Penalty %u (%u sec remaining)",
308  penalty,
309  seconds_remaining);
310  penalty_if_->set_penalty(penalty);
311  penalty_if_->set_remaining(seconds_remaining);
312  }
313 #endif
314 }
315 
316 void
318 {
319  gamestate_if_->write();
320 }
virtual void init()
Initialize the thread.
Definition: comm_thread.cpp:59
Blue goal.
Definition: enums.h:62
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:1026
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:54
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:63
if_gamestate_team_t our_team() const
Get our_team value.
Thread class encapsulation of pthreads.
Definition: thread.h:45
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:494
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:41
Magenta team.
Definition: enums.h:56
virtual void loop()
Code to execute in the thread.
SoccerPenaltyInterface Fawkes BlackBoard Interface.
First half.
Definition: enums.h:68
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:67
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:1179
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:35
virtual void refbox_process()=0
Process incoming refbox communication.
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1164
SetTeamColorMessage Fawkes BlackBoard Interface Message.
Both teams.
Definition: enums.h:57
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:814
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
Second half.
Definition: enums.h:69
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:91
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:879
SetStateTeamMessage Fawkes BlackBoard Interface Message.
Remote BlackBoard refbox repeater.
Definition: remotebb.h:35
Cyan team.
Definition: enums.h:55
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
worldinfo_gamestate_team_t
Team.
Definition: enums.h:53
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:105
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:41
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:61
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:84
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.