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 }
fawkes::worldinfo_gamestate_team_tostring
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
Definition: enums.cpp:75
fawkes::TEAM_MAGENTA
@ TEAM_MAGENTA
Magenta team.
Definition: enums.h:56
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
fawkes::GameStateInterface::SetKickoffMessage::is_kickoff
bool is_kickoff() const
Get kickoff value.
Definition: GameStateInterface.cpp:694
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::GameStateInterface::SetTeamColorMessage::our_team
if_gamestate_team_t our_team() const
Get our_team value.
Definition: GameStateInterface.cpp:578
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
RefBoxCommThread::set_gamestate
virtual void set_gamestate(int game_state, fawkes::worldinfo_gamestate_team_t state_team)
Set current game state.
Definition: comm_thread.cpp:212
RefBoxProcessor::set_handler
virtual void set_handler(RefBoxStateHandler *rsh)
Set handler.
Definition: processor.cpp:53
RefBoxProcessor::check_connection
virtual bool check_connection()=0
Check if the connection is alive and reconnect.
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
SplRefBoxProcessor
SPL league refbox repeater.
Definition: spl.h:85
fawkes::SwitchInterface::EnableDurationMessage
EnableDurationMessage Fawkes BlackBoard Interface Message.
Definition: SwitchInterface.h:156
fawkes::GameStateInterface::SetStateTeamMessage
SetStateTeamMessage Fawkes BlackBoard Interface Message.
Definition: GameStateInterface.h:184
fawkes::GameStateInterface
GameStateInterface Fawkes BlackBoard Interface.
Definition: GameStateInterface.h:34
fawkes::GameStateInterface::set_our_team
void set_our_team(const if_gamestate_team_t new_our_team)
Set our_team value.
Definition: GameStateInterface.cpp:262
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
RefBoxCommThread::loop
virtual void loop()
Code to execute in the thread.
Definition: comm_thread.cpp:159
fawkes::GameStateInterface::SetKickoffMessage
SetKickoffMessage Fawkes BlackBoard Interface Message.
Definition: GameStateInterface.h:155
fawkes::TEAM_BOTH
@ TEAM_BOTH
Both teams.
Definition: enums.h:57
fawkes::GameStateInterface::SetTeamColorMessage
SetTeamColorMessage Fawkes BlackBoard Interface Message.
Definition: GameStateInterface.h:126
fawkes::GameStateInterface::SetStateTeamMessage::state_team
if_gamestate_team_t state_team() const
Get state_team value.
Definition: GameStateInterface.cpp:810
fawkes::GOAL_YELLOW
@ GOAL_YELLOW
Yellow goal.
Definition: enums.h:63
fawkes::SoccerPenaltyInterface::SetPenaltyMessage
SetPenaltyMessage Fawkes BlackBoard Interface Message.
Definition: SoccerPenaltyInterface.h:65
fawkes::SoccerPenaltyInterface::SetPenaltyMessage::set_penalty
void set_penalty(const uint16_t new_penalty)
Set penalty value.
Definition: SoccerPenaltyInterface.cpp:257
fawkes::GameStateInterface::set_our_goal_color
void set_our_goal_color(const if_gamestate_goalcolor_t new_our_goal_color)
Set our_goal_color value.
Definition: GameStateInterface.cpp:292
RefBoxCommThread::add_penalty
virtual void add_penalty(unsigned int penalty, unsigned int seconds_remaining)
Add penalty.
Definition: comm_thread.cpp:301
fawkes::HALF_FIRST
@ HALF_FIRST
First half.
Definition: enums.h:68
fawkes::TEAM_NONE
@ TEAM_NONE
No team, not team-specific.
Definition: enums.h:54
RefBoxCommThread::RefBoxCommThread
RefBoxCommThread()
Constructor.
Definition: comm_thread.cpp:51
fawkes::GameStateInterface::set_score_magenta
void set_score_magenta(const uint32_t new_score_magenta)
Set score_magenta value.
Definition: GameStateInterface.cpp:442
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::worldinfo_gamestate_half_tostring
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
Definition: enums.cpp:105
fawkes::worldinfo_gamestate_goalcolor_tostring
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
Definition: enums.cpp:91
RefBoxCommThread::finalize
virtual void finalize()
Finalize the thread.
Definition: comm_thread.cpp:148
fawkes::HALF_SECOND
@ HALF_SECOND
Second half.
Definition: enums.h:69
fawkes
Fawkes library namespace.
RemoteBlackBoardRefBoxProcessor
Remote BlackBoard refbox repeater.
Definition: remotebb.h:36
fawkes::TEAM_CYAN
@ TEAM_CYAN
Cyan team.
Definition: enums.h:55
fawkes::worldinfo_gamestate_goalcolor_t
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:61
RefBoxCommThread::set_team_goal
virtual void set_team_goal(fawkes::worldinfo_gamestate_team_t our_team, fawkes::worldinfo_gamestate_goalcolor_t goal_color)
Set team and goal info.
Definition: comm_thread.cpp:247
fawkes::worldinfo_gamestate_team_t
worldinfo_gamestate_team_t
Team.
Definition: enums.h:53
fawkes::GameStateInterface::set_state_team
void set_state_team(const if_gamestate_team_t new_state_team)
Set state_team value.
Definition: GameStateInterface.cpp:232
RefBoxCommThread::set_score
virtual void set_score(unsigned int score_cyan, unsigned int score_magenta)
Set score.
Definition: comm_thread.cpp:233
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
RefBoxProcessor::refbox_process
virtual void refbox_process()=0
Process incoming refbox communication.
fawkes::GameStateInterface::set_kickoff
void set_kickoff(const bool new_kickoff)
Set kickoff value.
Definition: GameStateInterface.cpp:352
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
RefBoxCommThread::init
virtual void init()
Initialize the thread.
Definition: comm_thread.cpp:59
fawkes::GameStateInterface::set_game_state
void set_game_state(const uint32_t new_game_state)
Set game_state value.
Definition: GameStateInterface.cpp:202
fawkes::GameStateInterface::set_half
void set_half(const if_gamestate_half_t new_half)
Set half value.
Definition: GameStateInterface.cpp:322
fawkes::GOAL_BLUE
@ GOAL_BLUE
Blue goal.
Definition: enums.h:62
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::worldinfo_gamestate_half_t
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:67
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
RefBoxCommThread::set_half
virtual void set_half(fawkes::worldinfo_gamestate_half_t half, bool kickoff)
Set current half of the game time.
Definition: comm_thread.cpp:276
fawkes::SoccerPenaltyInterface
SoccerPenaltyInterface Fawkes BlackBoard Interface.
Definition: SoccerPenaltyInterface.h:34
Msl2010RefBoxProcessor
Mid-size league refbox repeater.
Definition: msl2010.h:43
fawkes::GameStateInterface::set_score_cyan
void set_score_cyan(const uint32_t new_score_cyan)
Set score_cyan value.
Definition: GameStateInterface.cpp:412
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::SoccerPenaltyInterface::SetPenaltyMessage::penalty
uint16_t penalty() const
Get penalty value.
Definition: SoccerPenaltyInterface.cpp:237
RefBoxCommThread::handle_refbox_state
virtual void handle_refbox_state()
Process the information set up to now.
Definition: comm_thread.cpp:317
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36