00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "comm_thread.h"
00025 #include "processor/remotebb.h"
00026 #ifdef HAVE_MSL2010
00027 # include "processor/msl2010.h"
00028 #endif
00029 #ifdef HAVE_SPL
00030 # include "processor/spl.h"
00031 #endif
00032
00033 #include <interfaces/GameStateInterface.h>
00034 #include <interfaces/SwitchInterface.h>
00035 #ifdef HAVE_SPL
00036 # include <interfaces/SoccerPenaltyInterface.h>
00037 #endif
00038
00039 #define CONFPREFIX "/plugins/refboxcomm"
00040
00041 using namespace fawkes;
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 RefBoxCommThread::RefBoxCommThread()
00052 : Thread("RefBoxCommThread", Thread::OPMODE_WAITFORWAKEUP),
00053 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR)
00054 {
00055 __refboxproc = NULL;
00056 }
00057
00058
00059 void
00060 RefBoxCommThread::init()
00061 {
00062 try {
00063 __refboxproc = NULL;
00064 __gamestate_if = NULL;
00065 __beep_if = NULL;
00066 #ifdef HAVE_SPL
00067 __penalty_if = NULL;
00068 #endif
00069 __last_half = (worldinfo_gamestate_half_t)-1;
00070 __last_score_cyan = 0xFFFFFFFF;
00071 __last_score_magenta = 0xFFFFFFFF;
00072 __last_gamestate = -1;
00073 __our_team = TEAM_NONE;
00074 __our_goal_color = GOAL_BLUE;
00075 __kickoff = false;
00076 __gamestate_modified = false;
00077
00078 std::string processor = "";
00079 try {
00080 processor = config->get_string(CONFPREFIX"/processor");
00081 } catch (Exception &e) {
00082
00083 std::string league = config->get_string("/general/league");
00084 if (league == "MSL" || league == "SPL") {
00085 processor = league;
00086 }
00087 }
00088 if (processor == "") {
00089 throw Exception("No valid processor defined");
00090 }
00091
00092 __cfg_beep_on_change = true;
00093 __cfg_beep_frequency = 1000.;
00094 __cfg_beep_duration = 0.5;
00095 try {
00096 __cfg_beep_on_change = config->get_bool(CONFPREFIX"/beep_on_change");
00097 } catch (Exception &e) {}
00098 try {
00099 __cfg_beep_frequency = config->get_float(CONFPREFIX"/beep_frequency");
00100 } catch (Exception &e) {}
00101 try {
00102 __cfg_beep_duration = config->get_float(CONFPREFIX"/beep_duration");
00103 } catch (Exception &e) {}
00104 if (__cfg_beep_on_change) {
00105 __beep_if = blackboard->open_for_reading<SwitchInterface>("Beep");
00106 }
00107
00108 if ( processor == "MSL" ) {
00109 #ifdef HAVE_MSL2010
00110 std::string refbox_host = config->get_string(CONFPREFIX"/MSL/host");
00111 unsigned int refbox_port = config->get_uint(CONFPREFIX"/MSL/port");
00112 __refboxproc = new Msl2010RefBoxProcessor(logger,
00113 refbox_host.c_str(), refbox_port);
00114 #else
00115 throw Exception("MSL2010 support not available at compile time");
00116 #endif
00117 } else if ( processor == "SPL" ) {
00118 #ifdef HAVE_SPL
00119 unsigned int refbox_port = config->get_uint(CONFPREFIX"/SPL/port");
00120 __team_number = config->get_uint("/general/team_number");
00121 __player_number = config->get_uint("/general/player_number");
00122 __refboxproc = new SplRefBoxProcessor(logger, refbox_port,
00123 __team_number, __player_number);
00124 #else
00125 throw Exception("SPL support not available at compile time");
00126 #endif
00127 } else if ( processor == "RemoteBB" ) {
00128 std::string bb_host = config->get_string(CONFPREFIX"/RemoteBB/host");
00129 unsigned int bb_port = config->get_uint(CONFPREFIX"/RemoteBB/port");
00130 std::string iface_id = config->get_string(CONFPREFIX"/RemoteBB/interface_id");
00131 __refboxproc = new RemoteBlackBoardRefBoxProcessor(logger,
00132 bb_host.c_str(), bb_port,
00133 iface_id.c_str());
00134 } else {
00135 throw Exception("Processor %s is not supported by refboxcomm plugin",
00136 processor.c_str());
00137 }
00138 __refboxproc->set_handler(this);
00139 __gamestate_if = blackboard->open_for_writing<GameStateInterface>("RefBoxComm");
00140 #ifdef HAVE_SPL
00141 __penalty_if = blackboard->open_for_writing<SoccerPenaltyInterface>("SPL Penalty");
00142 #endif
00143 } catch (Exception &e) {
00144 finalize();
00145 throw;
00146 }
00147 }
00148
00149
00150 void
00151 RefBoxCommThread::finalize()
00152 {
00153 delete __refboxproc;
00154 blackboard->close(__gamestate_if);
00155 blackboard->close(__beep_if);
00156 #ifdef HAVE_SPL
00157 blackboard->close(__penalty_if);
00158 #endif
00159 }
00160
00161 void
00162 RefBoxCommThread::loop()
00163 {
00164 while (!__gamestate_if->msgq_empty()) {
00165 if (__gamestate_if->msgq_first_is<GameStateInterface::SetTeamColorMessage>()) {
00166 GameStateInterface::SetTeamColorMessage *msg;
00167 msg = __gamestate_if->msgq_first<GameStateInterface::SetTeamColorMessage>();
00168 __gamestate_if->set_our_team(msg->our_team());
00169 __gamestate_modified = true;
00170 } else if (__gamestate_if->msgq_first_is<GameStateInterface::SetStateTeamMessage>()) {
00171 GameStateInterface::SetStateTeamMessage *msg;
00172 msg = __gamestate_if->msgq_first<GameStateInterface::SetStateTeamMessage>();
00173 __gamestate_if->set_state_team(msg->state_team());
00174 __gamestate_modified = true;
00175 } else if (__gamestate_if->msgq_first_is<GameStateInterface::SetKickoffMessage>()) {
00176 GameStateInterface::SetKickoffMessage *msg;
00177 msg = __gamestate_if->msgq_first<GameStateInterface::SetKickoffMessage>();
00178 __gamestate_if->set_kickoff(msg->is_kickoff());
00179 __gamestate_modified = true;
00180 }
00181 __gamestate_if->msgq_pop();
00182 }
00183 #ifdef HAVE_SPL
00184 while (!__penalty_if->msgq_empty()) {
00185 if (__penalty_if->msgq_first_is<SoccerPenaltyInterface::SetPenaltyMessage>()) {
00186 SoccerPenaltyInterface::SetPenaltyMessage *msg;
00187 msg = __penalty_if->msgq_first<SoccerPenaltyInterface::SetPenaltyMessage>();
00188 __penalty_if->set_penalty(msg->penalty());
00189 __gamestate_modified = true;
00190 }
00191 __penalty_if->msgq_pop();
00192 }
00193 #endif
00194 if (__refboxproc->check_connection()) {
00195 __refboxproc->refbox_process();
00196 }
00197 if (__gamestate_modified) {
00198 if (__cfg_beep_on_change && __beep_if->has_writer()) {
00199 try {
00200 __beep_if->msgq_enqueue(
00201 new SwitchInterface::EnableDurationMessage(__cfg_beep_duration,
00202 __cfg_beep_frequency));
00203 } catch (Exception &e) {}
00204 }
00205
00206 __gamestate_if->write();
00207 #ifdef HAVE_SPL
00208 __penalty_if->write();
00209 #endif
00210 __gamestate_modified = false;
00211 }
00212 }
00213
00214
00215 void
00216 RefBoxCommThread::set_gamestate(int game_state,
00217 fawkes::worldinfo_gamestate_team_t state_team)
00218 {
00219 if (game_state != __last_gamestate) {
00220 __last_gamestate = game_state;
00221 __gamestate_modified = true;
00222
00223 logger->log_debug("RefBoxCommThread", "Gamestate: %d State team: %s",
00224 game_state, worldinfo_gamestate_team_tostring(state_team));
00225 __gamestate_if->set_game_state(game_state);
00226 switch (state_team) {
00227 case TEAM_NONE:
00228 __gamestate_if->set_state_team(GameStateInterface::TEAM_NONE); break;
00229 case TEAM_CYAN:
00230 __gamestate_if->set_state_team(GameStateInterface::TEAM_CYAN); break;
00231 case TEAM_MAGENTA:
00232 __gamestate_if->set_state_team(GameStateInterface::TEAM_MAGENTA); break;
00233 case TEAM_BOTH:
00234 __gamestate_if->set_state_team(GameStateInterface::TEAM_BOTH); break;
00235 }
00236 }
00237 }
00238
00239 void
00240 RefBoxCommThread::set_score(unsigned int score_cyan, unsigned int score_magenta)
00241 {
00242 if ( (score_cyan != __last_score_cyan) || (score_magenta != __last_score_magenta) ) {
00243 __last_score_cyan = score_cyan;
00244 __last_score_magenta = score_magenta;
00245 __gamestate_modified = true;
00246
00247 logger->log_debug("RefBoxCommThread", "Score (cyan:magenta): %u:%u",
00248 score_cyan, score_magenta);
00249 __gamestate_if->set_score_cyan(score_cyan);
00250 __gamestate_if->set_score_magenta(score_magenta);
00251 }
00252 }
00253
00254
00255 void
00256 RefBoxCommThread::set_team_goal(fawkes::worldinfo_gamestate_team_t our_team,
00257 fawkes::worldinfo_gamestate_goalcolor_t goal_color)
00258 {
00259 if (our_team != __our_team)
00260 {
00261 logger->log_debug("RefBoxCommThread", "Team: %s",
00262 worldinfo_gamestate_team_tostring(our_team));
00263
00264 __our_team = our_team;
00265 switch (our_team) {
00266 case TEAM_CYAN:
00267 __gamestate_if->set_our_team(GameStateInterface::TEAM_CYAN);
00268 break;
00269 case TEAM_MAGENTA:
00270 __gamestate_if->set_our_team(GameStateInterface::TEAM_MAGENTA);
00271 break;
00272 default:
00273 break;
00274 }
00275 __gamestate_modified = true;
00276 }
00277
00278 if (goal_color != __our_goal_color)
00279 {
00280 logger->log_debug("RefBoxCommThread", "Our Goal: %s",
00281 worldinfo_gamestate_goalcolor_tostring(goal_color));
00282 __our_goal_color = goal_color;
00283 switch (goal_color)
00284 {
00285 case GOAL_BLUE:
00286 __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_BLUE);
00287 break;
00288 case GOAL_YELLOW:
00289 __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_YELLOW);
00290 break;
00291 }
00292 __gamestate_modified = true;
00293 }
00294 }
00295
00296
00297 void
00298 RefBoxCommThread::set_half(fawkes::worldinfo_gamestate_half_t half,
00299 bool kickoff)
00300 {
00301 if (half != __last_half) {
00302 __last_half = half;
00303 __gamestate_modified = true;
00304
00305 logger->log_debug("RefBoxCommThread", "Half time: %s (Kickoff? %s)",
00306 worldinfo_gamestate_half_tostring(half),
00307 kickoff ? "yes" : "no");
00308
00309 switch (half) {
00310 case HALF_FIRST:
00311 __gamestate_if->set_half(GameStateInterface::HALF_FIRST); break;
00312 case HALF_SECOND:
00313 __gamestate_if->set_half(GameStateInterface::HALF_SECOND); break;
00314 }
00315 }
00316
00317 if (kickoff != __kickoff)
00318 {
00319 __kickoff = kickoff;
00320 __gamestate_modified = true;
00321 __gamestate_if->set_kickoff(kickoff);
00322 }
00323 }
00324
00325
00326 void
00327 RefBoxCommThread::add_penalty(unsigned int penalty,
00328 unsigned int seconds_remaining)
00329 {
00330 #ifdef HAVE_SPL
00331 if ((penalty != __penalty_if->penalty()) ||
00332 (seconds_remaining != __penalty_if->remaining()))
00333 {
00334 __gamestate_modified = true;
00335 logger->log_debug("RefBoxCommThread", "Penalty %u (%u sec remaining)",
00336 penalty, seconds_remaining);
00337 __penalty_if->set_penalty(penalty);
00338 __penalty_if->set_remaining(seconds_remaining);
00339 }
00340 #endif
00341 }
00342
00343
00344
00345 void
00346 RefBoxCommThread::handle_refbox_state()
00347 {
00348 __gamestate_if->write();
00349 }