Fawkes API
Fawkes Development Version
|
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_ACQUIRE) 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 }