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
42using 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
58void
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
147void
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
158void
160{
161 while (!gamestate_if_->msgq_empty()) {
165 gamestate_if_->set_our_team(msg->our_team());
166 gamestate_modified_ = true;
167 } else if (gamestate_if_->msgq_first_is<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>()) {
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
211void
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
232void
233RefBoxCommThread::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
246void
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
275void
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
300void
301RefBoxCommThread::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
316void
318{
319 gamestate_if_->write();
320}
Mid-size league refbox repeater.
Definition: msl2010.h:43
virtual void add_penalty(unsigned int penalty, unsigned int seconds_remaining)
Add penalty.
virtual void set_score(unsigned int score_cyan, unsigned int score_magenta)
Set score.
virtual void handle_refbox_state()
Process the information set up to now.
virtual void set_gamestate(int game_state, fawkes::worldinfo_gamestate_team_t state_team)
Set current game state.
virtual void set_team_goal(fawkes::worldinfo_gamestate_team_t our_team, fawkes::worldinfo_gamestate_goalcolor_t goal_color)
Set team and goal info.
RefBoxCommThread()
Constructor.
Definition: comm_thread.cpp:51
virtual void set_half(fawkes::worldinfo_gamestate_half_t half, bool kickoff)
Set current half of the game time.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Definition: comm_thread.cpp:59
virtual void finalize()
Finalize the thread.
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.
virtual void refbox_process()=0
Process incoming refbox communication.
Remote BlackBoard refbox repeater.
Definition: remotebb.h:36
SPL league refbox repeater.
Definition: spl.h:85
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
SetKickoffMessage Fawkes BlackBoard Interface Message.
SetStateTeamMessage Fawkes BlackBoard Interface Message.
if_gamestate_team_t state_team() const
Get state_team value.
SetTeamColorMessage Fawkes BlackBoard Interface Message.
if_gamestate_team_t our_team() const
Get our_team value.
GameStateInterface Fawkes BlackBoard Interface.
void set_score_magenta(const uint32_t new_score_magenta)
Set score_magenta value.
void set_half(const if_gamestate_half_t new_half)
Set half value.
void set_our_goal_color(const if_gamestate_goalcolor_t new_our_goal_color)
Set our_goal_color value.
void set_game_state(const uint32_t new_game_state)
Set game_state value.
void set_kickoff(const bool new_kickoff)
Set kickoff value.
void set_state_team(const if_gamestate_team_t new_state_team)
Set state_team value.
void set_score_cyan(const uint32_t new_score_cyan)
Set score_cyan value.
void set_our_team(const if_gamestate_team_t new_our_team)
Set our_team value.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
SetPenaltyMessage Fawkes BlackBoard Interface Message.
void set_penalty(const uint16_t new_penalty)
Set penalty value.
SoccerPenaltyInterface Fawkes BlackBoard Interface.
EnableDurationMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Fawkes library namespace.
const char * worldinfo_gamestate_goalcolor_tostring(worldinfo_gamestate_goalcolor_t goal_color)
Convert goal color to a string.
Definition: enums.cpp:91
worldinfo_gamestate_goalcolor_t
Goal color.
Definition: enums.h:61
@ GOAL_BLUE
Blue goal.
Definition: enums.h:62
@ GOAL_YELLOW
Yellow goal.
Definition: enums.h:63
const char * worldinfo_gamestate_half_tostring(worldinfo_gamestate_half_t half)
Convert half time to a string.
Definition: enums.cpp:105
const char * worldinfo_gamestate_team_tostring(worldinfo_gamestate_team_t team)
Convert gamestate team to a string.
Definition: enums.cpp:75
worldinfo_gamestate_half_t
Game time half.
Definition: enums.h:67
@ HALF_SECOND
Second half.
Definition: enums.h:69
@ HALF_FIRST
First half.
Definition: enums.h:68
worldinfo_gamestate_team_t
Team.
Definition: enums.h:53
@ TEAM_MAGENTA
Magenta team.
Definition: enums.h:56
@ TEAM_NONE
No team, not team-specific.
Definition: enums.h:54
@ TEAM_CYAN
Cyan team.
Definition: enums.h:55
@ TEAM_BOTH
Both teams.
Definition: enums.h:57