comm_thread.cpp

00001 
00002 /***************************************************************************
00003  *  comm_thread.cpp - Fawkes RefBox Communication Thread
00004  *
00005  *  Created: Sun Apr 19 13:13:43 2009 (on way to German Open 2009)
00006  *  Copyright  2009  Tim Niemueller [www.niemueller.de]
00007  *             2009  Tobias Kellner
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
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 /** @class RefBoxCommThread "comm_thread.h"
00044  * Referee Box Communication Thread for robotic soccer.
00045  * This thread communicates with the refbox.
00046  * @author Tim Niemueller
00047  */
00048 
00049 
00050 /** Constructor. */
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       // try to get league
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) {} // ignored
00098     try {
00099       __cfg_beep_frequency = config->get_float(CONFPREFIX"/beep_frequency");
00100     } catch (Exception &e) {} // ignored
00101     try {
00102       __cfg_beep_duration = config->get_float(CONFPREFIX"/beep_duration");
00103     } catch (Exception &e) {} // ignored
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) {} // ignored
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 }

Generated on Tue Feb 22 13:31:30 2011 for Fawkes API by  doxygen 1.4.7