refbox_state_writer.cpp
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "refbox_state_writer.h"
00024
00025 #include <netcomm/worldinfo/transceiver.h>
00026 #include <utils/time/time.h>
00027 #include <utils/time/clock.h>
00028
00029 #include <set>
00030 #include <cstdio>
00031
00032 #define log(...) if (__debug) {\
00033 printf("%3u %s ", __counter, get_time().c_str());\
00034 printf(__VA_ARGS__);\
00035 fflush(stdout);\
00036 }
00037
00038 using namespace std;
00039 using namespace fawkes;
00040
00041 namespace {
00042 std::string get_time() throw()
00043 {
00044 Clock* c = Clock::instance();
00045 Time t = c->now();
00046 char* buf = new char[Time::TIMESTR_SIZE];
00047 t.str_r(buf, true);
00048 std::string str = buf;
00049 delete buf;
00050 std::string::size_type from =
00051 1+str.find_first_of(' ',
00052 1+str.find_first_of(' ',
00053 1+str.find_first_of(' ')));
00054 std::string::size_type to = str.find_last_of(' ');
00055 return str.substr(from, to - from + 1);
00056 }
00057 }
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070 RefBoxStateBBWriter::RefBoxStateBBWriter(vector<string> hosts, bool debug)
00071 {
00072 __counter = 0;
00073 __debug = debug;
00074
00075 __game_state = GS_FROZEN;
00076 __state_team = TEAM_BOTH;
00077 __score_cyan = 0;
00078 __score_magenta = 0;
00079 __our_team = TEAM_CYAN;
00080 __our_goal_color = GOAL_BLUE;
00081 __half = HALF_FIRST;
00082
00083 for (vector<string>::const_iterator it = hosts.begin(); it != hosts.end();
00084 it++) {
00085 connect(*it);
00086 }
00087 }
00088
00089
00090
00091 RefBoxStateBBWriter::~RefBoxStateBBWriter()
00092 {
00093 for (map<RemoteBlackBoard*, GameStateInterface*>::iterator it = __giss.begin();
00094 it != __giss.end(); it++) {
00095 RemoteBlackBoard* rbb = it->first;
00096 GameStateInterface* gis = it->second;
00097 rbb->close(gis);
00098 delete rbb;
00099 }
00100 }
00101
00102
00103
00104 void RefBoxStateBBWriter::connect(const string& host)
00105 {
00106 try {
00107 RemoteBlackBoard* rbb = new RemoteBlackBoard(host.c_str(), 1910);
00108 __rbbs[rbb] = host;
00109 GameStateInterface* gis = static_cast<GameStateInterface*>(rbb->open_for_writing("GameStateInterface", "WM GameState"));
00110 __giss[rbb] = gis;
00111 log("Successfully connected to %s\n", host.c_str());
00112 set_gamestate(__game_state, __state_team);
00113 set_score(__score_cyan, __score_magenta);
00114 set_team_goal(__our_team, __our_goal_color);
00115 set_half(__half);
00116 gis->write();
00117 } catch (Exception& e) {
00118 log("Connecting to %s failed\n", host.c_str());
00119 e.print_trace();
00120 log("\n");
00121 log("\n");
00122 }
00123 }
00124
00125
00126
00127
00128
00129 void
00130 RefBoxStateBBWriter::set_gamestate(int game_state,
00131 worldinfo_gamestate_team_t state_team)
00132 {
00133 log("Setting gamestate to '%d' for team '%s'\n",
00134 game_state, worldinfo_gamestate_team_tostring(state_team));
00135
00136 __game_state = game_state;
00137 __state_team = state_team;
00138
00139 for (map<RemoteBlackBoard*,GameStateInterface*>::iterator it = __giss.begin(); it != __giss.end(); it++) {
00140 GameStateInterface* gis = it->second;
00141 switch (game_state)
00142 {
00143 case(GS_FROZEN):
00144 gis->set_game_state( GameStateInterface::GS_FROZEN );
00145 break;
00146
00147 case(GS_PLAY):
00148 gis->set_game_state( GameStateInterface::GS_PLAY );
00149 break;
00150
00151 case(GS_KICK_OFF):
00152 gis->set_game_state( GameStateInterface::GS_KICK_OFF );
00153 break;
00154
00155 case(GS_DROP_BALL):
00156 gis->set_game_state( GameStateInterface::GS_DROP_BALL );
00157 break;
00158
00159 case(GS_PENALTY):
00160 gis->set_game_state( GameStateInterface::GS_PENALTY );
00161 break;
00162
00163 case(GS_CORNER_KICK):
00164 gis->set_game_state( GameStateInterface::GS_CORNER_KICK );
00165 break;
00166
00167 case(GS_THROW_IN):
00168 gis->set_game_state( GameStateInterface::GS_THROW_IN );
00169 break;
00170
00171 case(GS_FREE_KICK):
00172 gis->set_game_state( GameStateInterface::GS_FREE_KICK );
00173 break;
00174
00175 case(GS_GOAL_KICK):
00176 gis->set_game_state( GameStateInterface::GS_GOAL_KICK );
00177 break;
00178
00179 case(GS_HALF_TIME):
00180 gis->set_game_state( GameStateInterface::GS_HALF_TIME );
00181 break;
00182 }
00183
00184 switch (state_team)
00185 {
00186 case(TEAM_NONE):
00187 gis->set_state_team( GameStateInterface::TEAM_NONE );
00188 break;
00189
00190 case(TEAM_CYAN):
00191 gis->set_state_team( GameStateInterface::TEAM_CYAN );
00192 break;
00193
00194 case(TEAM_MAGENTA):
00195 gis->set_state_team( GameStateInterface::TEAM_MAGENTA );
00196 break;
00197
00198 case(TEAM_BOTH):
00199 gis->set_state_team( GameStateInterface::TEAM_BOTH );
00200 break;
00201 }
00202 }
00203 }
00204
00205
00206
00207
00208
00209
00210 void
00211 RefBoxStateBBWriter::set_score(unsigned int score_cyan, unsigned int score_magenta)
00212 {
00213 log("Setting score to %u:%u (cyan:magenta)\n", score_cyan, score_magenta);
00214
00215 __score_cyan = score_cyan;
00216 __score_magenta = score_magenta;
00217
00218 for (map<RemoteBlackBoard*,GameStateInterface*>::iterator it = __giss.begin(); it != __giss.end(); it++) {
00219 GameStateInterface* gis = it->second;
00220 gis->set_score_cyan( score_cyan );
00221 gis->set_score_magenta( score_magenta );
00222 }
00223 }
00224
00225
00226
00227
00228
00229
00230 void
00231 RefBoxStateBBWriter::set_team_goal(worldinfo_gamestate_team_t our_team,
00232 worldinfo_gamestate_goalcolor_t goal_color)
00233 {
00234 log("Setting team color to '%s' and goal color to '%s'\n",
00235 worldinfo_gamestate_team_tostring(our_team),
00236 worldinfo_gamestate_goalcolor_tostring(goal_color));
00237
00238 __our_team = our_team;
00239 __our_goal_color = goal_color;
00240
00241 for (map<RemoteBlackBoard*,GameStateInterface*>::iterator it = __giss.begin(); it != __giss.end(); it++) {
00242 GameStateInterface* gis = it->second;
00243 if (our_team == TEAM_CYAN) {
00244 gis->set_our_team( GameStateInterface::TEAM_CYAN );
00245 } else {
00246 gis->set_our_team( GameStateInterface::TEAM_MAGENTA );
00247 }
00248
00249 if (goal_color == GOAL_BLUE) {
00250 gis->set_our_goal_color( GameStateInterface::GOAL_BLUE );
00251 } else {
00252 gis->set_our_goal_color( GameStateInterface::GOAL_YELLOW );
00253 }
00254 }
00255 }
00256
00257
00258
00259
00260
00261 void
00262 RefBoxStateBBWriter::set_half(worldinfo_gamestate_half_t half)
00263 {
00264 log("Setting half to '%s'\n",
00265 worldinfo_gamestate_half_tostring(half));
00266
00267 __half = half;
00268
00269 for (map<RemoteBlackBoard*,GameStateInterface*>::iterator it = __giss.begin(); it != __giss.end(); it++) {
00270 GameStateInterface* gis = it->second;
00271 switch (half) {
00272 case HALF_FIRST:
00273 gis->set_half(GameStateInterface::HALF_FIRST);
00274 break;
00275 case HALF_SECOND:
00276 gis->set_half(GameStateInterface::HALF_SECOND);
00277 break;
00278 }
00279 }
00280 }
00281
00282
00283
00284 void
00285 RefBoxStateBBWriter::send()
00286 {
00287 ++__counter;
00288 log("Sending worldinfo\n");
00289
00290 set<RemoteBlackBoard*> erase_rbbs;
00291 set<string> reconnect_hosts;
00292
00293 unsigned int i = 0;
00294 for (map<RemoteBlackBoard*,GameStateInterface*>::iterator it = __giss.begin(); it != __giss.end(); it++) {
00295 RemoteBlackBoard* rbb = it->first;
00296 GameStateInterface* gis = it->second;
00297 const string host = __rbbs[rbb].c_str();
00298 try {
00299 gis->set_score_cyan(gis->score_cyan() + 1);
00300 gis->write();
00301 log("%u. Successfully wrote game state on %s\n", ++i, __rbbs[rbb].c_str());
00302 } catch (Exception& e) {
00303 log("%u. Writing game state on %s failed, reason:\n", ++i, __rbbs[rbb].c_str());
00304 e.print_trace();
00305 log("I will reconnect after this loop\n");
00306 erase_rbbs.insert(rbb);
00307 reconnect_hosts.insert(host);
00308 }
00309 }
00310 for (set<RemoteBlackBoard*>::iterator it = erase_rbbs.begin(); it != erase_rbbs.end(); it++) {
00311 RemoteBlackBoard* rbb = *it;
00312 __rbbs.erase(rbb);
00313 __giss.erase(rbb);
00314 }
00315 for (set<std::string>::iterator it = reconnect_hosts.begin(); it != reconnect_hosts.end(); it++) {
00316 std::string host = *it;
00317 log("Reconnecting to %s\n", host.c_str());
00318 connect(host);
00319 }
00320
00321 log("Sending worldinfo done\n");
00322 }