comm_thread.cpp
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/msl2008.h"
00026 #include "processor/spl.h"
00027
00028 #include <interfaces/GameStateInterface.h>
00029 #include <interfaces/SplPenaltyInterface.h>
00030
00031 using namespace fawkes;
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 RefBoxCommThread::RefBoxCommThread()
00042 : Thread("RefBoxCommThread", Thread::OPMODE_WAITFORWAKEUP),
00043 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR)
00044 {
00045 __refboxproc = NULL;
00046 }
00047
00048
00049 void
00050 RefBoxCommThread::init()
00051 {
00052 try {
00053 __refboxproc = NULL;
00054 __gamestate_if = NULL;
00055 __penalty_if = NULL;
00056 __last_half = (worldinfo_gamestate_half_t)-1;
00057 __last_score_cyan = 0xFFFFFFFF;
00058 __last_score_magenta = 0xFFFFFFFF;
00059 __last_gamestate = -1;
00060 __our_team = TEAM_NONE;
00061 __our_goal_color = GOAL_BLUE;
00062 __kickoff = false;
00063 __gamestate_modified = false;
00064
00065 std::string league = config->get_string("/general/league");
00066 if ( league == "MSL" ) {
00067 std::string refbox_host = config->get_string("/refboxcomm/MSL/host");
00068 unsigned int refbox_port = config->get_uint("/refboxcomm/MSL/port");
00069 __refboxproc = new Msl2008RefBoxProcessor(refbox_host.c_str(), refbox_port);
00070 } else if ( league == "SPL" ) {
00071 unsigned int refbox_port = config->get_uint("/refboxcomm/SPL/port");
00072 __team_number = config->get_uint("/general/team_number");
00073 __player_number = config->get_uint("/general/player_number");
00074 __refboxproc = new SplRefBoxProcessor(logger, refbox_port,
00075 __team_number, __player_number);
00076 } else {
00077 throw Exception("League %s is not supported by refboxcomm plugin", league.c_str());
00078 }
00079 __refboxproc->set_handler(this);
00080 __gamestate_if = blackboard->open_for_writing<GameStateInterface>("RefBoxComm");
00081 __penalty_if = blackboard->open_for_writing<SplPenaltyInterface>("SPL Penalty");
00082 } catch (Exception &e) {
00083 finalize();
00084 throw;
00085 }
00086 }
00087
00088
00089 void
00090 RefBoxCommThread::finalize()
00091 {
00092 delete __refboxproc;
00093 blackboard->close(__gamestate_if);
00094 blackboard->close(__penalty_if);
00095 }
00096
00097 void
00098 RefBoxCommThread::loop()
00099 {
00100 while (!__gamestate_if->msgq_empty()) {
00101 if (__gamestate_if->msgq_first_is<GameStateInterface::SetTeamColorMessage>()) {
00102 GameStateInterface::SetTeamColorMessage *msg;
00103 msg = __gamestate_if->msgq_first<GameStateInterface::SetTeamColorMessage>();
00104 __gamestate_if->set_our_team(msg->our_team());
00105 __gamestate_modified = true;
00106 } else if (__gamestate_if->msgq_first_is<GameStateInterface::SetStateTeamMessage>()) {
00107 GameStateInterface::SetStateTeamMessage *msg;
00108 msg = __gamestate_if->msgq_first<GameStateInterface::SetStateTeamMessage>();
00109 __gamestate_if->set_state_team(msg->state_team());
00110 __gamestate_modified = true;
00111 } else if (__gamestate_if->msgq_first_is<GameStateInterface::SetKickoffMessage>()) {
00112 GameStateInterface::SetKickoffMessage *msg;
00113 msg = __gamestate_if->msgq_first<GameStateInterface::SetKickoffMessage>();
00114 __gamestate_if->set_kickoff(msg->is_kickoff());
00115 __gamestate_modified = true;
00116 }
00117 __gamestate_if->msgq_pop();
00118 }
00119 while (!__penalty_if->msgq_empty()) {
00120 if (__penalty_if->msgq_first_is<SplPenaltyInterface::SetPenaltyMessage>()) {
00121 SplPenaltyInterface::SetPenaltyMessage *msg;
00122 msg = __penalty_if->msgq_first<SplPenaltyInterface::SetPenaltyMessage>();
00123 __penalty_if->set_penalty(msg->penalty());
00124 __gamestate_modified = true;
00125 }
00126 __penalty_if->msgq_pop();
00127 }
00128 __refboxproc->refbox_process();
00129 if (__gamestate_modified) {
00130 __gamestate_if->write();
00131 __penalty_if->write();
00132 __gamestate_modified = false;
00133 }
00134 }
00135
00136
00137 void
00138 RefBoxCommThread::set_gamestate(int game_state,
00139 fawkes::worldinfo_gamestate_team_t state_team)
00140 {
00141 if (game_state != __last_gamestate) {
00142 __last_gamestate = game_state;
00143 __gamestate_modified = true;
00144
00145 logger->log_debug("RefBoxCommThread", "Gamestate: %d State team: %s",
00146 game_state, worldinfo_gamestate_team_tostring(state_team));
00147 __gamestate_if->set_game_state(game_state);
00148 switch (state_team) {
00149 case TEAM_NONE:
00150 __gamestate_if->set_state_team(GameStateInterface::TEAM_NONE); break;
00151 case TEAM_CYAN:
00152 __gamestate_if->set_state_team(GameStateInterface::TEAM_CYAN); break;
00153 case TEAM_MAGENTA:
00154 __gamestate_if->set_state_team(GameStateInterface::TEAM_MAGENTA); break;
00155 case TEAM_BOTH:
00156 __gamestate_if->set_state_team(GameStateInterface::TEAM_BOTH); break;
00157 }
00158 }
00159 }
00160
00161 void
00162 RefBoxCommThread::set_score(unsigned int score_cyan, unsigned int score_magenta)
00163 {
00164 if ( (score_cyan != __last_score_cyan) || (score_magenta != __last_score_magenta) ) {
00165 __last_score_cyan = score_cyan;
00166 __last_score_magenta = score_magenta;
00167 __gamestate_modified = true;
00168
00169 logger->log_debug("RefBoxCommThread", "Score (cyan:magenta): %u:%u",
00170 score_cyan, score_magenta);
00171 __gamestate_if->set_score_cyan(score_cyan);
00172 __gamestate_if->set_score_magenta(score_magenta);
00173 }
00174 }
00175
00176
00177 void
00178 RefBoxCommThread::set_team_goal(fawkes::worldinfo_gamestate_team_t our_team,
00179 fawkes::worldinfo_gamestate_goalcolor_t goal_color)
00180 {
00181 if (our_team != __our_team)
00182 {
00183 logger->log_debug("RefBoxCommThread", "Team: %s",
00184 worldinfo_gamestate_team_tostring(our_team));
00185
00186 __our_team = our_team;
00187 switch (our_team) {
00188 case TEAM_CYAN:
00189 __gamestate_if->set_our_team(GameStateInterface::TEAM_CYAN);
00190 break;
00191 case TEAM_MAGENTA:
00192 __gamestate_if->set_our_team(GameStateInterface::TEAM_MAGENTA);
00193 break;
00194 default:
00195 break;
00196 }
00197 __gamestate_modified = true;
00198 }
00199
00200 if (goal_color != __our_goal_color)
00201 {
00202 logger->log_debug("RefBoxCommThread", "Our Goal: %s",
00203 worldinfo_gamestate_goalcolor_tostring(goal_color));
00204 __our_goal_color = goal_color;
00205 switch (goal_color)
00206 {
00207 case GOAL_BLUE:
00208 __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_BLUE);
00209 break;
00210 case GOAL_YELLOW:
00211 __gamestate_if->set_our_goal_color(GameStateInterface::GOAL_YELLOW);
00212 break;
00213 }
00214 __gamestate_modified = true;
00215 }
00216 }
00217
00218
00219 void
00220 RefBoxCommThread::set_half(fawkes::worldinfo_gamestate_half_t half,
00221 bool kickoff)
00222 {
00223 if (half != __last_half) {
00224 __last_half = half;
00225 __gamestate_modified = true;
00226
00227 logger->log_debug("RefBoxCommThread", "Half time: %s (Kickoff? %s)",
00228 worldinfo_gamestate_half_tostring(half),
00229 kickoff ? "yes" : "no");
00230
00231 switch (half) {
00232 case HALF_FIRST:
00233 __gamestate_if->set_half(GameStateInterface::HALF_FIRST); break;
00234 case HALF_SECOND:
00235 __gamestate_if->set_half(GameStateInterface::HALF_SECOND); break;
00236 }
00237 }
00238
00239 if (kickoff != __kickoff)
00240 {
00241 __kickoff = kickoff;
00242 __gamestate_modified = true;
00243 __gamestate_if->set_kickoff(kickoff);
00244 }
00245 }
00246
00247
00248 void
00249 RefBoxCommThread::add_penalty(unsigned int penalty,
00250 unsigned int seconds_remaining)
00251 {
00252 if ((penalty != __penalty_if->penalty()) ||
00253 (seconds_remaining != __penalty_if->remaining()))
00254 {
00255 __gamestate_modified = true;
00256 logger->log_debug("RefBoxCommThread", "Penalty %u (%u sec remaining)",
00257 penalty, seconds_remaining);
00258 __penalty_if->set_penalty(penalty);
00259 __penalty_if->set_remaining(seconds_remaining);
00260 }
00261 }
00262
00263
00264
00265 void
00266 RefBoxCommThread::handle_refbox_state()
00267 {
00268 __gamestate_if->write();
00269 }
